目录
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运动到目标位置: