Publish multiple points on trajectory_msgs::JointTrajectory topic.

edit

Hey,

I am trying to publish multiple trajectory points of type trajectory_msgs::JointTrajectoryPoint on /arm_manipulator_controller/command topic of type trajectory_msgs::JointTrajectoryPoint.

When I am running the node, my robotic arm is performing only first trajectory . The other two trajectories are performed when I am pressing ctrl+c command. Output of rostopic echo /arm_manipulator_command suggesting all three trajectory points are getting published correctly.

Is it happening because of inconsistent time_from_start parameter of trajectory_msgs::JointTrajectoryPoint ?

Output of rostopic echo /arm_manipulator_command :

header: 
  seq: 633154
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "B_Link"
joint_names: [J1, J2, J3, J4, J5, J6]
points: 
  - 
    positions: [1.5707963267948966, 1.0471975511965976, 1.5707963267948966, 3.141592653589793, -1.0471975511965976, 1.5707963267948966]
    velocities: []
    accelerations: []
    effort: []
    time_from_start: 
      secs: 1
      nsecs:         0
  - 
    positions: [-1.5707963267948966, -1.0471975511965976, -1.5707963267948966, -3.141592653589793, 1.0471975511965976, -1.5707963267948966]
    velocities: []
    accelerations: []
    effort: []
    time_from_start: 
      secs: 2
      nsecs:         0
  - 
    positions: [3.141592653589793, 0.0, 0.0, 0.0, 1.0471975511965976, 0.0]
    velocities: []
    accelerations: []
    effort: []
    time_from_start: 
      secs: 3
      nsecs:         0

My code:

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectory.h>
#include "ros/time.h"
#include <iostream>
#include <math.h>

ros::Publisher arm_pub;
std::string joint_names[6] = { "J1", "J2", "J3", "J4", "J5", "J6" };

trajectory_msgs::JointTrajectoryPoint jointTrajectory_point(double time, int num_joints, double *joint_values){

    trajectory_msgs::JointTrajectoryPoint point;
    point.positions.resize(num_joints);
    for(int i=0; i<num_joints; i++) {
        point.positions[i] = joint_values[i];
    }
    point.time_from_start = ros::Duration(time);
    return point;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher_node");
    ros::NodeHandle n;
    arm_pub = n.advertise<trajectory_msgs::JointTrajectory>("/arm_manipulator_controller/command",1);
    ros::Rate loop_rate(10);

    int num_joints = 6;

    trajectory_msgs::JointTrajectory traj;
    traj.header.frame_id = "B_Link";
    traj.header.stamp = ros::Time::now();

    traj.joint_names.resize(num_joints);
    for(int i=0;i<num_joints;i++){
        traj.joint_names[i] = joint_names[i];
    }

    traj.points.resize(3);

    double joint_values_1[6] = {M_PI/2, M_PI/3, M_PI/2, M_PI, -M_PI/3, M_PI/2}; 
    traj.points[0] = jointTrajectory_point(1.0, 6, joint_values_1);

    double joint_values_2[6] = {-M_PI/2, -M_PI/3, -M_PI/2, -M_PI, M_PI/3, -M_PI/2}; 
    traj.points[1] = jointTrajectory_point(2.0, 6, joint_values_2);

    double joint_values_3[6] = {M_PI, 0.0, 0.0, 0.0, M_PI/3, 0.0}; 
    traj.points[2] = jointTrajectory_point(3.0, 6, joint_values_3);

    while(ros::ok()) {
        arm_pub.publish(traj);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

Your help would be much appreciated. I have already given a lot of time on this without any useful output.

Thanks in advance !!