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

@@ -18,6 +18,37 @@
   <!-- robot_ip: IP-address of the robot's socket-messaging server -->
   <arg name="robot_ip" doc="IP of the 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,6 +61,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="$(arg ignore_motion_server_error)" />
+    <param name="consider_status_unknowns_ok" type="bool" value="$(arg consider_status_unknowns_ok)" />
+  </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>
-