Ask Your Question
1

Publishing problem with gazebo using trajectory_msgs

asked 2017-06-16 03:15:45 -0500

w.fradin gravatar image

updated 2017-06-16 03:18:25 -0500

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.

edit retag flag offensive close merge delete

Comments

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.

gvdhoorn gravatar imagegvdhoorn ( 2017-06-16 03:46:08 -0500 )edit

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?

w.fradin gravatar imagew.fradin ( 2017-06-16 08:41:43 -0500 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2017-06-17 04:01:14 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-06-21 03:14:40 -0500

w.fradin gravatar image

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.

edit flag offensive delete link more

Comments

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. juantelo95@gmail.com

JuanTelo gravatar imageJuanTelo ( 2018-01-18 07:55:12 -0500 )edit

Hi I am curious as to which tutorial you are viewing and how you have written the publisher node. Can you tell me?

HanYangRobotics gravatar imageHanYangRobotics ( 2018-11-29 22:54:13 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2017-06-16 03:15:45 -0500

Seen: 659 times

Last updated: Jan 18 '18