Revision history [back]

Now I got my robot arm to work. I switched from the python interface to c++. The central aspect for my problem with the jointTrajectory structure was, that I had to resize the arrays for joint_names, points, positions etc. to the desired number of elements.

Here my example code:

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "trajectory_msgs/JointTrajectory.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"

#define PI 3.141592653589793

int main(int argc, char **argv)
{
  ros::init(argc, argv, "topic_publisher");

  ros::NodeHandle n;

  ros::Publisher chatter_pub = n.advertise<trajectory_msgs::jointtrajectory>("arm_controller/command", 10);

  ros::Rate loop_rate(0.5);

 trajectory_msgs::JointTrajectory msg;
        msg.joint_names.resize(4);
        msg.joint_names[0] = "hip";
        msg.joint_names[1] = "shoulder";
        msg.joint_names[2] = "elbow";
        msg.joint_names[3] = "wrist";
        msg.points.resize(1);
        msg.points[0].positions.resize(4);
        msg.points[0].positions[0] = 1.5;
        msg.points[0].positions[1] = 0.5;
        msg.points[0].positions[2] = 0.6;
        msg.points[0].positions[3] = 0.6;

        msg.points[0].velocities.resize(4);
        msg.points[0].velocities[0] = 0.0;
        msg.points[0].velocities[1] = 0.0;
        msg.points[0].velocities[2] = 0.0;
        msg.points[0].velocities[3] = 0.0;

        msg.points[0].time_from_start=ros::Duration(2);
  while (ros::ok())
  {
          if (msg.points[0].positions[0] >= 2*PI)
                  msg.points[0].positions[0] = 0.0;
          else
                msg.points[0].positions[0] += 0.7;
//      ROS_INFO("%f:%f", msg.points[0].positions[0],msg.points[0].positions[0]);
    chatter_pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
  }


  return 0;
}