Ask Your Question
0

JointState push.back not working

asked 2017-11-03 15:26:56 -0500

jdeleon gravatar image

Hello, I'm very newbie in ROS and C++.

I'm trying to read the JointStates values and passed to a vector to work with that values.

     sensor_msgs::JointState pos_patas;

    std::vector<double> patas_posicion_actual;

    void lectura_posicion_patas(const sensor_msgs::JointStatePtr& msg)
    {
      pos_patas = *msg;

      for (int i = 0; i < pos_patas.position.size(); i++)
      {
        ROS_INFO("joint %d position  %.2f", i, pos_patas.position.at(i));
        patas_posicion_actual.push_back(msg->position.at(i));
        ROS_INFO("pata actual %d position %.2f", i, patas_posicion_actual);
      }
    }

int main(int argc, char **argv)
{

  //---------------------------------------------------------------------------------
  //  Setup
  //---------------------------------------------------------------------------------

  //Inicio del nodo en ROS y el node handle
  ros::init(argc, argv, "gait_template");
  ros::NodeHandle node_sub;   //Nodo que se suscribe al JointState

  //Suscripción al tópico del JointStates
  ros::Subscriber sub = node_sub.subscribe("/rhex/joint_states", 1, lectura_posicion_patas);
  ros::Rate rate(10.0);

  pos_patas.position.resize(6);
  patas_posicion_actual.resize(6);

  while(ros::ok()){

    rate.sleep();
    ros::spinOnce();
  }

  return 0;

}

And this is the output in the terminal:

[ INFO] [1509740062.384800961, 1010.545000000]: joint 0 position  -4.51
[ INFO] [1509740062.384837126, 1010.545000000]: pata actual 0 position 0.00
[ INFO] [1509740062.384851819, 1010.545000000]: joint 1 position  -4.51
[ INFO] [1509740062.384865605, 1010.545000000]: pata actual 1 position 0.00
[ INFO] [1509740062.384878372, 1010.545000000]: joint 2 position  -4.51
[ INFO] [1509740062.384891682, 1010.545000000]: pata actual 2 position 0.00
[ INFO] [1509740062.384904027, 1010.545000000]: joint 3 position  -4.51
[ INFO] [1509740062.384916920, 1010.545000000]: pata actual 3 position 0.00
[ INFO] [1509740062.384929075, 1010.545000000]: joint 4 position  -4.51
[ INFO] [1509740062.384942269, 1010.545000000]: pata actual 4 position 0.00
[ INFO] [1509740062.384954612, 1010.545000000]: joint 5 position  -4.51
[ INFO] [1509740062.384967704, 1010.545000000]: pata actual 5 position 0.00
edit retag flag offensive close merge delete

Comments

1

also: may I suggest a title change? JointState is a ROS msg type, but you're actually using a std::vector. And the problem is most likely not with push_back(..), but somewhere else.

gvdhoorn gravatar image gvdhoorn  ( 2017-11-04 06:22:26 -0500 )edit

I have solve without the push.back function. Just adding to the variable that value

patas_posicion_actual.at(i) = patas_joint_state.position.at(i);

jdeleon gravatar image jdeleon  ( 2017-11-04 06:43:11 -0500 )edit

I'm not entirely convinced that is really solving it. Passing a std::vector without an index to ROS_INFO(..) should not work.

gvdhoorn gravatar image gvdhoorn  ( 2017-11-04 06:47:14 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2017-11-04 06:03:57 -0500

gvdhoorn gravatar image

updated 2017-11-04 06:04:15 -0500

ROS_INFO("pata actual %d position %.2f", i, patas_posicion_actual);

I'm not sure, but I don't think you can just pass a std::vector to ROS_INFO(..) and expect it to print anything.

It could well be that the 0.00 you are seeing is actually just garbage memory that happens to evaluate to a 0.00 double.

Try this:

ROS_INFO("pata actual %d position %.2f", i, patas_posicion_actual[i]);

(note the [i] to index into the vector).

edit flag offensive delete link more

Comments

Hello, I'm very newbie in ROS and C++.

In that case I would recommend to first get some experience with C++, as your current question is not a ROS one, but a basic C++ one. You'll be much more comfortable with ROS once you know some C++.

gvdhoorn gravatar image gvdhoorn  ( 2017-11-04 06:05:21 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-11-03 15:26:56 -0500

Seen: 294 times

Last updated: Nov 04 '17