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

Demo2 Descartes Planning and Excution

 

2.2 Load Parameters

src/tasks/load_parameters.cpp

Goal1：

/* Fill Code:

* Goal:

* - Create a private handle by passing the "~" string to its constructor

* Hint:

* - Replace the string in ph below with "~" to make it a private node.

*/

ros::NodeHandle ph("[ COMPLETE HERE ]");

代码：

ros::NodeHandle ph("~");

Goal2:

/* Fill Code:

* Goal:

* - Read missing parameters 'tip_link' and 'world_frame' from ros parameter server

*/

if(ph.getParam("group_name",config_.group_name) &&

 

ph.getParam("[ COMPLETE HERE ]",config_.tip_link) &&

 

ph.getParam("base_link",config_.base_link) &&

 

ph.getParam("[ COMPLETE HERE ]",config_.world_frame) &&

代码：将[ COMPLETE HERE ]分别替换为tip_link和world_frame。

2.3 Initialize ROS

src/tasks/init_ros.cpp

Goal1:

/* Fill Code:

* Goal:

* - Create a "moveit_msgs::ExecuteTrajectoryAction" client and assign it to the "moveit_run_path_client_ptr_"

* application variable.

* - Uncomment the action client initialization code.

* Hint:

* - Enter the action type moveit_msgs::ExecuteTrajectoryAction in between the "< >" arrow brackets of

* the client type definition, replacing the placeholder type.

*/

typedef actionlib::SimpleActionClient< bool /* [ COMPLETE_HERE ] */ > client_type;

moveit_run_path_client_ptr_; /* = std::make_shared<client_type>(EXECUTE_TRAJECTORY_ACTION,true); */

主要是创建一个moveit_msgs::ExecuteTrajectoryAction客户端，客户端变量是moveit_run_path_client_ptr_。提示是在<>中输入moveit_msgs::ExecuteTrajectoryAction。代码：

typedef actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> client_type;

moveit_run_path_client_ptr_=std::make_shared<client_type>(EXECUTE_TRAJECTORY_ACTION,true);

2.4 Initialize Descartes

src/tasks/init_descartes.cpp

Goal1:

/* Fill Code:

* Goal:

* - Initialize the "robot_model_ptr" variable by passing the required application parameters

* into its "initialize" method.

* Hint:

* - The config_ structure contains the variables needed by the robot model

* - The "initialize" method takes the following arguments in this order

* a - robot description string

* b - group_name string.

* c - world_frame string

* d - tip_link string.

*/

if(robot_model_ptr_->initialize(ROBOT_DESCRIPTION_PARAM,

"[ COMPLETE HERE ]",

"[ COMPLETE HERE ]",

"[ COMPLETE HERE ]"))

这一步写的时候一定看一下ur5_demo.srdf中对各个link的命名，不然会提示找不到对应的关节。代码：

if(robot_model_ptr_->initialize(ROBOT_DESCRIPTION_PARAM,

"manipulator",

"world",

"tool"))

Goal2：

/* Fill Code:

* Goal:

* - Initialize the Descartes path planner by calling "planner_.initialize(...)".

* - Pass the robot_model_ptr_ created earlier into the initialize method and save the result

* into the "succeeded" variable.

*/

bool succeeded = false /* [ COMPLETE HERE ]*/;

主要是将planner_进行初始化，使用它的initialize()方法。提示是使用robot_model_ptr_作为model传入initialize()，并将初始化状态存在succeeded变量中。代码：

bool succeeded = planner_.initialize(robot_model_ptr_);

2.5 Move Home

src/tasks/move_home.cpp

Goal1:

/* Fill Code:

* Goal:

* - Call the move_group.move() and save the returned flag into the "result" variable

* - Verify that the robot reached the goal.

* Hint:

* - The "result.val" and "result.SUCCESS" flags can be used to verify that the move was completed

* -

*/

moveit_msgs::MoveItErrorCodes result /* [ COMPLETE HERE ]: = move_group.??() ;*/;

if(false /* [ COMPLETE HERE ]: result.?? != result.?? */)

调用move_group.move()方法并将其返回值存储在result中。提示是result.val和result.SUCCESS可以用来判断移动是否已经完成。代码：

moveit_msgs::MoveItErrorCodes result = move_group.move();

if(result.val != result.SUCCESS)

2.6 Generate a Semi-Constrained Trajectory

src/tasks/generate_trajectory.cpp

Goal1:

/* Fill Code:

* Goal:

* - Create AxialSymetricPt objects in order to define a trajectory cartesian point with

* rotational freedom about the tool's z axis.

*

* Hint:

* - The point can be constructed as follows:

*

* new AxialSymmetricPt(Pose ,Increment, Free Axis)

*

* - The Pose can be found in the for loop's "pose" variable.

* - The Increment can be found in the "ORIENTATION_INCREMENT" global variable.

* - The Free Axis can be selected from the AxialSymmetricPt::FreeAxis::Z_AXIS enumeration constants.

*

*/

descartes_core::TrajectoryPtPtr pt = descartes_core::TrajectoryPtPtr(/*[ COMPLETE HERE ]:

new descartes_trajectory::AxialSymmetricPt( ?? , ?? , descartes_trajectory::AxialSymmetricPt::FreeAxis::Z_AXIS) */);

提示给的很明确了。代码：

descartes_core::TrajectoryPtPtr pt =

descartes_core::TrajectoryPtPtr(new AxialSymmetricPt(pose,

ORIENTATION_INCREMENT,

AxialSymmetricPt::FreeAxis::Z_AXIS));

2.7 Plan a Robot Path

src/tasks/plan_path.cpp

Goal1:

/* Fill Code:

* Goal:

* - Call the "planner_.planPath(...)" method in order to plan a robot path from the trajectory.

* - Save the result of the planPath(...) call into the succeeded variable in order to verify that

* a valid robot path was generated.

* Hint:

* - The "planner_.planPath(...)" can take the "input_traj" Trajectory as an input argument.

*/

bool succeeded = false /* [ COMPLETE HERE ]*/;

调用planner_.planPath()方法从轨迹规划机器人路径，并将规划状态存储到succeeded变量中。提示可用input_traj作为函数的输入。代码：

bool succeeded = planner_.planPath(input_traj);

Goal2:

/* Fill Code:

* Goal:

* - Call the "planner_.getPath(...)" in order to retrieve the planned robot path.

* - Save the result of the planPath(...) call into the succeeded variable in order to verify that

* a valid robot path was generated.

* Hint:

* - The "planner_.getPath(...)" can take the "output_path" variable as an output argument.

*/

succeeded = false /* [ COMPLETE HERE ]*/;

同Goal1。代码：

succeeded = planner_.getPath(output_path);

2.8 Run a Robot Path

src/tasks/run_path.cpp

Goal1:

/* Fill Code:

* Goal:

* - Retrieve the first point in the path.

* - Save the joint values of the first point into "start_pose".

* Hint:

* - The first argument to "getNominalJointPose()" is a "seed" joint pose, used to find a nearby IK solution

* - The last argument to "getNominalJointPose()" is a "std::vector<double>" variable

* where the joint values are to be stored.

*/

std::vector<double> seed_pose(robot_model_ptr_->getDOF());

std::vector<double> start_pose;

 

descartes_core::TrajectoryPtPtr first_point_ptr /* [ COMPLETE HERE ]: = path[??]*/;

/*[ COMPLETE HERE ]: first_point_ptr->getNominalJointPose(??,*robot_model_ptr_,??); */

取回路径中的第一个点，并将这个点的joint值存储进变量start_pose中。代码：

descartes_core::TrajectoryPtPtr first_point_ptr=path[0];

first_point_ptr.getNominalJointPose(seed_pose, *robot_model_ptr_, start_pose);

Goal2:

/* Fill Code:

* Goal:

* - Complete the action goal by placing the "moveit_msgs::RobotTrajectory" trajectory in the goal object

* - Use the action client to send the trajectory for execution.

* Hint:

* - The "goal.trajectory" can be assigned the Moveit trajectory.

* - The "moveit_run_path_client_ptr_->sendGoalAndWait(goal)" sends a trajectory execution request.

*/

moveit_msgs::ExecuteTrajectoryGoal goal;

goal.trajectory ; /* [ COMPLETE HERE ]: = ?? */;

 

ROS_INFO_STREAM("Robot path sent for execution");

if(false /* [ COMPLETE HERE ]: moveit_run_path_client_ptr_->??( ?? ) == actionlib:SimpleClientGoalState::SUCCEEDED*/)

为前往目标点的路径赋值，并使用action的客户端发送轨迹信息来执行任务。代码：

code1:

goal.trajectory = moveit_traj;

code2:

if(moveit_run_path_client_ptr_->sendGoalAndWait(goal) == actionlib::SimpleClientGoalState::SUCCEEDED)