void JointMotionHandler::motionInterface(JointMessage & jMsg)
{
//
// Upon receiving...
// 1st point - initialize buffer, enable motion, start job, add point (increment buffer index),
// Nth point - add point (increment buffer index)
// end of trajectory - wait until buffer size = 0, disable motion, stop job, reset buffer indicies
//motion stop - disable motion, stop job
JointData joints;
switch (jMsg.getSequence())
{
case SpecialSeqValues::END_TRAJECTORY:
LOG_INFO("Received end trajectory command");
while(!pVarQ.bufferEmpty())
{
LOG_DEBUG("Waiting for motion buffer to empty");
mpTaskDelay(this->BUFFER_POLL_TICK_DELAY);
};
this->stopMotionJob();
break;
case SpecialSeqValues::STOP_TRAJECTORY:
LOG_WARN("Received stop command, stopping motion immediately");
this->stopMotionJob();
break;
default:
joints.copyFrom(jMsg.getJoints());
if (!(this->isJobStarted()))
{
//TODO: The velocity should be set from the message in the future.
pVarQ.init(joints, 0.0);
this->startMotionJob();
}
else
{
pVarQ.addPoint(joints, 0.0);
}
}
}
bool JointRelayHandler::convert_message(JointMessage& msg_in, JointTrajectoryPoint* joint_state)
{
// copy position data
int num_jnts = all_joint_names_.size();
joint_state->positions.resize(num_jnts);
for (int i=0; i<num_jnts; ++i)
{
shared_real value;
if (msg_in.getJoints().getJoint(i, value))
joint_state->positions[i] = value;
else
LOG_ERROR("Failed to parse position #%d from JointMessage", i);
}
// these fields are not provided by JointMessage
joint_state->velocities.clear();
joint_state->accelerations.clear();
joint_state->time_from_start = ros::Duration(0);
return true;
}
bool JointRelayHandler::convert_message(JointMessage& msg_in, DynamicJointsGroup* joint_state, int robot_id)
{
// copy position data
int num_jnts = robot_groups_[robot_id].get_joint_names().size();
joint_state->positions.resize(num_jnts);
for (int i = 0; i < num_jnts; ++i)
{
shared_real value;
if (msg_in.getJoints().getJoint(i, value))
{
joint_state->positions[i] = value;
}
else
LOG_ERROR("Failed to convert message");
}
// these fields are not provided by JointMessage
joint_state->velocities.clear();
joint_state->accelerations.clear();
joint_state->time_from_start = ros::Duration(0);
return true;
}
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");
}