From 1caa2c4a97dc4a879e89127f7574d9d3de539dc7 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:44:02 +0200 Subject: [PATCH 1/6] make ScaledJointHandle inherit from JointHandle This reduces code duplication --- .../scaled_joint_command_interface.h | 25 +++++-------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h index 7a828f1..edec13e 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h +++ b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h @@ -44,28 +44,26 @@ #include #include +#include namespace ur_controllers { -class ScaledJointHandle : public hardware_interface::JointStateHandle +class ScaledJointHandle : public hardware_interface::JointHandle { public: - ScaledJointHandle() : cmd_(0), scaling_factor_(0) + ScaledJointHandle() : hardware_interface::JointHandle(), scaling_factor_(0) + { } /** * \param js This joint's state handle * \param cmd A pointer to the storage for this joint's output command + * \param scaling_factor A pointer to the storage for this joint's scaling factor */ ScaledJointHandle(const hardware_interface::JointStateHandle& js, double* cmd, double* scaling_factor) - : hardware_interface::JointStateHandle(js), cmd_(cmd), scaling_factor_(scaling_factor) + : hardware_interface::JointHandle(js, cmd), scaling_factor_(scaling_factor) { - if (cmd_ == nullptr) - { - throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + - "'. Command data pointer is null."); - } if (scaling_factor_ == nullptr) { throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + @@ -75,16 +73,6 @@ public: virtual ~ScaledJointHandle() = default; - void setCommand(double command) - { - assert(cmd_); - *cmd_ = command; - } - double getCommand() const - { - assert(cmd_); - return *cmd_; - } void setScalingFactor(double scaling_factor) { assert(scaling_factor_); @@ -97,7 +85,6 @@ public: } private: - double* cmd_; double* scaling_factor_; }; From 8c9f79bc3b53ccf829961337fc7b4c479543d8ad Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:46:21 +0200 Subject: [PATCH 2/6] turn around order of commands and keepalive This way we can leave the commands empty very easily --- .../ur_rtde_driver/comm/reverse_interface.h | 23 +++++++++++-------- ur_rtde_driver/resources/servoj.urscript | 16 ++++++++----- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h index 24a190b..d8ecfe1 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h @@ -57,20 +57,23 @@ public: server_.disconnectClient(); } - bool write(const vector6d_t& positions) + bool write(const vector6d_t* positions, const int32_t type = 2) { uint8_t buffer[sizeof(uint32_t) * 7]; uint8_t* b_pos = buffer; - for (auto const& pos : positions) - { - int32_t val = static_cast(pos * MULT_JOINTSTATE); - val = htobe32(val); - b_pos += append(b_pos, val); - } - // TODO: Make this a parameter: Number of allowed missed packages - int32_t val = htobe32(5); - append(b_pos, val); + int32_t val = htobe32(type); + b_pos += append(b_pos, val); + + if (positions != nullptr) + { + for (auto const& pos : *positions) + { + int32_t val = static_cast(pos * MULT_JOINTSTATE); + val = htobe32(val); + b_pos += append(b_pos, val); + } + } size_t written; diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript index 0811219..f12b785 100644 --- a/ur_rtde_driver/resources/servoj.urscript +++ b/ur_rtde_driver/resources/servoj.urscript @@ -71,16 +71,20 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") thread_servo = run servoThread() keepalive = -2 -params_mult = socket_read_binary_integer(6+1, "reverse_socket") -keepalive = params_mult[7] +params_mult = socket_read_binary_integer(1+6, "reverse_socket") +keepalive = params_mult[1] while keepalive > 0: enter_critical socket_send_line(1, "reverse_socket") - params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation + params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation if params_mult[0] > 0: - keepalive = params_mult[7] - 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] - set_servo_setpoint(q) + if params_mult[1] > 1: + keepalive = params_mult[1] + q = [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, params_mult[7] / MULT_jointstate] + set_servo_setpoint(q) + else: + keepalive = 1 + end else: keepalive = keepalive - 1 end From 14b9dd4f1b0e04f3087879163600f4f21329b0c7 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:48:24 +0200 Subject: [PATCH 3/6] send keepalive signal when no (commanding) controller is active Otherwise the hardware disconnects as soon as we stop a controller by hand. --- .../include/ur_rtde_driver/ur/ur_driver.h | 18 ++++++++++++++++++ ur_rtde_driver/src/ros/hardware_interface.cpp | 4 ++++ ur_rtde_driver/src/ur/ur_driver.cpp | 17 +++++++++++------ 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h index a376286..ce29f37 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h @@ -81,8 +81,26 @@ public: return rtde_frequency_; } + /*! + * \brief Writes a joint command together with a keepalive signal onto the socket being sent to + * the robot. + * + * \param values Desired joint positions + * + * \returns True on successful write. + */ bool writeJointCommand(const vector6d_t& values); + /*! + * \brief Write a keepalive signal only. + * + * This signals the robot that the connection is still + * active in times when no commands are to be sent (e.g. no controller is active.) + * + * \returns True on successful write. + */ + bool writeKeepalive(); + void startWatchdog(); private: diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index c78cc03..1384455 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -292,6 +292,10 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio { ur_driver_->writeJointCommand(joint_position_command_); } + else + { + ur_driver_->writeKeepalive(); + } } bool HardwareInterface ::prepareSwitch(const std::list& start_list, diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 479fe09..d8df01b 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -138,14 +138,19 @@ bool UrDriver::writeJointCommand(const vector6d_t& values) { if (reverse_interface_active_) { - reverse_interface_->write(values); - } - else - { - return false; + return reverse_interface_->write(&values); } + return false; +} - return true; +bool UrDriver::writeKeepalive() +{ + if (reverse_interface_active_) + { + vector6d_t* fake = nullptr; + return reverse_interface_->write(fake, 1); + } + return false; } void UrDriver::startWatchdog() From 5934de2772d165843cbb4db342982c994bbfe497 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:49:51 +0200 Subject: [PATCH 4/6] re-added a non-scaled position interface for position controllers As this interface uses the same joint names as the scaled interface, resourced conflicts are already managed by ros_control's resource claim mechanisms. --- ur_rtde_driver/src/ros/hardware_interface.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 1384455..2ea6bfc 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -171,8 +171,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h &joint_velocities_[i], &joint_efforts_[i])); // Create joint position control interface - // pj_interface_.registerHandle( - // hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); + pj_interface_.registerHandle( + hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); spj_interface_.registerHandle(ur_controllers::ScaledJointHandle( js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_)); } @@ -186,7 +186,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h // Register interfaces registerInterface(&js_interface_); registerInterface(&spj_interface_); - // registerInterface(&pj_interface_); + registerInterface(&pj_interface_); registerInterface(&speedsc_interface_); registerInterface(&fts_interface_); @@ -317,6 +317,10 @@ void HardwareInterface ::doSwitch(const std::list Date: Wed, 26 Jun 2019 08:54:29 +0200 Subject: [PATCH 5/6] define a non-scaled position trajectory controller by default This way, users can choose whether they want to use non-scaled controllers as used by older drivers. --- ur_rtde_driver/config/ur10_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur10e_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur3_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur3e_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur5_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur5e_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/launch/ur10_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur10e_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur3_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur3e_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur5_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur5e_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur_common.launch | 4 ++-- ur_rtde_driver/launch/ur_control.launch | 4 ++-- 14 files changed, 154 insertions(+), 22 deletions(-) diff --git a/ur_rtde_driver/config/ur10_controllers.yaml b/ur_rtde_driver/config/ur10_controllers.yaml index 42bb62d..ebbd6be 100644 --- a/ur_rtde_driver/config/ur10_controllers.yaml +++ b/ur_rtde_driver/config/ur10_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + wrist_2_joint: {trajectory: 0.2, goal: 0.1} + wrist_3_joint: {trajectory: 0.2, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 500 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur10e_controllers.yaml b/ur_rtde_driver/config/ur10e_controllers.yaml index 345f4f5..c100dfc 100644 --- a/ur_rtde_driver/config/ur10e_controllers.yaml +++ b/ur_rtde_driver/config/ur10e_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + wrist_2_joint: {trajectory: 0.2, goal: 0.1} + wrist_3_joint: {trajectory: 0.2, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 500 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur3_controllers.yaml b/ur_rtde_driver/config/ur3_controllers.yaml index 42bb62d..ebbd6be 100644 --- a/ur_rtde_driver/config/ur3_controllers.yaml +++ b/ur_rtde_driver/config/ur3_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + wrist_2_joint: {trajectory: 0.2, goal: 0.1} + wrist_3_joint: {trajectory: 0.2, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 500 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur3e_controllers.yaml b/ur_rtde_driver/config/ur3e_controllers.yaml index 345f4f5..c100dfc 100644 --- a/ur_rtde_driver/config/ur3e_controllers.yaml +++ b/ur_rtde_driver/config/ur3e_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + wrist_2_joint: {trajectory: 0.2, goal: 0.1} + wrist_3_joint: {trajectory: 0.2, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 500 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur5_controllers.yaml b/ur_rtde_driver/config/ur5_controllers.yaml index 42bb62d..ebbd6be 100644 --- a/ur_rtde_driver/config/ur5_controllers.yaml +++ b/ur_rtde_driver/config/ur5_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + wrist_2_joint: {trajectory: 0.2, goal: 0.1} + wrist_3_joint: {trajectory: 0.2, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 500 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur5e_controllers.yaml b/ur_rtde_driver/config/ur5e_controllers.yaml index 345f4f5..c100dfc 100644 --- a/ur_rtde_driver/config/ur5e_controllers.yaml +++ b/ur_rtde_driver/config/ur5e_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + wrist_2_joint: {trajectory: 0.2, goal: 0.1} + wrist_3_joint: {trajectory: 0.2, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 500 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/launch/ur10_bringup.launch b/ur_rtde_driver/launch/ur10_bringup.launch index b60464c..ef2d990 100644 --- a/ur_rtde_driver/launch/ur10_bringup.launch +++ b/ur_rtde_driver/launch/ur10_bringup.launch @@ -10,8 +10,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur10e_bringup.launch b/ur_rtde_driver/launch/ur10e_bringup.launch index dadd662..317b44c 100644 --- a/ur_rtde_driver/launch/ur10e_bringup.launch +++ b/ur_rtde_driver/launch/ur10e_bringup.launch @@ -11,8 +11,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur3_bringup.launch b/ur_rtde_driver/launch/ur3_bringup.launch index ea0ea84..3ecd9eb 100644 --- a/ur_rtde_driver/launch/ur3_bringup.launch +++ b/ur_rtde_driver/launch/ur3_bringup.launch @@ -10,8 +10,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur3e_bringup.launch b/ur_rtde_driver/launch/ur3e_bringup.launch index aef3632..02ff890 100644 --- a/ur_rtde_driver/launch/ur3e_bringup.launch +++ b/ur_rtde_driver/launch/ur3e_bringup.launch @@ -11,8 +11,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur5_bringup.launch b/ur_rtde_driver/launch/ur5_bringup.launch index a28b4ee..60a2363 100644 --- a/ur_rtde_driver/launch/ur5_bringup.launch +++ b/ur_rtde_driver/launch/ur5_bringup.launch @@ -10,8 +10,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur5e_bringup.launch b/ur_rtde_driver/launch/ur5e_bringup.launch index 783f916..ad46d76 100644 --- a/ur_rtde_driver/launch/ur5e_bringup.launch +++ b/ur_rtde_driver/launch/ur5e_bringup.launch @@ -11,8 +11,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur_common.launch b/ur_rtde_driver/launch/ur_common.launch index d591941..646d378 100644 --- a/ur_rtde_driver/launch/ur_common.launch +++ b/ur_rtde_driver/launch/ur_common.launch @@ -8,8 +8,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur_control.launch b/ur_rtde_driver/launch/ur_control.launch index 7c98798..e2a6d40 100644 --- a/ur_rtde_driver/launch/ur_control.launch +++ b/ur_rtde_driver/launch/ur_control.launch @@ -11,8 +11,8 @@ - - + + From a6a06526ac36225ddaa6e5b05385f7079693243b Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 27 Jun 2019 13:25:07 +0200 Subject: [PATCH 6/6] disable starting controllers when robot control is inactive Since we use the external controller stopper to take down controllers when robot control is inactive, we also do not want that users can start controllers while robot control is deactivated. The only exception is at startup time. --- .../ur_rtde_driver/ros/hardware_interface.h | 1 + ur_rtde_driver/src/ros/hardware_interface.cpp | 20 +++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index a4f0605..a8f8d58 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -133,6 +133,7 @@ protected: ros::Publisher program_state_pub_; bool controller_reset_necessary_; + bool controllers_initialized_; std::string robot_ip_; }; diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 2ea6bfc..e65edf6 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -47,6 +47,7 @@ HardwareInterface::HardwareInterface() , position_controller_running_(false) , pausing_state_(PausingState::RUNNING) , pausing_ramp_up_increment_(0.01) + , controllers_initialized_(false) { } @@ -301,8 +302,23 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio bool HardwareInterface ::prepareSwitch(const std::list& start_list, const std::list& stop_list) { - // TODO: Implement - return true; + bool ret_val = true; + if (controllers_initialized_ && !isRobotProgramRunning() && !start_list.empty()) + { + for (auto& controller : start_list) + { + if (!controller.claimed_resources.empty()) + { + ROS_ERROR_STREAM("Robot control is currently inactive. Starting controllers that claim resources is currently " + "not possible. Not starting controller '" + << controller.name << "'"); + ret_val = false; + } + } + } + + controllers_initialized_ = true; + return ret_val; } void HardwareInterface ::doSwitch(const std::list& start_list,