ROS进阶——MoveIt Planning Request Adapters

Others 2020-04-02 14:37:40 views: null

一、Planning Request Adapters

Here Insert Picture Description

Planning Request Adapters planning processing adapter, the data for (motion_planer) request (before plan) and the response (after plan) motion planning process.

Note : ROS provide different versions of the adapter is slightly different, if you need to use the new version of ROS or custom adapter needs to be configured according to the process.

  • moveit_ros
adapter effect
default_planner_request_adapters::Empty
default_planner_request_adapters::FixStartStateBounds Repair joint initial limit
default_planner_request_adapters::FixStartStatePathConstraints Found to meet the constraints of the robot's posture as an initial gesture
default_planner_request_adapters::FixWorkspaceBounds Set the default size of the workspace
default_planner_request_adapters::FixStartStateCollision Collision repair profiles
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
adapter effect
industrial_trajectory_filters::NPointFilterAdapter Restricted maximum output points
industrial_trajectory_filters::UniformSampleFilterAdapter The output is adjusted to track the like
industrial_trajectory_filters::AddSmoothingFilter Smoothed trajectory (right adjacent dots by setting the position of the point is recomputed, the default is 0.25,0.5,1,0.5,0.25)

Second, the custom adapter

Since the basic flow is defined adapter

  1. Create a package (my_planner_request_adapters)
  2. Write algorithm (time_optimal_trajectory_generation.h, time_optimal_trajectory_generation.cpp)
  3. Add written algorithm interface file (add_time_optimal_parameterization.cpp)
  4. Written description of the adapter xml file (planning_request_adapters_plugin_description.xml)
  5. Modify CMakeLists and package, compiled using (CMakeLists.txt, package.xml)

2.1 Framework directory

.
├── 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 Creating interfaces

Interface needs to inherit planning_request_adapter, and override the correlation function

Analytic function

  • getDescription()

Get Plugin description and simple application.

  • adaptAndPlan()( Key function )

adaptAndPlanIs used planning pipelinein the trajectory data is processed in the plan Adpatersas a "chain" of a ring connected to a ring is called, must be called to ensure that the "ring" to work in planner function.

Adapters can do on the track pre-processing and post-processing, or use, pay attention to this treatment plannerbefore or after as a reference, the following is the trajectory for the treatment, first call plannerplan

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

If the planning result is correct, then the trajectory for post-processing

smoothing_filter_->applyFilter(*res.trajectory_);

All points on the trajectories increases operations are required to ensure that the corresponding label number std::vector<std::size_t>& added_path_indexfor the corresponding modification.

Interface file

  • add_time_optimal_parameterization.cpp

Inheritance planning_request_adapter::PlanningRequestAdapter, overloading adaptAndPlan, and addCLASS_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

Note : Inconsistent different versions, with specific reference to 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 add an adapter description file

for the path lib+PROJECT_NAME, class and content to CLASS_LOADER_REGISTER_CLASSthe corresponding set.

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

  • CMakeLists.txt

Compiled using add_librarygeneration 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

Add dependandexport

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

reference

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

white_Learner
Published 53 original articles · won praise 186 · views 180 000 +
Private letter concerns

Guess you like

Origin blog.csdn.net/Kalenee/article/details/103997531
Recommended
Ranking
[Nodejs] Native nodejs routing, getting parameters, static directory
Introduction and use of the datetime module in python - used to process date and time, calculate time intervals, etc.
Java bytecode decompiler in IntelliJIDEA for Scala
Resolved local variable 'str' referenced before assignment
Transformer Vision (2) || ViT-B/16 network structure
Translation: The most impressive YouTube channel that allows you to learn AI, machine learning and data science
gRPC- interceptor simple to use
Fortunately, three times faster investment has been lost how to do? There Baoying tutor you fly 69,937,609
Using Django to create a new project in the Windows10 environment gives an error [django-admin.py: The "django-admin.py" item cannot be recognized as the name of a cmdlet, function, script file, or executable program. 】
What should I do if I need to prepare the product standard
Daily
More
2024-06-05(0)
2024-06-04(0)
2024-06-03(1)
2024-06-02(0)
2024-06-01(1)
2024-05-31(1)
2024-05-30(0)
2024-05-29(1)
2024-05-28(1)
2024-05-27(1)

Code World

© 2018 codetd.com