【附带源码】机械臂MoveIt2极简教程（四）、第一个入门demo

系列文章目录

【附带源码】机械臂MoveIt2极简教程（一）、moveit2安装
【附带源码】机械臂MoveIt2极简教程（二）、move_group交互
【附带源码】机械臂MoveIt2极简教程（三）、URDF/SRDF介绍
【附带源码】机械臂MoveIt2极简教程（四）、第一个入门demo
【附带源码】机械臂MoveIt2极简教程（五）、第二个demo - rviz可视化

---

---

这里单独写一个简单的入门demo，更好地帮助大家学习和理解MoveIt2的使用。
其实在教程（二）中展示了好几个demo，感兴趣的同学可以跳转去看一下。

1. 创建package

可以新建一个工作空间，也可以在第一节创建的ws_moveit2工作空间里。

mkdir -p ~/ws_moveit2/src
cd ~/ws_moveit2/src
ros2 pkg create --build-type ament_cmake --dependencies rclcpp --node-name  demo_moveit demo_moveit

2. 修改C++代码

demo_moveit.cpp代码如下：

#include <memory>

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char *argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
      "demo_moveit",
      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

  // Create a ROS logger
  auto const logger = rclcpp::get_logger("demo_moveit");

  // Next step goes here
  // Create the MoveIt MoveGroup Interface
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "panda_arm");

  // Set a target Pose
  auto const target_pose = []
  {
    geometry_msgs::msg::Pose msg;
    msg.orientation.w = 1.0;
    msg.position.x = 0.28;
    msg.position.y = -0.2;
    msg.position.z = 0.5;
    return msg;
  }();
  move_group_interface.setPoseTarget(target_pose);

  // Create a plan to that target pose
  auto const [success, plan] = [&move_group_interface]
  {
    moveit::planning_interface::MoveGroupInterface::Plan msg;
    auto const ok = static_cast<bool>(move_group_interface.plan(msg));
    return std::make_pair(ok, msg);
  }();

  // Execute the plan
  if (success)
  {
    move_group_interface.execute(plan);
  }
  else
  {
    RCLCPP_ERROR(logger, "Planning failed!");
  }

  // Shutdown ROS
  rclcpp::shutdown();
  return 0;
}

3. 修改CMakeLists.txt

cmake_minimum_required(VERSION 3.10.2)
project(demo_moveit)

# Common cmake code applied to all moveit packages
find_package(moveit_common REQUIRED)
moveit_package()

find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED system filesystem date_time thread)
find_package(ament_cmake REQUIRED)
find_package(control_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_ros_perception REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(interactive_markers REQUIRED)
find_package(rviz_visual_tools REQUIRED)
find_package(moveit_visual_tools REQUIRED)
find_package(geometric_shapes REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(pluginlib REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

set(THIS_PACKAGE_INCLUDE_DIRS
  doc/interactivity/include
)

set(THIS_PACKAGE_INCLUDE_DEPENDS
  ament_cmake
  rclcpp
  rclcpp_action
  tf2_geometry_msgs
  tf2_ros
  moveit_core
  rviz_visual_tools
  moveit_visual_tools
  moveit_ros_planning_interface
  interactive_markers
  tf2_geometry_msgs
  moveit_ros_planning
  pluginlib
  Eigen3
  Boost
  control_msgs
  moveit_servo
)

include_directories(${THIS_PACKAGE_INCLUDE_DIRS})

ament_export_dependencies(
  ${THIS_PACKAGE_INCLUDE_DEPENDS}
)
ament_export_include_directories(include)

# --------------------------------------------------------------
add_executable(demo_moveit
  src/demo_moveit.cpp)

target_include_directories(demo_moveit
    PUBLIC include)

ament_target_dependencies(demo_moveit
    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
# -------------------------------------------------------------- install
  
install(TARGETS demo_moveit
  DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)


ament_package()

4. 创建launch文件

在demo_moveit文件夹下创建一个launch文件夹，里面创建demo_moveit.launch.py文件，内容如下：

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def generate_launch_description():
    robot_description_config = xacro.process_file(
        os.path.join(
            get_package_share_directory("moveit_resources_panda_moveit_config"),
            "config",
            "panda.urdf.xacro",
        )
    )
    robot_description = {"robot_description": robot_description_config.toxml()}

    robot_description_semantic_config = load_file(
        "moveit_resources_panda_moveit_config", "config/panda.srdf"
    )
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_config
    }

    kinematics_yaml = load_yaml(
        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
    )

    # MoveGroupInterface demo executable
    move_group_demo = Node(
        package="demo_moveit",
        executable="demo_moveit",
        name="demo_moveit",
        output="screen",
        parameters=[robot_description, 
                    robot_description_semantic, 
                    kinematics_yaml,
                    ],
    )

    return LaunchDescription([move_group_demo])

5. 编译

cd ~/ws_moveit2/
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select demo_moveit

6. 运行

首先开一个终端，进入系列教程第一节创建的工作空间中,

source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

此时，rviz中会出现一个机械臂，留意此时机械臂姿态。

再开一个终端，进入本节创建的工作空间中

cd ~/ws_moveit2/
source install/setup.bash
ros2 launch demo_moveit demo_moveit.launch.py

运行之后，机械臂就会开始运动到预设值的位置。

---

觉得对您有帮助的，可以点个赞👍支持一下，谢谢各位！

因为淋过雨，所以想为别人撑把伞；因为踩过太多坑，所以想让喜欢机器人的同学们减少试错成本！