【MoveIt 2】直接通过 C++ API 使用 MoveIt : 运动规划 API

最新推荐文章于 2024-08-03 20:27:43 发布
最新推荐文章于 2024-08-03 20:27:43 发布
阅读量627 收藏 8
点赞数 13
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/cxyhjl/article/details/140923727
版权

ff89aa73d8bcaca93e3e34ba317ab51b.png在 MoveIt 中，运动规划器是使用插件基础设施 plugin infrastructure 加载的。这允许 MoveIt 在运行时加载运动规划器。在这个例子中，我们将运行所需的 C++代码来实现这一点。

 入门 

如果您还没有这样做，请确保您已完成入门中的步骤。

 运行演示 

打开两个 shell。在第一个 shell 中启动 RViz 并等待所有内容加载完成：

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行启动文件：

ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py

注意：本教程使用 RvizVisualToolsGui 面板逐步演示。要将此面板添加到 RViz，请按照可视化教程中的说明进行操作。

片刻之后，RViz 窗口应该会出现，并且看起来与此页面顶部的窗口类似。要通过每个演示步骤，请按屏幕底部的 RvizVisualToolsGui 面板中的 Next 按钮，或在屏幕顶部的 Tools 面板中选择 Key Tool，然后在 RViz 聚焦时按键盘上的 N 键。

 预期输出 

0ec7a669826f88b9dc2220143f4239c0.png

在 RViz 中，我们最终应该能够看到四条轨迹被重放：

  1. 机器人将其手臂移动到第一个姿势目标，

    e59427cb7d1948313780a44134383020.png

  2. 机器人将其手臂移动到关节目标，

    7905a2989ff75773709e68a6f4748833.png

  3. 机器人将其手臂移回到原始姿势目标，

  4. 机器人在保持末端执行器水平的同时，将其手臂移动到一个新的姿态目标。

    3b2d8563ef6af3088226c3df402e5a69.png

整个代码 

整个代码可以在 moveit_tutorials GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/motion_planning_api

这段代码展示了如何使用MoveIt和ROS 2进行运动规划。以下是代码的主要步骤和功能：

  1. 初始化和节点创建：

  • 初始化ROS 2。

  • 创建一个名为motion_planning_api_tutorial的节点，并使用单线程执行器运行该节点。

加载机器人模型和规划场景：

  • 使用RobotModelLoader从ROS参数服务器加载机器人描述，并创建RobotModel对象。

  • 创建RobotState和JointModelGroup对象以跟踪当前的机器人姿态和规划组。

  • 使用RobotModel创建PlanningScene对象，该对象维护世界的状态（包括机器人）。

加载规划器插件：

  • 使用ROS的pluginlib库加载规划器插件。

  • 从ROS参数服务器获取规划插件的名称，并实例化规划器。

MoveGroupInterface和可视化工具：

  • 创建MoveGroupInterface对象以与规划组进行交互。

  • 使用MoveItVisualTools进行可视化，包括在RViz中显示文本、轴和轨迹。

姿态目标规划：

  • 创建一个运动规划请求，指定末端执行器的目标姿态。

  • 设置位置和方向的容差。

  • 调用规划器并解决规划问题。

  • 可视化规划结果并在RViz中显示轨迹。

关节空间目标规划：

  • 设置关节空间目标，并创建相应的运动规划请求。

  • 调用规划器并解决规划问题。

  • 可视化规划结果并在RViz中显示轨迹。

路径约束规划：

  • 创建一个新的姿态目标，并添加路径约束（例如，保持末端执行器水平）。

  • 设置规划空间的边界。

  • 调用规划器并解决规划问题。

  • 可视化规划结果并在RViz中显示轨迹。

结束和清理：

  • 等待用户输入以继续演示或退出。

  • 关闭ROS 2节点。

这段代码展示了如何使用MoveIt和ROS 2进行复杂的运动规划和可视化，包括姿态目标、关节空间目标和路径约束。

#include <pluginlib/class_loader.hpp> 
// 插件库加载器


// MoveIt 
#include <moveit/robot_model_loader/robot_model_loader.h> 
// 机器人模型加载器
#include <moveit/planning_interface/planning_interface.h> 
// 规划接口
#include <moveit/planning_scene/planning_scene.h> 
// 规划场景
#include <moveit/kinematic_constraints/utils.h> 
// 运动学约束工具
#include <moveit_msgs/msg/display_trajectory.hpp> 
// 显示轨迹消息
#include <moveit_msgs/msg/planning_scene.h> 
// 规划场景消息
#include <moveit_visual_tools/moveit_visual_tools.h> 
// MoveIt可视化工具
#include <moveit/move_group_interface/move_group_interface.h> 
// MoveIt移动组接口


static const rclcpp::Logger LOGGER = rclcpp::get_logger("motion_planning_api_tutorial"); 
// 定义日志记录器


int main(int argc, char** argv) 
{
  rclcpp::init(argc, argv); 
  // 初始化ROS
  rclcpp::NodeOptions node_options; 
  // 定义节点选项
  node_options.automatically_declare_parameters_from_overrides(true); 
  // 自动声明参数
  std::shared_ptr<rclcpp::Node> motion_planning_api_tutorial_node = 
      rclcpp::Node::make_shared("motion_planning_api_tutorial", node_options); 
  // 创建节点


  rclcpp::executors::SingleThreadedExecutor executor; 
  // 单线程执行器
  executor.add_node(motion_planning_api_tutorial_node); 
  // 添加节点到执行器
  std::thread([&executor]() { executor.spin(); }).detach(); 
  // 启动线程执行


  // BEGIN_TUTORIAL 
  // 开始教程
  // ^^^^^ 
  // 开始使用规划器的设置非常简单。规划器作为插件在MoveIt中设置，
  // 你可以使用ROS的插件库接口加载任何你想使用的规划器。
  // 在我们加载规划器之前，我们需要两个对象，一个是RobotModel，另一个是PlanningScene。
  // 我们首先实例化一个RobotModelLoader对象，该对象会在ROS参数服务器上查找机器人描述，
  // 并为我们构建一个RobotModel以供使用。
  const std::string PLANNING_GROUP = "panda_arm"; 
  // 规划组名称
  robot_model_loader::RobotModelLoader robot_model_loader(motion_planning_api_tutorial_node, "robot_description"); 
  // 加载机器人模型
  const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); 
  // 获取机器人模型
  /* 创建一个RobotState和JointModelGroup来跟踪当前的机器人姿态和规划组 */
  moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); 
  // 创建机器人状态
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); 
  // 获取关节模型组


  // 使用RobotModel，我们可以构建一个PlanningScene，它维护世界的状态（包括机器人）。
  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); 
  // 创建规划场景


  // 配置一个有效的机器人状态
  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready"); 
  // 设置默认机器人状态


  // 现在我们将构建一个加载器来按名称加载一个规划器。
  // 请注意，我们在这里使用的是ROS插件库。
  std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader; 
  // 定义规划器插件加载器
  planning_interface::PlannerManagerPtr planner_instance; 
  // 定义规划器实例
  std::vector<std::string> planner_plugin_names; 
  // 定义规划器插件名称列表


  // 我们将从ROS参数服务器获取我们想要加载的规划插件的名称，然后加载规划器，并确保捕获所有异常。
  if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugins", planner_plugin_names)) 
    RCLCPP_FATAL(LOGGER, "无法找到规划器插件名称"); 
  // 获取规划器插件名称，若获取失败则打印错误信息


  try 
  { 
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager")); 
    // 尝试创建规划器插件加载器
  } 
  catch (pluginlib::PluginlibException& ex) 
  { 
    RCLCPP_FATAL(LOGGER, "创建规划器插件加载器时出现异常 %s", ex.what()); 
    // 若创建失败则打印异常信息
  } 


  if (planner_plugin_names.empty()) 
  { 
    RCLCPP_ERROR(LOGGER, "未定义任何规划器插件。请确保planning_plugins参数不为空。"); 
    // 若未定义任何规划器插件则打印错误信息
    return -1; 
  } 


  const auto& planner_name = planner_plugin_names.at(0); 
  // 获取第一个规划器插件名称
  try 
  { 
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_name)); 
    // 尝试创建规划器实例
    if (!planner_instance->initialize(robot_model, motion_planning_api_tutorial_node,
                                      motion_planning_api_tutorial_node->get_namespace())) 
      RCLCPP_FATAL(LOGGER, "无法初始化规划器实例"); 
    // 若初始化失败则打印错误信息
    RCLCPP_INFO(LOGGER, "使用规划接口 '%s'", planner_instance->getDescription().c_str()); 
    // 打印使用的规划接口信息
  } 
  catch (pluginlib::PluginlibException& ex) 
  { 
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses(); 
    std::stringstream ss; 
    for (const auto& cls : classes) 
      ss << cls << " "; 
    RCLCPP_ERROR(LOGGER, "加载规划器 '%s' 时出现异常: %s\n可用插件: %s", planner_name.c_str(), 
                 ex.what(), ss.str().c_str()); 
    // 若加载规划器失败则打印异常信息以及可用插件列表
  } 


  moveit::planning_interface::MoveGroupInterface move_group(motion_planning_api_tutorial_node, PLANNING_GROUP); 
  // 初始化移动组接口


  // 可视化
  // ^^^^^^^^^^^^^
  // MoveItVisualTools包提供了许多功能，用于在RViz中可视化物体、机器人和轨迹，
  // 以及调试工具，如逐步检查脚本。
  namespace rvt = rviz_visual_tools; 
  moveit_visual_tools::MoveItVisualTools visual_tools(motion_planning_api_tutorial_node, "panda_link0",
                                                      "move_group_tutorial", move_group.getRobotModel()); 
  // 初始化MoveIt可视化工具
  visual_tools.enableBatchPublishing(); 
  // 启用批量发布
  visual_tools.deleteAllMarkers();  // 清除所有旧的标记 
  visual_tools.trigger(); 
  // 触发批量发布


  /* 远程控制是一个检查工具，允许用户通过RViz中的按钮和键盘快捷键逐步执行高级脚本 */
  visual_tools.loadRemoteControl(); 
  // 加载远程控制


  /* RViz提供了多种类型的标记，在本演示中我们将使用文本、圆柱体和球体 */
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); 
  text_pose.translation().z() = 1.75; 
  visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE); 
  // 发布文本标记
  /* 批量发布用于减少发送到RViz的大型可视化的消息数量 */
  visual_tools.trigger();
  
  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按'next'键以开始演示");
  
  // 姿态目标
  // ^^^^^^^^^
  // 我们现在为Panda机器人的手臂创建一个运动计划请求，
  // 指定末端执行器的期望姿态作为输入。
  visual_tools.trigger();
  planning_interface::MotionPlanRequest req; 
  // 定义运动计划请求
  planning_interface::MotionPlanResponse res; 
  // 定义运动计划响应
  geometry_msgs::msg::PoseStamped pose; 
  // 定义姿态消息
  pose.header.frame_id = "panda_link0"; 
  pose.pose.position.x = 0.3; 
  pose.pose.position.y = 0.4; 
  pose.pose.position.z = 0.75; 
  pose.pose.orientation.w = 1.0; 
  // 设置末端执行器的期望姿态
  
  // 指定0.01米的位置容差和0.01弧度的角度容差
  std::vector<double> tolerance_pose(3, 0.01); 
  // 设置姿态容差
  std::vector<double> tolerance_angle(3, 0.01); 
  // 设置角度容差
  
  // 我们将使用moveit_core/kinematic_constraints中的辅助函数构建约束请求。
  moveit_msgs::msg::Constraints pose_goal = 
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); 
  // 构建姿态目标
  
  req.group_name = PLANNING_GROUP;
  req.goal_constraints.push_back(pose_goal);
  
  // 定义工作空间边界
  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
      req.workspace_parameters.min_corner.z = -5.0;
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
      req.workspace_parameters.max_corner.z = 5.0;
  
  // 我们现在构造一个规划上下文，封装场景、请求和响应。
  // 我们使用这个规划上下文调用规划器
  planning_interface::PlanningContextPtr context =
      planner_instance->getPlanningContext(planning_scene, req, res.error_code);
  
  if (!context)
  {
      RCLCPP_ERROR(LOGGER, "创建规划上下文失败");
      return -1;
  }
  context->solve(res);
  if (res.error_code.val != res.error_code.SUCCESS)
  {
      RCLCPP_ERROR(LOGGER, "无法成功计算规划");
      return -1;
  }
  
  // 可视化结果
  // ^^^^^^^^^^^^^^^^^^^^
  std::shared_ptr<rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>> display_publisher =
      motion_planning_api_tutorial_node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path",
                                                                                               1);
  moveit_msgs::msg::DisplayTrajectory display_trajectory;
  
  /* 可视化轨迹 */
  moveit_msgs::msg::MotionPlanResponse response;
  res.getMessage(response);
  
  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);
  
  /* 将规划场景中的状态设置为上一个规划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());
  
  // 显示目标状态
  visual_tools.publishAxisLabeled(pose.pose, "goal_1");
  visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();
  
  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("请在RvizVisualToolsGui窗口中按'下一步'以继续演示");
  
  // 关节空间目标
  // ^^^^^^^^^^^^^^^^^
  // 现在，设置一个关节空间目标
  moveit::core::RobotState goal_state(robot_model);
  std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
  goal_state.setJointGroupPositions(joint_model_group, joint_values);
  moveit_msgs::msg::Constraints joint_goal =
      kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
  req.goal_constraints.clear();
  req.goal_constraints.push_back(joint_goal);
  
  // 调用规划器并可视化轨迹
  /* 重新构建规划上下文 */
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code);
  /* 调用规划器 */
  context->solve(res);
  /* 检查规划是否成功 */
  if (res.error_code.val != res.error_code.SUCCESS)
  {
      RCLCPP_ERROR(LOGGER, "无法成功计算规划");
      return -1;
  }
  /* 可视化轨迹 */
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  
  /* 现在你应该看到两个依次规划的轨迹 */
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);
  
  /* 我们将添加更多目标。但首先，将规划场景中的状态设置为上一个规划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());
  
  // 显示目标状态
  visual_tools.publishAxisLabeled(pose.pose, "goal_2");
  visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();
  
  /* 等待用户输入 */
  visual_tools.prompt("请在RvizVisualToolsGui窗口中按'下一步'以继续演示");
  
  /* 现在，我们回到第一个目标，以准备进行受方向约束的规划 */
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal);
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code);
  context->solve(res);
  res.getMessage(response);
  
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);
  
  /* 将规划场景中的状态设置为上一个规划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());
  
  // 显示目标状态
  visual_tools.trigger();
  
  /* 等待用户输入 */
  visual_tools.prompt("请在RvizVisualToolsGui窗口中按'下一步'以继续演示");
  
  // 添加路径约束
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // 让我们再次添加一个新的姿势目标。这次我们还会添加一个路径约束。
  /* 让我们创建一个新的姿势目标 */
  
  pose.pose.position.x = 0.32;
  pose.pose.position.y = -0.25;
  pose.pose.position.z = 0.65;
  pose.pose.orientation.w = 1.0;
  moveit_msgs::msg::Constraints pose_goal_2 =
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
  
  /* 现在，让我们尝试移动到这个新的姿势目标 */
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal_2);
  
  /* 但是，让我们对运动施加路径约束。
  // 在这里，我们要求末端执行器保持水平 */
  geometry_msgs::msg::QuaternionStamped quaternion;
  quaternion.header.frame_id = "panda_link0";
  req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);
  
  // 施加路径约束要求规划器在末端执行器的可能位置空间中进行推理
  // （机器人的工作空间）
  // 因此，我们需要为允许的规划体积指定一个边界；
  // 注意：默认边界由WorkspaceBounds请求适配器自动填充（OMPL管道的一部分，
  // 但在此示例中未使用）。
  // 我们使用一个绝对包括手臂可达空间的边界。这样做没问题，因为在规划手臂时不会在该体积内进行采样；
  // 边界仅用于确定采样配置是否有效
  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
      req.workspace_parameters.min_corner.z = -2.0;
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
      req.workspace_parameters.max_corner.z = 2.0;
  
  // 调用规划器并可视化迄今为止创建的所有规划。
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code);
  context->solve(res);
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);
  
  /* 将规划场景中的状态设置为上一个规划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());
  
  // 显示目标状态
  visual_tools.publishAxisLabeled(pose.pose, "goal_3");
  visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();
  
  // 结束教程
  /* 等待用户输入 */
  visual_tools.prompt("请在RvizVisualToolsGui窗口中按'下一步'以退出演示");
  planner_instance.reset();
  
  rclcpp::shutdown();
  return 0;
}

 开始 

设置开始使用规划器非常容易。规划器在 MoveIt 中设置为插件，您可以使用 ROS pluginlib 接口加载任何您想使用的规划器。在我们加载规划器之前，我们需要两个对象，一个 RobotModel 和一个 PlanningScene。我们将首先实例化一个 RobotModelLoader 对象，它将在 ROS 参数服务器上查找机器人描述并为我们构建一个 RobotModel

const std::string PLANNING_GROUP = "panda_arm"; // 规划组
  robot_model_loader::RobotModelLoader robot_model_loader(motion_planning_api_tutorial_node, "robot_description"); // 机器人模型加载器
  const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); // 获取机器人模型
  /* 创建一个RobotState和JointModelGroup来跟踪当前的机器人姿态和规划组 */
  moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); // 创建机器人状态
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); // 获取关节模型组

使用 RobotModel，我们可以构建一个保持世界状态（包括机器人）的 PlanningScene。

planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); // 创建规划场景

配置有效的机器人状态

planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready"); // 设置默认值

我们现在将构建一个加载器来加载一个规划器，按名称。请注意，我们在这里使用的是 ROS pluginlib 库。

std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader; // 规划器插件加载器
planning_interface::PlannerManagerPtr planner_instance; // 规划器实例
std::vector<std::string> planner_plugin_names; // 规划器插件名称列表

我们将从 ROS 参数服务器获取要加载的规划插件的名称，然后加载规划器，确保捕获所有异常。

if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugins", planner_plugin_names))
    RCLCPP_FATAL(LOGGER, "无法找到规划器插件名称"); // 获取规划器插件名称失败
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager")); // 重置规划器插件加载器
  }
  catch (pluginlib::PluginlibException& ex)
  {
    RCLCPP_FATAL(LOGGER, "创建规划插件加载器时发生异常 %s", ex.what()); // 创建规划插件加载器时发生异常
  }




  if (planner_plugin_names.empty())
  {
    RCLCPP_ERROR(LOGGER,
                 "未定义任何规划器插件。请确保planning_plugins参数不为空。"); // 未定义任何规划器插件
    return -1;
  }




  const auto& planner_name = planner_plugin_names.at(0); // 获取第一个规划器插件名称
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_name)); // 创建未管理的规划器实例
    if (!planner_instance->initialize(robot_model, motion_planning_api_tutorial_node,
                                      motion_planning_api_tutorial_node->get_namespace()))
      RCLCPP_FATAL(LOGGER, "无法初始化规划器实例"); // 初始化规划器实例失败
    RCLCPP_INFO(LOGGER, "使用规划接口 '%s'", planner_instance->getDescription().c_str()); // 使用规划接口
  }
  catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses(); // 获取声明的类
    std::stringstream ss;
    for (const auto& cls : classes)
      ss << cls << " ";
    RCLCPP_ERROR(LOGGER, "加载规划器 '%s' 时发生异常: %s\n可用插件: %s", planner_name.c_str(),
                 ex.what(), ss.str().c_str()); // 加载规划器时发生异常
  }




  moveit::planning_interface::MoveGroupInterface move_group(motion_planning_api_tutorial_node, PLANNING_GROUP); // Move组接口

可视化 

该软件包 MoveItVisualTools 提供了许多在 RViz 中可视化对象、机器人和轨迹的功能，以及诸如脚本逐步内省等调试工具。

namespace rvt = rviz_visual_tools; // rviz可视化工具命名空间
  moveit_visual_tools::MoveItVisualTools visual_tools(motion_planning_api_tutorial_node, "panda_link0",
                                                      "move_group_tutorial", move_group.getRobotModel()); // MoveIt可视化工具
  visual_tools.enableBatchPublishing(); // 启用批量发布
  visual_tools.deleteAllMarkers();  // 清除所有旧标记
  visual_tools.trigger(); // 触发可视化工具


  /* 远程控制是一个检查工具，允许用户通过RViz中的按钮和键盘快捷键逐步执行高级脚本 */
  visual_tools.loadRemoteControl(); // 加载远程控制
  /* RViz提供了多种类型的标记，在本演示中我们将使用文本、圆柱体和球体 */
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); // 文本姿态
  text_pose.translation().z() = 1.75; // 设置文本位置
  visual_tools.publishText(text_pose, "运动规划API演示", rvt::WHITE, rvt::XLARGE); // 发布文本


  /* 批量发布用于减少发送到RViz的大型可视化的消息数量 */
  visual_tools.trigger(); // 触发可视化工具
  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按'next'开始演示"); // 提示用户输入

姿势目标 

我们现在将为 Panda 的手臂创建一个运动规划请求，指定末端执行器的期望姿态作为输入。

visual_tools.trigger(); // 触发可视化工具
  planning_interface::MotionPlanRequest req; // 运动计划请求
  planning_interface::MotionPlanResponse res; // 运动计划响应
  geometry_msgs::msg::PoseStamped pose; // 姿态消息
  pose.header.frame_id = "panda_link0"; // 设置帧ID
  pose.pose.position.x = 0.3; // 设置位置x
  pose.pose.position.y = 0.4; // 设置位置y
  pose.pose.position.z = 0.75; // 设置位置z
  pose.pose.orientation.w = 1.0; // 设置方向w

位置公差为 0.01 米，方向公差为 0.01 弧度。

std::vector<double> tolerance_pose(3, 0.01); // 位置公差  
  std::vector<double> tolerance_angle(3, 0.01); // 角度公差

我们将使用 kinematic_constraints 包中可用的辅助函数将请求创建为约束。

moveit_msgs::msg::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); // 构建目标约束




  req.group_name = PLANNING_GROUP; // 设置规划组名称
  req.goal_constraints.push_back(pose_goal); // 添加目标约束

 定义工作区边界

req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
      req.workspace_parameters.min_corner.z = -5.0; // 设置最小角点
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
      req.workspace_parameters.max_corner.z = 5.0; // 设置最大角点

我们现在构建一个规划上下文，封装场景、请求和响应。我们使用这个规划上下文调用规划器。

planning_interface::PlanningContextPtr context =
      planner_instance->getPlanningContext(planning_scene, req, res.error_code); // 获取规划上下文




  if (!context)
  {
    RCLCPP_ERROR(LOGGER, "创建规划上下文失败"); // 创建规划上下文失败
    return -1;
  }
  context->solve(res); // 解决规划问题
  if (res.error_code.val != res.error_code.SUCCESS)
  {
    RCLCPP_ERROR(LOGGER, "无法成功计算规划"); // 计算规划失败
    return -1;
  }

可视化结果 

std::shared_ptr<rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>> display_publisher =
      motion_planning_api_tutorial_node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path",
                                                                                               1); // 创建显示轨迹的发布者
  moveit_msgs::msg::DisplayTrajectory display_trajectory; // 显示轨迹消息


  /* 可视化轨迹 */
  moveit_msgs::msg::MotionPlanResponse response; // 运动计划响应
  res.getMessage(response); // 获取响应消息


  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起点
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化工具
  display_publisher->publish(display_trajectory); // 发布显示轨迹


  /* 将规划场景中的状态设置为最后一个计划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  planning_scene->setCurrentState(*robot_state.get()); // 设置当前状态

显示目标状态

4df12d346480f9b31d4cdcc48ab7d762.png

visual_tools.publishAxisLabeled(pose.pose, "goal_1"); // 发布标记轴
  visual_tools.publishText(text_pose, "姿态目标 (1)", rvt::WHITE, rvt::XLARGE); // 发布文本
  visual_tools.trigger(); // 触发可视化工具


  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按'next'继续演示"); // 提示用户输入

 关节空间目标 

现在，设定一个关节空间目标

moveit::core::RobotState goal_state(robot_model); // 创建机器人状态
  std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 }; // 设置关节值
  goal_state.setJointGroupPositions(joint_model_group, joint_values); // 设置关节组位置
  moveit_msgs::msg::Constraints joint_goal =
      kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); // 构建关节目标约束
  req.goal_constraints.clear(); // 清除目标约束
  req.goal_constraints.push_back(joint_goal); // 添加关节目标约束

调用规划器并可视化轨迹

/* 重新构建规划上下文 */
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code); // 获取规划上下文
  /* 调用规划器 */
  context->solve(res); // 解决规划问题
  /* 检查规划是否成功 */
  if (res.error_code.val != res.error_code.SUCCESS)
  {
    RCLCPP_ERROR(LOGGER, "无法成功计算规划"); // 计算规划失败
    return -1;
  }
  /* 可视化轨迹 */
  res.getMessage(response); // 获取响应消息
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹


  /* 现在你应该看到两个连续的规划轨迹 */
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化工具
  display_publisher->publish(display_trajectory); // 发布显示轨迹
  /* 我们将添加更多目标。但首先，将规划场景中的状态设置为最后一个计划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  planning_scene->setCurrentState(*robot_state.get()); // 设置当前状态

显示目标状态

89b2f95e8fcbee58715d7aae843186a6.png

visual_tools.publishAxisLabeled(pose.pose, "goal_2"); // 发布标记轴
  visual_tools.publishText(text_pose, "关节空间目标 (2)", rvt::WHITE, rvt::XLARGE); // 发布文本
  visual_tools.trigger(); // 触发可视化工具




  /* 等待用户输入 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按'next'继续演示"); // 提示用户输入




  /* 现在，我们回到第一个目标，为方向约束规划做准备 */
  req.goal_constraints.clear(); // 清除目标约束
  req.goal_constraints.push_back(pose_goal); // 添加姿态目标约束
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code); // 获取规划上下文
  context->solve(res); // 解决规划问题
  res.getMessage(response); // 获取响应消息




  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化工具
  display_publisher->publish(display_trajectory); // 发布显示轨迹




  /* 将规划场景中的状态设置为最后一个计划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  planning_scene->setCurrentState(*robot_state.get()); // 设置当前状态

显示目标状态

bdd76f09253d72d7c7396ec7d8999e8a.png

// 显示目标状态
  visual_tools.trigger(); // 触发可视化工具


  /* 等待用户输入 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按'next'继续演示"); // 提示用户输入

添加路径约束 

让我们再次添加一个新的姿势目标。这次我们还将为运动添加路径约束。

pose.pose.position.x = 0.32; // 设置位置x
  pose.pose.position.y = -0.25; // 设置位置y
  pose.pose.position.z = 0.65; // 设置位置z
  pose.pose.orientation.w = 1.0; // 设置方向w
  moveit_msgs::msg::Constraints pose_goal_2 =
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); // 构建姿态目标约束
  /* 现在，让我们尝试移动到这个新的姿态目标 */
  req.goal_constraints.clear(); // 清除目标约束
  req.goal_constraints.push_back(pose_goal_2); // 添加姿态目标约束


  /* 但是，让我们对运动施加路径约束。
     在这里，我们要求末端执行器保持水平 */
  geometry_msgs::msg::QuaternionStamped quaternion; // 四元数消息
  quaternion.header.frame_id = "panda_link0"; // 设置帧ID
  req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion); // 构建路径约束

施加路径约束要求规划器在末端执行器可能的位置空间（机器人工作空间）中进行推理，因此，我们还需要为允许的规划体积指定一个边界；注意：默认边界由 WorkspaceBounds 请求适配器自动填充（OMPL 管道的一部分，但在此示例中未使用）。我们使用的边界肯定包括手臂的可达空间。这是可以的，因为在为手臂规划时不会在此体积中进行采样；边界仅用于确定采样的配置是否有效。

req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
      req.workspace_parameters.min_corner.z = -2.0; // 设置最小角点
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
      req.workspace_parameters.max_corner.z = 2.0// 设置最大角点

调用规划器并可视化到目前为止创建的所有规划。

context = planner_instance->getPlanningContext(planning_scene, req, res.error_code); // 获取规划上下文
  context->solve(res); // 解决规划问题
  res.getMessage(response); // 获取响应消息
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化工具
  display_publisher->publish(display_trajectory); // 发布显示轨迹


  /* 将规划场景中的状态设置为最后一个计划的最终状态 */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  planning_scene->setCurrentState(*robot_state.get()); // 设置当前状态

显示目标状态

7cb9a63cdcd9717bfcc60bcf73ee5404.png

visual_tools.publishAxisLabeled(pose.pose, "goal_3"); // 发布标记轴
  visual_tools.publishText(text_pose, "姿态目标 (3)", rvt::WHITE, rvt::XLARGE); // 发布文本
  visual_tools.trigger(); // 触发可视化工具


  /* 等待用户输入 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按'next'继续演示"); // 提示用户输入

 启动文件 

整个启动文件在 GitHub 上。本教程中的所有代码都可以从 moveit_tutorials 包中编译和运行。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/motion_planning_api/launch/motion_planning_api_tutorial.launch.py

from launch import LaunchDescription  # 从launch模块导入LaunchDescription类
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类
from moveit_configs_utils import MoveItConfigsBuilder  # 从moveit_configs_utils模块导入MoveItConfigsBuilder类


def generate_launch_description():  # 定义generate_launch_description函数
    moveit_config = (  # 创建moveit_config对象
        MoveItConfigsBuilder("moveit_resources_panda")  # 使用MoveItConfigsBuilder类构建配置
        .robot_description(file_path="config/panda.urdf.xacro")  # 设置机器人描述文件路径
        .robot_description_semantic(file_path="config/panda.srdf")  # 设置机器人语义描述文件路径
        .trajectory_execution(file_path="config/moveit_controllers.yaml")  # 设置轨迹执行配置文件路径
        .planning_pipelines(pipelines=["ompl"])  # 设置规划管道
        .to_moveit_configs()  # 转换为MoveIt配置
    )


    return LaunchDescription(  # 返回LaunchDescription对象
        [
            Node(  # 创建Node对象
                package="moveit2_tutorials",  # 设置包名为moveit2_tutorials
                executable="motion_planning_api_tutorial",  # 设置可执行文件名为motion_planning_api_tutorial
                name="motion_planning_api_tutorial",  # 设置节点名为motion_planning_api_tutorial
                output="screen",  # 设置输出为屏幕
                parameters=[moveit_config.to_dict()],  # 设置参数为moveit_config的字典表示
            )
        ]
    )
十年一梦实验室
关注
  • 13
    点赞
  • 踩
  • 8
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
11.Moveit API C++源码以及注释
m0_62365939的博客
07-19 151
我用的ROS版本为noetic（noetic页面无法打开），使用melodic页面的代码也可以运行。
ROS机械臂的moveit编程实现和视觉应用学习源码
09-07
提供了Python和C++两种API，你可以通过学习源码了解如何在两种语言中调用MoveIt!的功能，编写控制程序。 8. **Gazebo仿真**：MoveIt!支持与Gazebo仿真器集成，源码可能包含如何设置仿真环境，测试和调试运动规划。 ...
【MoveIt 2】直接通过 C++ API 使用 MoveIt : 时间参数化
cxyhjl的博客
08-03 401
MoveIt 目前主要是一个运动学运动规划框架 - 它规划关节或末端执行器的位置，但不规划速度或加速度。然而，MoveIt 确实利用后处理对运动学轨迹进行时间参数化，以获得速度和加速度值。下面我们解释了 MoveIt 这一部分涉及的设置和组件。速度控制从文件默认情况下，MoveIt 将关节轨迹的关节速度和加速度限制设置为机器人 URDF 或joint_limits.yaml中允许的默认...
【MoveIt 2】示例：直接通过 C++ API 使用 MoveIt -规划场景
cxyhjl的博客
08-02 254
规划场景PlanningScene 类提供了用于碰撞检查和约束检查的主要接口。在本教程中，我们将探讨该类的 C++ 接口。入门如果您还没有这样做，请确保您已完成入门中的步骤。整个代码整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/planning...
MoveIt2——8.运动规划API
Avada_C_L的博客
07-19 1184
Moveit2官方教程翻译
Moveit！编程C++ API详解（一）
xudesheng1234的博客
12-11 4332
Moveit中，MoveGroup类提供给我们丰富的函数可以用来设计机器人姿态、进行运动规划。本文以官方API文档以及古月的ROS探索总结为基础，详解Moveit中的C++接口编程说明。 1. 包含头文件 #include <moveit/move_group_interface/move_group_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> #include
MoveIt教程[9]：Motion Planning API
NLP工程化
01-07 1585
在MoveIt中，运动规划器加载使用插件基础设施。这允许MoveIt在运行时加载运动规划器。在本例中，将运行完成此任务所需的C++代码。 一.Running the Demo 打开两个shell。在第一个shell中启动RViz和等待一切完成加载： roslaunch panda_moveit_config demo.launch 在第二个shell中，运行launch文件： roslaunch...
【Moveit】Moveit Core结构、C++源码解析与规划流程举例
vrijheid的博客
04-22 1978
文章目录参考 moveit作为一个优秀的框架是很值得学习的，其中主要关注的点： 碰撞检测 路径规划 适配器 参考 official source code and api
MoveIt教程[3]：Move Group C++ Interface
NLP工程化
01-04 1423
在MoveIt中，最简单的用户接口是MoveGroup类。它为用户可能想要执行的大多数操作提供了易于使用的功能，特别是设置关节或姿势目标、创建运动规划、移动机器人、向环境中添加对象和从robot上附加/分离对象。这个接口通过ROS主题、服务和操作与MoveGroup节点通信。 一.Running the Code 打开两个shell。在第一个shell中开始RViz，和等待一切完成加载： rosl...
ROS之MoveIt!运动规划框架
zhangrelay的专栏
11-16 2940
  安装  文件  关于  帮助  博客 参与其中 2018年10月25日公告 World MoveIt！ 结合运动规划，操纵，3D感知，运动学，控制和导航方面的最新进展 海报如下  入门 最新消息：Melodic / Ubuntu 18.04 从DEBIAN安装 版本：0.10.0 从源安装 | 在Github上查看   运动规划 在杂乱的环境中生成...
课件_ROS机械臂开发_4.ROS机械臂开发中的主角MoveIt!.pdf
03-10
2. 用户接口move_group_interface（C++接口）：move_group_interface是MoveIt!的C++接口，提供了_MoveGroup类来访问move_group的功能。 3. moveit_commander（Python接口）：moveit_commander是MoveIt!的Python接口...
industrial_training:ROS-工业培训材料
05-07
它包括了机器人模型、控制器接口、运动规划和仿真工具等。 - **工业机器人模型**：ROS-Industrial提供精确的机器人型号和配置数据，允许用户在ROS环境中进行有效的建模和仿真。 - **控制器接口**：ROS-Industrial...
franka_ros:Franka Emika研究机器人的ROS集成
05-03
在ROS中，C++ API提供了高性能的接口来创建ROS节点，同时保持ROS消息和服务的使用简便性。 综上所述，`franka_ros`项目通过ROS为Franka Emika机器人提供了全面的软件支持，包括硬件接口、控制、规划、传感器数据...
新闻阅读器.rar
08-04
小程序的设计源码通常包含多个文件和文件夹，组织结构清晰，以便开发者能够快速上手并进行定制化开发。主要文件和文件夹包括： 页面文件夹：存放小程序的各个页面，每个页面通常由.wxml、.wxss、.js和.json文件组成。WXML文件负责页面的结构，类似于HTML；WXSS文件负责样式，类似于CSS；JS文件负责页面的逻辑和交互；JSON文件用于页面的配置，如导航栏标题等。 组件文件夹：存放可复用的UI组件。组件与页面类似，也由.wxml、.wxss、.js和.json文件组成。通过组件化设计，可以提高代码的复用性和维护性，减少重复工作。 静态资源文件夹：存放图片、音频、视频等静态资源，便于在小程序中引用。这些资源通常放在一个名为assets或static的文件夹中。 配置文件：小程序的根目录下通常有一个app.json文件，用于全局配置，如页面路径、导航栏样式、底部Tab栏等。此外，还有app.wxss和app.js文件，分别用于全局样式和全局逻辑。 工具文件夹：存放一些工具函数和库文件，便于在小程序中调用。这些文件通常放在一个名为utils的文件夹中。
Windows Server 2008R2 sp1 升级 Service Pack 1
08-04
windows Server 2008 R2 Standard 升级 Service Pack 1 《Windows Server 2008 R2 SP1与IE11及依赖包详解》 Windows Server 2008 R2 SP1（Service Pack 1）是微软公司针对其服务器操作系统Windows Server 2008 R2的一个重要更新，旨在增强系统性能、提升安全性，并修复了大量已知问题。SP1的引入，不仅包含了自Windows Server 2008 R2发布以来的所有累积更新，还提供了对新技术的支持，例如Hyper-V 3.0虚拟化技术，提高了云计算和数据中心的管理效率。 IE11（Internet Explorer 11）是微软推出的最后一个版本的传统IE浏览器，对于Windows Server 2008 R2 SP1来说，这是一个重要的升级。IE11带来了更快的网页加载速度、更好的标准支持以及增强的安全特性。它支持HTML5、CSS3等现代Web技术，提供了更好的用户体验，并且增强了对企业级应用的兼容性。
基于python的crazy管理系统 (5).zip
最新发布
08-04
安装包
ThinkPHP高仿蓝奏云网盘系统源码/对接易支付系统程序
08-04
基于ThinkPHP开发的高仿蓝奏云网盘系统源码，对接易支付系统，此网盘可以做网赚网盘，有VIP功能支持对接易支付，后台自行配置。 搭建教程： 1、修改数据库config/database.php 2、导入打包的数据库文件 3、设置为pcblic运行目录 4、设置伪静态规则为 thinkphp 5、上面全部设置好了就可以后台登入地址：域名/admin.php
moveit!的api
05-28
MoveIt! 是一个用于机器人运动规划、控制和仿真的ROS软件包。它提供了一组API，包括运动规划接口、控制接口、状态检查接口和仿真接口等，可以帮助用户轻松地进行机器人运动规划和控制。下面是一些常用的MoveIt! API： 1. move_group_interface：提供了一组高级接口，用于规划和执行机器人的运动。 2. planning_scene_interface：提供了一组接口，用于管理机器人的运动规划场景。 3. robot_model_loader：用于加载机器人模型。 4. robot_state_interface：提供了一组接口，用于管理机器人的状态。 5. trajectory_execution_manager：用于执行机器人的规划轨迹。 6. kinematic_constraints：提供了一组接口，用于定义机器人运动的约束条件。 7. collision_detection：提供了一组接口，用于检测机器人运动中的碰撞。 8. joint_trajectory_controller：用于控制机器人的关节轨迹。 以上是一些常用的MoveIt! API，可以帮助用户快速进行机器人运动规划和控制。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值