【Moveit2】第一个C++ Moveit程序

【Moveit2】第一个C++ Moveit程序

这个系列的blog会记录笔者学习moveit2的过程。

在第一篇环境配置中，我们对moveit进行了构建，第一次对moveit进行build可能会花费30min左右，请耐心等待。

1.创建功能包

和ROS2的开发流程一致，首先我们需要构建一个功能包来存放我们的脚本程序，进入之前的工作空间ws_moveit2，然后新建我们自己的功能包

cd ~/ws_moveit2/src
ros2 pkg create --build-type ament_cmake \
--dependencies moveit_ros_planning_interface rclcpp \
--node-name hello_moveit2 hello_moveit2

注意这里的选项--node-name hello_moveit2 hello_moveit2，它的用法是--node-name <node_name> <pkg_name>
这里是指定了在hello_moveit2包中的节点hello_moveit2，并且为这个节点自动创建脚本文件hello_moveit2.cpp，然后在CMakeLists.txt中自动配置依赖，主动添加的依赖如下：

add_executable(hello_moveit2 src/hello_moveit2.cpp)
target_include_directories(hello_moveit2 PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(hello_moveit2 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  hello_moveit2
  "moveit_ros_planning_interface"
  "rclcpp"
)

然后在hello_moveit2.cpp文件中写入以下内容，这些内容算是编写moveit2节点的基本模板

#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>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

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

  // Next step goes here

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

然后我们再次进入到ws_moveit2工作空间中，进行编译

cd ~/ws_moveit2
colcon build --mixin debug

这次编译的过程会比较快速，只用了1min左右的时间

然后我们可以运行上面的节点，来检验我们的各项配置没有问题

source install/setup.bash
ros2 run hello_moveit2 hello_moveit2

如果每报错就说明没有问题

2.编写运动控制脚本

然后将上面的hello_moveit2.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>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

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

  // Next step goes here
  // Create hte 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;
}

代码中指定了机械臂的一个位姿，然后使用规划器，自动规划了一条路径。
然后再回到工作空间中，再次进行编译，然后运行官方例程

cd ~/ws_moveit2
colcon build --mixin debug
ros2 launch moveit2_tutorials demo.launch.py

在rviz中的Displays窗口下的Motion Planning/Planning Request取消勾选Query Goal State，然后新建一个终端

cd ~/ws_moveit2
source install/setup.bash
ros2 run hello_moveit2 hello_moveit2

就可以看到机械臂规划运到到我们指定的位姿了

Reference

[1]Your First C++ MoveIt Project