基于Moveit2的人形机器人双臂规划2：对接真实机器人

在第一篇文章中我们完成了Moveit2的基本配置，可以在FakeSystem中测试规划，但是当机器人实际运行时，需要将规划结果发送给机器人。
在基本配置文件中有一个moveit_controllers.yaml，可以看到里面定义了控制器的类型

需要特别注意的一点是，标准的moveit配置文件生成的这个文件开头是

1 2	moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager moveit_simple_controller_manager:

但是用我接下来的控制机械臂方法是不会去写一个对接ROS controller的控制器，所以这个必须去掉，否则会一直报错无法找到控制器

controller_names:
  - left_arm_controller
  - right_arm_controller
  - left_leg_controller
  - right_leg_controller

left_arm_controller:
  type: FollowJointTrajectory
  action_ns: follow_joint_trajectory
  default: true
  joints:
    - left_shoulder_pitch_joint
    - left_shoulder_roll_joint
    - left_shoulder_yaw_joint
    - left_elbow_pitch_joint
    - left_elbow_roll_joint
    - left_wrist_pitch_joint
    - left_wrist_yaw_joint

在另一个配置文件ros2_controllers.yaml中定义了控制器的参数

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    left_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController


    right_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

实际上Moveit2将规划结果发送给控制器就是调用了对应控制器的action服务，我们需要自己来实现这个action的服务端，根据上面的配置文件可以知道这个服务端的类型是FollowJointTrajectory，可以如下定义服务端

using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using GoalHandleFJT = rclcpp_action::ServerGoalHandle<FollowJointTrajectory>;
rclcpp_action::Server<FollowJointTrajectory>::SharedPtr left_action_server_;
rclcpp_action::Server<FollowJointTrajectory>::SharedPtr right_action_server_;

enum class CommandSide
{
    LEFT,
    RIGHT
};

this->left_action_server_ = rclcpp_action::create_server<FollowJointTrajectory>(
    this, "/left_arm_controller/follow_joint_trajectory", std::bind(&H1Example::handle_goal, this, _1, _2),
    std::bind(&H1Example::handle_cancel, this, _1), std::bind(&H1Example::handle_accepted, this, _1, CommandSide::LEFT));
this->right_action_server_ = rclcpp_action::create_server<FollowJointTrajectory>(
    this, "/right_arm_controller/follow_joint_trajectory", std::bind(&H1Example::handle_goal, this, _1, _2),
    std::bind(&H1Example::handle_cancel, this, _1), std::bind(&H1Example::handle_accepted, this, _1, CommandSide::RIGHT));

在服务端handle_accepted中处理服务请求，将规划结果发送给机器人，我们可以通过goal_handle->get_goal()->trajectory获得规划结果，也就是需要执行的一系列电机角度数组，根据机器人的具体SDK来发送给机器人，也可以根据实际情况对这个路径进行插值操作。
在执行以后这个server还需要一个反馈，moveit2根据这个反馈来判断路径执行到哪一步
以下代码是一个简单的示例，实现了接收路径并执行反馈的过程，没有对接具体机器人部分。

(void) side;
const auto goal = goal_handle->get_goal();

trajectory_msgs::msg::JointTrajectory trajectory = goal->trajectory;

auto result = std::make_shared<FollowJointTrajectory::Result>();

size_t num_joints = trajectory.joint_names.size();
size_t num_points = trajectory.points.size();

for (size_t i = 0; i < num_points; ++i)
{
    auto feedback = std::make_shared<FollowJointTrajectory::Feedback>();
    std::vector<double> feedback_positions;

    for (size_t j = 0; j < num_joints; ++j)
    {
        auto motor_index = jointMap.at(trajectory.joint_names[j]);
        mJointStates[motor_index] = trajectory.points[i].positions[j];
        feedback_positions.push_back(trajectory.points[i].positions[j]);
    }
    feedback->header.frame_id = goal->trajectory.header.frame_id;
    feedback->header.stamp = this->now();
    feedback->joint_names = goal->trajectory.joint_names;
    feedback->actual.positions = feedback_positions; // 使用保存的数据
    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(control_dt_ * 1000)));
}

result->error_code = 0;
result->error_string = "";

goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal Succeeded");

如果您喜欢此博客或发现它对您有用，则欢迎对此发表评论。也欢迎您共享此博客，以便更多人可以参与。如果博客中使用的图像侵犯了您的版权，请与作者联系以将其删除。谢谢！

基于Moveit2的人形机器人双臂规划2:对接真实机器人

基于Moveit2的人形机器人双臂规划2：对接真实机器人

特色标签

链友