Does dynamixel_motor support multiple points in trajectory_msgs

edit

Hello All,

I am using Dynamixel XM-430 for my robot. And I am using dynamixel package from Dynamixel_Motor.

Here I am writing a C++ code to control my ARM. Here is my code as below:

int move_joint_in_sync(ros::Publisher joint_pub, double value, double speed)
{
    trajectory_msgs::JointTrajectory traj;
    trajectory_msgs::JointTrajectoryPoint point;

traj.header.stamp = ros::Time::now();
traj.header.frame_id = "/base_link";

traj.points.resize(2);

traj.joint_names.resize(3);
traj.joint_names[0] = "joint3";
traj.joint_names[1] = "joint4";
traj.joint_names[2] = "joint5";

point.positions.resize(3);
point.positions[0] = value;
point.positions[1] = 0.0;
point.positions[2] = 0.299853768905;

point.velocities.resize(3);
point.velocities[0] = speed;
point.time_from_start = ros::Duration(0.5);

traj.points[0] = point;
point.positions[0] = -value;
traj.points[1] = point;

joint_pub.publish(traj);
ROS_INFO("Moving joint in sync to %f", value);
ros::spinOnce();

return 0;

}

Here I am passing 2 points in the message, but it is reaching to only 2nd point. And here it is giving an error also, as below:

[ERROR] [1528373780.447280]: Attempt to get a goal id on an uninitialized ServerGoalHandle
[ERROR] [1528373780.451816]: Attempt to get a goal id on an uninitialized ServerGoalHandle
[ERROR] [1528373780.455750]: Attempt to set status on an uninitialized ServerGoalHandle

Even though it moves to 1 point, it generates error. Is there something wrong, because of that it is not moving to 1st target and generating error.

Please let me know if some more information is required.

Saurabh