JointState push.back not working

edit

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