二自由度机械臂软件系统（二）ros2_control控制算法层

田彼南山

于 2024-08-14 10:08:30 发布

阅读量218

点赞数 9

文章标签： ros2_control

本文链接：https://blog.csdn.net/weixin_41652700/article/details/141182898

版权

资料：
差速ros_control：https://zhuanlan.zhihu.com/p/682574842

1、ros2_control代码

这个控制器是指的控制算法层面的东西，一般来说有两种实现方式，一种是用moveit自己默认指定的，里面会实现插值和控制算法，另一种就是自己实现了，这里是根据差速demo自己修改的。

#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <algorithm>

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"

#include "tf2/LinearMath/Quaternion.h"
#include "motor_test_controller.hpp"

namespace
{
constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
constexpr auto DEFAULT_COMMAND_TOPIC = "/test_controller/cmd_vel";
constexpr auto DEFAULT_ODOMETRY_TOPIC = "/odom";
constexpr auto odom_frame_id = "odom";
constexpr auto base_frame_id = "base_link";
}  // namespace

namespace motor_test_controller
{
using namespace std::chrono_literals;
using controller_interface::interface_configuration_type;
using controller_interface::InterfaceConfiguration;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using lifecycle_msgs::msg::State;

MotorTestController::MotorTestController()
: controller_interface::ControllerInterface() {}

controller_interface::CallbackReturn MotorTestController::on_init()
{
  RCLCPP_INFO(get_node()->get_logger(), "Loading controller...");
  try {
    // Create the parameter listener and get the parameters
    param_listener_ = std::make_shared<ParamListener>(get_node());
    params_ = param_listener_->get_params();
  } catch (const std::exception & e) {
    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
    return controller_interface::CallbackReturn::ERROR;
  }

  return controller_interface::CallbackReturn::SUCCESS;
}


InterfaceConfiguration MotorTestController::command_interface_configuration() const
{
  std::vector<std::string> conf_names;
  for (const auto & joint_name : params_.joint1_names) {
    conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
  }
  for (const auto & joint_name : params_.joint2_names) {
    conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
  }
  return {interface_configuration_type::INDIVIDUAL, conf_names};
}

InterfaceConfiguration MotorTestController::state_interface_configuration() const
{
  std::vector<std::string> conf_names;
  for (const auto & joint_name : params_.joint1_names) {
    conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
  }
  for (const auto & joint_name : params_.joint2_names) {
    conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
  }
  return {interface_configuration_type::INDIVIDUAL, conf_names};
}

controller_interface::CallbackReturn MotorTestController::on_configure(
  const rclcpp_lifecycle::State &)
{
  auto logger = get_node()->get_logger();
  RCLCPP_INFO(logger, "Configuring controller...");

  // update parameters if they have changed
  if (param_listener_->is_old(params_)) {
    params_ = param_listener_->get_params();
    RCLCPP_INFO(logger, "Parameters were updated");
  }

  if (params_.joint1_names.size() != params_.joint2_names.size())
  {
    RCLCPP_ERROR(
      logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different",
      params_.joint1_names.size(), params_.joint2_names.size());
    return controller_interface::CallbackReturn::ERROR;
  }


  if (params_.joint1_names.empty()) {
    RCLCPP_ERROR(logger, "Left wheel names parameters are empty!");
    return controller_interface::CallbackReturn::ERROR;
  }

  if (params_.joint2_names.empty()) {
    RCLCPP_ERROR(logger, "Right wheel names parameters are empty!");
    return controller_interface::CallbackReturn::ERROR;
  }

  if (!reset()) {
    return controller_interface::CallbackReturn::ERROR;
  }
  const Twist empty_twist;
  received_velocity_msg_ptr_.set(std::make_shared<Twist>(empty_twist));
  // Fill last two commands with default constructed commands
  previous_commands_.emplace(empty_twist);
  previous_commands_.emplace(empty_twist);

  // initialize command subscriber   从topic中订阅数据
  velocity_command_subscriber_ =
    get_node()->create_subscription<sensor_msgs::msg::JointState>(
    DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
    [this](const std::shared_ptr<sensor_msgs::msg::JointState> msg) -> void
    {
      if (!subscriber_is_active_) {
        RCLCPP_WARN(
          get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
        return;
      }
      received_velocity_msg_ptr_.set(std::move(msg));

    });


  // initialize odometry publisher and messasge，  定义里程计发布者

  RCLCPP_INFO(logger, "Configure over...");

  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn MotorTestController::on_activate(
  const rclcpp_lifecycle::State &)
{
  const auto left_result =
    configure_side(
    "joint1", params_.joint1_names,
    registered_joint1_handles_);
  const auto right_result =
    configure_side(
    "joint2", params_.joint2_names,
    registered_joint2_handles_);

  if (
    left_result == controller_interface::CallbackReturn::ERROR ||
    right_result == controller_interface::CallbackReturn::ERROR)
  {
    return controller_interface::CallbackReturn::ERROR;
  }

  if (params_.joint1_names.empty() ||
    params_.joint2_names.empty())
  {
    RCLCPP_ERROR(
      get_node()->get_logger(),
      "wheel interfaces are non existent");
    return controller_interface::CallbackReturn::ERROR;
  }

  is_halted = false;
  subscriber_is_active_ = true;


  RCLCPP_INFO(get_node()->get_logger(), "Subscriber and publisher are now active.");
  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn MotorTestController::on_cleanup(
  const rclcpp_lifecycle::State &)
{
  if (!reset()) {
    return controller_interface::CallbackReturn::ERROR;
  }

  received_velocity_msg_ptr_.set(std::make_shared<Twist>());

  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn MotorTestController::on_error(
  const rclcpp_lifecycle::State &)
{
  if (!reset()) {
    return controller_interface::CallbackReturn::ERROR;
  }
  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn MotorTestController::on_deactivate(
  const rclcpp_lifecycle::State &)
{
  subscriber_is_active_ = false;
  if (!is_halted) {
    halt();
    is_halted = true;
  }
  return controller_interface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn MotorTestController::on_shutdown(
  const rclcpp_lifecycle::State &)
{
  return controller_interface::CallbackReturn::SUCCESS;
}


controller_interface::return_type MotorTestController::update(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  auto logger = get_node()->get_logger();
  //if controller unload
  if (get_state().id() == State::PRIMARY_STATE_INACTIVE) {
    if (!is_halted) {
      halt();
      is_halted = true;
    }
    return controller_interface::return_type::OK;
  }

  //update param
  this->params_ = param_listener_->get_params();


  //update cmd
  std::shared_ptr<Twist> last_command_msg;
  received_velocity_msg_ptr_.get(last_command_msg);

  if (last_command_msg == nullptr) {
    RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
    return controller_interface::return_type::ERROR;
  }


  // command may be limited
  Twist command = *last_command_msg;
  // Unit m/s
  if(command.position.size() == 0){
    joint1_command = 0;//std::clamp(command.position[0], -10.0,10.0);
    joint2_command = 1;//std::clamp(command.position[1], -10.0,10.0);
  }else{
      joint1_command = std::clamp(command.position[0], -10.0,10.0);
      joint2_command = std::clamp(command.position[1], -10.0,10.0);
  }
        RCLCPP_WARN(logger, "data is %d",command.position.size());
        // std::cout << "aaaa" << std::endl;

    //获取转速反馈,如果每边有多个车轮，计算平均值
    double left_feedback_mean = 0.0;
    double right_feedback_mean = 0.0;
    const double joint1_feedback = registered_joint1_handles_[0].state.get().get_value();
    const double joint2_feedback =registered_joint2_handles_[0].state.get().get_value();

    left_feedback_mean = joint1_feedback;
    right_feedback_mean = joint2_feedback;
  
  // Set wheels velocities:
    registered_joint1_handles_[0].command.get().set_value(joint1_command);
    registered_joint2_handles_[0].command.get().set_value(joint2_command);
  

  return controller_interface::return_type::OK;
}

controller_interface::CallbackReturn MotorTestController::configure_side(
  const std::string & joint_kind,
  const std::vector<std::string> & joint_names,
  std::vector<JointHandle> & registered_handles)
{
  auto logger = get_node()->get_logger();

  if (joint_names.empty()) {
    RCLCPP_ERROR(logger, "No '%s' motor names specified", joint_kind.c_str());
    return controller_interface::CallbackReturn::ERROR;
  }
  // register handles  注册句柄
  registered_handles.reserve(joint_names.size());  //储备大小
  for (const auto & joint_name : joint_names) {
    //查找指定状态句柄
    const auto state_handle = std::find_if(
      state_interfaces_.cbegin(), state_interfaces_.cend(),
      [&joint_name](const auto & interface)
      {
        return interface.get_prefix_name() == joint_name &&
        interface.get_interface_name() == HW_IF_POSITION;
      });
    if (state_handle == state_interfaces_.cend()) {  //如果没有找到
      RCLCPP_ERROR(logger, "Unable to obtain motor state handle for %s", joint_name.c_str());
      return controller_interface::CallbackReturn::ERROR;
    }

    //查找指定命令句柄
    const auto command_handle = std::find_if(
      command_interfaces_.begin(), command_interfaces_.end(),
      [&joint_name](const auto & interface)
      {
        return interface.get_prefix_name() == joint_name &&
        interface.get_interface_name() == HW_IF_POSITION;
      });

    if (command_handle == command_interfaces_.end()) {
      RCLCPP_ERROR(logger, "Unable to obtain motor command handle for %s", joint_name.c_str());
      return controller_interface::CallbackReturn::ERROR;
    }

    //添加到容器中
    registered_handles.emplace_back(
      JointHandle{std::ref(*state_handle), std::ref(
          *command_handle)});
  }

  return controller_interface::CallbackReturn::SUCCESS;
}

bool MotorTestController::reset()
{
  // release the old queue
  std::queue<Twist> empty;
  std::swap(previous_commands_, empty);

  registered_joint1_handles_.clear();
  registered_joint2_handles_.clear();

  subscriber_is_active_ = false;
  velocity_command_subscriber_.reset();
  received_velocity_msg_ptr_.set(nullptr);

  is_halted = false;
  return true;
}

void MotorTestController::halt()
{
  //make wheels stop
  const auto halt_wheels = [](auto & wheel_handles)
    {
      for (const auto & wheel_handle : wheel_handles) {
        wheel_handle.command.get().set_value(0.0);
      }
    };

  halt_wheels(registered_joint1_handles_);
  halt_wheels(registered_joint2_handles_);
}

} // namespace diff_test_controller

#include "class_loader/register_macro.hpp"

CLASS_LOADER_REGISTER_CLASS(
  motor_test_controller::MotorTestController, controller_interface::ControllerInterface)

2、连接action

这个代码写完后，实际上无法实现效果，原因在于moveit规划的结果没有和ros_control这边的代码连接起来，我这里moveit使用的是rviz插件的形式（共有c++、python、rviz插件三种形式），是用一个action和其他部分连接的。
我上面ros2_control的代码是通过订阅和发布topic实现的，所以要有一个程序作为一个转接点，连接action然后操作topic。
当然，这种方式其实是不合适的，多此一举了，像官方库一样直接在ros2_control中实现最好。这里还是把这部分代码给出，是用python写的。

#!/usr/bin/env python

import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from control_msgs.action import FollowJointTrajectory
from std_msgs.msg import Float32MultiArray
from sensor_msgs.msg import JointState 
import time

#这个地方是连接movit和ros_control连接的关键。
# movit的配置文件中指定了一个服务器，也就是这里的action服务器，叫做“follow_joint_trajectory”。movit会向这个服务器一次性放出所有的数据。
# 在拿到数据后，在回调函数中根据时间一次发送到指定的cmd_vel中，控制器的节点会读取这个topic的数据，至此已经进入ros_control的部分了。
# 似乎不使用自定义控制器的情况下，并不需要设置这个过程，movit官方的控制器是如何实现这个效果呢？
class TrajectoryServer(Node):

    def __init__(self):
        super().__init__('trajectory_server')
        self._action_server = ActionServer(
            self,
            FollowJointTrajectory,
            'test_controller/follow_joint_trajectory',
            self.execute_callback)

        self.publisher_ = self.create_publisher(JointState, '/test_controller/cmd_vel', 10)

    def execute_callback(self, goal_handle):
        print("22222")
        self.get_logger().info('Executing goal...')

        # 解析目标关节名称和位置
        goal = goal_handle.request
        joint_names = goal.trajectory.joint_names
        points = goal.trajectory.points

        if not joint_names or not points:
            self.get_logger().error('Invalid goal')
            goal_handle.abort()
            return FollowJointTrajectory.Result()
            
        print("======================")
        print(goal)
    # 发布所有轨迹点的关节位置
        for point in points:
            joint_positions = JointState()
            joint_positions.header.stamp = self.get_clock().now().to_msg()
            joint_positions.name = joint_names
            joint_positions.position = point.positions

            # 发布关节状态
            self.publisher_.publish(joint_positions)
            self.get_logger().info(f'Published joint positions: {joint_positions.position}')

            # 模拟执行时间
            time.sleep(1)

#            rclpy.sleep(point.time_from_start.sec + point.time_from_start.nanosec / 1e9)

        self.get_logger().info('Goal successfully executed!')
        goal_handle.succeed()

        result = FollowJointTrajectory.Result()
        return result


def main(args=None):
    print("start1")
    rclpy.init(args=args)
    print("start2")
    trajectory_server = TrajectoryServer()
    print("start3")
    rclpy.spin(trajectory_server)
    print("start4")    
if __name__ == '__main__':
    main()