Skip to content

Commit

Permalink
point-streaming: address @gavanderhoorn's comments
Browse files Browse the repository at this point in the history 
Addressed most of @gavanderhoorn's comments, except for comments 3 and 4.
  • Loading branch information
@tdl-ua
tdl-ua committed Jun 26, 2018
1 parent 7b316da commit 4440bd8
Show file tree
Hide file tree
Showing 4 changed files with 36 additions and 35 deletions.
7 changes: 4 additions & 3 deletions motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<SimpleMessage> current_traj_;
TransferState state_;
ros::Time streaming_start_;
int min_buffer_size_;

std::queue<SimpleMessage> streaming_queue_;

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
Expand Down
9 changes: 5 additions & 4 deletions motoman_driver/include/motoman_driver/joint_trajectory_streamer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,10 +144,11 @@ 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;

// 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,
Expand Down
16 changes: 8 additions & 8 deletions motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
}

Expand Down Expand Up @@ -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;
Expand All @@ -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

Expand Down
39 changes: 19 additions & 20 deletions motoman_driver/src/joint_trajectory_streamer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;

Expand All @@ -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);
Expand All @@ -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)
{
Expand Down Expand Up @@ -364,7 +364,7 @@ bool MotomanJointTrajectoryStreamer::VectorToJointData(const std::vector<double>
}
return true;
}

// override send_to_robot to provide controllerReady() and setTrajMode() calls
bool MotomanJointTrajectoryStreamer::send_to_robot(const std::vector<SimpleMessage>& messages)
{
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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
Expand All @@ -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))
Expand All @@ -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
Expand Down Expand Up @@ -612,4 +612,3 @@ bool MotomanJointTrajectoryStreamer::is_valid(const motoman_msgs::DynamicJointTr

} // namespace joint_trajectory_streamer
} // namespace motoman

0 comments on commit 4440bd8

Please sign in to comment.