removed anonymous namespace code used only for testing

Added a try/catch to running filer, but it does not need to exit

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 */

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<double> filter_coef_;
 

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<num_points; i++){
-	for(int j=0; j<num_states; j++){
-	  fprintf(fp,"%lf ", rob_trajectory.getWayPoint(i).getVariablePosition(j)); // j'th state of i'th waypoint
-	}
-	fprintf(fp, ";\n");
-      }
-      fprintf(fp, "];\n");
-      fclose(fp);
-    }// end of print function
-  } // end of un named namespace
 
 
   SmoothingTrajectoryFilter::SmoothingTrajectoryFilter()
@@ -92,11 +75,10 @@ void  printTragectoryToMatlabFormat(std::string filename, std::string mat_name,
     coef_.clear();
   }
 
- bool SmoothingTrajectoryFilter::applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory)
+ bool SmoothingTrajectoryFilter::applyFilter(robot_trajectory::RobotTrajectory& rob_trajectory) const
   {
     if(!initialized_) return(false);
 
-    //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,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()