ROS2中用MoveIt2控制自己的舵机机械手（1）

前言

此教程是基于上一篇的，假如还没看过上一篇，请按上一篇【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】说明的步骤操作一遍，然后再看这一篇。
目前我们先完成上位机的接口，以后再与下位机的舵机通讯。

1.ROS2节点架构

总共有三个“节点”需要启动：myActionRobot、robot_state_publisher、 moveIt2+rviz2.
需要注意的是，我这里没有用到ros2_control模块，而是用了自己编写的myActionRobot来代替ros2_control来接收轨迹、发布姿态。后面实力允许了，再研究一下怎么用ros2_control.

myActionRobot是自己编写的Node节点，负责提供Action接口用于接收moveit产生的轨迹信息，以及通过joint_states话题发布机械手的关节信息；由后面的【MyActionRobot.h、MyActionRobot.cpp】创建。

robot_state_publisher也是一个Node节点，可以根据urdf文件生成joint_states、tf、robot_description（robot_description的作用）话题。其中它订阅了joint_states话题，用于监听机械手的关节的状态信息，然后利用该信息“计算”tf信息，并通过tf/tf_static话题将坐标信息发布出去，让有需要的节点使用。由后面的【my_robot.launch.py】创建。

moveIt2+rviz2是一堆节点的集合，我这里为了简单，就把他们汇总在一起了。他们主要负责机械手的显示、控制、路径规划等操作。由后面的【my_moveit_rviz.launch.py】创建。

2.增加的内容

在完成上一篇教程后，这次要增加两部分内容：
a、首先在 myRobot/src/mybot/launch 文件夹下增加两个文件my_robot.launch.py、my_moveit_rviz.launch.py
b、然后在自己喜欢的、创建ros工程的地方增加ros的节点文件，我这里用的是Qt
具体的文件内容如下：
my_robot.launch.py

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

import xacro

def generate_launch_description():
    robot_name_in_model = 'six_arm'
    package_name = 'mybot_description'
    urdf_name = "six_arm.urdf"

    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    # 因为 urdf文件中有一句 $(find mybot) 需要用xacro进行编译一下才行;否则就是非法文件了
    xacro_file = urdf_model_path
    doc = xacro.parse(open(xacro_file))
    xacro.process_doc(doc)
    params = {'robot_description': doc.toxml()}

    # 启动了robot_state_publisher节点后，该节点会发布 robot_description 话题，话题内容是模型文件urdf的内容
    # 并且会订阅 /joint_states 话题，获取关节的数据，然后发布tf和tf_static话题； 这三个话题在该节点启动时就会出现
    # 这些节点、话题的名称可不可以自定义？
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        respawn=True,
        # parameters=[params, ],
        parameters=[params, {"publish_frequency": 15.0}],
        output='screen'
    )

    # 对于自己写的机械手控制，需要自己实现action、joint_states的发布；同时，也就不需要使用ros2_control了，因为它是用来实现前面提到的那两个的
    # 因此，这边只需要启动一个robot_state_publisher即可

    ld = LaunchDescription()

    ld.add_action(node_robot_state_publisher)

    return ld

my_moveit_rviz.launch.py
和前一篇的教程相比，只是去掉了use_sim_time这个参数。

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_moveit_rviz_launch

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
)
from moveit_configs_utils.launch_utils import (
    add_debuggable_node,
    DeclareBooleanLaunchArg,
)
from launch.substitutions import LaunchConfiguration
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("six_arm", package_name="mybot").to_moveit_configs()

    ld = LaunchDescription()

    # 启动move_group
    my_generate_move_group_launch(ld, moveit_config)
    # 启动rviz
    my_generate_moveit_rviz_launch(ld, moveit_config)

    return ld


def my_generate_move_group_launch(ld, moveit_config):

    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
    ld.add_action(
        DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
    )
    ld.add_action(
        DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
    )
    # load non-default MoveGroup capabilities (space separated)
    ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
    # inhibit these default MoveGroup capabilities (space separated)
    ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))

    # do not copy dynamics information from /joint_states to internal robot monitoring
    # default to false, because almost nothing in move_group relies on this information
    ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False))

    should_publish = LaunchConfiguration("publish_monitored_planning_scene")

    move_group_configuration = {
        "publish_robot_description_semantic": True,
        "allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
        # Note: Wrapping the following values is necessary so that the parameter value can be the empty string
        "capabilities": ParameterValue(
            LaunchConfiguration("capabilities"), value_type=str
        ),
        "disable_capabilities": ParameterValue(
            LaunchConfiguration("disable_capabilities"), value_type=str
        ),
        # Publish the planning scene of the physical robot so that rviz plugin can know actual robot
        "publish_planning_scene": should_publish,
        "publish_geometry_updates": should_publish,
        "publish_state_updates": should_publish,
        "publish_transforms_updates": should_publish,
        "monitor_dynamics": False,
    }

    move_group_params = [
        moveit_config.to_dict(),
        move_group_configuration,
    ]
    # move_group_params.append({"use_sim_time": True})

    add_debuggable_node(
        ld,
        package="moveit_ros_move_group",
        executable="move_group",
        commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"),
        output="screen",
        parameters=move_group_params,
        extra_debug_args=["--debug"],
        # Set the display variable, in case OpenGL code is used internally
        additional_env={"DISPLAY": ":0"},
    )
    return ld

def my_generate_moveit_rviz_launch(ld, moveit_config):
    """Launch file for rviz"""

    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
    ld.add_action(
        DeclareLaunchArgument(
            "rviz_config",
            default_value=str(moveit_config.package_path / "config/moveit.rviz"),
        )
    )

    rviz_parameters = [
        moveit_config.planning_pipelines,
        moveit_config.robot_description_kinematics,
    ]
    # rviz_parameters.append({"use_sim_time": True})

    add_debuggable_node(
        ld,
        package="rviz2",
        executable="rviz2",
        output="log",
        respawn=False,
        arguments=["-d", LaunchConfiguration("rviz_config")],
        parameters=rviz_parameters,
    )

    return ld

myactionrobot.h

#ifndef MYACTIONROBOT_H
#define MYACTIONROBOT_H

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "sensor_msgs/msg/joint_state.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"

class MyActionRobot : public rclcpp::Node
{
public:
    using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
    using GoalHandleFJT = rclcpp_action::ServerGoalHandle<FollowJointTrajectory>;


    explicit MyActionRobot(std::string name);

private:
    rclcpp_action::Server<FollowJointTrajectory>::SharedPtr action_server_;

    // 声明话题发布者
    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_publisher;

    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const FollowJointTrajectory::Goal> goal);
    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleFJT> goal_handle);
    void execute_move(const std::shared_ptr<GoalHandleFJT> goal_handle);
    void handle_accepted(const std::shared_ptr<GoalHandleFJT> goal_handle);

    // 定时发布关节状态
    void publish_joint_states();
    // 假装真正的关节状态
    std::vector<double> mJointStates;
};

#endif // MYACTIONROBOT_H

myactionrobot.cpp

#include "myactionrobot.h"

#include<QDebug>

MyActionRobot::MyActionRobot(std::string name) : Node(name)
{
    //    control_msgs::FollowJointTrajectoryAction

    using namespace std::placeholders;  // NOLINT

    this->action_server_ = rclcpp_action::create_server<FollowJointTrajectory>(
                this, "/my_group_controller/follow_joint_trajectory",
                std::bind(&MyActionRobot::handle_goal, this, _1, _2),
                std::bind(&MyActionRobot::handle_cancel, this, _1),
                std::bind(&MyActionRobot::handle_accepted, this, _1));

    qDebug() << "the action server:" << this->action_server_.get();

    rclcpp::QoS qos(10);
//    qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
//    qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
    joint_states_publisher = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", qos);

    mJointStates = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
    std::thread{std::bind(&MyActionRobot::publish_joint_states, this)}
    .detach();
}

rclcpp_action::GoalResponse MyActionRobot::handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const FollowJointTrajectory::Goal> goal)
{
    qDebug() << "---handle goal:" << goal->trajectory.joint_names.size();
    qDebug() << goal->trajectory.header.frame_id.c_str() << goal->trajectory.header.stamp.sec << goal->trajectory.header.stamp.nanosec;

    int pointSize = goal->trajectory.points.size();
    if(pointSize > 0)
    {
        for(int i = 0; i < pointSize; i++)
        {
            auto point = goal->trajectory.points.at(i);
            qDebug() << point.positions
                     << "[sec:" << point.time_from_start.sec << ", nanosec:" << point.time_from_start.nanosec << "]";
        }
    }

   (void)uuid;

    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse MyActionRobot::handle_cancel(const std::shared_ptr<GoalHandleFJT> goal_handle)
{
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");

    return rclcpp_action::CancelResponse::ACCEPT;
}

void MyActionRobot::execute_move(const std::shared_ptr<GoalHandleFJT> goal_handle)
{
    const auto goal = goal_handle->get_goal();

    RCLCPP_INFO(this->get_logger(), "开始执行移动 %f 。。。", goal->trajectory.points.size());

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

    auto trjPointsSize = goal->trajectory.points.size();
    int trjIdx = 0;

//    rclcpp::Rate rate = rclcpp::Rate(1); // 定时进行操作
    while (rclcpp::ok() && trjIdx < trjPointsSize) {
        auto point =  goal->trajectory.points.at(trjIdx);

        // 按照轨迹指定的时间，进行等待
        if(trjIdx > 0)
        {
            auto last_time = goal->trajectory.points.at(trjIdx - 1).time_from_start;
            auto current_time = goal->trajectory.points.at(trjIdx).time_from_start;

            rclcpp::Time time1(last_time.sec, last_time.nanosec);
            rclcpp::Time time2(current_time.sec, current_time.nanosec);
            auto time_to_sleep = time2 - time1;

            int64_t duration = ((int64_t)time_to_sleep.seconds()) * 1e9 + time_to_sleep.nanoseconds();
            std::chrono::nanoseconds ns(duration);

            qDebug() << "sleep for:" << ns.count();

            rclcpp::sleep_for(ns);
        }

        auto feedback = std::make_shared<FollowJointTrajectory::Feedback>();
        feedback->header.frame_id = goal->trajectory.header.frame_id;
        feedback->header.stamp = goal->trajectory.header.stamp;
        feedback->joint_names = goal->trajectory.joint_names;
//        feedback->actual = goal->trajectory.joint_names;
//        feedback->desired = ;
//        feedback->error = "";

        for(int i = 0; i < point.positions.size(); i++)
        {
            mJointStates[i] = point.positions.at(i);
        }

        goal_handle->publish_feedback(feedback);
        /*检测任务是否被取消*/
        if (goal_handle->is_canceling()) {
            result->error_code = -1;
            result->error_string = "has cancel";
            goal_handle->canceled(result);
            RCLCPP_INFO(this->get_logger(), "Goal Canceled");
            return;
        }
        RCLCPP_INFO(this->get_logger(), "Publish Feedback"); /*Publish feedback*/

        trjIdx++;

        //        rate.sleep();
    }

//    result->pose = robot.get_current_pose();
    result->error_code = 0;
    result->error_string = "";

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

void MyActionRobot::handle_accepted(const std::shared_ptr<GoalHandleFJT> goal_handle) {
    using std::placeholders::_1;

    std::thread{std::bind(&MyActionRobot::execute_move, this, _1), goal_handle}
    .detach();
}

void MyActionRobot::publish_joint_states()
{
    rclcpp::Rate rate = rclcpp::Rate(15); // 定时进行操作
    while (rclcpp::ok()) {
        sensor_msgs::msg::JointState jointStates;
        jointStates.header.frame_id = "";
// 这样子不对
//        jointStates.header.stamp.sec = this->now().seconds();
//        jointStates.header.stamp.nanosec = this->now().nanoseconds();

        double timeSec = this->now().seconds();
        qint32 sec = timeSec;
        jointStates.header.stamp.sec = sec;
        jointStates.header.stamp.nanosec = (timeSec - sec) * 1e9;
        
        jointStates.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"};
        jointStates.position = mJointStates;

        joint_states_publisher->publish(jointStates);

//        RCLCPP_INFO(this->get_logger(), "Publish joint states"); /*Publish feedback*/
//        qDebug() << "joint states:" << mJointStates;
        rate.sleep();
    }
}

3.文件说明以及注意事项

a、myactionrobot.h、myactionrobot.cpp是用了Qt的一些函数，假如你用的是ament，那需要将文件内的一些东西删除掉。
b、这几个节点之间要注意一个时间同步的问题，所以之前的那个use_sim_time要删除或者设置为False。除非你需要自己搞一个clock节点。

4.效果展示

在做好前面的操作后，先在 myRobot路径下执行colcon build编译一次，然后依次执行以下操作。在新的控制台里启动程序之前，记得要先做好环境变量的设置

source install/setup.bash

a、启动MyActionRobot节点
b、在一个控制台启动my_robot.launch.py

ros2 launch mybot my_robot.launch.py 

c、在一个控制台启动my_moveit_rviz.launch.py

ros2 launch mybot my_moveit_rviz.launch.py 

因为目前我还没弄下位机，这里就先把接收到的轨迹打印出来以及将目标位置作为关节状态反馈回去。将来搞好了下位机，直接在回调函数里面接入应该就可以了。