介绍ROS中sensor_msgs

介绍ROS中sensor_msgs

sensor_msgs 是 ROS（Robot Operating System）中一个常用的消息包，用于表示来自各种传感器的数据。它包含了一系列消息类型，如 LaserScan, PointCloud2, Image, Imu 等。这些消息类型在机器人感知、导航、控制等任务中非常有用。

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

  1. LaserScan
    LaserScan 用于表示从激光雷达（如 2D LIDAR）获取的扫描数据。它包含了关于激光雷达配置和扫描数据的信息。
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    #include <sensor_msgs/LaserScan.h>

    sensor_msgs::LaserScan scan;
    scan.header.frame_id = "laser_frame";
    scan.header.stamp = ros::Time::now();
    scan.angle_min = -1.57; // -90 degrees
    scan.angle_max = 1.57; // 90 degrees
    scan.angle_increment = 0.017; // 1 degree
    scan.time_increment = 0.001;
    scan.range_min = 0.1;
    scan.range_max = 10.0;
    scan.ranges.resize(181); // 181 readings
    scan.intensities.resize(181);

  2. PointCloud2
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    #include <sensor_msgs/PointCloud2.h>

    sensor_msgs::PointCloud2 pc;
    pc.header.frame_id = "sensor_frame";
    pc.header.stamp = ros::Time::now();
    pc.height = 1; // Unorganized point cloud
    pc.width = 1000;
    pc.is_bigendian = false;
    pc.point_step = 16; // Each point has 16 bytes (x, y, z, and intensity)
    pc.row_step = pc.point_step * pc.width;
    pc.is_dense = false;
    pc.data.resize(pc.row_step * pc.height);

    // Add point field information
    sensor_msgs::PointField field;
    field.name = "x";
    field.offset = 0;
    field.datatype = sensor_msgs::PointField::FLOAT32;
    field.count = 1;
    pc.fields.push_back(field);

    field.name = "y";
    field.offset = 4;
    pc.fields.push_back(field);

    field.name = "z";
    field.offset = 8;
    pc.fields.push_back(field);

    field.name = "intensity";
    field.offset = 12;
    pc.fields.push_back(field);

  3. Image
    Image 用于表示从摄像头（如 RGB 或灰度摄像头）获取的图像数据。它包含了关于图像大小、格式和实际像素数据的信息。
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    #include <sensor_msgs/Image.h>

    sensor_msgs::Image img;
    img.header.frame_id = "camera_frame";
    img.header.stamp = ros::Time::now();
    img.height = 480;
    img.width = 640;
    img.encoding = "rgb8";
    img.is_bigendian = false;
    img.step = img.width * 3; // 3 bytes per pixel for RGB image
    img.data.resize(img.step * img.height);

  4. Imu
    Imu 用于表示从惯性测量单元（IMU）获取的数据。它包含了关于加速度计、陀螺仪和磁力计的信息。
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    #include <sensor_msgs/Imu.h>

    sensor_msgs::Imu imu;
    imu.header.frame_id = "imu_frame";
    imu.header.stamp = ros::Time::now();
    imu.orientation_covariance[0] = -1; // Set orientation_covariance[0] to -1 if orientation is invalid
    imu.angular_velocity_covariance[0] = -1; // Set angular_velocity_covariance[0] to -1 if angular_velocity is invalid
    imu.linear_acceleration_covariance[0] = -1; // Set linear_acceleration_covariance[0] to -1 if linear_acceleration is invalid


下面是一个使用 C++ 编写的 ROS 节点示例，该节点发布一个假设的 sensor_msgs::LaserScan 消息。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

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

  ros::Publisher laser_pub = nh.advertise<sensor_msgs::LaserScan>("laser_scan", 1);

  ros::Rate loop_rate(10);

  while (ros::ok())
  {
    sensor_msgs::LaserScan scan;
    scan.header.frame_id = "laser_frame";
    scan.header.stamp = ros::Time::now();
    scan.angle_min = -1.57; // -90 degrees
    scan.angle_max = 1.57; // 90 degrees
    scan.angle_increment = 0.017; // 1 degree
    scan.time_increment = 0.001;
    scan.range_min = 0.1;
    scan.range_max = 10.0;
    scan.ranges.resize(181); // 181 readings
    scan.intensities.resize(181);

    for (size_t i = 0; i < scan.ranges.size(); ++i)
    {
      scan.ranges[i] = 5.0; // Assign a constant range value (5 meters)
      scan.intensities[i] = 50.0; // Assign a constant intensity value (50 units)
    }

    laser_pub.publish(scan);

    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}


在这个示例中，我们创建了一个名为 laser_scan 的 sensor_msgs::LaserScan 类型的发布器。然后我们初始化一个假设的 LaserScan 消息，并在循环中持续发布它。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES sensor_msgs_example
  CATKIN_DEPENDS roscpp sensor_msgs
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

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


在终端中，进入包的目录，然后运行以下命令以构建和运行节点：
1
2
3
4
$ catkin_make
$ source devel/setup.bash
$ rosrun sensor_msgs_example sensor_msgs_example_node


现在，你的节点应该在名为 /laser_scan 的主题上发布假设的激光扫描数据。你可以通过运行以下命令查看发布的数据：
1
2
$ rostopic echo /laser_scan


在实际应用中，你可能需要根据你的任务需求和系统配置来修改这些示例。例如，你可以将实际传感器（如激光雷达、摄像头或 IMU）的驱动程序与发布 sensor_msgs 消息的节点相结合，以执行更复杂的任务，如机器人导航、控制和感知。

接下来，我们将介绍如何在 ROS 节点中订阅 sensor_msgs::Image 消息并处理图像数据。在这个示例中，我们将简单地计算每个像素的灰度值并将其打印到控制台。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  if (msg->encoding != sensor_msgs::image_encodings::RGB8)
  {
    ROS_ERROR("Invalid image encoding, expected 'rgb8'.");
    return;
  }

  for (size_t y = 0; y < msg->height; ++y)
  {
    for (size_t x = 0; x < msg->width; ++x)
    {
      size_t index = y * msg->step + x * 3;
      uint8_t r = msg->data[index];
      uint8_t g = msg->data[index + 1];
      uint8_t b = msg->data[index + 2];

      // Calculate the grayscale value
      uint8_t gray = static_cast<uint8_t>(0.299 * r + 0.587 * g + 0.114 * b);

      // Print the grayscale value
      ROS_INFO("Pixel (%zu, %zu) gray value: %u", x, y, gray);
    }
  }
}

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

  ros::Subscriber image_sub = nh.subscribe("camera/image_raw", 1, imageCallback);

  ros::spin();

  return 0;
}


在这个示例中，我们创建了一个名为 imageCallback 的回调函数，该函数接收 sensor_msgs::Image 类型的消息。我们首先检查图像的编码是否为 RGB8。然后，我们遍历图像中的每个像素并计算其灰度值。

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

1
2
3
4
5
6
7
8
9
10
find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
)

# ...

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


在终端中，进入包的目录，然后运行以下命令以构建和运行节点：
1
2
3
4
$ catkin_make
$ source devel/setup.bash
$ rosrun your_package_name image_subscriber_node


请注意，为了让这个示例正常工作，你需要确保在 ROS 环境中有一个节点在名为 /camera/image_raw 的主题上发布 sensor_msgs::Image 类型的消息。这通常由摄像头驱动程序节点（如 usb_cam）执行。

如果你没有实际的摄像头设备，可以使用 rosbag 工具播放预先录制的图像数据。你还可以使用 image_transport 包中的 republish 工具将图像编码为 sensor_msgs::Image 消息类型，以便在本示例中使用。

1
2
$ rosrun image_transport republish raw in:=/camera/image_raw/compressed out:=/camera/image_raw _image_transport:=compressed

在这个示例中，我们假设已有一个节点发布名为 /camera/image_raw/compressed 的压缩图像数据。republish 工具将其重新发布为未压缩的 sensor_msgs::Image 类型消息，以便我们的 image_subscriber_node 节点能够订阅并处理它。

在实际应用中，你可能需要根据你的任务需求和系统配置来修改这些示例。例如，你可以将图像处理算法（如特征检测、目标跟踪等）与订阅 sensor_msgs::Image 消息的节点相结合，以执行更复杂的任务，如机器人导航、控制和感知。

总之，在本回答中，我们详细介绍了 ROS 中的 geometry_msgs 和 sensor_msgs，并提供了一些 C++ 示例。这些示例涵盖了创建、发布和订阅消息，以及处理这些消息以完成特定任务。在实际项目中，你可以根据具体需求来调整和扩展这些示例。

接下来，让我们深入了解如何在 ROS 中使用 sensor_msgs::PointCloud2 消息进行点云处理。我们将使用 PCL（Point Cloud Library）这个功能强大的点云处理库。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
  // Convert sensor_msgs::PointCloud2 to pcl::PointCloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*msg, *cloud);

  // Perform voxel grid filtering
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud(cloud);
  sor.setLeafSize(0.1f, 0.1f, 0.1f);
  sor.filter(*cloud_filtered);

  // Process the filtered point cloud
  // For example, you can calculate the centroid, perform clustering, etc.

  ROS_INFO("Filtered point cloud size: %zu", cloud_filtered->size());
}

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

  ros::Subscriber pointcloud_sub = nh.subscribe("sensor/point_cloud", 1, pointCloudCallback);

  ros::spin();

  return 0;
}

在这个示例中，我们创建了一个名为 pointCloudCallback 的回调函数，该函数接收 sensor_msgs::PointCloud2 类型的消息。我们首先将 sensor_msgs::PointCloud2 类型的数据转换为 PCL 中的 pcl::PointCloud。然后我们对点云应用体素网格滤波（Voxel Grid Filtering）来降低点云的密度。最后我们打印出过滤后的点云的大小。

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

1
2
3
4
5
6
7
8
9
10
11
12
find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  pcl_conversions
  pcl_ros
)

# ...

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

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

1
2
3
4
$ catkin_make
$ source devel/setup.bash
$ rosrun your_package_name pointcloud_processing_node

请注意，为了让这个示例正常工作，你需要确保在 ROS 环境中有一个节点在名为 /sensor/point_cloud 的主题上发布 sensor_msgs::PointCloud2 类型的消息。这通常由点云驱动程序节点（如 velodyne_pointcloud 或 depthimage_to_laserscan）执行。

本示例仅演示了 PCL 中一种基本点云处理方法。实际上，PCL 提供了许多高级的点云处理功能，例如点云分割、特征提取、配准、表面重建等。在实际应用中，你可以根据你的任务需求和系统配置来修改这些示例。例如，你可以将点云处理算法与订阅 sensor_msgs::PointCloud2 消息的节点相结合，以执行更复杂的任务，如机器人导航、控制和感知。

此外，当处理大型点云数据时，考虑使用八叉树（Octree）和 k-d树（k-d tree）等数据结构以提高计算效率。你还可以结合机器学习和深度学习技术来解决点云分类、目标检测和目标跟踪等问题。

总之，在本回答中，我们详细介绍了 ROS 中的 geometry_msgs 和 sensor_msgs，并提供了一些 C++ 示例，涵盖了创建、发布和订阅消息以及处理这些消息以完成特定任务。在实际项目中，你可以根据具体需求来调整和扩展这些示例，为你的机器人系统添加更多功能。

现在我们将讨论如何在 ROS 中使用 tf2 库进行坐标变换。tf2 库提供了处理坐标帧之间变换的功能，这对于机器人导航、感知和多传感器融合等任务至关重要。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

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

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  geometry_msgs::PointStamped point_in_base_frame;
  point_in_base_frame.header.frame_id = "base_link";
  point_in_base_frame.point.x = 1.0;
  point_in_base_frame.point.y = 0.0;
  point_in_base_frame.point.z = 0.0;

  ros::Rate rate(1.0);
  while (nh.ok())
  {
    point_in_base_frame.header.stamp = ros::Time::now();

    try
    {
      geometry_msgs::PointStamped point_in_map_frame;
      tfBuffer.transform(point_in_base_frame, point_in_map_frame, "map");
      ROS_INFO("Point in map frame: (%.2f, %.2f, %.2f)",
               point_in_map_frame.point.x,
               point_in_map_frame.point.y,
               point_in_map_frame.point.z);
    }
    catch (tf2::TransformException& ex)
    {
      ROS_WARN("%s", ex.what());
    }

    rate.sleep();
  }

  return 0;
}

在这个示例中，我们创建了一个名为 point_in_base_frame 的 geometry_msgs::PointStamped 消息，该消息包含了一个在 base_link 坐标系下的点。然后我们使用 tf2_ros::Buffer 和 tf2_ros::TransformListener 来监听坐标变换。我们尝试将点从 base_link 坐标系转换到 map 坐标系，并打印出转换后的点坐标。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
  tf2
  tf2_ros
  tf2_geometry_msgs
)

# ...

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


在终端中，进入包的目录，然后运行以下命令以构建和运行节点：
1
2
3
4
$ catkin_make
$ source devel/setup.bash
$ rosrun your_package_name tf2_example_node


请注意，为了让这个示例正常工作，你需要确保在 ROS 环境中有一个节点发布 map 和 base_link 之间的坐标变换。这通常由机器人定位系统（如 amcl）或静态变换发布器（如 static_transform_publisher）执行。在没有实际机器人的情况下，你可以使用 static_transform_publisher 来发布一个固定的坐标变换。在终端中，运行以下命令：
1
2
$ rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 map base_link


这将发布一个将 base_link 坐标系与 map 坐标系对齐的静态变换。

在实际应用中，你可能需要根据你的任务需求和系统配置来修改这些示例。例如，你可以订阅机器人或传感器发布的 geometry_msgs::PoseStamped 或 geometry_msgs::PoseWithCovarianceStamped 消息，然后将它们转换到全局坐标系，如 map 或 odom。这对于多机器人协同、全局路径规划和融合来自不同传感器的信息等任务非常有用。

总之，在本回答中，我们详细介绍了 ROS 中的 geometry_msgs 和 sensor_msgs，并提供了一些 C++ 示例。这些示例涵盖了创建、发布和订阅消息，以及处理这些消息以完成特定任务。本回答还讨论了如何在 ROS 中使用 tf2 库进行坐标变换。在实际项目中，你可以根据具体需求来调整和扩展这些示例，以便为你的机器人系统添加更多功能。

现在我们将讨论如何在ROS中使用动作服务器（Action Server）和动作客户端（Action Client）。动作允许你执行那些需要较长时间才能完成的任务，同时提供反馈和可抢占（可中止）的功能。

首先，让我们创建一个简单的斐波那契数列计算的动作。在你的ROS包中创建一个名为Fibonacci.action的文件，并添加以下内容：

1
2
3
4
5
6
7
8
9
# Goal definition
int32 order
---
# Result definition
int32[] sequence
---
# Feedback definition
int32[] sequence


这个动作接受一个整数（斐波那契数列的阶数）作为目标，返回一个整数数组（计算得到的斐波那契数列）作为结果，并在执行过程中提供整数数组作为反馈。

接下来，在CMakeLists.txt文件中添加以下内容以生成动作消息：

1
2
3
4
5
6
7
8
9
10
11
12
13
find_package(catkin REQUIRED COMPONENTS
  actionlib_msgs
)

add_action_files(
  DIRECTORY action
  FILES Fibonacci.action
)

generate_messages(
  DEPENDENCIES actionlib_msgs
)

创建一个名为fibonacci_server_node.cpp的文件以实现动作服务器。编辑文件并添加以下代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <your_package_name/FibonacciAction.h>

class FibonacciActionServer
{
  protected:
    ros::NodeHandle nh_;
    actionlib::SimpleActionServer<your_package_name::FibonacciAction> as_;
    std::string action_name_;
    your_package_name::FibonacciFeedback feedback_;
    your_package_name::FibonacciResult result_;

  public:
    FibonacciActionServer(const std::string& name) :
      as_(nh_, name, boost::bind(&FibonacciActionServer::executeCb, this, _1), false),
      action_name_(name)
    {
      as_.start();
    }

    void executeCb(const your_package_name::FibonacciGoalConstPtr& goal)
    {
      ros::Rate r(1);
      bool success = true;

      feedback_.sequence.clear();
      feedback_.sequence.push_back(0);
      feedback_.sequence.push_back(1);

      for (int i = 1; i <= goal->order; ++i)
      {
        if (as_.isPreemptRequested() || !ros::ok())
        {
          as_.setPreempted();
          success = false;
          break;
        }

        feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i - 1]);
        as_.publishFeedback(feedback_);
        r.sleep();
      }

      if (success)
      {
        result_.sequence = feedback_.sequence;
        as_.setSucceeded(result_);
      }
    }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "fibonacci_server_node");
  FibonacciActionServer fibonacci_action_server("fibonacci");
  ros::spin();
  return 0;
}


在这个示例中，我们创建了一个名为FibonacciActionServer的类，并实现了一个名为executeCb的回调函数。该回调函数在收到一个目标时计算斐波那契数列，并在执行过程中通过publishFeedback 方法发布反馈。如果动作被抢占或者 ROS 不再运行，该回调函数会提前结束并设置抢占状态。在成功完成斐波那契数列计算后，结果将被发布。

为了编译和运行此示例，你需要在你的 ROS 包的 CMakeLists.txt 文件中添加以下内容：

1
2
3
4
5
6
7
8
9
10
11
find_package(catkin REQUIRED COMPONENTS
  roscpp
  actionlib
  your_package_name
)

# ...

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


在终端中，进入包的目录，然后运行以下命令以构建和运行节点：
1
2
3
4
$ catkin_make
$ source devel/setup.bash
$ rosrun your_package_name fibonacci_server_node


现在我们将创建一个名为fibonacci_client_node.cpp的文件以实现动作客户端。编辑文件并添加以下代码：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <your_package_name/FibonacciAction.h>
#include <iostream>

void feedbackCb(const your_package_name::FibonacciFeedbackConstPtr& feedback)
{
  std::cout << "Current Fibonacci sequence: ";
  for (size_t i = 0; i < feedback->sequence.size(); ++i)
  {
    std::cout << feedback->sequence[i] << (i < feedback->sequence.size() - 1 ? ", " : "\n");
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "fibonacci_client_node");
  actionlib::SimpleActionClient<your_package_name::FibonacciAction> ac("fibonacci", true);
  ac.waitForServer();

  your_package_name::FibonacciGoal goal;
  goal.order = 10;

  ac.sendGoal(goal, actionlib::SimpleActionClient<your_package_name::FibonacciAction>::SimpleDoneCallback(),
              actionlib::SimpleActionClient<your_package_name::FibonacciAction>::SimpleActiveCallback(),
              boost::bind(&feedbackCb, _1));

  ac.waitForResult();

  if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
  {
    std::cout << "Successfully computed the Fibonacci sequence" << std::endl;
  }
  else
  {
    std::cout << "Failed to compute the Fibonacci sequence" << std::endl;
  }

  return 0;
}


在这个示例中，我们创建了一个动作客户端并等待动作服务器。然后，我们发送一个目标并等待结果。在此过程中，我们提供了一个名为feedbackCb的反馈回调函数，用于显示当前计算的斐波那契数列。

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

1
2
3
add_executable(fibonacci_client_node src/fibonacci_client_node.cpp)
target_link_libraries(fibonacci_client_node ${catkin_LIBRARIES})


在终端中，进入包的目录，然后运行以下命令以构建和运行节点：
1
2
3
4
5
$ catkin_make
$ source devel/setup.bash
$
$ rosrun your_package_name fibonacci_client_node


当客户端运行时，你将看到当前计算的斐波那契数列作为反馈，以及计算是否成功完成的最终结果。

在实际应用中，你可能需要根据你的任务需求和系统配置来修改这些示例。例如，你可以实现一个机器人导航任务，其中动作服务器负责接收目标位置并控制机器人移动，同时向客户端提供移动过程的反馈。客户端可以通过发送新的目标位置或取消当前目标来控制机器人的行为。这些功能对于实现可抢占的机器人任务和自适应控制策略非常有用。

总之，在本回答中，我们详细介绍了如何在 ROS 中实现动作服务器和动作客户端，并提供了一个简单的斐波那契数列计算示例。这些示例涵盖了创建动作服务器和客户端，发送目标，处理反馈和结果以完成特定任务。在实际项目中，你可以根据具体需求来调整和扩展这些示例，以便为你的机器人系统添加更多功能。

slam > ROS
#原创
介绍ROS中sensor_msgs
https://qiangsun89.github.io/2023/04/25/介绍ROS中sensor-msgs/
作者
Qiang Sun
发布于
2023年4月25日
许可协议