邢台专业做网站价格,网页制作初学者,网页建站总结报告,建程网官网平台四节教程会手把手带你写一个完整的 Moveit 控制程序#xff0c;包括轨迹规划、RViz可视化、添加碰撞物体、抓取和放置。
1 创建依赖包
进入到教程所在工作空间下的src目录#xff0c;创建一个新的依赖包。
ros2 pkg create \--build-type ament_cmake \--dependencies mov…四节教程会手把手带你写一个完整的 Moveit 控制程序包括轨迹规划、RViz可视化、添加碰撞物体、抓取和放置。
1 创建依赖包
进入到教程所在工作空间下的src目录创建一个新的依赖包。
ros2 pkg create \--build-type ament_cmake \--dependencies moveit_ros_planning_interface rclcpp \--node-name hello_moveit hello_moveit在新建的这个包中添加hello_moveit.cpp准备开始编码
2 创建ROS节点和执行器
#include memory#include rclcpp/rclcpp.hpp
#include moveit/move_group_interface/move_group_interface.hint main(int argc, char * argv[])
{// 初始化 ROS 并创建节点rclcpp::init(argc, argv);auto const node std::make_sharedrclcpp::Node(hello_moveit,rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));// 创建一个 ROS loggerauto const logger rclcpp::get_logger(hello_moveit);// Next step goes here// 创建 MoveIt 的 MoveGroup 接口
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface MoveGroupInterface(node, panda_arm);// 设置目标位姿
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);// 创建一个到目标位姿的规划
auto const [success, plan] [move_group_interface]{moveit::planning_interface::MoveGroupInterface::Plan msg;auto const ok static_castbool(move_group_interface.plan(msg));return std::make_pair(ok, msg);
}();// 计算这个规划
if(success) {move_group_interface.execute(plan);
} else {RCLCPP_ERROR(logger, Planing failed!);
}// 关闭 ROSrclcpp::shutdown();return 0;
}2.1 编译和运行
我们可以尝试编译并运行一下看看有无出现问题。
2.2 代码说明
最上面的是标准C头文件随后是为使用 ROS 和 Moveit 所添加的头文件。
在这之后我们初始化 rclcpp并创建了节点。
auto const node std::make_sharedrclcpp::Node(hello_moveit,rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);第一个参数是节点名称。第二个参数对于 Moveit 很重要因为我们要使用 ROS 参数。
最后关闭这个节点。
3 使用MoveGroupInterface来规划和执行
在代码中“Next step goes here,”的后面添加以下节点
// 创建 MoveIt 的 MoveGroup 接口 using moveit::planning_interface::MoveGroupInterface; auto move_group_interface MoveGroupInterface(node, panda_arm);
// 设置目标位姿 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);
// 创建一个到目标位姿的规划 auto const [success, plan] [move_group_interface]{ moveit::planning_interface::MoveGroupInterface::Plan msg; auto const ok static_castbool(move_group_interface.plan(msg)); return std::make_pair(ok, msg); }();
// 计算这个规划 if(success) { move_group_interface.execute(plan); } else { RCLCPP_ERROR(logger, Planing failed!); }
3.1 编译和运行
打开教程中的launch文件去启动rviz和 MoveGroup 节点在另一个终端中source这个工作空间并执行
ros2 launch moveit2_tutorials demo.launch.py然后在Displays窗口下面的MotionPlanning/Planning Request中取消选择Query Goal State。
在第三个终端中 source 并允许我们本节的程序
ros2 run hello_moveit hello_moveit注
如果你没有运行launch文件就运行了 hello_moveit 节点等待10s后将会报如下错误
[ERROR] [1644181704.350825487] [hello_moveit]: Could not find parameter robot_description and did not receive robot_description via std_msgs::msg::String subscription within 10.000000 seconds.这是因为demo.launch.py启动了MoveGroup节点其提供了机器人描述信息。当MoveGroupInterface被构建的时候会寻找发布机器人描述话题的节点如果在10秒内没找到就会打印错误信息并结束程序。
3.2 代码说明
首先要创建 MoveGroupInterface这个对象用来与 move_group 交互从而是我们能够去规划和执行轨迹。注意这是我们在程序中创建的唯一的可变对象。
其次是我们创建的 MoveGroupInterface 的第二个接口 panda_arm这是在机器人描述中定义的关节组我们将通过 MoveGroupInterface 去操作它。
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface MoveGroupInterface(node, panda_arm);之后设置目标位姿和规划。起始点位姿通过 joint state publisher 来发布它能够使用MoveGroupInterface::setStartState*中的函数来被改变本教程无
通过使用 lambda 表达式来构建信息类型 target_pose与规划。
// 利用 lambda 函数来创建目标位姿
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);// 利用 lambda 函数来创建到目标位姿的规划
// 注这里使用了 C 20标准的新特性
auto const [success, plan] [move_group_interface]{moveit::planning_interface::MoveGroupInterface::Plan msg;// 强制类型转换auto const ok static_castbool(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!);
}
