ROS进阶——MoveIt Planning Request Adapters

white_Learner

分类：ROS

发布时间 2021.07.27阅读数 3433 评论数 0

一、Planning Request Adapters

å¨è¿éæå¥å¾çæè¿°

Planning Request Adapters为规划处理适配器，用于处理运动规划（motion_planer）请求（before plan）和响应（after plan）的数据。

注意：不同版本ROS提供的适配器略有不同，如果需要使用新版本ROS或者自定义适配器，需要根据流程配置。

moveit_ros

二、自定义适配器

自定义适配器的基本流程为

创建包（my_planner_request_adapters）
编写算法（time_optimal_trajectory_generation.h，time_optimal_trajectory_generation.cpp）
编写添加算法用的接口文件（add_time_optimal_parameterization.cpp）
编写描述适配器的xml文件（planning_request_adapters_plugin_description.xml）
修改CMakeLists和package，编译使用（CMakeLists.txt，package.xml）

2.1 目录框架

.
├── CMakeLists.txt
├── include
│   └── my_planner_request_adapters
│       └── time_optimal_trajectory_generation.h
├── package.xml
├── planning_request_adapters_plugin_description.xml
└── src
    ├── add_time_optimal_parameterization.cpp
    └── time_optimal_trajectory_generation.cpp

2.2 创建接口

接口需要继承planning_request_adapter，并覆写相关函数

函数解析
getDescription()
获取插件描述及简单应用。

adaptAndPlan()（关键函数）
adaptAndPlan是用于在planning pipeline中对轨迹数据进行处理，在规划中Adpaters以“链条”的形式一环接一环被调用，为确保在该“环”的正常工作必须调用planner function。

适配器可对轨迹做前处理和后处理，或者同时使用，这个注意以处理在planner前还是后为基准，下面为对轨迹作后处理，首先调用planner进行规划

bool result = planner(planning_scene,req,res);

如果规划结果正确，则对轨迹作后处理

smoothing_filter_->applyFilter(*res.trajectory_);

所有对轨迹点进行增加操作的都需要保证对应的标签序号std::vector<std::size_t>& added_path_index进行对应修改。

接口文件
add_time_optimal_parameterization.cpp
继承planning_request_adapter::PlanningRequestAdapter，重载adaptAndPlan，并添加CLASS_LOADER_REGISTER_CLASS

#include <moveit/planning_request_adapter/planning_request_adapter.h>
#include <my_planner_request_adapters/time_optimal_trajectory_generation.h>
#include <class_loader/class_loader.hpp>
#include <ros/console.h>

namespace my_planner_request_adapters
{

using namespace trajectory_processing;

class AddTimeOptimalParameterization : public planning_request_adapter::PlanningRequestAdapter
{
public:
  AddTimeOptimalParameterization() : planning_request_adapter::PlanningRequestAdapter()
  {
  }

  virtual std::string getDescription() const
  {
    return "Add Time Optimal Parameterization";
  }

  virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const planning_interface::MotionPlanRequest& req,
                            planning_interface::MotionPlanResponse& res,
                            std::vector<std::size_t>& added_path_index) const
  {
    bool result = planner(planning_scene, req, res);//plan
    if (result && res.trajectory_);//post-plan
    {
      ROS_DEBUG("Running '%s'", getDescription().c_str());
      TimeOptimalTrajectoryGeneration totg;
      if (!totg.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor,
                                         req.max_acceleration_scaling_factor))
        ROS_WARN("Time parametrization for the solution path failed.");
    }
    return result;
  }
};
}

CLASS_LOADER_REGISTER_CLASS(my_planner_request_adapters::AddTimeOptimalParameterization,
                            planning_request_adapter::PlanningRequestAdapter);

planning_request_adapter

注意：不同版本不一致，具体参考moveit

moveit/planning_request_adapter/planning_request_adapter.h

#ifndef MOVEIT_PLANNING_REQUEST_ADAPTER_PLANNING_REQUEST_ADAPTER_
#define MOVEIT_PLANNING_REQUEST_ADAPTER_PLANNING_REQUEST_ADAPTER_

#include <moveit/macros/class_forward.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <boost/function.hpp>

/** \brief Generic interface to adapting motion planning requests */
namespace planning_request_adapter
{
MOVEIT_CLASS_FORWARD(PlanningRequestAdapter);

class PlanningRequestAdapter
{
public:
  typedef boost::function<bool(const planning_scene::PlanningSceneConstPtr& planning_scene,
                               const planning_interface::MotionPlanRequest& req,
                               planning_interface::MotionPlanResponse& res)>
      PlannerFn;

  PlanningRequestAdapter()
  {
  }

  virtual ~PlanningRequestAdapter()
  {
  }

  /// Get a short string that identifies the planning request adapter
  virtual std::string getDescription() const
  {
    return "";
  }

  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                    const planning_interface::MotionPlanRequest& req,
                    planning_interface::MotionPlanResponse& res) const;

  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
                    std::vector<std::size_t>& added_path_index) const;

  /** \brief Adapt the planning request if needed, call the planner
      function \e planner and update the planning response if
      needed. If the response is changed, the index values of the
      states added without planning are added to \e
      added_path_index */
  virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const planning_interface::MotionPlanRequest& req,
                            planning_interface::MotionPlanResponse& res,
                            std::vector<std::size_t>& added_path_index) const = 0;
};

/// Apply a sequence of adapters to a motion plan
class PlanningRequestAdapterChain
{
public:
  PlanningRequestAdapterChain()
  {
  }

  void addAdapter(const PlanningRequestAdapterConstPtr& adapter)
  {
    adapters_.push_back(adapter);
  }

  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                    const planning_interface::MotionPlanRequest& req,
                    planning_interface::MotionPlanResponse& res) const;

  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                    const planning_scene::PlanningSceneConstPtr& planning_scene,
                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
                    std::vector<std::size_t>& added_path_index) const;

private:
  std::vector<PlanningRequestAdapterConstPtr> adapters_;
};
}

#endif

2.3 添加适配器描述文件

path为lib+PROJECT_NAME，class的内容要与CLASS_LOADER_REGISTER_CLASS设置的对应。

planning_request_adapters_plugin_description.xml

<library path="libmy_planner_request_adapters">

  <class name="my_planner_request_adapters/AddTimeOptimalParameterization"
	      type="my_planner_request_adapters::AddTimeOptimalParameterization" 	
	      base_class_type="planning_request_adapter::PlanningRequestAdapter">
    <description>
      This is an improved time parameterization using Time-Optimal Trajectory Generation for Path Following With Bounded Acceleration and Velocity (Kunz and Stilman, RSS 2008) 
    </description>
  </class>

</library>

2.4 编译CMakeLists.txt编译使用add_library生成库

cmake_minimum_required(VERSION 2.8.3)
project(my_planner_request_adapters)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  moveit_core
  moveit_ros_planning 
  trajectory_msgs
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)

## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${PROJECT_NAME}
  CATKIN_DEPENDS moveit_core moveit_ros_planning trajectory_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
add_library(${PROJECT_NAME}
  src/add_time_optimal_parameterization.cpp
  src/time_optimal_trajectory_generation.cpp
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/my_planner_request_adapters_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

#############
## Install ##
#############

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
#  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

## Mark cpp header files for installation
install(DIRECTORY include/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
)

## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES planning_request_adapters_plugin_description.xml
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

package.xml添加depend和export

  <buildtool_depend>catkin</buildtool_depend>
  
  <depend>trajectory_msgs</depend>
  <depend>pluginlib</depend>
  <depend>moveit_core</depend>
  <depend>moveit_ros_planning</depend>
  <depend>orocos_kdl</depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>	
    <moveit_core plugin="${prefix}/planning_request_adapters_plugin_description.xml"/> 
  </export>

参考
Developing a Planning Request Adapter

This tutorial is a step by step development of a planning request adapter using a simple smoothing filter as an example

Using a planning adapter inside of MoveIt.

This tutorial will show you how to use a planning request adapter with MoveIt.

industrial_trajectory_filters

planning_request_adapter

libmoveit_default_planning_request_adapter_plugins

机械臂 ROS MoveIt 建模仿真运动控制

打赏 0

上一篇：ROS进阶——笛卡尔轨迹规划descartes

下一篇：ROS进阶——Behaviortree行为树

ROS进阶——MoveIt Planning Request Adapters

white_Learner

一、Planning Request Adapters

二、自定义适配器

2.1 目录框架

2.2 创建接口

2.3 添加适配器描述文件

2.4 编译CMakeLists.txt编译使用add_library生成库

为你推荐

SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结

机械臂在ROS2的rviz2中的显示问题

ROS:从入门到放弃(二)　发布接收不同类型消息1

ORB SLAM2源码解读(八)：Initializer类

ROS学习心得——机器人篇——同一局域网下机器人与主机的通信

ROS-创建点云数据并在Rviz中显示

关于作者

white_Learner

38

0

0

2

Tensorflow学习——利用Object Detection api训练自己的数据集

ROS学习——tf2

两轴机械臂动力学参数识别

相关推荐

从零开始仿真ROS小车（一）urdf模型+rviz可视化

SLAM各种并行加速方法

手把手教你用Gazebo仿真UUV水下机器人

TB2安装ubuntu16.04+kinetic的ROS包

SLAM14讲学习笔记（六）后端（最难一章：卡尔曼滤波器推导、理解以及扩展）

精选cartographer与ros环境开发经验技巧总结（持续更新）

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

ROS进阶——MoveIt Planning Request Adapters

white_Learner

一、Planning Request Adapters

二、自定义适配器

2.1 目录框架

2.2 创建接口

2.3 添加适配器描述文件

2.4 编译CMakeLists.txt编译使用add_library生成库

为你推荐

SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结

机械臂在ROS2的rviz2中的显示问题

ROS:从入门到放弃(二) 发布接收不同类型消息1

ORB SLAM2源码解读(八)：Initializer类

ROS学习心得——机器人篇——同一局域网下机器人与主机的通信

ROS-创建点云数据并在Rviz中显示

评论（0）

关于作者

white_Learner

38

0

0

2

Tensorflow学习——利用Object Detection api训练自己的数据集

ROS学习——tf2

两轴机械臂动力学参数识别

相关推荐

从零开始仿真ROS小车（一）urdf模型+rviz可视化

SLAM各种并行加速方法

手把手教你用Gazebo仿真UUV水下机器人

TB2安装ubuntu16.04+kinetic的ROS包

SLAM14讲学习笔记（六）后端（最难一章：卡尔曼滤波器推导、理解以及扩展）

精选cartographer与ros环境开发经验技巧总结（持续更新）

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

ROS:从入门到放弃(二)　发布接收不同类型消息1

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板