Sort-of rebase of 83: traj action aborts on error

industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h

@@ -176,6 +176,33 @@ class JointTrajectoryAction
    */
   static const double WATCHDOG_PERIOD_;// = 1.0;
 
+  /**
+   * \brief Should the action server abort goals if the OEM server
+   * program reports a problem?
+   *
+   * This is a backwards compatibility 'tunable knob'.
+   *
+   * Set this to true to keep the old behaviour.
+   *
+   * This is configurable, as it's possible drivers for certain robots
+   * cannot accurately report controller status (ie: have UNKNOWNs in
+   * the RobotStatus messages they publish).
+   */
+  bool ignore_motion_server_error_;
+
+  /**
+   * \brief Should the action server consider UNKNOWN for TriStates in
+   * a RobotStatus message as OK?
+   *
+   * This is a backwards compatibility 'tunable knob'.
+   *
+   * Use this to effectively ignore UNKNOWN states for TriStates in
+   * RobotStatus messages. This can help with OEM server programs which
+   * are unable to accurately report controller status, by allowing
+   * the action server to assume UNKNOWN == OK.
+   */
+  bool consider_status_unknowns_ok_;
+
   /**
    * \brief Watch dog callback, used to detect robot driver failures
    *
@@ -218,6 +245,11 @@ class JointTrajectoryAction
    */
   void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg);
 
+  /**
+   * \brief Sends a stop command (empty message) to the robot driver.
+   */
+  void stopRelay();
+
   /**
    * \brief Aborts the current action goal and sends a stop command
    * (empty message) to the robot driver.

industrial_robot_client/include/industrial_robot_client/utils.h

@@ -36,6 +36,9 @@
 #include <string>
 #include <map>
 
+#include <industrial_msgs/RobotStatus.h>
+
+
 namespace industrial_robot_client
 {
 namespace utils
@@ -116,6 +119,46 @@ bool isWithinRange(const std::vector<std::string> & lhs_keys, const std::vector<
                    const std::vector<std::string> & rhs_keys, const std::vector<double> & rhs_values,
                    double full_range);
 
+/**
+ * \brief Check whether the given TriState is set to UNKNOWN.
+ *
+ * \param[in] state to check
+ *
+ * \return true if the state is UNKNOWN
+ */
+bool is_unknown(industrial_msgs::TriState const& state)
+{
+  return state.val == industrial_msgs::TriState::UNKNOWN;
+}
+
+/**
+ * \brief Check whether the given TriState is set to ON (or HIGH or TRUE ..).
+ *
+ * \param[in] state the state to check
+ * \param[in] unknown_is_on should UNKNOWN be considered ON?
+ *
+ * \return true if the state is ON
+ */
+bool is_on(industrial_msgs::TriState const& state, bool unknown_is_on)
+{
+  return state.val == industrial_msgs::TriState::ON
+    || (unknown_is_on && is_unknown(state));
+}
+
+/**
+ * \brief Check whether the given TriState is set to OFF (or LOW or FALSE ..).
+ *
+ * \param[in] state the state to check
+ * \param[in] unknown_is_off should UNKNOWN be considered OFF?
+ *
+ * \return true if the state is OFF
+ */
+bool is_off(industrial_msgs::TriState const& state, bool unknown_is_off)
+{
+  return state.val == industrial_msgs::TriState::OFF
+    || (unknown_is_off && is_unknown(state));
+}
+
 } //utils
 } //industrial_robot_client
 

industrial_robot_client/launch/robot_interface_download.launch

@@ -30,6 +30,9 @@
   <node pkg="industrial_robot_client" type="motion_download_interface" name="motion_download_interface"/>
 
   <!-- joint_trajectory_action: provides actionlib interface for high-level robot control -->
-  <node pkg="industrial_robot_client" type="joint_trajectory_action" name="joint_trajectory_action"/>
+  <node pkg="industrial_robot_client" type="joint_trajectory_action" name="joint_trajectory_action">
+    <param name="ignore_motion_server_error" type="bool" value="true" />
+    <param name="consider_status_unknowns_ok" type="bool" value="true" />
+  </node>
 
 </launch>

industrial_robot_client/launch/robot_interface_streaming.launch

@@ -30,7 +30,9 @@
   <node pkg="industrial_robot_client" type="motion_streaming_interface" name="motion_streaming_interface"/>
 
   <!-- joint_trajectory_action: provides actionlib interface for high-level robot control -->
-  <node pkg="industrial_robot_client" type="joint_trajectory_action" name="joint_trajectory_action"/>
+  <node pkg="industrial_robot_client" type="joint_trajectory_action" name="joint_trajectory_action">
+    <param name="ignore_motion_server_error" type="bool" value="true" />
+    <param name="consider_status_unknowns_ok" type="bool" value="true" />
+  </node>
 
 </launch>
-

industrial_robot_client/src/joint_trajectory_action.cpp

@@ -45,12 +45,28 @@ const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01;
 JointTrajectoryAction::JointTrajectoryAction() :
     action_server_(node_, "joint_trajectory_action", boost::bind(&JointTrajectoryAction::goalCB, this, _1),
                    boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false),
-                   controller_alive_(false), has_moved_once_(false), name_("joint_trajectory_action")
+                   controller_alive_(false), has_moved_once_(false), name_("joint_trajectory_action"),
+                   ignore_motion_server_error_(false), consider_status_unknowns_ok_(false)
 {
   ros::NodeHandle pn("~");
 
   pn.param("constraints/goal_threshold", goal_threshold_, DEFAULT_GOAL_THRESHOLD_);
 
+  // Two parameters for bw-compatibility with the 'old' behaviour.
+  // Setting these by default to TRUE, to maintain the previous behaviour
+  pn.param("ignore_motion_server_error", ignore_motion_server_error_, true);
+  pn.param("consider_status_unknowns_ok", consider_status_unknowns_ok_, true);
+  std::string log_msg = std::string("Ignoring motion server errors: ") + (ignore_motion_server_error_ ? "true" : "false");
+  if (ignore_motion_server_error_)
+    ROS_WARN_STREAM_NAMED(name_, log_msg);
+  else
+    ROS_INFO_STREAM_NAMED(name_, log_msg);
+  log_msg = std::string("Treating RobotStatus fields with UNKNOWNs as ok: ") + (consider_status_unknowns_ok_ ? "true" : "false");
+  if (consider_status_unknowns_ok_)
+    ROS_WARN_STREAM_NAMED(name_, log_msg);
+  else
+    ROS_INFO_STREAM_NAMED(name_, log_msg);
+
   if (!industrial_utils::param::getJointNames("controller_joint_names", "robot_description", joint_names_))
     ROS_ERROR_NAMED(name_, "Failed to initialize joint_names.");
 
@@ -107,6 +123,58 @@ void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
   }
 }
 
+bool is_motion_server_ok(industrial_msgs::RobotStatusConstPtr& msg, bool unknown_is_ok = false)
+{
+  // unless it's OK for values to be UNKNOWN, the state relay must report
+  //  - motion_possible == true
+  //  - in_error == false
+  //  - e_stopped == false
+  //  - no error code
+  return utils::is_on(msg->motion_possible, unknown_is_ok)
+    && msg->error_code == 0
+    && utils::is_off(msg->in_error, unknown_is_ok)
+    && utils::is_off(msg->e_stopped, unknown_is_ok);
+}
+
+std::string describe_robot_status_msg(industrial_msgs::RobotStatusConstPtr& msg, bool unknown_is_on = false)
+{
+  std::stringstream ss;
+
+  // mention e-stop specifically
+  if (utils::is_on(msg->e_stopped, unknown_is_on))
+  {
+    ss.clear();
+    ss << "controller reported e-stop";
+  }
+  // some (generic ?) other error
+  else if (msg->error_code != 0 || utils::is_on(msg->in_error, unknown_is_on))
+  {
+    ss.clear();
+    ss << "controller reported (active) error";
+
+    // it could be state server does not report specific error codes
+    if (msg->error_code != 0)
+    {
+      ss << " (OEM code: " << msg->error_code << ")";
+    }
+    else
+    {
+      ss << " (0 or no OEM error code communicated)";
+    }
+  }
+  // we use this last, as it could be we currently don't yet know *why*
+  // the state server decides motion_possible == false. So we first
+  // check the specific problems above, then fall back to this generic
+  // "it doesn't work, don't know why" statement
+  else if (utils::is_off(msg->motion_possible, unknown_is_on))
+  {
+    ss.clear();
+    ss << "controller reported motion not possible (no further information)";
+  }
+
+  return ss.str();
+}
+
 void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh)
 {
   ROS_INFO_STREAM_NAMED(name_, "Received new goal");
@@ -123,6 +191,24 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh)
     return;
   }
 
+  // check robot can actually execute trajectory, if not, refuse goal.
+  // no point in accepting the goal only to cancel it immediately later
+  if (!is_motion_server_ok(last_robot_status_, consider_status_unknowns_ok_) && !ignore_motion_server_error_)
+  {
+    // translate status into user readable description
+    const std::string reject_msg = {"Rejecting goal: "
+      + describe_robot_status_msg(last_robot_status_, consider_status_unknowns_ok_) };
+    ROS_ERROR_STREAM_NAMED(name_, reject_msg);
+    control_msgs::FollowJointTrajectoryResult rslt;
+    // the goal is actually probably OK, but we have to choose one of the existing
+    // constants, and this one comes closest
+    rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
+    gh.setRejected(rslt, reject_msg);
+
+    // no point in continuing: already rejected
+    return;
+  }
+
   if (!gh.getGoal()->trajectory.points.empty())
   {
     if (industrial_utils::isSimilar(joint_names_, gh.getGoal()->trajectory.joint_names))
@@ -186,9 +272,7 @@ void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle gh)
   if (active_goal_ == gh)
   {
     // Stops the controller.
-    trajectory_msgs::JointTrajectory empty;
-    empty.joint_names = joint_names_;
-    pub_trajectory_command_.publish(empty);
+    stopRelay();
 
     // Marks the current goal as canceled.
     active_goal_.setCanceled();
@@ -227,9 +311,32 @@ void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTra
     return;
   }
 
+  // see if we need to abort the goal due to on-controller errors.
+  // NOTE: we do this *before* checking has_moved_once_, as otherwise we would not
+  // notice problems on the controller side unless the robot has already moved,
+  // which it may be unable to do.
+  if(!is_motion_server_ok(last_robot_status_, consider_status_unknowns_ok_) && !ignore_motion_server_error_)
+  {
+    const std::string abort_msg = {"Aborting goal: "
+      + describe_robot_status_msg(last_robot_status_, consider_status_unknowns_ok_) };
+
+    // Stop the relay
+    stopRelay();
+
+    // return abort to action client
+    control_msgs::FollowJointTrajectoryResult result;
+    // would like to use a better error constant, but we have to choose one of the existing
+    // constants, and this one comes closest
+    result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
+    active_goal_.setAborted(result, abort_msg);
+    has_active_goal_ = false;
+    ROS_ERROR_STREAM_NAMED(name_, abort_msg);
+    return;
+  }
+
   if (!has_moved_once_ && (ros::Time::now() < time_to_check_))
   {
-    ROS_INFO_NAMED(name_, "Waiting to check for goal completion until halfway through trajectory");
+    ROS_DEBUG_NAMED(name_, "Waiting to check for goal completion until halfway through trajectory");
     return;
   }
 
@@ -245,13 +352,13 @@ void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTra
       // be moving.  The current robot driver calls a motion stop if it receives
       // a new trajectory while it is still moving.  If the driver is not publishing
       // the motion state (i.e. old driver), this will still work, but it warns you.
-      if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE)
+      if (utils::is_off(last_robot_status_->in_motion, /*unknown_is_off=*/false))
       {
-        ROS_INFO_NAMED("joint_trajectory_action.controllerStateCB", "Inside goal constraints - stopped moving-  return success for action");
+        ROS_INFO_NAMED("joint_trajectory_action.controllerStateCB", "Inside goal constraints - stopped moving - return success for action");
         active_goal_.setSucceeded();
         has_active_goal_ = false;
       }
-      else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN)
+      else if (utils::is_unknown(last_robot_status_->in_motion))
       {
         ROS_INFO_NAMED(name_, "Inside goal constraints, return success for action");
         ROS_WARN_NAMED(name_, "Robot status in motion unknown, the robot driver node and controller code should be updated");
@@ -273,12 +380,16 @@ void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTra
   }
 }
 
-void JointTrajectoryAction::abortGoal()
+void JointTrajectoryAction::stopRelay()
 {
   // Stops the controller.
   trajectory_msgs::JointTrajectory empty;
   pub_trajectory_command_.publish(empty);
+}
 
+void JointTrajectoryAction::abortGoal()
+{
+  stopRelay();
   // Marks the current goal as aborted.
   active_goal_.setAborted();
   has_active_goal_ = false;