ros-industrial tutorial Training[melodic] demo代码补全记录（demo1）

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:

以上代码均可编译通过，得到的输出也与教程一致，但最后无法规划出到达放置目标点位姿的路径，清楚原因的大佬欢迎指出问题！