diff --git a/industrial_core/package.xml b/industrial_core/package.xml index c0eddfc8..3973c571 100644 --- a/industrial_core/package.xml +++ b/industrial_core/package.xml @@ -15,6 +15,7 @@ industrial_robot_simulator industrial_deprecated industrial_utils + industrial_trajectory_filters diff --git a/industrial_robot_simulator/industrial_robot_simulator b/industrial_robot_simulator/industrial_robot_simulator index a43e5f30..e0b83d82 100755 --- a/industrial_robot_simulator/industrial_robot_simulator +++ b/industrial_robot_simulator/industrial_robot_simulator @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/env python import roslib; roslib.load_manifest('industrial_robot_simulator') import rospy diff --git a/industrial_trajectory_filters/CMakeLists.txt b/industrial_trajectory_filters/CMakeLists.txt new file mode 100644 index 00000000..7911f47d --- /dev/null +++ b/industrial_trajectory_filters/CMakeLists.txt @@ -0,0 +1,35 @@ +# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html +cmake_minimum_required(VERSION 2.8.3) +project(industrial_trajectory_filters) + +find_package(catkin REQUIRED COMPONENTS moveit_ros_planning trajectory_msgs) + +catkin_package( + CATKIN_DEPENDS moveit_ros_planning trajectory_msgs + INCLUDE_DIRS include + LIBRARIES ${MOVEIT_LIB_NAME} +) + +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +########### +## Build ## +########### + +add_library(${PROJECT_NAME} src/n_point_filter.cpp src/uniform_sample_filter.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + + +############# +## Install ## +############# +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(FILES planning_request_adapters_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h new file mode 100644 index 00000000..5591d337 --- /dev/null +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h @@ -0,0 +1,272 @@ +/* + * 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 +#include "ros/assert.h" +#include "ros/console.h" +#include "ros/ros.h" +#include +#include + +namespace industrial_trajectory_filters +{ + +/** + * @brief Message Adapter structure serves as a stand-in for the arm navigation message type with + * which the filter was specialized. + * + */ +struct MessageAdapter +{ + + /** + * @brief Arm navigation message type for trajectory filter + * + */ + struct Request + { + trajectory_msgs::JointTrajectory trajectory; + } request; +}; + +/** + * @brief This version of the FilterBase 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 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 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 " header file in your filter's + * implementation header file. + * + * d- In your filter implementation, replace the base class "filters::FilterBase" with + * "industrial_trajectory_filters::FilterBase" + * + * e- Remove all calls to the getParam() methods from the old FilterBase 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 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 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: + * + * + * 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. + */ +template + class FilterBase : public planning_request_adapter::PlanningRequestAdapter + { + public: + + /** + * @brief Default constructor + */ + FilterBase() : + planning_request_adapter::PlanningRequestAdapter(), nh_("~"), configured_(false), filter_type_("FilterBase"), filter_name_( + "Unimplemented") + { + + } + + /** + * Default destructor + */ + virtual ~FilterBase() + { + } + ; + + /** + * @brief Update the filter and return the data separately. This function + * must be implemented in the derived class. + * + * @param data_in A reference to the data to be input to the filter + * @param data_out A reference to the data output location + * @return true on success, otherwise false. + */ + virtual bool update(const T& data_in, T& data_out)=0; + + /** + * @brief Original FilterBase method, return filter type + * @return filter type (as string) + */ + std::string getType() + { + return filter_type_; + } + ; + + /** + * @brief Original FitlerBase Method + * @return filter name (as string). + */ + inline const std::string& getName() + { + return filter_name_; + } + ; // original FilterBase method + + protected: + + /** + * @brief FilterBase method for the sub class to configure the filter + * This function must be implemented in the derived class. + * @return true if successful, otherwise false. + */ + virtual bool configure()=0; + + /** + * @brief filter name + */ + std::string filter_name_; + + /** + * The type of the filter (Used by FilterChain for Factory construction) + */ + std::string filter_type_; + + /** + * @brief Holds filter configuration state. + */ + bool configured_; + + /** + * @brief Internal node handle (used for parameter lookup) + */ + ros::NodeHandle nh_; + + protected: + + /** + * @brief 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 &added_path_index) const + { + + // non const pointer to this + FilterBase *p = const_cast*>(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; + } + + /** + * @brief Return description string + * @return description (as a string) + */ + virtual std::string getDescription() const + { + // non const pointer to this + FilterBase *p = const_cast*>(this); + + std::stringstream ss; + ss << "Trajectory filter '" << p->getName() << "' of type '" << p->getType() << "'"; + return ss.str(); + } + }; + +} + +#endif /* FILTER_BASE_H_ */ diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h new file mode 100644 index 00000000..612d6597 --- /dev/null +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h @@ -0,0 +1,89 @@ +/* + * 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 + +namespace industrial_trajectory_filters +{ + +/** + * @brief This is a simple filter which reduces a trajectory to N points or less + */ +template + + class NPointFilter : public industrial_trajectory_filters::FilterBase + { + public: + /** + * @brief Default constructor + */ + NPointFilter(); + /** + * @brief Default destructor + */ + ~NPointFilter(); + ; + + 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). + + /** + * \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). + * @param trajectory_in input trajectory + * @param trajectory_out filtered trajectory (N points or less + * @return true if successful + */ + bool update(const T& trajectory_in, T& trajectory_out); + + private: + /** + * @brief number of points to reduce trajectory to + */ + int n_points_; + + }; + +/** + * @brief Specializing trajectory filter implementation + */ +typedef NPointFilter NPointFilterAdapter; + +} + +#endif diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h new file mode 100644 index 00000000..9469c066 --- /dev/null +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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 UNIFORM_SAMPLE_FILTER_H_ +#define UNIFORM_SAMPLE_FILTER_H_ + +#include + +/* + * These headers were part of the trajectory filter interface from the + * arm navigation system. These can no longer be included in a catkin based + * package. + #include + #include + */ + +namespace industrial_trajectory_filters +{ + +/** + * \brief This is a simple filter which performs a uniforming sampling of + * a trajectory using linear interpolation. + * + */ +template + + class UniformSampleFilter : public industrial_trajectory_filters::FilterBase + { + public: + /** + * @brief Default constructor + */ + UniformSampleFilter(); + /** + * @brief Default destructor + */ + ~UniformSampleFilter(); + + virtual bool configure(); + + /** + * Uniformly samples(in terms of time) the input trajectory based upon the + * sample duration. Sampling is performed via interpolation ensuring smooth + * velocity and acceleration. NOTE: For this reason trajectories must be + * fully defined before using this filter. + * @param trajectory_in non uniform trajectory + * @param trajectory_out uniform(in terms of time) trajectory + * @return + */ + bool update(const T& trajectory_in, T& trajectory_out); + + /** + * @brief Perform interpolation between p1 and p2. Time from start must be + * in between p1 and p2 times. + * @param p1 prior trajectory point + * @param p2 subsequent trajectory point + * @param time_from_start time from start of trajectory (i.e. p0). + * @param interp_pt resulting interpolated point + * @return true if successful, otherwise false. + */ + bool interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1, trajectory_msgs::JointTrajectoryPoint & p2, + double time_from_start, trajectory_msgs::JointTrajectoryPoint & interp_pt); + + private: + /** + * @brief uniform sample duration (sec) + */ + double sample_duration_; + }; + +/** + * @brief Specializing trajectory filter implementation + */ +typedef UniformSampleFilter UniformSampleFilterAdapter; + +} + +#endif diff --git a/industrial_trajectory_filters/mainpage.dox b/industrial_trajectory_filters/mainpage.dox new file mode 100644 index 00000000..893983c5 --- /dev/null +++ b/industrial_trajectory_filters/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b industrial_trajectory_filters provides robot trajectory filters. It also includes plugin adapters for the moveit motion planning library. + + +\section codeapi Code API + +The main APIs of are largely captured in the following interface classes: +- industrial_trajectory_filters::NPointFilter : A simple filter that removes trajectory points until the trajectory is N or less points +- industrial_trajectory_filters::UniformSampleFilter : Resamples a trajectory uniformly (in time). +- industrial_trajectory_filters::FilterBase : A moveit adapter class for old arm navigation packages. +*/ diff --git a/industrial_trajectory_filters/package.xml b/industrial_trajectory_filters/package.xml new file mode 100644 index 00000000..7f572d3b --- /dev/null +++ b/industrial_trajectory_filters/package.xml @@ -0,0 +1,38 @@ + + industrial_trajectory_filters + 1.0.0 + +

+ ROS Industrial libraries/plugins for filtering trajectories. +

+

+ This package is part of the ROS Industrial program and contains libraries + and moveit plugins for filtering robot trajectories. +

+ + Shaun Edwards + Jorge Nicho + Shaun Edwards + BSD + http://ros.org/wiki/industrial_trajectory_filters + https://github.com/ros-industrial/industrial_core/issues + https://github.com/ros-industrial/industrial_core + + catkin + + trajectory_msgs + pluginlib + moveit_core + moveit_ros_planning + orocos_kdl + + trajectory_msgs + pluginlib + moveit_core + moveit_ros_planning + orocos_kdl + + + + + diff --git a/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml new file mode 100644 index 00000000..5a26761c --- /dev/null +++ b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml @@ -0,0 +1,24 @@ + + + + + This is a simple filter which reduces a trajectory to N points or less. + ROS parameters: + - n_points (default = 2) + + + + + + This is a simple filter which performs a uniforming sampling of + a trajectory using linear interpolation. + ROS parameters: + - sample_duration (default = 0.050) + + + + diff --git a/industrial_trajectory_filters/src/n_point_filter.cpp b/industrial_trajectory_filters/src/n_point_filter.cpp new file mode 100644 index 00000000..41d86dbe --- /dev/null +++ b/industrial_trajectory_filters/src/n_point_filter.cpp @@ -0,0 +1,136 @@ +/* + * 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. + */ + +//#include +#include +#include + +using namespace industrial_trajectory_filters; + +const int DEFAULT_N = 2; + +template + NPointFilter::NPointFilter() : + FilterBase() + { + ROS_INFO_STREAM("Constructing N point filter"); + n_points_ = DEFAULT_N; + this->filter_name_ = "NPointFilter"; + this->filter_type_ = "NPointFilter"; + } + +template + NPointFilter::~NPointFilter() + { + } + +template + bool NPointFilter::configure() + { + //if (!filters::FilterBase::getParam("n_points", n_points_)) + if (!this->nh_.getParam("n_points", n_points_)) + { + ROS_WARN_STREAM("NPointFilter, params has no attribute n_points."); + } + if (n_points_ < 2) + { + ROS_WARN_STREAM( "n_points attribute less than min(2), setting to minimum"); + n_points_ = 2; + } + ROS_INFO_STREAM("Using a n_points value of " << n_points_); + + return true; + } + +template + bool NPointFilter::update(const T& trajectory_in, T& trajectory_out) + { + bool success = false; + int size_in = trajectory_in.request.trajectory.points.size(); + + // Copy non point related data + trajectory_out.request.trajectory = trajectory_in.request.trajectory; + // Clear out the trajectory points + trajectory_out.request.trajectory.points.clear(); + + if (size_in > n_points_) + { + //Add first point to output trajectory + trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front()); + + int intermediate_points = n_points_ - 2; //subtract the first and last elements + double int_point_increment = double(size_in) / double(intermediate_points + 1.0); + ROS_INFO_STREAM( + "Number of intermediate points: " << intermediate_points << ", increment: " << int_point_increment); + + // The intermediate point index is determined by the following equation: + // int_point_index = i * int_point_increment + // Ex: n_points_ = 4, size_in = 11, intermediate_points = 2 -> + // int_point_increment = 3.66667, + // i = 1: int_point_index = 3 + // i = 2: int_point_index = 7 + for (int i = 1; i <= intermediate_points; i++) + { + int int_point_index = int(double(i) * int_point_increment); + ROS_INFO_STREAM("Intermediate point index: " << int_point_index); + trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points[int_point_index]); + } + + //Add last point to output trajectory + trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.back()); + + ROS_INFO_STREAM( + "Filtered trajectory from: " << trajectory_in.request.trajectory.points.size() << " to: " << trajectory_out.request.trajectory.points.size()); + + success = true; + } + else + { + ROS_WARN_STREAM( "Trajectory size less than n: " << n_points_ << ", pass through"); + trajectory_out.request.trajectory = trajectory_in.request.trajectory; + success = true; + } + + return success; + } + +// registering planner adapter +CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, + planning_request_adapter::PlanningRequestAdapter); + +/* + * Old plugin declaration for arm navigation trajectory filters + PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, + IndustrialNPointFilterJointTrajectoryWithConstraints, + industrial_trajectory_filters::NPointFilter, + filters::FilterBase); + + */ diff --git a/industrial_trajectory_filters/src/uniform_sample_filter.cpp b/industrial_trajectory_filters/src/uniform_sample_filter.cpp new file mode 100644 index 00000000..157c32cc --- /dev/null +++ b/industrial_trajectory_filters/src/uniform_sample_filter.cpp @@ -0,0 +1,227 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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. + */ + +#include +#include +#include + +using namespace industrial_trajectory_filters; + +const double DEFAULT_SAMPLE_DURATION = 0.050; //seconds + +template + UniformSampleFilter::UniformSampleFilter() : + industrial_trajectory_filters::FilterBase() + { + ROS_INFO_STREAM("Constructing N point filter"); + sample_duration_ = DEFAULT_SAMPLE_DURATION; + this->filter_name_ = "UniformSampleFilter"; + this->filter_type_ = "UniformSampleFilter"; + } + +template + UniformSampleFilter::~UniformSampleFilter() + { + } + +template + bool UniformSampleFilter::configure() + { + if (!this->nh_.getParam("sample_duration", sample_duration_)) + { + ROS_WARN_STREAM( "UniformSampleFilter, params has no attribute sample_duration."); + } + ROS_INFO_STREAM("Using a sample_duration value of " << sample_duration_); + + return true; + } + +template + bool UniformSampleFilter::update(const T& trajectory_in, T& trajectory_out) + { + bool success = false; + size_t size_in = trajectory_in.request.trajectory.points.size(); + double duration_in = trajectory_in.request.trajectory.points.back().time_from_start.toSec(); + double interpolated_time = 0.0; + size_t index_in = 0; + + trajectory_msgs::JointTrajectoryPoint p1, p2, interp_pt; + + trajectory_out = trajectory_in; + + // Clear out the trajectory points + trajectory_out.request.trajectory.points.clear(); + + while (interpolated_time < duration_in) + { + ROS_INFO_STREAM("Interpolated time: " << interpolated_time); + // Increment index until the interpolated time is past the start time. + while (interpolated_time > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec()) + { + ROS_INFO_STREAM( + "Interpolated time: " << interpolated_time << ", next point time: " << (trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())); + ROS_INFO_STREAM("Incrementing index"); + index_in++; + if (index_in >= size_in) + { + ROS_ERROR_STREAM( + "Programming error, index: " << index_in << ", greater(or equal) to size: " << size_in << " input duration: " << duration_in << " interpolated time:)" << interpolated_time); + return false; + } + } + p1 = trajectory_in.request.trajectory.points[index_in]; + p2 = trajectory_in.request.trajectory.points[index_in + 1]; + if (!interpolatePt(p1, p2, interpolated_time, interp_pt)) + { + ROS_ERROR_STREAM("Failed to interpolate point"); + return false; + } + trajectory_out.request.trajectory.points.push_back(interp_pt); + interpolated_time += sample_duration_; + + } + + ROS_INFO_STREAM( + "Interpolated time exceeds original trajectory (quitting), original: " << duration_in << " final interpolated time: " << interpolated_time); + p2 = trajectory_in.request.trajectory.points.back(); + p2.time_from_start = ros::Duration(interpolated_time); + // TODO: Really should check that appending the last point doesn't result in + // really slow motion at the end. This could happen if the sample duration is a + // large percentage of the trajectory duration (not likely). + trajectory_out.request.trajectory.points.push_back(p2); + + ROS_INFO_STREAM( + "Uniform sampling, resample duraction: " << sample_duration_ << " input traj. size: " << trajectory_in.request.trajectory.points.size() << " output traj. size: " << trajectory_out.request.trajectory.points.size()); + + success = true; + return success; + } + +template + bool UniformSampleFilter::interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1, + trajectory_msgs::JointTrajectoryPoint & p2, double time_from_start, + trajectory_msgs::JointTrajectoryPoint & interp_pt) + { + bool rtn = false; + double p1_time_from_start = p1.time_from_start.toSec(); + double p2_time_from_start = p2.time_from_start.toSec(); + + ROS_INFO_STREAM("time from start: " << time_from_start); + + if (time_from_start >= p1_time_from_start && time_from_start <= p2_time_from_start) + { + if (p1.positions.size() == p1.velocities.size() && p1.positions.size() == p1.accelerations.size()) + { + if (p1.positions.size() == p2.positions.size() && p1.velocities.size() == p2.velocities.size() + && p1.accelerations.size() == p2.accelerations.size()) + { + // Copy p1 to ensure the interp_pt has the correct size vectors + interp_pt = p1; + // TODO: Creating a new spline calculator in this function means that + // it may be created multiple times for the same points (assuming the + // resample duration is less that the actual duration, which it might + // be sometimes) + KDL::VelocityProfile_Spline spline_calc; + ROS_INFO_STREAM( "---------------Begin interpolating joint point---------------"); + + for (size_t i = 0; i < p1.positions.size(); ++i) + { + // Calculated relative times for spline calculation + double time_from_p1 = time_from_start - p1.time_from_start.toSec(); + double time_from_p1_to_p2 = p2_time_from_start - p1_time_from_start; + + ROS_INFO_STREAM("time from p1: " << time_from_p1); + ROS_INFO_STREAM( "time_from_p1_to_p2: " << time_from_p1_to_p2); + + spline_calc.SetProfileDuration(p1.positions[i], p1.velocities[i], p1.accelerations[i], p2.positions[i], + p2.velocities[i], p2.accelerations[i], time_from_p1_to_p2); + + ros::Duration time_from_start_dur(time_from_start); + ROS_INFO_STREAM( "time from start_dur: " << time_from_start_dur); + + interp_pt.time_from_start = time_from_start_dur; + interp_pt.positions[i] = spline_calc.Pos(time_from_p1); + interp_pt.velocities[i] = spline_calc.Vel(time_from_p1); + interp_pt.accelerations[i] = spline_calc.Acc(time_from_p1); + + ROS_INFO_STREAM( + "p1.pos: " << p1.positions[i] << ", vel: " << p1.velocities[i] << ", acc: " << p1.accelerations[i] << ", tfs: " << p1.time_from_start); + + ROS_INFO_STREAM( + "p2.pos: " << p2.positions[i] << ", vel: " << p2.velocities[i] << ", acc: " << p2.accelerations[i] << ", tfs: " << p2.time_from_start); + + ROS_INFO_STREAM( + "interp_pt.pos: " << interp_pt.positions[i] << ", vel: " << interp_pt.velocities[i] << ", acc: " << interp_pt.accelerations[i] << ", tfs: " << interp_pt.time_from_start); + } + ROS_INFO_STREAM( "---------------End interpolating joint point---------------"); + rtn = true; + } + else + { + ROS_ERROR_STREAM("Trajectory point size mismatch"); + ROS_ERROR_STREAM( + "Trajectory point 1, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); + ROS_ERROR_STREAM( + "Trajectory point 2, pos: " << p2.positions.size() << " vel: " << p2.velocities.size() << " acc: " << p2.accelerations.size()); + rtn = false; + } + + } + else + { + ROS_ERROR_STREAM( + "Trajectory point not fully defined, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); + rtn = false; + } + } + else + { + ROS_ERROR_STREAM( + "Time: " << time_from_start << " not between interpolation point times[" << p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]"); + rtn = false; + } + + return rtn; + } + +// registering planner adapter +CLASS_LOADER_REGISTER_CLASS( industrial_trajectory_filters::UniformSampleFilterAdapter, + planning_request_adapter::PlanningRequestAdapter); + +/* + * Old plugin declaration for arm navigation trajectory filters + PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, + IndustrialNPointFilterJointTrajectoryWithConstraints, + industrial_trajectory_filters::NPointFilter, + filters::FilterBase); + + */ +