ROS进阶——MoveIt Planning Request Adapters

一、Planning Request Adapters

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

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

• moveit_ros

适配器	作用
default_planner_request_adapters::Empty
default_planner_request_adapters::FixStartStateBounds	修复 joint 初始极限
default_planner_request_adapters::FixStartStatePathConstraints	找到满足约束的姿态作为机器人的初始姿态
default_planner_request_adapters::FixWorkspaceBounds	设置默认尺寸的工作空间
default_planner_request_adapters::FixStartStateCollision	修复碰撞配置文件
default_planner_request_adapters::AddTimeParameterization	Iterative Parabolic Time Parameterization
default_planner_request_adapters::AddIterativeSplineParameterization	Iterative Spline Parameterization
default_planner_request_adapters::AddTimeOptimalParameterization(>=melodic)	Time-optimal Trajectory Parameterization
default_planner_request_adapters::ResolveConstraintFrames(master)	Resolves constraints that are defined in collision objects or subframes to robot links, because the former are not known to the planner.

• industrial_trajectory_filters

适配器	作用
industrial_trajectory_filters::NPointFilterAdapter	约束最大输出点数
industrial_trajectory_filters::UniformSampleFilterAdapter	调整输出轨迹为等时
industrial_trajectory_filters::AddSmoothingFilter	平滑轨迹（通过设置相邻点的权重计算该点的位置，默认为0.25,0.5,1,0.5,0.25）

二、自定义适配器

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

1. 创建包（my_planner_request_adapters）
2. 编写算法（time_optimal_trajectory_generation.h，time_optimal_trajectory_generation.cpp）
3. 编写添加算法用的接口文件（add_time_optimal_parameterization.cpp）
4. 编写描述适配器的xml文件（planning_request_adapters_plugin_description.xml）
5. 修改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& added_path_index进行对应修改。

接口文件

• add_time_optimal_parameterization.cpp

继承planning_request_adapter::PlanningRequestAdapter，重载adaptAndPlan，并添加CLASS_LOADER_REGISTER_CLASS

#include 
#include 
#include 
#include 

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& 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 
#include 
#include 
#include 

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

class PlanningRequestAdapter
{
public:
  typedef boost::function
      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& 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& 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& added_path_index) const;

private:
  std::vector 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>catkinbuildtool_depend>
  
  <depend>trajectory_msgsdepend>
  <depend>pluginlibdepend>
  <depend>moveit_coredepend>
  <depend>moveit_ros_planningdepend>
  <depend>orocos_kdldepend>

  
  <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