Publishing problem with gazebo using trajectory_msgs

edit

Hi everyone,

I'm working on arm robotic, first, i wrote a physical model of the arm with a URDF files. This model work with Rviz and Gazebo. Moreover, i created a controllers.yaml file (for controls all robot's joints) and when I use the command : rostopic pub /arm_controller/command trajectory_msgs/JointTrajectory '{joint_names: ["hip","shoulder","elbow","wrist"], points: [{positions:[0.1,-0.5,0.5,0.75], time_from_start: [1.0,0.0]}]}' -1 both models (on rivz and gazebo) move simultaneously.

But now, I want to create a .cpp file for the arm to move independently by using trajectory_msgs::JointTrajectory. Here's my cpp file :

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectory.h>

ros::Publisher arm_pub;

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

trajectory_msgs::JointTrajectory traj;

traj.header.stamp = ros::Time::now();
traj.header.frame_id = "base_link";
traj.joint_names.resize(4);
traj.points.resize(4);

traj.joint_names[0] ="hip";
traj.joint_names[1] ="shoulder";
traj.joint_names[2] ="elbow";
traj.joint_names[3] ="wrist";

while(ros::ok()){
   for(int i=0;i<4;i++) {
      int j = i%3;
      trajectory_msgs::JointTrajectoryPoint points_n;
      points_n.positions.push_back(j+0.1);
      points_n.positions.push_back(j+0.3);
      points_n.positions.push_back(j+0.3);
      points_n.positions.push_back(j);
      traj.points.push_back(points_n);
      traj.points[i].time_from_start = ros::Duration(1.0,0.0);
   }

  arm_pub.publish(traj);
  ros::spinOnce();
 }

return 0;
}

When i launch my file.launch and I rosrun my cpp file, both are connected on rqt_graph. But immediately, i have an error (on launch terminal) :

[ERROR] [1497596211.214814221, 9.889000000]: Trajectory message contains waypoints that are not strictly increasing in time.

Effectively, when i use the command rostopic echo /arm_controller/command, i have :

`positions: [0.0, 1.0, 0.0, 2.0]
 velocities: []
 accelerations: []
 effort: []
 time_from_start: 
  secs: 0
  nsecs: 0`

The time_from_start is always 0. So, I think i've a problem in my code but i don't know where.

Does anyone have an idea what is wrong? Thank you.