diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index a8288d2720f48..41c7294ded849 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -120,8 +120,6 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); -unsigned char getApproximateOccupancyState(const unsigned char & value); - } // namespace utils } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 902de2148cffe..9d190664b8858 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -191,21 +191,5 @@ bool extractCommonPointCloud( return true; } -/** - * @brief Convert unsigned char value to closest cost value - * @param cost Cost value - * @return Probability - */ -unsigned char getApproximateOccupancyState(const unsigned char & value) -{ - 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 cost_value::NO_INFORMATION; - } -} - } // namespace utils } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 0a78b10055dd0..28a5759eeb7b0 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -369,42 +369,6 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( return fused_map; } -/** - * @brief Update occupancy grid map with asynchronous input (currently unused) - * - * @param occupancy_grid_msg - */ -void GridMapFusionNode::updateGridMap( - const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg) -{ - // get updater map origin - - // origin is set to current updater map - auto map_for_update = OccupancyGridMsgToCostmap2D(*occupancy_grid_msg); - - // update - occupancy_grid_map_updater_ptr_->update(map_for_update); -} - -nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( - const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) -{ - nav2_costmap_2d::Costmap2D costmap2d( - occupancy_grid_map.info.width, occupancy_grid_map.info.height, - occupancy_grid_map.info.resolution, occupancy_grid_map.info.origin.position.x, - occupancy_grid_map.info.origin.position.y, 0); - - for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { - 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, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); - } - } - - return costmap2d; -} - OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) { diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 691575299a8c7..23aeb76826e5f 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -66,16 +66,12 @@ class GridMapFusionNode : public rclcpp::Node const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map); - nav2_costmap_2d::Costmap2D OccupancyGridMsgToCostmap2D( - const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap( const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion( std::vector & occupancy_grid_maps, const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights); - void updateGridMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg); - void setPeriod(const int64_t new_period); void timer_callback(); void publish();