Feature/points map filter (autowarefoundation#1658)

* add points_map_filter node

* add passthrough filter

* fix filter function

* apply clang-format

* add README.md

ros/src/data/packages/map_file/CMakeLists.txt

@@ -9,6 +9,9 @@ find_package(catkin REQUIRED COMPONENTS
   visualization_msgs
   geometry_msgs
   tf
+  pcl_ros
+  tf2_ros
+  tf2_geometry_msgs
   vector_map
   autoware_msgs
 )
@@ -60,6 +63,10 @@ add_dependencies(points_map_loader ${catkin_EXPORTED_TARGETS})
 add_executable(vector_map_loader nodes/vector_map_loader/vector_map_loader.cpp)
 target_link_libraries(vector_map_loader ${catkin_LIBRARIES} get_file ${CURL_LIBRARIES})
 
+add_executable(points_map_filter nodes/points_map_filter/points_map_filter_node.cpp nodes/points_map_filter/points_map_filter.cpp)
+target_link_libraries(points_map_filter ${catkin_LIBRARIES})
+add_dependencies(points_map_filter ${catkin_EXPORTED_TARGETS})
+
 ## Install executables and/or libraries
 install(TARGETS get_file points_map_loader vector_map_loader
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

ros/src/data/packages/map_file/README.md

@@ -0,0 +1,22 @@
+# map_file package
+## points_map_filter
+### feature
+points_map_filter_node subscribe pointcloud maps and current pose, the node extract pointcloud near to the current pose.
+
+#### subscribed topics
+/points_map (sensor_msgs/PointCloud2)  : Raw pointcloud map. This topic usually comes from points_map_loader.  
+/current_pose (geometry_msgs/PoseStamped) : Current pose of the car. This topic usually comes from pose_relay node.  
+
+#### published topics
+/points_map/filtered (sensor_msgs/PointCloud2) : Filtered pointcloud submap.  
+
+#### parameters
+load_grid_size (double) : grid size of submap.  
+load_trigger_distance (double) : if the car moves load_trigger_distance(m), the map filter publish filtered submap. 
+
+### how it works
+map_filter_node relay /points_map topic until it recieves /current_pose topic.  
+Then, the /current_pose topic recieved, the map_filter_node publish submap.
+
+## demonstration
+[![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/LpKIuI5b4DU/0.jpg)](http://www.youtube.com/watch?v=LpKIuI5b4DU)

ros/src/data/packages/map_file/include/map_file/points_map_filter.h

@@ -0,0 +1,82 @@
+/*
+ *  Copyright (c) 2018, TierIV, 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 Autoware 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 HOLDER 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.
+ */
+
+#ifndef POINTS_MAP_FILTER_H_INCLUDED
+#define POINTS_MAP_FILTER_H_INCLUDED
+
+// headers in ROS
+#include <geometry_msgs/PoseStamped.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+
+// headers in PCL
+#include <pcl/filters/passthrough.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+
+// headers in boost
+#include <boost/optional.hpp>
+
+// headers in STL
+#include <mutex>
+#include <regex>
+
+class points_map_filter {
+public:
+  points_map_filter(ros::NodeHandle nh, ros::NodeHandle pnh);
+  ~points_map_filter();
+  void init();
+  void run();
+
+private:
+  pcl::PassThrough<pcl::PointXYZ> pass_;
+  ros::Subscriber map_sub_;
+  ros::Subscriber pose_sub_;
+  ros::Publisher map_pub_;
+  std::mutex mtx_;
+  ros::NodeHandle nh_, pnh_;
+  void map_callback_(const sensor_msgs::PointCloud2::ConstPtr msg);
+  void current_pose_callback_(const geometry_msgs::PoseStamped::ConstPtr msg);
+  double load_grid_size_;
+  double load_trigger_distance_;
+  std::string map_frame_;
+  boost::optional<geometry_msgs::PoseStamped> last_load_pose_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;
+  volatile bool map_recieved_;
+};
+
+#endif // POINTS_MAP_FILTER_H_INCLUDED

ros/src/data/packages/map_file/launch/map_filter.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+    <node pkg="map_file" type="points_map_filter" name="points_map_filter" output="screen" respawn="true" respawn_delay="0">
+        <param name="load_grid_size" value="500.0"/>
+        <param name="load_trigger_distance" value="100.0"/>
+    </node>
+</launch>

ros/src/data/packages/map_file/nodes/points_map_filter/points_map_filter.cpp

@@ -0,0 +1,133 @@
+/*
+ *  Copyright (c) 2018, TierIV, 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 Autoware 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 HOLDER 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.
+ */
+
+#include <map_file/points_map_filter.h>
+
+points_map_filter::points_map_filter(ros::NodeHandle nh, ros::NodeHandle pnh) {
+  map_cloud_ =
+      pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
+  nh_ = nh;
+  pnh_ = pnh;
+}
+
+points_map_filter::~points_map_filter() {}
+
+void points_map_filter::init() {
+  std::lock_guard<std::mutex> lock(mtx_);
+  pnh_.param("load_grid_size", load_grid_size_, 100.0);
+  pnh_.param("load_trigger_distance", load_trigger_distance_, 20.0);
+  pnh_.param("map_frame", map_frame_, std::string("map"));
+  last_load_pose_ = boost::none;
+  map_sub_.shutdown();
+  pose_sub_.shutdown();
+  map_recieved_ = false;
+  return;
+}
+
+void points_map_filter::run() {
+  std::lock_guard<std::mutex> lock(mtx_);
+  map_pub_ =
+      nh_.advertise<sensor_msgs::PointCloud2>("/points_map/filtered", 10);
+  map_sub_ =
+      nh_.subscribe("/points_map", 1, &points_map_filter::map_callback_, this);
+  pose_sub_ = nh_.subscribe("/current_pose", 1,
+                            &points_map_filter::current_pose_callback_, this);
+  return;
+}
+
+void points_map_filter::map_callback_(
+    const sensor_msgs::PointCloud2::ConstPtr msg) {
+  std::lock_guard<std::mutex> lock(mtx_);
+  ROS_INFO_STREAM("loading map started.");
+  pcl::fromROSMsg(*msg, *map_cloud_);
+  map_recieved_ = true;
+  map_pub_.publish(*msg);
+  ROS_INFO_STREAM("loading map finished");
+  return;
+}
+
+void points_map_filter::current_pose_callback_(
+    const geometry_msgs::PoseStamped::ConstPtr msg) {
+  std::lock_guard<std::mutex> lock(mtx_);
+  ROS_INFO_STREAM("pose received");
+  if (!last_load_pose_ && map_recieved_) {
+    last_load_pose_ = *msg;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    pass_.setInputCloud(map_cloud_);
+    pass_.setFilterFieldName("x");
+    pass_.setFilterLimits(
+        last_load_pose_.get().pose.position.x - (load_grid_size_ / 2),
+        last_load_pose_.get().pose.position.x + (load_grid_size_ / 2));
+    pass_.filter(*cloud_filtered);
+    pass_.setInputCloud(cloud_filtered);
+    pass_.setFilterFieldName("y");
+    pass_.setFilterLimits(
+        last_load_pose_.get().pose.position.y - (load_grid_size_ / 2),
+        last_load_pose_.get().pose.position.y + (load_grid_size_ / 2));
+    pass_.filter(*cloud_filtered);
+    sensor_msgs::PointCloud2 msg;
+    pcl::toROSMsg(*cloud_filtered, msg);
+    ROS_INFO_STREAM("update map");
+    map_pub_.publish(msg);
+  } else if (last_load_pose_ && map_recieved_) {
+    double dist = std::sqrt(
+        std::pow(last_load_pose_.get().pose.position.x - msg->pose.position.x,
+                 2) +
+        std::pow(last_load_pose_.get().pose.position.y - msg->pose.position.y,
+                 2));
+    if (dist > load_trigger_distance_) {
+      last_load_pose_ = *msg;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(
+          new pcl::PointCloud<pcl::PointXYZ>);
+      pass_.setInputCloud(map_cloud_);
+      pass_.setFilterFieldName("x");
+      pass_.setFilterLimits(
+          last_load_pose_.get().pose.position.x - (load_grid_size_ / 2),
+          last_load_pose_.get().pose.position.x + (load_grid_size_ / 2));
+      pass_.filter(*cloud_filtered);
+      pass_.setInputCloud(cloud_filtered);
+      pass_.setFilterFieldName("y");
+      pass_.setFilterLimits(
+          last_load_pose_.get().pose.position.y - (load_grid_size_ / 2),
+          last_load_pose_.get().pose.position.y + (load_grid_size_ / 2));
+      pass_.filter(*cloud_filtered);
+      sensor_msgs::PointCloud2 msg;
+      pcl::toROSMsg(*cloud_filtered, msg);
+      map_pub_.publish(msg);
+      ROS_INFO_STREAM("update map");
+    }
+  }
+  return;
+}

ros/src/data/packages/map_file/nodes/points_map_filter/points_map_filter_node.cpp

@@ -0,0 +1,49 @@
+/*
+ *  Copyright (c) 2018, TierIV, 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 Autoware 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 HOLDER 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.
+ */
+
+// headers for ros
+#include <ros/ros.h>
+
+#include <map_file/points_map_filter.h>
+
+int main(int argc, char *argv[]) {
+  ros::init(argc, argv, "points_map_filter");
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+  points_map_filter filter(nh, pnh);
+  filter.init();
+  filter.run();
+  ros::spin();
+  return 0;
+}

ros/src/data/packages/map_file/package.xml

@@ -16,6 +16,9 @@
   <build_depend>autoware_msgs</build_depend>
   <build_depend>curl</build_depend>
   <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
@@ -26,6 +29,9 @@
   <run_depend>autoware_msgs</run_depend>
   <run_depend>curl</run_depend>
   <run_depend>libpcl-all-dev</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
   <export>
   </export>
 </package>