Publishing problem with gazebo using trajectory_msgs

Question

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.

---

Originally posted by w.fradin on ROS Answers with karma: 38 on 2017-06-16

Post score: 1

---

Original comments

Comment by gvdhoorn on 2017-06-16:
Not a complete answer, but: know that time_from_start does not encode segment length directly, but is actually the point in time at which that TrajectoryPoint should be executed. Setting them all to 1.0 is not going to work, as all traj pts will then effectively 'overlap' in time.

Comment by w.fradin on 2017-06-16:
I understood that each trajectory point's time_from_start must be greater than the last. But I tried to increase this time but i have the same error. The problem is only in my code or it could come from my launch files?

Comment by gvdhoorn on 2017-06-17:
As I wrote, it was just a comment, not an answer to your problem. I just wanted to make you aware that your example code seems to be setting time_from_start to incorrect values.

Answer 1

I resolved my problem. Here is my first example that working and i explain it after :

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

ros::Publisher arm_pub;

int setValeurPoint(trajectory_msgs::JointTrajectory* trajectoire,int pos_tab, int val);

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);
    ros::Rate loop_rate(10);

    trajectory_msgs::JointTrajectory traj;
    trajectory_msgs::JointTrajectoryPoint points_n;

    traj.header.frame_id = "base_link";
    traj.joint_names.resize(4);
    traj.points.resize(1);

    traj.points[0].positions.resize(4);

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

    int i(1);

    while(ros::ok()) {

        traj.header.stamp = ros::Time::now();

        for(int j=0; j<4; j++) {
                setValeurPoint(&traj,j,i);
        }

        traj.points[0].time_from_start = ros::Duration(1);

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

        loop_rate.sleep();
        i++;
    }

    return 0;
}

int setValeurPoint(trajectory_msgs::JointTrajectory* trajectoire,int pos_tab, int val){
    trajectoire->points[0].positions[pos_tab] = val;
    return 0;
}

If you compare the two codes, I initialized (in the first one) "traj.points.resize()" to 4. It was a mistake because all points are connected to each other with 1 parent or 1 child. So, i've only 1 way-points (if i've 2 robot arm, i will have had 2 way-points...) and this way-points is defined by 4 positions (hip,shoulder,elbow,wrist). Moreover, i had forgotten to initialize and resize "traj.points[0].positions" to 4 (numbers of joints). Finally, "time_from_start = ros::Duration(1)" doesn't need to be incremented, as I read it, because it's the speed of the movement of the robot arm.

---

Originally posted by w.fradin with karma: 38 on 2017-06-21

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by JuanTelo on 2018-01-18:
Hi, did you manage to move the model? Is it posible you can contact me trough my e-mail, because I have some doubts about a similar thing. [email protected]

Comment by HanYangRobotics on 2018-11-29:
Hi I am curious as to which tutorial you are viewing and how you have written the publisher node. Can you tell me?

Comment by RobDawson on 2020-06-16:
I realise this is a late reply, from the look of the code however, the tutorial is the Cougarbot from Programming Robots with ROS.