Imported industrial trajectory filters package from industrial experi…

…mental

industrial_trajectory_filters/CMakeLists.txt

@@ -0,0 +1,66 @@
+# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
+cmake_minimum_required(VERSION 2.8.3)
+project(industrial_trajectory_filters)
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS moveit_ros_planning trajectory_msgs)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system thread)
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include
+  ${catkin_INCLUDE_DIRS}
+)
+
+# Package definitions
+set(MOVEIT_LIB_NAME moveit_industrial_trajectory_filters_plugins)
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## 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
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+    CATKIN_DEPENDS moveit_ros_planning trajectory_msgs
+    INCLUDE_DIRS include
+    LIBRARIES ${MOVEIT_LIB_NAME}
+)
+
+###########
+## Build ##
+###########
+
+add_library(${MOVEIT_LIB_NAME} src/n_point_filter.cpp src/uniform_sample_filter.cpp)
+target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+#install(TARGETS ${MOVEIT_LIB_NAME}
+#  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+
+
+

industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h

@@ -0,0 +1,239 @@
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Southwest Research Institute
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 	* Redistributions of source code must retain the above copyright
+* 	notice, this list of conditions and the following disclaimer.
+* 	* Redistributions in binary form must reproduce the above copyright
+* 	notice, this list of conditions and the following disclaimer in the
+* 	documentation and/or other materials provided with the distribution.
+* 	* Neither the name of the Southwest Research Institute, nor the names
+*	of its contributors may be used to endorse or promote products derived
+*	from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef FILTER_BASE_H_
+#define FILTER_BASE_H_
+
+#include <typeinfo>
+#include "ros/assert.h"
+#include "ros/console.h"
+#include "ros/ros.h"
+#include <moveit/planning_request_adapter/planning_request_adapter.h>
+#include <class_loader/class_loader.h>
+
+/** Trajectory filter interface:
+ *
+ * Description:
+ *   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:
+ *	     <param name="my_filter_parameter" value="5" />
+ *
+ *	 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.
+ */
+
+namespace industrial_trajectory_filters
+{
+
+/*
+ * Message Adapter structure serves as a stand-in for the arm navigation message type that
+ * the filter was specialized with.
+ *
+ */
+struct MessageAdapter
+{
+	struct Request
+	{
+		trajectory_msgs::JointTrajectory trajectory;
+	} request;
+};
+
+/*
+ * Filter Base Interface
+ * 	Replaces the original arm navigation FilterBase<T>.
+ */
+template<typename T>
+class FilterBase: public planning_request_adapter::PlanningRequestAdapter
+{
+public:
+
+  FilterBase(): planning_request_adapter::PlanningRequestAdapter(),
+  	  nh_("~"),
+  	  configured_(false),
+  	  filter_type_("FilterBase"),
+  	  filter_name_("Unimplemented")
+  {
+
+  }
+
+  virtual ~FilterBase(){};
+
+
+  /** update: Original FilterBase method
+   *  	Update the filter and return the data separately
+   * 	- param data_in A reference to the data to be input to the filter
+   * 	- param data_out A reference to the data output location
+   * 	This function must be implemented in the derived class.
+   */
+  virtual bool update(const T& data_in, T& data_out)=0;
+
+
+  std::string getType() {return filter_type_;}; // original FilterBase method
+  inline const std::string& getName(){return filter_name_;}; // original FilterBase method
+
+
+protected:
+
+  /** configure: Original FilterBase method
+   * Pure virtual function for the sub class to configure the filter
+   * This function must be implemented in the derived class.
+   */
+  virtual bool configure()=0;
+
+  ///The name of the filter
+  std::string filter_name_;
+
+  ///The type of the filter (Used by FilterChain for Factory construction)
+  std::string filter_type_;
+
+  /// Whether the filter has been configured.
+  bool configured_;
+
+  // node handle that can be used for param lookup
+  ros::NodeHandle nh_;
+
+
+protected:
+
+  /*
+   *  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.
+   */
+  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
+	  {
+
+		  // non const pointer to this
+		  FilterBase<MessageAdapter> *p = const_cast<FilterBase<MessageAdapter>* >(this);
+
+	  	  // calling the configure method
+	  	  if(!configured_ && p->configure())
+	  	  {
+	  		  p->configured_ = true;
+	  	  }
+
+	  	  // moveit messages for saving trajectory data
+	  	  moveit_msgs::RobotTrajectory robot_trajectory_in,robot_trajectory_out;
+	  	  MessageAdapter trajectory_in, trajectory_out; // mapping structures
+
+	  	  // calling planner first
+	  	  bool result = planner(planning_scene, req, res);
+
+	  	  // applying filter to planned trajectory
+	  	  if (result && res.trajectory_)
+	  	  {
+	  		// mapping arguments into message adapter struct
+	  		res.trajectory_->getRobotTrajectoryMsg(robot_trajectory_in);
+	  		trajectory_in.request.trajectory =robot_trajectory_in.joint_trajectory;
+
+	  		// applying arm navigation filter to planned trajectory
+	  		p->update(trajectory_in,trajectory_out);
+
+	  		// saving filtered trajectory into moveit message.
+	  		robot_trajectory_out.joint_trajectory = trajectory_out.request.trajectory;
+	  		res.trajectory_->setRobotTrajectoryMsg(planning_scene->getCurrentState(),robot_trajectory_out);
+
+	  	  }
+
+		  return result;
+	  }
+
+  	  virtual std::string getDescription() const
+  	  {
+  		  // non const pointer to this
+	      FilterBase<MessageAdapter> *p = const_cast<FilterBase<MessageAdapter>* >(this);
+
+  		  std::stringstream ss;
+  		  ss<<"Trajectory filter '"<<p->getName()<<"' of type '"<<p->getType()<<"'";
+  		  return ss.str();
+  	  }
+};
+
+}
+
+
+#endif /* FILTER_BASE_H_ */

industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h

@@ -0,0 +1,69 @@
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Southwest Research Institute
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 	* Redistributions of source code must retain the above copyright
+* 	notice, this list of conditions and the following disclaimer.
+* 	* Redistributions in binary form must reproduce the above copyright
+* 	notice, this list of conditions and the following disclaimer in the
+* 	documentation and/or other materials provided with the distribution.
+* 	* Neither the name of the Southwest Research Institute, nor the names
+*	of its contributors may be used to endorse or promote products derived
+*	from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef N_POINT_FILTER_H_
+#define N_POINT_FILTER_H_
+
+#include <industrial_trajectory_filters/filter_base.h>
+
+
+
+namespace industrial_trajectory_filters
+{
+
+// \brief This is a simple filter which reduces a trajectory to N points or less
+template <typename T>
+
+class NPointFilter: public industrial_trajectory_filters::FilterBase<T>
+{
+public:
+	NPointFilter();
+  ~NPointFilter();
+
+  /// \brief Configures the filter
+  virtual bool configure();
+
+  // \brief Reduces a trajectory to N points or less.  The resulting trajectory
+  // contains only point within the original trajectory (no interpolation is done
+  // between points).
+  bool update(const T& trajectory_in, T& trajectory_out);
+
+private:
+  int			n_points_;					// @brief number of points to reduce trajectory to
+
+};
+
+// Specializing trajectory filter implementation
+typedef NPointFilter<MessageAdapter> NPointFilterAdapter;
+
+}
+
+#endif