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

Revision history [back]

Just in case if someone has the same issue:

std::string tf_msg_static     =  "/tf_static";
std::vector<std::string> topics;
topics.push_back(tf_msg_static);
rosbag::View view(bag, rosbag::TopicQuery(topics));
BOOST_FOREACH(rosbag::MessageInstance const m, view)
     {
      if (m.getTopic() == tf_msg_static || ("/" + m.getTopic() == tf_msg_static))  
      {
        tf2_msgs::TFMessage::ConstPtr tf_info = m.instantiate<tf2_msgs::TFMessage>();
        if (tf_info != NULL)
        {
          for(int i=0; i < tf_info->transforms.size();i++){
            if (tf_info->transforms[i].header.frame_id == "base_link"){
              if (tf_info->transforms[i].child_frame_id == "base_footprint"){
                Rotation_bl_bf.setRotation(tf::Quaternion(tf_info->transforms[i].transform.rotation.x, tf_info->transforms[i].transform.rotation.y, tf_info->transforms[i].transform.rotation.z,tf_info->transforms[i].transform.rotation.w));
                traslation_bl_bf = tf::Vector3(tf_info->transforms[i].transform.translation.x,tf_info->transforms[i].transform.translation.y,tf_info->transforms[i].transform.translation.z);
                Matrix_bl_bf << Rotation_bl_bf.getRow(0)[0],Rotation_bl_bf.getRow(0)[1] ,Rotation_bl_bf.getRow(0)[2],traslation_bl_bf.getX(),
                                Rotation_bl_bf.getRow(1)[0],Rotation_bl_bf.getRow(1)[1] ,Rotation_bl_bf.getRow(1)[2],traslation_bl_bf.getY(),
                                Rotation_bl_bf.getRow(2)[0],Rotation_bl_bf.getRow(2)[1] ,Rotation_bl_bf.getRow(2)[2],traslation_bl_bf.getZ(),
                                0,0,0,1;
              }
              if (tf_info->transforms[i].child_frame_id == "velodyne_front_link"){
                Rotation_bl_vl.setRotation(tf::Quaternion(tf_info->transforms[i].transform.rotation.x, tf_info->transforms[i].transform.rotation.y, tf_info->transforms[i].transform.rotation.z,tf_info->transforms[i].transform.rotation.w));
                traslation_bl_vl = tf::Vector3(tf_info->transforms[i].transform.translation.x,tf_info->transforms[i].transform.translation.y,tf_info->transforms[i].transform.translation.z);
                Matrix_bl_vl << Rotation_bl_vl.getRow(0)[0],Rotation_bl_vl.getRow(0)[1] ,Rotation_bl_vl.getRow(0)[2],traslation_bl_vl.getX(),
                                Rotation_bl_vl.getRow(1)[0],Rotation_bl_vl.getRow(1)[1] ,Rotation_bl_vl.getRow(1)[2],traslation_bl_vl.getY(),
                                Rotation_bl_vl.getRow(2)[0],Rotation_bl_vl.getRow(2)[1] ,Rotation_bl_vl.getRow(2)[2],traslation_bl_vl.getZ(),
                                0,0,0,1;
              }
              Matrix_bf_vl = Matrix_bl_bf.pow(-1) * Matrix_bl_vl;
              }
            }
          }
        }