Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(probabilistic_occupancy_grid_map)!: fix namespace and directory structure #7872

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 18 additions & 17 deletions perception/probabilistic_occupancy_grid_map/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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}
Expand All @@ -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
Expand All @@ -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
Expand All @@ -51,25 +51,25 @@ 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
${PCL_LIBRARIES}
)

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
)

Expand All @@ -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}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 <algorithm>

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;
Expand Down Expand Up @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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 <nav2_costmap_2d/costmap_2d.hpp>
#include <rclcpp/rclcpp.hpp>
Expand All @@ -59,6 +59,8 @@
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

namespace autoware::occupancy_grid_map
{
namespace costmap_2d
{
using geometry_msgs::msg::Pose;
Expand Down Expand Up @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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 <nav2_costmap_2d/costmap_2d.hpp>
#include <rclcpp/rclcpp.hpp>
Expand All @@ -59,6 +59,8 @@
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

namespace autoware::occupancy_grid_map
{
namespace costmap_2d
{
using geometry_msgs::msg::Pose;
Expand Down Expand Up @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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 <grid_map_core/GridMap.hpp>

#include <grid_map_msgs/msg/grid_map.hpp>

namespace autoware::occupancy_grid_map
{
namespace costmap_2d
{
using geometry_msgs::msg::Pose;
Expand Down Expand Up @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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 <cmath>
#include <iostream>
Expand All @@ -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 };
Expand Down Expand Up @@ -61,6 +63,8 @@ unsigned char singleFrameOccupancyFusion(
unsigned char singleFrameOccupancyFusion(
const std::vector<unsigned char> & occupancy, FusionMethod method,
const std::vector<double> & 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_
Original file line number Diff line number Diff line change
Expand Up @@ -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 <Eigen/Core>
#include <Eigen/Geometry>

namespace autoware::occupancy_grid_map
{
namespace costmap_2d
{
class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface
Expand All @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,21 @@
// 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 <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

// LOBF means: Log Odds Bayes Filter
// cspell: ignore LOBF

namespace autoware::occupancy_grid_map
{
namespace costmap_2d
{
class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface
Expand All @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -12,23 +12,24 @@
// 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 <nav2_costmap_2d/costmap_2d.hpp>
#include <rclcpp/node.hpp>

namespace autoware::occupancy_grid_map
{
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)
: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION)
{
}
virtual ~OccupancyGridMapUpdaterInterface() = default;
Expand All @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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 <builtin_interfaces/msg/time.hpp>
#include <pcl_ros/transforms.hpp>
Expand Down Expand Up @@ -45,6 +45,8 @@
#include <cmath>
#include <vector>

namespace autoware::occupancy_grid_map
{
namespace utils
{

Expand Down Expand Up @@ -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_
Loading
Loading