From 37053ba8b029e980122869659ec178119331f367 Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Thu, 26 Dec 2013 15:33:27 -0500 Subject: [PATCH 1/9] Imported industrial trajectory filters package from industrial experimental --- industrial_trajectory_filters/CMakeLists.txt | 66 +++++ .../filter_base.h | 239 +++++++++++++++++ .../n_point_filter.h | 69 +++++ .../uniform_sample_filter.h | 83 ++++++ industrial_trajectory_filters/mainpage.dox | 14 + industrial_trajectory_filters/package.xml | 29 +++ ...ng_request_adapters_plugin_description.xml | 24 ++ .../src/n_point_filter.cpp | 141 ++++++++++ .../src/uniform_sample_filter.cpp | 242 ++++++++++++++++++ 9 files changed, 907 insertions(+) create mode 100644 industrial_trajectory_filters/CMakeLists.txt create mode 100644 industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h create mode 100644 industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h create mode 100644 industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h create mode 100644 industrial_trajectory_filters/mainpage.dox create mode 100644 industrial_trajectory_filters/package.xml create mode 100644 industrial_trajectory_filters/planning_request_adapters_plugin_description.xml create mode 100644 industrial_trajectory_filters/src/n_point_filter.cpp create mode 100644 industrial_trajectory_filters/src/uniform_sample_filter.cpp diff --git a/industrial_trajectory_filters/CMakeLists.txt b/industrial_trajectory_filters/CMakeLists.txt new file mode 100644 index 00000000..9efddecf --- /dev/null +++ b/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}) + + + + 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..f4d2ffa0 --- /dev/null +++ b/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 +#include "ros/assert.h" +#include "ros/console.h" +#include "ros/ros.h" +#include +#include + +/** Trajectory filter interface: + * + * Description: + * 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. + */ + +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. + */ +template +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 *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; + } + + virtual std::string getDescription() const + { + // non const pointer to this + FilterBase *p = const_cast* >(this); + + std::stringstream ss; + ss<<"Trajectory filter '"<getName()<<"' of type '"<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..a9fae83e --- /dev/null +++ b/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 + + + +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: + 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 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..85cff110 --- /dev/null +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h @@ -0,0 +1,83 @@ +/* +* 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: + UniformSampleFilter(); + ~UniformSampleFilter(); + + /// \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); + + // \brief Perform interpolation between p1 and p2. Time from start must be + // in between p1 and p2 times. + bool interpolatePt( + trajectory_msgs::JointTrajectoryPoint & p1, + trajectory_msgs::JointTrajectoryPoint & p2, + double time_from_start, trajectory_msgs::JointTrajectoryPoint & interp_pt); + + +private: + double sample_duration_; // @brief uniform sample duration (sec) +}; + +typedef UniformSampleFilter UniformSampleFilterAdapter; + +} + +#endif diff --git a/industrial_trajectory_filters/mainpage.dox b/industrial_trajectory_filters/mainpage.dox new file mode 100644 index 00000000..38059cd8 --- /dev/null +++ b/industrial_trajectory_filters/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b industrial_trajectory_filters + + + +--> + + +*/ diff --git a/industrial_trajectory_filters/package.xml b/industrial_trajectory_filters/package.xml new file mode 100644 index 00000000..6e2b071a --- /dev/null +++ b/industrial_trajectory_filters/package.xml @@ -0,0 +1,29 @@ + + industrial_trajectory_filters + 1.0.0 + industrial_trajectory_filters + + Shaun Edwards + BSD + http://ros.org/wiki/industrial_robot_client + Shaun Edwards + + 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..333e4fc6 --- /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..cb67e526 --- /dev/null +++ b/industrial_trajectory_filters/src/n_point_filter.cpp @@ -0,0 +1,141 @@ +/* +* 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..73459a84 --- /dev/null +++ b/industrial_trajectory_filters/src/uniform_sample_filter.cpp @@ -0,0 +1,242 @@ +/* +* 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); + +*/ + From 4789ec73204da1f090089cd8a461e045afe9c40f Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Thu, 26 Dec 2013 17:16:11 -0500 Subject: [PATCH 2/9] Cleaned up CMakeLists. Renamed library file and added install targets --- industrial_trajectory_filters/CMakeLists.txt | 56 +++++-------------- ...ng_request_adapters_plugin_description.xml | 2 +- 2 files changed, 14 insertions(+), 44 deletions(-) diff --git a/industrial_trajectory_filters/CMakeLists.txt b/industrial_trajectory_filters/CMakeLists.txt index 9efddecf..3c140a10 100644 --- a/industrial_trajectory_filters/CMakeLists.txt +++ b/industrial_trajectory_filters/CMakeLists.txt @@ -1,65 +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) -# 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 -# ) +find_package(catkin REQUIRED COMPONENTS moveit_ros_planning trajectory_msgs) -################################### -## 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} ) +include_directories(include + ${catkin_INCLUDE_DIRS} +) + ########### ## Build ## ########### -add_library(${MOVEIT_LIB_NAME} src/n_point_filter.cpp src/uniform_sample_filter.cpp) -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME} src/n_point_filter.cpp src/uniform_sample_filter.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + ############# ## Install ## ############# -#install(TARGETS ${MOVEIT_LIB_NAME} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml index 333e4fc6..5a26761c 100644 --- a/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml +++ b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml @@ -1,4 +1,4 @@ - + Date: Thu, 26 Dec 2013 17:24:24 -0500 Subject: [PATCH 3/9] Updated package.xml --- industrial_trajectory_filters/package.xml | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/industrial_trajectory_filters/package.xml b/industrial_trajectory_filters/package.xml index 6e2b071a..7f572d3b 100644 --- a/industrial_trajectory_filters/package.xml +++ b/industrial_trajectory_filters/package.xml @@ -1,12 +1,22 @@ industrial_trajectory_filters 1.0.0 - industrial_trajectory_filters - + +

+ 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_robot_client - Shaun Edwards + http://ros.org/wiki/industrial_trajectory_filters + https://github.com/ros-industrial/industrial_core/issues + https://github.com/ros-industrial/industrial_core catkin @@ -24,6 +34,5 @@ - From c0c9a8d8a428199749f0bd8753834be5734e87ea Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Thu, 26 Dec 2013 19:15:45 -0500 Subject: [PATCH 4/9] Added comments to header files, corrected formating --- .../filter_base.h | 341 ++++++++-------- .../n_point_filter.h | 110 +++--- .../uniform_sample_filter.h | 132 ++++--- .../src/n_point_filter.cpp | 181 ++++----- .../src/uniform_sample_filter.cpp | 369 +++++++++--------- 5 files changed, 587 insertions(+), 546 deletions(-) diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h index f4d2ffa0..3e76acb9 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h @@ -1,33 +1,33 @@ /* -* 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. -*/ + * 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_ @@ -39,11 +39,28 @@ #include #include -/** Trajectory filter interface: +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. * - * Description: - * This version of the FilterBase can be used to encapsulate the functionality from an arm navigation + */ +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. @@ -99,141 +116,153 @@ * 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. - */ template -class FilterBase: public planning_request_adapter::PlanningRequestAdapter -{ +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 - + /** + * @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: - /** 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_; - + /** + * @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: - /* - * 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 *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; - } - - virtual std::string getDescription() const - { - // non const pointer to this - FilterBase *p = const_cast* >(this); - - std::stringstream ss; - ss<<"Trajectory filter '"<getName()<<"' of type '"<getType()<<"'"; - return ss.str(); - } + /** + * @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 index a9fae83e..ec53c8cd 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h @@ -1,67 +1,85 @@ /* -* 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. -*/ + * 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 -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 -{ +class NPointFilter: public industrial_trajectory_filters::FilterBase { public: + /** + * @brief Default constructor + */ NPointFilter(); - ~NPointFilter(); + /** + * @brief Default destructor + */ + ~NPointFilter(); + ; + + virtual bool configure(); - /// \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). - // \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); + /** + * \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: - int n_points_; // @brief number of points to reduce trajectory to + /** + * @brief number of points to reduce trajectory to + */ + int n_points_; }; -// Specializing trajectory filter implementation +/** + * @brief Specializing trajectory filter implementation + */ typedef NPointFilter NPointFilterAdapter; } 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 index 85cff110..c3c33054 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h @@ -1,81 +1,103 @@ /* -* 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. -*/ + * 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 -*/ + #include + #include + */ -namespace industrial_trajectory_filters -{ +namespace industrial_trajectory_filters { -// \brief This is a simple filter which performs a uniforming sampling of -// a trajectory using linear interpolation. -template +/** + * \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 -{ +class UniformSampleFilter: public industrial_trajectory_filters::FilterBase { public: + /** + * @brief Default constructor + */ UniformSampleFilter(); - ~UniformSampleFilter(); - - /// \brief Configures the filter - virtual bool configure(); + /** + * @brief Default destructor + */ + ~UniformSampleFilter(); - // \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); + virtual bool configure(); - // \brief Perform interpolation between p1 and p2. Time from start must be - // in between p1 and p2 times. - bool interpolatePt( - trajectory_msgs::JointTrajectoryPoint & p1, - trajectory_msgs::JointTrajectoryPoint & p2, - double time_from_start, trajectory_msgs::JointTrajectoryPoint & interp_pt); + /** + * 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: - double sample_duration_; // @brief uniform sample duration (sec) + /** + * @brief uniform sample duration (sec) + */ + double sample_duration_; }; +/** + * @brief Specializing trajectory filter implementation + */ typedef UniformSampleFilter UniformSampleFilterAdapter; } diff --git a/industrial_trajectory_filters/src/n_point_filter.cpp b/industrial_trajectory_filters/src/n_point_filter.cpp index cb67e526..0f7c8977 100644 --- a/industrial_trajectory_filters/src/n_point_filter.cpp +++ b/industrial_trajectory_filters/src/n_point_filter.cpp @@ -1,33 +1,33 @@ /* -* 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. -*/ + * 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 @@ -35,64 +35,57 @@ using namespace industrial_trajectory_filters; -const int DEFAULT_N=2; +const int DEFAULT_N = 2; - -template -NPointFilter::NPointFilter(): - FilterBase() -{ +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 +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(); +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(); + // 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_) - { + if (size_in > n_points_) { //Add first point to output trajectory - trajectory_out.request.trajectory.points.push_back( - trajectory_in.request.trajectory.points.front()); + 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); + 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 @@ -100,31 +93,29 @@ bool NPointFilter::update(const T& trajectory_in, // 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++) - { + 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]); + 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()); + 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()); + 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; + } 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 @@ -133,9 +124,9 @@ CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, /* * Old plugin declaration for arm navigation trajectory filters -PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, - IndustrialNPointFilterJointTrajectoryWithConstraints, - industrial_trajectory_filters::NPointFilter, - filters::FilterBase); + 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 index 73459a84..2bfb6ea9 100644 --- a/industrial_trajectory_filters/src/uniform_sample_filter.cpp +++ b/industrial_trajectory_filters/src/uniform_sample_filter.cpp @@ -1,33 +1,33 @@ /* -* 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. -*/ + * 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 @@ -35,85 +35,76 @@ using namespace industrial_trajectory_filters; -const double DEFAULT_SAMPLE_DURATION=0.050; //seconds +const double DEFAULT_SAMPLE_DURATION = 0.050; //seconds - -template -UniformSampleFilter::UniformSampleFilter(): - industrial_trajectory_filters::FilterBase() -{ +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 +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_); +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; + 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_; + + } -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 ); + 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 @@ -121,122 +112,112 @@ bool UniformSampleFilter::update(const T& trajectory_in, // 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()); + 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 +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; + 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); } - - } - else - { - ROS_ERROR_STREAM("Trajectory point not fully defined, pos: " << p1.positions.size() << - " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); + 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("Time: " << time_from_start << " not between interpolation point times[" << - p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]"); + + } 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; - } - - + return rtn; +} // registering planner adapter -CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::UniformSampleFilterAdapter, +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); + PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, + IndustrialNPointFilterJointTrajectoryWithConstraints, + industrial_trajectory_filters::NPointFilter, + filters::FilterBase); -*/ + */ From 2653b2a7cf4ca639f339c2eb0c82ca4b530595ab Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Thu, 26 Dec 2013 21:26:33 -0500 Subject: [PATCH 5/9] Filled out mainpage.dox --- industrial_trajectory_filters/mainpage.dox | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/industrial_trajectory_filters/mainpage.dox b/industrial_trajectory_filters/mainpage.dox index 38059cd8..893983c5 100644 --- a/industrial_trajectory_filters/mainpage.dox +++ b/industrial_trajectory_filters/mainpage.dox @@ -2,13 +2,13 @@ \mainpage \htmlinclude manifest.html -\b industrial_trajectory_filters +\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. */ From 3a6d004c976573f7de213f6ce2a5bbb7f1d410cb Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Fri, 27 Dec 2013 11:00:38 -0500 Subject: [PATCH 6/9] Added industrial trajectory filters to core meta-package --- industrial_core/package.xml | 1 + 1 file changed, 1 insertion(+) 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 From 77663afc686a1c74047df1c79b66034637a3b4bd Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Fri, 27 Dec 2013 17:39:08 +0100 Subject: [PATCH 7/9] traj_filters: add missing install rule for plugin xml. Fix #50. --- industrial_trajectory_filters/CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/industrial_trajectory_filters/CMakeLists.txt b/industrial_trajectory_filters/CMakeLists.txt index 3c140a10..7911f47d 100644 --- a/industrial_trajectory_filters/CMakeLists.txt +++ b/industrial_trajectory_filters/CMakeLists.txt @@ -27,10 +27,9 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ############# 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}) From cd21d82df303e8dc9bf3d7f7d2c1b7723a602d44 Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Fri, 27 Dec 2013 12:31:07 -0500 Subject: [PATCH 8/9] Fixed formatting --- .../filter_base.h | 314 ++++++++--------- .../n_point_filter.h | 64 ++-- .../uniform_sample_filter.h | 81 ++--- .../src/n_point_filter.cpp | 152 ++++---- .../src/uniform_sample_filter.cpp | 328 +++++++++--------- 5 files changed, 477 insertions(+), 462 deletions(-) diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h index 3e76acb9..5591d337 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h @@ -39,22 +39,25 @@ #include #include -namespace industrial_trajectory_filters { +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; +struct MessageAdapter +{ + + /** + * @brief Arm navigation message type for trajectory filter + * + */ + struct Request + { + trajectory_msgs::JointTrajectory trajectory; + } request; }; /** @@ -117,151 +120,152 @@ struct MessageAdapter { * 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(); - } -}; + 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(); + } + }; } 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 index ec53c8cd..612d6597 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h @@ -34,48 +34,50 @@ #include -namespace industrial_trajectory_filters { +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(); - ; + class NPointFilter : public industrial_trajectory_filters::FilterBase + { + public: + /** + * @brief Default constructor + */ + NPointFilter(); + /** + * @brief Default destructor + */ + ~NPointFilter(); + ; - virtual bool configure(); + 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). - /** - * \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); + /** + * \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_; + private: + /** + * @brief number of points to reduce trajectory to + */ + int n_points_; -}; + }; /** * @brief Specializing trajectory filter implementation 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 index c3c33054..9469c066 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h @@ -42,7 +42,8 @@ #include */ -namespace industrial_trajectory_filters { +namespace industrial_trajectory_filters +{ /** * \brief This is a simple filter which performs a uniforming sampling of @@ -51,49 +52,49 @@ namespace industrial_trajectory_filters { */ template -class UniformSampleFilter: public industrial_trajectory_filters::FilterBase { -public: - /** - * @brief Default constructor - */ - UniformSampleFilter(); - /** - * @brief Default destructor - */ - ~UniformSampleFilter(); + class UniformSampleFilter : public industrial_trajectory_filters::FilterBase + { + public: + /** + * @brief Default constructor + */ + UniformSampleFilter(); + /** + * @brief Default destructor + */ + ~UniformSampleFilter(); - virtual bool configure(); + 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); + /** + * 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); + /** + * @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_; -}; + private: + /** + * @brief uniform sample duration (sec) + */ + double sample_duration_; + }; /** * @brief Specializing trajectory filter implementation diff --git a/industrial_trajectory_filters/src/n_point_filter.cpp b/industrial_trajectory_filters/src/n_point_filter.cpp index 0f7c8977..41d86dbe 100644 --- a/industrial_trajectory_filters/src/n_point_filter.cpp +++ b/industrial_trajectory_filters/src/n_point_filter.cpp @@ -38,89 +38,93 @@ 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"; -} + 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() { -} + 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; -} + 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; -} + 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); + planning_request_adapter::PlanningRequestAdapter); /* * Old plugin declaration for arm navigation trajectory filters diff --git a/industrial_trajectory_filters/src/uniform_sample_filter.cpp b/industrial_trajectory_filters/src/uniform_sample_filter.cpp index 2bfb6ea9..157c32cc 100644 --- a/industrial_trajectory_filters/src/uniform_sample_filter.cpp +++ b/industrial_trajectory_filters/src/uniform_sample_filter.cpp @@ -38,179 +38,183 @@ 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"; -} + 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() { -} + 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_); + 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; -} + 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; -} + 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; -} + 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); +CLASS_LOADER_REGISTER_CLASS( industrial_trajectory_filters::UniformSampleFilterAdapter, + planning_request_adapter::PlanningRequestAdapter); /* * Old plugin declaration for arm navigation trajectory filters From 94c453a9ee5c2ab1c345bb8d32360a3ba239639d Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Sat, 28 Dec 2013 15:36:16 +0100 Subject: [PATCH 9/9] robot_simulator: avoid hardcoded python path. Fix #53. --- industrial_robot_simulator/industrial_robot_simulator | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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