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<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