diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py index 7e8dda8749251..d371fa43986ab 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py @@ -76,8 +76,8 @@ def add_launch_arg(name: str, default_value=None): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ComposableNode( - package="laserscan_to_occupancy_grid_map", - plugin="occupancy_grid_map::OccupancyGridMapNode", + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 920d6e8a80cd0..1fd60e4292dee 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -51,11 +51,11 @@ - - - - - + + + + + diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml new file mode 100644 index 0000000000000..7e2e48474548f --- /dev/null +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/laserscan_to_occupancy_grid_map/CMakeLists.txt b/sensing/laserscan_to_occupancy_grid_map/CMakeLists.txt deleted file mode 100644 index 05fb920933356..0000000000000 --- a/sensing/laserscan_to_occupancy_grid_map/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(laserscan_to_occupancy_grid_map) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_library(laserscan_to_occupancy_grid_map SHARED - src/laserscan_to_occupancy_grid_map_node.cpp - src/occupancy_grid_map.cpp - src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp -) - -target_link_libraries(laserscan_to_occupancy_grid_map - ${PCL_LIBRARIES} -) - -rclcpp_components_register_node(laserscan_to_occupancy_grid_map - PLUGIN "occupancy_grid_map::OccupancyGridMapNode" - EXECUTABLE laserscan_to_occupancy_grid_map_node -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - # To avoid conflicts between cpplint and uncrustify w.r.t. inclusion guards - ament_cmake_uncrustify - ) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/sensing/probabilistic_occupancy_grid_map/CMakeLists.txt b/sensing/probabilistic_occupancy_grid_map/CMakeLists.txt new file mode 100644 index 0000000000000..d082019df49c2 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.5) +project(probabilistic_occupancy_grid_map) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +ament_auto_find_build_dependencies() + +# PointcloudBasedOccupancyGridMap +ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED + src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp + src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp + src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +) + +target_link_libraries(pointcloud_based_occupancy_grid_map + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(pointcloud_based_occupancy_grid_map + PLUGIN "occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" + EXECUTABLE pointcloud_based_occupancy_grid_map_node +) + +# LaserscanBasedOccupancyGridMap +ament_auto_add_library(laserscan_based_occupancy_grid_map SHARED + src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp + src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp + src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +) + +target_link_libraries(laserscan_based_occupancy_grid_map + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(laserscan_based_occupancy_grid_map + PLUGIN "occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" + EXECUTABLE laserscan_based_occupancy_grid_map_node +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + # To avoid conflicts between cpplint and uncrustify w.r.t. inclusion guards + ament_cmake_uncrustify + ) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/sensing/probabilistic_occupancy_grid_map/README.md b/sensing/probabilistic_occupancy_grid_map/README.md new file mode 100644 index 0000000000000..c0de3fc4b38e1 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/README.md @@ -0,0 +1,11 @@ +# probabilistic_occupancy_grid_map + +## Purpose + +This package outputs the probability of having an obstacle as occupancy grid map. +![pointcloud_based_occupancy_grid_map_sample_image](./image/pointcloud_based_occupancy_grid_map_sample_image.gif) + +## References/External links + +- [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md) +- [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md) diff --git a/sensing/laserscan_to_occupancy_grid_map/image/bresenham.svg b/sensing/probabilistic_occupancy_grid_map/image/bresenham.svg similarity index 100% rename from sensing/laserscan_to_occupancy_grid_map/image/bresenham.svg rename to sensing/probabilistic_occupancy_grid_map/image/bresenham.svg diff --git a/sensing/laserscan_to_occupancy_grid_map/image/occupancy_grid_map_sample_image.png b/sensing/probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png similarity index 100% rename from sensing/laserscan_to_occupancy_grid_map/image/occupancy_grid_map_sample_image.png rename to sensing/probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png diff --git a/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg new file mode 100644 index 0000000000000..d1528b1d7378e --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg @@ -0,0 +1,4 @@ + + + +
angle_increment
angle_increment
raw pointcloud
raw pointcloud
obstacle pointcloud
obstacle pointcloudText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif new file mode 100644 index 0000000000000..0e2d49037e1b6 Binary files /dev/null and b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif differ diff --git a/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg new file mode 100644 index 0000000000000..95d0f32a35350 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg @@ -0,0 +1,4 @@ + + + +
ground
ground
lidar
lidarText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg new file mode 100644 index 0000000000000..4ff9323ad9710 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg @@ -0,0 +1,4 @@ + + + +
ground
ground
lidar
lidarText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg new file mode 100644 index 0000000000000..4c5016fe85e9a --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg @@ -0,0 +1,4 @@ + + + +
ground
ground
lidar
lidar
distance margin
distance margin
ignored
ignoredText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg new file mode 100644 index 0000000000000..7104f6ff9282d --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg @@ -0,0 +1,4 @@ + + + +
ground
ground
lidar
lidar
distance margin
distance marginText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/laserscan_to_occupancy_grid_map/image/update_with_pointcloud.svg b/sensing/probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg similarity index 100% rename from sensing/laserscan_to_occupancy_grid_map/image/update_with_pointcloud.svg rename to sensing/probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/cost_value.hpp b/sensing/probabilistic_occupancy_grid_map/include/cost_value.hpp similarity index 94% rename from sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/cost_value.hpp rename to sensing/probabilistic_occupancy_grid_map/include/cost_value.hpp index f5edd9a0a6fe7..e9667ccf65577 100644 --- a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/cost_value.hpp +++ b/sensing/probabilistic_occupancy_grid_map/include/cost_value.hpp @@ -48,8 +48,8 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ -#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#ifndef COST_VALUE_HPP_ +#define COST_VALUE_HPP_ #include @@ -74,4 +74,4 @@ struct CostTranslationTable static const CostTranslationTable cost_translation_table; } // namespace occupancy_cost_value -#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#endif // COST_VALUE_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp b/sensing/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 83% rename from sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp rename to sensing/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index 670b7404b0e6e..b138afbecd2ec 100644 --- a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp +++ b/sensing/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__LASERSCAN_TO_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__LASERSCAN_TO_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp" -#include "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -48,10 +48,10 @@ using sensor_msgs::msg::PointCloud2; using tf2_ros::Buffer; using tf2_ros::TransformListener; -class OccupancyGridMapNode : public rclcpp::Node +class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node { public: - explicit OccupancyGridMapNode(const rclcpp::NodeOptions & node_options); + explicit LaserscanBasedOccupancyGridMapNode(const rclcpp::NodeOptions & node_options); private: PointCloud2::SharedPtr convertLaserscanToPointCLoud2(const LaserScan::ConstSharedPtr & input); @@ -97,4 +97,4 @@ class OccupancyGridMapNode : public rclcpp::Node } // namespace occupancy_grid_map -#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__LASERSCAN_TO_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp b/sensing/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp similarity index 92% rename from sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp rename to sensing/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp index 7ecb2ce8a9faa..a9bbc3fda6ab4 100644 --- a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp +++ b/sensing/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -85,10 +85,10 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; - rclcpp::Logger logger_{rclcpp::get_logger("occupancy_grid_map")}; + rclcpp::Logger logger_{rclcpp::get_logger("laserscan_based_occupancy_grid_map")}; rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace costmap_2d -#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ diff --git a/sensing/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp b/sensing/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp new file mode 100644 index 0000000000000..092ad3891f9b5 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp @@ -0,0 +1,94 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, 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: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ + +#include +#include + +#include +#include +#include + +namespace costmap_2d +{ +using geometry_msgs::msg::Pose; +using sensor_msgs::msg::PointCloud2; + +class OccupancyGridMap : public nav2_costmap_2d::Costmap2D +{ +public: + OccupancyGridMap( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution); + + void updateWithPointCloud( + const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, + const Pose & robot_pose); + + void updateOrigin(double new_origin_x, double new_origin_y) override; + + void setCellValue(const double wx, const double wy, const unsigned char cost); + + void raytrace( + const double source_x, const double source_y, const double target_x, const double target_y, + const unsigned char cost); + +private: + bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; + + rclcpp::Logger logger_{rclcpp::get_logger("pointcloud_based_occupancy_grid_map")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; +}; + +} // namespace costmap_2d + +#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ diff --git a/sensing/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/sensing/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp new file mode 100644 index 0000000000000..8969b67cf2686 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -0,0 +1,87 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ + +#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace occupancy_grid_map +{ +using builtin_interfaces::msg::Time; +using costmap_2d::OccupancyGridMapUpdaterInterface; +using laser_geometry::LaserProjection; +using nav2_costmap_2d::Costmap2D; +using nav_msgs::msg::OccupancyGrid; +using sensor_msgs::msg::LaserScan; +using sensor_msgs::msg::PointCloud2; +using tf2_ros::Buffer; +using tf2_ros::TransformListener; + +class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node +{ +public: + explicit PointcloudBasedOccupancyGridMapNode(const rclcpp::NodeOptions & node_options); + +private: + void onPointcloudWithObstacleAndRaw( + const PointCloud2::ConstSharedPtr & input_obstacle_msg, + const PointCloud2::ConstSharedPtr & input_raw_msg); + OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr( + const std::string & frame_id, const Time & stamp, const float & robot_pose_z, + const Costmap2D & occupancy_grid_map); + +private: + rclcpp::Publisher::SharedPtr occupancy_grid_map_pub_; + message_filters::Subscriber obstacle_pointcloud_sub_; + message_filters::Subscriber raw_pointcloud_sub_; + + std::shared_ptr tf2_{std::make_shared(get_clock())}; + std::shared_ptr tf2_listener_{std::make_shared(*tf2_)}; + + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_; + + std::shared_ptr occupancy_grid_map_updater_ptr_; + + // ROS Parameters + std::string map_frame_; + std::string base_link_frame_; + bool use_height_filter_; + bool enable_single_frame_mode_; +}; + +} // namespace occupancy_grid_map + +#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/sensing/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp similarity index 79% rename from sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to sensing/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp index 91610a70ae869..1edd63fffc2a0 100644 --- a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/sensing/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#define UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -45,4 +45,4 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface } // namespace costmap_2d -#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp b/sensing/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp similarity index 77% rename from sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp rename to sensing/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp index 33615fc9065e5..e098e26a1af1e 100644 --- a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp +++ b/sensing/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#define UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#include "laserscan_to_occupancy_grid_map/cost_value.hpp" +#include "cost_value.hpp" #include @@ -36,4 +36,4 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/README.md b/sensing/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md similarity index 96% rename from sensing/laserscan_to_occupancy_grid_map/README.md rename to sensing/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md index 6d3417409613d..9babaf2063058 100644 --- a/sensing/laserscan_to_occupancy_grid_map/README.md +++ b/sensing/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md @@ -1,10 +1,6 @@ -# laserscan_to_occupancy_grid_map +# laserscan based occupancy grid map -## Purpose - -This package outputs the probability of having an obstacle as occupancy grid map. - -![occupancy_grid_map_sample_image](./image/occupancy_grid_map_sample_image.png) +![laserscan_based_occupancy_grid_map_sample_image](./image/laserscan_based_occupancy_grid_map_sample_image.png) ## Inner-workings / Algorithms diff --git a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py b/sensing/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py similarity index 97% rename from sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py rename to sensing/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 9b550c359a9ab..dbbfbb2d876ad 100644 --- a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py +++ b/sensing/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -78,8 +78,8 @@ def add_launch_arg(name: str, default_value=None): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ComposableNode( - package="laserscan_to_occupancy_grid_map", - plugin="occupancy_grid_map::OccupancyGridMapNode", + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), diff --git a/sensing/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/sensing/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..2b7f9f8e83228 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -0,0 +1,93 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + name="occupancy_grid_map_node", + remappings=[ + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), + ], + parameters=[ + { + "map_resolution": 0.5, + "use_height_filter": True, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ] + + occupancy_grid_map_container = ComposableNodeContainer( + condition=LaunchConfigurationEquals("container", ""), + name="occupancy_grid_map_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals("container", ""), + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container"), + ) + + return LaunchDescription( + [ + add_launch_arg("container", ""), + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "false"), + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), + add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), + add_launch_arg("output", "occupancy_grid"), + set_container_executable, + set_container_mt_executable, + occupancy_grid_map_container, + load_composable_nodes, + ] + ) diff --git a/sensing/laserscan_to_occupancy_grid_map/package.xml b/sensing/probabilistic_occupancy_grid_map/package.xml similarity index 89% rename from sensing/laserscan_to_occupancy_grid_map/package.xml rename to sensing/probabilistic_occupancy_grid_map/package.xml index df2868ba9dfdd..7c4d01e27df6c 100644 --- a/sensing/laserscan_to_occupancy_grid_map/package.xml +++ b/sensing/probabilistic_occupancy_grid_map/package.xml @@ -1,8 +1,8 @@ - laserscan_to_occupancy_grid_map + probabilistic_occupancy_grid_map 0.1.0 - generate occupancy grid map from laser scan + generate probabilistic occupancy grid map Yukihiro Saito Yukihiro Saito diff --git a/sensing/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md b/sensing/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md new file mode 100644 index 0000000000000..44e2306daff50 --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md @@ -0,0 +1,106 @@ +# pointcloud based occupancy grid map + +![pointcloud_based_occupancy_grid_map_sample_image](./image/pointcloud_based_occupancy_grid_map_sample_image.gif) + +## Inner-workings / Algorithms + +### 1st step + +First of all, obstacle and raw pointcloud as input are transformed into a polar coordinate system and divided into bin per angle_increment. +At this time, each point belonging to each bin is stored as range data. In addition, the x,y information in the map coordinate is also stored for ray trace on map coordinate. +The bin contains the following information for each point + +- range data from origin of raytrace +- x on map coordinate +- y on map coordinate + +![pointcloud_based_occupancy_grid_map_bev](./image/pointcloud_based_occupancy_grid_map_bev.svg) + +The following figure shows each of the bins from side view. +![pointcloud_based_occupancy_grid_map_side_view](./image/pointcloud_based_occupancy_grid_map_side_view.svg) + +### 2nd step + +The ray trace is performed in three steps for each cell. +The ray trace is done by Bresenham's line algorithm. +![Bresenham's line algorithm](./image/bresenham.svg) + +1. Initialize freespace to the farthest point of each bin. + + ![pointcloud_based_occupancy_grid_map_side_view_1st](./image/pointcloud_based_occupancy_grid_map_side_view_1st.svg) + +2. Fill in the unknown cells. + Assume that unknown is behind the obstacle, since the back of the obstacle is a blind spot. + Therefore, the unknown are assumed to be the cells that are more than a distance margin from each obstacle point. + + ![pointcloud_based_occupancy_grid_map_side_view_2nd](./image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg) + + There are three reasons for setting a distance margin. + + - It is unlikely that a point on the ground will be immediately behind an obstacle. + - The obstacle point cloud is processed and may not match the raw pointcloud. + - The input may be inaccurate and obstacle points may not be determined as obstacles. + +3. Fill in the occupied cells. + Fill in the point where the obstacle point is located with occupied. + In addition, If the distance between obstacle points is less than or equal to the distance margin, it is filled with occupied because the input may be inaccurate and obstacle points may not be determined as obstacles. + + ![pointcloud_based_occupancy_grid_map_side_view_3rd](./image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg) + +### 3rd step + +Using the previous occupancy grid map, update the existence probability using a binary Bayesian filter (1). Also, the unobserved cells are time-decayed like the system noise of the Kalman filter (2). + +```math + \hat{P_{o}} = \frac{(P_{o} * P_{z})}{(P_{o} * P_{z} + (1 - P_{o}) * \bar{P_{z}})} \tag{1} +``` + +```math + \hat{P_{o}} = \frac{(P_{o} + 0.5 * \frac{1}{ratio})}{(\frac{1}{ratio} + 1)} \tag{2} +``` + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ----------------------------- | -------------------------- | -------------------------------------------------------------- | +| `~/input/obstacle_pointcloud` | `sensor_msgs::PointCloud2` | obstacle pointcloud | +| `~/input/raw_pointcloud` | `sensor_msgs::PointCloud2` | The overall point cloud used to input the obstacle point cloud | + +### Output + +| Name | Type | Description | +| ----------------------------- | ------------------------- | ------------------ | +| `~/output/occupancy_grid_map` | `nav_msgs::OccupancyGrid` | occupancy grid map | + +## Parameters + +### Node Parameters + +| Name | Type | Description | +| ------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------- | +| `map_frame` | string | map frame | +| `base_link_frame` | string | base_link frame | +| `use_height_filter` | bool | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. | +| `map_length` | double | The length of the map. -100 if it is 50~50[m] | +| `map_resolution` | double | The map cell resolution [m] | + +## Assumptions / Known limits + +In several places we have modified the external code written in BSD3 license. + +- occupancy_grid_map.hpp +- cost_value.hpp +- occupancy_grid_map.cpp + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts + +- The update probability of the binary Bayesian filter is currently hard-coded and requires a code change to be modified. +- Since there is no special support for moving objects, the probability of existence is not increased for fast objects. diff --git a/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp similarity index 86% rename from sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp rename to sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 971303b5ce72c..67f6b52c6fb0f 100644 --- a/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp" +#include "laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" -#include "laserscan_to_occupancy_grid_map/cost_value.hpp" +#include "cost_value.hpp" #include #include @@ -56,20 +56,7 @@ bool cropPointcloudByHeight( sensor_msgs::msg::PointCloud2 trans_input_tmp; const bool is_target_frame = (input.header.frame_id == target_frame); if (!is_target_frame) { - try { - geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2.lookupTransform( - target_frame, input.header.frame_id, input.header.stamp, - rclcpp::Duration::from_seconds(0.5)); - // transform pointcloud - Eigen::Matrix4f tf_matrix = - tf2::transformToEigen(tf_stamped.transform).matrix().cast(); - pcl_ros::transformPointCloud(tf_matrix, input, trans_input_tmp); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("laserscan_to_occupancy_grid_map"), clock, 5000, "%s", ex.what()); - return false; - } + if (!transformPointcloud(input, tf2, target_frame, trans_input_tmp)) return false; } const sensor_msgs::msg::PointCloud2 & trans_input = is_target_frame ? input : trans_input_tmp; @@ -85,7 +72,7 @@ bool cropPointcloudByHeight( // Convert to ros msg pcl::toROSMsg(*pcl_output, output); - output.header = input.header; + output.header = trans_input.header; return true; } @@ -108,8 +95,9 @@ using costmap_2d::OccupancyGridMap; using costmap_2d::OccupancyGridMapBBFUpdater; using geometry_msgs::msg::Pose; -OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_options) -: Node("occupancy_grid_map_node", node_options) +LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( + const rclcpp::NodeOptions & node_options) +: Node("laserscan_based_occupancy_grid_map_node", node_options) { using std::placeholders::_1; using std::placeholders::_2; @@ -134,7 +122,8 @@ OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_opti raw_pointcloud_sub_.subscribe( this, "~/input/raw_pointcloud", rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); // add dummy callback to enable passthrough filter - laserscan_sub_.registerCallback(std::bind(&OccupancyGridMapNode::onDummyPointCloud2, this, _1)); + laserscan_sub_.registerCallback( + std::bind(&LaserscanBasedOccupancyGridMapNode::onDummyPointCloud2, this, _1)); if (input_obstacle_and_raw_pointcloud) { sync_ptr_ = std::make_shared( SyncPolicy(5), laserscan_sub_, obstacle_pointcloud_sub_, raw_pointcloud_sub_); @@ -145,8 +134,9 @@ OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_opti sync_ptr_ = std::make_shared(SyncPolicy(3), laserscan_sub_, passthrough_, passthrough_); } - sync_ptr_->registerCallback( - std::bind(&OccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw, this, _1, _2, _3)); + sync_ptr_->registerCallback(std::bind( + &LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw, this, _1, _2, + _3)); occupancy_grid_map_pub_ = create_publisher("~/output/occupancy_grid_map", 1); /* Occupancy grid */ @@ -154,7 +144,7 @@ OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_opti map_length / map_resolution, map_length / map_resolution, map_resolution); } -PointCloud2::SharedPtr OccupancyGridMapNode::convertLaserscanToPointCLoud2( +PointCloud2::SharedPtr LaserscanBasedOccupancyGridMapNode::convertLaserscanToPointCLoud2( const LaserScan::ConstSharedPtr & input) { // check over max range point @@ -179,7 +169,7 @@ PointCloud2::SharedPtr OccupancyGridMapNode::convertLaserscanToPointCLoud2( return pointcloud_ptr; } -void OccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw( +void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw( const LaserScan::ConstSharedPtr & input_laserscan_msg, const PointCloud2::ConstSharedPtr & input_obstacle_msg, const PointCloud2::ConstSharedPtr & input_raw_msg) @@ -241,7 +231,7 @@ void OccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw( map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z, *occupancy_grid_map_updater_ptr_)); } -OccupancyGrid::UniquePtr OccupancyGridMapNode::OccupancyGridMapToMsgPtr( +OccupancyGrid::UniquePtr LaserscanBasedOccupancyGridMapNode::OccupancyGridMapToMsgPtr( const std::string & frame_id, const Time & stamp, const float & robot_pose_z, const Costmap2D & occupancy_grid_map) { @@ -274,4 +264,4 @@ OccupancyGrid::UniquePtr OccupancyGridMapNode::OccupancyGridMapToMsgPtr( } // namespace occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::OccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) diff --git a/sensing/laserscan_to_occupancy_grid_map/src/occupancy_grid_map.cpp b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp similarity index 98% rename from sensing/laserscan_to_occupancy_grid_map/src/occupancy_grid_map.cpp rename to sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp index 24b0c65adbb81..7da817366024f 100644 --- a/sensing/laserscan_to_occupancy_grid_map/src/occupancy_grid_map.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -49,9 +49,9 @@ * David V. Lu!! *********************************************************************/ -#include "laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp" +#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "laserscan_to_occupancy_grid_map/cost_value.hpp" +#include "cost_value.hpp" #include diff --git a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp new file mode 100644 index 0000000000000..b779334d9893d --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -0,0 +1,408 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, 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: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp" + +#include "cost_value.hpp" + +#include +#include + +#include + +#include +#include + +#include +namespace +{ +void transformPointcloud( + const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose, + sensor_msgs::msg::PointCloud2 & output) +{ + const auto transform = tier4_autoware_utils::pose2transform(pose); + Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); + + pcl_ros::transformPointCloud(tf_matrix, input, output); + output.header.stamp = input.header.stamp; + output.header.frame_id = ""; +} +} // namespace + +namespace costmap_2d +{ +using sensor_msgs::PointCloud2ConstIterator; + +OccupancyGridMap::OccupancyGridMap( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +{ +} + +bool OccupancyGridMap::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const +{ + if (wx < origin_x_ || wy < origin_y_) { + return false; + } + + mx = static_cast(std::floor((wx - origin_x_) / resolution_)); + my = static_cast(std::floor((wy - origin_y_) / resolution_)); + + if (mx < size_x_ && my < size_y_) { + return true; + } + + return false; +} + +void OccupancyGridMap::updateOrigin(double new_origin_x, double new_origin_y) +{ + // project the new origin into the grid + int cell_ox{static_cast(std::floor((new_origin_x - origin_x_) / resolution_))}; + int cell_oy{static_cast(std::floor((new_origin_y - origin_y_) / resolution_))}; + + // compute the associated world coordinates for the origin cell + // because we want to keep things grid-aligned + double new_grid_ox{origin_x_ + cell_ox * resolution_}; + double new_grid_oy{origin_y_ + cell_oy * resolution_}; + + // To save casting from unsigned int to int a bunch of times + int size_x{static_cast(size_x_)}; + int size_y{static_cast(size_y_)}; + + // we need to compute the overlap of the new and existing windows + int lower_left_x{std::min(std::max(cell_ox, 0), size_x)}; + int lower_left_y{std::min(std::max(cell_oy, 0), size_y)}; + int upper_right_x{std::min(std::max(cell_ox + size_x, 0), size_x)}; + int upper_right_y{std::min(std::max(cell_oy + size_y, 0), size_y)}; + + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + + // we need a map to store the obstacles in the window temporarily + unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y]; + + // copy the local window in the costmap to the local map + copyMapRegion( + costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, + cell_size_y); + + // now we'll set the costmap to be completely unknown if we track unknown space + resetMaps(); + + // update the origin with the appropriate world coordinates + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; + + // compute the starting cell location for copying data back in + int start_x{lower_left_x - cell_ox}; + int start_y{lower_left_y - cell_oy}; + + // now we want to copy the overlapping information back into the map, but in its new location + copyMapRegion( + local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); + + // make sure to clean up + delete[] local_map; +} + +void OccupancyGridMap::updateWithPointCloud( + const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, + const Pose & robot_pose) +{ + constexpr double ep = 0.001; + constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); + constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); + constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); + const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); + + // Transform to map frame + PointCloud2 trans_raw_pointcloud, trans_obstacle_pointcloud; + transformPointcloud(raw_pointcloud, robot_pose, trans_raw_pointcloud); + transformPointcloud(obstacle_pointcloud, robot_pose, trans_obstacle_pointcloud); + + // Create angle bins + struct BinInfo + { + BinInfo() = default; + BinInfo(const double _range, const double _wx, const double _wy) + : range(_range), wx(_wx), wy(_wy) + { + } + double range; + double wx; + double wy; + }; + std::vector> obstacle_pointcloud_angle_bins; + std::vector> raw_pointcloud_angle_bins; + obstacle_pointcloud_angle_bins.resize(angle_bin_size); + raw_pointcloud_angle_bins.resize(angle_bin_size); + for (PointCloud2ConstIterator iter_x(raw_pointcloud, "x"), iter_y(raw_pointcloud, "y"), + iter_wx(trans_raw_pointcloud, "x"), iter_wy(trans_raw_pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { + const double angle = atan2(*iter_y, *iter_x); + const int angle_bin_index = (angle - min_angle) / angle_increment; + raw_pointcloud_angle_bins.at(angle_bin_index) + .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy)); + } + for (PointCloud2ConstIterator iter_x(obstacle_pointcloud, "x"), + iter_y(obstacle_pointcloud, "y"), iter_wx(trans_obstacle_pointcloud, "x"), + iter_wy(trans_obstacle_pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { + const double angle = atan2(*iter_y, *iter_x); + int angle_bin_index = (angle - min_angle) / angle_increment; + obstacle_pointcloud_angle_bins.at(angle_bin_index) + .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy)); + } + + // Sort by distance + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { + std::sort( + obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(), + [](auto a, auto b) { return a.range < b.range; }); + } + for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { + std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) { + return a.range < b.range; + }); + } + + // First step: Initialize cells to the final point with freespace + constexpr double distance_margin = 1.0; + for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { + auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); + + BinInfo end_distance; + if (raw_pointcloud_angle_bin.empty() && obstacle_pointcloud_angle_bin.empty()) { + continue; + } else if (raw_pointcloud_angle_bin.empty()) { + end_distance = obstacle_pointcloud_angle_bin.back(); + } else if (obstacle_pointcloud_angle_bin.empty()) { + end_distance = raw_pointcloud_angle_bin.back(); + } else { + end_distance = obstacle_pointcloud_angle_bin.back().range + distance_margin < + raw_pointcloud_angle_bin.back().range + ? raw_pointcloud_angle_bin.back() + : obstacle_pointcloud_angle_bin.back(); + } + raytrace( + robot_pose.position.x, robot_pose.position.y, end_distance.wx, end_distance.wy, + occupancy_cost_value::FREE_SPACE); + } + + // Second step: Add uknown cell + for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { + auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); + auto raw_distance_iter = raw_pointcloud_angle_bin.begin(); + for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { + // Calculate next raw point from obstacle point + while (raw_distance_iter != raw_pointcloud_angle_bin.end()) { + if ( + raw_distance_iter->range < + obstacle_pointcloud_angle_bin.at(dist_index).range + distance_margin) + raw_distance_iter++; + else + break; + } + + // There is no point far than the obstacle point. + const bool no_freespace_point = (raw_distance_iter == raw_pointcloud_angle_bin.end()); + + if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { + const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); + if (!no_freespace_point) { + const auto & target = *raw_distance_iter; + raytrace( + source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + } + continue; + } + + auto next_obstacle_point_distance = std::abs( + obstacle_pointcloud_angle_bin.at(dist_index + 1).range - + obstacle_pointcloud_angle_bin.at(dist_index).range); + if (next_obstacle_point_distance <= distance_margin) { + continue; + } else if (no_freespace_point) { + const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); + const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); + raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + continue; + } + + auto next_raw_distance = + std::abs(obstacle_pointcloud_angle_bin.at(dist_index).range - raw_distance_iter->range); + if (next_raw_distance < next_obstacle_point_distance) { + const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); + const auto & target = *raw_distance_iter; + raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + continue; + } else { + const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); + const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); + raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + continue; + } + } + } + + // Third step: Overwrite occupied cell + for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { + auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); + auto raw_distance_iter = raw_pointcloud_angle_bin.begin(); + for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { + const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); + setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + + if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { + continue; + } + + auto next_obstacle_point_distance = std::abs( + obstacle_pointcloud_angle_bin.at(dist_index + 1).range - + obstacle_pointcloud_angle_bin.at(dist_index).range); + if (next_obstacle_point_distance <= distance_margin) { + const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); + const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); + raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + continue; + } + } + } +} + +void OccupancyGridMap::setCellValue(const double wx, const double wy, const unsigned char cost) +{ + MarkCell marker(costmap_, cost); + unsigned int mx{}; + unsigned int my{}; + if (!worldToMap(wx, wy, mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + return; + } + const unsigned int index = getIndex(mx, my); + marker(index); +} + +void OccupancyGridMap::raytrace( + const double source_x, const double source_y, const double target_x, const double target_y, + const unsigned char cost) +{ + unsigned int x0{}; + unsigned int y0{}; + const double ox{source_x}; + const double oy{source_y}; + if (!worldToMap(ox, oy, x0, y0)) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, + "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot " + "raytrace for it.", + ox, oy); + return; + } + + // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later + const double origin_x = origin_x_, origin_y = origin_y_; + const double map_end_x = origin_x + size_x_ * resolution_; + const double map_end_y = origin_y + size_y_ * resolution_; + + double wx = target_x; + double wy = target_y; + + // now we also need to make sure that the endpoint we're ray-tracing + // to isn't off the costmap and scale if necessary + const double a = wx - ox; + const double b = wy - oy; + + // the minimum value to raytrace from is the origin + if (wx < origin_x) { + const double t = (origin_x - ox) / a; + wx = origin_x; + wy = oy + b * t; + } + if (wy < origin_y) { + const double t = (origin_y - oy) / b; + wx = ox + a * t; + wy = origin_y; + } + + // the maximum value to raytrace to is the end of the map + if (wx > map_end_x) { + const double t = (map_end_x - ox) / a; + wx = map_end_x - .001; + wy = oy + b * t; + } + if (wy > map_end_y) { + const double t = (map_end_y - oy) / b; + wx = ox + a * t; + wy = map_end_y - .001; + } + + // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint + unsigned int x1{}; + unsigned int y1{}; + + // check for legality just in case + if (!worldToMap(wx, wy, x1, y1)) { + return; + } + + constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold + MarkCell marker(costmap_, cost); + raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); +} + +} // namespace costmap_2d diff --git a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp new file mode 100644 index 0000000000000..ce92f15d4649d --- /dev/null +++ b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -0,0 +1,219 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" + +#include "cost_value.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +namespace +{ +bool transformPointcloud( + const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, + const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output) +{ + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + // transform pointcloud + Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(tf_matrix, input, output); + output.header.stamp = input.header.stamp; + output.header.frame_id = target_frame; + return true; +} + +bool cropPointcloudByHeight( + const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, + const std::string & target_frame, const float min_height, const float max_height, + sensor_msgs::msg::PointCloud2 & output) +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + // Transformed pointcloud on target frame + sensor_msgs::msg::PointCloud2 trans_input_tmp; + const bool is_target_frame = (input.header.frame_id == target_frame); + if (!is_target_frame) { + if (!transformPointcloud(input, tf2, target_frame, trans_input_tmp)) return false; + } + const sensor_msgs::msg::PointCloud2 & trans_input = is_target_frame ? input : trans_input_tmp; + + // Apply height filter + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + for (sensor_msgs::PointCloud2ConstIterator iter_x(trans_input, "x"), + iter_y(trans_input, "y"), iter_z(trans_input, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + if (min_height < *iter_z && *iter_z < max_height) { + pcl_output->push_back(pcl::PointXYZ(*iter_x, *iter_y, *iter_z)); + } + } + + // Convert to ros msg + pcl::toROSMsg(*pcl_output, output); + output.header = trans_input.header; + return true; +} + +geometry_msgs::msg::Pose getPose( + const std_msgs::msg::Header & source_header, const tf2_ros::Buffer & tf2, + const std::string & target_frame) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = tf2.lookupTransform( + target_frame, source_header.frame_id, source_header.stamp, rclcpp::Duration::from_seconds(0.5)); + pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + return pose; +} +} // namespace + +namespace occupancy_grid_map +{ +using costmap_2d::OccupancyGridMap; +using costmap_2d::OccupancyGridMapBBFUpdater; +using geometry_msgs::msg::Pose; + +PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( + const rclcpp::NodeOptions & node_options) +: Node("pointcloud_based_occupancy_grid_map_node", node_options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + /* params */ + map_frame_ = declare_parameter("map_frame", "map"); + base_link_frame_ = declare_parameter("base_link_frame", "base_link"); + use_height_filter_ = declare_parameter("use_height_filter", true); + enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); + const double map_length{declare_parameter("map_length", 100.0)}; + const double map_resolution{declare_parameter("map_resolution", 0.5)}; + + /* Subscriber and publisher */ + obstacle_pointcloud_sub_.subscribe( + this, "~/input/obstacle_pointcloud", + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + raw_pointcloud_sub_.subscribe( + this, "~/input/raw_pointcloud", rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + sync_ptr_ = std::make_shared(SyncPolicy(5), obstacle_pointcloud_sub_, raw_pointcloud_sub_); + + sync_ptr_->registerCallback( + std::bind(&PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw, this, _1, _2)); + occupancy_grid_map_pub_ = create_publisher("~/output/occupancy_grid_map", 1); + + /* Occupancy grid */ + occupancy_grid_map_updater_ptr_ = std::make_shared( + map_length / map_resolution, map_length / map_resolution, map_resolution); +} + +void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( + const PointCloud2::ConstSharedPtr & input_obstacle_msg, + const PointCloud2::ConstSharedPtr & input_raw_msg) +{ + // Apply height filter + PointCloud2 cropped_obstacle_pc{}; + PointCloud2 cropped_raw_pc{}; + if (use_height_filter_) { + constexpr float min_height = -1.0, max_height = 2.0; + if (!cropPointcloudByHeight( + *input_obstacle_msg, *tf2_, base_link_frame_, min_height, max_height, + cropped_obstacle_pc)) { + return; + } + if (!cropPointcloudByHeight( + *input_raw_msg, *tf2_, base_link_frame_, min_height, max_height, cropped_raw_pc)) { + return; + } + } + const PointCloud2 & filtered_obstacle_pc = + use_height_filter_ ? cropped_obstacle_pc : *input_obstacle_msg; + const PointCloud2 & filtered_raw_pc = use_height_filter_ ? cropped_raw_pc : *input_raw_msg; + + // Get from map to sensor frame pose + Pose pose{}; + try { + pose = getPose(input_raw_msg->header, *tf2_, map_frame_); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(get_logger(), ex.what()); + return; + } + + // Create single frame occupancy grid map + OccupancyGridMap single_frame_occupancy_grid_map( + occupancy_grid_map_updater_ptr_->getSizeInCellsX(), + occupancy_grid_map_updater_ptr_->getSizeInCellsY(), + occupancy_grid_map_updater_ptr_->getResolution()); + single_frame_occupancy_grid_map.updateOrigin( + pose.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2, + pose.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2); + single_frame_occupancy_grid_map.updateWithPointCloud(filtered_raw_pc, filtered_obstacle_pc, pose); + + if (enable_single_frame_mode_) { + // publish + occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( + map_frame_, input_raw_msg->header.stamp, pose.position.z, single_frame_occupancy_grid_map)); + } else { + // Update with bayes filter + occupancy_grid_map_updater_ptr_->update(single_frame_occupancy_grid_map); + + // publish + occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( + map_frame_, input_raw_msg->header.stamp, pose.position.z, *occupancy_grid_map_updater_ptr_)); + } +} + +OccupancyGrid::UniquePtr PointcloudBasedOccupancyGridMapNode::OccupancyGridMapToMsgPtr( + const std::string & frame_id, const Time & stamp, const float & robot_pose_z, + const Costmap2D & occupancy_grid_map) +{ + auto msg_ptr = std::make_unique(); + + msg_ptr->header.frame_id = frame_id; + msg_ptr->header.stamp = stamp; + msg_ptr->info.resolution = occupancy_grid_map.getResolution(); + + msg_ptr->info.width = occupancy_grid_map.getSizeInCellsX(); + msg_ptr->info.height = occupancy_grid_map.getSizeInCellsY(); + + double wx{}; + double wy{}; + occupancy_grid_map.mapToWorld(0, 0, wx, wy); + msg_ptr->info.origin.position.x = occupancy_grid_map.getOriginX(); + msg_ptr->info.origin.position.y = occupancy_grid_map.getOriginY(); + msg_ptr->info.origin.position.z = robot_pose_z; + msg_ptr->info.origin.orientation.w = 1.0; + + msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); + + unsigned char * data = occupancy_grid_map.getCharMap(); + for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { + msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + } + return msg_ptr; +} + +} // namespace occupancy_grid_map + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) diff --git a/sensing/laserscan_to_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/sensing/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp similarity index 93% rename from sensing/laserscan_to_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp rename to sensing/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp index 100fcd517e11e..ed5f860ae5c55 100644 --- a/sensing/laserscan_to_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "laserscan_to_occupancy_grid_map/cost_value.hpp" +#include "cost_value.hpp" #include