Made most changes suggested by reviewer. Good Review, I was sloppy in…

… many places.

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<double> coef);
-
-  /*!  \brief Destructor */
-  ~SmoothingTrajectoryFilter();
+  bool init(std::vector<double> &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

industrial_trajectory_filters/src/add_smoothing_filter.cpp

@@ -50,57 +50,59 @@ 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);
       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_)){
+    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<double> 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<double> temp_coef;
-	  for(int i=0; i<num_coef; i++){
-	    char post[100];
-	    sprintf(post, "/coeficient_%d", i);
-	    std::string coef_name = filter_base_name_ + post;
-	    double coef;
-	    if(!nh_.getParam(coef_name, coef)){ // this order of coef exist for this base name 
-	      ROS_INFO_STREAM("Param '" << coef_name << "' was not set. Using default filter values " );
-	      break;
-	    }
-	    temp_coef.push_back(coef);
-	  }
-	  filter_coef_.clear();
-	  for(int i=0; i<num_coef; i++){ // install the filter coefficients
-	    filter_coef_.push_back(temp_coef[i]);
-	  }
-	} // odd number of coef?
+      }
+      else{
+	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_);
+  }
+
+  /*!  \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<double> filter_coef_;
 
 };

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<double> coef)
+  bool SmoothingTrajectoryFilter::init(std::vector<double> &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);