说明
主要包括定义起点和终点，设定障碍物，rviz可视化；
源码及注释
int main(int argc, char** argv) { ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;//句柄
ros::AsyncSpinner spinner(1);
spinner.start();
//MoveIt！对称为“计划组”的关节集进行操作，并将它们存储在名为JointModelGroup的对象中。整个MoveIt！术语“计划组”和“联合模型组”可互换使用。//
static const std::string PLANNING_GROUP = "panda_arm";
// 该MoveGroup类可以轻松设置使用规划小组的只是名字，你想控制和规划。
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
//我们将使用PlanningSceneInterface 类在“虚拟世界”场景中添加和删除碰撞对象
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
//原始指针经常用于指代规划组以提高性能。
const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
//MoveItVisualTools包提供了许多功能，可用于可视化RViz中的对象，机器人和轨迹以及调试工具，
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
//Remote控制是一种内省工具，允许用户通过RViz中的按钮和键盘快捷键逐步执行高级脚本
visual_tools.loadRemoteControl();
//RViz提供了许多类型的标记，在本演示中我们将使用文本，圆柱和球体
Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
//批量发布用于减少为大型可视化发送到RViz的消息数
visual_tools.trigger();
//我们可以打印这个机器人的参考框架的名称。
ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());
//我们还可以打印该组的末端效应器链接的名称。 ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
//我们可以为这个组计划一个运动，使其达到最终效果器所需的姿势。
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);
//现在，我们调用规划器来计算计划并将其可视化。请注意，我们只是计划，而不是要求move_group实际移动机器人。 moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
//可视化为带有RViz中标记的线
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Moving to a pose goal
移动到姿势目标与上面的步骤类似，除了我们现在使用move（）函数。请注意，我们之前设置的姿势目标仍处于活动状态， 因此机器人将尝试移动到该目标。我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器处于活动状态并报告执行轨迹的成功。 让我们设定一个联合空间目标并向它迈进。这将取代我们上面设置的姿势目标。 首先，我们将创建一个引用当前机器人状态的指针。RobotState是包含所有当前位置/速度/加速度数据的对象。
moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); //接下来获取该组的当前关节集合。
// Next get the current set of joint values for the group.
std::vector joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
// 现在，让我们修改其中一个关节，计划新的关节空间目标并可视化计划。(1弧度) joint_group_positions [0] = -1.0; // radians
move_group.setJointValueTarget(joint_group_positions);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
//可以轻松地为机器人上的链接指定路径约束。让我们为我们的组指定路径约束和姿势目标。首先定义路径约束。
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;
// Now, set it as the path constraint for the group.
//现在，将其设置为组的路径约束。
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
//我们将重用我们拥有的旧目标并计划它。请注意，这仅在当前状态已满足路径约束时才有效。因此，我们需要将开始状态设置为新姿势。 robot_state::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state);
//现在我们将从刚刚创建的新启动状态计划更早的姿势目标。
move_group.setPoseTarget(target_pose1);
//使用约束进行规划可能会很慢，因为每个样本都必须调用反向运动学求解器。让我们将计划时间从默认的5秒增加到确保规划人员有足够的时间成功。 move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
// Visualize the plan in RViz visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");
// When done with the path constraint be sure to clear it. move_group.clearPathConstraints();
// Since we set the start state we have to clear it before planning other paths move_group.setStartStateToCurrentState();
//您可以通过指定末端执行器经过的航点列表来直接规划笛卡尔路径。请注意，我们从上面的新开始状态开始。初始姿势（开始状态）不需要添加到航点列表，但添加它可以帮助进行可视化 geometry_msgs::Pose target_pose3 = move_group.getCurrentPose().pose;
std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(target_pose3);
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3); // down
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); // right
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3); // up and left
//对于诸如逼近和撤退抓握动作之类的动作，笛卡尔运动经常需要较慢。在这里，我们演示了如何通过每个关节的最大速度的比例因子来降低机器人手臂的速度。请注意，这不是末端执行器点的速度。 move_group.setMaxVelocityScalingFactor(0.1);
//我们希望笛卡尔路径以1 cm的分辨率进行插值，这就是为什么我们将指定0.01作为笛卡尔平移的最大步长。我们将跳转阈值指定为0.0， //从而有效地禁用它。警告 - 在操作真实硬件时禁用跳转阈值可能会导致冗余关节的大量不可预测的运动，这可能是一个安全问题
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
// Visualize the plan in RViz visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); 复制代码
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// Adding/Removing Objects and Attaching/Detaching Objects // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ //定义冲突对象ROS消息。 // Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it. collision_object.id = "box1";
// Define a box to add to the world.
//定义一个框以添加到世界中。 shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
// Define a pose for the box (specified relative to frame_id) //为框定义一个姿势 geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects; collision_objects.push_back(collision_object);
// 在场景中添加障碍物 ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
// Show text in RViz of status visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
// 等待MoveGroup接收并处理碰撞对象消息
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
// 当我们计划一条轨道时，它将避开障碍物
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");
// 在 RViz实现可视化
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("next step");
ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group.attachObject(collision_object.id);
visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/等待MoveGroup接收并处理附加的碰撞对象消息/ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the " "robot");
// 现在，让我们从机器人上分离碰撞对象. ROS_INFO_NAMED("tutorial", "Detach the object from the robot"); move_group.detachObject(collision_object.id);
// 在RViz中显示状态文本 visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/等待MoveGroup接收并处理附加的碰撞对象消息/
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the " "robot");
// 去掉障碍物信息.
ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vectorstd::string object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
// 在rviz中显示状态
visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
ros::shutdown();
return 0;
}