merge branch boneil_point_streaming manually

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h

@@ -300,6 +300,8 @@ class JointTrajectoryInterface
    */
   virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
 
+  virtual void jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
+
   /**
    * \brief Callback function registered to ROS stopMotion service
    *   Sends stop-motion command to robot.
@@ -342,6 +344,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);
@@ -129,11 +132,13 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface
 
   boost::thread* streaming_thread_;
   boost::mutex mutex_;
-  int current_point_;
+  int current_point_, streaming_sequence;
   std::vector<SimpleMessage> current_traj_;
   TransferState state_;
   ros::Time streaming_start_;
   int min_buffer_size_;
+
+  std::queue<SimpleMessage> streaming_queue_;
 };
 
 }  // namespace joint_trajectory_streamer

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

@@ -130,7 +130,7 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
 
 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_  = 1e-4;  // max difference btwn start & current position, for validation (rad)
+  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 +143,10 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
 
   static bool VectorToJointData(const std::vector<double> &vec,
                                 industrial::joint_data::JointData &joints);
+
+  double time_of_last;
+  double time_since_last;
+  static const double point_streaming_timeout = 3.0;
 };
 
 }  // namespace joint_trajectory_streamer

motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

@@ -194,6 +194,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;
 }
 
@@ -337,6 +339,12 @@ void JointTrajectoryInterface::jointTrajectoryCB(
   send_to_robot(robot_msgs);
 }
 
+void JointTrajectoryInterface::jointCommandCB(
+  const trajectory_msgs::JointTrajectoryConstPtr &msg)
+{
+  //ROS_INFO("Yessir?");
+}
+
 bool JointTrajectoryInterface::trajectory_to_msgs(
   const motoman_msgs::DynamicJointTrajectoryConstPtr& traj,
   std::vector<SimpleMessage>* msgs)

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

@@ -159,6 +159,73 @@ 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);
+
+  //If current state is idle, set to POINT_STREAMING
+  if (TransferStates::IDLE == state)
+  {
+    this->mutex_.lock();
+    this->state_ = TransferStates::POINT_STREAMING;
+    this->current_point_ = 0;
+    this->streaming_sequence = 0;
+    this->streaming_start_ = ros::Time::now();
+    this->mutex_.unlock();
+    state = TransferStates::POINT_STREAMING;
+    ROS_INFO("First joint point received. Starting on-the-fly streaming.");
+  }
+
+  //if current state is POINT_STREAMING, process incoming point.
+  if (TransferStates::POINT_STREAMING == state)
+  {
+    if (msg->points.empty())
+    {
+      ROS_INFO("Empty point received, cancelling current trajectory");
+      return;
+    }
+
+    //Else, Push point into queue
+    SimpleMessage message;
+    trajectory_msgs::JointTrajectoryPoint rbt_pt, xform_pt;
+
+    // select / reorder joints for sending to robot
+    if (!select(msg->joint_names, msg->points[0], this->all_joint_names_, &rbt_pt))
+      return;
+
+    // transform point data (e.g. for joint-coupling)
+    if (!transform(rbt_pt, &xform_pt))
+      return;
+
+    // convert trajectory point to ROS message
+    if (!create_message(this->streaming_sequence, xform_pt, &message))
+      return;
+
+    //Points get pushed into queue here. They will be popped in the Streaming Thread and sent to controller.
+    this->mutex_.lock();
+    this->streaming_queue_.push(message);
+    this->streaming_sequence++;
+    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 +343,43 @@ void JointTrajectoryStreamer::streamingThread()
         ROS_WARN("Failed sent joint point, will try again");
 
       break;
+
+    case TransferStates::POINT_STREAMING:
+      ROS_INFO("I'm streaming, sir.");
+      // if no points in queue, streaming complete, set to idle.
+      if (this->streaming_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->streaming_queue_.front();
+      this->streaming_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;
+      // 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;
     }

motoman_driver/src/joint_trajectory_streamer.cpp

@@ -295,7 +295,7 @@ bool MotomanJointTrajectoryStreamer::VectorToJointData(const std::vector<double>
     JointData &joints)
 {
   if (vec.size() > joints.getMaxNumJoints())
-    ROS_ERROR_RETURN(false, "Failed to copy to JointData.  Len (%d) out of range (0 to %d)",
+    ROS_ERROR_RETURN(false, "Failed to copy to JointData.  Len (%ld) out of range (0 to %d)",
                      vec.size(), joints.getMaxNumJoints());
 
   joints.init();
@@ -401,6 +401,74 @@ void MotomanJointTrajectoryStreamer::streamingThread()
         }
       }
       break;
+
+    case TransferStates::POINT_STREAMING:
+      // if no points in queue, streaming complete, set to idle.
+      if (this->streaming_queue_.empty())
+      {
+        if (this->time_since_last < this->point_streaming_timeout)
+        {
+          time_since_last = ros::Time::now().toSec() - time_of_last;
+          ros::Duration(0.005).sleep();
+          //ROS_INFO("Time since last point: %f", time_since_last);
+          break;
+        }
+        else
+        {
+          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->streaming_queue_.front();
+      msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
+               ReplyTypes::INVALID, tmpMsg.getData());  // set commType=REQUEST
+
+      ROS_INFO("Sending joint trajectory point");
+      if (this->connection_->sendAndReceiveMsg(msg, reply, false))
+      {
+        MotionReplyMessage reply_status;
+        if (!reply_status.init(reply))
+        {
+          ROS_ERROR("Aborting point stream operation.");
+          this->state_ = TransferStates::IDLE;
+          break;
+        }
+        if (reply_status.reply_.getResult() == MotionReplyResults::SUCCESS)
+        {
+          ROS_INFO("Point[%d] sent to controller", this->current_point_);
+          this->current_point_++;
+          time_since_last = 0.0;
+          time_of_last = ros::Time::now().toSec();
+          this->streaming_queue_.pop();
+        }
+        else if (reply_status.reply_.getResult() == MotionReplyResults::BUSY)
+        {
+          ROS_INFO("silently resending.");
+          break;  // silently retry sending this point
+        }
+        else
+        {
+          ROS_ERROR_STREAM("Aborting point stream operation.  Failed to send point"
+                           << " (#" << this->current_point_ << "): "
+                           << MotomanMotionCtrl::getErrorString(reply_status.reply_));
+          this->state_ = TransferStates::IDLE;
+          break;
+        }
+      }
+      else
+        ROS_WARN("Failed sent joint point, will try again");
+
+      break;
+
     default:
       ROS_ERROR("Joint trajectory streamer: unknown state");
       this->state_ = TransferStates::IDLE;