Cleanup indentation, function names, and license agreement.

Author: svn

include/pointcloud_to_laserscan/frame_publisher.h

@@ -1,3 +1,43 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Fraunhofer IPA.
+ *  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 copyright holder 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: Sofie Nilsson
+ */
+
 #ifndef FRAME_PUB_H
 #define FRAME_PUB_H
 
@@ -10,22 +50,22 @@
 class FramePublisher
 {
 public:
-    bool initialize();
+  bool initialize();
 
 private:
-    void frameBroadcastCallback(const ros::TimerEvent& event);
+  void frameBroadcastCallback(const ros::TimerEvent& event);
 
 private:
-    ros::NodeHandle nh_, priv_nh_;
-    tf::TransformListener tf_listener_;
-    tf::TransformBroadcaster tf_broadcaster_;
-
-    ros::Timer frame_broadcast_timer_;
-    double update_rate_;
-	std::string base_frame_;
-    std::string rotation_frame_;
-    std::string target_frame_;
-	bool rot_z_, rot_x_, rot_y_;
+  ros::NodeHandle nh_, priv_nh_;
+  tf::TransformListener tf_listener_;
+  tf::TransformBroadcaster tf_broadcaster_;
+
+  ros::Timer frame_broadcast_timer_;
+  double update_rate_;
+  std::string base_frame_;
+  std::string rotation_frame_;
+  std::string target_frame_;
+  bool rot_z_, rot_x_, rot_y_;
 };
 
 #endif  // FRAME_PUB_H

include/pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet.h

@@ -2,6 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  Copyright (c) 2015, Fraunhofer IPA.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
  *     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, Inc. nor the names of its
+ *   * Neither the name of the copyright holders nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -33,7 +34,7 @@
  *
  *
  */
-
+ 
 /*
  * Author: Paul Bovbel
  * Author: Sofie Nilsson
@@ -83,7 +84,7 @@ namespace pointcloud_to_laserscan
     boost::shared_ptr<tf2_ros::Buffer> tf2_;
     boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
 
-	ros::Subscriber sub_;
+    ros::Subscriber sub_;
 
     scan_outlier_filter::ScanOutlierRemovalFilter outlier_filter_;
     // ROS Parameters

include/pointcloud_to_laserscan/roi_outlier_removal.h

@@ -1,7 +1,7 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  Copyright (c) 2016, Fraunhofer IPA.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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, Inc. nor the names of its
+ *   * Neither the name of the copyright holder nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -33,7 +33,7 @@
  *
  *
  */
-
+ 
 /*
  * Author: Sofie Nilsson
  */
@@ -72,7 +72,7 @@ namespace pointcloud_to_laserscan
 
   public:
     RoiOutlierRemovalNodelet();
-    void configure_filter();
+    void configure_roi_settings();
 
   private:
     virtual void onInit();
@@ -89,7 +89,7 @@ namespace pointcloud_to_laserscan
     boost::shared_ptr<tf2_ros::Buffer> tf2_;
     boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
 
-	ros::Subscriber sub_;
+    ros::Subscriber sub_;
 
     // ROS Parameters
     unsigned int input_queue_size_;

include/pointcloud_to_laserscan/scan_outlier_removal_filter.h

@@ -2,7 +2,7 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2010-2012, Fraunhofer IPA
+ *  Copyright (c) 2016, Fraunhofer IPA.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -15,7 +15,7 @@
  *     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, Inc. nor the names of its
+ *   * Neither the name of the copyright holder nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -39,31 +39,37 @@
  * Author: Sofie Nilsson
  */
 
+/* 
+ * The scan_outlier_removal_filter removes noise clusters with much smaller range than the surrounding points. 
+ * The filter can be configured with the following three parameters cluster_break_distance, max_noise_cluster_size, 
+ * max_noise_cluster_distance. 
+ */
+
 #ifndef IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
 #define IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
 
 #include <sensor_msgs/LaserScan.h>
 
-namespace scan_outlier_filter
-{
-	class ScanOutlierRemovalFilter
-	{
-	private:
-		bool filter_configured_;
-		double cluster_break_distance_; 
-		int max_noise_cluster_size_; 
-		double max_noise_cluster_distance_;
+ namespace scan_outlier_filter
+ {
+   class ScanOutlierRemovalFilter
+   {
+   private:
+    bool filter_configured_;
+    double cluster_break_distance_; 
+    int max_noise_cluster_size_; 
+    double max_noise_cluster_distance_;
 
-	public:
-		ScanOutlierRemovalFilter(): 
-			filter_configured_(false), 
-			cluster_break_distance_(0.0), 
-			max_noise_cluster_size_(0), 
-			max_noise_cluster_distance_(0.0)
-			{};
-		void configure(const double cluster_break_distance, const int max_noise_cluster_size, const double max_noise_cluster_distance);
+  public:
+    ScanOutlierRemovalFilter(): 
+    filter_configured_(false), 
+    cluster_break_distance_(0.0), 
+    max_noise_cluster_size_(0), 
+    max_noise_cluster_distance_(0.0)
+    {};
+    void configure(const double cluster_break_distance, const int max_noise_cluster_size, const double max_noise_cluster_distance);
 
-		void remove_outliers(sensor_msgs::LaserScan &scan);
-	};
+    void remove_outliers(sensor_msgs::LaserScan &scan);
+  };
 }
 #endif //IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER

src/frame_publisher.cpp

@@ -1,55 +1,96 @@
+
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Fraunhofer IPA.
+ *  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 copyright holder 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: Sofie Nilsson
+ */
+ 
 #include <pointcloud_to_laserscan/frame_publisher.h>
 
 bool FramePublisher::initialize()
 {
-	priv_nh_ = ros::NodeHandle("~");
+  priv_nh_ = ros::NodeHandle("~");
+
+  priv_nh_.param<double>("update_rate", update_rate_, 0.01); // 100Hz
+
+  priv_nh_.param<std::string>("base_frame", base_frame_, "base_link");
+  priv_nh_.param<std::string>("rotation_frame", rotation_frame_, "torso_center_link");
+  priv_nh_.param<std::string>("target_frame", target_frame_, "torso_rotated_base_link");
 
-	priv_nh_.param<double>("update_rate", update_rate_, 0.01); // 100Hz
+  priv_nh_.param<bool>("rot_z", rot_z_, false);
+  priv_nh_.param<bool>("rot_x", rot_x_, false);
+  priv_nh_.param<bool>("rot_y", rot_y_, false);
 
-	priv_nh_.param<std::string>("base_frame", base_frame_, "base_link");
-	priv_nh_.param<std::string>("rotation_frame", rotation_frame_, "torso_center_link");
-	priv_nh_.param<std::string>("target_frame", target_frame_, "torso_rotated_base_link");
-    
-	priv_nh_.param<bool>("rot_z", rot_z_, false);
-	priv_nh_.param<bool>("rot_x", rot_x_, false);
-	priv_nh_.param<bool>("rot_y", rot_y_, false);
-        
-    frame_broadcast_timer_ = nh_.createTimer(ros::Duration(update_rate_), &FramePublisher::frameBroadcastCallback, this);
+  frame_broadcast_timer_ = nh_.createTimer(ros::Duration(update_rate_), &FramePublisher::frameBroadcastCallback, this);
 
-    ros::Duration(1.0).sleep(); //give tf_listener some time
+  ros::Duration(1.0).sleep(); //give tf_listener some time
 
-    return true;
+  return true;
 }
 
 /// Broadcast a frame aligned with the base frame but rotated around specified axes as rotation_frame 
 void FramePublisher::frameBroadcastCallback(const ros::TimerEvent& event)
 {   
-    tf::StampedTransform frame_transform;
-    try
-    {
-        tf_listener_.waitForTransform(base_frame_, rotation_frame_, event.current_real, ros::Duration(0.1));
-        tf_listener_.lookupTransform(base_frame_, rotation_frame_, ros::Time(0), frame_transform);
-    }
-    catch (tf::TransformException& ex)
-    {
-        ROS_ERROR("FramePublisher::getTransform: \n%s", ex.what());
-        return;
-    }
-
-    double rot_frame_roll, rot_frame_pitch, rot_frame_yaw;
-    frame_transform.getBasis().getRPY(rot_frame_roll, rot_frame_pitch, rot_frame_yaw);
-	tf::Transform target_transform(frame_transform.getRotation());
-
-	// Use rotations according to settings
-	double target_frame_roll = 0;
-	double target_frame_pitch = 0;
-	double target_frame_yaw = 0; 
-	if (rot_z_){ target_frame_yaw = rot_frame_yaw;}
-	if (rot_x_){ target_frame_roll = rot_frame_roll;}
-	if (rot_y_){ target_frame_pitch = rot_frame_pitch;}
-
-	target_transform.getBasis().setRPY(target_frame_roll, target_frame_pitch, target_frame_yaw);
-
-	// Broadcast new frame
-    tf_broadcaster_.sendTransform(tf::StampedTransform(target_transform, frame_transform.stamp_, base_frame_, target_frame_));
+  tf::StampedTransform frame_transform;
+  try
+  {
+    tf_listener_.waitForTransform(base_frame_, rotation_frame_, event.current_real, ros::Duration(0.1));
+    tf_listener_.lookupTransform(base_frame_, rotation_frame_, ros::Time(0), frame_transform);
+  }
+  catch (tf::TransformException& ex)
+  {
+    ROS_ERROR("FramePublisher::getTransform: \n%s", ex.what());
+    return;
+  }
+
+  double rot_frame_roll, rot_frame_pitch, rot_frame_yaw;
+  frame_transform.getBasis().getRPY(rot_frame_roll, rot_frame_pitch, rot_frame_yaw);
+  tf::Transform target_transform(frame_transform.getRotation());
+
+  // Use rotations according to settings
+  double target_frame_roll = 0;
+  double target_frame_pitch = 0;
+  double target_frame_yaw = 0; 
+  if (rot_z_){ target_frame_yaw = rot_frame_yaw;}
+  if (rot_x_){ target_frame_roll = rot_frame_roll;}
+  if (rot_y_){ target_frame_pitch = rot_frame_pitch;}
+
+  target_transform.getBasis().setRPY(target_frame_roll, target_frame_pitch, target_frame_yaw);
+
+  // Broadcast new frame
+  tf_broadcaster_.sendTransform(tf::StampedTransform(target_transform, frame_transform.stamp_, base_frame_, target_frame_));
 }