在类中使用参数(C++)

**目标:**使用C++创建和运行具有ROS参数的类。

教程级别: 初学者

**时间:**20分钟

背景

当创建自己的 节点 时,有时需要添加可以从启动文件中设置的参数。

本教程将向您展示如何在C++类中创建这些参数,并如何在启动文件中设置它们。

先决条件

在之前的教程中,您已经学习了如何 创建工作空间创建包。您还了解了 ROS 2 系统中参数的作用和功能。

任务

1 创建一个包

在一个新的终端中 源化你的 ROS 2 安装,这样 ros2 命令才能正常工作。

按照:ref:这些指示 创建名为``ros2_ws``的新工作空间。

请记住,包应该在“src”目录中创建,而不是工作区的根目录。进入“ros2_ws/src”并创建一个新的包:

ros2 pkg create --build-type ament_cmake cpp_parameters --dependencies rclcpp

您的终端将返回一条消息,验证您的包“cpp_parameters”及其所有必要的文件和文件夹的创建。

“--dependencies”参数将自动添加必要的依赖项行到“package.xml”和“CMakeLists.txt”中。

1.1 更新 package.xml

因为您在包创建过程中使用了“--dependencies”选项,所以无需手动添加依赖项到“package.xml”或“CMakeLists.txt”中。

同样,确保将描述、维护者电子邮件和姓名以及许可证信息添加到 package.xml 中。

<description>C++ parameter tutorial</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

2 编写 C++ 节点

ros2_ws/src/cpp_parameters/src 目录中创建一个名为 cpp_parameters_node.cpp 的新文件,并将以下代码粘贴到其中:

#include <chrono>
#include <functional>
#include <string>

#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

class MinimalParam : public rclcpp::Node
{
public:
  MinimalParam()
  : Node("minimal_param_node")
  {
    this->declare_parameter("my_parameter", "world");

    timer_ = this->create_wall_timer(
      1000ms, std::bind(&MinimalParam::timer_callback, this));
  }

  void timer_callback()
  {
    std::string my_param = this->get_parameter("my_parameter").as_string();

    RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str());

    std::vector<rclcpp::Parameter> all_new_parameters{rclcpp::Parameter("my_parameter", "world")};
    this->set_parameters(all_new_parameters);
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalParam>());
  rclcpp::shutdown();
  return 0;
}

2.1 检查代码

顶部的 #include 语句是包依赖项。

下面的代码段创建了一个类和构造函数。构造函数的第一行创建了一个名为 my_parameter 的参数,其默认值为 world。参数类型是根据默认值推断的,在这种情况下,参数类型将设置为字符串类型。接下来,timer_ 使用1000ms的周期进行初始化,这会导致每秒执行一次 timer_callback 函数。

class MinimalParam : public rclcpp::Node
{
public:
  MinimalParam()
  : Node("minimal_param_node")
  {
    this->declare_parameter("my_parameter", "world");

    timer_ = this->create_wall_timer(
      1000ms, std::bind(&MinimalParam::timer_callback, this));
  }

我们的 timer_callback 函数的第一行从节点中获取参数 my_parameter,并将其存储在 my_param 中。接下来,RCLCPP_INFO 函数确保事件被记录。然后,set_parameters 函数将参数 my_parameter 设置回默认的字符串值 world。在用户外部更改参数的情况下,这可以确保它始终被重置回原始值。

void timer_callback()
{
  std::string my_param = this->get_parameter("my_parameter").as_string();

  RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str());

  std::vector<rclcpp::Parameter> all_new_parameters{rclcpp::Parameter("my_parameter", "world")};
  this->set_parameters(all_new_parameters);
}

最后是 timer_ 的声明。

private:
  rclcpp::TimerBase::SharedPtr timer_;

在我们的 MinimalParam 后面是我们的 main 函数。在这里,ROS 2 被初始化,构造了一个 MinimalParam 类的实例,并且 rclcpp::spin 开始处理来自节点的数据。

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalParam>());
  rclcpp::shutdown();
  return 0;
}
2.1.1(可选)添加 ParameterDescriptor。

可选的,您可以为参数设置一个描述符。描述符允许您指定参数及其约束的文本描述,例如设置为只读、指定范围等。为了使其生效,构造函数中的代码必须进行更改:

// ...

class MinimalParam : public rclcpp::Node
{
public:
  MinimalParam()
  : Node("minimal_param_node")
  {
    auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
    param_desc.description = "This parameter is mine!";

    this->declare_parameter("my_parameter", "world", param_desc);

    timer_ = this->create_wall_timer(
      1000ms, std::bind(&MinimalParam::timer_callback, this));
  }

其余的代码保持不变。运行节点后,您可以运行``ros2 param describe /minimal_param_node my_parameter``来查看类型和描述。

2.2 添加可执行文件

现在打开``CMakeLists.txt``文件。在依赖项``find_package(rclcpp REQUIRED)``下方添加以下代码行。

add_executable(minimal_param_node src/cpp_parameters_node.cpp)
ament_target_dependencies(minimal_param_node rclcpp)

install(TARGETS
    minimal_param_node
  DESTINATION lib/${PROJECT_NAME}
)

3. 构建和运行

在构建之前,最好在工作空间的根目录(ros2_ws)中运行``rosdep``来检查是否缺少依赖项:

rosdep install -i --from-path src --rosdistro humble -y

返回到您的工作空间的根目录 ros2_ws,并构建您的新包:

colcon build --packages-select cpp_parameters

打开一个新的终端,导航到 ros2_ws,并加载设置文件:

source install/setup.bash

现在运行节点:

ros2 run cpp_parameters minimal_param_node

终端应该每秒返回以下消息:

[INFO] [minimal_param_node]: Hello world!

现在您可以看到参数的默认值,但您希望能够自己设置它。有两种方法可以实现这一点。

3.1 通过控制台进行更改

本部分将使用您从 参数教程 中获得的知识,并将其应用到您刚刚创建的节点上。

确保节点正在运行:

ros2 run cpp_parameters minimal_param_node

在另一个终端中打开,再次从``ros2_ws``目录中加载设置文件,然后输入以下命令行:

ros2 param list

在那里,您将看到自定义参数``my_parameter``。要进行更改,只需在控制台中运行以下命令行:

ros2 param set /minimal_param_node my_parameter earth

如果您看到输出``Set parameter successful``,则表示更改成功。如果您查看另一个终端,您应该看到输出更改为``[INFO] [minimal_param_node]: Hello earth!``

3.2 通过启动文件进行更改

您还可以在启动文件中设置该参数,但首先需要添加启动目录。在``ros2_ws/src/cpp_parameters/``目录内,创建一个名为``launch``的新目录。在其中,创建一个名为``cpp_parameters_launch.py``的新文件。

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package="cpp_parameters",
            executable="minimal_param_node",
            name="custom_minimal_param_node",
            output="screen",
            emulate_tty=True,
            parameters=[
                {"my_parameter": "earth"}
            ]
        )
    ])

在这里,您可以看到当我们启动节点``minimal_param_node``时,我们将``my_parameter``设置为``earth``。通过添加以下两行,我们确保输出打印在控制台上。

output="screen",
emulate_tty=True,

现在打开``CMakeLists.txt``文件。在您之前添加的行下方,添加以下代码行。

install(
  DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

打开一个控制台并导航到您的工作空间的根目录``ros2_ws``,然后构建您的新软件包:

colcon build --packages-select cpp_parameters

然后在新的终端中加载设置文件:

source install/setup.bash

现在使用刚刚创建的启动文件运行节点:

ros2 launch cpp_parameters cpp_parameters_launch.py

终端应该每秒返回以下消息:

[INFO] [custom_minimal_param_node]: Hello earth!

总结

您创建了一个带有自定义参数的节点,可以从启动文件或命令行中进行设置。您将依赖项、可执行文件和启动文件添加到软件包配置文件中,以便您可以构建和运行它们,并查看参数的效果。

下一步

现在您拥有了一些软件包和自己的ROS 2系统,下一个教程 将向您展示如何在您的环境和系统中检查问题,以防出现问题。