Merge pull request #3 from ipa-svn/feature/without_msgs_filter

Merge clean and working version without message filter interface: Feature/without msgs filter

Author: ipa-svn

CMakeLists.txt

@@ -6,23 +6,34 @@ find_package(catkin REQUIRED COMPONENTS
   nodelet
   roscpp
   sensor_msgs
+  tf
   tf2
   tf2_ros
   tf2_sensor_msgs
   tf2_geometry_msgs
+  pcl_ros
 )
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES pointcloud_to_laserscan ipa_pointcloud_to_laserscan
-  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros tf2_sensor_msgs tf2_geometry_msgs
+  LIBRARIES pointcloud_to_laserscan ipa_pointcloud_to_laserscan roi_outlier_removal
+  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros tf2_sensor_msgs tf2_geometry_msgs pcl_ros
 )
 
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
 )
 
+add_library(frame_publisher src/frame_publisher.cpp)
+target_link_libraries(frame_publisher ${catkin_LIBRARIES})
+
+add_executable(frame_publisher_node src/frame_publisher_node.cpp)
+target_link_libraries(frame_publisher_node frame_publisher ${catkin_LIBRARIES})
+
+add_library(roi_outlier_removal src/roi_outlier_removal_nodelet.cpp)
+target_link_libraries(roi_outlier_removal ${catkin_LIBRARIES})
+
 add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp)
 target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
 

include/pointcloud_to_laserscan/frame_publisher.h

@@ -0,0 +1,31 @@
+#ifndef FRAME_PUB_H
+#define FRAME_PUB_H
+
+#include <ros/ros.h>
+
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+
+class FramePublisher
+{
+public:
+    bool initialize();
+
+private:
+    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_;
+};
+
+#endif  // FRAME_PUB_H

include/pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet.h

@@ -49,7 +49,8 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/message_filter.h"
-#include "message_filters/subscriber.h"
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include "sensor_msgs/PointCloud2.h"
 #include <pointcloud_to_laserscan/scan_outlier_removal_filter.h>
 
@@ -72,21 +73,17 @@ namespace pointcloud_to_laserscan
     virtual void onInit();
 
     void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
-    void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
-        tf2_ros::filter_failure_reasons::FilterFailureReason reason);
 
-    void connectCb();
-
-    void disconnectCb();
+    void convert_pointcloud_to_laserscan(const sensor_msgs::PointCloud2ConstPtr &cloud, sensor_msgs::LaserScan &output, const tf2::Transform &T, const double range_min );
 
     ros::NodeHandle nh_, private_nh_;
     ros::Publisher pub_;
     boost::mutex connect_mutex_;
 
     boost::shared_ptr<tf2_ros::Buffer> tf2_;
     boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
-    message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
-    boost::shared_ptr<MessageFilter> message_filter_;
+
+	ros::Subscriber sub_;
 
     scan_outlier_filter::ScanOutlierRemovalFilter outlier_filter_;
     // ROS Parameters

include/pointcloud_to_laserscan/roi_outlier_removal.h

@@ -0,0 +1,103 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  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, Inc. 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 ROI_OUTLIER_REMOVAL_NODELET
+#define ROI_OUTLIER_REMOVAL_NODELET
+
+#include "ros/ros.h"
+#include "boost/thread/mutex.hpp"
+
+#include "nodelet/nodelet.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/message_filter.h"
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include "sensor_msgs/PointCloud2.h"
+
+// includes for pcl filtering
+#include <pcl_ros/point_cloud.h>
+
+namespace pointcloud_to_laserscan
+{
+  typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
+/**
+* The ROI outlier removal removes all points outside the specified region of interest (ROI) and publishes the remaining points into a new pointcloud.
+* The roi is defined as a z value range, angle range in the xy plane, and a max and min range in the xyy plane, for a chosen frame.
+* The filter converts the roi borders to to the original pointcloud frame in order to not have to waist time on transforming points 
+* that are not to be used later on.
+* The points within the roi are transformed to and published in the same frame as the borders were given.
+* The filter is relevant to the pointcloud to laserscan transformation since it creates a possibility to apply additional statistical filtering
+* to a reduced pointcloud before creating the scan. The loclation of this filter within the pointcloud to laserscan package is however questionable.
+*/
+  class RoiOutlierRemovalNodelet : public nodelet::Nodelet
+  {
+
+  public:
+    RoiOutlierRemovalNodelet();
+    void configure_filter();
+
+  private:
+    virtual void onInit();
+
+    void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
+
+    void reduce_point_cloud_to_roi(const pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud,  
+                                  pcl::PointCloud<pcl::PointXYZ>::Ptr reduced_pcl_cloud,
+                                  const tf2::Transform &T);
+
+    ros::NodeHandle nh_, private_nh_;
+    ros::Publisher pub_;
+
+    boost::shared_ptr<tf2_ros::Buffer> tf2_;
+    boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+
+	ros::Subscriber sub_;
+
+    // ROS Parameters
+    unsigned int input_queue_size_;
+    std::string roi_def_frame_;
+    double tolerance_;
+    double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, range_min_, range_max_;    
+  };
+
+}  // roi_outlier_removal
+
+#endif  // ROI_OUTLIER_REMOVAL_NODELET

nodelets.xml

@@ -21,3 +21,15 @@
   </class>
 
 </library>
+
+<library path="lib/libroi_outlier_removal">
+
+  <class name="pointcloud_to_laserscan/RoiOutlierRemovalNodelet"
+   type="pointcloud_to_laserscan::RoiOutlierRemovalNodelet"
+   base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet to remove all points not witin the defined field of view. The remaining points are published to a new pointcloud.
+    </description>
+  </class>
+
+</library>

package.xml

@@ -20,19 +20,23 @@
     <build_depend>nodelet</build_depend>
     <build_depend>roscpp</build_depend>
     <build_depend>sensor_msgs</build_depend>
+	<build_depend>tf</build_depend>
     <build_depend>tf2</build_depend>
     <build_depend>tf2_ros</build_depend>
     <build_depend>tf2_sensor_msgs</build_depend>
     <build_depend>tf2_geometry_msgs</build_depend>
+    <build_depend>pcl_ros</build_depend>
 
     <run_depend>message_filters</run_depend>
     <run_depend>nodelet</run_depend>
     <run_depend>roscpp</run_depend>
     <run_depend>sensor_msgs</run_depend>
+	<run_depend>tf</run_depend>
     <run_depend>tf2</run_depend>
     <run_depend>tf2_ros</run_depend>
     <run_depend>tf2_sensor_msgs</run_depend>
     <run_depend>tf2_geometry_msgs</run_depend>
+    <run_depend>pcl_ros</run_depend>
 
     <export>
         <nodelet plugin="${prefix}/nodelets.xml"/>

src/frame_publisher.cpp

@@ -0,0 +1,55 @@
+#include <pointcloud_to_laserscan/frame_publisher.h>
+
+bool FramePublisher::initialize()
+{
+	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<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);
+
+    ros::Duration(1.0).sleep(); //give tf_listener some time
+
+    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_));
+}

src/frame_publisher_node.cpp

@@ -0,0 +1,17 @@
+#include <ros/ros.h>
+#include <pointcloud_to_laserscan/frame_publisher.h>
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "msh_frame_publisher_node");
+    FramePublisher* ad = new FramePublisher();
+
+    if (!ad->initialize())
+    {
+        ROS_ERROR("Failed to initialize FramePublisher");
+        return -1;
+    }
+
+    ros::spin();
+    return 0;
+}