publishing trajectory_msgs/jointtrajectory msgs - ros

Question

When i set the position and velocities of the joints in the trajectory msgs i got an error: \
[state_publisher-2] process has died [pid 13362, exit code -11, cmd /home/rob/catkin_ws/devel/lib/r2d2/state_publisher __name:=state_publisher __log:=/home/rob/.ros/log/9980f352-cf74-11e6-8644-d4c9efe8bd37/state_publisher-2.log].
log file: /home/rob/.ros/log/9980f352-cf74-11e6-8644-d4c9efe8bd37/state_publisher-2*.log
My ros node to send geometry_msgs is:
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <vector>
int main(int argc, char** argv) {
ros::init(argc, argv, "state_publisher");
ros::NodeHandle n;
ros::Publisher joint_pub = n.advertise<trajectory_ms
gs::JointTrajectory>("set_joint_trajectory", 1);
ros::Rate loop_rate(30);
const double degree = M_PI/180;
// robot state
double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;
// message declarations
trajectory_msgs::JointTrajectory joint_state;
std::vector<trajectory_msgs::JointTrajectoryPoint> points_n(3);
points_n[0].positions[0] = 1; points_n[0].velocities[0]=10;
while (ros::ok()) {
//update joint_state
joint_state.header.stamp = ros::Time::now();
joint_state.joint_names.resize(3);
joint_state.points.resize(3);
joint_state.joint_names[0] ="swivel";
joint_state.points[0] = points_n[0];
joint_state.joint_names[1] ="tilt";
joint_state.points[1] = points_n[1];
joint_state.joint_names[2] ="periscope";
joint_state.points[2] = points_n[2];
joint_pub.publish(joint_state);
// This will adjust as needed per iteration
loop_rate.sleep();
}
return 0;
}
Here when i donot set the position and velocity value it runs without error and when i run rostopic echo /set_joint_trajectory i can clearly see the outputs as all the parameters of points is 0. I also tried below program but it published nothing:
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <vector>
int main(int argc, char** argv) {
ros::init(argc, argv, "state_publisher");
ros::NodeHandle n;
ros::Publisher joint_pub = n.advertise<trajectory_msgs::JointTrajectory>("set_joint_trajectory", 1);
trajectory_msgs::JointTrajectory joint_state;
joint_state.header.stamp = ros::Time::now();
joint_state.header.frame_id = "camera_link";
joint_state.joint_names.resize(3);
joint_state.points.resize(3);
joint_state.joint_names[0] ="swivel";
joint_state.joint_names[1] ="tilt";
joint_state.joint_names[2] ="periscope";
size_t size = 2;
for(size_t i=0;i<=size;i++) {
trajectory_msgs::JointTrajectoryPoint points_n;
int j = i%3;
points_n.positions.push_back(j);
points_n.positions.push_back(j+1);
points_n.positions.push_back(j*2);
joint_state.points.push_back(points_n);
joint_state.points[i].time_from_start = ros::Duration(0.01);
}
joint_pub.publish(joint_state);
ros::spinOnce();
return 0;
}

Answer 1

You are accessing points_n[0].positions[0] and points_n[0].velocities[0] without allocating the memory for positions and velocities. Use
...
// message declarations
trajectory_msgs::JointTrajectory joint_state;
std::vector<trajectory_msgs::JointTrajectoryPoint> points_n(3);
points_n[0].positions.resize(1);
points_n[0].velocities.resize(1);
...
then set the values or use points_n[0].positions.push_back(...) instead. The same applies to points_n[1] and points_n[2].
In your second example it looks like your program terminates before anything is sent. Try to publish repeatedly in a while-loop with
while(ros::ok()){
...
ros::spinOnce();
}