一、第一个C++MoveIt2项目，控制机械臂到固定位姿

目录

1.前提条件

2.创建功能包

3.编写功能代码

4.构建并运行

5.结果

---

1.前提条件

        安装ubuntu22.04系统与ROS2 humble、MoveIt2功能包

2.创建功能包

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

3.编写功能代码

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

using moveit::planning_interface::MoveGroupInterface;
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日志记录器
  auto const logger = rclcpp::get_logger("hello_moveit");
  RCLCPP_INFO(logger,"hello_moveit");

  // 创建MoveIt MoveGroup Interface类的对象，需要两个参数，"node"代表当前的ROS2节点，"panda_arm"是机械臂的命名模型
  auto move_group_interface = MoveGroupInterface(node, "panda_arm");

  // 这里定义了一个lambda表达式来创建目标姿态，是一个geometry_msgs::msg::Pose类型的消息。这个姿态被设置为x=0.28，y=-0.2，z=0.5，w=1.0 
  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方法将这个目标姿态设置为机械臂的运动目标
  move_group_interface.setPoseTarget(target_pose);

  //lambda表达式中生成计划并传入msg，返回到列表中plan
  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);
  }();

  // 判断标志位，执行plan
  if(success) {
    move_group_interface.execute(plan);
  } else {
    RCLCPP_ERROR(logger, "Planing failed!");
  }
  // 关闭ROS 
  rclcpp::shutdown();
  return 0;
}

4.构建并运行

colcon build --packages-select hello_moveit --mixin debug

 刷新环境变量：

. install/setup.bash

 在另一个终端启动示例文件中的rviz与MoveGroup节点：

ros2 launch moveit2_tutorials demo.launch.py

 原终端运行功能程序：

ros2 run hello_moveit hello_moveit

5.结果

panda_arm运动到目标位置：