ROS中geometry_msgs

最新推荐文章于 2025-03-10 11:21:55 发布

Just-do-it！

最新推荐文章于 2025-03-10 11:21:55 发布

阅读量4.3k

点赞数 36

文章标签：机器人算法

本文链接：https://blog.csdn.net/weixin_52399078/article/details/138423549

版权

本文介绍了ROS中的geometry_msgs消息包，包括Point、Vector3、Quaternion、Pose、Twist等，以及它们在机器人定位、导航和控制中的作用。提供C++示例展示了如何在ROS中发布和订阅这些消息，并演示了如何使用tf库进行坐标系变换。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

ROS中geometry_msgs

geometry_msgs 是 ROS（Robot Operating System）中一个常用的消息包，用于表示机器人系统中几何形状和变换。它包括了一系列消息类型，如 Point, Vector3, Quaternion, Pose, Twist 等。这些消息类型在机器人定位、导航、控制等任务中非常有用。

以下是 geometry_msgs 中一些常用消息类型的简介以及 C++ 示例：

Point
Point 用于表示三维空间中的一个点。它有三个浮点类型的成员变量：x, y, z。

#include <geometry_msgs/Point.h>

geometry_msgs::Point point;
point.x = 1.0;
point.y = 2.0;
point.z = 3.0;

Vector3
Vector3 用于表示三维空间中的一个向量。它同样有三个浮点类型的成员变量：x, y, z。
```
#include <geometry_msgs/Vector3.h>

geometry_msgs::Vector3 vector;
vector.x = 1.0;
vector.y = 2.0;
vector.z = 3.0;
```

Quaternion
Quaternion 用于表示四元数，用于描述三维空间中的旋转。它有四个浮点类型的成员变量：x, y, z, w。

#include <geometry_msgs/Quaternion.h>

geometry_msgs::Quaternion quaternion;
quaternion.x = 0.0;
quaternion.y = 0.0;
quaternion.z = 0.0;
quaternion.w = 1.0;

Pose
Pose 用于表示机器人在三维空间中的位置和方向。它包括一个 Point 类型的成员变量 position 和一个 Quaternion 类型的成员变量 orientation。

#include <geometry_msgs/Pose.h>

geometry_msgs::Pose pose;
pose.position.x = 1.0;
pose.position.y = 2.0;
pose.position.z = 3.0;
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
pose.orientation.z = 0.0;
pose.orientation.w = 1.0;

Twist

Twist 用于表示机器人的线速度和角速度。它包括两个 Vector3 类型的成员变量：linear 和 angular。

#include <geometry_msgs/Twist.h>

geometry_msgs::Twist twist;
twist.linear.x = 1.0;
twist.linear.y = 0.0;
twist.linear.z = 0.0;
twist.angular.x = 0.0;
twist.angular.y = 0.0;
twist.angular.z = 1.0;

这些消息类型可以用于发布和订阅 ROS 主题。在实际应用中，你可能需要根据你的任务需求来使用这些消息类型。

接下来，我们将介绍如何在 ROS 中发布和订阅这些 geometry_msgs 类型的消息。

以下是一个使用 C++ 编写的 ROS 节点示例，该节点发布 geometry_msgs::Twist 消息并订阅 geometry_msgs::Pose 消息。

首先，创建一个名为 geometry_example 的 ROS 节点包，并在其中创建一个名为 geometry_example_node.cpp 的文件。编辑文件并添加以下代码：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>

// Pose callback function
void poseCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
  ROS_INFO("Received pose: [x: %f, y: %f, z: %f]", msg->position.x, msg->position.y, msg->position.z);
}

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

  // Publisher
  ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

  // Subscriber
  ros::Subscriber pose_sub = nh.subscribe("robot_pose", 1, poseCallback);

  // Set the loop rate
  ros::Rate loop_rate(10);

  while (ros::ok())
  {
    geometry_msgs::Twist twist_msg;
    twist_msg.linear.x = 1.0;
    twist_msg.angular.z = 0.5;

    // Publish the Twist message
    twist_pub.publish(twist_msg);

    // Call any pending callbacks
    ros::spinOnce();

    // Sleep for the remaining time to enforce loop rate
    loop_rate.sleep();
  }

  return 0;
}

在这个示例中，我们创建了一个名为 cmd_vel 的 geometry_msgs::Twist 类型的发布器。这个发布器可以用于发布机器人的速度指令。同时，我们订阅了一个名为 robot_pose 的 geometry_msgs::Pose 类型的主题。一旦接收到新的 Pose 消息，我们将在控制台上打印出机器人的位置。

要编译和运行此示例，请在 geometry_example 包的 CMakeLists.txt 文件中添加以下内容：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES geometry_example
  CATKIN_DEPENDS roscpp geometry_msgs
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(geometry_example_node src/geometry_example_node.cpp)
target_link_libraries(geometry_example_node ${catkin_LIBRARIES})

在终端中，进入包的目录，然后运行以下命令以构建和运行节点：

catkin_make
source devel/setup.bash
rosrun geometry_example geometry_example_node

现在，该节点将持续发布 cmd_vel 主题并订阅 robot_pose 主题。你可以使用 rostopic 工具查看发布的 Twist 消息，并使用模拟器或其他 ROS 节点发布 Pose 消息以查看节点如何响应。

现在我们已经创建了一个简单的 ROS 节点，它可以发布 geometry_msgs::Twist 类型的消息并订阅 geometry_msgs::Pose 类型的消息。下面我们将介绍如何在 ROS 环境中使用 tf 库进行坐标系变换。

tf 是一个 ROS 库，用于处理多个坐标系之间的转换。它可以将一个点或姿态从一个坐标系转换到另一个坐标系。这对于机器人定位、导航和控制等任务非常重要。

以下是使用 C++ 编写的一个简单示例，演示如何在 ROS 中使用 tf 进行坐标系变换：

首先，在 geometry_example 包中创建一个名为 tf_example_node.cpp 的文件。编辑文件并添加以下代码：

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

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

  // Create a TransformListener
  tf::TransformListener listener;

  // Set the loop rate
  ros::Rate loop_rate(10);

  while (ros::ok())
  {
    // Create a PointStamped message
    geometry_msgs::PointStamped point_in;
    point_in.header.frame_id = "base_link";
    point_in.header.stamp = ros::Time::now();
    point_in.point.x = 1.0;
    point_in.point.y = 0.0;
    point_in.point.z = 0.0;

    try
    {
      geometry_msgs::PointStamped point_out;
      listener.transformPoint("map", point_in, point_out);

      ROS_INFO("Point in map frame: [x: %f, y: %f, z: %f]", point_out.point.x, point_out.point.y, point_out.point.z);
    }
    catch (tf::TransformException& ex)
    {
      ROS_ERROR("Transform error: %s", ex.what());
    }

    // Call any pending callbacks
    ros::spinOnce();

    // Sleep for the remaining time to enforce loop rate
    loop_rate.sleep();
  }

  return 0;
}

在这个示例中，我们创建了一个名为 listener 的 tf::TransformListener 对象，用于监听坐标系之间的转换。然后我们创建了一个 geometry_msgs::PointStamped 消息，并将其 frame_id 设置为 “base_link”。我们尝试将这个点从 “base_link” 坐标系转换到 “map” 坐标系。如果转换成功，我们将在控制台上打印出转换后的点的坐标。

要编译和运行此示例，请在 geometry_example 包的 CMakeLists.txt 文件中添加以下内容：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
  tf
)

# ...

add_executable(tf_example_node src/tf_example_node.cpp)
target_link_libraries(tf_example_node ${catkin_LIBRARIES})

在终端中，进入包的目录，然后运行以下命令以构建和运行节点：
```
catkin_make
source devel/setup.bash
rosrun geometry_example tf_example_node
```

在此示例中，我们尝试将一个点从 “base_link” 坐标系转换到 “map” 坐标系。要使这个示例正常工作，你需要确保已经在 ROS 环境中发布了这两个坐标系之间的转换。在实际应用中，这些转换通常由传感器（例如激光雷达或摄像头）的驱动程序或机器人的定位系统（例如 SLAM 或 AMCL）发布。

你可以使用以下命令来检查是否有正在发布这两个坐标系之间的转换：

rosrun tf tf_echo base_link map

如果没有发布这些转换，你可以使用 static_transform_publisher 工具发布一个静态转换，以便在运行上述示例时进行测试。以下命令将发布一个从 “base_link” 到 “map” 的恒定转换：

rosrun tf static_transform_publisher 1.0 0.0 0.0 0.0 0.0 0.0 base_link map 100

这将使 “map” 坐标系相对于 “base_link” 坐标系沿 x 轴平移 1.0 米。现在，当你运行 tf_example_node 时，它应该能够成功地将点从 “base_link” 坐标系转换到 “map” 坐标系，并在控制台上打印出转换后的点的坐标。

在实际应用中，你可能需要根据你的任务需求和系统配置来修改这些示例。例如，你可以将点和姿态的转换与订阅和发布的 geometry_msgs 消息相结合，以执行更复杂的任务，如机器人导航和控制。