ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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;
}
}
}
}