在ros中获取机器人的当前位姿

可以使用ROS提供的tf库来获取机器人的当前位姿。具体步骤如下：

1. 确定需要获取位姿的机器人或机器人部件的frame_id和target_frame_id。

2. 创建一个tf::TransformListener对象来监听tf变换。

tf::TransformListener listener;

3. 使用lookupTransform函数获取机器人或机器人部件的当前位姿。

tf::StampedTransform transform;
try{
  listener.lookupTransform(target_frame_id, frame_id, ros::Time(0), transform); 
}
catch (tf::TransformException &ex) {
  ROS_ERROR("%s",ex.what());
  ros::Duration(1.0).sleep();
}

其中，target_frame_id表示目标坐标系，frame_id表示源坐标系，ros::Time(0)表示获取最近的tf变换，transform表示返回的位姿信息。如果出现异常，可以通过ROS_ERROR输出错误信息并延时等待再次尝试。

4. 从返回的位姿信息中获取机器人的当前位姿。

double x = transform.getOrigin().x();
double y = transform.getOrigin().y();
double z = transform.getOrigin().z();
tf::Quaternion q = transform.getRotation();
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

其中，x、y、z表示机器人当前位置的三个坐标，q表示机器人当前姿态的四元数，roll、pitch、yaw表示机器人当前姿态的欧拉角。

完整代码示例：

#include <ros/ros.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "tf_listener");
  ros::NodeHandle node;

  tf::TransformListener listener;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("base_link", "odom", ros::Time(0), transform); // 获取base_link相对于odom的变换
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    double x = transform.getOrigin().x();
    double y = transform.getOrigin().y();
    double z = transform.getOrigin().z();
    tf::Quaternion q = transform.getRotation();
    double roll, pitch, yaw;
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

    ROS_INFO("Current pose: x=%f, y=%f, z=%f, roll=%f, pitch=%f, yaw=%f",
             x, y, z, roll, pitch, yaw);

    rate.sleep();
  }
  return 0;
}

在该示例中，我们获取机器人base_link相对于odom的变换，并输出机器人的当前位姿信息。其中，rate(10.0)表示每秒执行10次循环，即每隔0.1秒获取一次机器人的位姿信息。