WIP: Register DoneCallback to trajectory passthrough

UniversalRobots · Jun 2, 2021 · 01a1cf0 · 01a1cf0
1 parent 1852baf
commit 01a1cf0
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 0 deletions.
diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h
@@ -46,6 +46,7 @@
 #include <cartesian_control_msgs/FollowCartesianTrajectoryAction.h>
 #include <cartesian_control_msgs/FollowCartesianTrajectoryFeedback.h>
 
+#include <ur_client_library/control/trajectory_point_interface.h>
 #include <ur_msgs/IOStates.h>
 #include <ur_msgs/ToolDataMsg.h>
 #include <ur_msgs/SetIO.h>
@@ -226,6 +227,8 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   void cancelInterpolation();
 
+  void passthroughTrajectoryDoneCb(urcl::control::TrajectoryResult result);
+
   ros::ServiceServer deactivate_srv_;
   ros::ServiceServer tare_sensor_srv_;
   ros::ServiceServer set_payload_srv_;

diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
@@ -25,6 +25,7 @@
  */
 //----------------------------------------------------------------------
 #include <pluginlib/class_list_macros.hpp>
+#include <ur_client_library/control/trajectory_point_interface.h>
 #include <ur_robot_driver/hardware_interface.h>
 #include <ur_client_library/ur/tool_communication.h>
 #include <ur_client_library/exceptions.h>
@@ -36,6 +37,7 @@
 #include <cartesian_control_msgs/FollowCartesianTrajectoryAction.h>
 
 #include <Eigen/Geometry>
+#include <stdexcept>
 
 using industrial_robot_status_interface::RobotMode;
 using industrial_robot_status_interface::TriState;
@@ -267,6 +269,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     ur_driver_.reset(
         new urcl::UrDriver(robot_ip_, script_filename, output_recipe_filename, input_recipe_filename,
                            std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
+                           std::bind(&HardwareInterface::passthroughTrajectoryDoneCb, this, std::placeholders::_1),
                            headless_mode, std::move(tool_comm_setup), calibration_checksum, (uint32_t)reverse_port,
                            (uint32_t)script_sender_port, servoj_gain, servoj_lookahead_time, non_blocking_read_));
   }
@@ -1179,6 +1182,36 @@ void HardwareInterface::cancelInterpolation()
   ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL);
 }
 
+void HardwareInterface::passthroughTrajectoryDoneCb(urcl::control::TrajectoryResult result)
+{
+  // This should be forwarded to the passthrough controller. Probably there's a better way than
+  // using a member function, this is just an example of registering a callback for the trajectory
+  // end.
+  switch (result)
+  {
+    case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS:
+    {
+      ROS_INFO_STREAM("Forwarded trajectory finished successful.");
+      break;
+    }
+    case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED:
+    {
+      ROS_INFO_STREAM("Forwarded trajectory execution preempted by user.");
+      break;
+    }
+    case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE:
+    {
+      ROS_INFO_STREAM("Forwarded trajectory execution failed.");
+      break;
+    }
+    default:
+    {
+      std::stringstream ss;
+      ss << "Unknown trajectory result: " << urcl::toUnderlying(result);
+      throw(std::invalid_argument(ss.str()));
+    }
+  }
+}
 }  // namespace ur_driver
 
 PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)