From 374c3826a33ada498caa8af25942ebceb7bbf88d Mon Sep 17 00:00:00 2001 From: Jim Rees Date: Thu, 21 Apr 2016 16:28:59 -0400 Subject: [PATCH 1/4] merge branch boneil_point_streaming manually original author: Brian O'Neil --- .../joint_trajectory_interface.h | 5 + .../joint_trajectory_streamer.h | 9 +- .../joint_trajectory_streamer.h | 6 +- .../joint_trajectory_interface.cpp | 8 ++ .../joint_trajectory_streamer.cpp | 104 +++++++++++++++++- .../src/joint_trajectory_streamer.cpp | 70 +++++++++++- 6 files changed, 197 insertions(+), 5 deletions(-) diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h index 2df9bd9a..cbd29fcb 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h +++ b/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 diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h index 26b2e867..df45ecb3 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h @@ -37,6 +37,7 @@ #include #include #include +#include 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* msgs); virtual bool trajectory_to_msgs(const motoman_msgs::DynamicJointTrajectoryConstPtr &traj, std::vector* 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 current_traj_; TransferState state_; ros::Time streaming_start_; int min_buffer_size_; + + std::queue streaming_queue_; }; } // namespace joint_trajectory_streamer diff --git a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h index 0479f0f0..a820ff07 100644 --- a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h +++ b/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 &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 diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp index 804198f9..bffcaf51 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp +++ b/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* msgs) diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp index 3995cae6..b8c3307a 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp +++ b/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& 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; } diff --git a/motoman_driver/src/joint_trajectory_streamer.cpp b/motoman_driver/src/joint_trajectory_streamer.cpp index 9547a68c..4d265753 100644 --- a/motoman_driver/src/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/joint_trajectory_streamer.cpp @@ -295,7 +295,7 @@ bool MotomanJointTrajectoryStreamer::VectorToJointData(const std::vector 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; From 7b316dac968a0dbf91e756bbe2a0efd2d3d3320f Mon Sep 17 00:00:00 2001 From: Thomas Lehmann Date: Wed, 16 May 2018 15:50:34 -0600 Subject: [PATCH 2/4] point-streaming: address some of the comments made in PR #88 * Addressed comments from @shaun-edwards: - Comment 1: Made JointTrajectoryInterface::jointCommandCB() pure virtual so that JointTrajectoryInterface::jointCommandCB() implementation can be deleted. - Comment 2: Message was changed to ROS_DEBUG * Some other minor changes (e.g., commenting) --- .../industrial_robot_client/joint_trajectory_interface.h | 8 +++++++- .../joint_trajectory_interface.cpp | 7 ------- .../joint_trajectory_streamer.cpp | 9 ++++++--- motoman_driver/src/joint_trajectory_streamer.cpp | 2 +- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h index cbd29fcb..e7870c18 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_interface.h @@ -300,7 +300,13 @@ class JointTrajectoryInterface */ virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); - virtual void jointCommandCB(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 diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp index 13b34999..7b464bc0 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp @@ -271,12 +271,6 @@ 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* msgs) @@ -741,4 +735,3 @@ void JointTrajectoryInterface::jointStateCB( } // namespace joint_trajectory_interface } // namespace industrial_robot_client - diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp index b8c3307a..3b49c4af 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp @@ -345,7 +345,7 @@ void JointTrajectoryStreamer::streamingThread() 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()) { @@ -376,7 +376,11 @@ void JointTrajectoryStreamer::streamingThread() 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. + // 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, %d", this->state_); @@ -400,4 +404,3 @@ void JointTrajectoryStreamer::trajectoryStop() } // namespace joint_trajectory_streamer } // namespace industrial_robot_client - diff --git a/motoman_driver/src/joint_trajectory_streamer.cpp b/motoman_driver/src/joint_trajectory_streamer.cpp index 0ae0ceb4..3b0976ba 100644 --- a/motoman_driver/src/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/joint_trajectory_streamer.cpp @@ -469,7 +469,7 @@ void MotomanJointTrajectoryStreamer::streamingThread() { 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); + ROS_DEBUG("Time since last point: %f", time_since_last); break; } else From 4440bd813a2eeea93dabaf343725bf43ce5870a9 Mon Sep 17 00:00:00 2001 From: Thomas Lehmann Date: Tue, 26 Jun 2018 13:19:29 -0600 Subject: [PATCH 3/4] point-streaming: address @gavanderhoorn's comments Addressed most of @gavanderhoorn's comments, except for comments 3 and 4. --- .../joint_trajectory_streamer.h | 7 ++-- .../joint_trajectory_streamer.h | 9 +++-- .../joint_trajectory_streamer.cpp | 16 ++++---- .../src/joint_trajectory_streamer.cpp | 39 +++++++++---------- 4 files changed, 36 insertions(+), 35 deletions(-) diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h index df45ecb3..949e303a 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h @@ -132,13 +132,14 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface boost::thread* streaming_thread_; boost::mutex mutex_; - int current_point_, streaming_sequence; + int current_point_; std::vector current_traj_; TransferState state_; ros::Time streaming_start_; int min_buffer_size_; - - std::queue streaming_queue_; + + int ptstreaming_seq_count_; // sequence count for point streaming (--> JointTrajPtFull::sequence_) + std::queue ptstreaming_queue_; // message queue for point streaming }; } // namespace joint_trajectory_streamer diff --git a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h index 6a66316a..7c84405f 100644 --- a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h @@ -144,10 +144,11 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer static bool VectorToJointData(const std::vector &vec, industrial::joint_data::JointData &joints); - - double time_of_last; - double time_since_last; - static const double point_streaming_timeout = 3.0; + + // 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, diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp index 3b49c4af..4f71b3f0 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp @@ -172,7 +172,7 @@ void JointTrajectoryStreamer::jointCommandCB(const trajectory_msgs::JointTraject this->mutex_.lock(); this->state_ = TransferStates::POINT_STREAMING; this->current_point_ = 0; - this->streaming_sequence = 0; + this->ptstreaming_seq_count_ = 0; this->streaming_start_ = ros::Time::now(); this->mutex_.unlock(); state = TransferStates::POINT_STREAMING; @@ -201,13 +201,13 @@ void JointTrajectoryStreamer::jointCommandCB(const trajectory_msgs::JointTraject return; // convert trajectory point to ROS message - if (!create_message(this->streaming_sequence, xform_pt, &message)) + if (!create_message(this->ptstreaming_seq_count_, 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->ptstreaming_queue_.push(message); + this->ptstreaming_seq_count_++; this->mutex_.unlock(); } @@ -345,9 +345,9 @@ void JointTrajectoryStreamer::streamingThread() break; case TransferStates::POINT_STREAMING: - + // if no points in queue, streaming complete, set to idle. - if (this->streaming_queue_.empty()) + if (this->ptstreaming_queue_.empty()) { ROS_INFO("Point streaming complete, setting state to IDLE"); this->state_ = TransferStates::IDLE; @@ -361,8 +361,8 @@ void JointTrajectoryStreamer::streamingThread() break; } // otherwise, send point to robot. - tmpMsg = this->streaming_queue_.front(); - this->streaming_queue_.pop(); + tmpMsg = this->ptstreaming_queue_.front(); + this->ptstreaming_queue_.pop(); msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID, tmpMsg.getData()); // set commType=REQUEST diff --git a/motoman_driver/src/joint_trajectory_streamer.cpp b/motoman_driver/src/joint_trajectory_streamer.cpp index 3b0976ba..a82593f6 100644 --- a/motoman_driver/src/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/joint_trajectory_streamer.cpp @@ -128,9 +128,9 @@ bool MotomanJointTrajectoryStreamer::disableRobotCB(std_srvs::Trigger::Request & trajectoryStop(); - bool ret = motion_ctrl_.setTrajMode(false); + bool ret = motion_ctrl_.setTrajMode(false); res.success = ret; - + if (!res.success) { res.message="Motoman robot was NOT disabled. Please re-examine and retry."; ROS_ERROR_STREAM(res.message); @@ -139,7 +139,7 @@ bool MotomanJointTrajectoryStreamer::disableRobotCB(std_srvs::Trigger::Request & res.message="Motoman robot is now disabled and will NOT accept motion commands."; ROS_WARN_STREAM(res.message); } - + return true; @@ -148,9 +148,9 @@ bool MotomanJointTrajectoryStreamer::disableRobotCB(std_srvs::Trigger::Request & bool MotomanJointTrajectoryStreamer::enableRobotCB(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - bool ret = motion_ctrl_.setTrajMode(true); + bool ret = motion_ctrl_.setTrajMode(true); res.success = ret; - + if (!res.success) { res.message="Motoman robot was NOT enabled. Please re-examine and retry."; ROS_ERROR_STREAM(res.message); @@ -165,7 +165,7 @@ bool MotomanJointTrajectoryStreamer::enableRobotCB(std_srvs::Trigger::Request &r } - + // override create_message to generate JointTrajPtFull message (instead of default JointTrajPt) bool MotomanJointTrajectoryStreamer::create_message(int seq, const trajectory_msgs::JointTrajectoryPoint &pt, SimpleMessage *msg) { @@ -364,7 +364,7 @@ bool MotomanJointTrajectoryStreamer::VectorToJointData(const std::vector } return true; } - + // override send_to_robot to provide controllerReady() and setTrajMode() calls bool MotomanJointTrajectoryStreamer::send_to_robot(const std::vector& messages) { @@ -428,7 +428,7 @@ void MotomanJointTrajectoryStreamer::streamingThread() tmpMsg = this->current_traj_[this->current_point_]; msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST, - ReplyTypes::INVALID, tmpMsg.getData()); // set commType=REQUEST + ReplyTypes::INVALID, tmpMsg.getData()); if (!this->connection_->sendAndReceiveMsg(msg, reply, false)) ROS_WARN("Failed sent joint point, will try again"); @@ -463,13 +463,13 @@ void MotomanJointTrajectoryStreamer::streamingThread() case TransferStates::POINT_STREAMING: // if no points in queue, streaming complete, set to idle. - if (this->streaming_queue_.empty()) + if (this->ptstreaming_queue_.empty()) { - if (this->time_since_last < this->point_streaming_timeout) + if (this->dt_ptstreaming_points_ < this->ptstreaming_timeout_) { - time_since_last = ros::Time::now().toSec() - time_of_last; + this->dt_ptstreaming_points_ = ros::Time::now().toSec() - this->time_ptstreaming_last_point_; ros::Duration(0.005).sleep(); - ROS_DEBUG("Time since last point: %f", time_since_last); + ROS_DEBUG("Time since last point: %f", this->dt_ptstreaming_points_); break; } else @@ -487,9 +487,9 @@ void MotomanJointTrajectoryStreamer::streamingThread() break; } // otherwise, send point to robot. - tmpMsg = this->streaming_queue_.front(); + tmpMsg = this->ptstreaming_queue_.front(); msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST, - ReplyTypes::INVALID, tmpMsg.getData()); // set commType=REQUEST + ReplyTypes::INVALID, tmpMsg.getData()); ROS_INFO("Sending joint trajectory point"); if (this->connection_->sendAndReceiveMsg(msg, reply, false)) @@ -503,15 +503,15 @@ void MotomanJointTrajectoryStreamer::streamingThread() } if (reply_status.reply_.getResult() == MotionReplyResults::SUCCESS) { - ROS_INFO("Point[%d] sent to controller", this->current_point_); + ROS_DEBUG("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(); + this->dt_ptstreaming_points_ = 0.0; + this->time_ptstreaming_last_point_ = ros::Time::now().toSec(); + this->ptstreaming_queue_.pop(); } else if (reply_status.reply_.getResult() == MotionReplyResults::BUSY) { - ROS_INFO("silently resending."); + ROS_DEBUG("silently resending."); break; // silently retry sending this point } else @@ -612,4 +612,3 @@ bool MotomanJointTrajectoryStreamer::is_valid(const motoman_msgs::DynamicJointTr } // namespace joint_trajectory_streamer } // namespace motoman - From 8ca501669102fdf9881b86288663e63d928b28bb Mon Sep 17 00:00:00 2001 From: Dan Ambrosio Date: Thu, 14 Feb 2019 12:43:26 -0700 Subject: [PATCH 4/4] Added sequence checks in jointCommandCB to enable, stop and run POINT_STREAMING state, removed updates to local data that didn't correspond to POINT_STREAMING state --- .../joint_trajectory_streamer.h | 2 + .../joint_trajectory_streamer.cpp | 123 ++++++++++++++++-- .../src/joint_trajectory_streamer.cpp | 4 +- 3 files changed, 114 insertions(+), 15 deletions(-) diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h index 949e303a..f642d074 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h @@ -128,6 +128,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface bool send_to_robot(const std::vector& messages); protected: + static const size_t max_ptstreaming_queue_elements = 20; void trajectoryStop(); boost::thread* streaming_thread_; @@ -138,6 +139,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface 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 ptstreaming_queue_; // message queue for point streaming }; diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp index 4f71b3f0..a18beb0c 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp +++ b/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 +#include #include #include #include @@ -166,45 +168,142 @@ void JointTrajectoryStreamer::jointCommandCB(const trajectory_msgs::JointTraject 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->current_point_ = 0; this->ptstreaming_seq_count_ = 0; - this->streaming_start_ = ros::Time::now(); + this->ptstreaming_queue_ = std::queue(); this->mutex_.unlock(); + + // Set the local state to point streaming to force enqueuing of the starting point state = TransferStates::POINT_STREAMING; - ROS_INFO("First joint point received. Starting on-the-fly streaming."); + + ROS_INFO("Starting joint point received. Starting on-the-fly streaming."); } - //if current state is POINT_STREAMING, process incoming point. + // 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, cancelling current trajectory"); - return; + 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; + } } - //Else, Push point into queue - SimpleMessage message; - trajectory_msgs::JointTrajectoryPoint rbt_pt, xform_pt; + 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; + } - // select / reorder joints for sending to robot - if (!select(msg->joint_names, msg->points[0], this->all_joint_names_, &rbt_pt)) + // 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; - //Points get pushed into queue here. They will be popped in the Streaming Thread and sent to controller. + // 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_++; diff --git a/motoman_driver/src/joint_trajectory_streamer.cpp b/motoman_driver/src/joint_trajectory_streamer.cpp index c1cd25cf..ebba4357 100644 --- a/motoman_driver/src/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/joint_trajectory_streamer.cpp @@ -491,7 +491,6 @@ void MotomanJointTrajectoryStreamer::streamingThread() msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID, tmpMsg.getData()); - ROS_INFO("Sending joint trajectory point"); if (this->connection_->sendAndReceiveMsg(msg, reply, false)) { MotionReplyMessage reply_status; @@ -503,11 +502,10 @@ void MotomanJointTrajectoryStreamer::streamingThread() } if (reply_status.reply_.getResult() == MotionReplyResults::SUCCESS) { - ROS_DEBUG("Point[%d] sent to controller", this->current_point_); - this->current_point_++; this->dt_ptstreaming_points_ = 0.0; this->time_ptstreaming_last_point_ = ros::Time::now().toSec(); this->ptstreaming_queue_.pop(); + ROS_DEBUG("Point sent to controller, remaining points in queue [%lu]", this->ptstreaming_queue_.size()); } else if (reply_status.reply_.getResult() == MotionReplyResults::BUSY) {