ROS-I trajectory downloading using TYPE 11 instead of 12?
I want to use the trajectory downloading method in order to send trajectory commands to my robot controller. As written below, the trajectory downloading method should use the Joint trajectory message (JOINT_TRAJ):
JOINT_TRAJ_PT = 11, //Joint trajectory point message (typically for streaming)
JOINT_TRAJ = 12, //Joint trajectory message (typically for trajectory downloading)
However, the JointTrajectoryDownloader class used in the generic_joint_downloader_node.cpp, has a method send_to_robot which converts each Joint trajectory point from the Joint trajectory array into a simple message and sends it (with type JOINT_TRAJ_PT = 11)
A piece of code from the send_to_robot method:
for (int i = 0; i < (int)points.size(); ++i)
{
ROS_DEBUG("Sending joints trajectory point[%d]", i);
points[i].toTopic(msg);
bool ptRslt = this->connection_->sendMsg(msg);
if (ptRslt)
ROS_DEBUG("Point[%d] sent to controller", i);
else
ROS_WARN("Failed sent joint point, skipping point");
rslt &= ptRslt;
}
So I don't understand how should I create a message handler for the JOINT_TRAJ type if what the server gets is only JOINT_TRAJ_PT messages.
An othere matter, this send_to_robot method adds a sequence to the first and last JOINT_TRAJ_PT messages so it would be possible to know where the path starts and where it ends:
// The first and last points are assigned special sequence values
points.begin()->setSequence(SpecialSeqValues::START_TRAJECTORY_DOWNLOAD);
points.back().setSequence(SpecialSeqValues::END_TRAJECTORY);
But I don't understand how could my massage handler know how to read this sequence from the sent ByteArray. this sequence is not included in one of the typical simple message types, so how could I know if the integer I'm bytes I'm reading are sequence or joint data?
I desperately need help here, thank you!
you may feel this is nitpicking, but there is no obligation here. Note the word typically, which expresses a possibility that is probably true, but doesn't have to be.
As to the rest: in order to avoid a possible xy-problem, could you please tell us what you are actually trying to achieve?
I'm trying to create a driver (as ROS-I defines it) for a robot. I have a RT motion controller and I want it to communicate with ROS over TCP with simple messages. I want the communication to be "trajectory downloading" for the moment.