poc for a feedback way from JointTrajectoryRelay to JointTrajectoryAc…

…tion See ros-industrial#131 and ros-industrial#154 for discussion
isys-vision · Nov 22, 2018 · a9ae87f · a9ae87f
1 parent ff1cdc7
commit a9ae87f
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 7 deletions.
diff --git a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h
@@ -97,6 +97,12 @@ class JointTrajectoryAction
    */
   ros::Subscriber sub_robot_status_;
 
+  /**
+   * \brief Subscribes to the relay status (typically published by the
+   * robot driver)
+   */
+  ros::Subscriber sub_relay_status_;
+
   /**
    * \brief Watchdog time used to fail the action request if the robot
    * driver is not responding.
@@ -213,6 +219,14 @@ class JointTrajectoryAction
    */
   void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg);
 
+  /**
+   * \brief Relay status callback (executed when relay status
+   * message received)
+   *
+   * \param msg relay status message
+   */
+  void relayStatusCB(const industrial_msgs::RobotStatusConstPtr &msg);
+
   /**
    * \brief Aborts the current action goal and sends a stop command
    * (empty message) to the robot driver.

diff --git a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h
@@ -74,7 +74,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface
   /**
    * \brief Default constructor
    *
-   * \param min_buffer_size minimum number of points as required by robot implementation
+   * \param min_buffer_size minimum number of points as required by robot implementation
    */
   JointTrajectoryStreamer(int min_buffer_size = 1) : min_buffer_size_(min_buffer_size) {};
 
@@ -114,6 +114,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface
   TransferState state_;
   ros::Time streaming_start_;
   int min_buffer_size_;
+  ros::Publisher feedback_publisher_;
 };
 
 } //joint_trajectory_streamer

diff --git a/industrial_robot_client/src/joint_trajectory_action.cpp b/industrial_robot_client/src/joint_trajectory_action.cpp
@@ -62,6 +62,7 @@ JointTrajectoryAction::JointTrajectoryAction() :
   pub_trajectory_command_ = node_.advertise<trajectory_msgs::JointTrajectory>("joint_path_command", 1);
   sub_trajectory_state_ = node_.subscribe("feedback_states", 1, &JointTrajectoryAction::controllerStateCB, this);
   sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this);
+  sub_relay_status_ = node_.subscribe("relay_feedback", 1, &JointTrajectoryAction::relayStatusCB, this);
 
   watchdog_timer_ = node_.createTimer(ros::Duration(WATCHDOG_PERIOD_), &JointTrajectoryAction::watchdog, this, true);
   action_server_.start();
@@ -77,6 +78,18 @@ void JointTrajectoryAction::robotStatusCB(const industrial_msgs::RobotStatusCons
   has_moved_once_ = has_moved_once_ ? true : (last_robot_status_->in_motion.val == industrial_msgs::TriState::TRUE);
 }
 
+void JointTrajectoryAction::relayStatusCB(const industrial_msgs::RobotStatusConstPtr &msg)
+{
+    ROS_ERROR("FEEDBACK from relay received");
+    if (msg->in_error.val == industrial_msgs::TriState::TRUE) {
+        ROS_ERROR("ERROR in relay");
+        control_msgs::FollowJointTrajectoryResult rslt;
+        rslt.error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
+        active_goal_.setAborted(rslt, "Error in trajectory relay");
+        has_active_goal_ = false;
+    }
+}
+
 void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
 {
   // Some debug logging

diff --git a/industrial_robot_client/src/joint_trajectory_streamer.cpp b/industrial_robot_client/src/joint_trajectory_streamer.cpp
@@ -30,6 +30,7 @@
  */
 
 #include "industrial_robot_client/joint_trajectory_streamer.h"
+#include "industrial_msgs/RobotStatus.h"
 
 using industrial::simple_message::SimpleMessage;
 
@@ -47,6 +48,8 @@ bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vec
 
   rtn &= JointTrajectoryInterface::init(connection, joint_names, velocity_limits);
 
+  feedback_publisher_ = node_.advertise<industrial_msgs::RobotStatus>("relay_feedback", 1);
+
   this->mutex_.lock();
   this->current_point_ = 0;
   this->state_ = TransferStates::IDLE;
@@ -73,14 +76,19 @@ void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTraj
   ROS_DEBUG("Current state is: %d", state);
   if (TransferStates::IDLE != state)
   {
-    if (msg->points.empty())
+    if (msg->points.empty()) {
       ROS_INFO("Empty trajectory received, canceling current trajectory");
-    else
-      ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");
 
-	this->mutex_.lock();
-    trajectoryStop();
-	this->mutex_.unlock();
+      this->mutex_.lock();
+      trajectoryStop();
+      this->mutex_.unlock();
+    } else {
+      ROS_ERROR("Trajectory splicing not yet implemented, finishing current motion. Try again later");
+      industrial_msgs::RobotStatus feedback;
+      feedback.in_error.val = industrial_msgs::TriState::TRUE;
+      feedback_publisher_.publish(feedback);
+    }
+
     return;
   }