On-the-fly point streaming and Cartesian jog

motoman_config/cfg/sia10d_mesh.xml

@@ -182,4 +182,3 @@
     <limit effort="100" lower="-3.1416" upper="3.1416" velocity="6.97"/>
   </joint>
 </robot>
-

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h

@@ -69,7 +69,7 @@ class JointTrajectoryInterface
 public:
 
  /**
-  * \brief Default constructor.
+  * \brief Default constructor.
   */
     JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0) {};
 
@@ -180,7 +180,7 @@ class JointTrajectoryInterface
    * \param[in] pt trajectory point data, in order/count expected by robot connection
    * \param[out] rbt_velocity computed velocity scalar for robot message (if needed by robot)
    *
-   * \return true on success, false otherwise
+   * \return true on success, false otherwise
    */
   virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity);
 
@@ -212,6 +212,8 @@ class JointTrajectoryInterface
    * \param msg JointTrajectory message from ROS trajectory-planner
    */
   virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
+
+  virtual void jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
 
   /**
    * \brief Callback function registered to ROS stopMotion service
@@ -245,6 +247,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::ServiceServer srv_stop_motion_;   // handle for stop_motion service
   std::vector<std::string> all_joint_names_;

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h

@@ -34,6 +34,7 @@
 
 #include <boost/thread/thread.hpp>
 #include "motoman_driver/industrial_robot_client/joint_trajectory_interface.h"
+#include <queue>
 
 namespace industrial_robot_client
 {
@@ -48,7 +49,7 @@ namespace TransferStates
 {
 enum TransferState
 {
-  IDLE = 0, STREAMING =1 //,STARTING, //, STOPPING
+  IDLE = 0, STREAMING =1, POINT_STREAMING = 2 //,STARTING, //, STOPPING
 };
 }
 typedef TransferStates::TransferState TransferState;
@@ -74,7 +75,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) {};
 
@@ -96,6 +97,8 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface
   ~JointTrajectoryStreamer();
 
   virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
+
+  virtual void jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
 
   virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<SimpleMessage>* msgs);
 
@@ -109,11 +112,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_;
 };
 
 } //joint_trajectory_streamer

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

@@ -71,7 +71,7 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
   /**
    * \brief Default constructor
    *
-   * \param robot_id robot group # on this controller (for multi-group systems)
+   * \param robot_id robot group # on this controller (for multi-group systems)
    */
   MotomanJointTrajectoryStreamer(int robot_id=-1) : JointTrajectoryStreamer(1),
                                                   robot_id_(robot_id) {}
@@ -110,7 +110,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_;
@@ -120,6 +120,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;
 };
 
 } //joint_trajectory_streamer

motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

@@ -104,6 +104,8 @@ bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::ve
   this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this);
   this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this);
   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;
 }
@@ -148,6 +150,11 @@ void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTra
   send_to_robot(robot_msgs);
 }
 
+void JointTrajectoryInterface::jointCommandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
+{
+  //ROS_INFO("Yessir?");
+}
+
 bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector<SimpleMessage>* msgs)
 {
   msgs->clear();

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

@@ -99,6 +99,74 @@ 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");
@@ -199,8 +267,41 @@ 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

@@ -235,8 +235,75 @@ void MotomanJointTrajectoryStreamer::streamingThread()
             break;
           }
         }
+
+        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;