本文主要是介绍ROS Industrial 软件包_笛卡尔路径规划器_实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
笛卡尔(Descarte)路径规划
使用笛卡尔包
使用笛卡尔包需要软件开发人员创建三个对象Obiects：
一个机器人模型(a robot model)，将用来计算正向运动学和逆向运动学。
一个轨迹点的轨迹(a trajectory of trajectory points)，用于定义路径。
一个规划器(a planner)，将使用提供的机器人模型来完成沿着轨迹寻找有效路线的工作。
代码解读
源码地址
在descartes_tutorials/src/中，tutorial1.cpp是一个单个文件程序，该程序定义了一条简单的轨迹，使用Moveit创建了机器人模型，规划了轨迹并执行了它。
顶部是几个函数声明，源代码将在下文中详细解读。
1 // Core ros functionality like ros::init and spin
2 #include <ros/ros.h>
3 // ROS Trajectory Action server definition
4 #include <control_msgs/FollowJointTrajectoryAction.h>
5 // Means by which we communicate with above action-server
6 #include <actionlib/client/simple_action_client.h>
7
头文件的第一部分包含了打印，定义轨迹和发送这些轨迹所需的基本ROS组件。 ROS-Industrial支持的大多数机器人都为仿真模拟和实际硬件提供了FollowJointTrajectory动作(action)接口。
8 // Includes the descartes robot model we will be using
9 #include <descartes_moveit/moveit_state_adapter.h>
10 // Includes the descartes trajectory type we will be using
11 #include <descartes_trajectory/axial_symmetric_pt.h>
12 #include <descartes_trajectory/cart_trajectory_pt.h>
13 // Includes the planner we will be using
14 #include <descartes_planner/dense_planner.h>
第二批标题头包含我们生成轨迹所需的所有笛卡尔特定文件。 moveit_state_adapter.h定义了笛卡尔机器人模型(RobotModel)，而descartes_trajectory中的两个文件定义了轨迹点(Trajectory Points)的特定类型。 最后，densage_planner.h提供对笛卡尔规划器的访问。 有关特定组件的更多信息，请参见笛卡尔（descartes）。
46 {
47 // Initialize ROS
48 ros::init(argc, argv, "descartes_tutorial");
49 ros::NodeHandle nh;
50
51 // Required for communication with moveit components
52 ros::AsyncSpinner spinner (1);
53 spinner.start();
在使用ROS组件之前，我们必须通过调用ros :: init初始化节点。 我还将在此处创建一个NodeHandle，该节点将稍后用于从ROS参数服务器中检索组件。
第51-52行创建一个新线程来处理在执行笛卡尔时可能需要处理的所有回调。 在没有微调器(spinner)的情况下初始化MoveIt似乎会引起问题。
54 // 1. Define sequence of points
55 TrajectoryVec points;
56 for (unsigned int i = 0; i < 10; ++i)
57 {
58 Eigen::Affine3d pose;
59 pose = Eigen::Translation3d(0.0, 0.0, 1.0 + 0.05 * i);
60 descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
61 points.push_back(pt);
62 }
63
64 for (unsigned int i = 0; i < 5; ++i)
65 {
66 Eigen::Affine3d pose;
67 pose = Eigen::Translation3d(0.0, 0.04 * i, 1.3);
68 descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
69 points.push_back(pt);
70 }
在这里，我们使用本征变换(Eigen transformations)定义了一系列轨迹点。 这些转换定义了末端执行器的向上运动，然后是侧向平移。
makeTolerancedCartesianPoint函数是用于生成笛卡尔点的辅助函数，该笛卡尔点绕末端执行器框架(end effector frame)的z轴的旋转不受限制。 求解图(最短路径)时，笛卡尔将尝试通过实质上围绕z轴旋转目标姿态来搜索有效的IK解。
makeTolerancedCartesianPoint的定义很简单，将在稍后讨论。
72 // 2. Create a robot model and initialize it
73 descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);
74
75 // Name of description on parameter server. Typically just "robot_description".
76 const std::string robot_description = "robot_description";
77
78 // name of the kinematic group you defined when running MoveitSetupAssistant
79 const std::string group_name = "manipulator";
80
81 // Name of frame in which you are expressing poses.
82 // Typically "world_frame" or "base_link".
83 const std::string world_frame = "base_link";
84
85 // tool center point frame (name of link associated with tool)
86 const std::string tcp_frame = "tool0";
87
88 if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
89 {
90 ROS_INFO("Could not initialize robot model");
91 return -1;
在此代码块中，我们通过提供有关我们要控制的机器人的信息来创建和初始化机器人模型。使用提供的moveit_state_adapter的先决条件是，在运行此示例之前，必须已运行Moveit设置助手并至少创建了一个运动学组。
接下来，代码从ROS参数服务器中搜索robot_description参数以获取对机器人（或包含机器人的系统）的描述。group_name参数应与运行Moveit设置助手时创建的运动组的名称匹配。默认的ROS-Industrial软件包通常使用'manipulator'作为其默认组名。
world_frame参数指定您传入的位置姿势的参考框架。如果要指定相对于某个预定原点的全局位置，则该原点的名称是world_frame的参数（它也是URDF中的根框架）。如果这些点是相对于机器人基座或其他固定框架表示的，则输入基础链接(base link)或您要使用的任何其他框架的名称。
tcp_frame或工具中心点框架(tool center point frame)是计算正向和反向运动学时使用的参考框架。它几乎始终是操纵器组中的最后一个链接（与名称group_name关联）。
94 // 3. Create a planner and initialize it with our robot model
95 descartes_planner::DensePlanner planner;
96 planner.initialize(model);
97
98 // 4. Feed the trajectory to the planner
99 if (!planner.planPath(points))
100 {
101 ROS_ERROR("Could not solve for a valid path");
102 return -2;
103 }
104
105 TrajectoryVec result;
106 if (!planner.getPath(result))
107 {
108 ROS_ERROR("Could not retrieve path");
109 return -3;
一旦定义了机器人模型和轨迹，规划轨迹就很简单。 创建你所选择的规划器(DensePlanner / Sparse Planner)，使用刚刚创建的模型对其进行初始化（并自行初始化），并在传递所需轨迹的同时调用planPath函数。 此函数调用会进行大量计算以生成适当的关节轨迹，并且可能需要一段时间才能返回，具体取决于所分析计划的复杂性。
getPath函数用于检索结果轨迹。 为了使规划器尽可能通用，它返回多态类型TrajectoryPtPtr，但对于密集规划器和稀疏规划器而言，其基础类型都是JointTrajectoryPt。 关节值本身将在稍后的toROSJointTrajectory函数中解压缩。
确保检查planPath和getPath函数的返回值是否正确，因为它们可能由于各种原因而失败，包括无法到达的点和不平滑的联合轨迹。
112 // 5. Translate the result into a type that ROS understands
113 // Get Joint Names
114 std::vector<std::string> names;
115 nh.getParam("controller_joint_names", names);
116 // Generate a ROS joint trajectory with the result path, robot model,
117 // joint names, and a certain time delta between each trajectory point
118 trajectory_msgs::JointTrajectory joint_solution =
119 toROSJointTrajectory(result, *model, names, 1.0);
120
121 // 6. Send the ROS trajectory to the robot for execution
122 if (!executeTrajectory(joint_solution))
123 {
124 ROS_ERROR("Could not execute trajectory!");
125 return -4;
这个最终的代码块将笛卡尔生成的关节轨迹转换为ROS可以理解的格式。 toROSJointTrajectory辅助函数执行此任务，但需要一些信息：
1.笛卡尔关节轨迹解
2.用于插补这些点的笛卡尔机器人模型
3.被主动操控的关节名称
4.每个关节位置之间的时间
第114-115行从ROS参数服务器中检索教程中机器人的关节名称。
辅助函数
1 descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose)
2 {
3 using namespace descartes_core;
4 using namespace descartes_trajectory;
5 return TrajectoryPtPtr(
new AxialSymmetricPt(pose, // Nominal pose
6 M_PI/2.0, // Search discretization
7 AxialSymmetricPt::Z_AXIS) ); // Free axis
8 }
makeTolerancedCartesianPoint是围绕笛卡尔的AxialSymmetricPt类的小型包装函数。 此类本身是笛卡尔点的一种特殊形式，其中的最后一个参数（此处为AxialSymmetricPt :: Z_AXIS）是工具可以围绕其自由旋转的名义姿态(nominal pose)的轴。 其他选项包括X_AXIS和Y_AXIS。
如果要创建固定的6DOF(6自由度)笛卡尔姿势，或者想要更好地控制可接受的公差，则可以创建具有相对应属性的笛卡尔点。
TrajectoryPtPtr是类型为descartes_core :: TrajectoryPt的boost :: shared_ptr的别名。 共享指针是一种自动管理对象生存期的方法，在许多情况下，共享指针可以像普通指针一样使用。
1 trajectory_msgs::JointTrajectory
2 toROSJointTrajectory(const TrajectoryVec& trajectory,
3 const descartes_core::RobotModel& model,
4 const std::vector<std::string>& joint_names,
5 double time_delay)
6 {
7 // Fill out information about our trajectory
8 trajectory_msgs::JointTrajectory result;
9 result.header.stamp = ros::Time::now();
10 result.header.frame_id = "world_frame";
11 result.joint_names = joint_names;
12
13 // For keeping track of time-so-far in the trajectory
14 double time_offset = 0.0;
15 // Loop through the trajectory
16 for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it)
17 {
18 // Find nominal joint solution at this point
19 std::vector<double> joints;
20 it->get()->getNominalJointPose(std::vector<double>(), model, joints);
21
22 // Fill out a ROS trajectory point
23 trajectory_msgs::JointTrajectoryPoint pt;
24 pt.positions = joints;
25 // velocity, acceleration, and effort are given dummy values
26 // we'll let the controller figure them out
27 pt.velocities.resize(joints.size(), 0.0);
28 pt.accelerations.resize(joints.size(), 0.0);
29 pt.effort.resize(joints.size(), 0.0);
30 // set the time into the trajectory
31 pt.time_from_start = ros::Duration(time_offset);
32 // increment time
33 time_offset += time_delay;
34
35 result.points.push_back(pt);
36 }
37
38 return result;
39 }
该函数负责将笛卡尔轨迹转换为ROS trajectory_msgs :: JointTrajectory。 与任何ROS消息类型一样，请确保填写标头信息，包括frame_id和时间戳。
笛卡尔的规划器返回轨迹点指针的向量，因此我们必须以多态方式访问基础数据。 稀疏和密集规划器返回的基础类型是关节轨迹点。 getNominalJointPose函数可用于提取所需的关节位置。 对于这种类型的点，函数的第一个参数只是一个哑元(虚设的参数)。 接下来，我们调整速度，加速度和力度字段(effort fields)的大小，并将所有值设置为零，以使控制器不会拒绝我们的点，并自己计算出合适的值。
最终，我们对轨迹中的时间进行了连续计数。 相对于轨迹的起点（而不是先前的点）指定ROS轨迹点的时间。
1 bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
2 {
3 // Create a Follow Joint Trajectory Action Client
4 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("joint_trajectory_action", true);
5 if (!ac.waitForServer(ros::Duration(2.0)))
6 {
7 ROS_ERROR("Could not connect to action server");
8 return false;
9 }
10
11 control_msgs::FollowJointTrajectoryGoal goal;
12 goal.trajectory = trajectory;
13 goal.goal_time_tolerance = ros::Duration(1.0);
14
15 ac.sendGoal(goal);
16
17 if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start + ros::Duration(5)))
18 {
19 ROS_INFO("Action server reported successful execution");
20 return true;
21 } else {
22 ROS_WARN("Action server could not execute trajectory");
23 return false;
24 }
25 }
executeTrajectory函数创建了FollowJointTrajectoryAction客户端，通过动作客户端调用ac.sendGoal(goal)函数调用动作服务器执行动作。
构建
descartes_tutorials存储库中的CMakeLists.txt显示了上面代码的最小示例。 将必要的配置添加到您的CMakeLists.txt，然后通过catkin_make/catkin build等进行构建。
这篇关于ROS Industrial 软件包_笛卡尔路径规划器_实现的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
