Demo1
最近按老师要求在看ros industrial相关的知识，打算先跟着官方教程走一遍，由于对ros还不是特别熟悉，在进行demo代码补全时遇到许多问题，主要是英文不过关，有一些地方提示没能理解，浪费了许多时间，在此用中文记录一下思路以及每个部分的答案（可能是）。
1.5 Move Arm to Wait Position
相关文件位于src/tasks/move_to_wait_position.cpp。要求如下：
目标1：
/* Fill Code:
* Goal:
* - Set robot wait target
* Hints:
* - Use the "setNamedTarget" method in the "move_group_ptr" object.
* - Look in the "cfg.WAIT_POSE_NAME" object for the name of the target.
*/
/* ======== ENTER CODE HERE ======== */
主要目标是设置机械臂的起始位置，提示是使用move_group_ptr中的setNamedTarget()方法，在文章下面有相关的函数信息点击进入可查看所需相关参数，WAIT_POSE_NAME其实是一个定义好的常量，在pick_and_place_utilities.h中都有定义，代码如下：
move_group_ptr->setNamedTarget(cfg.WAIT_POSE_NAME);
目标2：
/* Fill Code:
* Goal:
* - Move the robot
* Hints:
* - Use the "move" method in the "move_group_ptr" object and save the result
* in the "error" variable
*/
/* ======== ENTER CODE HERE ======== */
主要目标是控制机械臂移动，同样使用move_group_ptr中的move()方法，并将结果存储在error变量内。代码如下：
error = move_group_ptr->move();
1.6 Open Gripper
相关函数位于src/tasks/set_gripper.cpp。目标如下：
目标1：
/* Fill Code:
* Goal:
* - Send the grasp goal to the server.
* Hints:
* - Use the "sendGoal" method of the grasp client "grasp_action_client_ptr"
* to make a call to the server.
*/
/* ======== ENTER CODE HERE ======== */
主要是设置抓取目标，并将这个目标发送给server。提示是使用抓取客户端grasp_action_client_ptr中的sendoal()方法将抓取目标相关信息发送给server。代码如下：
grasp_action_client_ptr->sendGoal(grasp_goal);
目标2：
/* Fill Code:
* Goal:
* - Confirm that client service call succeeded.
* Hints:
* - Use the "waitForResult" method of the client to wait for completion.
* - Give "waitForResult" a timeout value of 4 seconds
* - Timeouts in ros can be created using "ros::Duration(4.0f)".
* - Save returned boolean from waitForResult() in the "success" variable.
*/
/* ======== ENTER CODE HERE ======== */
主要是确认客户端服务是否正确接收到发送去的抓取目标。提示是使用客户端的waitForResult()方法来告诉客户端等待4秒来接收抓取目标信息，并将结果存储到success变量中，按照函数描述的所需参数加入变量即可。代码如下：
success = grasp_action_client_ptr->waitForResult(ros::Duration(4.0f));
1.7 Detect Box Pick Point
相关函数位于/src/tasks/detect_box_pick.cpp。目标如下：
目标1：
/* Fill Code:
* Goal:
* - Call target recognition service and save results.
* Hint:
* - Use the service response member to access the
* detected pose "srv.response.target_pose".
* - Assign the target_pose in the response to the box_pose variable in
* order to save the results.
*/
geometry_msgs::Pose box_pose;
if(/* ======== ENTER CODE HERE ======== (replace "true")*/)
{
if(srv.response.succeeded)
{
/* ======== ENTER CODE HERE ======== */
目标是调用目标检测服务并存储调用状态（是否成功调用）。提示是使用服务响应的函数来获取检测目标的位姿srv.response.target_pose，并且将获取到的目标位姿存储到变量box_pose中。既然是调用服务，那肯定是使用客户端来请求服务，问题就在于是哪一个客户端请求服务，通过查看pick_and_place_node.cpp中的代码可知有关目标检测的客户端节点是target_recognition_client，这一点在官方的tutorial中也给出了提示，因此我们直接调用target_recognition_client的call()方法对相应的srv进行调用即可，并将target_pose存储到box_pose中。代码如下：
code1:
target_recognition_client.call(srv)
code2:
box_pose = srv.response.target_pose;
1.8 Create Pick Moves
相关函数在src/tasks/create_pick_moves.cpp中，目标如下：
目标1：
/* Fill Code:
* Goal:
* - Create tcp pose at box pick.
* Hints:
* - Use the "setOrigin" to set the position of "world_to_tcp_tf".
*/
tf::poseMsgToTF(box_pose,world_to_box_tf);
tf::Vector3 box_position(box_pose.position.x, box_pose.position.y, box_pose.position.z);
/* ======== ENTER CODE HERE ======== */
主要是创建抓取box的tcp位姿。提示是使用world_to_tcp_tf的setOrigin()方法。看函数名可知我们想得到在世界坐标系下的tcp位姿，那我们就要先获得一个位置点，而这个点就是要抓取的物体的Origin点，我们可以在上面的代码中看到world_to_tcp_tf是tf::Transform数据类型，然后我们看到这个数据类型中的setOrigin()方法，就可知道需要使用world_to_box_tf的getOrigin()方法先获得抓取物体的Origin点，在将其赋予world_to_tcp_tf。其实也可参照代码下方给出的setRotation()方法的使用，进而推断出setOrigin()的使用方法。代码如下：
world_to_tcp_tf.setOrigin(world_to_box_tf.getOrigin());
目标2：
/* Fill Code:
* Goal:
* - Find transform from tcp to wrist.
* Hints:
* - Use the "lookupTransform" method in the transform listener.
*/
transform_listener_ptr->waitForTransform(cfg.TCP_LINK_NAME, cfg.WRIST_LINK_NAME,ros::Time::now(),ros::Duration(3.0f));
/* ======== ENTER CODE HERE ======== */
主要是找到从tcp位姿到wrist位姿的转换，类似于转移矩阵。提示是使用transform listener中的lookupTransform()方法，通过观察上方的代码可以知道transform listener的变量是transform_listener_ptr，然后再去看lookupransform()的相关信息，可以看到这个函数最少需要4个参数，分别是两个string数据target_frame和source_frame、一个时间ros::Time以及一个StampedTransform数据，通过观察waitForTransform()方法我们可以发现target_frame和source_frame分别是TCP_LINK_NAME和WRIST_LINK_NAME，而在此文件中只有一个StampedTransform类型的数据tcp_to_wrist_tf，因此可得到该函数的所有输入。代码如下：
transform_listener_ptr->lookupTransform(cfg.TCP_LINK_NAME, cfg.WRIST_LINK_NAME, ros::Time::now(), tcp_to_wrist_tf);
目标3：
/* Fill Code:
* Goal:
* - Transform list of pick poses from tcp frame to wrist frame
* Hint:
* - Use the "transform_from_tcp_to_wrist" function and save results into
* "wrist_pick_poses".
* - The "tcp_to_wrist_tf" is the transform that will help convert "tcp_pick_poses"
* into "wrist_pick_poses".
*/
/* ======== ENTER CODE HERE ======== */
主要是将一系列抓取位姿从tcp坐标转换到wrist坐标。提示是使用transform_from_tcp_to_wrist()方法并将获取到的位姿存储到wrist_pick_poses中，并且可通过tco_to_wrist_tf将tcp_pick_poses转换到wrist_pick_poses（类似于转移矩阵）。而transform_from_tcp_to_wrist()这个方法我们可以在pick_and_place_utilities.h中找到定义，并在pick_and_place_utilities.cpp中找到实现，然后根据所需参数设置输入即可。代码如下：
wrist_pick_poses = transform_from_tcp_to_wrist(tcp_to_wrist_tf,tcp_pick_poses);
1.9 Pick Up Box
函数位于src/tasks/pickup_box.cpp。目标如下：
目标1：
/* Fill Code:
* Goal:
* - Set the wrist as the end-effector link
* - The robot will try to move this link to the specified pose
* - If not specified, moveit will use the last link in the arm group.
* Hints:
* - Use the "setEndEffectorLink" in the "move_group_ptr" object.
* - The WRIST_LINK_NAME field in the "cfg" configuration member contains
* the name for the arm's wrist link.
*/
/* ======== ENTER CODE HERE ======== */
主要是将关节设置为end-effector link(eelink)。提示是使用move_group_ptr中的setEndEffectorLink()方法，并使用WRIST_LINK_NAME作为输入。代码如下：
move_group_ptr->setEndEffectorLink(cfg.WRIST_LINK_NAME);
目标2：
/* Fill Code:
* Goal:
* - Set world frame as the reference
* - The target position is specified relative to this frame
* - If not specified, MoveIt will use the parent frame of the SRDF "Virtual Joint"
* Hints:
* - Use the "setPoseReferenceFrame" in the "move_group_ptr" object.
* - The WORLD_FRAME_ID in the "cfg" configuration member contains the name
* for the reference frame.
*/
/* ======== ENTER CODE HERE ======== */
与目标1大致相似，使用方法也一样。代码如下：
move_group_ptr->setPoseReferenceFrame(cfg.WORLD_FRAME_ID);
目标3：
/* Fill Code:
* Goal:
* - Turn on gripper suction after approach pose is reached.
* Hints:
* - Call the "set_gripper" function to turn on suction.
* - The input to the set_gripper method takes a "true" or "false"
* boolean argument.
*/
/* ======== ENTER CODE HERE ======== */
主要是控制gripper在靠近目标位姿后进行开启。这个方法我们在1.6中使用过，不再细说。代码如下：
set_gripper(true);
1.10 Create Place Moves
函数位于/src/tasks/create_place_moves.cpp。目标如下：
目标1：
/* Fill Code:
* Objective:
* - Find the desired tcp pose at box place
* Hints:
* - Use the "setOrigin" method to set the position of "world_to_tcp_tf"
* using cfg.BOX_PLACE_TF.
* - cfg.BOX_PLACE_TF is a tf::Transform object so it provides a getOrigin() method.
*/
/* ======== ENTER CODE HERE ======== */
主要是找到box位置的期望tcp位姿。提示是使用world_to_tcp_tf中的setOrigin()方法，这次是使用BOX_PLACE_TF所给出的box的位置信息。代码如下
world_to_tcp_tf.setOrigin(cfg.BOX_PLACE_TF.getOrigin());
目标2：
/* Fill Code:
* Goal:
* - Reorient the tool so that the tcp points towards the box.
* Hints:
* - Use the "setRotation" to set the orientation of "world_to_tcp_tf".
* - The quaternion value "tf::Quaternion(0.707, 0.707, 0, 0)" will point
* the tcp's direction towards the box.
*/
/* ======== ENTER CODE HERE ======== */
主要是调整机械臂使得tcp点朝向box。提示是使用world_to_tcp_tf的setRotation()方法，四元组的值也已给出，函数具体用法参照1.8。代码如下：
world_to_tcp_tf.setRotation(world_to_tcp_tf.getRotation()*tf::Quaternion(0.707, 0.707, 0, 0));
目标2：
/* Fill Code:
* Goal:
* - Create place poses for tcp. *
* Hints:
* - Use the "create_manipulation_poses" and save results to "tcp_place_poses".
* - Look in the "cfg" object to find the corresponding retreat and approach distance
* values.
*/
/* ======== ENTER CODE HERE ======== */
主要是创建放置位置的tcp坐标。提示是使用create_manipulation_poses()并将结果存储在tcp_place_poses变量中，并在cfg中寻找corresponding retreat和approach distance，其用法在1.8中的文件里有用到过，或者直接去pick_and_place_utilities.cpp中查看其实现。代码如下：
tcp_place_poses = create_manipulation_poses(cfg.RETREAT_DISTANCE, cfg.APPROACH_DISTANCE, world_to_tcp_tf);
目标3：
/* Fill Code:
* Goal:
* - Find transform from tcp to wrist.
* Hints:
* - Use the "lookupTransform" method in the transform listener.
* */
transform_listener_ptr->waitForTransform(cfg.TCP_LINK_NAME, cfg.WRIST_LINK_NAME, ros::Time(0.0f), ros::Duration(3.0f));
/* ======== ENTER CODE HERE ======== */
与1.8目标2相同。代码如下：
transform_listener_ptr->lookupTransform(cfg.TCP_LINK_NAME, cfg.WRIST_LINK_NAME, ros::Time(0.0f), tcp_to_wrist_tf);
目标4：
/* Fill Code:
* Goal:
* - Transform list of place poses from the tcp to the wrist coordinate frame.
* Hints:
* - Use the "transform_from_tcp_to_wrist" function and save results into
* "wrist_place_poses".
* - The "tcp_to_wrist_tf" is the transform that will help convert "tcp_place_poses"
* into "wrist_place_poses".
*/
/* ======== ENTER CODE HERE ======== */
与1.8目标3相同。代码如下：
wrist_place_poses = transform_from_tcp_to_wrist(tcp_to_wrist_tf, tcp_place_poses);
1.11 Place Box
函数位于src/tasks/place_box.cpp。目标如下：
/* Fill Code:
* Goal:
* - Set the ReferenceFrame and EndEffectorLink
* Hints:
* - Use the "setEndEffectorLink" and "setPoseReferenceFrame" methods of "move_group_ptr"
*/
/* ======== ENTER CODE HERE ======== */
/* ======== ENTER CODE HERE ======== */
主要是设置ReferenceFrame和EndEffectorLink，方法与1.9中的目标1、2类似。代码如下：
code1:
move_group_ptr->setEndEffectorLink(cfg.WRIST_LINK_NAME);
code2:
move_group_ptr->setPoseReferenceFrame(cfg.WORLD_FRAME_ID);
目标2：
/* Fill Code:
* Goal:
* - Turn off gripper suction after the release pose is reached.
* Hints:
* - Call the "set_gripper" function to turn on suction.
* - The input to the set_gripper method takes a "true" or "false"
* boolean argument.
*/
/* ======== ENTER CODE HERE ======== */
与1.9中的目标3类似，只不过是关闭gripper。代码如下：
set_gripper(false);
PS:
以上代码均可编译通过，得到的输出也与教程一致，但最后无法规划出到达放置目标点位姿的路径，清楚原因的大佬欢迎指出问题！