This version of the FilterBase<T> can be used to encapsulate the functionality from an arm navigation trajectory filter into a PlanningRequestAdapter that is used by MoveIt. More...
#include <filter_base.h>
Public Member Functions | |
FilterBase () | |
Default constructor. More... | |
const std::string & | getName () |
Original FitlerBase Method. More... | |
std::string | getType () |
Original FilterBase method, return filter type. More... | |
virtual bool | update (const T &data_in, T &data_out)=0 |
Update the filter and return the data separately. This function must be implemented in the derived class. More... | |
virtual | ~FilterBase () |
Public Member Functions inherited from planning_request_adapter::PlanningRequestAdapter | |
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 |
PlanningRequestAdapter () | |
virtual | ~PlanningRequestAdapter () |
Protected Member Functions | |
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 |
Moveit Planning Request Adapter method. This basic implementation of the adaptAndPlan method calls the planner and then maps the trajectory data from the MotionPlanResponse object into the MessageAdapter mapping structure. The MessageAdapter object is then passed to the "update" method that resembles that from the old FilterBase interface class. The filtered trajectory is finally saved in the MotionPlanResponse object. More... | |
virtual bool | configure ()=0 |
FilterBase method for the sub class to configure the filter This function must be implemented in the derived class. More... | |
virtual std::string | getDescription () const |
Return description string. More... | |
Protected Attributes | |
bool | configured_ |
Holds filter configuration state. More... | |
std::string | filter_name_ |
filter name More... | |
std::string | filter_type_ |
ros::NodeHandle | nh_ |
Internal node handle (used for parameter lookup) More... | |
Additional Inherited Members | |
Public Types inherited from planning_request_adapter::PlanningRequestAdapter | |
typedef boost::function< bool(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res)> | PlannerFn |
This version of the FilterBase<T> can be used to encapsulate the functionality from an arm navigation trajectory filter into a PlanningRequestAdapter that is used by MoveIt.
Since this version of the FilterBase<T> class only provides some degree of backwards compatibility with the original FilterBase interface, then some parts of the arm navigation filter must be modified in order to accommodate the differences introduced by this new interface class.
The purely virtual methods "update" and "configure" have been preserved since it is assumed that any trajectory filter specialization implemented at least these two methods. The getter methods for the filter's type and name have been kept as well, however the corresponding members aren't populated from within the scope of the FilterBase<T> class. Thus, the users implementation must fill these members with the appropriate string data.
All the "getParam" methods for parameter lookup have been removed and so the filter implementation must be modified to read parameters using the regular roscpp parameter API (either through the node handle or the bare version)
Steps for conversion to a Moveit PlanningRequestAdapter:
a - Remove all arm_navigation header files since these can not be included in a catkin based package.
b - Remove all header files that belong to a rosbuild only package.
c - Include the #include <industrial_trajectory_filters/filter_base.h>" header file in your filter's implementation header file.
d- In your filter implementation, replace the base class "filters::FilterBase<T>" with "industrial_trajectory_filters::FilterBase<T>"
e- Remove all calls to the getParam() methods from the old FilterBase<T> class and use the roscpp parameter interface for any parameter lookup tasks instead.
f- In your filter's header file, declare a typedef that specializes your filter template implementation to a version that uses the "MessageAdapter" structure. For instance, if we are converting the NPointFilter filter class then we would add the following after the class declaration:
typedef NPointFilter<MessageAdapter> NPointFilterAdapter;
In case the "MessageAdapter" structure does not fit the expected template class then the filter's implementation must provide its own adapter structure that resembles the necessary parts of expected arm_navigation message type. The specialization of such filter would then pass the custom adapter structure in the template argument as follows:
typedef NPointFilter<CustomMessageAdapter> NPointFilterAdapter;
g- In your filter's source file, remove the plugin declaration "PLUGINLIB_DECLARE_CLASS" and add the class loader macro. In the case of the "NPointFilter" this would be as follows:
CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, planning_request_adapter::PlanningRequestAdapter);
h - All parameters used by your filter must me made available as a regular ros parameter. For instance, in the "ompl_planning_pipeline.launch" file in a moveit package for your robot you would add the following element:
my_filter_parameter |
i - (Optional) the "adaptAndPlan" methods default implementation already maps the trajectory data between the MessageAdapter structure and the planning interface objects in the argument list. The filter's implementation should override this method whenever a custom adapter structure is used.
Definition at line 123 of file filter_base.h.
|
inline |
Default constructor.
Definition at line 130 of file filter_base.h.
|
inlinevirtual |
Default destructor
Definition at line 140 of file filter_base.h.
|
inlineprotectedvirtual |
Moveit Planning Request Adapter method. This basic implementation of the adaptAndPlan method calls the planner and then maps the trajectory data from the MotionPlanResponse object into the MessageAdapter mapping structure. The MessageAdapter object is then passed to the "update" method that resembles that from the old FilterBase interface class. The filtered trajectory is finally saved in the MotionPlanResponse object.
Implements planning_request_adapter::PlanningRequestAdapter.
Definition at line 214 of file filter_base.h.
|
protectedpure virtual |
FilterBase method for the sub class to configure the filter This function must be implemented in the derived class.
Implemented in industrial_trajectory_filters::UniformSampleFilter< T >, and industrial_trajectory_filters::NPointFilter< T >.
|
inlineprotectedvirtual |
Return description string.
Reimplemented from planning_request_adapter::PlanningRequestAdapter.
Definition at line 259 of file filter_base.h.
|
inline |
Original FitlerBase Method.
Definition at line 169 of file filter_base.h.
|
inline |
Original FilterBase method, return filter type.
Definition at line 159 of file filter_base.h.
|
pure virtual |
Update the filter and return the data separately. This function must be implemented in the derived class.
data_in | A reference to the data to be input to the filter |
data_out | A reference to the data output location |
Implemented in industrial_trajectory_filters::UniformSampleFilter< T >, and industrial_trajectory_filters::NPointFilter< T >.
|
protected |
Holds filter configuration state.
Definition at line 197 of file filter_base.h.
|
protected |
filter name
Definition at line 187 of file filter_base.h.
|
protected |
The type of the filter (Used by FilterChain for Factory construction)
Definition at line 192 of file filter_base.h.
|
protected |
Internal node handle (used for parameter lookup)
Definition at line 202 of file filter_base.h.