From 19b6a44f7a6c89c8c980f4a1acb98a2e489b7228 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 30 May 2014 08:45:07 -0500 Subject: [PATCH] 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);