ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Publish multiple points on trajectory_msgs::JointTrajectory topic.

asked 2020-09-26 00:13:45 -0500

updated 2020-09-26 00:15:48 -0500

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 !!

edit retag flag offensive close merge delete

Comments

If you are looking to execute trajectories, why are you using the command topic, instead of constructing a proper FollowJointTrajectory action goal, creating a client and then submitting the goal using the client to the JointTrajectory action server?

gvdhoorn gravatar image gvdhoorn  ( 2020-09-26 01:39:53 -0500 )edit

Thanks for replying. Yeah, that's what I am planning to try next. Above is what I was thinking of doing since a long time (in order to test robot trajectories in gazebo). So, I gave it a try.

Anubhav Singh gravatar image Anubhav Singh  ( 2020-09-26 06:12:38 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-09-28 02:49:06 -0500

gvdhoorn gravatar image

updated 2020-09-28 02:49:44 -0500

When I am running the node, my robotic arm is performing only first trajectory .

(note: I'll assume trajectory here should actually read trajectory point)

This is a guess, but I believe the following may explain what you observe:

  1. you are publishing messages to the command topic. This is a fire-and-forget interface to the JointTrajectoryController, meaning you get no feedback and are responsible yourself to make sure "the current trajectory" has finished executing before sending a new one
  2. if you don't check for completion of a trajectory, and send a new one while the controller is executing the last trajectory you sent, the controller will try to replace the current trajectory with the new one
  3. you are not sending "three trajectories", but only a single one -- with three trajectory points
  4. the while-loop in your main(..) causes the same trajectory to be published again and again at a frequency of 10 Hz

Taking all of this together, it's possible that what you see is completely according to how the JointTrajectoryController is specced to work:

  • the same trajectory is sent as input to the controller at 10 Hz, which causes the controller to attempt execution of that trajectory
  • while executing, it receives a new trajectory and will start to calculate how to join the two (ie: current and next trajectory)
  • the replacement trajectory is the same trajectory, and has the first time_from_start always set to 1.0
  • as 1.0 / 10 == 0.1 < 1.0, the first point of the trajectory will always be the destination point

In essence, you're giving the JointTrajectoryController a single destination (the first point in the trajectory) and always 1 second to reach it and tell it to go again to that point at a frequency of 10 Hz.

The other two trajectories are performed when I am pressing ctrl+c command

(again: I'll assume trajectory should actually read trajectory point)

This can be explained by the fact that once you stop your script, the JointTrajectoryController finally gets a chance to completely execute the trajectory (ie: without any replacement trajectories coming in), so it will (should) reach points 2 and 3 in 2 and 3 seconds respectively.

Finally: as I wrote in my initial comment: it's ok to use the command interface/topic, but you'll have to make sure the controller has executed the trajectory before sending a new one (or actually make use of the replacement capabilities of course).

If you're not interested in replacement, I would suggest you instead use the action interface which lets you wait on completion of a goal, notifying you when it's ok to send a new trajectory.

edit flag offensive delete link more

Comments

Thank you so much for your answer. I will try these points.

Anubhav Singh gravatar image Anubhav Singh  ( 2020-09-29 10:00:41 -0500 )edit

I met the same problem. Using action interface actually solved it. Thanks a lot !

Contour gravatar image Contour  ( 2021-12-09 01:35:46 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-09-26 00:13:45 -0500

Seen: 1,075 times

Last updated: Sep 28 '20