Duruofu's blog

Appearance

构建启动包 ​

我们不能一直使用提供的demo包来启动机器人，我们还需要自己去编写启动文件，以便于添加自己需要的东西。

一、准备工作 ​

创建一个新的包作为启动包：

bash
cd ros2_ws/src
ros2 pkg create my_robot_bringup

然后新建launch,config 文件夹，并在CMakelist里添加：

install(
  DIRECTORY launch config
  DESTINATION share/${PROJECT_NAME}
)

将my_robot_moveit_config包里的ros2_controllers.yaml文件复制一份到config文件夹

将my_robot_moveit_config包里my_robot.ros2_control.xacro复制到robot_description包的urdf文件夹里。并在主文件robot.urdf.xacro包含：

xml
<?xml version="1.0"?>
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro"> 

    <xacro:include filename="common_properties.xacro"/>
    <xacro:include filename="arm.xacro"/>
    <xacro:include filename="gripper.xacro"/>
    <xacro:include filename="my_robot.ros2_control.xacro"/>

    <joint name="gripper_base_joint" type="fixed">
        <parent link="tool_link"/>
        <child link="gripper_base_link"/>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    </joint>

</robot>

并且删除my_robot.ros2_control.xacro中的这部分：

xml
    <xacro:macro name="my_robot_ros2_control" params="name initial_positions_file">
        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
    </xacro:macro>

并且修改所有关节的初始位置为 0,最终如下：

xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

      <ros2_control name="Arm" type="system">
          <hardware>
              <!-- By default, set up controllers for simulation. This won't work on real hardware -->
              <plugin>mock_components/GenericSystem</plugin>
          </hardware>
          <joint name="joint1">
              <command_interface name="position"/>
              <state_interface name="position">
                <param name="initial_value">0</param>
              </state_interface>
              <state_interface name="velocity"/>
          </joint>
          <joint name="joint2">
              <command_interface name="position"/>
              <state_interface name="position">
                <param name="initial_value">0</param>
              </state_interface>
              <state_interface name="velocity"/>
          </joint>
          <joint name="joint3">
              <command_interface name="position"/>
              <state_interface name="position">
                <param name="initial_value">0</param>
              </state_interface>
              <state_interface name="velocity"/>
          </joint>
          <joint name="joint4">
              <command_interface name="position"/>
              <state_interface name="position">
                <param name="initial_value">0</param>
              </state_interface>
              <state_interface name="velocity"/>
          </joint>
          <joint name="joint5">
              <command_interface name="position"/>
              <state_interface name="position">
                <param name="initial_value">0</param>
              </state_interface>
              <state_interface name="velocity"/>
          </joint>
          <joint name="joint6">
              <command_interface name="position"/>
              <state_interface name="position">
                <param name="initial_value">0</param>
              </state_interface>
              <state_interface name="velocity"/>
          </joint>
          <joint name="gripper_left_finger_joint">
              <command_interface name="position"/>
              <state_interface name="position">
                <param name="initial_value">0</param>
              </state_interface>
              <state_interface name="velocity"/>
          </joint>

        </ros2_control>

</robot>

代码： https://github.com/DuRuofu/moveit2_learn/tree/a40aef29c1aa0f0710407ffcc8881a052fad3df2

二、编写启动文件 ​

在my_robot_bringup/launch下新建robot.launch.xml文件:

xml
<launch>
    <let name="urdf_path"
            value="$(find-pkg-share robot_description)/urdf/robot.urdf.xacro"/>

    <node pkg="robot_state_publisher" exec="robot_state_publisher">
        <param name="robot_description" 
        value="$(command 'xacro $(var urdf_path)')"/>
    </node>       



    <node pkg="rviz2" exec="rviz2" output="screen"/>
</launch>

上面只是简单的加载URDf和启动机器人状态发布节点，下面我们加上其他节点：

xml
<launch>
    <let name="urdf_path"
            value="$(find-pkg-share robot_description)/urdf/robot.urdf.xacro"/>

    <!-- 状态发布器 -->
    <node pkg="robot_state_publisher" exec="robot_state_publisher">
        <param name="robot_description" 
        value="$(command 'xacro $(var urdf_path)')"/>
    </node>    

    <!-- 控制器管理器：必须传入 robot_description 参数，否则 ros2_control 无法加载硬件接口 -->
    <node pkg="controller_manager" exec="ros2_control_node" output="screen">
        <param name="robot_description" value="$(command 'xacro $(var urdf_path)')"/>
        <param from="$(find-pkg-share my_robot_bringup)/config/ros2_controllers.yaml"/> 
    </node>

    <!-- 控制器 -->
    <node pkg="controller_manager"  exec="spawner" args="joint_state_broadcaster"/>
    <node pkg="controller_manager"  exec="spawner" args="arm_controller"/>
    <node pkg="controller_manager"  exec="spawner" args="gripper_controller"/>

    <include file="$(find-pkg-share my_robot_moveit_config)/launch/move_group.launch.py"/>


    <node pkg="rviz2" exec="rviz2" output="screen"/>
</launch>

这个 launch 文件完成了从加载机器人模型、启动 ros2_control 控制器、运行 MoveIt2 规划功能到开启 RViz2 可视化的完整机械臂系统启动流程。

最后在package.xml里添加依赖：

xml
  <execute_depend>robot_description</execute_depend>
  <execute_depend>my_robot_moveit_config</execute_depend>
  <execute_depend>robot_state_publisher</execute_depend>
  <execute_depend>controller_manager</execute_depend>
  <execute_depend>rviz2</execute_depend>

编译运行：

cd ros2_ws
colcon build
source install/setup.bash
ros2 launch my_robot_bringup robot.launch.xml

可以正常使用：

对应python版本：

python
#!/usr/bin/env python3
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import Command, PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():

    # URDF 路径
    urdf_path = PathJoinSubstitution([
        FindPackageShare("robot_description"),
        "urdf",
        "robot.urdf.xacro"
    ])

    # robot_state_publisher
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{
            "robot_description": Command(["xacro ", urdf_path])
        }],
        output="screen"
    )

    # ros2_control_node
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            {"robot_description": Command(["xacro ", urdf_path])},
            PathJoinSubstitution([
                FindPackageShare("my_robot_bringup"),
                "config",
                "ros2_controllers.yaml"
            ])
        ],
        output="screen"
    )

    # 控制器启动器
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"],
        output="screen"
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["arm_controller"],
        output="screen"
    )

    gripper_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["gripper_controller"],
        output="screen"
    )

    # MoveIt2
    move_group_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare("my_robot_moveit_config"),
                "launch",
                "move_group.launch.py"
            ])
        )
    )

    # RViz2
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        output="screen"
    )

    return LaunchDescription([
        robot_state_publisher,
        ros2_control_node,
        joint_state_broadcaster_spawner,
        arm_controller_spawner,
        gripper_controller_spawner,
        move_group_launch,
        rviz_node
    ])

代码： https://github.com/DuRuofu/moveit2_learn/commit/6f4972708eee071415164f4e5d76c5910fac2a8b