From c4533d3231584b56e162092012af14384d0f00c8 Mon Sep 17 00:00:00 2001 From: Nicholas Ardavin Date: Tue, 15 Mar 2022 13:32:27 -0700 Subject: [PATCH] Abort trajectory actions after motoros errors Add functionality in the motoman driver stack for the trajectory action server to abort the active trajectory when a motoros error is thrown. --- .../joint_trajectory_action.h | 15 +++++++++++++++ .../motoman_driver/joint_trajectory_streamer.h | 6 ++++++ .../joint_trajectory_action.cpp | 13 +++++++++++++ .../joint_trajectory_interface.cpp | 1 - motoman_driver/src/joint_trajectory_streamer.cpp | 12 ++++++++++++ motoman_msgs/CMakeLists.txt | 1 + motoman_msgs/msg/MotorosError.msg | 6 ++++++ 7 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 motoman_msgs/msg/MotorosError.msg diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h index f0ef8f8c..6cc256db 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h @@ -47,6 +47,7 @@ #include #include #include +#include namespace industrial_robot_client { namespace joint_trajectory_action @@ -109,6 +110,11 @@ class JointTrajectoryAction */ ros::Subscriber sub_robot_status_; + /** + * \brief Subscribes to motoros error messages to abort trajectories. + */ + ros::Subscriber sub_motoros_errors_; + /** * \brief Watchdog time used to fail the action request if the robot * driver is not responding. @@ -226,6 +232,15 @@ class JointTrajectoryAction */ void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg); + /** + * \brief Motoross error callback (executed when motoros error + * message received) + * + * \param msg Motoros error message + * + */ + void motorosErrorCB(const motoman_msgs::MotorosError &msg); + /** * \brief Marks the current action goal as succeeded. * diff --git a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h index 1e53cf0a..cc0b8352 100644 --- a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h @@ -168,6 +168,12 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer */ ros::ServiceServer srv_select_tool_; + /** + * \brief Publisher to announce motoros failures. This allows higher-level + * drivers to retry certain actions. + */ + ros::Publisher motoros_error_pub_; + /** * \brief Disable the robot. Response is true if the state was flipped or * false if the state has not changed. diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp index 65ac23ba..3ca1c8cd 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp @@ -84,6 +84,8 @@ JointTrajectoryAction::JointTrajectoryAction() : sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this); + sub_motoros_errors_ = node_.subscribe("motoros_error", 1, &JointTrajectoryAction::motorosErrorCB, this); + // Set watchdog timer for entire robot state. watchdog_timer_ = node_.createTimer(ros::Duration(WATCHDOG_PERIOD_), boost::bind(&JointTrajectoryAction::watchdog, this, _1)); @@ -448,6 +450,17 @@ void JointTrajectoryAction::controllerStateCB( } } +void JointTrajectoryAction::motorosErrorCB(const motoman_msgs::MotorosError &msg) +{ + ROS_WARN_STREAM( + "Encountered motoros error " << msg.code << "-" << msg.subcode << ": " + << msg.code_description << " " << msg.subcode_description); + if (has_active_goal_) + { + abortGoal(); + } +} + void JointTrajectoryAction::setGoalSuccess() { ROS_INFO("Marking goal as successful."); diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp index 69223609..9a2dc016 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp @@ -738,4 +738,3 @@ void JointTrajectoryInterface::jointStateCB( } // namespace joint_trajectory_interface } // namespace industrial_robot_client - diff --git a/motoman_driver/src/joint_trajectory_streamer.cpp b/motoman_driver/src/joint_trajectory_streamer.cpp index ed5910b1..ca230c2a 100644 --- a/motoman_driver/src/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/joint_trajectory_streamer.cpp @@ -33,6 +33,7 @@ #include "motoman_driver/simple_message/messages/motoman_motion_reply_message.h" #include "simple_message/messages/joint_traj_pt_full_message.h" #include "motoman_driver/simple_message/messages/joint_traj_pt_full_ex_message.h" +#include "motoman_msgs/MotorosError.h" #include "industrial_robot_client/utils.h" #include "industrial_utils/param_utils.h" #include @@ -93,6 +94,8 @@ bool MotomanJointTrajectoryStreamer::init(SmplMsgConnection* connection, const s srv_select_tool_ = node_.advertiseService("select_tool", &MotomanJointTrajectoryStreamer::selectToolCB, this); + motoros_error_pub_ = node_.advertise("motoros_error", 10); + return rtn; } @@ -118,6 +121,8 @@ bool MotomanJointTrajectoryStreamer::init(SmplMsgConnection* connection, const s srv_select_tool_ = node_.advertiseService("select_tool", &MotomanJointTrajectoryStreamer::selectToolCB, this); + motoros_error_pub_ = node_.advertise("motoros_error", 10); + return rtn; } @@ -548,6 +553,13 @@ void MotomanJointTrajectoryStreamer::streamingThread() << " (#" << this->current_point_ << "): " << MotomanMotionCtrl::getErrorString(reply_status.reply_)); this->state_ = TransferStates::IDLE; + + motoman_msgs::MotorosError error; + error.code = reply_status.reply_.getResult(); + error.subcode = reply_status.reply_.getSubcode(); + error.code_description = reply_status.reply_.getResultString(); + error.subcode_description = reply_status.reply_.getSubcodeString(); + motoros_error_pub_.publish(error); break; } } diff --git a/motoman_msgs/CMakeLists.txt b/motoman_msgs/CMakeLists.txt index 8e7a8595..6593d869 100644 --- a/motoman_msgs/CMakeLists.txt +++ b/motoman_msgs/CMakeLists.txt @@ -18,6 +18,7 @@ add_message_files( DynamicJointState.msg DynamicJointTrajectory.msg DynamicJointTrajectoryFeedback.msg + MotorosError.msg ) add_service_files( diff --git a/motoman_msgs/msg/MotorosError.msg b/motoman_msgs/msg/MotorosError.msg new file mode 100644 index 00000000..e5a45d6d --- /dev/null +++ b/motoman_msgs/msg/MotorosError.msg @@ -0,0 +1,6 @@ +# Message to represent errors in the motoros driver. + +int32 code +int32 subcode +string code_description +string subcode_description