point-streaming: further development based on PR #88

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h

@@ -300,6 +300,14 @@ class JointTrajectoryInterface
    */
   virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
 
+  /**
+   * \brief Callback function registered to joint_command topic subscriber.
+   *   Specific method is implemented in JointTrajectoryStreamer class.
+   *
+   * \param msg JointTrajectory message
+   */
+  virtual void jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg) = 0;
+
   /**
    * \brief Callback function registered to ROS stopMotion service
    *   Sends stop-motion command to robot.
@@ -342,6 +350,9 @@ class JointTrajectoryInterface
   SmplMsgConnection* connection_;
   ros::Subscriber sub_cur_pos_;  // handle for joint-state topic subscription
   ros::Subscriber sub_joint_trajectory_;  // handle for joint-trajectory topic subscription
+
+  ros::Subscriber sub_joint_command_; // handle for joint-trajectory topic subscription
+
   ros::ServiceServer srv_joint_trajectory_;  // handle for joint-trajectory service
   ros::Subscriber sub_joint_trajectory_ex_;  // handle for joint-trajectory topic subscription
   ros::ServiceServer srv_joint_trajectory_ex_;  // handle for joint-trajectory service

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h

@@ -37,6 +37,7 @@
 #include <map>
 #include <vector>
 #include <string>
+#include <queue>
 
 namespace industrial_robot_client
 {
@@ -51,7 +52,7 @@ namespace TransferStates
 {
 enum TransferState
 {
-  IDLE = 0, STREAMING = 1  // ,STARTING, //, STOPPING
+  IDLE = 0, STREAMING = 1, POINT_STREAMING = 2  // ,STARTING, //, STOPPING
 };
 }
 typedef TransferStates::TransferState TransferState;
@@ -116,6 +117,8 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface
 
   virtual void jointTrajectoryCB(const motoman_msgs::DynamicJointTrajectoryConstPtr &msg);
 
+  virtual void jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
+
   virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<SimpleMessage>* msgs);
 
   virtual bool trajectory_to_msgs(const motoman_msgs::DynamicJointTrajectoryConstPtr &traj, std::vector<SimpleMessage>* msgs);
@@ -125,6 +128,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface
   bool send_to_robot(const std::vector<SimpleMessage>& messages);
 
 protected:
+  static const size_t max_ptstreaming_queue_elements = 20;
   void trajectoryStop();
 
   boost::thread* streaming_thread_;
@@ -134,6 +138,10 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface
   TransferState state_;
   ros::Time streaming_start_;
   int min_buffer_size_;
+
+  ros::Duration ptstreaming_last_time_from_start_;   // last valid point streaming point time from start
+  int ptstreaming_seq_count_; // sequence count for point streaming (--> JointTrajPtFull::sequence_)
+  std::queue<SimpleMessage> ptstreaming_queue_; // message queue for point streaming
 };
 
 }  // namespace joint_trajectory_streamer

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

@@ -130,6 +130,8 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
   virtual void streamingThread();
 
 protected:
+  static const double pos_stale_time_ = 1.0;  // max time since last "current position" update, for validation (sec)
+  static const double start_pos_tol_  = 5e-4; // max difference btwn start & current position, for validation (rad)
 
   int robot_id_;
   MotomanMotionCtrl motion_ctrl_;
@@ -143,6 +145,11 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
   static bool VectorToJointData(const std::vector<double> &vec,
                                 industrial::joint_data::JointData &joints);
 
+  // variables for point streaming
+  double time_ptstreaming_last_point_;  // time at which the last point was received
+  double dt_ptstreaming_points_;        // elapsed time between two received points
+  static const double ptstreaming_timeout_ = 3.0; // seconds
+
   /**
    * \brief Service used to disable the robot controller.  When disabled,
    * all incoming goals are ignored.

motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

@@ -126,6 +126,8 @@ bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::ve
   this->sub_cur_pos_ = this->node_.subscribe(
                          "joint_states", 1, &JointTrajectoryInterface::jointStateCB, this);
 
+  this->sub_joint_command_ = this->node_.subscribe("joint_command", 0, &JointTrajectoryInterface::jointCommandCB, this);
+
   return true;
 }
 
@@ -733,4 +735,3 @@ void JointTrajectoryInterface::jointStateCB(
 
 }  // namespace joint_trajectory_interface
 }   // namespace industrial_robot_client
-

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

@@ -30,6 +30,8 @@
  */
 
 #include "motoman_driver/industrial_robot_client/joint_trajectory_streamer.h"
+#include <algorithm>
+#include <queue>
 #include <map>
 #include <vector>
 #include <string>
@@ -159,6 +161,170 @@ void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTraj
   send_to_robot(new_traj_msgs);
 }
 
+void JointTrajectoryStreamer::jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
+{
+  // read current state value (should be atomic)
+  int state = this->state_;
+
+  ROS_DEBUG("Current state is: %d", state);
+
+  // Point specific variables that can persist between IDLE and POINT_STREAM state
+  bool start_point = false;
+
+  const size_t num_msg_points = msg->points.size();
+
+  trajectory_msgs::JointTrajectoryPoint rbt_pt;
+
+  //If current state is idle, set to POINT_STREAMING
+  if (TransferStates::IDLE == state)
+  {
+    // Check to see if the message contains more than one trajectory point, currently the
+    // POINT_STREAMING state only accepts a single point
+    if (num_msg_points != 1)
+    {
+      ROS_ERROR("JointTrajectory command must contain a single point, ignoring message and maintaining IDLE state");
+      return;
+    }
+
+    // Get the message point and select
+    const trajectory_msgs::JointTrajectoryPoint msg_pt = msg->points[0];
+
+    if (!select(msg->joint_names, msg_pt, this->all_joint_names_, &rbt_pt))
+    {
+      // Select function will report message to console, just return here to stay in IDLE state
+      return;
+    }
+    else
+    {
+      // Check for required zero velocities
+      const double zero_tolerance = 1e-5;
+      bool zero_velocities = true;
+
+      for (size_t i = 0; i < rbt_pt.velocities.size(); ++i)
+      {
+        if (std::abs(rbt_pt.velocities[i]) > zero_tolerance )
+        {
+          zero_velocities = false;
+          break;
+        }
+      }
+
+      if (!zero_velocities)
+      {
+        ROS_ERROR("Starting joint point must contain zero velocity for each joint,"
+                  " unable to transition to on-the-fly streaming");
+        return;
+      }
+    }
+
+    // Have a valid starting point, update the flag and the time from start variable
+    // Note: last_time_from_start_ variable is only used in this function therefore we do not need to wrap in lock
+    start_point = true;
+    ptstreaming_last_time_from_start_ = msg_pt.time_from_start;
+
+    // Update point streaming sequence count, empty the point queue and set internal state to point streaming
+    this->mutex_.lock();
+    this->state_ = TransferStates::POINT_STREAMING;
+    this->ptstreaming_seq_count_ = 0;
+    this->ptstreaming_queue_ = std::queue<SimpleMessage>();
+    this->mutex_.unlock();
+
+    // Set the local state to point streaming to force enqueuing of the starting point
+    state = TransferStates::POINT_STREAMING;
+
+    ROS_INFO("Starting joint point received. Starting on-the-fly streaming.");
+  }
+
+  // Process incoming point since we are in point streaming mode
+  if (TransferStates::POINT_STREAMING == state)
+  {
+    bool stop_trajectory = false;
+
+    ros::Duration pt_time_from_start;
+
+    // Empty points is a trigger to abort the POINT_STREAMING mode by stopping the trajectory
+    if (msg->points.empty())
+    {
+      ROS_INFO("Empty point received");
+      stop_trajectory = true;
+    }
+    else if (num_msg_points != 1)  // Check for invalid number of trajectory points
+    {
+      ROS_ERROR("JointTrajectory command must contain a single point");
+      stop_trajectory = true;
+    }
+    else  // We have at one point, let check for timing if we are not the start point
+    {
+      pt_time_from_start = msg->points[0].time_from_start;
+      if (!start_point && (pt_time_from_start <= ptstreaming_last_time_from_start_))
+      {
+        ROS_ERROR("JointTrajectory point must have a time from start duration that is greater than the previously "
+                  "processes point");
+        stop_trajectory = true;
+      }
+    }
+
+    if (ptstreaming_queue_.size() > max_ptstreaming_queue_elements)  // Check for max queue size
+    {
+      ROS_ERROR("Point streaming queue has reached max allowed elements");
+      stop_trajectory = true;
+    }
+
+    // Stop the trajectory and cancel the point streaming mode if we need to
+    if (stop_trajectory)
+    {
+      ROS_INFO("Canceling on-the-fly streaming");
+      this->mutex_.lock();
+      trajectoryStop();
+      this->mutex_.unlock();
+      return;
+    }
+
+    // select / reorder joints for sending to robot (if it is not the starting point)
+    if (!start_point)
+    {
+      if (!select(msg->joint_names, msg->points[0], this->all_joint_names_, &rbt_pt))
+      {
+        return;
+      }
+    }
+
+    trajectory_msgs::JointTrajectoryPoint xform_pt;
+    // transform point data (e.g. for joint-coupling)
+    if (!transform(rbt_pt, &xform_pt))
+      return;
+
+    SimpleMessage message;
+    // convert trajectory point to ROS message
+    if (!create_message(this->ptstreaming_seq_count_, xform_pt, &message))
+      return;
+
+    // Update the last time from start
+    // Note: last_time_from_start_ variable is only used in this function therefore we do not need to wrap in lock
+    ptstreaming_last_time_from_start_ = pt_time_from_start;
+
+    // Points get pushed into queue here. They will be popped in the Streaming Thread and sent to controller.
+    this->mutex_.lock();
+    this->ptstreaming_queue_.push(message);
+    this->ptstreaming_seq_count_++;
+    this->mutex_.unlock();
+  }
+
+  //Else, cannot splice. Cancel current motion.
+  else
+  {
+    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();
+    return;
+  }
+}
+
 bool JointTrajectoryStreamer::send_to_robot(const std::vector<SimpleMessage>& messages)
 {
   ROS_INFO("Loading trajectory, setting state to streaming");
@@ -276,8 +442,47 @@ void JointTrajectoryStreamer::streamingThread()
         ROS_WARN("Failed sent joint point, will try again");
 
       break;
+
+    case TransferStates::POINT_STREAMING:
+
+      // if no points in queue, streaming complete, set to idle.
+      if (this->ptstreaming_queue_.empty())
+      {
+        ROS_INFO("Point streaming complete, setting state to IDLE");
+        this->state_ = TransferStates::IDLE;
+        break;
+      }
+      // if not connected, reconnect.
+      if (!this->connection_->isConnected())
+      {
+        ROS_DEBUG("Robot disconnected.  Attempting reconnect...");
+        connectRetryCount = 5;
+        break;
+      }
+      // otherwise, send point to robot.
+      tmpMsg = this->ptstreaming_queue_.front();
+      this->ptstreaming_queue_.pop();
+      msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
+               ReplyTypes::INVALID, tmpMsg.getData());  // set commType=REQUEST
+
+      ROS_DEBUG("Sending joint trajectory point");
+      if (this->connection_->sendAndReceiveMsg(msg, reply, false))
+      {
+        ROS_INFO("Point[%d] sent to controller", this->current_point_);
+        this->current_point_++;
+      }
+      else
+        ROS_WARN("Failed sent joint point, will try again");
+
+      break;
+      // TODO Consider checking for controller point starvation here. use a
+      //      timer to check if the state is popping in and out of
+      //      POINT_STREAMING mode, indicating something is trying to send
+      //      streaming points, but is doing so too slowly. It may, in fact, not
+      //      matter other than motion won't be smooth.
+
     default:
-      ROS_ERROR("Joint trajectory streamer: unknown state");
+      ROS_ERROR("Joint trajectory streamer: unknown state, %d", this->state_);
       this->state_ = TransferStates::IDLE;
       break;
     }
@@ -298,4 +503,3 @@ void JointTrajectoryStreamer::trajectoryStop()
 
 }  // namespace joint_trajectory_streamer
 }  // namespace industrial_robot_client
-