ROS2教程(5) - 自定义接口、在类中使用参数、使用插件 - Linux

9分25秒

于 2024-07-18 10:34:12 发布

阅读量677

点赞数 12

分类专栏： ROS2 文章标签： linux 算法

本文链接：https://blog.csdn.net/2301_79695159/article/details/140515801

版权

ROS2 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

自定义接口

create pkg

cd ~/ros2_ws/src && ros2 pkg create --build-type ament_cmake --license Apache-2.0 more_interfaces
mkdir more_interfaces/msg

msg

~/ros2_ws/src/more_interfaces/msg/AddressBook.msg

uint8 PHONE_TYPE_HOME=0
uint8 PHONE_TYPE_WORK=1
uint8 PHONE_TYPE_MOBILE=2

string first_name
string last_name
string phone_number
uint8 phone_type

package.xml

~/ros2_ws/src/more_interfaces/package.xml

<!-- 添加 -->

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

cpp

~/ros2_ws/src/more_interfaces/src/publish_address_book.cpp

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "more_interfaces/msg/address_book.hpp"

using namespace std::chrono_literals;

class AddressBookPublisher : public rclcpp::Node
{
public:
  AddressBookPublisher()
  : Node("address_book_publisher")
  {
    address_book_publisher_ =
      this->create_publisher<more_interfaces::msg::AddressBook>("address_book", 10);

    // 创建回调以定期发布消息
    auto publish_msg = [this]() -> void {
        // 消息实例
        auto message = more_interfaces::msg::AddressBook();
        message.first_name = "John";
        message.last_name = "Doe";
        message.phone_number = "1234567890";
        message.phone_type = message.PHONE_TYPE_MOBILE;
        // 定期发布消息
        std::cout << "Publishing Contact\nFirst:" << message.first_name <<
          "  Last:" << message.last_name << std::endl;

        this->address_book_publisher_->publish(message);
      };
    // 创建一个1s的定时器，每秒调用我们的函数
    timer_ = this->create_wall_timer(1s, publish_msg);
  }

private:
  rclcpp::Publisher<more_interfaces::msg::AddressBook>::SharedPtr address_book_publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<AddressBookPublisher>());
  rclcpp::shutdown();

  return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(more_interfaces)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find_package
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rclcpp REQUIRED)

set(msg_files
  "msg/AddressBook.msg"
  # "msg/Message1.msg"
  # "msg/Message2.msg"
)

# set(srv_files
#   "srv/Service1.srv"
#   "srv/Service2.srv"
#   )

rosidl_generate_interfaces(${PROJECT_NAME}
  ${msg_files}
  ${srv_files}
)

# 导出消息的依赖项
ament_export_dependencies(rosidl_default_runtime)

add_executable(publish_address_book src/publish_address_book.cpp)
ament_target_dependencies(publish_address_book rclcpp)

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

# 使用同一包中生成的消息
rosidl_get_typesupport_target(cpp_typesupport_target
  ${PROJECT_NAME} rosidl_typesupport_cpp)

target_link_libraries(publish_address_book "${cpp_typesupport_target}")

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

RUN

# 终端1
cd ~/ros2_ws
colcon build --packages-up-to more_interfaces
source install/local_setup.bash
ros2 run more_interfaces publish_address_book
# 终端2
cd ~/ros2_ws
source install/setup.bash
ros2 topic echo /address_book

拓展

您可以在新的接口定义中使用现有接口定义。例如，有一个名为Contact.msg的消息，属于名为rosidl_tutorials_msgs的ros2包。假设它的定义与我们之前定义的AddressBook.msg接口相同。

在这种情况下，您可以将AddressBook.msg（包中与节点的接口）定义为Contact类型（独立包中的接口）。你甚至可以将AddressBook.msg定义为Contact类型的数组，如下所示

rosidl_tutorials_msgs/Contact[] address_book

要生成此消息，您需要在package.xml中声明Contact.msg的依赖包rosidl_utorials_msgs

<build_depend>rosidl_tutorials_msgs</build_depend>
<exec_depend>rosidl_tutorials_msgs</exec_depend>

在CMakeLists.txt中

find_package(rosidl_tutorials_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  ${msg_files}
  DEPENDENCIES rosidl_tutorials_msgs
)

还需要在发布者节点中include

#include "rosidl_tutorials_msgs/msg/contact.hpp"

这样修改回调

auto publish_msg = [this]() -> void {
   auto msg = std::make_shared<more_interfaces::msg::AddressBook>();
   {
     rosidl_tutorials_msgs::msg::Contact contact;
     contact.first_name = "John";
     contact.last_name = "Doe";
     contact.phone_number = "1234567890";
     contact.phone_type = message.PHONE_TYPE_MOBILE;
     msg->address_book.push_back(contact);
   }
   {
     rosidl_tutorials_msgs::msg::Contact contact;
     contact.first_name = "Jane";
     contact.last_name = "Doe";
     contact.phone_number = "4254242424";
     contact.phone_type = message.PHONE_TYPE_HOME;
     msg->address_book.push_back(contact);
   }

   std::cout << "Publishing address book:" << std::endl;
   for (auto contact : msg->address_book) {
     std::cout << "First:" << contact.first_name << "  Last:" << contact.last_name <<
       std::endl;
   }

   address_book_publisher_->publish(*msg);
 };

在类中使用参数

create pkg

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake --license Apache-2.0 cpp_parameters --dependencies rclcpp

cpp

~/ros2_ws/src/cpp_parameters/src/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")
  {
    // 参数描述
    auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
    param_desc.description = "This parameter is mine!";

    // 创建名为my_parameter的参数，默认值为world
    this->declare_parameter("my_parameter", "world", param_desc);
    
    // 将timer_初始化为1000ms周期，使得timer_callback函数每秒执行一次
    timer_ = this->create_wall_timer(
      1000ms, std::bind(&MinimalParam::timer_callback, this));
  }

  void timer_callback()
  {
    // 从节点获取参数my_parameter 并储存在my_param中
    std::string my_param = this->get_parameter("my_parameter").as_string();

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

    // set_parameters函数将参数my_parameter设置回默认值 (如果用户在外部改变了参数，这将确保它始终重置为原始值)
    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);
  // 这里MinimalParam类的一个实例被构造
  // rclcpp::spin
  rclcpp::spin(std::make_shared<MinimalParam>());
  rclcpp::shutdown();
  return 0;
}

CMakeLists.txt

# add
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}
)

run

# 终端1
cd ~/ros2_ws
colcon build --packages-select cpp_parameters
source install/setup.bash
ros2 run cpp_parameters minimal_param_node

# 终端2
source install/setup.bash
ros2 param list
ros2 param set /minimal_param_node my_parameter earth # 在控制台修改参数

launch

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,
            # 将my_parameter设置为earth
            parameters=[
                {"my_parameter": "earth"}
            ]
            
        )
    ])

CMakeLists.txt

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

run

cd ~/ros2_ws && colcon build --packages-select cpp_parameters
source install/setup.bash
ros2 launch cpp_parameters cpp_parameters_launch.py

插件

pluginlib是一个C++库，用于从ROS包中加载和卸载插件。插件是从运行时库（即共享对象、动态链接库）加载的动态可加载类。
使用pluginlib，您不必显式地将应用程序与包含类的库链接起来——相反，pluginlib可以在任何时候打开包含导出类的库，而应用程序事先不知道该库或包含类定义的头文件。插件可用于扩展/修改应用程序行为，而不需要应用程序源代码。

基类包

create pkg

cd ~/ros2_ws/src && ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies pluginlib --node-name area_node polygon_base

hpp

~/ros2_ws/src/polygon_base/include/polygon_base/regular_polygon.hpp

#ifndef POLYGON_BASE_REGULAR_POLYGON_HPP
#define POLYGON_BASE_REGULAR_POLYGON_HPP

namespace polygon_base
{
  class RegularPolygon
  {
    public:
      virtual void initialize(double side_length) = 0;
      virtual double area() = 0;
      virtual ~RegularPolygon(){}

    protected:
      RegularPolygon(){}
  };
}  // namespace polygon_base

#endif  // POLYGON_BASE_REGULAR_POLYGON_HPP

上面的代码创建了一个名为RegularPolygon的抽象类。需要注意的一点是initialize方法的存在。使用pluginlib，需要一个没有参数的构造函数，因此如果需要类的任何参数，我们使用initialize方法将它们传递给对象。

CMakeLists.txt

~/ros2_ws/src/polygon_base/CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(polygon_base)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(pluginlib REQUIRED)

add_executable(area_node src/area_node.cpp)
target_include_directories(area_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(area_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  area_node
  "pluginlib"
)

# add
install(
  DIRECTORY include/
  DESTINATION include
)

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

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# add
ament_export_include_directories(
  include
)

ament_package()

插件包

create pkg

cd ~/ros2_ws/src && ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies polygon_base pluginlib --library-name polygon_plugins polygon_plugins

cpp

~/ros2_ws/src/polygon_plugins/src/polygon_plugins.cpp

#include <polygon_base/regular_polygon.hpp>
#include <cmath>

namespace polygon_plugins
{
  class Square : public polygon_base::RegularPolygon
  {
    public:
      void initialize(double side_length) override
      {
        side_length_ = side_length;
      }
      double area() override
      {
        return side_length_ * side_length_;
      }
    protected:
      double side_length_;
  };

  class Triangle : public polygon_base::RegularPolygon
  {
    public:
      void initialize(double side_length) override
      {
        side_length_ = side_length;
      }
      double area() override
      {
        return 0.5 * side_length_ * getHeight();
      }
      double getHeight()
      {
        return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
      }
    protected:
      double side_length_;
  };
}

#include <pluginlib/class_list_macros.hpp>

PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)

Square和Triangle类的实现相当简单：保存边长，并用它来计算面积。唯一特定于pluginlib的部分是最后三行，它调用了一些神奇的宏，将类注册为实际的插件。让我们浏览一下PLUGINLIB_EXPORT_CLASS宏的参数：

在本例中，完全限定的插件类型是polygon_plugins::Square。

在本例中，完全限定的基类型是polygon_base::RegularPolygon。

插件声明xml

上述步骤允许在加载包含库时创建插件实例，但插件加载器仍然需要一种方法来找到该库并知道在该库中引用什么。为此，我们还将创建一个XML文件，该文件以及包清单中的一个特殊导出行，使ROS工具链可以使用有关我们插件的所有必要信息。

~/ros2_ws/src/polygon_plugins/plugins.xml

<library path="polygon_plugins">
  <class type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
    <description>This is a square plugin.</description>
  </class>
  <class type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon">
    <description>This is a triangle plugin.</description>
  </class>
</library>

library tag 给出了包含我们要导出的插件的库的相对路径，在ROS2中这只是库的名称。在ROS1中，它包含前缀lib，有时也包含lib/lib（即lib/libpolygon_plugs）。
class tag 中声明了一个我们想要从库中导出的插件：

type : 插件的完全限定类型。

base_class : 插件的完全限定基类类型。

description : 插件及其功能的描述

CMake插件声明

~/ros2_ws/src/polygon_plugins/CMakeLists.txt

# add
pluginlib_export_plugin_description_file(polygon_base plugins.xml)
# 语法
pluginlib_export_plugin_description_file(基类包 插件声明xml的相对路径)

使用插件

~/ros2_ws/src/polygon_base/src/area_node.cpp

#include <pluginlib/class_loader.hpp>
#include <polygon_base/regular_polygon.hpp>

int main(int argc, char** argv)
{
  // 避免未使用的参数警告
  (void) argc;
  (void) argv;

  pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("polygon_base", "polygon_base::RegularPolygon");

  try
  {
    std::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createSharedInstance("polygon_plugins::Triangle");
    triangle->initialize(10.0);

    std::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createSharedInstance("polygon_plugins::Square");
    square->initialize(10.0);

    printf("Triangle area: %.2f\n", triangle->area());
    printf("Square area: %.2f\n", square->area());
  }
  catch(pluginlib::PluginlibException& ex)
  {
    printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
  }

  return 0;
}

ClassLoader是需要理解的关键类，在class_loader.hpp头文件中定义：

它是用基类模板化的，即polygon_base::RegularPolygon。

第一个参数是基类包名的字符串，即polygon_base。

第二个参数是一个具有插件完全限定基类类型的字符串，即polygon_base::RegularPolygon。

实例化类实例的方法有很多。在这个例子中，我们使用共享指针。我们只需要使用插件类的完全限定类型调用createSharedInstance，在本例中为polygon_plugins::Square。

重要提示：定义此节点的polygon_base包不依赖于polygon_plugins类。插件将动态加载，无需声明任何依赖关系。此外，我们使用硬编码的插件名称实例化类，但您也可以使用参数等动态地这样做。

run

cd ~/ros2_ws
colcon build --packages-select polygon_base polygon_plugins
source install/setup.bash
ros2 run polygon_base area_node
# 输出
Triangle area: 43.30
Square area: 100.00