记录的一些ROS2高级用法

举报
Hermit_Rabbit 发表于 2022/11/29 22:06:40 2022/11/29
【摘要】 0. 简介作者最近发现ROS2目前的功能越来越完善了，其中也新增了很多比较好用的高级玩法，这里作者来一个个向大家展示。这里是小鱼做的ROS2官方文档的中文翻译平台，可以学习和推荐一下 1. 动态参数 1.1 代码编写对于动态参数，大家学过ROS1的话应该都应该有所耳闻吧，ROS1的动态参数的操作还需要dynamic_reconfigure，ROS2中我们直接使用declare_parame...

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添加以下内容

…详情请参照古月居

【版权声明】本文为华为云社区用户原创内容，未经允许不得转载，如需转载请自行联系原作者进行授权。如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱： cloudbbs@huaweicloud.com
  • 点赞
  • 收藏
  • 关注作者

评论（0）

0/1000
抱歉，系统识别当前为高风险访问，暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称，即可参与社区互动！

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

确认 取消

加入云驻计划，成为创作者

  • 华为云周边好礼
  • 免费体验产品
  • 特殊身份标识
  • 线下官方门票
  • 内部专家零距离
  • 与10000+优质创作者共同成长
立即加入