0. 简介

作者最近发现ROS2目前的功能越来越完善了，其中也新增了很多比较好用的高级玩法，这里作者来一个个向大家展示。这里是小鱼做的ROS2官方文档的中文翻译平台，可以学习和推荐一下

1. 动态参数

1.1 代码编写

对于动态参数，大家学过ROS1的话应该都应该有所耳闻吧，ROS1的动态参数的操作还需要dynamic_reconfigure，ROS2中我们直接使用declare_parameter声明参数，可以在rqt-reconfigure中动态配置，之前我们在声明时新增了一个只读的约束。我们这里参考gitee中的代码。

首先和ROS1一样设置cfg文件：

#!/usr/bin/env python

PACKAGE = 'pibot_bringup'

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t

gen = ParameterGenerator()

model_type_enum = gen.enum([ gen.const("2WD_Diff", int_t, 1, "Two-wheel Diff Drive"),
                           gen.const("4WD_Diff",   int_t, 2, "Four-wheel Diff Drive"),
                           gen.const("3WD_Omni",  int_t, 101, "Three-wheel Omni Drvie"),
                        #    gen.const("4WD_Omni",  int_t, 102, "Four-wheel Omni Drvie"),
                           gen.const("4WD_Mecanum",  int_t, 201, "Four-wheel Mecanum Drvie"),
                           gen.const("UNKNOWN",  int_t, 255, "unknown model")],
                           "pibot dirver list")

gen.add("model_type", int_t, 0, "model type", 1, 1, 255, edit_method = model_type_enum)

gen.add("motor1_exchange_flag",   bool_t,   0, "exchange motor1 wire",  False)
gen.add("motor2_exchange_flag",   bool_t,   0, "exchange motor2 wire",  False)
gen.add("motor3_exchange_flag",   bool_t,   0, "exchange motor3 wire",  False)
gen.add("motor4_exchange_flag",   bool_t,   0, "exchange motor4 wire",  False)

gen.add("encoder1_exchange_flag",   bool_t,   0, "exchange encoder1 wire",  False)
gen.add("encoder2_exchange_flag",   bool_t,   0, "exchange encoder2 wire",  False)
gen.add("encoder3_exchange_flag",   bool_t,   0, "exchange encoder3 wire",  False)
gen.add("encoder4_exchange_flag",   bool_t,   0, "exchange encoder4 wire",  False)

gen.add("wheel_diameter", int_t, 0, "The diameter of the wheel", 30, 10, 500)
gen.add("wheel_track", int_t, 0, "The track of the wheel", 300, 50, 1000)
gen.add("encoder_resolution", int_t, 0, "The resolution of the encoder", 1560, 1 , 32000)
gen.add("motor_ratio", int_t, 0, "The ratio of the motor", 1, 1, 1000)

gen.add("do_pid_interval", int_t, 0, "The interval of do pid", 10, 1, 80)
gen.add("kp", int_t, 0, "Kp value", 20, 0, 10000)
gen.add("ki", int_t, 0, "Ki value", 20, 0, 32000)
gen.add("kd", int_t, 0, "Kd value", 20, 0, 1000)
gen.add("ko", int_t, 0, "Ko value", 20, 0, 1000)

gen.add("cmd_last_time", int_t, 0, "cmd_last_time value", 200, 0, 1000)

gen.add("max_v_liner_x", int_t, 0, "liner x", 60, 0, 500)
gen.add("max_v_liner_y", int_t, 0, "liner y", 0, 0, 500)
gen.add("max_v_angular_z", int_t, 0, "angular z", 120, 0, 2000)

imu_type_enum = gen.enum([ gen.const("GY65", int_t, 49, "mpu6050"),
                           gen.const("GY85",   int_t, 69, "adxl345_itg3200_hmc5883l"),
                           gen.const("GY87",  int_t, 71, "mpu6050_hmc5883l"),
                           gen.const("nonimu",  int_t, 255, "disable imu")],
                           "imu type list")

gen.add("imu_type", int_t, 0, "imu type", 69, 1, 255, edit_method = imu_type_enum)

exit(gen.generate(PACKAGE, "pibot_bringup", "pibot_driver"))

同时还可以新增其他约束以限制参数设置的范围

    rcl_interfaces::msg::ParameterDescriptor descriptor;
    descriptor.description = "";
    descriptor.name = "name";
    descriptor.integer_range.resize(1);
    descriptor.integer_range[0].from_value = 10;
    descriptor.integer_range[0].to_value = 1000;
    descriptor.integer_range[0].step = 1;
    node->declare_parameter<uint16_t>("wheel_diameter", rp->wheel_diameter, descriptor);

同时我们设置一个参数修改的回调通知，来根据设置的参数下发至下位机

#include <vector>
#include <rclcpp/rclcpp.hpp>

struct Robot_parameter {
  union {
    char buff[64];
    struct
    {
      unsigned short wheel_diameter;      //轮子直径  mm
      unsigned short wheel_track;         //差分：轮距， 三全向轮：直径，四全向：前后轮距+左右轮距 mm
      unsigned short encoder_resolution;  //编码器分辨率
      unsigned char do_pid_interval;      // pid间隔 (ms)
      unsigned short kp;
      unsigned short ki;
      unsigned short kd;
      unsigned short ko;                       // pid参数比例
      unsigned short cmd_last_time;            //命令持久时间ms 超过该时间会自动停止运动
      unsigned short max_v_liner_x;            // 最大x线速度
      unsigned short max_v_liner_y;            // 最大y线速度
      unsigned short max_v_angular_z;          // 最大角速度
      unsigned char imu_type;                  // imu类型 参见IMU_TYPE
      unsigned short motor_ratio;              // 电机减速比
      unsigned char model_type;                // 运动模型类型 参见MODEL_TYPE
      unsigned char motor_nonexchange_flag;    // 电机标志参数        1 正接      0 反接(相当于电机线交换)
      unsigned char encoder_nonexchange_flag;  // 编码器标志参数      1 正接      0 反接(相当于编码器ab相交换)
    };
  };
};

class MyNode : public rclcpp::Node
{
public:
  MyNode()
  {
     Robot_parameter* rp; 
    // Declare parameters first
    descriptor.description = "";
    descriptor.name = "name";
    descriptor.integer_range.resize(1);
    descriptor.integer_range[0].from_value = 10;
    descriptor.integer_range[0].to_value = 1000;
    descriptor.integer_range[0].step = 1;
    node->declare_parameter<uint16_t>("wheel_diameter", rp->wheel_diameter, descriptor);
    // Then create callback
    callback_handle_= this->add_on_set_parameters_callback(
      std::bind(&MyNode::SetParametersCallback, this, std::placeholders::_1,rp));
  }

private:
  // This will get called whenever a parameter gets updated
  rcl_interfaces::msg::SetParametersResult SetParametersCallback(
    const std::vector<rclcpp::Parameter> & parameters, Robot_parameter* rp)
  {
      rcl_interfaces::msg::SetParametersResult result;
      result.successful = true;
      result.reason = "success";
      for (auto& param : parameters) {
        RCLCPP_INFO(this->get_logger(), "param %s update", param.get_name().c_str());
        if (param.get_name() == "motor1_exchange_flag") {
          RCLCPP_INFO(this->get_logger(), "++param %d", rp->motor_nonexchange_flag);
          std::bitset<8> val(rp->motor_nonexchange_flag);
          val[0] = !param.as_bool();
          rp->motor_nonexchange_flag = val.to_ulong();
          RCLCPP_INFO(this->get_logger(), "--param %d", rp->motor_nonexchange_flag);
        } else if (param.get_name() == "motor2_exchange_flag") {
          std::bitset<8> val(rp->motor_nonexchange_flag);
          val[1] = !param.as_bool();
          rp->motor_nonexchange_flag = val.to_ulong();
        } else if (param.get_name() == "motor3_exchange_flag") {
          std::bitset<8> val(rp->motor_nonexchange_flag);
          val[2] = !param.as_bool();
          rp->motor_nonexchange_flag = val.to_ulong();
        } else if (param.get_name() == "motor4_exchange_flag") {
          std::bitset<8> val(rp->motor_nonexchange_flag);
          val[3] = !param.as_bool();
          rp->motor_nonexchange_flag = val.to_ulong();
        } else if (param.get_name() == "encoder1_exchange_flag") {
          std::bitset<8> val(rp->encoder_nonexchange_flag);
          val[0] = !param.as_bool();
          rp->encoder_nonexchange_flag = val.to_ulong();
        } else if (param.get_name() == "encoder2_exchange_flag") {
          std::bitset<8> val(rp->encoder_nonexchange_flag);
          val[1] = !param.as_bool();
          rp->encoder_nonexchange_flag = val.to_ulong();
        } else if (param.get_name() == "encoder3_exchange_flag") {
          std::bitset<8> val(rp->encoder_nonexchange_flag);
          val[2] = !param.as_bool();
          rp->encoder_nonexchange_flag = val.to_ulong();
        } else if (param.get_name() == "encoder4_exchange_flag") {
          std::bitset<8> val(rp->encoder_nonexchange_flag);
          val[3] = !param.as_bool();
          rp->encoder_nonexchange_flag = val.to_ulong();
        } else if (param.get_name() == "model_type") {
          rp->model_type = param.as_int();
        } else if (param.get_name() == "wheel_diameter") {
          rp->wheel_diameter = param.as_int();
        } else if (param.get_name() == "wheel_track") {
          rp->wheel_track = param.as_int();
        } else if (param.get_name() == "encoder_resolution") {
          rp->encoder_resolution = param.as_int();
        } else if (param.get_name() == "do_pid_interval") {
          rp->do_pid_interval = param.as_int();
        } else if (param.get_name() == "kp") {
          rp->kp = param.as_int();
        } else if (param.get_name() == "ki") {
          rp->ki = param.as_int();
        } else if (param.get_name() == "kd") {
          rp->kd = param.as_int();
        } else if (param.get_name() == "ko") {
          rp->ko = param.as_int();
        } else if (param.get_name() == "cmd_last_time") {
          rp->cmd_last_time = param.as_int();
        } else if (param.get_name() == "max_v_liner_x") {
          rp->max_v_liner_x = param.as_int();
        } else if (param.get_name() == "max_v_liner_y") {
          rp->max_v_liner_y = param.as_int();
        } else if (param.get_name() == "max_v_angular_z") {
          rp->max_v_angular_z = param.as_int();
        } else if (param.get_name() == "imu_type") {
          rp->imu_type = param.as_int();
        } else if (param.get_name() == "motor_ratio") {
          rp->motor_ratio = param.as_int();
        }
      }

      DataHolder::dump_params(rp);

      param_update_flag_ = true;
      return result;
  }

  // Need to hold a pointer to the callback
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handle_;
  rcl_interfaces::msg::ParameterDescriptor descriptor;
};

该回调被调用会设置一个update_flag的变量，主线程会处理执行一次参数同步操作

1.2 运行测试

ros2 launch  pibot_bringup bringup_launch.py
ros2 run rqt_reconfigure rqt_reconfigure

不同于ROS1的dynamic_reconfigure, 显示的参数不会按照我们声明的顺序，而是按照字母排序，会显得有点杂乱。

在这里插入图片描述

2. 动态插件

ROS2的动态插件和ROS1其实差别不太大，但是写法还是有所差距的，这里也举个栗子，这里参照了官网的程序

安装pluginlib

sudo apt-get install ros-foxy-pluginlib

创建一个新包

cd dev_ws/src
ros2 pkg create --build-type ament_cmake polygon_base --dependencies pluginlib --node-name area_node

编辑文件dev_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

在dev_ws/src/polygon_base/CMakeLists.txt添加。在ament_target_dependencies命令后添加以下行

install(
  DIRECTORY include/
  DESTINATION include
)

在命令之前添加此ament_package命令

ament_export_include_directories(
  include
)

创建插件包

cd dev_ws/src
ros2 pkg create --build-type ament_cmake polygon_plugins --dependencies polygon_base pluginlib --library-name polygon_plugins

编辑文件 dev_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)

创建XML文件，在dev_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>

在dev_ws/src/polygon_plugins/CMakeLists.txt添加以下内容

add_library(polygon_plugins src/polygon_plugins.cpp)
target_include_directories(polygon_plugins PUBLIC
<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
<INSTALL_INTERFACE:include>)
ament_target_dependencies(
  polygon_plugins
  polygon_base
  pluginlib
)
pluginlib_export_plugin_description_file(polygon_base plugins.xml)

install(
  TARGETS polygon_plugins
  EXPORT export_${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

在ament_package命令之前，添加

ament_export_libraries(
  polygon_plugins
)
ament_export_targets(
  export_${PROJECT_NAME}
)

使用插件。在dev_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)
{
  // To avoid unused parameter warnings
  (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;
}

构建和运行

cd dev_ws
colcon build --packages-select polygon_base polygon_plugins
. install/setup.bash

运行节点

ros2 run polygon_base area_node

运行结果

Triangle area: 43.30
Square area: 100.00

3. Component多节点生成

这部分在之前的一篇文章中就已经提及了，这里就不详细展开说了：文章链接：https://hermit.blog.csdn.net/article/details/125492694

4. TF2坐标系变换

4.1 发布TF

#include <tf2_ros/transform_broadcaster.h>
std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster;

broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(nodePtr);

geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node->now();
transform.header.frame_id = "odom";
transform.child_frame_id = "base_link";

// Fill in transform.transform.translation
// Fill in transform.transform.rotation

broadcaster->sendTransform(transform);

4.2 监听TF

#include "tf2_ros/transform_listener.h"

std::shared_ptr<tf2_ros::Buffer> tf_buffer;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;

rclcpp::Node node("name_of_node");

tf_buffer.reset(new tf2_ros::Buffer(node.get_clock()));
tf_listener.reset(new tf2_ros::TransformListener(*tf_buffer_));

4.3 TF变换

TF2 可以通过提供实现的包进行扩展。tf2_geometry_msgs程序包为 msgs 提供这些。下面的示例使用 geometry_msg::msg::PointStamed，但这应该适用于 msgs 中的任何数据类型：

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion quat_tf;
geometry_msgs::Quaternion quat_msg = ...;

tf2::convert(quat_msg , quat_tf);
// or
tf2::fromMsg(quat_msg, quat_tf);
// or for the other conversion direction
quat_msg = tf2::toMsg(quat_tf);

convert() 即 将 msg 类型 转换 为 tf 类型
fromMsg() 即 将 msg 类型 转换 为 tf 类型
toMsg() 即 将 tf 类型 转换 为 msg 类型

transform 函数还可以接受超时。然后它将等待转换可用的时间达到这个时间量:

tf_buffer->transform(in, out, "target_frame", tf2::durationFromSec(1.0));

4.4 获取最新TF

常见的工作方式是获得“最新”转换。在 ros2中，这可以使用 tf2::TimePointZero 来实现，但是需要使用 lookupTransform 然后调用 doTransform (基本上就是在内部进行转换) :

geometry_msgs::msg::PointStamped in, out;

geometry_msgs::msg::TransformStamped transform =
   tf_buffer->lookupTransform("target_frame",
                              in.header.frame_id,
                              tf2::TimePointZero);

tf2::doTransform(in, out, transform);

5. 懒订阅

ROS2 还没有订户连接回叫。此代码创建一个函数，定期调用该函数来检查我们是否需要启动或停止订阅者：

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>

class LazyPublisherEx : rclcpp::Node
{
public:
  LazyPublisherEx(const rclcpp::NodeOptions & options) :
    Node("lazy_ex", options)
  {
    // Setup timer
    timer = this->create_wall_timer(
      std::chrono::seconds(1),
      std::bind(&LazyPublisher::periodic, this));
  }

private:
  void periodic()
  {
    if (pub_.get_subscription_count() > 0)
    {
        // We have a subscriber, do any setup required
    }
    else
    {
        // Subscriber has disconnected, do any shutdown
    }
  }

  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
};

6. colcon 编译

6.1 Build

用于在工作区构建 ROS2 包。

生成所有包：

colcon build

要避免在调整 Python 脚本、配置文件和启动文件时重新构建：

colcon build --symlink-install

要修复大多数构建问题，特别是在向工作区添加或删除包或安装新的 RMW 实现的情况下，请清除 CMake 缓存。

colcon build --cmake-clean-cache

6.2 CMake 参数

colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo

6.3 测试

要测试并将结果显示在屏幕上，请执行以下操作：

colcon test
colcon test-result --verbose

构建/测试单个包：

colcon <verb> --packages-select <package-name>

6.4格式化

将输出显示到屏幕上：

colcon <verb> --event-handlers console_direct+

7. 参考链接

https://blog.csdn.net/baimei4833953/article/details/124577887

https://fishros.com/d2lros2foxy/#/

https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Pluginlib.html

https://blog.csdn.net/qq_32761549/article/details/109110598