/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, SRI International
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of SRI International nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************//* Author: Sachin Chitta, Dave Coleman, Mike Lautman */#include<moveit/move_group_interface/move_group_interface.h>#include<moveit/planning_scene_interface/planning_scene_interface.h>#include<moveit_msgs/DisplayRobotState.h>#include<moveit_msgs/DisplayTrajectory.h>#include<moveit_msgs/AttachedCollisionObject.h>#include<moveit_msgs/CollisionObject.h>#include<moveit_visual_tools/moveit_visual_tools.h>intmain(int argc,char** argv){/* 通过命令行参数初始化ROS节点，该节点的名字为move_group_interface_tutorial */
ros::init(argc, argv,"move_group_interface_tutorial");/* 通过创建一个ros::NodeHandle启动roscpp节点，创建该节点时将调用ros::start()，销毁该节点时调用ros::shutdown() */
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();// BEGIN_TUTORIAL//// 1. Setup// ^^^^^//// MoveIt! operates on sets of joints called "planning groups" and stores them in an object called// the `JointModelGroup`. Throughout MoveIt! the terms "planning group" and "joint model group"// are used interchangably./**
* 1.1 MoveIt在称为“规划组”（planning groups）的一组关节上进行操作，并将它们存储在名为“JointModelGroup”的对象中。
* 在整个MoveIt中！ 术语“规划组”和“联合模型组”可互换使用。
*/staticconst std::string PLANNING_GROUP ="panda_arm";// The :move_group_interface:`MoveGroup` class can be easily// setup using just the name of the planning group you would like to control and plan for.// MoveGroup class继承自MoveGroupInterface class/* 1.2 可以使用您要控制和规划的规划组的名称轻松设置：move_group_interface：`MoveGroup`类。 MoveGroup类继承自MoveGroupInterface类 */
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);// 1.3 我们将会使用 :planning_scene_interface:`PlanningSceneInterface`这个类来在虚拟世界场景中添加和移除碰撞物体
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;// 1.4 经常使用原指针来指向规划组，用于提高性能const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);// 2. 可视化// ^^^^^^^^^^^^^//// 2.1 功能包MoveItVisualTools提供了很多在RViz中可视化物体、机器人和轨迹的能力，以及debug的工具。namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();// 2.2 远程控制是一个自省工具，允许用户使用高级脚本在RViz中使用按钮和快捷键
visual_tools.loadRemoteControl();// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres/* 2.3 RViz提供了很多种类型的marker，在这个demo中我们将会使用text, cylinders, and spheres */
Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z()=1.75;/* 将文本标记发布到RViz */
visual_tools.publishText(text_pose,"MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations/* 2.4 批量发布用于减少发送到RViz进行大型可视化的消息数量 */
visual_tools.trigger();// 3. Getting Basic Information// ^^^^^^^^^^^^^^^^^^^^^^^^^//// We can print the name of the reference frame for this robot./* 3.1 我们可以为该机器人打印参考坐标系(Reference frame)的名称。 */ROS_INFO_NAMED("tutorial","Reference frame: %s", move_group.getPlanningFrame().c_str());// We can also print the name of the end-effector link for this group./* 3.2 我们也能打印这个规划组末端效应器关节的名称。 */ROS_INFO_NAMED("tutorial","End effector link: %s", move_group.getEndEffectorLink().c_str());// Start the demo// ^^^^^^^^^^^^^^^^^^^^^^^^^
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");// 4. 规划一个目标位姿 (Plan 1)// ^^^^^^^^^^^^^^^^^^^^^^^// 4.1 我们能为这个规划组规划一个运动，使得末端效应器到达一个期望的位姿。
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);// 4.2 现在我们调用规划器来计算出轨迹并可视化它。注意我们仅仅是规划，并没有让move_group来移动机器人
moveit::planning_interface::MoveGroupInterface::Plan my_plan;/* 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");// 5. 可视化规划// ^^^^^^^^^^^^^^^^^// We can also visualize the plan as a line with markers in RViz./* 5.1 我们还可以将轨迹可视化为带有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");// 6. 移动到目标位姿 (Plan 1)// ^^^^^^^^^^^^^^^^^^^^^//// 移动到一个目标位姿，与以上的步骤是相似的，除了我们现在使用move()函数。// 需要注意的是我们早先设定的目标位姿仍然有效，因此机器人将尝试到达该位姿。// 在这个教程中我们将不会使用这个函数，因为move()是一个阻塞函数，// 需要一个控制器处于激活状态，并且报告轨迹执行是否成功。/* Uncomment below line when working with a real robot *//* move_group.move(); */// 7. 规划到一个关节空间的目标 (Plan 2)// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^//// 7.1 让我们设定一个期望的关节目标并向它移动，这将会替代上面设定的期望位姿。//// 7.2 首先，我们将会创建一个指针指向机器人当前的状态，机器人状态是一个包含当前位置、速度和加速度数据的对象。
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();//// 7.3 接下来获取这个规划组的当前关节值的集合。
std::vector<double> joint_group_positions;/* void copyJointGroupPositions (const JointModelGroup *group, std::vector< double > &gstate) const *//* 对于给定的规划组，以在组中找到变量的顺序，将组成该组的变量的位置值复制到另一个位置。
这不一定是RobotState本身中的连续内存块，因此我们复制而不是返回指针。 *//* 从joint_model_group指向的位置复制到joint_group_positions中 */
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);// 7.4 现在，让我们修改其中的一个关节，规划到新的关节空间目标并可视化这个规划
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");// 7.5 Visualize the plan in RViz
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");// 8. 带路径约束的规划 (Plan 3)// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^//// 8.1 机器人上的一个关节的路径约束是很容易指定的，让我们为规划组指定一个路径约束和期望位姿。// 首先定义路径约束。/* http://docs.ros.org/en/jade/api/moveit_msgs/html/msg/OrientationConstraint.html */
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;// 现在，设定它为规划组的路径约束
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);// 8.2 我们将会复用我们已经规划了的旧目标，需要注意的是只有当前状态满足路径约束才能工作，// 所以我们需要设定到一个新姿态的初始状态// We will reuse the old goal that we had and plan to it.// Note that this will only work if the current state already// satisfies the path constraints. So, we need to set the start// state to a new pose.
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);// 8.3 然后规划从刚才创建的新的初始位姿到早先的目标位姿的路径
move_group.setPoseTarget(target_pose1);// 8.4 带约束的规划可能是很慢的，因为每一个例子必须调用逆运动学求解器。// 让我们把默认的规划时间从5秒钟增加到10秒使得有足够的时间去成功。
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");// 8.5 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./* 8.6 完成路径约束后，请务必清除它。 */
move_group.clearPathConstraints();// 8.7 由于我们设定了初始状态，因此我们必须在规划其他路径前清除掉它// Since we set the start state we have to clear it before planning other paths
move_group.setStartStateToCurrentState();// 9. Cartesian Paths笛卡尔路径 (Plan 4)// ^^^^^^^^^^^^^^^// 9.1 通过直接指定一组末端效应器经过的点，能规划一个笛卡尔路径。需要注意的是我们从上面的新开始状态开始。// 这个初始位姿不需要添加路径点但是添加它能帮助可视化。// You can plan a Cartesian path directly by specifying a list of waypoints// for the end-effector to go through. Note that we are starting// from the new start state above. The initial pose (start state) does not// need to be added to the waypoint list but adding it can help with visualizations
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，第四个路径点// Cartesian motions are frequently needed to be slower for actions such as approach and retreat// grasp motions. Here we demonstrate how to reduce the speed of the robot arm via a scaling factor// of the maxiumum speed of each joint. Note this is not the speed of the end effector point./* 9.2 对于诸如靠近和后退抓取之类的动作，经常需要使笛卡尔动作变慢。 在这里，我们演示了如何通过每个关节的
最大速度的比例因子来降低机器人手臂的速度。 请注意，这不是末端执行器点的速度。 */
move_group.setMaxVelocityScalingFactor(0.1);// We want the Cartesian path to be interpolated at a resolution of 1 cm// which is why we will specify 0.01 as the max step in Cartesian// translation. We will specify the jump threshold as 0.0, effectively disabling it.// Warning - disabling the jump threshold while operating real hardware can cause// large unpredictable motions of redundant joints and could be a safety issue/* 9.3 我们希望以1cm的分辨率插值笛卡尔路径，这就是为什么我们在笛卡尔转换中将0.01指定为最大步长的原因。
我们将跳转阈值指定为0.0，以将其有效地禁用。
警告-在运行实际硬件时禁用跳跃阈值可能会导致冗余关节大量无法预测的运动，并且可能是安全问题 */
moveit_msgs::RobotTrajectory trajectory;constdouble jump_threshold =0.0;constdouble eef_step =0.01;/* 设定笛卡尔路径插值分辨率为0.01米 */double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);/* %.2f是把float输出为2位小数，%%表示输出百分号 */ROS_INFO_NAMED("tutorial","Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction *100.0);// 9.4 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");// 10. Adding/Removing Objects and Attaching/Detaching Objects// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^//// 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. Solid Primitive实体图元
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;/* 图元类型为BOX, 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);// Now, let's add the collision object into the worldROS_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();// Wait for MoveGroup to recieve and process the collision object message
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");// Now when we plan a trajectory it will avoid the obstacle
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);/* cuboid：长方体 */ROS_INFO_NAMED("tutorial","Visualizing plan 5 (pose goal move around cuboid) %s", success ?"":"FAILED");// Visualize the plan in 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");// Now, let's attach the collision object to the robot.ROS_INFO_NAMED("tutorial","Attach the object to the robot");
move_group.attachObject(collision_object.id);// Show text in RViz of status
visual_tools.publishText(text_pose,"Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the ""robot");// Now, let's detach the collision object from the robot.ROS_INFO_NAMED("tutorial","Detach the object from the robot");
move_group.detachObject(collision_object.id);// Show text in RViz of status
visual_tools.publishText(text_pose,"Object dettached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the ""robot");// Now, let's remove the collision object from the world.ROS_INFO_NAMED("tutorial","Remove the object from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);// Show text in RViz of status
visual_tools.publishText(text_pose,"Object removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");// END_TUTORIAL
ros::shutdown();/* 手动关闭节点 */return0;}