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

industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h

@@ -176,6 +176,37 @@ 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'.
+   *
+   * The constructor overrides the default here with the value of the private
+   * parameter on the parameter server with the same name.
+   *
+   * 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_ = false;
+
+  /**
+   * \brief Should the action server consider UNKNOWN for TriStates in
+   * a RobotStatus message as OK?
+   *
+   * This is a backwards compatibility 'tunable knob'.
+   *
+   * The constructor overrides the default here with the value of the private
+   * parameter on the parameter server with the same name.
+   *
+   * 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_ = false;
+
   /**
    * \brief Watch dog callback, used to detect robot driver failures
    *
@@ -218,6 +249,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/TriState.h>
+
+
 namespace industrial_robot_client
 {
 namespace utils
@@ -116,6 +119,51 @@ 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);
 
+
+namespace tri_state {
+
+/**
+ * \brief Check whether the given TriState is set to UNKNOWN.
+ *
+ * \param[in] state to check
+ *
+ * \return true if the state is UNKNOWN
+ */
+bool isUnknown(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 isOn(industrial_msgs::TriState const& state, bool unknown_is_on)
+{
+  return state.val == industrial_msgs::TriState::ON
+    || (unknown_is_on && isUnknown(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 isOff(industrial_msgs::TriState const& state, bool unknown_is_off)
+{
+  return state.val == industrial_msgs::TriState::OFF
+    || (unknown_is_off && isUnknown(state));
+}
+
+} //tri_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

@@ -12,12 +12,43 @@
          - joint_trajectory_action : actionlib interface to control robot motion
 
     Usage:
-      robot_interface_streaming.launch robot_ip:=<value>
+      robot_interface_streaming.launch robot_ip:=<value> [other arguments [..]]
   -->
 
   <!-- robot_ip: IP-address of the robot's socket-messaging server -->
   <arg name="robot_ip" doc="IP of controller" />
 
+  <!-- Prior to https://github.com/ros-industrial/industrial_core/pull/271, the
+       joint_trajectory_action server ignored the state of the OEM server program
+       and essentially open-loop forwarded incoming trajectories to the other
+       nodes in industrial_robot_client.
+
+       The new behaviour is to monitor the OEM server programs and reject
+       incoming goals and abort active ones when needed.
+
+       Set this argument to true to revert back to the old behaviour.
+       If this argument is set to false, the new behaviour will be used.
+  -->
+  <arg name="ignore_motion_server_error" default="false"
+    doc="Should OEM controller state be monitored while accepting goals and during execution?" />
+
+  <!-- Some OEM server programs (state relays running on the OEM controller)
+       cannot (reliably) determine values for certain fields in RobotStatus
+       messages. Those fields should then be set to TriState::UNKNOWN by those
+       servers.
+
+       UNKNOWN is essentially treated like OFF (or FALSE) by the code which
+       monitors the OEM server program, so for those server programs which must
+       send UNKNOWN, the monitoring code will always detect errors and refuse to
+       execute any trajectories.
+
+       Set this parameter to true to avoid this with such OEM server programs,
+       at the cost of potentially accepting goals and attempting to execute them
+       even if the server program would not be able to actually do that.
+  -->
+  <arg name="consider_status_unknowns_ok" default="false"
+    doc="Should UNKNOWNs in RobotStatus messages be considered as OKs?" />
+
   <!-- copy the specified IP address to the Parameter Server, for use by nodes below -->
   <param name="/robot_ip_address" type="str" value="$(arg robot_ip)"/>
 
@@ -30,7 +61,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="$(arg ignore_motion_server_error)" />
+    <param name="consider_status_unknowns_ok" type="bool" value="$(arg consider_status_unknowns_ok)" />
+  </node>
 
 </launch>
-

industrial_robot_client/src/joint_trajectory_action.cpp

@@ -51,6 +51,20 @@ JointTrajectoryAction::JointTrajectoryAction() :
 
   pn.param("constraints/goal_threshold", goal_threshold_, DEFAULT_GOAL_THRESHOLD_);
 
+  // Two parameters for bw-compatibility with the 'old' behaviour.
+  pn.param("ignore_motion_server_error", ignore_motion_server_error_, ignore_motion_server_error_);
+  pn.param("consider_status_unknowns_ok", consider_status_unknowns_ok_, consider_status_unknowns_ok_);
+  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 +121,58 @@ void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
   }
 }
 
+bool isMotionServerOK(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::tri_state::isOn(msg->motion_possible, unknown_is_ok)
+    && msg->error_code == 0
+    && utils::tri_state::isOff(msg->in_error, unknown_is_ok)
+    && utils::tri_state::isOff(msg->e_stopped, unknown_is_ok);
+}
+
+std::string describeRobotStatusMsg(industrial_msgs::RobotStatusConstPtr& msg, bool unknown_is_on = false)
+{
+  std::stringstream ss;
+
+  // mention e-stop specifically
+  if (utils::tri_state::isOn(msg->e_stopped, unknown_is_on))
+  {
+    ss.clear();
+    ss << "controller reported e-stop";
+  }
+  // some (generic ?) other error
+  else if (msg->error_code != 0 || utils::tri_state::isOn(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::tri_state::isOff(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 +189,25 @@ 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 (!isMotionServerOK(last_robot_status_, consider_status_unknowns_ok_) && !ignore_motion_server_error_)
+  {
+    // translate status into user readable description
+    const std::string reject_msg = {"Rejecting goal: "
+      + describeRobotStatusMsg(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;
+    rslt.error_string = reject_msg;
+    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 +271,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 +310,33 @@ 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(!isMotionServerOK(last_robot_status_, consider_status_unknowns_ok_) && !ignore_motion_server_error_)
+  {
+    const std::string abort_msg = {"Aborting goal: "
+      + describeRobotStatusMsg(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;
+    result.error_string = abort_msg;
+    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::tri_state::isOff(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::tri_state::isUnknown(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;