Read Rosbag into Variable

edit

Hello everyone,

I am trying to read from a rosbag within my c++ node, and store the value of a message in a variable: This is my rosbag info:

path:        moveToPreGraspPosition.bag
version:     2.0
duration:    0.0s
start:       Nov 03 2015 09:50:28.73 (1446544228.73)
end:         Nov 03 2015 09:50:28.73 (1446544228.73)
size:        21.0 KB
messages:    1
compression: none [1/1 chunks]
types:       control_msgs/FollowJointTrajectoryActionGoal [cff5c1d533bf2f82dd0138d57f4304bb]
topics:      /mbot05/cyton_joint_trajectory_action_controller/follow_joint_trajectory/goal   1 msg     : control_msgs/FollowJointTrajectoryActionGoal

So, I have one message of type control_msgs/FollowJointTrajectoryActionGoal and I want to create a variable of this type in my code and store this info in that variable. I followed some tutorials online and this is what I came up with:

actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("cyton_joint_trajectory_action_controller/follow_joint_trajectory", true);
ac.waitForServer(); //will wait for infinite time

rosbag::Bag bag;
bag.open("/home/catkin_ws/src/pick_place_demo/moveToPreGraspPosition.bag", rosbag::bagmode::Read);

std::vector<std::string> topics;
topics.push_back(std::string("/mbot05/cyton_joint_trajectory_action_controller/follow_joint_trajectory/goal"));
rosbag::View view(bag, rosbag::TopicQuery(topics));

BagSubscriber<control_msgs::FollowJointTrajectoryActionGoal> teste;  

BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
    control_msgs::FollowJointTrajectoryActionGoal::ConstPtr s = m.instantiate<control_msgs::FollowJointTrajectoryActionGoal>();
    if (s != NULL)
    teste.newMessage(s);
}

bag.close();
ac.sendGoal(teste.goal);

//control_msgs::FollowJointTrajectoryActionGoal sd2;
//ac.sendGoal(sd2.goal);

bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
if (finished_before_timeout){
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
}else{
    ROS_INFO("Action did not finish before the time out.");
}

Where BagSubscriber is defined like this:

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
    signalMessage(msg);
  }
};

When I run this, however, I cannot access the field "goal" inside my variable because I have the wrong type, giving me a compiling error:

/home/catkin_ws/src/pick_place_demo/src/PickObject.cpp:108:21: error: ‘class BagSubscriber<control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> > >’ has no member named ‘goal’

If run the commented lines instead, it compiles smoothly. Do you know how I can copy the contents of my bag topic to my variable so it's clean? If I try other approaches to make s igual to sd2 I usually get this:

no known conversion for argument 1 from ‘control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> >::ConstPtr {aka boost::shared_ptr<const control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> > >}’ to ‘const control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> >&’