From 46f4e493cfe99d9ea9ca87541c59fb543e1eb1a5 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 17 Apr 2017 13:03:54 +0200 Subject: [PATCH] Big code dump --- CMakeLists.txt | 9 +- include/ur_modern_driver/ros/action_server.h | 69 ++++++ include/ur_modern_driver/ros/controller.h | 80 ++++++ .../ur_modern_driver/ros/hardware_interface.h | 144 ++--------- include/ur_modern_driver/ros/io_service.h | 6 +- include/ur_modern_driver/ros/robot_hardware.h | 4 +- include/ur_modern_driver/ros/ros_controller.h | 60 ----- .../ur_modern_driver/ros/service_stopper.h | 52 ++++ .../ros/trajectory_follower.h | 44 ++++ include/ur_modern_driver/ur/commander.h | 3 +- include/ur_modern_driver/ur/robot_mode.h | 3 +- include/ur_modern_driver/ur/server.h | 19 ++ include/ur_modern_driver/ur/stream.h | 23 ++ src/ros/action_server.cpp | 231 ++++++++++++++++++ src/ros/controller.cpp | 84 +++++++ src/ros/hardware_interface.cpp | 74 ++++++ src/ros/robot_hardware.cpp | 42 ---- src/ros/service_stopper.cpp | 47 ++++ src/ros/trajectory_follower.cpp | 137 +++++++++++ src/ros_main.cpp | 37 ++- src/ur/commander.cpp | 16 +- src/ur/robot_mode.cpp | 9 +- src/ur/server.cpp | 51 ++++ tests/ur/robot_mode.cpp | 2 +- 24 files changed, 989 insertions(+), 257 deletions(-) create mode 100644 include/ur_modern_driver/ros/action_server.h create mode 100644 include/ur_modern_driver/ros/controller.h delete mode 100644 include/ur_modern_driver/ros/ros_controller.h create mode 100644 include/ur_modern_driver/ros/service_stopper.h create mode 100644 include/ur_modern_driver/ros/trajectory_follower.h create mode 100644 include/ur_modern_driver/ur/server.h create mode 100644 src/ros/action_server.cpp create mode 100644 src/ros/controller.cpp create mode 100644 src/ros/hardware_interface.cpp create mode 100644 src/ros/service_stopper.cpp create mode 100644 src/ros/trajectory_follower.cpp create mode 100644 src/ur/server.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 944df13..1de2725 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,10 +158,15 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES - src/ros/rt_publisher.cpp + src/ros/action_server.cpp + src/ros/controller.cpp + src/ros/hardware_interface.cpp src/ros/mb_publisher.cpp - src/ros/robot_hardware.cpp + src/ros/rt_publisher.cpp + src/ros/service_stopper.cpp + src/ros/trajectory_follower.cpp src/ur/stream.cpp + src/ur/server.cpp src/ur/commander.cpp src/ur/robot_mode.cpp src/ur/master_board.cpp diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h new file mode 100644 index 0000000..dd11de4 --- /dev/null +++ b/include/ur_modern_driver/ros/action_server.h @@ -0,0 +1,69 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/consumer.h" +#include "ur_modern_driver/ur/master_board.h" +#include "ur_modern_driver/ur/state.h" +#include "ur_modern_driver/ros/service_stopper.h" +#include "ur_modern_driver/ros/trajectory_follower.h" + + +class ActionServer : public URRTPacketConsumer, public Service +{ +private: + typedef control_msgs::FollowJointTrajectoryAction Action; + typedef control_msgs::FollowJointTrajectoryResult Result; + typedef actionlib::ServerGoalHandle GoalHandle; + typedef actionlib::ActionServer Server; + + ros::NodeHandle nh_; + Server as_; + + std::vector joint_names_; + std::set joint_set_; + double max_velocity_; + RobotState state_; + + + GoalHandle curr_gh_; + std::atomic has_goal_, running_; + std::mutex tj_mutex_; + std::condition_variable tj_cv_; + + TrajectoryFollower& follower_; + + void onGoal(GoalHandle gh); + void onCancel(GoalHandle gh); + + bool validate(GoalHandle& gh, Result& res); + bool validateState(GoalHandle& gh, Result& res); + bool validateJoints(GoalHandle& gh, Result& res); + bool validateTrajectory(GoalHandle& gh, Result& res); + + bool try_execute(GoalHandle& gh, Result& res); + + std::vector reorderMap(std::vector goal_joints); + double interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); + + void trajectoryThread(); + + template + double toSec(U const& u) + { + return std::chrono::duration_cast>(u).count(); + } + +public: + ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity); + + virtual void onRobotStateChange(RobotState state); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h new file mode 100644 index 0000000..eb940d0 --- /dev/null +++ b/include/ur_modern_driver/ros/controller.h @@ -0,0 +1,80 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/consumer.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ros/hardware_interface.h" +#include "ur_modern_driver/ros/service_stopper.h" + +class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service +{ +private: + ros::NodeHandle nh_; + ros::Time lastUpdate_; + controller_manager::ControllerManager controller_; + + // state interfaces + JointInterface joint_interface_; + WrenchInterface wrench_interface_; + // controller interfaces + PositionInterface position_interface_; + VelocityInterface velocity_interface_; + + // currently activated controller + HardwareInterface* active_interface_; + // map of switchable controllers + std::map available_interfaces_; + + std::atomic service_enabled_; + + // helper functions to map interfaces + template + void registerInterface(T* interface) + { + RobotHW::registerInterface(interface); + } + template + void registerControllereInterface(T* interface) + { + registerInterface(interface); + available_interfaces_[hardware_interface::internal::demangledTypeName()] = interface; + } + + void read(RTShared& state); + bool update(RTShared& state); + bool write(); + +public: + ROSController(URCommander& commander, std::vector& joint_names, double max_vel_change); + virtual ~ROSController() { } + // from RobotHW + void doSwitch(const std::list& start_list, const std::list& stop_list); + // from URRTPacketConsumer + virtual void setupConsumer(); + virtual bool consume(RTState_V1_6__7& state) + { + return update(state); + } + virtual bool consume(RTState_V1_8& state) + { + return update(state); + } + virtual bool consume(RTState_V3_0__1& state) + { + return update(state); + } + virtual bool consume(RTState_V3_2__3& state) + { + return update(state); + } + + virtual void onRobotStateChange(RobotState state); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index dfb36c7..0c6618d 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -10,39 +10,22 @@ class HardwareInterface { public: - virtual void write() = 0; - virtual void start() - { - } - virtual void stop() - { - } + virtual bool write() = 0; + virtual void start() {} + virtual void stop() {} }; using hardware_interface::JointHandle; -using hardware_interface::JointStateHandle; -using hardware_interface::JointStateInterface; -class JointInterface : public JointStateInterface +class JointInterface : public hardware_interface::JointStateInterface { private: std::array velocities_, positions_, efforts_; public: - JointInterface(std::vector& joint_names) - { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); - } - } - - void update(RTShared& packet) - { - positions_ = packet.q_actual; - velocities_ = packet.qd_actual; - efforts_ = packet.i_actual; - } + JointInterface(std::vector &joint_names); + void update(RTShared &packet); + typedef hardware_interface::JointStateInterface parent_type; }; class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface @@ -50,124 +33,35 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface std::array tcp_; public: - WrenchInterface() - { - registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); - } - - void update(RTShared& packet) - { - tcp_ = packet.tcp_force; - } + WrenchInterface(); + void update(RTShared &packet); + typedef hardware_interface::ForceTorqueSensorInterface parent_type; }; class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface { private: - URCommander& commander_; + URCommander &commander_; std::array velocity_cmd_, prev_velocity_cmd_; - double max_vel_change_ = 0.12 / 125; + double max_vel_change_; public: - VelocityInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) - : commander_(commander) - { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); - } - } - - virtual void write() - { - for (size_t i = 0; i < 6; i++) - { - double prev = prev_velocity_cmd_[i]; - double lo = prev - max_vel_change_; - double hi = prev + max_vel_change_; - // clamp value to ±max_vel_change - prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); - } - - // times 125??? - commander_.speedj(prev_velocity_cmd_, max_vel_change_ * 125); - } - + VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change); + virtual bool write(); typedef hardware_interface::VelocityJointInterface parent_type; }; -static const std::string POSITION_PROGRAM = R"( -def driverProg(): - MULT_jointstate = XXXXX - - SERVO_IDLE = 0 - SERVO_RUNNING = 1 - cmd_servo_state = SERVO_IDLE - cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - def set_servo_setpoint(q): - enter_critical - cmd_servo_state = SERVO_RUNNING - cmd_servo_q = q - exit_critical - end - thread servoThread(): - state = SERVO_IDLE - while True: - enter_critical - q = cmd_servo_q - do_brake = False - if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): - do_brake = True - end - state = cmd_servo_state - cmd_servo_state = SERVO_IDLE - exit_critical - if do_brake: - stopj(1.0) - sync() - elif state == SERVO_RUNNING: - servoj(q, YYYYYYYY) - else: - sync() - end - end - end - - thread_servo = run servoThread() - keepalive = 1 - while keepalive > 0: - params_mult = socket_read_binary_integer(6+1) - if params_mult[0] > 0: - q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] - keepalive = params_mult[7] - set_servo_setpoint(q) - end - end - sleep(.1) - socket_close() - kill thread_servo -end -)"; - class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface { private: - URCommander& commander_; + URCommander &commander_; std::array position_cmd_; public: - PositionInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) - : commander_(commander) - { - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); - } - } - - virtual void write() - { - } + PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names); + virtual bool write(); + virtual void start(); + virtual void stop(); typedef hardware_interface::PositionJointInterface parent_type; }; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/io_service.h b/include/ur_modern_driver/ros/io_service.h index be59376..36ce662 100644 --- a/include/ur_modern_driver/ros/io_service.h +++ b/include/ur_modern_driver/ros/io_service.h @@ -35,18 +35,18 @@ private: res = commander_.setToolVoltage(static_cast(req.state)); break; case ur_msgs::SetIO::Request::FUN_SET_FLAG: - res = commander_.setFlag(flag); + res = commander_.setFlag(req.pin, flag); break; } - return resp.success = res; + return (resp.success = res); } bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) { LOG_INFO("setPayload called"); //TODO check min and max payload? - return resp.success = commander_.setPayload(req.payload); + return (resp.success = commander_.setPayload(req.payload)); } diff --git a/include/ur_modern_driver/ros/robot_hardware.h b/include/ur_modern_driver/ros/robot_hardware.h index 5e7c566..47ef455 100644 --- a/include/ur_modern_driver/ros/robot_hardware.h +++ b/include/ur_modern_driver/ros/robot_hardware.h @@ -29,11 +29,11 @@ private: } public: - RobotHardware(URCommander& commander, std::vector& joint_names) + RobotHardware(URCommander& commander, std::vector& joint_names, double max_vel_change) : joint_interface_(joint_names) , wrench_interface_() , position_interface_(commander, joint_interface_, joint_names) - , velocity_interface_(commander, joint_interface_, joint_names) + , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) { registerInterface(&joint_interface_); registerInterface(&wrench_interface_); diff --git a/include/ur_modern_driver/ros/ros_controller.h b/include/ur_modern_driver/ros/ros_controller.h deleted file mode 100644 index ad9f2ae..0000000 --- a/include/ur_modern_driver/ros/ros_controller.h +++ /dev/null @@ -1,60 +0,0 @@ -#pragma once -#include -#include -#include "ur_modern_driver/log.h" -#include "ur_modern_driver/pipeline.h" -#include "ur_modern_driver/ur/rt_state.h" -#include "ur_modern_driver/ur/commander.h" -#include "ur_modern_driver/ros/robot_hardware.h" - -class ROSController : public URRTPacketConsumer -{ -private: - ros::NodeHandle nh_; - ros::Time lastUpdate_; - RobotHardware robot_; - controller_manager::ControllerManager controller_; - -public: - ROSController(URCommander& commander, std::vector& joint_names) - : robot_(commander, joint_names) - , controller_(&robot_, nh_) - { - } - - virtual void setupConsumer() - { - lastUpdate_ = ros::Time::now(); - } - - bool handle(RTShared& state) - { - auto time = ros::Time::now(); - auto diff = time - lastUpdate_; - lastUpdate_ = time; - - robot_.read(state); - controller_.update(time, diff); - robot_.write(); - //todo: return result of write - return true; - } - - - virtual bool consume(RTState_V1_6__7& state) - { - return handle(state); - } - virtual bool consume(RTState_V1_8& state) - { - return handle(state); - } - virtual bool consume(RTState_V3_0__1& state) - { - return handle(state); - } - virtual bool consume(RTState_V3_2__3& state) - { - return handle(state); - } -}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h new file mode 100644 index 0000000..af09dfa --- /dev/null +++ b/include/ur_modern_driver/ros/service_stopper.h @@ -0,0 +1,52 @@ +#pragma once +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/consumer.h" + +enum class RobotState +{ + Running, + Error, + EmergencyStopped, + ProtectiveStopped +}; + +class Service +{ +public: + virtual void onRobotStateChange(RobotState state) = 0; +}; + +class ServiceStopper : public URStatePacketConsumer { +private: + ros::NodeHandle nh_; + ros::ServiceServer enable_service_; + std::vector services_; + RobotState last_state_; + + void notify_all(RobotState state); + bool handle(SharedRobotModeData &data, bool error); + bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + +public: + ServiceStopper(std::vector services); + + virtual bool consume(RobotModeData_V1_X& data) + { + return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE); + } + virtual bool consume(RobotModeData_V3_0__1& data) + { + return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); + } + virtual bool consume(RobotModeData_V3_2& data) + { + return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING); + } + + //unused + virtual bool consume(MasterBoardData_V1_X& data) { return true; } + virtual bool consume(MasterBoardData_V3_0__1& data) { return true; } + virtual bool consume(MasterBoardData_V3_2& data) { return true; } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h new file mode 100644 index 0000000..a1df97f --- /dev/null +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/server.h" +#include "ur_modern_driver/ur/stream.h" + +class TrajectoryFollower +{ +private: + const int32_t MULT_JOINTSTATE_ = 1000000; + double servoj_time_, servoj_lookahead_time_, servoj_gain_; + std::atomic running_; + std::array last_positions_; + URCommander &commander_; + URServer server_; + URStream stream_; + std::string program_; + + template + size_t append(uint8_t *buffer, T &val) + { + size_t s = sizeof(T); + std::memcpy(buffer, &val, s); + return s; + } + + bool execute(std::array &positions, bool keep_alive); + +public: + TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3); + + std::string buildProgram(bool version_3); + + bool start(); + bool execute(std::array &positions); + void stop(); + void halt(); //maybe +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h index 6325f8c..da92ae8 100644 --- a/include/ur_modern_driver/ur/commander.h +++ b/include/ur_modern_driver/ur/commander.h @@ -17,11 +17,12 @@ public: { } + virtual bool uploadProg(std::string &s); virtual bool speedj(std::array &speeds, double acceleration); virtual bool stopj(double a = 10.0); virtual bool setDigitalOut(uint8_t pin, bool value); virtual bool setAnalogOut(uint8_t pin, double value); virtual bool setToolVoltage(uint8_t voltage); - virtual bool setFlag(bool value); + virtual bool setFlag(uint8_t pin, bool value); virtual bool setPayload(double value); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h index 90c9eb8..6c7c641 100644 --- a/include/ur_modern_driver/ur/robot_mode.h +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -16,6 +16,7 @@ public: bool real_robot_enabled; bool robot_power_on; bool emergency_stopped; + bool protective_stopped; //AKA security_stopped bool program_running; bool program_paused; @@ -43,7 +44,6 @@ public: virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - bool security_stopped; robot_mode_V1_X robot_mode; double speed_fraction; @@ -79,7 +79,6 @@ public: virtual bool parseWith(BinParser& bp); virtual bool consumeWith(URStatePacketConsumer& consumer); - bool protective_stopped; robot_mode_V3_X robot_mode; robot_control_mode_V3_X control_mode; diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h new file mode 100644 index 0000000..8740806 --- /dev/null +++ b/include/ur_modern_driver/ur/server.h @@ -0,0 +1,19 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/ur/stream.h" + +class URServer +{ +private: + int socket_fd_ = -1; + +public: + URServer(int port); + URStream accept(); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index eac3c09..16719d7 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -19,15 +19,38 @@ private: std::mutex send_mutex_, receive_mutex_; public: + URStream() + { + } + URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false) { } + URStream(int socket_fd) : socket_fd_(socket_fd), initialized_(true), stopping_(false) + { + + } + + URStream(URStream&& other) noexcept : socket_fd_(other.socket_fd_), host_(other.host_), initialized_(other.initialized_.load()), stopping_(other.stopping_.load()) + { + + } + ~URStream() { disconnect(); } + URStream& operator=(URStream&& other) + { + socket_fd_ = std::move(other.socket_fd_); + host_ = std::move(other.host_); + initialized_ = std::move(other.initialized_.load()); + stopping_ = std::move(other.stopping_.load()); + return *this; + } + bool connect(); void disconnect(); void reconnect(); diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp new file mode 100644 index 0000000..3b6637d --- /dev/null +++ b/src/ros/action_server.cpp @@ -0,0 +1,231 @@ +#include +#include "ur_modern_driver/ros/action_server.h" + +ActionServer::ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity) + : as_( + nh_, + "follow_joint_trajectory", + boost::bind(&ActionServer::onGoal, this, _1), + boost::bind(&ActionServer::onCancel, this, _1), + false + ) + , joint_names_(joint_names) + , joint_set_(joint_names.begin(), joint_names.end()) + , max_velocity_(max_velocity) + , state_(RobotState::Error) + , follower_(follower) +{ + +} + +void ActionServer::onRobotStateChange(RobotState state) +{ + state_ = state; +} + +void ActionServer::onGoal(GoalHandle gh) +{ + Result res; + res.error_code = -100; + + if(!validate(gh, res) || !try_execute(gh, res)) + gh.setRejected(res, res.error_string); +} + +void ActionServer::onCancel(GoalHandle gh) +{ + +} + +bool ActionServer::validate(GoalHandle& gh, Result& res) +{ + return !validateState(gh, res) || !validateJoints(gh, res) || !validateTrajectory(gh, res); +} + +bool ActionServer::validateState(GoalHandle& gh, Result& res) +{ + switch(state_) + { + case RobotState::EmergencyStopped: + res.error_string = "Robot is emergency stopped"; + return false; + + case RobotState::ProtectiveStopped: + res.error_string = "Robot is protective stopped"; + return false; + + case RobotState::Error: + res.error_string = "Robot is not ready, check robot_mode"; + return false; + + case RobotState::Running: + return true; + + default: + res.error_string = "Undefined state"; + return false; + } +} + +bool ActionServer::validateJoints(GoalHandle& gh, Result& res) +{ + auto goal = gh.getGoal(); + auto const& joints = goal->trajectory.joint_names; + std::set goal_joints(joints.begin(), joints.end()); + + if(goal_joints == joint_set_) + return true; + + res.error_code = Result::INVALID_JOINTS; + res.error_string = "Invalid joint names for goal"; + return false; +} + +bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) +{ + auto goal = gh.getGoal(); + res.error_code = Result::INVALID_GOAL; + + for(auto const& point : goal->trajectory.points) + { + if(point.velocities.size() != joint_names_.size()) + { + res.error_code = Result::INVALID_GOAL; + res.error_string = "Received a goal with an invalid number of velocities"; + return false; + } + + if(point.positions.size() != joint_names_.size()) + { + res.error_code = Result::INVALID_GOAL; + res.error_string = "Received a goal with an invalid number of positions"; + return false; + } + + for(auto const& velocity : point.velocities) + { + if(!std::isfinite(velocity)) + { + res.error_string = "Received a goal with infinities or NaNs in velocity"; + return false; + } + if(std::fabs(velocity) > max_velocity_) + { + res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); + return false; + } + } + for(auto const& position : point.positions) + { + if(!std::isfinite(position)) + { + res.error_string = "Received a goal with infinities or NaNs in positions"; + return false; + } + } + } + + return true; +} + +bool ActionServer::try_execute(GoalHandle& gh, Result& res) +{ + if(!running_) + { + res.error_string = "Internal error"; + return false; + } + if(!tj_mutex_.try_lock()) + { + has_goal_ = false; + //stop_trajectory(); + res.error_string = "Received another trajectory"; + curr_gh_.setAborted(res, res.error_string); + tj_mutex_.lock(); + } + //locked here + curr_gh_ = gh; + has_goal_ = true; + tj_mutex_.unlock(); + tj_cv_.notify_one(); +} + +inline double ActionServer::interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel) +{ + using std::pow; + double a = p0_pos; + double b = p0_vel; + double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2); + double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3); + return a + b * t + c * pow(t, 2) + d * pow(t, 3); +} + +std::vector ActionServer::reorderMap(std::vector goal_joints) +{ + std::vector indecies; + for(auto const& aj : joint_names_) + { + size_t j = 0; + for(auto const& gj : goal_joints) + { + if(aj == gj) + break; + j++; + } + indecies.push_back(j); + } + return indecies; +} + +void ActionServer::trajectoryThread() +{ + while(running_) + { + std::unique_lock lk(tj_mutex_); + if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;})) + continue; + + auto g = curr_gh_.getGoal(); + auto const& traj = g->trajectory; + auto const& points = traj.points; + size_t len = points.size(); + auto const& last_point = points[points.size() - 1]; + double end_time = last_point.time_from_start.toSec(); + + auto mapping = reorderMap(traj.joint_names); + std::chrono::high_resolution_clock::time_point t0, t; + t = t0 = std::chrono::high_resolution_clock::now(); + + size_t i = 0; + while(end_time >= toSec(t - t0) && has_goal_) + { + while(points[i].time_from_start.toSec() <= toSec(t - t0) && i < len) + i++; + + auto const& pp = points[i-1]; + auto const& p = points[i]; + + auto pp_t = pp.time_from_start.toSec(); + auto p_t =p.time_from_start.toSec(); + + std::array pos; + for(size_t j = 0; j < pos.size(); j++) + { + pos[i] = interp_cubic( + toSec(t - t0) - pp_t, + p_t - pp_t, + pp.positions[j], + p.positions[j], + pp.velocities[j], + p.velocities[j] + ); + } + + follower_.execute(pos); + //std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); + t = std::chrono::high_resolution_clock::now(); + } + + has_goal_ = false; + } +} \ No newline at end of file diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp new file mode 100644 index 0000000..753f28d --- /dev/null +++ b/src/ros/controller.cpp @@ -0,0 +1,84 @@ +#include "ur_modern_driver/ros/controller.h" + +ROSController::ROSController(URCommander &commander, std::vector &joint_names, double max_vel_change) + : controller_(this, nh_) + , joint_interface_(joint_names) + , wrench_interface_() + , position_interface_(commander, joint_interface_, joint_names) + , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) +{ + registerInterface(&joint_interface_); + registerInterface(&wrench_interface_); + registerControllereInterface(&position_interface_); + registerControllereInterface(&velocity_interface_); +} + +void ROSController::setupConsumer() +{ + lastUpdate_ = ros::Time::now(); +} + +void ROSController::doSwitch(const std::list& start_list, const std::list& stop_list) +{ + if (active_interface_ != nullptr && stop_list.size() > 0) + { + LOG_INFO("Stopping active interface"); + active_interface_->stop(); + active_interface_ = nullptr; + } + + for (auto const& ci : start_list) + { + auto ait = available_interfaces_.find(ci.hardware_interface); + + if (ait == available_interfaces_.end()) + continue; + + auto new_interface = ait->second; + + LOG_INFO("Starting %s", ci.hardware_interface.c_str()); + active_interface_ = new_interface; + new_interface->start(); + + return; + } + + LOG_WARN("Failed to start interface!"); +} + +bool ROSController::write() +{ + if (active_interface_ == nullptr) + return true; + + return active_interface_->write(); +} + +void ROSController::read(RTShared& packet) +{ + joint_interface_.update(packet); + wrench_interface_.update(packet); +} + + +bool ROSController::update(RTShared& state) +{ + auto time = ros::Time::now(); + auto diff = time - lastUpdate_; + lastUpdate_ = time; + + read(state); + controller_.update(time, diff); + + //emergency stop and such should not kill the pipeline + //but still prevent writes + if(!service_enabled_) + return true; + + return write(); +} + +void ROSController::onRobotStateChange(RobotState state) +{ + service_enabled_ = (state == RobotState::Running); +} \ No newline at end of file diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp new file mode 100644 index 0000000..1bf0cc4 --- /dev/null +++ b/src/ros/hardware_interface.cpp @@ -0,0 +1,74 @@ +#include "ur_modern_driver/ros/hardware_interface.h" + +JointInterface::JointInterface(std::vector &joint_names) +{ + for (size_t i = 0; i < 6; i++) + { + registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); + } +} + +void JointInterface::update(RTShared &packet) +{ + positions_ = packet.q_actual; + velocities_ = packet.qd_actual; + efforts_ = packet.i_actual; +} + +WrenchInterface::WrenchInterface() +{ + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); +} + +void WrenchInterface::update(RTShared &packet) +{ + tcp_ = packet.tcp_force; +} + +VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) + : commander_(commander), max_vel_change_(max_vel_change) +{ + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); + } +} + +bool VelocityInterface::write() +{ + for (size_t i = 0; i < 6; i++) + { + double prev = prev_velocity_cmd_[i]; + double lo = prev - max_vel_change_; + double hi = prev + max_vel_change_; + // clamp value to ±max_vel_change + prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); + } + + return commander_.speedj(prev_velocity_cmd_, max_vel_change_); +} + + +PositionInterface:: PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) + : commander_(commander) +{ + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); + } +} + +bool PositionInterface::write() +{ + +} + +void PositionInterface::start() +{ + +} + +void PositionInterface::stop() +{ + +} \ No newline at end of file diff --git a/src/ros/robot_hardware.cpp b/src/ros/robot_hardware.cpp index fce866d..008daec 100644 --- a/src/ros/robot_hardware.cpp +++ b/src/ros/robot_hardware.cpp @@ -28,45 +28,3 @@ bool RobotHardware::canSwitch(const std::list& start_list, return true; } */ - -void RobotHardware::doSwitch(const std::list& start_list, const std::list& stop_list) -{ - if (active_interface_ != nullptr && stop_list.size() > 0) - { - LOG_INFO("Stopping active interface"); - active_interface_->stop(); - active_interface_ = nullptr; - } - - for (auto const& ci : start_list) - { - auto ait = available_interfaces_.find(ci.hardware_interface); - - if (ait == available_interfaces_.end()) - continue; - - auto new_interface = ait->second; - - LOG_INFO("Starting %s", ci.hardware_interface.c_str()); - active_interface_ = new_interface; - new_interface->start(); - - return; - } - - LOG_WARN("Failed to start interface!"); -} - -void RobotHardware::write() -{ - if (active_interface_ == nullptr) - return; - - active_interface_->write(); -} - -void RobotHardware::read(RTShared& packet) -{ - joint_interface_.update(packet); - wrench_interface_.update(packet); -} \ No newline at end of file diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp new file mode 100644 index 0000000..53cb43c --- /dev/null +++ b/src/ros/service_stopper.cpp @@ -0,0 +1,47 @@ +#include "ur_modern_driver/ros/service_stopper.h" + +ServiceStopper::ServiceStopper(std::vector services) + : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this)) + , services_(services) + , last_state_(RobotState::Error) +{ + //enable_all(); +} + + +bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +{ + notify_all(RobotState::Running); + return true; +} + +void ServiceStopper::notify_all(RobotState state) +{ + if(last_state_ == state) + return; + + for(auto const service : services_) + { + service->onRobotStateChange(state); + } + + last_state_ = state; +} + +bool ServiceStopper::handle(SharedRobotModeData& data, bool error) +{ + if(data.emergency_stopped) + { + notify_all(RobotState::EmergencyStopped); + } + else if(data.protective_stopped) + { + notify_all(RobotState::ProtectiveStopped); + } + else if(error) + { + notify_all(RobotState::Error); + } + + return true; +} \ No newline at end of file diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp new file mode 100644 index 0000000..ca36e59 --- /dev/null +++ b/src/ros/trajectory_follower.cpp @@ -0,0 +1,137 @@ +#include +#include "ur_modern_driver/ros/trajectory_follower.h" + + +TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3) + : running_(false) + , commander_(commander) + , server_(reverse_port) + , program_(buildProgram(version_3)) +{ +} +static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); +static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); +static const std::string POSITION_PROGRAM = R"( +def driverProg(): + MULT_jointstate = {{JOINT_STATE_REPLACE}} + + SERVO_IDLE = 0 + SERVO_RUNNING = 1 + cmd_servo_state = SERVO_IDLE + cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + def set_servo_setpoint(q): + enter_critical + cmd_servo_state = SERVO_RUNNING + cmd_servo_q = q + exit_critical + end + thread servoThread(): + state = SERVO_IDLE + while True: + enter_critical + q = cmd_servo_q + do_brake = False + if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): + do_brake = True + end + state = cmd_servo_state + cmd_servo_state = SERVO_IDLE + exit_critical + if do_brake: + stopj(1.0) + sync() + elif state == SERVO_RUNNING: + servoj(q, {{SERVO_J_REPLACE}}) + else: + sync() + end + end + end + + thread_servo = run servoThread() + keepalive = 1 + while keepalive > 0: + params_mult = socket_read_binary_integer(6+1) + if params_mult[0] > 0: + q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] + keepalive = params_mult[7] + set_servo_setpoint(q) + end + end + sleep(.1) + socket_close() + kill thread_servo +end +)"; +std::string TrajectoryFollower::buildProgram(bool version_3) +{ + std::string res(POSITION_PROGRAM); + size_t js_idx = POSITION_PROGRAM.find(JOINT_STATE_REPLACE); + size_t sj_idx = POSITION_PROGRAM.find(SERVO_J_REPLACE); + + + std::ostringstream out; + out << "t=" << std::fixed << std::setprecision(4) << servoj_time_; + + if(version_3) + out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; + + res.replace(js_idx, JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); + res.replace(sj_idx, SERVO_J_REPLACE.length(), out.str()); + return res; +} + +bool TrajectoryFollower::start() +{ + if(running_) + return true; //not sure + + //TODO + std::string prog(""); // buildProg(); + if(!commander_.uploadProg(prog)) + return false; + + stream_ = std::move(server_.accept()); //todo: pointer instead? + return (running_ = true); +} + +bool TrajectoryFollower::execute(std::array &positions, bool keep_alive) +{ + if(!running_) + return false; + + last_positions_ = positions; + + uint8_t buf[sizeof(uint32_t)*7]; + uint8_t *idx = buf; + + for(auto const& pos : positions) + { + int32_t val = static_cast(pos * MULT_JOINTSTATE_); + val = htobe32(val); + idx += append(idx, val); + } + + int32_t val = htobe32(static_cast(keep_alive)); + append(idx, val); + + ssize_t res = stream_.send(buf, sizeof(buf)); + return res > 0 && res == sizeof(buf); +} + +bool TrajectoryFollower::execute(std::array &positions) +{ + return execute(positions, true); +} + +void TrajectoryFollower::stop() +{ + if(!running_) + return; + + std::array empty; + execute(empty, false); + + stream_.disconnect(); + running_ = false; +} \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index b08e90b..7f76792 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -8,8 +8,9 @@ #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ros/io_service.h" #include "ur_modern_driver/ros/mb_publisher.h" -#include "ur_modern_driver/ros/ros_controller.h" +#include "ur_modern_driver/ros/controller.h" #include "ur_modern_driver/ros/rt_publisher.h" +#include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/messages.h" @@ -20,8 +21,8 @@ static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); -static const std::string SIM_TIME_ARG("~use_sim_time"); static const std::string ROS_CONTROL_ARG("~use_ros_control"); +static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); @@ -40,12 +41,12 @@ public: std::vector joint_names; double max_acceleration; double max_velocity; + double max_vel_change; int32_t reverse_port; - bool use_sim_time; bool use_ros_control; }; -bool parse_args(ProgArgs& args) +bool parse_args(ProgArgs &args) { if (!ros::param::get(IP_ADDR_ARG, args.host)) { @@ -53,7 +54,7 @@ bool parse_args(ProgArgs& args) return false; } ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); @@ -64,7 +65,7 @@ bool parse_args(ProgArgs& args) #include "ur_modern_driver/event_counter.h" -int main(int argc, char** argv) +int main(int argc, char **argv) { ros::init(argc, argv, "ur_driver"); @@ -75,18 +76,24 @@ int main(int argc, char** argv) } URFactory factory(args.host); + + vector services; + // RT packets auto rt_parser = factory.getRTParser(); URStream rt_stream(args.host, UR_RT_PORT); URProducer rt_prod(rt_stream, *rt_parser); RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control); URCommander rt_commander(rt_stream); - vector*> rt_vec{ &rt_pub }; + vector *> rt_vec{&rt_pub}; + ROSController *controller(nullptr); if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - rt_vec.push_back(new ROSController(rt_commander, args.joint_names)); + controller = new ROSController(rt_commander, args.joint_names, args.max_vel_change); + rt_vec.push_back(controller); + services.push_back(controller); } MultiConsumer rt_cons(rt_vec); @@ -97,7 +104,10 @@ int main(int argc, char** argv) URStream state_stream(args.host, UR_SECONDARY_PORT); URProducer state_prod(state_stream, *state_parser); MBPublisher state_pub; - vector*> state_vec{ &state_pub }; + + ServiceStopper service_stopper(services); + + vector *> state_vec{&state_pub, &service_stopper}; MultiConsumer state_cons(state_vec); Pipeline state_pl(state_prod, state_cons); @@ -107,12 +117,19 @@ int main(int argc, char** argv) state_pl.run(); URCommander state_commander(state_stream); - IOService service(state_commander); + IOService io_service(state_commander); ros::spin(); + LOG_INFO("ROS stopping, shutting down pipelines"); + rt_pl.stop(); state_pl.stop(); + + if(controller) + delete controller; + + LOG_INFO("Pipelines shutdown complete"); return EXIT_SUCCESS; } \ No newline at end of file diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index d9e16d2..333bc52 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -9,6 +9,10 @@ bool URCommander::write(std::string& s) return res > 0 && static_cast(res) == len; } +bool URCommander::uploadProg(std::string &s) +{ + return write(s); +} bool URCommander::speedj(std::array &speeds, double acceleration) { @@ -52,11 +56,17 @@ bool URCommander::setToolVoltage(uint8_t voltage) } -bool URCommander::setFlag(bool value) +bool URCommander::setFlag(uint8_t pin, bool value) { - + std::ostringstream out; + out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n"; + std::string s(out.str()); + return write(s); } bool URCommander::setPayload(double value) { - + std::ostringstream out; + out << "set_payload(" << std::fixed << std::setprecision(4) << value << ")\n"; + std::string s(out.str()); + return write(s); } \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp index 5bee4cf..da3a65b 100644 --- a/src/ur/robot_mode.cpp +++ b/src/ur/robot_mode.cpp @@ -8,6 +8,9 @@ bool SharedRobotModeData::parseWith(BinParser& bp) bp.parse(real_robot_enabled); bp.parse(robot_power_on); bp.parse(emergency_stopped); + bp.parse(protective_stopped); + bp.parse(program_running); + bp.parse(program_paused); return true; } @@ -18,9 +21,6 @@ bool RobotModeData_V1_X::parseWith(BinParser& bp) SharedRobotModeData::parseWith(bp); - bp.parse(security_stopped); - bp.parse(program_running); - bp.parse(program_paused); bp.parse(robot_mode); bp.parse(speed_fraction); @@ -34,9 +34,6 @@ bool RobotModeData_V3_0__1::parseWith(BinParser& bp) SharedRobotModeData::parseWith(bp); - bp.parse(protective_stopped); - bp.parse(program_running); - bp.parse(program_paused); bp.parse(robot_mode); bp.parse(control_mode); bp.parse(target_speed_fraction); diff --git a/src/ur/server.cpp b/src/ur/server.cpp new file mode 100644 index 0000000..d99f1a9 --- /dev/null +++ b/src/ur/server.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/server.h" + +URServer::URServer(int port) +{ + std::string service = std::to_string(port); + struct addrinfo hints, *result; + std::memset(&hints, 0, sizeof(hints)); + + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + + if (getaddrinfo(nullptr, service.c_str(), &hints, &result) != 0) + { + LOG_ERROR("Failed to setup recieving server"); + return; + } + + // loop through the list of addresses untill we find one that's connectable + for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) + { + socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol); + + if (socket_fd_ == -1) // socket error? + continue; + + if (bind(socket_fd_, p->ai_addr, p->ai_addrlen) != 0) + continue; + + // disable Nagle's algorithm to ensure we sent packets as fast as possible + int flag = 1; + setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); + LOG_INFO("Server awaiting connection"); + return; + } + + LOG_ERROR("Failed to setup recieving server"); + std::exit(EXIT_FAILURE); +} + +URStream URServer::accept() +{ + struct sockaddr addr; + socklen_t addr_len; + int client_fd = ::accept(socket_fd_, &addr, &addr_len); + return URStream(client_fd); +} \ No newline at end of file diff --git a/tests/ur/robot_mode.cpp b/tests/ur/robot_mode.cpp index a13fcb8..4209191 100644 --- a/tests/ur/robot_mode.cpp +++ b/tests/ur/robot_mode.cpp @@ -18,7 +18,7 @@ TEST(RobotModeData_V1_X, testRandomDataParsing) ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); ASSERT_EQ(rdt.getNext(), state.robot_power_on); ASSERT_EQ(rdt.getNext(), state.emergency_stopped); - ASSERT_EQ(rdt.getNext(), state.security_stopped); + ASSERT_EQ(rdt.getNext(), state.protective_stopped); ASSERT_EQ(rdt.getNext(), state.program_running); ASSERT_EQ(rdt.getNext(), state.program_paused); ASSERT_EQ(rdt.getNext(), state.robot_mode);