From 1c7861455b693544d3908600f7be308cfdb7d7da Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Thu, 29 May 2014 13:17:22 -0500 Subject: [PATCH 1/6] Added a smoothing filter as a planning request adapter plugin --- industrial_trajectory_filters/CMakeLists.txt | 7 +- .../smoothing_trajectory_filter.h | 74 +++++++++ ...ng_request_adapters_plugin_description.xml | 17 ++ .../src/add_smoothing_filter.cpp | 142 ++++++++++++++++ .../src/smoothing_trajectory_filter.cpp | 155 ++++++++++++++++++ 5 files changed, 394 insertions(+), 1 deletion(-) create mode 100644 industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h create mode 100644 industrial_trajectory_filters/src/add_smoothing_filter.cpp create mode 100644 industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp diff --git a/industrial_trajectory_filters/CMakeLists.txt b/industrial_trajectory_filters/CMakeLists.txt index 88d77a3d..87adc712 100644 --- a/industrial_trajectory_filters/CMakeLists.txt +++ b/industrial_trajectory_filters/CMakeLists.txt @@ -15,7 +15,12 @@ include_directories(include ) -add_library(${PROJECT_NAME} src/n_point_filter.cpp src/uniform_sample_filter.cpp) +add_library(${PROJECT_NAME} + src/n_point_filter.cpp + src/uniform_sample_filter.cpp + src/add_smoothing_filter.cpp + src/smoothing_trajectory_filter.cpp +) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h new file mode 100644 index 00000000..3313ec0d --- /dev/null +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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 Willow Garage 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. + *********************************************************************/ + +/* Author: Chris Lewis */ + +#ifndef MOVEIT_TRAJECTORY_PROCESSING_SMOOTHING_TRAJECTORY_FILTER_ +#define MOVEIT_TRAJECTORY_PROCESSING_SMOOTHING_TRAJECTORY_FILTER_ + +#include + +namespace industrial_trajectory_filters +{ + +/// \brief This class filters the trajectory using a Finite Impluse Response filter +class SmoothingTrajectoryFilter +{ +public: + /*! \brief Constructor */ + SmoothingTrajectoryFilter(); + + /*! \brief Constructor + * @param coef a vector of Smoothing coeficients with an odd number of values + * @return true if the number of coeficients is odd, otherwise, false. All smoothing filters have odd number of coef + */ + bool Init(std::vector coef); + + /*! \brief Destructor */ + ~SmoothingTrajectoryFilter(); + + /* \brief action of filter depends on the coefficients, intended to be a low pass filter + * @param rob_trajectory A robot_trajectory::RobotTrajectory to be filtered + */ + bool applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory); + +private: + double gain_; /*!< gain_ is the sum of the coeficients to achieve unity gain overall */ + int num_coef_; /*< the number of coefficients */ + std::vector coef_; /*!< Vector of coefficients */ + bool initialized_; /*!< was the Init() function called sucessfully? */ +}; + +} +#endif diff --git a/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml index 5a26761c..e37e17e0 100644 --- a/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml +++ b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml @@ -20,5 +20,22 @@ - sample_duration (default = 0.050) + + + This filter replaces each point with a weighted average of the adjacent + waypoints/states in the trajectory. Odd number of weights are provided + via a yaml file loaded upon launch onto the ROS parameter server. + ROS Parameters Example: + smoothing_filter_name: /move_group/smoothing_5_coef + smoothing_5_coef: + num_coeficients: 5 + coeficient_0: 0.25 + coeficient_1: 0.50 + coeficient_2: 1.00 + coeficient_3: 0.50 + coeficient_4: 0.25 + + + diff --git a/industrial_trajectory_filters/src/add_smoothing_filter.cpp b/industrial_trajectory_filters/src/add_smoothing_filter.cpp new file mode 100644 index 00000000..c935b144 --- /dev/null +++ b/industrial_trajectory_filters/src/add_smoothing_filter.cpp @@ -0,0 +1,142 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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 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. + *********************************************************************/ + +/* Author: Chris Lewis */ + +#include +#include + +#include +#include // required for NodeHandle +#include + +namespace industrial_trajectory_filters +{ + +class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapter +{ +public: + + static const std::string FILTER_PARAMETER_NAME_; // base name for filter parameters + + AddSmoothingFilter() : planning_request_adapter::PlanningRequestAdapter(), nh_("~") + { + int num_coef; + + // set of default filter coefficients in case paramter, or its associated file, or its syntax is wrong + filter_coef_.push_back(0.25); + filter_coef_.push_back(0.5); + filter_coef_.push_back(1.0); + filter_coef_.push_back(0.5); + filter_coef_.push_back(0.25); + + // see if a new set of filter parameters is on the parameter server, if so, install them + if (!nh_.getParam(FILTER_PARAMETER_NAME_, filter_base_name_)){ + ROS_INFO_STREAM("Param '" << FILTER_PARAMETER_NAME_ << "' was not set. Using default filter values " ); + } + else{ // base filter name exists + std::string filter_order_param = filter_base_name_ + "/num_coeficients"; + if(!nh_.getParam(filter_order_param, num_coef)){ + ROS_INFO_STREAM("Param '" << filter_order_param << "' was not set. Using default filter values " ); + } + else{ // num_coef exists for this base name + if(num_coef %2 == 0){ + ROS_ERROR("Smoothing filters must have odd number of coeficients"); + } + else{ + std::vector temp_coef; + for(int i=0; iInit(filter_coef_)) + ROS_ERROR("Initialization error on smoothing filter. Requires an odd number of coeficients"); + }; + + virtual std::string getDescription() const { return "Add Smoothing Trajectory Filter"; } + + 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 + { + // do anything prior to calling the planner here + // .... + // IN this case nothing was done + + // call the nested planner or adapter + bool result = planner(planning_scene, req, res); + + // do anything after calling the nested planner or adapters here + if (result && res.trajectory_) + { + ROS_DEBUG("Running '%s'", getDescription().c_str()); // inform the user + if (!smoothing_filter_->applyFilter(*res.trajectory_)) // do the work of smoothing using a smoothing object + ROS_WARN("Smoothing filter of the solution path failed."); + } + + return result; + } + + +private: + ros::NodeHandle nh_; + industrial_trajectory_filters::SmoothingTrajectoryFilter *smoothing_filter_; + std::string filter_base_name_; + std::vector filter_coef_; + +}; + +const std::string AddSmoothingFilter::FILTER_PARAMETER_NAME_ = "/move_group/smoothing_filter_name"; + +} + +CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::AddSmoothingFilter, + planning_request_adapter::PlanningRequestAdapter); diff --git a/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp new file mode 100644 index 00000000..aa26b164 --- /dev/null +++ b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp @@ -0,0 +1,155 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, 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 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. + *********************************************************************/ + +/* Author: Chris Lewis */ + +#include +#include +#include +#include + +#include +#include + +namespace industrial_trajectory_filters +{ + namespace{ +void print_tragectory_to_matlab_format(std::string filename, std::string mat_name, robot_trajectory::RobotTrajectory& rob_trajectory) + { + FILE *fp = fopen(filename.c_str(),"w"); + const int num_points = rob_trajectory.getWayPointCount(); + const int num_states = rob_trajectory.getWayPoint(0).getVariableCount(); + fprintf(fp, "%s = [\n",mat_name.c_str()); + for(int i=0; i coef) + { + if(coef.size()%2 == 1) { // smoothing filters must have an odd number of coefficients + initialized_ = true; + num_coef_ = coef.size(); + double sum =0; + for(int i=0; i xv; + + // filter each variable independently + for(int i=0; isetVariablePosition(i,sum/gain_); // j'th waypoint, i'th variable set to output value + + }// end for every waypoint + + } // end for every state + + print_tragectory_to_matlab_format("/home/clewis/test2.m","A2", rob_trajectory); + + // TODO don't be lazy, do some error checking!! + return(true); + +}// end SmoothingTrajectoryFilter::applyfilter() + +} // end namespace From 2069beb6fc50c18905e98323ac4863fd47f003ba Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Thu, 29 May 2014 13:47:53 -0500 Subject: [PATCH 2/6] removed call to anonomous function used only for debugging --- .../src/smoothing_trajectory_filter.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp index aa26b164..fb05dfe3 100644 --- a/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp +++ b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp @@ -96,7 +96,7 @@ void print_tragectory_to_matlab_format(std::string filename, std::string mat_na { if(!initialized_) return(false); - print_tragectory_to_matlab_format("/home/clewis/test1.m","A1", rob_trajectory); + // print_tragectory_to_matlab_format("/home/clewis/test1.m","A1", rob_trajectory); const int num_points = rob_trajectory.getWayPointCount(); if(num_points <=2) return(false); // nothing to do here, can't change either first or last point const int num_states = rob_trajectory.getWayPoint(0).getVariableCount(); @@ -145,9 +145,8 @@ void print_tragectory_to_matlab_format(std::string filename, std::string mat_na } // end for every state - print_tragectory_to_matlab_format("/home/clewis/test2.m","A2", rob_trajectory); + //print_tragectory_to_matlab_format("/home/clewis/test2.m","A2", rob_trajectory); - // TODO don't be lazy, do some error checking!! return(true); }// end SmoothingTrajectoryFilter::applyfilter() From 19b6a44f7a6c89c8c980f4a1acb98a2e489b7228 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 30 May 2014 08:45:07 -0500 Subject: [PATCH 3/6] Made most changes suggested by reviewer. Good Review, I was sloppy in many places. --- .../smoothing_trajectory_filter.h | 10 ++-- .../src/add_smoothing_filter.cpp | 60 ++++++++++--------- .../src/smoothing_trajectory_filter.cpp | 8 +-- 3 files changed, 40 insertions(+), 38 deletions(-) diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h index 3313ec0d..18439a63 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h @@ -42,21 +42,21 @@ namespace industrial_trajectory_filters { -/// \brief This class filters the trajectory using a Finite Impluse Response filter + /*! \brief This class filters the trajectory using a Finite Impluse Response filter */ class SmoothingTrajectoryFilter { public: /*! \brief Constructor */ SmoothingTrajectoryFilter(); + /*! \brief Destructor */ + ~SmoothingTrajectoryFilter(); + /*! \brief Constructor * @param coef a vector of Smoothing coeficients with an odd number of values * @return true if the number of coeficients is odd, otherwise, false. All smoothing filters have odd number of coef */ - bool Init(std::vector coef); - - /*! \brief Destructor */ - ~SmoothingTrajectoryFilter(); + bool init(std::vector &coef); /* \brief action of filter depends on the coefficients, intended to be a low pass filter * @param rob_trajectory A robot_trajectory::RobotTrajectory to be filtered diff --git a/industrial_trajectory_filters/src/add_smoothing_filter.cpp b/industrial_trajectory_filters/src/add_smoothing_filter.cpp index c935b144..a23f7ccc 100644 --- a/industrial_trajectory_filters/src/add_smoothing_filter.cpp +++ b/industrial_trajectory_filters/src/add_smoothing_filter.cpp @@ -50,11 +50,15 @@ class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapt static const std::string FILTER_PARAMETER_NAME_; // base name for filter parameters + /*! \brief Constructor AddSmoothingFilter is a planning request adapter plugin which post-processes + * The robot's trajectory to round out corners + * The filter name and then its coefficients are obtained from the ROS parameter server + */ AddSmoothingFilter() : planning_request_adapter::PlanningRequestAdapter(), nh_("~") { int num_coef; - // set of default filter coefficients in case paramter, or its associated file, or its syntax is wrong + // set of default filter coefficients in case paramter, or its associated file, or its syntax is wrong filter_coef_.push_back(0.25); filter_coef_.push_back(0.5); filter_coef_.push_back(1.0); @@ -62,45 +66,43 @@ class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapt filter_coef_.push_back(0.25); // see if a new set of filter parameters is on the parameter server, if so, install them - if (!nh_.getParam(FILTER_PARAMETER_NAME_, filter_base_name_)){ + if (!nh_.getParam(FILTER_PARAMETER_NAME_, filter_name_)){ ROS_INFO_STREAM("Param '" << FILTER_PARAMETER_NAME_ << "' was not set. Using default filter values " ); } else{ // base filter name exists - std::string filter_order_param = filter_base_name_ + "/num_coeficients"; - if(!nh_.getParam(filter_order_param, num_coef)){ - ROS_INFO_STREAM("Param '" << filter_order_param << "' was not set. Using default filter values " ); - } - else{ // num_coef exists for this base name - if(num_coef %2 == 0){ - ROS_ERROR("Smoothing filters must have odd number of coeficients"); + std::vector temp_coef; + nh_.getParam(filter_name_.c_str(),temp_coef); // no need to check for success + if(temp_coef.size()%2 == 1 && temp_coef.size()>2){ // need to have odd number of coefficients greater than 2 + filter_coef_.clear(); + for(int i=0; i< (int) temp_coef.size(); i++){ // install the filter coefficients + filter_coef_.push_back(temp_coef[i]); } - else{ - std::vector temp_coef; - for(int i=0; iInit(filter_coef_)) + if(!smoothing_filter_->init(filter_coef_)) ROS_ERROR("Initialization error on smoothing filter. Requires an odd number of coeficients"); + }; + /*! \brief Destructor */ + ~AddSmoothingFilter(){ + delete(smoothing_filter_); + } + + /*! \brief Returns a short description of this plugin */ virtual std::string getDescription() const { return "Add Smoothing Trajectory Filter"; } + /*! \brief The work hourse of planning request adapters + * \param planner A function called somewhere within this subroutine + * \param planning_scene an object describing the objects and kinematics + * \param req the request, includes the starting config, the goal, constraints, etc + * \param res the response, includes the robot trajectory and other info + * \param added_path_index, a index of the points added by this adapter, which in this case will be empty + */ virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest &req, @@ -129,7 +131,7 @@ class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapt private: ros::NodeHandle nh_; industrial_trajectory_filters::SmoothingTrajectoryFilter *smoothing_filter_; - std::string filter_base_name_; + std::string filter_name_; std::vector filter_coef_; }; diff --git a/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp index fb05dfe3..062caf38 100644 --- a/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp +++ b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp @@ -45,7 +45,7 @@ namespace industrial_trajectory_filters { namespace{ -void print_tragectory_to_matlab_format(std::string filename, std::string mat_name, robot_trajectory::RobotTrajectory& rob_trajectory) +void printTragectoryToMatlabFormat(std::string filename, std::string mat_name, robot_trajectory::RobotTrajectory& rob_trajectory) { FILE *fp = fopen(filename.c_str(),"w"); const int num_points = rob_trajectory.getWayPointCount(); @@ -68,7 +68,7 @@ void print_tragectory_to_matlab_format(std::string filename, std::string mat_na initialized_ = false; } - bool SmoothingTrajectoryFilter::Init(std::vector coef) + bool SmoothingTrajectoryFilter::init(std::vector &coef) { if(coef.size()%2 == 1) { // smoothing filters must have an odd number of coefficients initialized_ = true; @@ -96,7 +96,7 @@ void print_tragectory_to_matlab_format(std::string filename, std::string mat_na { if(!initialized_) return(false); - // print_tragectory_to_matlab_format("/home/clewis/test1.m","A1", rob_trajectory); + //Analyse Freq Response in Matlab?==> printTrajectoryToMatlabFormat("full_file1_path","Name_of_Matrix1", rob_trajectory); const int num_points = rob_trajectory.getWayPointCount(); if(num_points <=2) return(false); // nothing to do here, can't change either first or last point const int num_states = rob_trajectory.getWayPoint(0).getVariableCount(); @@ -145,7 +145,7 @@ void print_tragectory_to_matlab_format(std::string filename, std::string mat_na } // end for every state - //print_tragectory_to_matlab_format("/home/clewis/test2.m","A2", rob_trajectory); + //Analyse Filter Response in Matlab?==> printTrajectoryToMatlabFormat("full_file2_path","Name_of_Matrix2", rob_trajectory); return(true); From 51cf27275b580d4fdfa58f6eb67db47e88a8bc95 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 30 May 2014 09:58:25 -0500 Subject: [PATCH 4/6] Minor changes requested by reviewer --- .../smoothing_trajectory_filter.h | 2 +- .../planning_request_adapters_plugin_description.xml | 11 +++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h index 18439a63..251c56bd 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h @@ -67,7 +67,7 @@ class SmoothingTrajectoryFilter double gain_; /*!< gain_ is the sum of the coeficients to achieve unity gain overall */ int num_coef_; /*< the number of coefficients */ std::vector coef_; /*!< Vector of coefficients */ - bool initialized_; /*!< was the Init() function called sucessfully? */ + bool initialized_; /*!< was the init() function called sucessfully? */ }; } diff --git a/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml index e37e17e0..71917ceb 100644 --- a/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml +++ b/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml @@ -28,12 +28,11 @@ ROS Parameters Example: smoothing_filter_name: /move_group/smoothing_5_coef smoothing_5_coef: - num_coeficients: 5 - coeficient_0: 0.25 - coeficient_1: 0.50 - coeficient_2: 1.00 - coeficient_3: 0.50 - coeficient_4: 0.25 + - 0.25 + - 0.50 + - 1.00 + - 0.50 + - 0.25 From ff01a7d056e8de0298ae49234dbe94dc5a81261b Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 30 May 2014 10:40:30 -0500 Subject: [PATCH 5/6] removed anonymous namespace code used only for testing Added a try/catch to running filer, but it does not need to exit --- .../smoothing_trajectory_filter.h | 2 +- .../src/add_smoothing_filter.cpp | 25 +++++++++++-------- .../src/smoothing_trajectory_filter.cpp | 22 +--------------- 3 files changed, 16 insertions(+), 33 deletions(-) diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h index 251c56bd..2ab7c5c9 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/smoothing_trajectory_filter.h @@ -61,7 +61,7 @@ class SmoothingTrajectoryFilter /* \brief action of filter depends on the coefficients, intended to be a low pass filter * @param rob_trajectory A robot_trajectory::RobotTrajectory to be filtered */ - bool applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory); + bool applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory) const; private: double gain_; /*!< gain_ is the sum of the coeficients to achieve unity gain overall */ diff --git a/industrial_trajectory_filters/src/add_smoothing_filter.cpp b/industrial_trajectory_filters/src/add_smoothing_filter.cpp index a23f7ccc..e611ffce 100644 --- a/industrial_trajectory_filters/src/add_smoothing_filter.cpp +++ b/industrial_trajectory_filters/src/add_smoothing_filter.cpp @@ -82,16 +82,13 @@ class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapt ROS_INFO_STREAM("Could not read filter, using default filter coefficients"); } } - smoothing_filter_ = new industrial_trajectory_filters::SmoothingTrajectoryFilter(); //construct the filter - if(!smoothing_filter_->init(filter_coef_)) + if(!smoothing_filter_.init(filter_coef_)) ROS_ERROR("Initialization error on smoothing filter. Requires an odd number of coeficients"); }; /*! \brief Destructor */ - ~AddSmoothingFilter(){ - delete(smoothing_filter_); - } + ~AddSmoothingFilter(){ }; /*! \brief Returns a short description of this plugin */ virtual std::string getDescription() const { return "Add Smoothing Trajectory Filter"; } @@ -120,17 +117,23 @@ class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapt if (result && res.trajectory_) { ROS_DEBUG("Running '%s'", getDescription().c_str()); // inform the user - if (!smoothing_filter_->applyFilter(*res.trajectory_)) // do the work of smoothing using a smoothing object - ROS_WARN("Smoothing filter of the solution path failed."); - } - + try{ + if (!smoothing_filter_.applyFilter(*res.trajectory_)) // do the work + { + throw 31415; + } + } // end of try + catch(int x){ + ROS_ERROR("Smoothing filter of the solution path failed. Filter Not Initialized "); + }// end of catch + }// end of if successful plan return result; - } + }; private: ros::NodeHandle nh_; - industrial_trajectory_filters::SmoothingTrajectoryFilter *smoothing_filter_; + industrial_trajectory_filters::SmoothingTrajectoryFilter smoothing_filter_; std::string filter_name_; std::vector filter_coef_; diff --git a/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp index 062caf38..6828ec27 100644 --- a/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp +++ b/industrial_trajectory_filters/src/smoothing_trajectory_filter.cpp @@ -44,23 +44,6 @@ namespace industrial_trajectory_filters { - namespace{ -void printTragectoryToMatlabFormat(std::string filename, std::string mat_name, robot_trajectory::RobotTrajectory& rob_trajectory) - { - FILE *fp = fopen(filename.c_str(),"w"); - const int num_points = rob_trajectory.getWayPointCount(); - const int num_states = rob_trajectory.getWayPoint(0).getVariableCount(); - fprintf(fp, "%s = [\n",mat_name.c_str()); - for(int i=0; i printTrajectoryToMatlabFormat("full_file1_path","Name_of_Matrix1", rob_trajectory); const int num_points = rob_trajectory.getWayPointCount(); if(num_points <=2) return(false); // nothing to do here, can't change either first or last point const int num_states = rob_trajectory.getWayPoint(0).getVariableCount(); @@ -145,8 +127,6 @@ void printTragectoryToMatlabFormat(std::string filename, std::string mat_name, } // end for every state - //Analyse Filter Response in Matlab?==> printTrajectoryToMatlabFormat("full_file2_path","Name_of_Matrix2", rob_trajectory); - return(true); }// end SmoothingTrajectoryFilter::applyfilter() From bc40d6b7e3bec63d160737bde7646506bf45aa3e Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Mon, 23 Jun 2014 14:46:06 -0500 Subject: [PATCH 6/6] removed try/catch/throw that had no point. --- .../src/add_smoothing_filter.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/industrial_trajectory_filters/src/add_smoothing_filter.cpp b/industrial_trajectory_filters/src/add_smoothing_filter.cpp index e611ffce..639b5409 100644 --- a/industrial_trajectory_filters/src/add_smoothing_filter.cpp +++ b/industrial_trajectory_filters/src/add_smoothing_filter.cpp @@ -114,18 +114,12 @@ class AddSmoothingFilter : public planning_request_adapter::PlanningRequestAdapt bool result = planner(planning_scene, req, res); // do anything after calling the nested planner or adapters here - if (result && res.trajectory_) + if (result && res.trajectory_) // successful plan { ROS_DEBUG("Running '%s'", getDescription().c_str()); // inform the user - try{ - if (!smoothing_filter_.applyFilter(*res.trajectory_)) // do the work - { - throw 31415; - } - } // end of try - catch(int x){ + if (!smoothing_filter_.applyFilter(*res.trajectory_)) {// do the smoothing ROS_ERROR("Smoothing filter of the solution path failed. Filter Not Initialized "); - }// end of catch + } }// end of if successful plan return result; };