Add ROS class for occupancy map

car_demo/car_demo/config/reeds_shepp_planner_ros.yaml

@@ -28,14 +28,17 @@ state_validator_ros:
   robot_footprint_length_backward:  0.40
   robot_footprint_width_left:       0.35
   robot_footprint_width_right:      0.35
+map_ros:
+  layer_name: elevation
+  grid_map_sub_topic: /se2_grid_map_generator_node/grid_map
+  grid_map_pub_topic: /occupancy_map_ros/map_out
 
 planner_ros:
   nav_msgs_path_topic: ompl_rs_planner_ros/nav_msgs_path
   planning_service_name: ompl_rs_planner_ros/planning_service
   path_msg_topic: ompl_rs_planner_ros/path_msgs_path # this is the topic the PriusControllerRos.cpp subscribes to (or any other pure_pursuit_controller)
   path_frame: map
   nav_msg_path_spatial_resolution: 1.5
-  state_space_bounds_margin: 0.5
 
 planner:
   path_spatial_resolution: 0.05

se2_planning_ros/CMakeLists.txt

@@ -6,10 +6,11 @@ add_compile_options(-Wall -Wextra -Wpedantic)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 set(SRC_FILES
-  src/OmplReedsSheppPlannerRos.cpp
   src/GridMapLazyStateValidatorRos.cpp
-  src/loaders.cpp
+  src/OmplReedsSheppPlannerRos.cpp
+  src/OccupancyMapRos.cpp
   src/PlannerRos.cpp
+  src/loaders.cpp
   src/common.cpp
 )
 

se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml

@@ -27,14 +27,17 @@ state_validator_ros:
   robot_footprint_length_backward:  0.75
   robot_footprint_width_left:       0.50
   robot_footprint_width_right:      0.50
+map_ros:
+  layer_name: elevation
+  grid_map_sub_topic: /se2_grid_map_generator_node/grid_map
+  grid_map_pub_topic: /occupancy_map_ros/map_out
 
 planner_ros:
   nav_msgs_path_topic: ompl_rs_planner_ros/nav_msgs_path
   planning_service_name: ompl_rs_planner_ros/planning_service
   path_msg_topic: ompl_rs_planner_ros/path_msgs_path
   path_frame: odom
   nav_msg_path_spatial_resolution: 1.5
-  state_space_bounds_margin: 0.5
 
 planner:
   path_spatial_resolution: 0.05

se2_planning_ros/include/se2_planning_ros/OccupancyMapRos.hpp

@@ -0,0 +1,45 @@
+/*
+ * OccupancyMapRos.hpp
+ *
+ *  Created on: May 16, 2021
+ *      Author: meyerc
+ */
+
+#pragma once
+
+#include <ros/ros.h>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include "grid_map_core/GridMap.hpp"
+#include "se2_planning/OccupancyMap.hpp"
+
+struct OccupancyMapRosParameters {
+  std::string gridMapMsgSubTopic_ = "occupancy_map_ros/map_in";
+  std::string layerName_ = "obstacle";
+  std::string gridMapMsgPubTopic_ = "occupancy_map_ros/map_out";
+};
+
+namespace se2_planning {
+
+class OccupancyMapRos : public OccupancyMap {
+  using BASE = OccupancyMap;
+
+ public:
+  explicit OccupancyMapRos(ros::NodeHandlePtr nh);
+  ~OccupancyMapRos() override = default;
+
+  void init();
+  void setParameters(const OccupancyMapRosParameters& parameters);
+
+ private:
+  void initRos();
+  void mapCb(const grid_map_msgs::GridMap& msg);
+
+  ros::NodeHandlePtr nh_;
+  ros::Subscriber gridMapSubscriber_;
+  ros::Publisher gridMapPublisher_;
+  OccupancyMapRosParameters parameters_;
+};
+
+std::unique_ptr<OccupancyMapRos> createOccupancyMapRos(const ros::NodeHandlePtr nh, const OccupancyMapRosParameters& params);
+
+} /* namespace se2_planning*/

se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp

@@ -24,7 +24,6 @@ struct OmplReedsSheppPlannerRosParameters {
   std::string planningServiceName_ = "ompl_rs_planner_ros/planning_service";
   std::string pathMsgTopic_ = "ompl_rs_planner_ros/path";
   double pathNavMsgResolution_ = 1.0;
-  double stateSpaceBoundsMargin_ = 0.5;
 };
 
 class OmplReedsSheppPlannerRos : public PlannerRos {

se2_planning_ros/include/se2_planning_ros/loaders.hpp

@@ -10,12 +10,14 @@
 #include "se2_planning/OmplReedsSheppPlanner.hpp"
 #include "se2_planning/ompl_planner_creators.hpp"
 #include "se2_planning_ros/GridMapLazyStateValidatorRos.hpp"
+#include "se2_planning_ros/OccupancyMapRos.hpp"
 #include "se2_planning_ros/OmplReedsSheppPlannerRos.hpp"
 
 namespace se2_planning {
 
 OmplReedsSheppPlannerParameters loadOmplReedsSheppPlannerParameters(const std::string& filename);
 OmplReedsSheppPlannerRosParameters loadOmplReedsSheppPlannerRosParameters(const std::string& filename);
+OccupancyMapRosParameters loadOccupancyMapRosParameters(const std::string& filename);
 GridMapLazyStateValidatorRosParameters loadGridMapLazyStateValidatorRosParameters(const std::string& filename);
 
 void loadOmplPlannerParameters(const std::string& plannerName, const std::string& filename, OmplPlannerParameters* params);

se2_planning_ros/src/OccupancyMapRos.cpp

@@ -0,0 +1,49 @@
+/*
+ * OccupancyMapRos.cpp
+ *
+ *  Created on: May 16, 2021
+ *      Author: meyerc
+ */
+
+#include "se2_planning_ros/OccupancyMapRos.hpp"
+
+namespace se2_planning {
+
+OccupancyMapRos::OccupancyMapRos(ros::NodeHandlePtr nh) : BASE(), nh_(nh) {}
+
+void OccupancyMapRos::init() {
+  initRos();
+}
+
+void OccupancyMapRos::initRos() {
+  gridMapPublisher_ = nh_->advertise<grid_map_msgs::GridMap>(parameters_.gridMapMsgPubTopic_, 1);
+  gridMapSubscriber_ = nh_->subscribe(parameters_.gridMapMsgSubTopic_, 1, &OccupancyMapRos::mapCb, this);
+}
+
+void OccupancyMapRos::setParameters(const OccupancyMapRosParameters& parameters) {
+  parameters_ = parameters;
+}
+
+void OccupancyMapRos::mapCb(const grid_map_msgs::GridMap& msg) {
+  grid_map::GridMap newMap{};
+  grid_map::GridMapRosConverter::fromMessage(msg, newMap);
+
+  if (newMap.exists(parameters_.layerName_)) {
+    BASE::lock();
+    BASE::setGridMap(newMap, parameters_.layerName_);
+    BASE::unlock();
+  } else {
+    ROS_ERROR("GlobalMap: No %s layer found to load!", parameters_.layerName_.c_str());
+  }
+
+  gridMapPublisher_.publish(msg);
+}
+
+std::unique_ptr<OccupancyMapRos> createOccupancyMapRos(const ros::NodeHandlePtr nh, const OccupancyMapRosParameters& params) {
+  std::unique_ptr<OccupancyMapRos> map = std::make_unique<OccupancyMapRos>(nh);
+  map->setParameters(params);
+  map->init();
+  return map;
+}
+
+}  // namespace se2_planning

se2_planning_ros/src/loaders.cpp

@@ -53,7 +53,22 @@ OmplReedsSheppPlannerRosParameters loadOmplReedsSheppPlannerRosParameters(const
   parameters.planningServiceName_ = node["planning_service_name"].as<std::string>();
   parameters.pathNavMsgResolution_ = node["nav_msg_path_spatial_resolution"].as<double>();
   parameters.pathMsgTopic_ = node["path_msg_topic"].as<std::string>();
-  parameters.stateSpaceBoundsMargin_ = node["state_space_bounds_margin"].as<double>();
+
+  return parameters;
+}
+
+OccupancyMapRosParameters loadOccupancyMapRosParameters(const std::string& filename) {
+  YAML::Node basenode = YAML::LoadFile(filename);
+
+  if (basenode.IsNull()) {
+    throw std::runtime_error("OccupancyMapRosParameters::loadOccupancyMapRosParameters loading failed");
+  }
+
+  OccupancyMapRosParameters parameters;
+  auto node = basenode["map_ros"];
+  parameters.layerName_ = node["layer_name"].as<std::string>();
+  parameters.gridMapMsgSubTopic_ = node["grid_map_sub_topic"].as<std::string>();
+  parameters.gridMapMsgPubTopic_ = node["grid_map_pub_topic"].as<std::string>();
 
   return parameters;
 }