diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt new file mode 100644 index 0000000000000..89906f86c5ce5 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.14) +project(probabilistic_occupancy_grid_map) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +# 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 +) + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md new file mode 100644 index 0000000000000..c0de3fc4b38e1 --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/image/bresenham.svg b/perception/probabilistic_occupancy_grid_map/image/bresenham.svg new file mode 100644 index 0000000000000..61b107efe3f9e --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/bresenham.svg @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/perception/probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png b/perception/probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png new file mode 100644 index 0000000000000..ee9881071fb34 Binary files /dev/null and b/perception/probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png differ diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg b/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg new file mode 100644 index 0000000000000..d1528b1d7378e --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif b/perception/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/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif differ diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg b/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg new file mode 100644 index 0000000000000..95d0f32a35350 --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg b/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg new file mode 100644 index 0000000000000..4ff9323ad9710 --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg b/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg new file mode 100644 index 0000000000000..4c5016fe85e9a --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg b/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg new file mode 100644 index 0000000000000..7104f6ff9282d --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg b/perception/probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg new file mode 100644 index 0000000000000..ebdaf0351b0ca --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp new file mode 100644 index 0000000000000..e9667ccf65577 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp @@ -0,0 +1,77 @@ +// 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 + *********************************************************************/ + +#ifndef COST_VALUE_HPP_ +#define COST_VALUE_HPP_ + +#include + +namespace occupancy_cost_value +{ +static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 +static const unsigned char LETHAL_OBSTACLE = 255; +static const unsigned char FREE_SPACE = 0; + +struct CostTranslationTable +{ + CostTranslationTable() + { + for (int i = 0; i < 256; i++) { + const auto value = static_cast(static_cast(i) * 100.f / 255.f); + data[i] = std::max(std::min(value, static_cast(99)), static_cast(1)); + } + } + char operator[](unsigned char n) const { return data[n]; } + char data[256]; +}; +static const CostTranslationTable cost_translation_table; +} // namespace occupancy_cost_value + +#endif // COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp new file mode 100644 index 0000000000000..b3ac0295d1c8a --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -0,0 +1,101 @@ +// 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 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_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 LaserscanBasedOccupancyGridMapNode : public rclcpp::Node +{ +public: + explicit LaserscanBasedOccupancyGridMapNode(const rclcpp::NodeOptions & node_options); + +private: + PointCloud2::SharedPtr convertLaserscanToPointCLoud2(const LaserScan::ConstSharedPtr & input); + void onLaserscanPointCloud2WithObstacleAndRaw( + const LaserScan::ConstSharedPtr & input_laserscan_msg, + 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); + inline void onDummyPointCloud2(const LaserScan::ConstSharedPtr & input) + { + PointCloud2 dummy; + sensor_msgs::PointCloud2Modifier modifier(dummy); + modifier.setPointCloud2FieldsByString(1, "xyz"); + dummy.header = input->header; + passthrough_.add(std::make_shared(dummy)); + } + +private: + rclcpp::Publisher::SharedPtr occupancy_grid_map_pub_; + message_filters::Subscriber laserscan_sub_; + message_filters::Subscriber obstacle_pointcloud_sub_; + message_filters::Subscriber raw_pointcloud_sub_; + message_filters::PassThrough passthrough_; + + 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_; + + LaserProjection laserscan2pointcloud_converter_; + + 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 // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp new file mode 100644 index 0000000000000..a9bbc3fda6ab4 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/laserscan_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 LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#define LASERSCAN_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 raytrace2D(const PointCloud2 & pointcloud, const Pose & robot_pose); + + void updateFreespaceCells(const PointCloud2 & pointcloud); + + void updateOccupiedCells(const PointCloud2 & pointcloud); + + void updateOrigin(double new_origin_x, double new_origin_y) override; + +private: + void raytraceFreespace(const PointCloud2 & pointcloud, const Pose & robot_pose); + + void updateCellsByPointCloud(const PointCloud2 & pointcloud, const unsigned char cost); + + bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; + + rclcpp::Logger logger_{rclcpp::get_logger("laserscan_based_occupancy_grid_map")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; +}; + +} // namespace costmap_2d + +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp new file mode 100644 index 0000000000000..092ad3891f9b5 --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/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/perception/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/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp new file mode 100644 index 0000000000000..fd12d7529bcc4 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -0,0 +1,48 @@ +// 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 UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#define UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ + +#include "updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include + +namespace costmap_2d +{ +class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface +{ +public: + enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + OccupancyGridMapBBFUpdater( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) + : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + { + probability_matrix_(Index::OCCUPIED, Index::OCCUPIED) = 0.95; + probability_matrix_(Index::FREE, Index::OCCUPIED) = + 1.0 - probability_matrix_(OCCUPIED, OCCUPIED); + probability_matrix_(Index::FREE, Index::FREE) = 0.8; + probability_matrix_(Index::OCCUPIED, Index::FREE) = 1.0 - probability_matrix_(FREE, FREE); + } + bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + +private: + inline unsigned char applyBBF(const unsigned char & z, const unsigned char & o); + Eigen::Matrix2f probability_matrix_; +}; + +} // namespace costmap_2d + +#endif // UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp new file mode 100644 index 0000000000000..f7ae908c4b0d0 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp @@ -0,0 +1,39 @@ +// 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 UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#define UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ + +#include "cost_value.hpp" + +#include + +namespace costmap_2d +{ +class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D +{ +public: + OccupancyGridMapUpdaterInterface( + 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) + { + } + virtual ~OccupancyGridMapUpdaterInterface() = default; + virtual bool update(const Costmap2D & single_frame_occupancy_grid_map) = 0; +}; + +} // namespace costmap_2d + +#endif // UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md b/perception/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md new file mode 100644 index 0000000000000..9babaf2063058 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md @@ -0,0 +1,78 @@ +# laserscan based occupancy grid map + +![laserscan_based_occupancy_grid_map_sample_image](./image/laserscan_based_occupancy_grid_map_sample_image.png) + +## Inner-workings / Algorithms + +The basic idea is to take a 2D laserscan and ray trace it to create a time-series processed occupancy grid map. + +1. the node take a laserscan and make an occupancy grid map with one frame. ray trace is done by Bresenham's line algorithm. + ![Bresenham's line algorithm](./image/bresenham.svg) +2. Optionally, obstacle point clouds and raw point clouds can be received and reflected in the occupancy grid map. The reason is that laserscan only uses the most foreground point in the polar coordinate system, so it throws away a lot of information. As a result, the occupancy grid map is almost an UNKNOWN cell. + Therefore, the obstacle point cloud and the raw point cloud are used to reflect what is judged to be the ground and what is judged to be an obstacle in the occupancy grid map. + ![Bresenham's line algorithm](./image/update_with_pointcloud.svg) + The black and red dots represent raw point clouds, and the red dots represent obstacle point clouds. In other words, the black points are determined as the ground, and the red point cloud is the points determined as obstacles. The gray cells are represented as UNKNOWN cells. + +3. 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/laserscan` | `sensor_msgs::LaserScan` | laserscan | +| `~/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 | +| `input_obstacle_pointcloud` | bool | whether to use the optional obstacle point cloud? If this is true, `~/input/obstacle_pointcloud` topics will be received. | +| `input_obstacle_and_raw_pointcloud` | bool | whether to use the optional obstacle and raw point cloud? If this is true, `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud` topics will be received. | +| `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 + +Bresenham's_line_algorithm + +- +- + +## (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/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..dbbfbb2d876ad --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -0,0 +1,139 @@ +# 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="pointcloud_to_laserscan", + plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode", + name="pointcloud_to_laserscan_node", + remappings=[ + ("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/output/laserscan", LaunchConfiguration("output/laserscan")), + ("~/output/pointcloud", LaunchConfiguration("output/pointcloud")), + ("~/output/ray", LaunchConfiguration("output/ray")), + ("~/output/stixel", LaunchConfiguration("output/stixel")), + ], + parameters=[ + { + "target_frame": "base_link", # Leave disabled to output scan in pointcloud frame + "transform_tolerance": 0.01, + "min_height": 0.0, + "max_height": 2.0, + "angle_min": -3.141592, # -M_PI + "angle_max": 3.141592, # M_PI + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 + "scan_time": 0.0, + "range_min": 1.0, + "range_max": 200.0, + "use_inf": True, + "inf_epsilon": 1.0, + # Concurrency level, affects number of pointclouds queued for processing + # and number of threads used + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + "concurrency_level": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", + name="occupancy_grid_map_node", + remappings=[ + ("~/input/laserscan", LaunchConfiguration("output/laserscan")), + ("~/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, + "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), + "input_obstacle_and_raw_pointcloud": LaunchConfiguration( + "input_obstacle_and_raw_pointcloud" + ), + } + ], + 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"), + add_launch_arg("output/laserscan", "virtual_scan/laserscan"), + add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"), + add_launch_arg("output/ray", "virtual_scan/ray"), + add_launch_arg("output/stixel", "virtual_scan/stixel"), + add_launch_arg("input_obstacle_pointcloud", "false"), + add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), + set_container_executable, + set_container_mt_executable, + occupancy_grid_map_container, + load_composable_nodes, + ] + ) diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..2b7f9f8e83228 --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml new file mode 100644 index 0000000000000..127380427bb2f --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -0,0 +1,41 @@ + + + + probabilistic_occupancy_grid_map + 0.1.0 + generate probabilistic occupancy grid map + Yukihiro Saito + Apache License 2.0 + + Yukihiro Saito + + ament_cmake_auto + + autoware_cmake + + eigen3_cmake_module + laser_geometry + message_filters + nav2_costmap_2d + nav_msgs + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + tier4_autoware_utils + visualization_msgs + + pointcloud_to_laserscan + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md b/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md new file mode 100644 index 0000000000000..44e2306daff50 --- /dev/null +++ b/perception/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/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp new file mode 100644 index 0000000000000..a4ab213fb2374 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -0,0 +1,283 @@ +// 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 "laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" + +#include "cost_value.hpp" + +#include +#include + +#include + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#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; + +LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( + const rclcpp::NodeOptions & node_options) +: Node("laserscan_based_occupancy_grid_map_node", node_options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + + /* 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_width{declare_parameter("map_width", 100.0)}; + const double map_resolution{declare_parameter("map_resolution", 0.5)}; + const bool input_obstacle_pointcloud{declare_parameter("input_obstacle_pointcloud", true)}; + const bool input_obstacle_and_raw_pointcloud{ + declare_parameter("input_obstacle_and_raw_pointcloud", true)}; + + /* Subscriber and publisher */ + laserscan_sub_.subscribe( + this, "~/input/laserscan", rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + 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()); + // add dummy callback to enable passthrough filter + 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_); + } else if (input_obstacle_pointcloud) { + sync_ptr_ = + std::make_shared(SyncPolicy(3), laserscan_sub_, obstacle_pointcloud_sub_, passthrough_); + } else { + sync_ptr_ = std::make_shared(SyncPolicy(3), laserscan_sub_, passthrough_, passthrough_); + } + + sync_ptr_->registerCallback(std::bind( + &LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw, this, _1, _2, + _3)); + 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_width / map_resolution, map_resolution); +} + +PointCloud2::SharedPtr LaserscanBasedOccupancyGridMapNode::convertLaserscanToPointCLoud2( + const LaserScan::ConstSharedPtr & input) +{ + // check over max range point + const float max_range = + static_cast(occupancy_grid_map_updater_ptr_->getSizeInCellsX()) * 0.5f + + occupancy_grid_map_updater_ptr_->getResolution(); + constexpr float epsilon = 0.001; + LaserScan laserscan = *input; + laserscan.range_max = max_range; + for (auto & range : laserscan.ranges) { + if (max_range < range || std::isinf(range)) { + range = max_range - epsilon; + } + } + + // convert to pointcloud + PointCloud2::SharedPtr pointcloud_ptr = std::make_shared(); + pointcloud_ptr->header = laserscan.header; + laserscan2pointcloud_converter_.transformLaserScanToPointCloud( + laserscan.header.frame_id, laserscan, *pointcloud_ptr, *tf2_); + + return pointcloud_ptr; +} + +void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw( + const LaserScan::ConstSharedPtr & input_laserscan_msg, + const PointCloud2::ConstSharedPtr & input_obstacle_msg, + const PointCloud2::ConstSharedPtr & input_raw_msg) +{ + // Laserscan to pointcloud2 + PointCloud2::ConstSharedPtr laserscan_pc_ptr = convertLaserscanToPointCLoud2(input_laserscan_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; + + // Transform pointcloud and get frame pose + PointCloud2 trans_laserscan_pc{}; + PointCloud2 trans_obstacle_pc{}; + PointCloud2 trans_raw_pc{}; + Pose pose{}; + try { + transformPointcloud(*laserscan_pc_ptr, *tf2_, map_frame_, trans_laserscan_pc); + transformPointcloud(filtered_obstacle_pc, *tf2_, map_frame_, trans_obstacle_pc); + transformPointcloud(filtered_raw_pc, *tf2_, map_frame_, trans_raw_pc); + pose = getPose(laserscan_pc_ptr->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.updateFreespaceCells(trans_raw_pc); + single_frame_occupancy_grid_map.raytrace2D(trans_laserscan_pc, pose); + single_frame_occupancy_grid_map.updateOccupiedCells(trans_obstacle_pc); + + if (enable_single_frame_mode_) { + // publish + occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( + map_frame_, laserscan_pc_ptr->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_, laserscan_pc_ptr->header.stamp, pose.position.z, + *occupancy_grid_map_updater_ptr_)); + } +} + +OccupancyGrid::UniquePtr LaserscanBasedOccupancyGridMapNode::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::LaserscanBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp new file mode 100644 index 0000000000000..59f562cb58f35 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -0,0 +1,252 @@ +// 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 "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" + +#include "cost_value.hpp" + +#include + +#include + +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::raytrace2D(const PointCloud2 & pointcloud, const Pose & robot_pose) +{ + // freespace + raytraceFreespace(pointcloud, robot_pose); + + // occupied + MarkCell marker(costmap_, occupancy_cost_value::LETHAL_OBSTACLE); + for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y) { + unsigned int mx, my; + if (!worldToMap(*iter_x, *iter_y, mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + const unsigned int index = getIndex(mx, my); + marker(index); + } +} + +void OccupancyGridMap::updateFreespaceCells(const PointCloud2 & pointcloud) +{ + updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE); +} + +void OccupancyGridMap::updateOccupiedCells(const PointCloud2 & pointcloud) +{ + updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE); +} + +void OccupancyGridMap::updateCellsByPointCloud( + const PointCloud2 & pointcloud, const unsigned char cost) +{ + MarkCell marker(costmap_, cost); + for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y) { + unsigned int mx{}; + unsigned int my{}; + if (!worldToMap(*iter_x, *iter_y, mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + const unsigned int index = getIndex(mx, my); + marker(index); + } +} + +void OccupancyGridMap::raytraceFreespace(const PointCloud2 & pointcloud, const Pose & robot_pose) +{ + unsigned int x0{}; + unsigned int y0{}; + const double ox{robot_pose.position.x}; + const double oy{robot_pose.position.y}; + if (!worldToMap(robot_pose.position.x, robot_pose.position.y, x0, y0)) { + RCLCPP_DEBUG( + logger_, + "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_; + + for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y) { + double wx = *iter_x; + double wy = *iter_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)) { + continue; + } + + constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold + MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE); + raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); + } +} + +} // namespace costmap_2d diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp new file mode 100644 index 0000000000000..ef5e8951fe988 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -0,0 +1,411 @@ +// 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 + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#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 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); + 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_DEBUG( + logger_, + "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/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp new file mode 100644 index 0000000000000..2459e2e0c0439 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -0,0 +1,225 @@ +// 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 +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#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/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp new file mode 100644 index 0000000000000..2438a05bbc5fc --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -0,0 +1,60 @@ +// 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 "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" + +#include "cost_value.hpp" + +#include + +namespace costmap_2d +{ +inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( + const unsigned char & z, const unsigned char & o) +{ + constexpr float cost2p = 1.f / 255.f; + const float po = o * cost2p; + float pz{}; + float not_pz{}; + float po_hat{}; + if (z == occupancy_cost_value::LETHAL_OBSTACLE) { + pz = probability_matrix_(Index::OCCUPIED, Index::OCCUPIED); + not_pz = probability_matrix_(Index::FREE, Index::OCCUPIED); + po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); + } else if (z == occupancy_cost_value::FREE_SPACE) { + pz = 1.f - probability_matrix_(Index::FREE, Index::FREE); + not_pz = 1.f - probability_matrix_(Index::OCCUPIED, Index::FREE); + po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); + } else if (z == occupancy_cost_value::NO_INFORMATION) { + constexpr float inv_v_ratio = 1.f / 10.f; + po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); + } + return std::min( + std::max(static_cast(po_hat * 255.f + 0.5f), static_cast(1)), + static_cast(254)); +} + +bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map) +{ + updateOrigin( + single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY()); + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyBBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + } + } + return true; +} +} // namespace costmap_2d