Direct streaming

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h

@@ -69,9 +69,9 @@ class JointTrajectoryInterface
 public:
 
  /**
-  * \brief Default constructor.
+  * \brief Default constructor.
   */
-    JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0) {};
+    JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0), timeout_(ros::Duration(0.2)) {};
 
     /**
      * \brief Initialize robot connection using default method.
@@ -116,6 +116,28 @@ class JointTrajectoryInterface
    */
   virtual void run() { ros::spin(); }
 
+  /**
+   * \brief Setter function for the timeout parameter.
+   * 
+   * \param timeout double Timeout value in seconds (must be greater than 0).
+   * \return true if the timeout value is correct, false otherwise.
+   */
+  bool setTimeout(double timeout) { 
+	  if(timeout > 0)
+	  {
+		timeout_ = ros::Duration(timeout);
+		return true;
+	  }
+	  else return false;
+  }
+
+   /**
+   * \brief Getter function for the timeout parameter.
+   *
+   * \return Current timeout value in ros::Duration.
+   */
+  ros::Duration getTimeout() { return timeout_; }
+
 protected:
 
   /**
@@ -180,7 +202,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);
 
@@ -253,6 +275,7 @@ class JointTrajectoryInterface
   double default_duration_;   // default duration to use for joint commands, if no
   std::map<std::string, double> joint_vel_limits_;  // cache of max joint velocities from URDF
   sensor_msgs::JointState cur_joint_pos_;  // cache of last received joint state
+  ros::Duration timeout_;   // Time in seconds between the last sent point to the controller and going back to IDLE state. By default 200ms.
 
 private:
   /**

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

@@ -74,7 +74,7 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
    * \param robot_id robot group # on this controller (for multi-group systems)
    */
   MotomanJointTrajectoryStreamer(int robot_id=-1) : JointTrajectoryStreamer(1),
-                                                  robot_id_(robot_id) {}
+                                                  robot_id_(robot_id), seq_offset_(0) {}
 
   ~MotomanJointTrajectoryStreamer();
 
@@ -112,6 +112,8 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer
   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)
 
+  size_t seq_offset_;
+
   int robot_id_;
   MotomanMotionCtrl motion_ctrl_;
 

motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

@@ -51,10 +51,12 @@ bool JointTrajectoryInterface::init(std::string default_ip, int default_port)
 {
   std::string ip;
   int port;
+  double timeout;
 
-  // override IP/port with ROS params, if available
+  // override IP/port/timeout with ROS params, if available
   ros::param::param<std::string>("robot_ip_address", ip, default_ip);
   ros::param::param<int>("~port", port, default_port);
+  ros::param::param<double>("timeout", timeout, timeout_.toSec());
 
   // check for valid parameter values
   if (ip.empty())
@@ -67,6 +69,11 @@ bool JointTrajectoryInterface::init(std::string default_ip, int default_port)
     ROS_ERROR("No valid robot IP port found.  Please set ROS '~port' param");
     return false;
   }
+  if(!setTimeout(timeout)) 
+  {
+    ROS_ERROR("Invalid timeout. Please set ROS 'timeout' param to a positive value (in seconds)");
+    return false;
+  }
 
   char* ip_addr = strdup(ip.c_str()); // connection.init() requires "char*", not "const char*"
   ROS_INFO("Joint Trajectory Interface connecting to IP address: '%s:%d'", ip_addr, port);
@@ -89,7 +96,7 @@ bool JointTrajectoryInterface::init(SmplMsgConnection* connection)
 }
 
 bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
-                                    const std::map<std::string, double> &velocity_limits)
+  const std::map<std::string, double> &velocity_limits)
 {
   this->connection_ = connection;
   this->all_joint_names_ = joint_names;
@@ -115,7 +122,7 @@ JointTrajectoryInterface::~JointTrajectoryInterface()
 }
 
 bool JointTrajectoryInterface::jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
-                                                 industrial_msgs::CmdJointTrajectory::Response &res)
+  industrial_msgs::CmdJointTrajectory::Response &res)
 {
   trajectory_msgs::JointTrajectoryPtr traj_ptr(new trajectory_msgs::JointTrajectory);
   *traj_ptr = req.trajectory;  // copy message data
@@ -180,7 +187,7 @@ bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTr
 }
 
 bool JointTrajectoryInterface::select(const std::vector<std::string>& ros_joint_names, const ros_JointTrajPt& ros_pt,
-                      const std::vector<std::string>& rbt_joint_names, ros_JointTrajPt* rbt_pt)
+  const std::vector<std::string>& rbt_joint_names, ros_JointTrajPt* rbt_pt)
 {
   ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size());
 
@@ -254,7 +261,7 @@ bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTraject
 
   // find largest velocity-ratio (closest to max joint-speed)
   int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin();
-  
+
   if (vel_ratios[max_idx] > 0)
     *rbt_velocity = vel_ratios[max_idx];
   else
@@ -268,7 +275,7 @@ bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTraject
     ROS_WARN("computed velocity (%.1f %%) is out-of-range.  Clipping to [0-100%%]", *rbt_velocity * 100);
     *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity));  // clip to [0,1]
   }
-  
+
   return true;
 }
 
@@ -323,7 +330,7 @@ void JointTrajectoryInterface::trajectoryStop()
 }
 
 bool JointTrajectoryInterface::stopMotionCB(industrial_msgs::StopMotion::Request &req,
-                                            industrial_msgs::StopMotion::Response &res)
+  industrial_msgs::StopMotion::Response &res)
 {
   trajectoryStop();
 

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

@@ -39,7 +39,7 @@ namespace joint_trajectory_streamer
 {
 
 bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
-                                   const std::map<std::string, double> &velocity_limits)
+  const std::map<std::string, double> &velocity_limits)
 {
   bool rtn = true;
 
@@ -51,7 +51,7 @@ bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vec
   this->current_point_ = 0;
   this->state_ = TransferStates::IDLE;
   this->streaming_thread_ =
-      new boost::thread(boost::bind(&JointTrajectoryStreamer::streamingThread, this));
+    new boost::thread(boost::bind(&JointTrajectoryStreamer::streamingThread, this));
   ROS_INFO("Unlocking mutex");
   this->mutex_.unlock();
 
@@ -73,20 +73,14 @@ void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTraj
   ROS_DEBUG("Current state is: %d", state);
   if (TransferStates::IDLE != state)
   {
-    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;
+    if (!msg->points.empty())
+      ROS_WARN("Trajectory splicing not yet implemented, switching to this new trajectory.");
   }
 
   if (msg->points.empty())
   {
     ROS_INFO("Empty trajectory received while in IDLE state, nothing is done");
+    trajectoryStop();
     return;
   }
 
@@ -136,6 +130,7 @@ bool JointTrajectoryStreamer::trajectory_to_msgs(const trajectory_msgs::JointTra
 void JointTrajectoryStreamer::streamingThread()
 {
   int connectRetryCount = 1;
+  ros::Time timeoutStart = ros::Time::now();
 
   ROS_INFO("Starting joint trajectory streamer thread");
   while (ros::ok())
@@ -154,55 +149,63 @@ void JointTrajectoryStreamer::streamingThread()
       else if (connectRetryCount <= 0)
       {
         ROS_ERROR("Timeout connecting to robot controller.  Send new motion command to retry.");
-        this->state_ = TransferStates::IDLE;
+        this->mutex_.lock();
+        trajectoryStop();
+        this->mutex_.unlock();
       }
       continue;
     }
 
     this->mutex_.lock();
 
     SimpleMessage msg, tmpMsg, reply;
-        
+
     switch (this->state_)
     {
-      case TransferStates::IDLE:
-        ros::Duration(0.250).sleep();  //  slower loop while waiting for new trajectory
-        break;
+    case TransferStates::IDLE:
+      ros::Duration(0.250).sleep();  //  slower loop while waiting for new trajectory
+      break;
 
-      case TransferStates::STREAMING:
-        if (this->current_point_ >= (int)this->current_traj_.size())
+    case TransferStates::STREAMING:
+      if (this->current_point_ >= (int)this->current_traj_.size())
+      {
+        if(ros::Time::now() - timeoutStart > timeout_)
         {
           ROS_INFO("Trajectory streaming complete, setting state to IDLE");
-          this->state_ = TransferStates::IDLE;
-          break;
+          trajectoryStop();
         }
+        else ROS_DEBUG("Waiting for a new point");
+        break;
+      }
 
-        if (!this->connection_->isConnected())
-        {
-          ROS_DEBUG("Robot disconnected.  Attempting reconnect...");
-          connectRetryCount = 5;
-          break;
-        }
+      if (!this->connection_->isConnected())
+      {
+        ROS_DEBUG("Robot disconnected.  Attempting reconnect...");
+        connectRetryCount = 5;
+        break;
+      }
 
-        tmpMsg = this->current_traj_[this->current_point_];
-        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 of %d] sent to controller",
-                   this->current_point_, (int)this->current_traj_.size());
-          this->current_point_++;
-        }
-        else
-          ROS_WARN("Failed sent joint point, will try again");
+      tmpMsg = this->current_traj_[this->current_point_];
+      msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
+        ReplyTypes::INVALID, tmpMsg.getData());  // set commType=REQUEST
 
-        break;
-      default:
-        ROS_ERROR("Joint trajectory streamer: unknown state");
-        this->state_ = TransferStates::IDLE;
-        break;
+      ROS_DEBUG("Sending joint trajectory point");
+      if (this->connection_->sendAndReceiveMsg(msg, reply, false))
+      {
+        ROS_INFO("Point[%d of %d] sent to controller",
+          this->current_point_, (int)this->current_traj_.size());
+        this->current_point_++;
+      }
+      else
+        ROS_WARN("Failed sent joint point, will try again");
+
+      timeoutStart = ros::Time::now();   // keep track of the timeout
+
+      break;
+    default:
+      ROS_ERROR("Joint trajectory streamer: unknown state");
+      trajectoryStop();
+      break;
     }
 
     this->mutex_.unlock();