ROS Industrial教程(五)

一、通过C++进行运动规划

更改myworkcell_core节点，使之根据service call的vision node的发布，将机器人的工具框移动到零件位置的中心。

1. 修改myworkcell_node.cpp文件

1.1. 添加#include <tf/tf.h>以允许访问tf库（用于框架转换/应用）。

·之前已经添加了对tf包的依赖。

1.2. 在ScanNPlan类的start方法中，使用LocalizePart服务的响应来初始化新的move_目标变量：在scannplan类的start方法中，使用localizepart服务的响应来初始化新的move_target变量：

geometry_msgs::Pose move_target = srv.response.pose;

·确保在调用vision_node服务后放置此代码。

2. 使用MoveGroupInterface计划/执行到move_target位置的移动：

2.1. 为了使用MoveGroupInterface类，有必要添加moveit_ros_planning_interface程序包作为myworkcell_core程序包的依赖项。通过修改程序包的CMakeLists.txt（2行）和package.xml（1行）来添加moveit_ros_planning_interface依赖项。

 

2.2. 添加适当的“ include”引用以允许使用MoveGroupInterface：

#include <moveit/move_group_interface/move_group_interface.h>

 

2.3. 在ScanNPlan类的start()方法中创建一个moveit::planning_interface::MoveGroupInterface对象。 它具有单个构造函数，该构造函数采用您在创建工作单元moveit程序包时定义的计划组的名称（“manipulator”）。

 

2.4. 使用move_group对象的setPoseTarget函数设置所需的笛卡尔目标位置。 调用对象的move()函数以计划并执行向目标位置的移动。

// Plan for robot to move to part

move_group.setPoseReferenceFrame(base_frame);

move_group.setPoseTarget(move_target);

move_group.move();

 

2.5. move_group.move()命令要求使用“异步”微调器，以允许在阻塞move()命令期间处理ROS消息。 在ros::init(argc, argv, "myworkcell_node")之后在main()例程的开始附近初始化微调器，并用ros::waitForShutdown()替换现有的ros::spin()命令，如下所示：

ros::AsyncSpinner async_spinner(1);

async_spinner.start();

...

ros::waitForShutdown();

 

3. 测试运行

catkin build

roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch

roslaunch myworkcell_support workcell.launch

 

4. 更多

·在RViz中，添加主题“ / ar_pose_visual”的“标记Marker”显示，以确认机器人的最终位置与fake_ar_publisher发布的位置匹配

·尝试更新workcell_node的start方法，使其在移至AR_target位置后自动移回allZeros位置。 有关move group可用方法的列表，请参见此处。

可使用bool setNamedTarget (const std::string &name)方法，将当前关节值设置为RememberJointValues（）先前记住的值，或者，如果找不到，则设置为在SRDF中以名称name命名的组状态（此例中为“allZeros”，组状态可在创建moveIt包时添加）。在start函数中添加如下代码：

    if(move_group.setNamedTarget ("allZeros"))

      {

        ROS_INFO("setNamedTarget Success!");

        move_group.move();

      }

    else

      ROS_INFO("setNamedTarget failed!");

 

·在最终移动到目标之前，尝试移动到距离目标位置几英寸远的“接近位置approach position”。

       可通过直接设置目标姿态的坐标和指向实现。

    move_target.orientation.x = 0;

    move_target.orientation.y = 1;

    move_target.orientation.z = 0;

    move_target.orientation.w = 0;

    move_target.position.x = 0.35;

    move_target.position.y = -0.3;

    move_target.position.z = 0.8;

    move_group.setPoseTarget(move_target);

    move_group.move();

 

 

 

二、笛卡尔Descartes路径规划简介

MoveIt！是一个主要用于执行“自由空间”运动的框架，该框架的目标是将机器人从A点移动到B点。 这些类型的问题只是频繁执行任务的一部分，例如焊接、喷漆、激光切割。这些过程中十分重视工具在机器人整个工作期间所指向的位置。

接下来将介绍笛卡尔，一种“直角坐标”运动规划器，用于沿某个过程路径移动机器人。其具有以下特性：

·可获得全局最优解（对于特定的搜索分辨率而言）。

·可以搜索问题中的多余自由度（例如有7个机器人关节，或者过程中工具的Z轴旋转无关紧要）。

接下来将建立一个新的节点，功能为：

(1). 通过ROS服务将标记的名义姿态作为输入。

(2). 产生一条关节轨迹，该轨迹命令机器人跟踪标记物的周边（就像它正在喷漆一样）。

 

为了节省时间，文件descartes_node.cpp已定义，该文件：

(1). 为笛卡尔路径规划定义一个新的节点和伴随类。

(2). 定义实际服务并初始化笛卡尔库。

(3). 提供高级工作流程（请参阅planPath函数）。

剩下需要完成的是：

(1). 定义一系列构成机器人“路径”的笛卡尔姿势。

(2). 将这些路径转化为笛卡尔可以理解的东西。

 

配置工作空间

1. 将笛卡尔库拷贝到工作区中

cd ~/catkin_ws/src

git clone -b melodic-devel https://github.com/ros-industrial-consortium/descartes.git

 

2. 将ur5_demo_descartes包复制到工作区中

cp -r ~/industrial_training/exercises/4.1/src/ur5_demo_descartes .

 

3. 将descartes_node_unfinished.cpp复制到核心软件包的src /文件夹中，并将其重命名为descartes_node.cpp。

cp ~/industrial_training/exercises/4.1/src/descartes_node_unfinished.cpp myworkcell_core/src/descartes_node.cpp

 

4.在CMakeLists.txt & package.xml文件中添加依赖

• ur5_demo_descartes
• descartes_trajectory
• descartes_planner
• descartes_utilities
• eigen_conversions

5. 在myworkcell_core程序包的CMakeLists.txt中创建规则，以建立一个名为descartes_node的新节点。

add_executable(descartes_node src/descartes_node.cpp)

add_dependencies(descartes_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(descartes_node ${catkin_LIBRARIES})

 

完成Descartes节点

       需要创建一个服务接口来执行笛卡尔计划算法。

1. 在myworkcell_core软件包的srv/目录中定义一个名为PlanCartesianPath.srv的新服务。 该服务获取目标中心位置并计算关节轨迹以跟踪目标边缘。

# request

geometry_msgs/Pose pose

 

---

 

# response

trajectory_msgs/JointTrajectory trajectory

 

2. 将新创建的服务文件添加到程序包CMakeLists.txt中的add_service_file()规则中。

 

3. 由于新服务引用了另一个包中的消息类型，因此我们需要将该另一个包（trajectory_msgs）作为依赖项添加到myworkcell_core 的 CMakeLists.txt（3行）和package.xml（1行）文件中。

 

4. 查看descartes_node.cpp以了解代码结构。 特别是，planPath方法概述了步骤的主要顺序。

 

1.   

·在makeToolPoses中，生成跟踪“ AR标记”目标零件外部的路径的其余3边。

我们在makeToolPoses()中定义的路径是相对于您正在使用的零件上某个已知参考点的。 因此，（0，0，0）的刀具姿态将完全位于参考点，而不是世界坐标系的原点。

 

     

·在makeDescartesTrajectory中，将您创建的路径转换为“笛卡尔轨迹”，一次转换一个点。（不要忘记通过指定的参考姿势来变换每个标称点：ref * point）

在makeDescartesTrajectorty(...)中，我们需要使用“ ref”姿势将相对工具姿势转换为世界坐标。

      

·在makeTolerancedCartesianPoint中，根据给定的姿势创建一个new AxialSymmetricPt。（有关此点类型的更多文档，请参见此处。允许点绕Z轴对称（AxialSymmetricPt::Z_AXIS），增量为90度（PI / 2弧度） ）

在makeTolerancedCartesianPoint(...)中，考虑此文档以了解常见的关节轨迹点的具体实现：

 

6.Build & Debug

 

更新Workcell节点

完成笛卡尔节点后，我们现在想通过向主工作单元节点添加新的ServiceClient来调用其逻辑。 该服务的结果是我们必须在机器人上执行的关节轨迹。 这可以通过许多方式来完成。 在这里，我们将直接调用JointTrajectoryAction。

1. 在myworkcell_node.cpp中，添加头文件include语句：

#include <actionlib/client/simple_action_client.h>

#include <control_msgs/FollowJointTrajectoryAction.h>

#include <myworkcell_core/PlanCartesianPath.h>

无需为这些库/消息添加新的依赖项，因为它们是从moveit传递而来的。

 

2. 在ScanNPlan类中，添加新的私有成员变量：PlanCartesianPath服务的ServiceClient和FollowJointTrajectoryAction的操作客户端：

ros::ServiceClient cartesian_client_;

actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac_;

 

3. 在构造函数中初始化这些新对象。 注意，动作客户端必须在所谓的构造函数初始化列表中初始化。

ScanNPlan(ros::NodeHandle& nh) : ac_("joint_trajectory_action", true)

{

  // ... code

  cartesian_client_ = nh.serviceClient<myworkcell_core::PlanCartesianPath>("plan_path");

}

 

4. 在start（）函数的结尾，创建一个新的笛卡尔服务并发出服务请求：

// Plan cartesian path

myworkcell_core::PlanCartesianPath cartesian_srv;

cartesian_srv.request.pose = move_target;

if (!cartesian_client_.call(cartesian_srv))

{

  ROS_ERROR("Could not plan for path");

  return;

}

 

5. 继续添加以下行，以通过直接将动作发送到动作服务器（绕过MoveIt）来执行该路径：

// Execute descartes-planned path directly (bypassing MoveIt)

ROS_INFO("Got cart path, executing");

control_msgs::FollowJointTrajectoryGoal goal;

goal.trajectory = cartesian_srv.response.trajectory;

ac_.sendGoal(goal);

ac_.waitForResult();

ROS_INFO("Done");

 

6.Build & Debug

 

 

测试

1. 创建一个新的setup.launch文件（在workcell_support程序包中），该文件会打开除workcell_node之外的所有内容：

<include file="$(find myworkcell_moveit_config)/launch/myworkcell_planning_execution.launch"/>

<node name="fake_ar_publisher" pkg="fake_ar_publisher" type="fake_ar_publisher_node" />

<node name="vision_node" type="vision_node" pkg="myworkcell_core" output="screen"/>

<node name="descartes_node" type="descartes_node" pkg="myworkcell_core" output="screen"/>

 

2.运行setup文件，然后运行workcell节点

roslaunch myworkcell_support setup.launch

rosrun myworkcell_core myworkcell_node

（为了看清运动过程，在rviz中禁用循环动画）