void JointTrajectoryHandler::trajectoryStop()
{
JointMessage jMsg;
SimpleMessage msg;
SimpleMessage reply;
ROS_INFO("Joint trajectory handler: entering stopping state");
jMsg.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
jMsg.toRequest(msg);
ROS_DEBUG("Sending stop command");
this->robot_->sendAndReceiveMsg(msg, reply);
ROS_DEBUG("Stop command sent, entring idle mode");
this->state_ = JointTrajectoryStates::IDLE;
}
void JointTrajectoryHandler::trajectoryHandler()
{
JointMessage jMsg;
SimpleMessage msg;
SimpleMessage reply;
trajectory_msgs::JointTrajectoryPoint pt;
ROS_INFO("Starting joint trajectory handler state");
while (ros::ok())
{
this->mutex_.lock();
if (this->robot_->isConnected())
{
//TODO: These variables should be moved outside of this loop
//so that we aren't contantly reinitializing them.
JointMessage jMsg;
SimpleMessage msg;
SimpleMessage reply;
trajectory_msgs::JointTrajectoryPoint pt;
switch (this->state_)
{
case JointTrajectoryStates::IDLE:
ros::Duration(0.250).sleep();
break;
case JointTrajectoryStates::STREAMING:
if (this->currentPoint < this->current_traj_.points.size())
{
// unsigned int lastCurrentPoint = currentPoint;
// currentPoint = getNextTrajectoryPoint(current_traj_,
// streaming_start_,
// ros::Time::now());
//ROS_INFO("Skipping from point[%d] to point[%d]", lastCurrentPoint, currentPoint);
pt = this->current_traj_.points[this->currentPoint];
jMsg.setSequence(this->currentPoint);
for (int i = 0; i < this->current_traj_.joint_names.size(); i++)
{
jMsg.getJoints().setJoint(i, pt.positions[i]);
}
ROS_DEBUG("Sending joint point");
jMsg.toRequest(msg);
if (this->robot_->sendAndReceiveMsg(msg, reply, false))
//jMsg.toTopic(msg);
//if (this->robot_->sendMsg(msg))
{
ROS_INFO("Point[%d of %d] sent to controller",
this->currentPoint, this->current_traj_.points.size());
this->currentPoint++;
}
else
{
ROS_WARN("Failed sent joint point, will try again");
}
//ROS_INFO_STREAM("Time taken to stream single point is " << (ros::WallTime::now()-start));
}
else
{
ROS_INFO("Trajectory streaming complete, setting state to IDLE");
this->state_ = JointTrajectoryStates::IDLE;
}
break;
default:
ROS_ERROR("Joint trajectory handler: unknown state");
this->state_ = JointTrajectoryStates::IDLE;
break;
}
}
else
{
ROS_INFO("Connecting to robot motion server");
this->robot_->makeConnect();
}
this->mutex_.unlock();
ros::Duration(0.005).sleep();
}
ROS_WARN("Exiting trajectory handler thread");
}