PIBOT移植ROS2记录(5)-参数及动态设置

1. pid参数调节的topic

1.1 移植

pid的输入输出作为topic分别输出，这个只需要创建一个int类型的pub即可

void BaseDriver::init_pid_debug() {
  if (config_.out_pid_debug_enable) {
    const char* input_topic_name[MAX_MOTOR_COUNT] = {"motor1_input", "motor2_input", "motor3_input", "motor4_input"};
    const char* output_topic_name[MAX_MOTOR_COUNT] = {"motor1_output", "motor2_output", "motor3_output", "motor4_output"};
    for (size_t i = 0; i < MAX_MOTOR_COUNT; i++) {
      pid_input_pub_[i] = this->create_publisher<std_msgs::msg::Int32>(input_topic_name[i], 10);
      pid_output_pub_[i] = this->create_publisher<std_msgs::msg::Int32>(output_topic_name[i], 10);
    }
  }
}

void BaseDriver::update_pid_debug() {
  if (config_.out_pid_debug_enable) {
    frame_->interact(ID_GET_PID_DATA);

    for (size_t i = 0; i < MAX_MOTOR_COUNT; i++) {
      pid_input_msg_[i].data = DataHolder::get()->pid_data.input[i];
      pid_output_msg_[i].data = DataHolder::get()->pid_data.output[i];

      pid_input_pub_[i]->publish(pid_input_msg_[i]);
      pid_output_pub_[i]->publish(pid_output_msg_[i]);
    }
  }
}

1.2 运行测试

在launch文件中打开out_pid_debug_enable后启动

...
{"out_pid_debug_enable": True},

ros2 launch  pibot_bringup bringup_launch.py

查看topic列表

➜  ros2 topic list
/cmd_vel
/motor1_input
/motor1_output
/motor2_input
/motor2_output
/motor3_input
/motor3_output
/motor4_input
/motor4_output
/odom
/parameter_events
/rosout
/tf

1.3 rqt plot

安装rqt-plot

sudo apt-get install ros-galactic-rqt-plot

安装后rqt未发现plot的插件，原因暂时位置

2. 动态参数

2.1 移植

不同于ROS1中的dynamic_reconfigure,ROS2中我们直接使用declare_parameter声明参数，可以在rqt-reconfigure中动态配置，之前我们在声明时新增了一个只读的约束

这里我们还可以新增其他约束以限制参数设置的范围

    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);

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

  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callback_handle_;

    callback_handle_ = node->add_on_set_parameters_callback(std::bind(&BaseDriverConfig::SetParametersCallback,
                                                                      this,
                                                                      std::placeholders::_1,
                                                                      node,
                                                                      rp));

回调中我们需要循环所有参数列表，并且判断参数名称设置相应的变量

rcl_interfaces::msg::SetParametersResult BaseDriverConfig::SetParametersCallback(const std::vector<rclcpp::Parameter>& parameters, rclcpp::Node* node, Robot_parameter* rp) {
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  result.reason = "success";
  for (auto& param : parameters) {
    RCLCPP_INFO(node->get_logger(), "param %s update", param.get_name().c_str());
    if (param.get_name() == "motor1_exchange_flag") {
      RCLCPP_INFO(node->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(node->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;
}

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

2.2 运行测试

ros2 launch  pibot_bringup bringup_launch.py

ros2 run rqt_reconfigure rqt_reconfigure

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

同时暂时无法配置枚举类型，对于之前的model_type，在ui中无法下拉列表选择，略显麻烦

本文代码https://gitee.com/pibot/pibot_bringup/tree/25ed34acda8a6e850a1c96fcee8d3a762374a135