【MoveIt 2】围绕对象进行规划

十年一梦实验室

于 2024-08-01 12:42:09 发布

阅读量199

点赞数 3

本文链接：https://blog.csdn.net/cxyhjl/article/details/140867055

版权

本教程将向您介绍如何将对象插入到规划场景中并围绕它们进行规划。

先决条件

如果您还没有完成，请确保您已经完成了在 RViz 中可视化的步骤。该项目假设您从上一个教程结束的 hello_moveit 项目开始。如果您只想运行教程，可以按照 Docker 指南启动一个包含完整教程的容器。

步骤

1 添加包含用于规划场景接口

在你的源文件顶部，将此添加到包含列表中：

#include <moveit/planning_scene_interface/planning_scene_interface.h>

2 更改目标姿势

首先，通过以下更改更新目标姿态，使机器人规划到不同的位置：

// 设置目标位姿，并使用更新的值
auto const target_pose = [] {
  geometry_msgs::msg::Pose msg; // 创建一个Pose消息对象
  msg.orientation.y = 0.8; // 设置目标位姿的四元数中的y分量
  msg.orientation.w = 0.6; // 设置目标位姿的四元数中的w分量
  msg.position.x = 0.1; // 设置目标位姿的X坐标
  msg.position.y = 0.4; // 设置目标位姿的Y坐标
  msg.position.z = 0.4; // 设置目标位姿的Z坐标
  return msg; // 返回定义好的Pose消息
}();


// 将定义好的目标位姿设置为MoveGroup的目标
move_group_interface.setPoseTarget(target_pose);

3 创建碰撞对象

在下一个代码块中，我们创建了一个碰撞对象。首先要注意的是，它被放置在机器人的坐标系中。如果我们有一个感知系统报告我们场景中障碍物的位置，那么这就是它会构建的消息。因为这只是一个示例，所以我们手动创建它。需要注意的是，在这个代码块的末尾，我们将此消息的操作设置为 ADD 。这会导致对象被添加到碰撞场景中。将此代码块放在设置目标姿势和创建计划之间。

// 创建一个机器人需要避开的碰撞物体
auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] {
  // 创建一个CollisionObject对象，用于描述碰撞物体
  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = frame_id; // 设置参考坐标系的ID
  collision_object.id = "box1"; // 给碰撞物体一个唯一的ID


  shape_msgs::msg::SolidPrimitive primitive;
  // 定义箱子的大小（单位：米）
  primitive.type = primitive.BOX; // 设置物体类型为BOX
  primitive.dimensions.resize(3); // 为三个维度分配空间
  primitive.dimensions[primitive.BOX_X] = 0.5; // 设置箱子的X方向长度为0.5米
  primitive.dimensions[primitive.BOX_Y] = 0.1; // 设置箱子的Y方向长度为0.1米
  primitive.dimensions[primitive.BOX_Z] = 0.5; // 设置箱子的Z方向长度为0.5米


  // 定义箱子的位姿（相对于frame_id）
  geometry_msgs::msg::Pose box_pose;
  box_pose.orientation.w = 1.0; // 设置箱子的四元数旋转部分
  box_pose.position.x = 0.2; // 设置箱子的X坐标
  box_pose.position.y = 0.2; // 设置箱子的Y坐标
  box_pose.position.z = 0.25; // 设置箱子的Z坐标


  collision_object.primitives.push_back(primitive); // 将定义好的形状添加到碰撞物体中
  collision_object.primitive_poses.push_back(box_pose); // 将定义好的位姿添加到碰撞物体中
  collision_object.operation = collision_object.ADD; // 设置操作类型为添加


  return collision_object; // 返回定义好的碰撞物体
}();

4 将对象添加到规划场景中

最后，我们需要将此对象添加到碰撞场景中。为此，我们使用一个名为 PlanningSceneInterface 的对象，该对象使用 ROS 接口将更改传达给 MoveGroup 的规划场景。此代码块应直接跟在创建碰撞对象的代码块之后。

// 将碰撞物体添加到场景中
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object); // 应用碰撞物体到规划场景中

5 运行程序并观察变化

cxy@cxy-Ubuntu2404:~/ws_moveit$ colcon build --packages-select hello_moveit --mixin debug
Starting >>> hello_moveit
[Processing: hello_moveit]                              
Finished <<< hello_moveit [36.9s]                       


Summary: 1 package finished [48.4s]

正如我们在上一个教程中所做的那样，使用 demo.launch.py 脚本启动 RViz 并运行我们的程序。如果您使用的是其中一个 Docker 教程容器，您可以指定一个不同的 RViz 配置，该配置已经使用了 RvizVisualToolsGui 面板：

ros2 launch moveit2_tutorials demo.launch.py rviz_config:=kinova_hello_moveit.rviz

cxy@cxy-Ubuntu2404:~/ws_moveit$ ros2 run hello_moveit hello_moveit

摘要

您使用 MoveIt 编写的程序扩展了场景中对象的规划。
以下是完整的 hello_moveit.cpp 源代码副本。https://github.com/moveit/moveit2_tutorials/blob/main/doc/tutorials/planning_around_objects/hello_moveit.cpp

进一步阅读

使用规划场景进行碰撞和约束检查的示例。https://moveit.picknik.ai/main/doc/examples/planning_scene/planning_scene_tutorial.html
使用规划场景 ROS API 的示例。https://moveit.picknik.ai/main/doc/examples/planning_scene_ros_api/planning_scene_ros_api_tutorial.html
可视化碰撞对象的示例。https://moveit.picknik.ai/main/doc/examples/visualizing_collisions/visualizing_collisions_tutorial.html
用于规划对象的子帧示例。https://moveit.picknik.ai/main/doc/examples/subframes/subframes_tutorial.html

下一步

在下一个教程《使用 MoveIt 任务构造器进行拾取和放置》中 https://moveit.picknik.ai/main/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.html ，您将被介绍一个用于解决更复杂运动计划的高级工具。在这个下一个教程中，您将创建一个程序来拾取和放置对象。

附录：完整代码

#include <moveit/move_group_interface/move_group_interface.h>
// 包含MoveIt的MoveGroup接口头文件，用于控制机械臂运动


#include <moveit/planning_scene_interface/planning_scene_interface.h>
// 包含MoveIt的规划场景接口头文件，用于定义规划场景中的物体


#include <moveit_visual_tools/moveit_visual_tools.h>
// 包含MoveIt的可视化工具头文件，用于在Rviz中显示和调试


#include <memory>
// 包含C++标准库中的内存管理工具，如std::shared_ptr


#include <rclcpp/rclcpp.hpp>
// 包含ROS 2的核心客户端库


#include <thread>
// 包含C++标准库中的线程工具


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));
  // 创建节点 "hello_moveit"，并自动声明从外部传入的参数


  // 创建ROS的日志记录器
  auto const logger = rclcpp::get_logger("hello_moveit");


  // 我们使用SingleThreadedExecutor来获取机器人状态监控器的当前状态信息
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });
  // 创建一个线程来运行executor


  // 创建MoveIt的MoveGroup接口，用于控制名为"manipulator"的机械臂
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "manipulator");


  // 构建和初始化MoveItVisualTools
  auto moveit_visual_tools =
      moveit_visual_tools::MoveItVisualTools{ node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                              move_group_interface.getRobotModel() };
  // 删除所有标记
  moveit_visual_tools.deleteAllMarkers();
  // 加载远程控制
  moveit_visual_tools.loadRemoteControl();


  // 创建一个闭包函数用于在Rviz中更新文本
  auto const draw_title = [&moveit_visual_tools](auto text) {
    auto const text_pose = [] {
      auto msg = Eigen::Isometry3d::Identity();
      msg.translation().z() = 1.0;
      return msg;
    }();
    // 在指定位置发布文本
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
  };


  auto const prompt = [&moveit_visual_tools](auto text) { moveit_visual_tools.prompt(text); };
  // 显示提示信息


  auto const draw_trajectory_tool_path =
      [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("manipulator")](
          auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); };
  // 显示轨迹路径


  // 设置目标位姿
  auto const target_pose = [] {
    geometry_msgs::msg::Pose msg;
    msg.orientation.y = 0.8;
    msg.orientation.w = 0.6;
    msg.position.x = 0.1;
    msg.position.y = 0.4;
    msg.position.z = 0.4;
    return msg;
  }();
  // 将目标位姿设置为目标
  move_group_interface.setPoseTarget(target_pose);


  // 创建机器人需要避开的碰撞物体
  auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] {
    moveit_msgs::msg::CollisionObject collision_object;
    collision_object.header.frame_id = frame_id;
    collision_object.id = "box1";
    shape_msgs::msg::SolidPrimitive primitive;


    // 定义箱子的尺寸（以米为单位）
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[primitive.BOX_X] = 0.5;
    primitive.dimensions[primitive.BOX_Y] = 0.1;
    primitive.dimensions[primitive.BOX_Z] = 0.5;


    // 定义箱子的位姿（相对于frame_id）
    geometry_msgs::msg::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.2;
    box_pose.position.y = 0.2;
    box_pose.position.z = 0.25;


    // 将箱子加入碰撞物体的列表
    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;


    return collision_object;
  }();


  // 将碰撞物体添加到场景中
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  planning_scene_interface.applyCollisionObject(collision_object);


  // 为目标位姿创建规划
  prompt("Press 'next' in the RvizVisualToolsGui window to plan");
  draw_title("Planning");
  moveit_visual_tools.trigger();
  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)
  {
    draw_trajectory_tool_path(plan.trajectory);
    moveit_visual_tools.trigger();
    prompt("Press 'next' in the RvizVisualToolsGui window to execute");
    draw_title("Executing");
    moveit_visual_tools.trigger();
    move_group_interface.execute(plan);
  }
  else
  {
    draw_title("Planning Failed!");
    moveit_visual_tools.trigger();
    RCLCPP_ERROR(logger, "Planning failed!");
  }


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

笔记

十年一梦实验室

关注

3
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
【MoveIt 2】围绕对象进行规划

本教程将向您介绍如何将对象插入到规划场景中并围绕它们进行规划。先决条件如果您还没有完成，请确保您已经完成了在 RViz 中可视化的步骤。该项目假设您从上一个教程结束的hello_moveit项目开始。如果您只想运行教程，可以按照 Docker 指南启动一个包含完整教程的容器。步骤1 添加包含用于规划场景接口在你的源文件顶部，将此添加到包含列表中：#include <moveit...
复制链接

扫一扫