Adapt se2_grid_map_generator config and add TODOs to fix frames

se2_grid_map_generator/config/default.yaml

@@ -1,5 +1,5 @@
 map:
-  frame_id:                     odom
+  frame_id:                     map # TODO(christoph): planning breaks if start and goal are not set using map as reference frame in rviz
   layers:                       [elevation, traversability]
   default_values:               [0.0, 1.0]
   resolution:                   0.1

se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp

@@ -49,6 +49,7 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl
 
   planTimeStamp_ = req.pathRequest.header.stamp;
 
+  // TODO(christoph): start and goal state are provided in map frame, grid map position is in odom frame?
   const auto start = se2_planning::convert(req.pathRequest.startingPose);
   const auto goal = se2_planning::convert(req.pathRequest.goalPose);
   setStartingState(start);
@@ -184,6 +185,7 @@ void OmplReedsSheppPlannerRos::publishGoalState() const {
 
 void OmplReedsSheppPlannerRos::publishStateSpaceBoundaryMarker() {
   // Set marker info.
+  // TODO(christoph): This should be extracted from the map reference frame!!!
   stateSpaceBoundaryMarker_.header.frame_id = parameters_.pathFrame_;
   stateSpaceBoundaryMarker_.header.stamp = planTimeStamp_;
 

se2_planning_ros/src/se2_planner_node.cpp

@@ -19,6 +19,8 @@ using namespace se2_planning;
 auto planner = std::make_shared<OmplReedsSheppPlanner>();
 
 void gridMapCallback(const grid_map_msgs::GridMap& msg) {
+  // TODO(christoph): Have to transform map position in map frame (or start and goal state in odom frame)?
+  //  Start and goal state are provided in map frame?
   // Update planner bounds when new map is published, placed here due to class structure
   grid_map::GridMap map;
   grid_map::GridMapRosConverter::fromMessage(msg, map);

se2_planning_rviz/src/PlanningPanel.cpp

@@ -399,6 +399,8 @@ std::thread t([this] {
 
 const bool useCurrentStateAsStartingPose = currentStateAsStartCheckBox_->isChecked();
 geometry_msgs::Pose startingPose, goalPose;
+// TODO(christoph): This does not work correctly together (map and getStartPoseFromService) => /prius/get_current_state_service
+//  /prius/base_pose_ground_truth => has to be synced with state input to gridMapStateValidator?
 if (useCurrentStateAsStartingPose) {
   getStartPoseFromService(&startingPose);
   lastPose_ = startingPose;  //update last state