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

Revision history [back]

click to hide/show revision 1
initial version

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.