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.

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h

@@ -47,6 +47,7 @@
 #include <industrial_msgs/RobotStatus.h>
 #include <motoman_driver/industrial_robot_client/robot_group.h>
 #include <motoman_msgs/DynamicJointTrajectory.h>
+#include <motoman_msgs/MotorosError.h>
 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.
    *

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.

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.");

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
-

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 <map>
@@ -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<motoman_msgs::MotorosError>("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<motoman_msgs::MotorosError>("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;
         }
       }

motoman_msgs/CMakeLists.txt

@@ -18,6 +18,7 @@ add_message_files(
     DynamicJointState.msg
     DynamicJointTrajectory.msg
     DynamicJointTrajectoryFeedback.msg
+    MotorosError.msg
 )
 
 add_service_files(

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