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

Problem fetching the ros_controller state from controller/state topic

asked 2018-03-23 05:02:05 -0500

Pulkit123 gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-03-23 05:24:26 -0500

gvdhoorn gravatar image

Have you made sure the accelerations and efforts fields are actually populated?

Note in the documentation for trajectory_msgs/JointTrajectoryPoint:

# Each trajectory point specifies either positions[, velocities[, accelerations]]
# or positions[, effort] for the trajectory to be executed.

The [] in [, velocities[, accelerations]] and [, effort] indicate that these are optional. Not all implementations may populate those fields.

edit flag offensive delete link more

Comments

Actually I am giving trajectory through MoveIt! and capturing the controller_state data. I checked using rostopic echo and found that the acceleration and effort fields are not populated by default for actual and error parts. So how to rectify this ??

Pulkit123 gravatar image Pulkit123  ( 2018-03-23 06:04:19 -0500 )edit

I would suggest to check that (for instance) actual.efforts.size() > 0.

gvdhoorn gravatar image gvdhoorn  ( 2018-03-23 08:55:05 -0500 )edit

I checked it, actually the acceleration ad effort fields of actual and error parts of control_msgs::JointTrajectoryControllerState are never been filled by default. That's why it is giving segmentation fault. However only the effort field of desired part is not populated by default.

Pulkit123 gravatar image Pulkit123  ( 2018-03-23 23:57:44 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2018-03-23 05:02:05 -0500

Seen: 342 times

Last updated: Mar 23 '18