From 58361900b8682931f6b0ef91c109fbeb9d72a34e Mon Sep 17 00:00:00 2001 From: Jeppe Walther Date: Tue, 6 Oct 2015 17:14:59 +0200 Subject: [PATCH 01/31] UR3 Support --- config/ur3_controllers.yaml | 97 +++++++++++++++++++++++++ launch/ur3_bringup.launch | 29 ++++++++ launch/ur3_bringup_joint_limited.launch | 22 ++++++ launch/ur3_ros_control.launch | 42 +++++++++++ 4 files changed, 190 insertions(+) create mode 100644 config/ur3_controllers.yaml create mode 100644 launch/ur3_bringup.launch create mode 100644 launch/ur3_bringup_joint_limited.launch create mode 100644 launch/ur3_ros_control.launch diff --git a/config/ur3_controllers.yaml b/config/ur3_controllers.yaml new file mode 100644 index 0000000..8e26689 --- /dev/null +++ b/config/ur3_controllers.yaml @@ -0,0 +1,97 @@ +# Currently simply a copy of ur5_controllers.yaml + +# Settings for ros_control control loop +hardware_control_loop: + loop_hz: 125 + +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +# Publish all joint states ---------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 125 + +# Publish wrench ---------------------------------- +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 125 + +# Joint Trajectory Controller - position based ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +position_based_position_trajectory_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.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values have not been optimized!! + shoulder_pan_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + shoulder_lift_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + elbow_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_1_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_2_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_3_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + +# Joint Trajectory Controller - velocity based ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +velocity_based_position_trajectory_controller: + type: velocity_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.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values have not been optimized!! + shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch new file mode 100644 index 0000000..1b28f72 --- /dev/null +++ b/launch/ur3_bringup.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/ur3_bringup_joint_limited.launch b/launch/ur3_bringup_joint_limited.launch new file mode 100644 index 0000000..6d40005 --- /dev/null +++ b/launch/ur3_bringup_joint_limited.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch new file mode 100644 index 0000000..1c1cd89 --- /dev/null +++ b/launch/ur3_ros_control.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 589dca8e2a02e26ae03deec6785c0877f40d4f6a Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 15:37:49 +0200 Subject: [PATCH 02/31] Improved ros_control performance and stability --- README.md | 10 +++- config/ur10_controllers.yaml | 55 ++++++++++++++++--- config/ur5_controllers.yaml | 18 +++--- .../ur_modern_driver/ur_hardware_interface.h | 8 ++- launch/ur10_ros_control.launch | 6 +- launch/ur5_ros_control.launch | 4 +- src/ur_hardware_interface.cpp | 29 +++++++++- src/ur_ros_wrapper.cpp | 8 +++ 8 files changed, 108 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index 8748d54..88a3acd 100644 --- a/README.md +++ b/README.md @@ -28,9 +28,13 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Added support for ros_control. * As ros_control wants to have control over the robot at all times, ros_control compatability is set via a parameter at launch-time. * With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead. + * Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution + * The velocity based controller sends joint speed commands to the robot, using the speej command + * The position based controller send joint position commands to the robot, using the servoj command + * I have so far only used the velocity based controller, but which one is optimal depends on the application. * As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant: * A list of the loaded and running controllers can be found by a call to the controller_manager ```rosservice call /controller_manager/list_controllers {} ``` - * The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'position_based_position_trajectory_controller' strictness: 1" ``` (Remember you can use tab-completion for this) + * The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" ``` (Remember you can use tab-completion for this) ## Installation @@ -61,12 +65,12 @@ Be sure to stop the currently running controller **either before or in the same The position based controller *should* stay closer to the commanded path, while the velocity based react faster (trajectory execution start within 50-70 ms, while it is in the 150-180ms range for the position_based. Usage without ros_control as well as the old driver is also in the 170ms range, as mentioned at my lightning talk @ ROSCon 2013). -**Note** that the PID values are not tweaked as of this moment. +**Note** that the PID values are not optimally tweaked as of this moment. To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following ``` controller_list: - - name: velocity_based_position_trajectory_controller #or position_based_position_trajectory_controller + - name: vel_based_pos_traj_controller #or pos_based_pos_traj_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml index dd679bc..8c0c5bc 100644 --- a/config/ur10_controllers.yaml +++ b/config/ur10_controllers.yaml @@ -22,10 +22,10 @@ force_torque_sensor_controller: type: force_torque_sensor_controller/ForceTorqueSensorController publish_rate: 125 -# Joint Trajectory Controller ------------------------------- +# Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -velocity_based_position_trajectory_controller: - type: velocity_controllers/JointTrajectoryController +pos_based_pos_traj_controller: + type: position_controllers/JointTrajectoryController joints: - shoulder_pan_joint - shoulder_lift_joint @@ -47,12 +47,49 @@ velocity_based_position_trajectory_controller: action_monitor_rate: 10 gains: #!!These values have not been optimized!! - shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_pan_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + shoulder_lift_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + elbow_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_1_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_2_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_3_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +vel_based_pos_traj_controller: + type: velocity_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.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values are useable, but maybe not optimal + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_3_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + # state_publish_rate: 50 # Defaults to 50 # action_monitor_rate: 20 # Defaults to 20 diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml index 57591ca..a1054b7 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -24,7 +24,7 @@ force_torque_sensor_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -position_based_position_trajectory_controller: +pos_based_pos_traj_controller: type: position_controllers/JointTrajectoryController joints: - shoulder_pan_joint @@ -60,7 +60,7 @@ position_based_position_trajectory_controller: # Joint Trajectory Controller - velocity based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -velocity_based_position_trajectory_controller: +vel_based_pos_traj_controller: type: velocity_controllers/JointTrajectoryController joints: - shoulder_pan_joint @@ -82,13 +82,13 @@ velocity_based_position_trajectory_controller: state_publish_rate: 125 action_monitor_rate: 10 gains: - #!!These values have not been optimized!! - shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} - wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + #!!These values are useable, but maybe not optimal + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + wrist_3_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} # state_publish_rate: 50 # Defaults to 50 # action_monitor_rate: 20 # Defaults to 20 diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index c2de2d7..ff9572a 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -58,6 +58,7 @@ #include #include #include +#include #include "do_output.h" #include "ur_driver.h" @@ -86,6 +87,8 @@ public: /// \brief write the command to the robot hardware. virtual void write(); + void setMaxVelChange(double inp); + bool canSwitch( const std::list &start_list, const std::list &stop_list) const; @@ -111,10 +114,13 @@ protected: std::vector joint_effort_; std::vector joint_position_command_; std::vector joint_velocity_command_; - std::size_t num_joints_; + std::vector prev_joint_velocity_command_; + std::size_t num_joints_; double robot_force_[3] = { 0., 0., 0. }; double robot_torque_[3] = { 0., 0., 0. }; + double max_vel_change_; + // Robot API UrDriver* robot_; diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index f570298..d60d02f 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -29,13 +29,13 @@ + output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" /> + output="screen" args="load pos_based_pos_traj_controller" /> - \ No newline at end of file + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index b03a107..b81b017 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -30,11 +30,11 @@ + output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" /> + output="screen" args="load pos_based_pos_traj_controller" /> diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 60dfadc..0222ede 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -57,6 +57,8 @@ UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : // Initialize shared memory and interfaces here init(); // this implementation loads from rosparam + max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); } @@ -79,6 +81,7 @@ void UrHardwareInterface::init() { joint_effort_.resize(num_joints_); joint_position_command_.resize(num_joints_); joint_velocity_command_.resize(num_joints_); + prev_joint_velocity_command_.resize(num_joints_); // Initialize controller for (std::size_t i = 0; i < num_joints_; ++i) { @@ -102,6 +105,7 @@ void UrHardwareInterface::init() { hardware_interface::JointHandle( joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i])); + prev_joint_velocity_command_[i] = 0.; } // Create force torque interface @@ -135,11 +139,30 @@ void UrHardwareInterface::read() { } +void UrHardwareInterface::setMaxVelChange(double inp) { + max_vel_change_ = inp; +} + void UrHardwareInterface::write() { if (velocity_interface_running_) { - robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1], - joint_velocity_command_[2], joint_velocity_command_[3], - joint_velocity_command_[4], joint_velocity_command_[5], 100); + std::vector cmd; + //do some rate limiting + cmd.resize(joint_velocity_command_.size()); + for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) { + cmd[i] = joint_velocity_command_[i]; + if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; + } else if (cmd[i] + < prev_joint_velocity_command_[i] - max_vel_change_) { + cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; + } + if (cmd[i] > M_PI/2.) + cmd[i] = M_PI/2.; + else if (cmd[i] < -M_PI/2.) + cmd[i] = -M_PI/2.; + prev_joint_velocity_command_[i] = cmd[i]; + } + robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125); } else if (position_interface_running_) { robot_->servoj(joint_position_command_); } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 2035161..eabfdbd 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -129,6 +129,14 @@ public: } robot_.setServojTime(servoj_time); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) { + max_vel_change = max_vel_change/125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change*125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); + if (robot_.start()) { if (use_ros_control_) { ros_control_thread_ = new std::thread( From 1d54bcca1d4d6fcc5881c1a747b014c38abc041a Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 15:47:39 +0200 Subject: [PATCH 03/31] Stop sending data to servoj when stopTraj is called --- include/ur_modern_driver/ur_driver.h | 1 + src/ur_driver.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index de74a82..e26f869 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -40,6 +40,7 @@ private: int incoming_sockfd_; int new_sockfd_; double servoj_time_; + bool executing_traj_; public: UrRealtimeCommunication* rt_interface_; UrCommunication* sec_interface_; diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index c141ec3..6b72008 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -126,13 +126,14 @@ void UrDriver::doTraj(std::vector inp_timestamps, std::vector positions; unsigned int j; + executing_traj_ = true; UrDriver::uploadProg(); t0 = std::chrono::high_resolution_clock::now(); t = t0; j = 0; - while (inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast>(t - t0).count()) { + while ((inp_timestamps[inp_timestamps.size() - 1] + >= std::chrono::duration_cast>(t - t0).count()) and executing_traj_) { while (inp_timestamps[j] <= std::chrono::duration_cast>( t - t0).count() && j < inp_timestamps.size() - 1) { @@ -151,6 +152,7 @@ void UrDriver::doTraj(std::vector inp_timestamps, t = std::chrono::high_resolution_clock::now(); } //Signal robot to stop driverProg() + executing_traj_ = false; UrDriver::closeServo(positions); } @@ -183,6 +185,7 @@ void UrDriver::servoj(std::vector positions, } void UrDriver::stopTraj() { + executing_traj_ = false; rt_interface_->addCommandToQueue("stopj(10)\n"); } From 52b68cbd9cc5f71ebeb515fceefef816d3d14945 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 15:48:05 +0200 Subject: [PATCH 04/31] Stop sending data to servoj when stopTraj is called --- src/ur_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 6b72008..cd85423 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -152,7 +152,6 @@ void UrDriver::doTraj(std::vector inp_timestamps, t = std::chrono::high_resolution_clock::now(); } //Signal robot to stop driverProg() - executing_traj_ = false; UrDriver::closeServo(positions); } @@ -281,6 +280,7 @@ void UrDriver::closeServo(std::vector positions) { UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); else UrDriver::servoj(positions, 0); + executing_traj_ = false; close(new_sockfd_); } From d8b8d86ecb490800ae4e1b0a52c1ace12ef6fe2a Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 15:50:22 +0200 Subject: [PATCH 05/31] Changed minimum servoj time. Fixes #3 --- src/ur_driver.cpp | 4 ++-- src/ur_ros_wrapper.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index cd85423..c02ffdf 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -369,9 +369,9 @@ void UrDriver::setMaxPayload(double m) { maximum_payload_ = m; } void UrDriver::setServojTime(double t) { - if (t > 0.016) { + if (t > 0.008) { servoj_time_ = t; } else { - servoj_time_ = 0.016; + servoj_time_ = 0.008; } } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 8af68bc..8b1c0af 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -122,7 +122,7 @@ public: min_payload, max_payload); print_debug(buf); - double servoj_time = 0.016; + double servoj_time = 0.008; if (ros::param::get("~servoj_time", servoj_time)) { sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); print_debug(buf); From d4102dd97dbb6a0366f5967c571821433959cb7a Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 17:01:37 +0200 Subject: [PATCH 06/31] Changed from simpleActionServer to Actionserver. Fixed bug that prevented starting the driver in non-ros-control mode --- src/ur_ros_wrapper.cpp | 67 ++++++++++++++++++++++-------------------- 1 file changed, 35 insertions(+), 32 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 8b1c0af..ba135d1 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -26,7 +26,8 @@ #include "sensor_msgs/JointState.h" #include "geometry_msgs/WrenchStamped.h" #include "control_msgs/FollowJointTrajectoryAction.h" -#include "actionlib/server/simple_action_server.h" +#include "actionlib/server/action_server.h" +#include "actionlib/server/server_goal_handle.h" #include "trajectory_msgs/JointTrajectoryPoint.h" #include "ur_msgs/SetIO.h" #include "ur_msgs/SetPayload.h" @@ -46,8 +47,8 @@ protected: std::condition_variable rt_msg_cond_; std::condition_variable msg_cond_; ros::NodeHandle nh_; - actionlib::SimpleActionServer as_; - actionlib::SimpleActionServer::Goal goal_; + actionlib::ActionServer as_; + actionlib::ActionServer::Goal goal_; control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; ros::Subscriber speed_sub_; @@ -66,9 +67,11 @@ protected: public: RosWrapper(std::string host) : - as_(nh_, "follow_joint_trajectory", false), robot_(rt_msg_cond_, - msg_cond_, host), io_flag_delay_(0.05), joint_offsets_(6, - 0.0) { + as_(nh_, "follow_joint_trajectory", + boost::bind(&RosWrapper::goalCB, this, _1), + boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( + rt_msg_cond_, msg_cond_, host), io_flag_delay_(0.05), joint_offsets_( + 6, 0.0) { std::string joint_prefix = ""; std::vector joint_names; @@ -95,6 +98,14 @@ public: controller_manager_.reset( new controller_manager::ControllerManager( hardware_interface_.get(), nh_)); + double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 + if (ros::param::get("~max_acceleration", max_vel_change)) { + max_vel_change = max_vel_change / 125; + } + sprintf(buf, "Max acceleration set to: %f [rad/sec²]", + max_vel_change * 125); + print_debug(buf); + hardware_interface_->setMaxVelChange(max_vel_change); } //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! max_velocity_ = 10.; @@ -129,14 +140,6 @@ public: } robot_.setServojTime(servoj_time); - double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - if (ros::param::get("~max_acceleration", max_vel_change)) { - max_vel_change = max_vel_change/125; - } - sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change*125); - print_debug(buf); - hardware_interface_->setMaxVelChange(max_vel_change); - if (robot_.start()) { if (use_ros_control_) { ros_control_thread_ = new std::thread( @@ -144,12 +147,7 @@ public: print_debug( "The control thread for this driver has been started"); } else { - //register the goal and feedback callbacks - as_.registerGoalCallback( - boost::bind(&RosWrapper::goalCB, this)); - as_.registerPreemptCallback( - boost::bind(&RosWrapper::preemptCB, this)); - + //start actionserver as_.start(); //subscribe to the data topic of interest @@ -178,12 +176,12 @@ public: } private: - void goalCB() { + void goalCB( + actionlib::ServerGoalHandle< + control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_goal"); - actionlib::SimpleActionServer::GoalConstPtr goal = - as_.acceptNewGoal(); - goal_ = *goal; //make a copy that we can modify + goal_ = *gh.getGoal(); //make a copy that we can modify if (!validateJointNames()) { std::string outp_joint_names = ""; for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); @@ -194,28 +192,28 @@ private: result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); } if (!has_positions()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without positions"; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); } if (!has_velocities()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without velocities"; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); } if (!traj_is_finite()) { result_.error_string = "Received a goal with infinites or NaNs"; result_.error_code = result_.INVALID_GOAL; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); } @@ -223,7 +221,7 @@ private: result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal with velocities that are higher than %f", max_velocity_; - as_.setAborted(result_, result_.error_string); + gh.setRejected(result_, result_.error_string); print_error(result_.error_string); } @@ -237,16 +235,21 @@ private: velocities.push_back(goal_.trajectory.points[i].velocities); } + gh.setAccepted(); robot_.doTraj(timestamps, positions, velocities); result_.error_code = result_.SUCCESSFUL; - as_.setSucceeded(result_); + gh.setSucceeded(result_); } - void preemptCB() { + void cancelCB( + actionlib::ServerGoalHandle< + control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_cancel"); // set the action state to preempted robot_.stopTraj(); - as_.setPreempted(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Goal cancelled by client"; + gh.setCanceled(result_); } bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) { From e970a99b017c654918f84b411ce61103fa811ea0 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 17:04:31 +0200 Subject: [PATCH 07/31] fixed typo in error string --- src/ur_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ba135d1..f1620ca 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -211,7 +211,7 @@ private: } if (!traj_is_finite()) { - result_.error_string = "Received a goal with infinites or NaNs"; + result_.error_string = "Received a goal with infinities or NaNs"; result_.error_code = result_.INVALID_GOAL; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); From e354ada3abb400fd36f73d9e413993f4c224c3c0 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 8 Oct 2015 17:23:19 +0200 Subject: [PATCH 08/31] changed to use goal_handle_ instead of goal_ --- src/ur_ros_wrapper.cpp | 61 +++++++++++++++++++++++------------------- 1 file changed, 33 insertions(+), 28 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index f1620ca..92c1e1b 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -48,7 +48,7 @@ protected: std::condition_variable msg_cond_; ros::NodeHandle nh_; actionlib::ActionServer as_; - actionlib::ActionServer::Goal goal_; + actionlib::ServerGoalHandle* goal_handle_; control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; ros::Subscriber speed_sub_; @@ -180,13 +180,13 @@ private: actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_goal"); - - goal_ = *gh.getGoal(); //make a copy that we can modify + goal_handle_ = &gh; + actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); //make a copy that we can modify if (!validateJointNames()) { std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { - outp_joint_names += goal_.trajectory.joint_names[i] + " "; + outp_joint_names += goal.trajectory.joint_names[i] + " "; } result_.error_code = result_.INVALID_JOINTS; result_.error_string = @@ -225,14 +225,14 @@ private: print_error(result_.error_string); } - reorder_traj_joints(goal_.trajectory); + reorder_traj_joints(goal.trajectory); std::vector timestamps; std::vector > positions, velocities; - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { timestamps.push_back( - goal_.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal_.trajectory.points[i].positions); - velocities.push_back(goal_.trajectory.points[i].velocities); + goal.trajectory.points[i].time_from_start.toSec()); + positions.push_back(goal.trajectory.points[i].positions); + velocities.push_back(goal.trajectory.points[i].velocities); } gh.setAccepted(); @@ -285,16 +285,17 @@ private: bool validateJointNames() { std::vector actual_joint_names = robot_.getJointNames(); - if (goal_.trajectory.joint_names.size() != actual_joint_names.size()) + actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + if (goal.trajectory.joint_names.size() != actual_joint_names.size()) return false; - for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); i++) { + for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { unsigned int j; for (j = 0; j < actual_joint_names.size(); j++) { - if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) break; } - if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) { + if (goal.trajectory.joint_names[i] == actual_joint_names[j]) { actual_joint_names.erase(actual_joint_names.begin() + j); } else { return false; @@ -335,30 +336,33 @@ private: } bool has_velocities() { - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { - if (goal_.trajectory.points[i].positions.size() - != goal_.trajectory.points[i].velocities.size()) + actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.points[i].velocities.size()) return false; } return true; } bool has_positions() { - if (goal_.trajectory.points.size() == 0) + actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + if (goal.trajectory.points.size() == 0) return false; - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { - if (goal_.trajectory.points[i].positions.size() - != goal_.trajectory.joint_names.size()) + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { + if (goal.trajectory.points[i].positions.size() + != goal.trajectory.joint_names.size()) return false; } return true; } bool has_limited_velocities() { - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; - j < goal_.trajectory.points[i].velocities.size(); j++) { - if (fabs(goal_.trajectory.points[i].velocities[j]) + j < goal.trajectory.points[i].velocities.size(); j++) { + if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_) return false; } @@ -367,12 +371,13 @@ private: } bool traj_is_finite() { - for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { + actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; - j < goal_.trajectory.points[i].velocities.size(); j++) { - if (!std::isfinite(goal_.trajectory.points[i].positions[j])) + j < goal.trajectory.points[i].velocities.size(); j++) { + if (!std::isfinite(goal.trajectory.points[i].positions[j])) return false; - if (!std::isfinite(goal_.trajectory.points[i].velocities[j])) + if (!std::isfinite(goal.trajectory.points[i].velocities[j])) return false; } } From 80e344d167337a5d0153021b6bb03afa68405cbf Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 9 Oct 2015 10:47:29 +0200 Subject: [PATCH 09/31] Fixed an issue where canceling a trajectory wouldn't stop the robot. --- src/ur_driver.cpp | 125 +++++++++++++++++++++-------------------- src/ur_ros_wrapper.cpp | 51 ++++++++++++----- 2 files changed, 101 insertions(+), 75 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index c02ffdf..acd5703 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -13,15 +13,17 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, - double max_time_step, double min_payload, double max_payload) : + unsigned int reverse_port, double servoj_time, + unsigned int safety_count_max, double max_time_step, double min_payload, + double max_payload) : REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( - min_payload), maximum_payload_(max_payload), servoj_time_(servoj_time) { + min_payload), maximum_payload_(max_payload), servoj_time_( + servoj_time) { char buffer[256]; struct sockaddr_in serv_addr; int n, flag; //char *ip_addr; - + executing_traj_ = false; rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); new_sockfd_ = -1; @@ -63,77 +65,76 @@ std::vector UrDriver::interp_cubic(double t, double T, } return positions; } - /* -void UrDriver::addTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) { - // DEPRECATED - printf("!! addTraj is deprecated !!\n"); - std::vector timestamps; - std::vector > positions; - std::string command_string = "def traj():\n"; +/* + void UrDriver::addTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities) { + // DEPRECATED + printf("!! addTraj is deprecated !!\n"); + std::vector timestamps; + std::vector > positions; + std::string command_string = "def traj():\n"; - for (unsigned int i = 1; i < inp_timestamps.size(); i++) { - timestamps.push_back(inp_timestamps[i - 1]); - double dt = inp_timestamps[i] - inp_timestamps[i - 1]; - unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_); - double step_size = dt / steps; - for (unsigned int j = 1; j < steps; j++) { - timestamps.push_back(inp_timestamps[i - 1] + step_size * j); - } - } - // //make sure we come to a smooth stop - // while (timestamps.back() < inp_timestamps.back()) { - // timestamps.push_back(timestamps.back() + 0.008); - // } - // timestamps.pop_back(); + for (unsigned int i = 1; i < inp_timestamps.size(); i++) { + timestamps.push_back(inp_timestamps[i - 1]); + double dt = inp_timestamps[i] - inp_timestamps[i - 1]; + unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_); + double step_size = dt / steps; + for (unsigned int j = 1; j < steps; j++) { + timestamps.push_back(inp_timestamps[i - 1] + step_size * j); + } + } + // //make sure we come to a smooth stop + // while (timestamps.back() < inp_timestamps.back()) { + // timestamps.push_back(timestamps.back() + 0.008); + // } + // timestamps.pop_back(); - unsigned int j = 0; - for (unsigned int i = 0; i < timestamps.size(); i++) { - while (inp_timestamps[j] <= timestamps[i]) { - j += 1; - } - positions.push_back( - UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], - inp_positions[j - 1], inp_positions[j], - inp_velocities[j - 1], inp_velocities[j])); - } + unsigned int j = 0; + for (unsigned int i = 0; i < timestamps.size(); i++) { + while (inp_timestamps[j] <= timestamps[i]) { + j += 1; + } + positions.push_back( + UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1], + inp_timestamps[j] - inp_timestamps[j - 1], + inp_positions[j - 1], inp_positions[j], + inp_velocities[j - 1], inp_velocities[j])); + } - timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); - positions.push_back(inp_positions[inp_positions.size() - 1]); - /// This is actually faster than using a stringstream :-o - for (unsigned int i = 1; i < timestamps.size(); i++) { - char buf[128]; - sprintf(buf, - "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n", - positions[i][0], positions[i][1], positions[i][2], - positions[i][3], positions[i][4], positions[i][5], - timestamps[i] - timestamps[i - 1]); - command_string += buf; - } - command_string += "end\n"; - //printf("%s", command_string.c_str()); - rt_interface_->addCommandToQueue(command_string); + timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); + positions.push_back(inp_positions[inp_positions.size() - 1]); + /// This is actually faster than using a stringstream :-o + for (unsigned int i = 1; i < timestamps.size(); i++) { + char buf[128]; + sprintf(buf, + "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n", + positions[i][0], positions[i][1], positions[i][2], + positions[i][3], positions[i][4], positions[i][5], + timestamps[i] - timestamps[i - 1]); + command_string += buf; + } + command_string += "end\n"; + //printf("%s", command_string.c_str()); + rt_interface_->addCommandToQueue(command_string); -} -*/ + } + */ void UrDriver::doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { - std::chrono::high_resolution_clock::time_point t0, t; std::vector positions; unsigned int j; executing_traj_ = true; UrDriver::uploadProg(); - t0 = std::chrono::high_resolution_clock::now(); t = t0; j = 0; while ((inp_timestamps[inp_timestamps.size() - 1] - >= std::chrono::duration_cast>(t - t0).count()) and executing_traj_) { + >= std::chrono::duration_cast>(t - t0).count()) + and executing_traj_) { while (inp_timestamps[j] <= std::chrono::duration_cast>( t - t0).count() && j < inp_timestamps.size() - 1) { @@ -144,19 +145,19 @@ void UrDriver::doTraj(std::vector inp_timestamps, t - t0).count() - inp_timestamps[j - 1], inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); // oversample with 4 * sample_time - std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_*1000)/4.))); + std::this_thread::sleep_for( + std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); t = std::chrono::high_resolution_clock::now(); } //Signal robot to stop driverProg() UrDriver::closeServo(positions); } -void UrDriver::servoj(std::vector positions, - int keepalive, double time) { +void UrDriver::servoj(std::vector positions, int keepalive, + double time) { unsigned int bytes_written; int tmp; unsigned char buf[32]; diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 92c1e1b..494e6c1 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -148,6 +148,7 @@ public: "The control thread for this driver has been started"); } else { //start actionserver + goal_handle_ = NULL; as_.start(); //subscribe to the data topic of interest @@ -180,8 +181,13 @@ private: actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_goal"); + actionlib::ActionServer::Goal goal = + *gh.getGoal(); //make a copy that we can modify + if (goal_handle_ != NULL) { + RosWrapper::cancelCB(*goal_handle_); + } goal_handle_ = &gh; - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); //make a copy that we can modify + if (!validateJointNames()) { std::string outp_joint_names = ""; for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); @@ -195,7 +201,6 @@ private: gh.setRejected(result_, result_.error_string); print_error(result_.error_string); } - if (!has_positions()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without positions"; @@ -226,8 +231,15 @@ private: } reorder_traj_joints(goal.trajectory); + std::vector timestamps; std::vector > positions, velocities; + if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { + print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); + timestamps.push_back(0.0); + positions.push_back(robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual()); + } for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { timestamps.push_back( goal.trajectory.points[i].time_from_start.toSec()); @@ -235,10 +247,14 @@ private: velocities.push_back(goal.trajectory.points[i].velocities); } - gh.setAccepted(); + + goal_handle_->setAccepted(); robot_.doTraj(timestamps, positions, velocities); - result_.error_code = result_.SUCCESSFUL; - gh.setSucceeded(result_); + if (goal_handle_ != NULL) { + result_.error_code = result_.SUCCESSFUL; + goal_handle_->setSucceeded(result_); + goal_handle_ = NULL; + } } void cancelCB( @@ -246,9 +262,13 @@ private: control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_cancel"); // set the action state to preempted - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Goal cancelled by client"; + if (goal_handle_ != NULL) { + print_info("Stopping previous traj"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Goal cancelled by client"; + goal_handle_ = NULL; + } gh.setCanceled(result_); } @@ -285,7 +305,8 @@ private: bool validateJointNames() { std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); if (goal.trajectory.joint_names.size() != actual_joint_names.size()) return false; @@ -336,7 +357,8 @@ private: } bool has_velocities() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size()) @@ -346,7 +368,8 @@ private: } bool has_positions() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); if (goal.trajectory.points.size() == 0) return false; for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { @@ -358,7 +381,8 @@ private: } bool has_limited_velocities() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) { @@ -371,7 +395,8 @@ private: } bool traj_is_finite() { - actionlib::ActionServer::Goal goal = *goal_handle_->getGoal(); + actionlib::ActionServer::Goal goal = + *goal_handle_->getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) { From d8602c2246bf79556eef2340b617c0a1c95d124c Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 9 Oct 2015 13:18:38 +0200 Subject: [PATCH 10/31] Added detection of E-stop and protective stop. --- include/ur_modern_driver/robot_state.h | 31 ++++++- src/robot_state.cpp | 108 +++++++++++++++++++++++-- src/ur_communication.cpp | 1 + src/ur_ros_wrapper.cpp | 71 ++++++++++++++-- 4 files changed, 199 insertions(+), 12 deletions(-) diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 2a2b077..48d390f 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -88,13 +88,28 @@ struct masterboard_data { int euromapOutputBits; float euromapVoltage; float euromapCurrent; +}; +struct robot_mode_data { + uint64_t timestamp; + bool isRobotConnected; + bool isRealRobotEnabled; + bool isPowerOnRobot; + bool isEmergencyStopped; + bool isProtectiveStopped; + bool isProgramRunning; + bool isProgramPaused; + unsigned char robotMode; + unsigned char controlMode; + double targetSpeedFraction; + double speedScaling; }; class RobotState { private: version_message version_msg_; masterboard_data mb_data_; + robot_mode_data robot_mode_; std::recursive_mutex val_lock_; // Locks the variables while unpack parses data; @@ -119,6 +134,7 @@ public: char getAnalogOutputDomain1(); double getAnalogOutput0(); double getAnalogOutput1(); + std::vector getVActual(); float getMasterBoardTemperature(); float getRobotVoltage48V(); float getRobotCurrent(); @@ -130,16 +146,27 @@ public: int getEuromapOutputBits(); float getEuromapVoltage(); float getEuromapCurrent(); + + bool isRobotConnected(); + bool isRealRobotEnabled(); + bool isPowerOnRobot(); + bool isEmergencyStopped(); + bool isProtectiveStopped(); + bool isProgramRunning(); + bool isProgramPaused(); + + void setDisconnected(); + bool getNewDataAvailable(); void finishedReading(); - std::vector getVActual(); + void unpack(uint8_t * buf, unsigned int buf_length); void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len); void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, uint32_t len); - void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len); void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset); + void unpackRobotMode(uint8_t * buf, unsigned int offset); }; #endif /* ROBOT_STATE_H_ */ diff --git a/src/robot_state.cpp b/src/robot_state.cpp index d56f7ca..ed5928e 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -12,6 +12,7 @@ RobotState::RobotState(std::condition_variable& msg_cond) { version_msg_.minor_version = 0; new_data_available_ = false; pMsg_cond_ = &msg_cond; + RobotState::setDisconnected(); } double RobotState::ntohd(uint64_t nf) { double x; @@ -28,7 +29,7 @@ void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { memcpy(&len, &buf[offset], sizeof(len)); len = ntohl(len); if (len + offset > buf_length) { - return ; + return; } memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); switch (message_type) { @@ -63,7 +64,7 @@ void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, switch (robot_message_type) { case robotMessageType::ROBOT_MESSAGE_VERSION: val_lock_.lock(); - version_msg_.timestamp = RobotState::ntohd(timestamp); + version_msg_.timestamp = timestamp; version_msg_.source = source; version_msg_.robot_message_type = robot_message_type; RobotState::unpackRobotMessageVersion(buf, offset, len); @@ -83,11 +84,18 @@ void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset, uint8_t package_type; memcpy(&length, &buf[offset], sizeof(length)); length = ntohl(length); - memcpy(&package_type, &buf[offset+sizeof(length)], sizeof(package_type)); + memcpy(&package_type, &buf[offset + sizeof(length)], + sizeof(package_type)); switch (package_type) { + case packageType::ROBOT_MODE_DATA: + val_lock_.lock(); + RobotState::unpackRobotMode(buf, offset + 5); + val_lock_.unlock(); + break; + case packageType::MASTERBOARD_DATA: val_lock_.lock(); - RobotState::unpackRobotStateMasterboard(buf, offset+5); + RobotState::unpackRobotStateMasterboard(buf, offset + 5); val_lock_.unlock(); break; default: @@ -123,10 +131,73 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, version_msg_.build_date[len - offset] = '\0'; } +void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) { + memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); + offset += sizeof(robot_mode_.timestamp); + uint8_t tmp; + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRobotConnected = true; + else + robot_mode_.isRobotConnected = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isRealRobotEnabled = true; + else + robot_mode_.isRealRobotEnabled = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + //printf("PowerOnRobot: %d\n", tmp); + if (tmp > 0) + robot_mode_.isPowerOnRobot = true; + else + robot_mode_.isPowerOnRobot = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isEmergencyStopped = true; + else + robot_mode_.isEmergencyStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProtectiveStopped = true; + else + robot_mode_.isProtectiveStopped = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramRunning = true; + else + robot_mode_.isProgramRunning = false; + offset += sizeof(tmp); + memcpy(&tmp, &buf[offset], sizeof(tmp)); + if (tmp > 0) + robot_mode_.isProgramPaused = true; + else + robot_mode_.isProgramPaused = false; + offset += sizeof(tmp); + memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); + offset += sizeof(robot_mode_.robotMode); + uint64_t temp; + if (RobotState::getVersion() > 2.) { + memcpy(&robot_mode_.controlMode, &buf[offset], + sizeof(robot_mode_.controlMode)); + offset += sizeof(robot_mode_.controlMode); + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); + } + memcpy(&temp, &buf[offset], sizeof(temp)); + offset += sizeof(temp); + robot_mode_.speedScaling = RobotState::ntohd(temp); +} + void RobotState::unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset) { if (RobotState::getVersion() < 3.0) { - int16_t digital_input_bits, digital_output_bits; + int16_t digital_input_bits, digital_output_bits; memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); offset += sizeof(digital_input_bits); memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); @@ -263,3 +334,30 @@ double RobotState::getAnalogOutput0() { double RobotState::getAnalogOutput1() { return mb_data_.analogOutput1; } +bool RobotState::isRobotConnected() { + return robot_mode_.isRobotConnected; +} +bool RobotState::isRealRobotEnabled() { + return robot_mode_.isRealRobotEnabled; +} +bool RobotState::isPowerOnRobot() { + return robot_mode_.isPowerOnRobot; +} +bool RobotState::isEmergencyStopped() { + return robot_mode_.isEmergencyStopped; +} +bool RobotState::isProtectiveStopped() { + return robot_mode_.isProtectiveStopped; +} +bool RobotState::isProgramRunning() { + return robot_mode_.isProgramRunning; +} +bool RobotState::isProgramPaused() { + return robot_mode_.isProgramPaused; +} + +void RobotState::setDisconnected() { + robot_mode_.isRobotConnected = false; + robot_mode_.isRealRobotEnabled = false; + robot_mode_.isPowerOnRobot = false; +} diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 3b81892..09954ed 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -122,6 +122,7 @@ void UrCommunication::run() { robot_state_->unpack(buf, bytes_read); } else { connected_ = false; + robot_state_->setDisconnected(); close(sec_sockfd_); } } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 494e6c1..ca34236 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -181,6 +181,38 @@ private: actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_goal"); + if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { + result_.error_code = -100; + result_.error_string = + "Cannot accept new trajectories: Robot arm is not powered on"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { + result_.error_code = -100; + result_.error_string = + "Cannot accept new trajectories: Robot is not enabled"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { + result_.error_code = -100; + result_.error_string = + "Cannot accept new trajectories: Robot is emergency stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + result_.error_code = -100; + result_.error_string = + "Cannot accept new trajectories: Robot is protective stopped"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify if (goal_handle_ != NULL) { @@ -200,12 +232,14 @@ private: + outp_joint_names; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } if (!has_positions()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without positions"; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } if (!has_velocities()) { @@ -213,6 +247,7 @@ private: result_.error_string = "Received a goal without velocities"; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } if (!traj_is_finite()) { @@ -220,6 +255,7 @@ private: result_.error_code = result_.INVALID_GOAL; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } if (!has_limited_velocities()) { @@ -228,6 +264,7 @@ private: "Received a goal with velocities that are higher than %f", max_velocity_; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); + return; } reorder_traj_joints(goal.trajectory); @@ -235,10 +272,13 @@ private: std::vector timestamps; std::vector > positions, velocities; if (goal.trajectory.points[0].time_from_start.toSec() != 0.) { - print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); + print_warning( + "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory"); timestamps.push_back(0.0); - positions.push_back(robot_.rt_interface_->robot_state_->getQActual()); - velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual()); + positions.push_back( + robot_.rt_interface_->robot_state_->getQActual()); + velocities.push_back( + robot_.rt_interface_->robot_state_->getQdActual()); } for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { timestamps.push_back( @@ -260,10 +300,9 @@ private: void cancelCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { - print_info("on_cancel"); // set the action state to preempted + print_info("on_cancel"); if (goal_handle_ != NULL) { - print_info("Stopping previous traj"); robot_.stopTraj(); result_.error_code = result_.SUCCESSFUL; result_.error_string = "Goal cancelled by client"; @@ -500,6 +539,7 @@ private: } void publishMbMsg() { + bool warned = false; ros::Publisher io_pub = nh_.advertise( "ur_driver/io_states", 1); @@ -541,6 +581,27 @@ private: io_msg.analog_out_states.push_back(ana); io_pub.publish(io_msg); + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + or robot_.sec_interface_->robot_state_->isProtectiveStopped()) { + if (robot_.sec_interface_->robot_state_->isEmergencyStopped() + and !warned) { + print_error("Emergency stop pressed!"); + } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() + and !warned) { + print_error("Robot is protective stopped!"); + } + if (goal_handle_ != NULL) { + print_error("Aborting trajectory"); + robot_.stopTraj(); + result_.error_code = result_.SUCCESSFUL; + result_.error_string = "Robot was halted"; + goal_handle_->setAborted(result_, result_.error_string); + goal_handle_ = NULL; + } + warned = true; + } else + warned = false; + robot_.sec_interface_->robot_state_->finishedReading(); } From 6b6ded461be4824ec9b6ea62894b9626ddc1b816 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 9 Oct 2015 14:39:24 +0200 Subject: [PATCH 11/31] Improved goal handling. If the robot receives a new goal before the previous is succeeded, the previous will be aborted --- src/ur_ros_wrapper.cpp | 80 ++++++++++++++++++++++++++---------------- 1 file changed, 49 insertions(+), 31 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ca34236..3164653 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "ros/ros.h" @@ -48,7 +49,8 @@ protected: std::condition_variable msg_cond_; ros::NodeHandle nh_; actionlib::ActionServer as_; - actionlib::ServerGoalHandle* goal_handle_; + actionlib::ServerGoalHandle goal_handle_; + bool has_goal_; control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; ros::Subscriber speed_sub_; @@ -148,7 +150,7 @@ public: "The control thread for this driver has been started"); } else { //start actionserver - goal_handle_ = NULL; + has_goal_ = false; as_.start(); //subscribe to the data topic of interest @@ -177,12 +179,23 @@ public: } private: + void trajThread(std::vector timestamps, + std::vector > positions, + std::vector > velocities) { + + robot_.doTraj(timestamps, positions, velocities); + if (has_goal_) { + result_.error_code = result_.SUCCESSFUL; + goal_handle_.setSucceeded(result_); + has_goal_ = false; + } + } void goalCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { print_info("on_goal"); if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { - result_.error_code = -100; + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; gh.setRejected(result_, result_.error_string); @@ -190,7 +203,7 @@ private: return; } if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { - result_.error_code = -100; + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; gh.setRejected(result_, result_.error_string); @@ -198,7 +211,7 @@ private: return; } if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) { - result_.error_code = -100; + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; gh.setRejected(result_, result_.error_string); @@ -206,7 +219,7 @@ private: return; } if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) { - result_.error_code = -100; + result_.error_code = -100; //nothing is defined for this...? result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; gh.setRejected(result_, result_.error_string); @@ -215,11 +228,17 @@ private: } actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify - if (goal_handle_ != NULL) { - RosWrapper::cancelCB(*goal_handle_); + if (has_goal_) { + print_warning( + "Received new goal while still executing previous trajectory. Canceling previous trajectory"); + has_goal_ = false; + robot_.stopTraj(); + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Received another trajectory"; + goal_handle_.setAborted(result_, result_.error_string); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); } - goal_handle_ = &gh; - + goal_handle_ = gh; if (!validateJointNames()) { std::string outp_joint_names = ""; for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); @@ -288,26 +307,25 @@ private: } - goal_handle_->setAccepted(); - robot_.doTraj(timestamps, positions, velocities); - if (goal_handle_ != NULL) { - result_.error_code = result_.SUCCESSFUL; - goal_handle_->setSucceeded(result_); - goal_handle_ = NULL; - } + goal_handle_.setAccepted(); + has_goal_ = true; + std::thread(&RosWrapper::trajThread, this, timestamps, positions, + velocities).detach(); } void cancelCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { // set the action state to preempted - print_info("on_cancel"); - if (goal_handle_ != NULL) { - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Goal cancelled by client"; - goal_handle_ = NULL; + print_info("on_cancel"); + if (has_goal_) { + if (gh == goal_handle_) { + robot_.stopTraj(); + has_goal_ = false; + } } + result_.error_code = -100; //nothing is defined for this...? + result_.error_string = "Goal cancelled by client"; gh.setCanceled(result_); } @@ -345,7 +363,7 @@ private: bool validateJointNames() { std::vector actual_joint_names = robot_.getJointNames(); actionlib::ActionServer::Goal goal = - *goal_handle_->getGoal(); + *goal_handle_.getGoal(); if (goal.trajectory.joint_names.size() != actual_joint_names.size()) return false; @@ -397,7 +415,7 @@ private: bool has_velocities() { actionlib::ActionServer::Goal goal = - *goal_handle_->getGoal(); + *goal_handle_.getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size()) @@ -408,7 +426,7 @@ private: bool has_positions() { actionlib::ActionServer::Goal goal = - *goal_handle_->getGoal(); + *goal_handle_.getGoal(); if (goal.trajectory.points.size() == 0) return false; for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { @@ -421,7 +439,7 @@ private: bool has_limited_velocities() { actionlib::ActionServer::Goal goal = - *goal_handle_->getGoal(); + *goal_handle_.getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) { @@ -435,7 +453,7 @@ private: bool traj_is_finite() { actionlib::ActionServer::Goal goal = - *goal_handle_->getGoal(); + *goal_handle_.getGoal(); for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) { for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) { @@ -590,13 +608,13 @@ private: and !warned) { print_error("Robot is protective stopped!"); } - if (goal_handle_ != NULL) { + if (has_goal_) { print_error("Aborting trajectory"); robot_.stopTraj(); result_.error_code = result_.SUCCESSFUL; result_.error_string = "Robot was halted"; - goal_handle_->setAborted(result_, result_.error_string); - goal_handle_ = NULL; + goal_handle_.setAborted(result_, result_.error_string); + has_goal_ = false; } warned = true; } else From cb5409685f24caaecd36c9ada47fe6e81a369c63 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 10:09:49 +0200 Subject: [PATCH 12/31] Added install targets --- CMakeLists.txt | 28 +++++++++------------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a9702e..15fd2e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,29 +174,19 @@ target_link_libraries(ur_driver ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) ## Mark executables and/or libraries for installation -# install(TARGETS ur_modern_driver ur_driver -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +install(TARGETS ur_driver ur_hardware_interface + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES From 0c901bd7e34df3db8da4ca3806aef3a6d70ae63e Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 10:14:57 +0200 Subject: [PATCH 13/31] Included suggestion to update build-essential pkg if compiler doesn't support c++11 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 15fd2e9..cfd2a37 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -126,7 +126,7 @@ if(COMPILER_SUPPORTS_CXX11) elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "-std=c++0x") else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ") endif() ## Specify additional locations of header files From 189ecc71ed072f3bc98ce8b3f688ea6eb3193465 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 10:57:39 +0200 Subject: [PATCH 14/31] Changed servoj_time handling to minimize network transfers. Fixes #7 --- src/ur_driver.cpp | 32 +++++++++++--------------------- 1 file changed, 11 insertions(+), 21 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index acd5703..cdbffb1 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -160,7 +160,7 @@ void UrDriver::servoj(std::vector positions, int keepalive, double time) { unsigned int bytes_written; int tmp; - unsigned char buf[32]; + unsigned char buf[28]; if (time < 0.016) { time = servoj_time_; } @@ -171,17 +171,12 @@ void UrDriver::servoj(std::vector positions, int keepalive, buf[i * 4 + 2] = (tmp >> 16) & 0xff; buf[i * 4 + 3] = (tmp >> 24) & 0xff; } - tmp = htonl((int) (time * MULT_TIME_)); + tmp = htonl((int) keepalive); buf[6 * 4] = tmp & 0xff; buf[6 * 4 + 1] = (tmp >> 8) & 0xff; buf[6 * 4 + 2] = (tmp >> 16) & 0xff; buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - tmp = htonl((int) keepalive); - buf[7 * 4] = tmp & 0xff; - buf[7 * 4 + 1] = (tmp >> 8) & 0xff; - buf[7 * 4 + 2] = (tmp >> 16) & 0xff; - buf[7 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 32); + bytes_written = write(new_sockfd_, buf, 28); } void UrDriver::stopTraj() { @@ -197,20 +192,14 @@ void UrDriver::uploadProg() { sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); cmd_str += buf; - sprintf(buf, "\tMULT_time = %i\n", MULT_TIME_); - cmd_str += buf; - cmd_str += "\tSERVO_IDLE = 0\n"; cmd_str += "\tSERVO_RUNNING = 1\n"; cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_id = 0 # 0 = idle, -1 = stop\n"; cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tcmd_servo_dt = 0.0\n"; - cmd_str += "\tdef set_servo_setpoint(q, dt):\n"; + cmd_str += "\tdef set_servo_setpoint(q):\n"; cmd_str += "\t\tenter_critical\n"; cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\tcmd_servo_dt = dt\n"; cmd_str += "\t\texit_critical\n"; cmd_str += "\tend\n"; cmd_str += "\tthread servoThread():\n"; @@ -218,7 +207,6 @@ void UrDriver::uploadProg() { cmd_str += "\t\twhile True:\n"; cmd_str += "\t\t\tenter_critical\n"; cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdt = cmd_servo_dt\n"; cmd_str += "\t\t\tdo_brake = False\n"; cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; @@ -231,7 +219,10 @@ void UrDriver::uploadProg() { cmd_str += "\t\t\t\tstopj(1.0)\n"; cmd_str += "\t\t\t\tsync()\n"; cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - cmd_str += "\t\t\t\tservoj(q, t=dt)\n"; + + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); + cmd_str += buf; + cmd_str += "\t\t\telse:\n"; cmd_str += "\t\t\t\tsync()\n"; cmd_str += "\t\t\tend\n"; @@ -245,7 +236,7 @@ void UrDriver::uploadProg() { cmd_str += "\tthread_servo = run servoThread()\n"; cmd_str += "\tkeepalive = 1\n"; cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1+1)\n"; + cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; cmd_str += "\t\tif params_mult[0] > 0:\n"; cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; cmd_str += "params_mult[2] / MULT_jointstate, "; @@ -253,9 +244,8 @@ void UrDriver::uploadProg() { cmd_str += "params_mult[4] / MULT_jointstate, "; cmd_str += "params_mult[5] / MULT_jointstate, "; cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tt = params_mult[7] / MULT_time\n"; - cmd_str += "\t\t\tkeepalive = params_mult[8]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q, t)\n"; + cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; + cmd_str += "\t\t\tset_servo_setpoint(q)\n"; cmd_str += "\t\tend\n"; cmd_str += "\tend\n"; cmd_str += "\tsleep(.1)\n"; From 8a4cdb51dac631d72d70e4d173660a25dccb41bd Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 11:01:54 +0200 Subject: [PATCH 15/31] Made sure to stop any executing trajectory when the driver is halted --- src/ur_driver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index cdbffb1..52be630 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -291,6 +291,9 @@ bool UrDriver::start() { } void UrDriver::halt() { + if (executing_traj_) { + UrDriver::stopTraj(); + } sec_interface_->halt(); rt_interface_->halt(); close(incoming_sockfd_); From 1c3fe82116802a5cd7d56b8dbbd6a595b007197a Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 12:16:38 +0200 Subject: [PATCH 16/31] Fixed an issue with using a simulated robot on firmware <= 1.8 --- include/ur_modern_driver/robot_state.h | 35 ++++++++++++++++++++++++++ src/robot_state.cpp | 13 ++++++++++ src/ur_ros_wrapper.cpp | 32 ++++++++++++++--------- 3 files changed, 68 insertions(+), 12 deletions(-) diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 48d390f..cc920f8 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -54,6 +54,38 @@ enum robot_message_type { } typedef robot_message_types::robot_message_type robotMessageType; +namespace robot_state_type_v18 { +enum robot_state_type { + ROBOT_RUNNING_MODE = 0, + ROBOT_FREEDRIVE_MODE = 1, + ROBOT_READY_MODE = 2, + ROBOT_INITIALIZING_MODE = 3, + ROBOT_SECURITY_STOPPED_MODE = 4, + ROBOT_EMERGENCY_STOPPED_MODE = 5, + ROBOT_FATAL_ERROR_MODE = 6, + ROBOT_NO_POWER_MODE = 7, + ROBOT_NOT_CONNECTED_MODE = 8, + ROBOT_SHUTDOWN_MODE = 9, + ROBOT_SAFEGUARD_STOP_MODE = 10 +}; +} +typedef robot_state_type_v18::robot_state_type robotStateTypeV18; +namespace robot_state_type_v30 { +enum robot_state_type { + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 +}; +} + +typedef robot_state_type_v30::robot_state_type robotStateTypeV30; + struct version_message { uint64_t timestamp; int8_t source; @@ -115,6 +147,7 @@ private: std::condition_variable* pMsg_cond_; //Signals that new vars are available bool new_data_available_; //to avoid spurious wakes + unsigned char robot_mode_running_; double ntohd(uint64_t nf); @@ -154,6 +187,8 @@ public: bool isProtectiveStopped(); bool isProgramRunning(); bool isProgramPaused(); + unsigned char getRobotMode(); + bool isReady(); void setDisconnected(); diff --git a/src/robot_state.cpp b/src/robot_state.cpp index ed5928e..c5026cd 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -13,6 +13,7 @@ RobotState::RobotState(std::condition_variable& msg_cond) { new_data_available_ = false; pMsg_cond_ = &msg_cond; RobotState::setDisconnected(); + robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; } double RobotState::ntohd(uint64_t nf) { double x; @@ -129,6 +130,9 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, version_msg_.svn_revision = ntohl(version_msg_.svn_revision); memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); version_msg_.build_date[len - offset] = '\0'; + if (version_msg_.major_version < 2) { + robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; + } } void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) { @@ -355,6 +359,15 @@ bool RobotState::isProgramRunning() { bool RobotState::isProgramPaused() { return robot_mode_.isProgramPaused; } +unsigned char RobotState::getRobotMode() { + return robot_mode_.robotMode; +} +bool RobotState::isReady() { + if (robot_mode_.robotMode == robot_mode_running_) { + return true; + } + return false; +} void RobotState::setDisconnected() { robot_mode_.isRobotConnected = false; diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 3164653..66ff4a6 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -193,19 +193,26 @@ private: void goalCB( actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction> gh) { + std::string buf; print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { + if (!robot_.sec_interface_->robot_state_->isReady()) { result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot arm is not powered on"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { - result_.error_code = -100; //nothing is defined for this...? - result_.error_string = - "Cannot accept new trajectories: Robot is not enabled"; + + if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) { + result_.error_string = + "Cannot accept new trajectories: Robot arm is not powered on"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) { + result_.error_string = + "Cannot accept new trajectories: Robot is not enabled"; + gh.setRejected(result_, result_.error_string); + print_error(result_.error_string); + return; + } + result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")"; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); return; @@ -226,6 +233,7 @@ private: print_error(result_.error_string); return; } + actionlib::ActionServer::Goal goal = *gh.getGoal(); //make a copy that we can modify if (has_goal_) { @@ -280,7 +288,7 @@ private: if (!has_limited_velocities()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = - "Received a goal with velocities that are higher than %f", max_velocity_; + "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); gh.setRejected(result_, result_.error_string); print_error(result_.error_string); return; From 6c3cade12fb47f41b816d05377feaea993742a28 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 12:47:28 +0200 Subject: [PATCH 17/31] Changed license to Apache 2.0 --- LICENSE | 181 +++++++++++++++++- include/ur_modern_driver/do_output.h | 19 +- include/ur_modern_driver/robot_state.h | 15 +- include/ur_modern_driver/robot_state_RT.h | 19 +- include/ur_modern_driver/ur_communication.h | 19 +- include/ur_modern_driver/ur_driver.h | 19 +- .../ur_modern_driver/ur_hardware_interface.h | 21 +- .../ur_realtime_communication.h | 19 +- src/do_output.cpp | 19 +- src/robot_state.cpp | 15 +- src/robot_state_RT.cpp | 19 +- src/ur_communication.cpp | 19 +- src/ur_driver.cpp | 19 +- src/ur_hardware_interface.cpp | 21 +- src/ur_realtime_communication.cpp | 19 +- src/ur_ros_wrapper.cpp | 19 +- 16 files changed, 374 insertions(+), 88 deletions(-) diff --git a/LICENSE b/LICENSE index b68de61..35548b1 100644 --- a/LICENSE +++ b/LICENSE @@ -1,7 +1,180 @@ -"THE BEER-WARE LICENSE" (Revision 42): +Copyright 2015 Thomas Timm Andersen - wrote this software. As long as you retain this notice you -can do whatever you want with this stuff. If we meet some day, and you think -this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + diff --git a/include/ur_modern_driver/do_output.h b/include/ur_modern_driver/do_output.h index 8031821..d52420e 100644 --- a/include/ur_modern_driver/do_output.h +++ b/include/ur_modern_driver/do_output.h @@ -1,12 +1,19 @@ /* * do_output.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_DO_OUTPUT_H_ diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index cc920f8..159974b 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -1,8 +1,19 @@ /* * robot_state.h * - * Created on: Sep 10, 2015 - * Author: ttan + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef ROBOT_STATE_H_ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index bec0767..1f5b2af 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -1,12 +1,19 @@ /* * robotStateRT.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef ROBOT_STATE_RT_H_ diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index 7adb165..5173c45 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -1,12 +1,19 @@ /* * ur_communication.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_COMMUNICATION_H_ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index e26f869..1cec8df 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -1,12 +1,19 @@ /* * ur_driver * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_DRIVER_H_ diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index ff9572a..ac9f310 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -1,15 +1,22 @@ /* * ur_hardware_control_loop.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ -/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */ +/* Based on original source from University of Colorado, Boulder. License copied below. */ /********************************************************************* * Software License Agreement (BSD License) diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 66b5a4b..9d6b445 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -1,12 +1,19 @@ /* * ur_realtime_communication.h * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #ifndef UR_REALTIME_COMMUNICATION_H_ diff --git a/src/do_output.cpp b/src/do_output.cpp index a4b8b68..048cce1 100644 --- a/src/do_output.cpp +++ b/src/do_output.cpp @@ -1,12 +1,19 @@ /* * do_output.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/do_output.h" diff --git a/src/robot_state.cpp b/src/robot_state.cpp index c5026cd..a7eb589 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -1,8 +1,19 @@ /* * robot_state.cpp * - * Created on: Sep 10, 2015 - * Author: ttan + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/robot_state.h" diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 273db0e..03921e6 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -1,12 +1,19 @@ /* * robotStateRT.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/robot_state_RT.h" diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 09954ed..e7e944f 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -1,12 +1,19 @@ /* * ur_communication.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_communication.h" diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 52be630..54d81d9 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -1,12 +1,19 @@ /* * ur_driver.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_driver.h" diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 0222ede..ab97678 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -1,15 +1,22 @@ /* * ur_hardware_control_loop.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ -/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */ +/* Based on original source from University of Colorado, Boulder. License copied below. */ /********************************************************************* * Software License Agreement (BSD License) diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 9f12b70..53e18f3 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -1,12 +1,19 @@ /* * ur_realtime_communication.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_realtime_communication.h" diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 66ff4a6..54251d4 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -1,12 +1,19 @@ /* * ur_driver.cpp * - * ---------------------------------------------------------------------------- - * "THE BEER-WARE LICENSE" (Revision 42): - * wrote this file. As long as you retain this notice you - * can do whatever you want with this stuff. If we meet some day, and you think - * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen - * ---------------------------------------------------------------------------- + * Copyright 2015 Thomas Timm Andersen + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. */ #include "ur_modern_driver/ur_driver.h" From f856487e9f0fc6c23e3d789b69418c174ba94377 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 13:21:23 +0200 Subject: [PATCH 18/31] Added stability to reverse connection. Resolves #6 --- include/ur_modern_driver/ur_driver.h | 7 ++++--- src/ur_driver.cpp | 28 +++++++++++++++++++++------- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 1cec8df..2a2391b 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -46,6 +46,7 @@ private: const unsigned int REVERSE_PORT_; int incoming_sockfd_; int new_sockfd_; + bool reverse_connected_; double servoj_time_; bool executing_traj_; public: @@ -66,15 +67,15 @@ public: std::vector inp_timestamps, //DEPRECATED std::vector > positions, std::vector > velocities); */ - void doTraj(std::vector inp_timestamps, + bool doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities); void servoj(std::vector positions, int keepalive = 1, double time = 0); void stopTraj(); - void uploadProg(); - void openServo(); + bool uploadProg(); + bool openServo(); void closeServo(std::vector positions); std::vector interp_cubic(double t, double T, diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 54d81d9..a34d69b 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -29,7 +29,8 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, char buffer[256]; struct sockaddr_in serv_addr; int n, flag; - //char *ip_addr; + + reverse_connected_ = false; executing_traj_ = false; rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); @@ -127,15 +128,17 @@ std::vector UrDriver::interp_cubic(double t, double T, } */ -void UrDriver::doTraj(std::vector inp_timestamps, +bool UrDriver::doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { std::chrono::high_resolution_clock::time_point t0, t; std::vector positions; unsigned int j; + if (!UrDriver::uploadProg()) { + return false; + } executing_traj_ = true; - UrDriver::uploadProg(); t0 = std::chrono::high_resolution_clock::now(); t = t0; j = 0; @@ -159,12 +162,18 @@ void UrDriver::doTraj(std::vector inp_timestamps, std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); t = std::chrono::high_resolution_clock::now(); } + executing_traj_ = false; //Signal robot to stop driverProg() UrDriver::closeServo(positions); + return true; } void UrDriver::servoj(std::vector positions, int keepalive, double time) { + if (!reverse_connected_) { + print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " + std::to_string(keepalive)); + return; + } unsigned int bytes_written; int tmp; unsigned char buf[28]; @@ -191,7 +200,7 @@ void UrDriver::stopTraj() { rt_interface_->addCommandToQueue("stopj(10)\n"); } -void UrDriver::uploadProg() { +bool UrDriver::uploadProg() { std::string cmd_str; char buf[128]; cmd_str = "def driverProg():\n"; @@ -255,15 +264,16 @@ void UrDriver::uploadProg() { cmd_str += "\t\t\tset_servo_setpoint(q)\n"; cmd_str += "\t\tend\n"; cmd_str += "\tend\n"; + cmd_str += "\tstopj(10)\n"; cmd_str += "\tsleep(.1)\n"; cmd_str += "\tsocket_close()\n"; cmd_str += "end\n"; rt_interface_->addCommandToQueue(cmd_str); - UrDriver::openServo(); + return UrDriver::openServo(); } -void UrDriver::openServo() { +bool UrDriver::openServo() { struct sockaddr_in cli_addr; socklen_t clilen; clilen = sizeof(cli_addr); @@ -271,14 +281,18 @@ void UrDriver::openServo() { &clilen); if (new_sockfd_ < 0) { print_fatal("ERROR on accepting reverse communication"); + return false; } + reverse_connected_ = true; + return true; } void UrDriver::closeServo(std::vector positions) { if (positions.size() != 6) UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); else UrDriver::servoj(positions, 0); - executing_traj_ = false; + + reverse_connected_ = false; close(new_sockfd_); } From 1de174d73f47769d9757aaa8ee4ea254c9ca1b5b Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 13:24:52 +0200 Subject: [PATCH 19/31] code cleanup --- include/ur_modern_driver/ur_driver.h | 2 +- src/ur_driver.cpp | 18 ++++++++---------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 2a2391b..6bc1405 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -70,7 +70,7 @@ public: bool doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities); - void servoj(std::vector positions, int keepalive = 1, double time = 0); + void servoj(std::vector positions, int keepalive = 1); void stopTraj(); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index a34d69b..c748334 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -168,18 +168,16 @@ bool UrDriver::doTraj(std::vector inp_timestamps, return true; } -void UrDriver::servoj(std::vector positions, int keepalive, - double time) { +void UrDriver::servoj(std::vector positions, int keepalive) { if (!reverse_connected_) { - print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " + std::to_string(keepalive)); + print_error( + "UrDriver::servoj called without a reverse connection present. Keepalive: " + + std::to_string(keepalive)); return; } unsigned int bytes_written; int tmp; unsigned char buf[28]; - if (time < 0.016) { - time = servoj_time_; - } for (int i = 0; i < 6; i++) { tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_)); buf[i * 4] = tmp & 0xff; @@ -303,10 +301,10 @@ bool UrDriver::start() { sec_interface_->robot_state_->getVersion()); if (!rt_interface_->start()) return false; - ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); - char buf[256]; - sprintf(buf, "Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); - print_debug(buf); + ip_addr_ = rt_interface_->getLocalIp(); + print_debug( + "Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + + "\n"); return true; } From a918626dad65c19e87c351e4d65bb3ec36dfe8ff Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 13:41:45 +0200 Subject: [PATCH 20/31] Added info message about reconnection aquired --- src/ur_communication.cpp | 1 + src/ur_realtime_communication.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index e7e944f..2b5224b 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -163,6 +163,7 @@ void UrCommunication::run() { print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); } else { connected_ = true; + print_info("Secondary port: Reconnected"); } } } diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 53e18f3..413b4e6 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -162,6 +162,7 @@ void UrRealtimeCommunication::run() { print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); } else { connected_ = true; + print_info("Realtime port: Reconnected"); } } } From 806548dd80762bef4ee80e402bc0e2be5b0a2cc2 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 20 Oct 2015 14:23:15 +0200 Subject: [PATCH 21/31] Tuned PID values to work with a real robot --- config/ur10_controllers.yaml | 12 ++++++------ config/ur5_controllers.yaml | 12 ++++++------ launch/ur10_ros_control.launch | 2 +- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml index 8c0c5bc..19e0394 100644 --- a/config/ur10_controllers.yaml +++ b/config/ur10_controllers.yaml @@ -83,12 +83,12 @@ vel_based_pos_traj_controller: action_monitor_rate: 10 gains: #!!These values are useable, but maybe not optimal - shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - elbow_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - wrist_1_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - wrist_2_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - wrist_3_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} # state_publish_rate: 50 # Defaults to 50 diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml index a1054b7..4eab52e 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -83,12 +83,12 @@ vel_based_pos_traj_controller: action_monitor_rate: 10 gains: #!!These values are useable, but maybe not optimal - shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - elbow_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - wrist_1_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - wrist_2_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} - wrist_3_joint: {p: 20.0, i: 0.0, d: 0.7, i_clamp: 1} + shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} # state_publish_rate: 50 # Defaults to 50 # action_monitor_rate: 20 # Defaults to 20 diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index d60d02f..ce6c1e1 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -25,7 +25,7 @@ - + Date: Tue, 20 Oct 2015 14:38:03 +0200 Subject: [PATCH 22/31] Changed default servoj_time for better backwards compatibility and smoother pos_based ros_control --- src/ur_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 54251d4..514d8d9 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -142,7 +142,7 @@ public: min_payload, max_payload); print_debug(buf); - double servoj_time = 0.008; + double servoj_time = 0.08; if (ros::param::get("~servoj_time", servoj_time)) { sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); print_debug(buf); From c3f3da922a762cd522eae7ef6ef871f2bc17c805 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 27 Oct 2015 11:43:45 +0100 Subject: [PATCH 23/31] Fixed #9. Now data streaming is much more regular --- src/ur_communication.cpp | 10 ++++++++-- src/ur_realtime_communication.cpp | 6 +++++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 2b5224b..0f27cc7 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -44,10 +44,14 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond, flag_ = 1; setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); @@ -69,7 +73,7 @@ bool UrCommunication::start() { } print_debug("Acquire firmware version: Got connection"); bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); robot_state_->unpack(buf, bytes_read); //wait for some traffic so the UR socket doesn't die in version 3.1. @@ -124,7 +128,7 @@ void UrCommunication::run() { select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes if (bytes_read > 0) { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); robot_state_->unpack(buf, bytes_read); } else { @@ -143,6 +147,8 @@ void UrCommunication::run() { flag_ = 1; setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 413b4e6..62724bb 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -36,6 +36,7 @@ UrRealtimeCommunication::UrRealtimeCommunication( serv_addr_.sin_port = htons(30003); flag_ = 1; setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sockfd_, F_SETFL, O_NONBLOCK); connected_ = false; @@ -121,7 +122,7 @@ void UrRealtimeCommunication::run() { select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); bytes_read = read(sockfd_, buf, 2048); if (bytes_read > 0) { - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int)); robot_state_->unpack(buf); if (safety_count_ == safety_count_max_) { @@ -143,6 +144,9 @@ void UrRealtimeCommunication::run() { flag_ = 1; setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); + setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, + sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); fcntl(sockfd_, F_SETFL, O_NONBLOCK); From 839d90c4ab6734907515b12ed6fd0ad3ab91f799 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Wed, 28 Oct 2015 11:25:59 +0100 Subject: [PATCH 24/31] Added a check to see that the robot is connected before attempting to write to the socket. Fixes #11 --- src/ur_realtime_communication.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 62724bb..c4881c0 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -89,7 +89,10 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp) { if (inp.back() != '\n') { inp.append("\n"); } - bytes_written = write(sockfd_, inp.c_str(), inp.length()); + if (connected_) + bytes_written = write(sockfd_, inp.c_str(), inp.length()); + else + print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" ); } void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, From 2e37fb00df56487dad7f9e0cdcdc90796c8ce27b Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Wed, 28 Oct 2015 11:27:38 +0100 Subject: [PATCH 25/31] Code cleanup --- include/ur_modern_driver/ur_driver.h | 5 +-- src/ur_communication.cpp | 1 - src/ur_driver.cpp | 54 ---------------------------- src/ur_realtime_communication.cpp | 1 - 4 files changed, 1 insertion(+), 60 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 6bc1405..f2d9b7b 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -63,10 +63,7 @@ public: void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - /* void addTraj( - std::vector inp_timestamps, //DEPRECATED - std::vector > positions, - std::vector > velocities); */ + bool doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities); diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 0f27cc7..cf04cc3 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -155,7 +155,6 @@ void UrCommunication::run() { while (keepalive_ && !connected_) { std::this_thread::sleep_for(std::chrono::seconds(10)); fd_set writefds; - keepalive_ = true; connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, sizeof(sec_serv_addr_)); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index c748334..d2252bf 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -73,61 +73,7 @@ std::vector UrDriver::interp_cubic(double t, double T, } return positions; } -/* - void UrDriver::addTraj(std::vector inp_timestamps, - std::vector > inp_positions, - std::vector > inp_velocities) { - // DEPRECATED - printf("!! addTraj is deprecated !!\n"); - std::vector timestamps; - std::vector > positions; - std::string command_string = "def traj():\n"; - for (unsigned int i = 1; i < inp_timestamps.size(); i++) { - timestamps.push_back(inp_timestamps[i - 1]); - double dt = inp_timestamps[i] - inp_timestamps[i - 1]; - unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_); - double step_size = dt / steps; - for (unsigned int j = 1; j < steps; j++) { - timestamps.push_back(inp_timestamps[i - 1] + step_size * j); - } - } - // //make sure we come to a smooth stop - // while (timestamps.back() < inp_timestamps.back()) { - // timestamps.push_back(timestamps.back() + 0.008); - // } - // timestamps.pop_back(); - - unsigned int j = 0; - for (unsigned int i = 0; i < timestamps.size(); i++) { - while (inp_timestamps[j] <= timestamps[i]) { - j += 1; - } - positions.push_back( - UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], - inp_positions[j - 1], inp_positions[j], - inp_velocities[j - 1], inp_velocities[j])); - } - - timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); - positions.push_back(inp_positions[inp_positions.size() - 1]); - /// This is actually faster than using a stringstream :-o - for (unsigned int i = 1; i < timestamps.size(); i++) { - char buf[128]; - sprintf(buf, - "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n", - positions[i][0], positions[i][1], positions[i][2], - positions[i][3], positions[i][4], positions[i][5], - timestamps[i] - timestamps[i - 1]); - command_string += buf; - } - command_string += "end\n"; - //printf("%s", command_string.c_str()); - rt_interface_->addCommandToQueue(command_string); - - } - */ bool UrDriver::doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index c4881c0..f841455 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -156,7 +156,6 @@ void UrRealtimeCommunication::run() { while (keepalive_ && !connected_) { std::this_thread::sleep_for(std::chrono::seconds(10)); fd_set writefds; - keepalive_ = true; connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)); From 3f0b03a76837e111fa6bca0817a25f43273dd2f4 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 30 Oct 2015 12:47:32 +0100 Subject: [PATCH 26/31] Added launch files for making the driver behave just like the old python driver --- launch/ur10_bringup.launch | 2 +- launch/ur_common.launch | 16 +++++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/launch/ur10_bringup.launch b/launch/ur10_bringup.launch index ef7befd..97d5584 100644 --- a/launch/ur10_bringup.launch +++ b/launch/ur10_bringup.launch @@ -12,7 +12,7 @@ - + diff --git a/launch/ur_common.launch b/launch/ur_common.launch index b2ceb0a..6cd06dd 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -8,9 +8,10 @@ --> - - - + + + + @@ -21,9 +22,10 @@ - - - - + + + + + From 14e9a25dd876b61ad59db5523e241ae872ddff1b Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 30 Oct 2015 12:48:30 +0100 Subject: [PATCH 27/31] Removed hardcoded velocity limit in vel_based ros_control --- src/ur_hardware_interface.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index ab97678..ea66395 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -163,10 +163,6 @@ void UrHardwareInterface::write() { < prev_joint_velocity_command_[i] - max_vel_change_) { cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; } - if (cmd[i] > M_PI/2.) - cmd[i] = M_PI/2.; - else if (cmd[i] < -M_PI/2.) - cmd[i] = -M_PI/2.; prev_joint_velocity_command_[i] = cmd[i]; } robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125); From 09dbe010aed2a66c9565a1786059bfede60bb0a5 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 30 Oct 2015 12:52:46 +0100 Subject: [PATCH 28/31] Optimized servoj function call for firmware version >= 3.1 --- src/ur_driver.cpp | 6 ++++-- src/ur_ros_wrapper.cpp | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index d2252bf..70503e9 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -180,7 +180,10 @@ bool UrDriver::uploadProg() { cmd_str += "\t\t\t\tsync()\n"; cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); + if (sec_interface_->robot_state_->getVersion() >= 3.1) + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n", servoj_time_); + else + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); cmd_str += buf; cmd_str += "\t\t\telse:\n"; @@ -208,7 +211,6 @@ bool UrDriver::uploadProg() { cmd_str += "\t\t\tset_servo_setpoint(q)\n"; cmd_str += "\t\tend\n"; cmd_str += "\tend\n"; - cmd_str += "\tstopj(10)\n"; cmd_str += "\tsleep(.1)\n"; cmd_str += "\tsocket_close()\n"; cmd_str += "end\n"; diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 514d8d9..54251d4 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -142,7 +142,7 @@ public: min_payload, max_payload); print_debug(buf); - double servoj_time = 0.08; + double servoj_time = 0.008; if (ros::param::get("~servoj_time", servoj_time)) { sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); print_debug(buf); From 58352a950274a2bf8df4b7da027dcde9e9311c45 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Fri, 30 Oct 2015 13:05:26 +0100 Subject: [PATCH 29/31] Added parameter for reverse port. closes #12 --- src/ur_ros_wrapper.cpp | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 54251d4..b9f15ca 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -75,11 +75,11 @@ protected: boost::shared_ptr controller_manager_; public: - RosWrapper(std::string host) : + RosWrapper(std::string host, int reverse_port) : as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1), boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( - rt_msg_cond_, msg_cond_, host), io_flag_delay_(0.05), joint_offsets_( + rt_msg_cond_, msg_cond_, host, reverse_port), io_flag_delay_(0.05), joint_offsets_( 6, 0.0) { std::string joint_prefix = ""; @@ -219,7 +219,11 @@ private: print_error(result_.error_string); return; } - result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")"; + result_.error_string = + "Cannot accept new trajectories. (Debug: Robot mode is " + + std::to_string( + robot_.sec_interface_->robot_state_->getRobotMode()) + + ")"; gh.setRejected(result_, result_.error_string); print_error(result_.error_string); return; @@ -295,7 +299,8 @@ private: if (!has_limited_velocities()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = - "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); + "Received a goal with velocities that are higher than " + + std::to_string(max_velocity_); gh.setRejected(result_, result_.error_string); print_error(result_.error_string); return; @@ -645,6 +650,7 @@ private: int main(int argc, char **argv) { bool use_sim_time = false; std::string host; + int reverse_port = 50001; ros::init(argc, argv, "ur_driver"); ros::NodeHandle nh; @@ -663,8 +669,15 @@ int main(int argc, char **argv) { } } + if ((ros::param::get("~reverse_port", reverse_port))) { + if((reverse_port <= 0) or (reverse_port >= 65535)) { + print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); + reverse_port = 50001; + } + } else + reverse_port = 50001; - RosWrapper interface(host); + RosWrapper interface(host, reverse_port); ros::AsyncSpinner spinner(3); spinner.start(); From 702165624ef4f054e788204fbb50bcb07771c272 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 10 Nov 2015 13:05:04 +0100 Subject: [PATCH 30/31] Update README.md Added Citation information --- README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/README.md b/README.md index 88a3acd..46e98e4 100644 --- a/README.md +++ b/README.md @@ -104,3 +104,15 @@ Should be compatible with all robots and control boxes with the newest firmware. *Simulated UR5 running 1.6.08725 +#Credits +Please cite the following report if using this driver + +@book{andersen2015optimizing, +title = "Optimizing the Universal Robots ROS driver.", +publisher = "Technical University of Denmark, Department of Electrical Engineering", +author = "Andersen, {Thomas Timm}", +year = "2015", +url = "http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html" +} + +The report can be downloaded from http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html From 2887d803ebcfbf93a1a0203b6ca947a823b998b0 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 10 Nov 2015 13:10:46 +0100 Subject: [PATCH 31/31] Update README.md fixed bibtex citation --- README.md | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 46e98e4..62946e8 100644 --- a/README.md +++ b/README.md @@ -107,12 +107,13 @@ Should be compatible with all robots and control boxes with the newest firmware. #Credits Please cite the following report if using this driver -@book{andersen2015optimizing, -title = "Optimizing the Universal Robots ROS driver.", -publisher = "Technical University of Denmark, Department of Electrical Engineering", -author = "Andersen, {Thomas Timm}", -year = "2015", -url = "http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html" -} +@techreport{andersen2015optimizing, + title = {Optimizing the Universal Robots ROS driver.}, + institution = {Technical University of Denmark, Department of Electrical Engineering}, + author = {Andersen, Thomas Timm}, + year = {2015}, + url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html} + } + The report can be downloaded from http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html