Alternative implementation of multi-group trajectories and update motoman error handling

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
@@ -95,8 +96,6 @@ class JointTrajectoryAction
    */
   ros::Publisher pub_trajectory_command_;
 
-  std::map<int, ros::Publisher> pub_trajectories_;
-
   std::map<int, RobotGroup> robot_groups_;
 
   /**
@@ -105,45 +104,50 @@ class JointTrajectoryAction
    */
   ros::Subscriber sub_trajectory_state_;
 
-  std::map<int, ros::Subscriber> sub_trajectories_;
-
   /**
    * \brief Subscribes to the robot status (typically published by the
    * robot driver).
    */
   ros::Subscriber sub_robot_status_;
 
-  std::map<int, ros::Subscriber> sub_status_;
+  /**
+   * \brief Subscribes to motoros error messages to abort trajectories.
+   */
+  ros::Subscriber sub_motoros_errors_;
 
-  std::map<int, JointTractoryActionServer*> act_servers_;
   /**
    * \brief Watchdog time used to fail the action request if the robot
    * driver is not responding.
    */
   ros::Timer watchdog_timer_;
 
-  std::map<int, ros::Timer>watchdog_timer_map_;
+  /**
+   * \brief The time that the active trajectory is expected to end execution.
+   * Used to determine when to begin checking for goal completion.
+   */
+  ros::Time expected_traj_end_;
 
   /**
    * \brief Indicates action has an active goal
    */
   bool has_active_goal_;
 
-  std::map<int, bool> has_active_goal_map_;
+  /**
+   * \brief Indicates which groups are active on `multi-group` action goals,
+   * Note: independant of `has_active_goal_map_` which handles single group goals.
+   */
+  std::map<int, bool> has_active_goals_map_;
 
   /**
    * \brief Cache of the current active goal
    */
   JointTractoryActionServer::GoalHandle active_goal_;
 
-  std::map<int, JointTractoryActionServer::GoalHandle> active_goal_map_;
   /**
    * \brief Cache of the current active trajectory
    */
   trajectory_msgs::JointTrajectory current_traj_;
 
-  std::map<int, trajectory_msgs::JointTrajectory> current_traj_map_;
-
   std::vector<std::string> all_joint_names_;
 
   /**
@@ -161,13 +165,6 @@ class JointTrajectoryAction
    */
   double goal_threshold_;
 
-  /**
-   * \brief The joint names associated with the robot the action is
-   * interfacing with.  The joint names must be the same as expected
-   * by the robot driver.
-   */
-  std::vector<std::string> joint_names_;
-
   /**
    * \brief Cache of the last subscribed feedback message
    */
@@ -179,8 +176,6 @@ class JointTrajectoryAction
    * \brief Indicates trajectory state has been received.  Used by
    * watchdog to determine if the robot driver is responding.
    */
-  bool trajectory_state_recvd_;
-
   std::map<int, bool> trajectory_state_recvd_map_;
 
   /**
@@ -191,7 +186,7 @@ class JointTrajectoryAction
   /**
    * \brief The watchdog period (seconds)
    */
-  static const double WATCHD0G_PERIOD_;  // = 1.0;
+  static const double WATCHDOG_PERIOD_;  // = 1.0;
 
   /**
    * \brief Watch dog callback, used to detect robot driver failures
@@ -201,8 +196,6 @@ class JointTrajectoryAction
    */
   void watchdog(const ros::TimerEvent &e);
 
-  void watchdog(const ros::TimerEvent &e, int group_number);
-
   /**
    * \brief Action server goal callback method
    *
@@ -220,44 +213,39 @@ class JointTrajectoryAction
    */
 
   void cancelCB(JointTractoryActionServer::GoalHandle gh);
+
   /**
    * \brief Controller state callback (executed when feedback message
    * received)
    *
    * \param msg joint trajectory feedback message
    *
    */
-
-  void goalCB(JointTractoryActionServer::GoalHandle gh, int group_number);
+  void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg);
 
   /**
-   * \brief Action server cancel callback method
+   * \brief Controller status callback (executed when robot status
+   *  message received)
    *
-   * \param gh goal handle
+   * \param msg robot status message
    *
    */
+  void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg);
 
-  void cancelCB(JointTractoryActionServer::GoalHandle gh, int group_number);
   /**
-   * \brief Controller state callback (executed when feedback message
-   * received)
+   * \brief Motoross error callback (executed when motoros error
+   * message received)
    *
-   * \param msg joint trajectory feedback message
+   * \param msg Motoros error message
    *
    */
-  void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg);
-
-  void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, int robot_id);
-
+  void motorosErrorCB(const motoman_msgs::MotorosError &msg);
 
   /**
-   * \brief Controller status callback (executed when robot status
-   *  message received)
-   *
-   * \param msg robot status message
+   * \brief Marks the current action goal as succeeded.
    *
    */
-  void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg);
+  void setGoalSuccess();
 
   /**
    * \brief Aborts the current action goal and sends a stop command
@@ -267,7 +255,12 @@ class JointTrajectoryAction
    */
   void abortGoal();
 
-  void abortGoal(int robot_id);
+  /**
+   * \brief Cancels the current action goal and sends a stop command
+   * (empty message) to the robot driver.
+   *
+   */
+  void cancelGoal();
 
   /**
    * \brief Controller status callback (executed when robot status
@@ -279,15 +272,10 @@ class JointTrajectoryAction
    * \return true if all joints are within goal contraints
    *
    */
-  bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
-                             const trajectory_msgs::JointTrajectory & traj);
-
-  bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
-                             const trajectory_msgs::JointTrajectory & traj, int robot_id);
+  bool withinGoalConstraints(const trajectory_msgs::JointTrajectory & traj);
 };
 
 }  // namespace joint_trajectory_action
 }  // namespace industrial_robot_client
 
 #endif  // MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_ACTION_H
-

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

@@ -132,6 +132,11 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
 
   virtual void streamingThread();
 
+  /**
+   * \brief Safely disable Yaskawa arm before system shutdown.
+   */
+  void shutdown();
+
 protected:
   int robot_id_;
   MotomanMotionCtrl motion_ctrl_;
@@ -168,6 +173,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.