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

最新推荐文章于 2025-03-06 23:30:00 发布

田彼南山

最新推荐文章于 2025-03-06 23:30:00 发布

阅读量678

点赞数 10

文章标签： 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(

最低0.47元/天解锁文章