【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
就可以看到机械臂规划运到到我们指定的位姿了
