diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 928532f928e38..d01ef157e21b4 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -14,8 +14,8 @@ include_directories( ) ament_auto_add_library(${PROJECT_NAME}_common SHARED - src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/updater/binary_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(${PROJECT_NAME}_common ${PCL_LIBRARIES} @@ -24,9 +24,9 @@ target_link_libraries(${PROJECT_NAME}_common # 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_base.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp + lib/costmap_2d/occupancy_grid_map_base.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/costmap_2d/occupancy_grid_map_projective.cpp ) target_link_libraries(pointcloud_based_occupancy_grid_map @@ -35,14 +35,14 @@ target_link_libraries(pointcloud_based_occupancy_grid_map ) rclcpp_components_register_node(pointcloud_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" + PLUGIN "autoware::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 + lib/costmap_2d/occupancy_grid_map.cpp ) target_link_libraries(laserscan_based_occupancy_grid_map @@ -51,17 +51,17 @@ target_link_libraries(laserscan_based_occupancy_grid_map ) rclcpp_components_register_node(laserscan_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" EXECUTABLE laserscan_based_occupancy_grid_map_node ) # GridMapFusionNode ament_auto_add_library(synchronized_grid_map_fusion SHARED src/fusion/synchronized_grid_map_fusion_node.cpp - src/fusion/single_frame_fusion_policy.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/fusion_policy/fusion_policy.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/updater/log_odds_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(synchronized_grid_map_fusion @@ -69,7 +69,7 @@ target_link_libraries(synchronized_grid_map_fusion ) rclcpp_components_register_node(synchronized_grid_map_fusion - PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + PLUGIN "autoware::occupancy_grid_map::GridMapFusionNode" EXECUTABLE synchronized_grid_map_fusion_node ) @@ -88,13 +88,14 @@ if(BUILD_TESTING) # gtest ament_add_gtest(test_utils - test/test_utils.cpp + test/test_utils.cpp ) ament_add_gtest(costmap_unit_tests - test/cost_value_test.cpp) + test/cost_value_test.cpp + ) ament_add_gtest(fusion_policy_unit_tests - test/fusion_policy_test.cpp - src/fusion/single_frame_fusion_policy.cpp + test/fusion_policy_test.cpp + lib/fusion_policy/fusion_policy.cpp ) target_link_libraries(test_utils ${PCL_LIBRARIES} diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp index 880297a1210ed..d470d206c41c6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp @@ -48,12 +48,14 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ #include -namespace occupancy_cost_value +namespace autoware::occupancy_grid_map +{ +namespace cost_value { static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; @@ -101,6 +103,8 @@ struct InverseCostTranslationTable static const CostTranslationTable cost_translation_table; static const InverseCostTranslationTable inverse_cost_translation_table; -} // namespace occupancy_cost_value -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +} // namespace cost_value +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp index d2210cf9c348a..93e768c0f6b4e 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -90,5 +92,6 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index f01b2d74e160b..2b2057af9cc12 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -91,5 +93,6 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp similarity index 74% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 298f327d632d7..9dfd8b8689123 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -43,5 +45,6 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp similarity index 77% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 67b51d6184c8c..1f0f92d933ff3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -51,5 +53,6 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp index 1f0553878ef5a..a2d7c90a023a5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -25,6 +25,8 @@ /*min and max prob threshold to prevent log-odds to diverge*/ #define EPSILON_PROB 0.03 +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; @@ -61,6 +63,8 @@ unsigned char singleFrameOccupancyFusion( unsigned char singleFrameOccupancyFusion( const std::vector & occupancy, FusionMethod method, const std::vector & reliability); + } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp similarity index 72% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp index 3a921035ef219..af28b7962b3bc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface @@ -38,5 +40,6 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp similarity index 69% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp index 245484e442609..d7bb1184c06d6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -25,6 +25,8 @@ // LOBF means: Log Odds Bayes Filter // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface @@ -45,5 +47,6 @@ class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp similarity index 68% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp index e85edf2245ef3..75231089ac33c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D @@ -27,8 +29,7 @@ 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) + : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } virtual ~OccupancyGridMapUpdaterInterface() = default; @@ -37,5 +38,6 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 0950272dac470..50b470d904ef6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -45,6 +45,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -117,6 +119,8 @@ bool extractCommonPointCloud( sensor_msgs::msg::PointCloud2 & output_obstacle_pc); unsigned char getApproximateOccupancyState(const unsigned char & value); + } // namespace utils +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ 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 index 62cfa4bcb5228..1edc538b3de3f 100644 --- 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 @@ -85,7 +85,7 @@ def launch_setup(context, *args, **kwargs): ), ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index b112dd0a84b83..ef40839d2a5aa 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -196,7 +196,7 @@ def launch_setup(context, *args, **kwargs): # generate composable node node = ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", namespace=frame_name, remappings=[ 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 index 0e84e4860fdf3..bb5ef025a7e47 100644 --- 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 @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): composable_nodes.append( ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ( diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp index 3d02be9ca7156..25eafcd564e2d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp @@ -49,21 +49,23 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ 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) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -140,7 +142,7 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r raytraceFreespace(pointcloud, robot_pose); // occupied - MarkCell marker(costmap_, occupancy_cost_value::LETHAL_OBSTACLE); + MarkCell marker(costmap_, 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; @@ -155,12 +157,12 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r void OccupancyGridMap::updateFreespaceCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE); + updateCellsByPointCloud(pointcloud, cost_value::FREE_SPACE); } void OccupancyGridMap::updateOccupiedCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE); + updateCellsByPointCloud(pointcloud, cost_value::LETHAL_OBSTACLE); } void OccupancyGridMap::updateCellsByPointCloud( @@ -244,9 +246,10 @@ void OccupancyGridMap::raytraceFreespace(const PointCloud2 & pointcloud, const P } constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold - MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE); + MarkCell marker(costmap_, cost_value::FREE_SPACE); raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); } } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 3d176e41583a0..5e9b5598c8f63 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -69,13 +69,16 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapInterface::OccupancyGridMapInterface( 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) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -230,3 +233,4 @@ void OccupancyGridMapInterface::raytrace( } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index d4e177209f99d..f25009d47bdcc 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -33,6 +33,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -136,7 +139,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } // Second step: Add unknown cell @@ -162,9 +165,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); } continue; } @@ -177,7 +179,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } @@ -186,13 +188,13 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -203,7 +205,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -215,7 +217,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -229,3 +231,4 @@ void OccupancyGridMapFixedBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 03d7a42ae7043..a2400fb3c81b2 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -34,6 +34,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -192,7 +195,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, ray_end.wx, ray_end.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } if (pub_debug_grid_) @@ -219,7 +222,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); break; } @@ -227,7 +230,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); continue; } @@ -243,13 +246,13 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -262,7 +265,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( 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); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -274,7 +277,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= obstacle_separation_threshold_) { 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); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -299,3 +302,4 @@ void OccupancyGridMapProjectiveBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp similarity index 97% rename from perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp rename to perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp index 7e2c3f7ba304a..1e88b7a39fd14 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { @@ -61,9 +63,9 @@ namespace overwrite_fusion */ State getApproximateState(const unsigned char & occupancy) { - if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + if (occupancy >= cost_value::OCCUPIED_THRESHOLD) { return State::OCCUPIED; - } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + } else if (occupancy <= cost_value::FREE_THRESHOLD) { return State::FREE; } else { return State::UNKNOWN; @@ -320,3 +322,4 @@ unsigned char singleFrameOccupancyFusion( } } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp index 40f538a64f6e9..74506d095b36d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -48,15 +50,15 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( float pz{}; float not_pz{}; float po_hat{}; - if (z == occupancy_cost_value::LETHAL_OBSTACLE) { + if (z == 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) { + } else if (z == 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) { + } else if (z == cost_value::NO_INFORMATION) { const float inv_v_ratio = 1.f / v_ratio_; po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); } @@ -79,3 +81,4 @@ bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp index 607694d6da571..9f3c3c7e40eaf 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -35,7 +37,7 @@ inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( using fusion_policy::convertProbabilityToChar; using fusion_policy::log_odds_fusion::logOddsFusion; - constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown = cost_value::NO_INFORMATION; constexpr unsigned char unknown_margin = 1; /* Tau and ST decides how fast the observation decay to the unknown status*/ constexpr double tau = 0.75; @@ -68,4 +70,6 @@ bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupanc } return true; } + } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp rename to perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 209207fe10f34..e8c2cdb2617df 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -189,13 +191,14 @@ bool extractCommonPointCloud( */ unsigned char getApproximateOccupancyState(const unsigned char & value) { - if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { - return occupancy_cost_value::LETHAL_OBSTACLE; - } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { - return occupancy_cost_value::FREE_SPACE; + if (value >= cost_value::OCCUPIED_THRESHOLD) { + return cost_value::LETHAL_OBSTACLE; + } else if (value <= cost_value::FREE_THRESHOLD) { + return cost_value::FREE_SPACE; } else { - return occupancy_cost_value::NO_INFORMATION; + return cost_value::NO_INFORMATION; } } } // namespace utils +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index d8ae702ef60ca..d8647ae2ba744 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -13,9 +13,9 @@ ament_cmake_auto autoware_cmake + eigen3_cmake_module autoware_universe_utils - eigen3_cmake_module grid_map_costmap_2d grid_map_msgs grid_map_ros @@ -27,12 +27,9 @@ rclcpp rclcpp_components sensor_msgs - tf2 tf2_eigen - tf2_geometry_msgs tf2_ros tf2_sensor_msgs - visualization_msgs pointcloud_preprocessor pointcloud_to_laserscan diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index fa93db802b0ce..a271e17a1e566 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" +#include "synchronized_grid_map_fusion_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; using costmap_2d::OccupancyGridMapLOBFUpdater; @@ -404,7 +404,7 @@ nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; costmap2d.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } @@ -424,7 +424,7 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; gridmap.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } return gridmap; @@ -453,14 +453,14 @@ nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsg msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); - unsigned char * data = occupancy_grid_map.getCharMap(); + const 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]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp similarity index 85% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 84ca13c8b1881..c839f160aab9b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -45,7 +45,7 @@ // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -124,6 +124,6 @@ class GridMapFusionNode : public rclcpp::Node fusion_policy::FusionMethod fusion_method_; }; -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ 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 index 777c180fe1a3a..6c4ad8135b37d 100644 --- 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 @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "laserscan_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -35,7 +35,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMap; using costmap_2d::OccupancyGridMapBBFUpdater; @@ -229,12 +229,12 @@ OccupancyGrid::UniquePtr LaserscanBasedOccupancyGridMapNode::OccupancyGridMapToM 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]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 82% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index a8adea6e700e5..a599f7b564c8b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -36,7 +36,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapUpdaterInterface; @@ -100,6 +100,6 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node bool enable_single_frame_mode_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ 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 index d019280aefda0..3fe75263b3f28 100644 --- 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 @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "pointcloud_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -40,7 +40,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapBBFUpdater; using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -250,12 +250,12 @@ OccupancyGrid::UniquePtr PointcloudBasedOccupancyGridMapNode::OccupancyGridMapTo 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]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 81% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 1d119102dc28d..e0c7ef74988f4 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#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 "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -38,7 +38,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapInterface; @@ -93,6 +93,6 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node bool filter_obstacle_pointcloud_by_raw_pointcloud_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp index c41c809a92b0c..f03852562f7ca 100644 --- a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the CostTranslationTable and InverseCostTranslationTable functions -using occupancy_cost_value::cost_translation_table; -using occupancy_cost_value::inverse_cost_translation_table; +using autoware::occupancy_grid_map::cost_value::cost_translation_table; +using autoware::occupancy_grid_map::cost_value::inverse_cost_translation_table; TEST(CostTranslationTableTest, TestRange) { diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp index 6b3dc8a2ebcef..78137371c720e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" + +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the log-odds update rule TEST(FusionPolicyTest, TestLogOddsUpdateRule) { - using fusion_policy::log_odds_fusion::logOddsFusion; + using autoware::occupancy_grid_map::fusion_policy::log_odds_fusion::logOddsFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; @@ -49,7 +50,7 @@ TEST(FusionPolicyTest, TestLogOddsUpdateRule) // Test the dempster-shafer update rule TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) { - using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + using autoware::occupancy_grid_map::fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 4bc3dca0a67bd..17df981d849d4 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. // autoware -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -77,14 +77,14 @@ TEST(TestUtils, TestCropPointcloudByHeight) sensor_msgs::msg::PointCloud2 test1_output, test2_output, test3_output; // case1: normal input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); // case2: normal input, empty output - EXPECT_NO_THROW(utils::cropPointcloudByHeight( + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( ros_cloud_10, mock_buffer, "base_link", -2.0, -0.1, test2_output)); // case3: empty input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); }