ros点的坐标转换PointStamped

void base2map(float cx, float cy, float cz, float &mx, float &my, float &mz) {
     
  geometry_msgs::PointStamped point_laser, point_map;
  point_laser.point.x = cx;
  point_laser.point.y = cy;
  point_laser.point.z = cz;
  point_laser.header.frame_id = "base_link";
  point_laser.header.stamp = ros::Time();
  try {
     
    listener.transformPoint("map", point_laser, point_map);
  } catch(tf::TransformException& ex) {
     
    ROS_ERROR("%s", ex.what());
    return;
  }
  mx = point_map.point.x;
  my = point_map.point.y;
  mz = point_map.point.z;
}

你可能感兴趣的:(ROS)