bool TrajectoryDownloadHandler::internalCB(industrial::simple_message::SimpleMessage & in)
{
bool rtn = false;
SimpleMessage reply;
JointTrajPtMessage jMsg;
jMsg.init(in);
switch( jMsg.point_.getSequence() )
{
case SpecialSeqValues::START_TRAJECTORY_DOWNLOAD:
startTrajectory(jMsg);
rtn = true;
break;
case SpecialSeqValues::END_TRAJECTORY:
endTrajectory(jMsg);
rtn = true;
break;
case SpecialSeqValues::STOP_TRAJECTORY:
LOG_DEBUG("Stoping trajectory");
this->ctrl_->stopMotionJob(JOB_NAME);
rtn = true;
break;
default:
LOG_ERROR("Adding point: %d to trajectory", jMsg.point_.getSequence());
this->traj_.addPoint(jMsg.point_);
break;
}
// Send ack
if (CommTypes::SERVICE_REQUEST == in.getCommType())
{
if (rtn)
{
jMsg.toReply(reply, ReplyTypes::SUCCESS);
}
else
{
jMsg.toReply(reply, ReplyTypes::FAILURE);
}
if(this->getConnection()->sendMsg(reply))
{
LOG_INFO("Ack sent");
}
else
{
LOG_ERROR("Failed to send joint ack");
}
}
return rtn;
}
void evaluate_workspace()
{
JointTrajPtMessage jMsg;
SimpleMessage msg;
SimpleMessage reply;
ROS_INFO("Joint trajectory downloader: entering stopping state");
jMsg.point_.setSequence(SpecialSeqValues::STOP_TRAJECTORY);
jMsg.toRequest(msg);
ROS_DEBUG("Sending stop command");
this->robot_->sendAndReceiveMsg(msg, reply);
ROS_DEBUG("Stop command sent");
}
// helper function
static JointTrajPtMessage create_message(int seq,
const std::vector<double>& joint_pos,
double velocity, double duration)
{
industrial::joint_data::JointData pos;
ROS_ASSERT(joint_pos.size() <= (unsigned int)pos.getMaxNumJoints());
for (size_t i = 0; i < joint_pos.size(); ++i)
pos.setJoint(i, joint_pos[i]);
rbt_JointTrajPt pt;
pt.init(seq, pos, velocity, duration);
JointTrajPtMessage msg;
msg.init(pt);
return msg;
}
void JointTrajectoryDownloader::jointTrajectoryCB(
const trajectory_msgs::JointTrajectoryConstPtr &msg)
{
ROS_INFO("Receiving joint trajectory message");
/*
if (!checkTrajectory(msg))
{
ROS_ERROR("Joint trajectory check failed, trajectory not downloaded");
return;
}
std::vector<double> joint_velocity_limits;
if (!getVelocityLimits("robot_description", msg->joint_names, joint_velocity_limits))
{
ROS_ERROR("Failed to get joint velocity limits");
return;
}
*/
for (int i = 0; i < msg->points.size(); i++)
{
ROS_INFO("Sending joints trajectory point[%d]", i);
JointTrajPt jPt;
JointTrajPtMessage jMsg;
SimpleMessage topic;
trajectory_msgs::JointTrajectoryPoint pt;
pt = msg->points[i];
// The first and last sequence values must be given a special sequence
// value
if (0 == i)
{
ROS_DEBUG("First trajectory point, setting special sequence value");
jPt.setSequence(SpecialSeqValues::START_TRAJECTORY_DOWNLOAD);
}
else if (msg->points.size() - 1 == i)
{
ROS_DEBUG("Last trajectory point, setting special sequence value");
jMsg.point_.setSequence(SpecialSeqValues::END_TRAJECTORY);
}
else
{
jMsg.point_.setSequence(i);
}
// Copy position data to local variable
JointData data;
for (int j = 0; j < msg->joint_names.size(); j++)
{
pt = msg->points[j];
data.setJoint(j, pt.positions[j]);
}
// Initialize joint trajectory message
jPt.setJointPosition(data);
jMsg.init(jPt);
jMsg.toTopic(topic);
ROS_DEBUG("Sending joint trajectory point");
if (this->robot_->sendMsg(topic))
{
ROS_INFO("Point[%d] sent to controller", i);
}
else
{
ROS_WARN("Failed sent joint point, skipping point");
}
}
}
void JointTrajectoryDownloader::jointTrajectoryCB(
const trajectory_msgs::JointTrajectoryConstPtr &msg)
{
ROS_INFO("Receiving joint trajectory message");
// TBD: re-enable in a more generic fashion?
//if (!checkTrajectory(msg))
//{
// ROS_ERROR("Joint trajectory check failed, trajectory not downloaded");
// return;
//}
std::vector<double> joint_velocity_limits;
std::vector<trajectory_msgs::JointTrajectoryPoint> points(msg->points);
// TBD: re-enable velocity scaling
//if (!getVelocityLimits("robot_description", msg, joint_velocity_limits))
//{
// ROS_ERROR("Failed to get joint velocity limits");
// return;
//}
// Trajectory download requires at least two points (START/END)
if (points.size() < 2)
points.push_back(trajectory_msgs::JointTrajectoryPoint(points[0]));
if (!this->robot_->isConnected())
{
ROS_WARN("Attempting robot reconnection");
this->robot_->makeConnect();
}
ROS_INFO("Sending trajectory points, size: %d", points.size());
for (int i = 0; i < points.size(); i++)
{
ROS_INFO("Sending joints trajectory point[%d]", i);
JointTrajPt jPt;
JointTrajPtMessage jMsg;
SimpleMessage topic;
// Performing a manual copy of the joint velocities in order to send them
// to the utility function. Passing the pt data members doesn't seem to
// work.
std::vector<double> joint_velocities(0.0);
double velocity =0 ;
joint_velocities.resize(msg->joint_names.size(), 0.0);
for (int j = 0; j < joint_velocities.size(); j++)
{
joint_velocities[j] = points[i].velocities[j];
}
ROS_DEBUG("Joint velocities copied");
// TBD: restore joint-velocity scaling
//velocity = toMotomanVelocity(joint_velocity_limits, joint_velocities);
velocity = 0.20; // TBD: hardcode for now
jPt.setVelocity(velocity);
// The first and last sequence values must be given a special sequence
// value
if (0 == i)
{
ROS_DEBUG("First trajectory point, setting special sequence value");
jPt.setSequence(SpecialSeqValues::START_TRAJECTORY_DOWNLOAD);
}
else if (points.size() - 1 == i)
{
ROS_DEBUG("Last trajectory point, setting special sequence value");
jPt.setSequence(SpecialSeqValues::END_TRAJECTORY);
}
else
{
jPt.setSequence(i);
}
// Copy position data to local variable
JointData data;
for (int j = 0; j < msg->joint_names.size(); j++)
{
data.setJoint(j, points[i].positions[j]);
}
// Initialize joint trajectory message
jPt.setJointPosition(data);
jMsg.init(jPt);
jMsg.toTopic(topic);
ROS_DEBUG("Sending joint trajectory point");
if (this->robot_->sendMsg(topic))
{
ROS_INFO("Point[%d] sent to controller", i);
}
else
{
ROS_WARN("Failed sent joint point, skipping point");
}
}
}
