Moveit2 第一个C++规划程序

编写C++项目

1、创建包

cd到src目录

cd src

创建C++包

ros2 pkg create \
 --build-type ament_cmake \
 --dependencies moveit_ros_planning_interface rclcpp \
 --node-name hello_moveit hello_moveit

请注意，我们已将 <package_name>``moveit_ros_planning_interface和 <package_name> 添加为依赖项。这将对 <package_name>和 <package_name> 文件rclcpp进行必要的更改，以便我们可以依赖这两个软件包。package.xml``CMakeLists.txt

2、创建 ROS 节点和执行器

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

int main(int argc, char *argv[])
{
  // 初始化ROS并创建节点
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
      "hello_moveit",
      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

  // 创建 ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // 下一步，创建 MoveIt! 的 MoveGroupInterface 对象

  // 关闭 ROS
  rclcpp::shutdown();
  return 0;
}

将目录切换回工作区目录ws_moveit即cd ..，然后运行以下命令：

colcon build --mixin debug

成功后，打开一个新的终端，然后在该新终端中执行工作区环境脚本

cd ~/ws_moveit
source install/setup.bash

运行程序并查看输出结果

ros2 run hello_moveit hello_moveit

使用 MoveGroupInterface 进行计划和执行

将“下一步，创建 MoveIt! 的 MoveGroupInterface 对象”的注释替换为以下代码

  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "manipulator");

  // 设置一个目标位姿
  // 起始姿态隐式地等于关节状态发布器发布的位置，
  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);

  // 创建一个目标点规划
  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);
  }();

  // 执行规划
  if (success)
  {
    move_group_interface.execute(plan);
  }
  else
  {
    RCLCPP_ERROR(logger, "Planning failed!");
  }

• 启始姿态可以通过MoveGroupInterface::setStartState*设置

在工作区目录中ws_moveit，运行以下命令：

colcon build --mixin debug

在另一个终端中，加载工作区，然后执行以下命令：

ros2 launch moveit2_tutorials demo.launch.py

在第三个终端中，加载工作区并运行你的程序

source install/setup.bash

ros2 run hello_moveit hello_moveit

RViz 中的机器人会移动并最终摆出以下姿势：

如果在 10 秒内找不到robot_description就会报错退出