Problem fetching the ros_controller state from controller/state topic
I am using ROS-Indigo on Ubunu 14.04. While using MoveIt! with ros-controllers I wanted to capture the data published on coro/coro_arm_controller/state topic. However while the acceleration and effort fields, segmentation fault error is coming. Below is my code. Can anyone suggest me on what to do next ??
#include "ros/ros.h"
#include <control_msgs/JointTrajectoryControllerState.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <vector>
using namespace std;
void callback(const control_msgs::JointTrajectoryControllerState& traj)
{ trajectory_msgs::JointTrajectoryPoint desired, actual, error_;
ros::Duration t;
actual.accelerations.resize(7);
actual.effort.resize(7);
for(int j=0; j<traj.joint_names.size();++j)
{
// Desired
desired.positions.push_back(traj.desired.positions[j]);
desired.velocities.push_back(traj.desired.velocities[j]);
desired.accelerations.push_back(traj.desired.accelerations[j]);
desired.effort.push_back(traj.desired.accelerations[j]);
actual.positions.push_back(traj.actual.positions[j]);
actual.velocities.push_back(traj.actual.velocities[j]);
//actual.accelerations.push_back(traj.actual.accelerations[j]);
//actual.effort.push_back(traj.actual.accelerations[j]);
error_.positions.push_back(traj.error.positions[j]);
error_.velocities.push_back(traj.error.velocities[j]);
//error_.accelerations.push_back(traj.error.accelerations[j]);
// error_.effort.push_back(traj.error.effort[j]);
}
for(int i= 0; i< traj.joint_names.size();++i)
{
ROS_INFO_STREAM( "position for joint["<< i << "] :" << desired.positions[i]);
ROS_INFO_STREAM( "velocity for joint["<< i << "] :" << desired.velocities[i]);
ROS_INFO_STREAM( "acceleration for joint["<< i << "] :" << desired.accelerations[i]);
ROS_INFO_STREAM( "effort for joint["<< i << "] :" << desired.effort[i]);
}
ROS_INFO_STREAM( "time is :" << t << "\n");
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "trajectory");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/zuti_bot/zuti_bot_arm_controller/state", 1000, callback);
while(ros::ok())
{
ros::spinOnce();
}
return 0;
}
After many iterations I found that there is some problem while capturing the acceleration and effort value of the joints.