Initial draft of changes for online map / planner updates #12
Conversation
| grid_map_msg_sub_topic: /se2_grid_map_generator_node/grid_map | ||
| grid_map_msg_pub_topic: state_validator/grid_map | ||
| grid_map_obstacle_layer_name: traversability | ||
| grid_map_state_validity_checking_method: traversability # see GridMapLazyStateValidator.hpp for list of available methods |
There was a problem hiding this comment.
What is this parameter exactly? (grid_map_state_validity_checking_method)
| @@ -15,6 +15,8 @@ | |||
|
|
|||
| namespace se2_planning { | |||
|
|
|||
| enum StateValidityCheckingMethod : int { COLLISION = 0, TRAVERSABILITY = 1, ROBUST_TRAVERSABILITY = 2 }; | |||
There was a problem hiding this comment.
ah okay, I see it now. But we need to explain a bit what is behind these methods.
There was a problem hiding this comment.
Yes, I have just left all methods there that we can discuss/check what is actually useful. Improving the documentation is important for that, sounds good!
| void setStateValidityCheckingMethod(StateValidityCheckingMethod value); | ||
| StateValidityCheckingMethod getStateValidityCheckingMethod() const; | ||
|
|
||
| void setStateValidityThreshold(double value); |
There was a problem hiding this comment.
we need to explain what is that threshold. e.g. cell value < threshold is considered valid.
| const std::string& obstacleLayer); | ||
| std::unique_ptr<GridMapLazyStateValidator> createGridMapLazyStateValidator( | ||
| const grid_map::GridMap& gridMap, const RobotFootprint& footprint, const std::string& obstacleLayer, | ||
| const StateValidityCheckingMethod& stateValidityCheckingMethod, const double stateValidityThreshold, |
There was a problem hiding this comment.
do all methods require all of these parameters? e.g. if I just use the obstacle method?
There was a problem hiding this comment.
No, you are right, this should probably be organized differently. The stateValidityThreshold is used by all three functions, the unsafeTraversabilityThreshold and maxNumberOfUnsafeCells is only used by the robust traversability function.
There was a problem hiding this comment.
yes, I think we should remove the functions that are not used from the function calls.
| initialize(); | ||
| ompl::base::StateSpacePtr space = simpleSetup_->getStateSpace(); | ||
| auto bounds = space->as<ompl::base::SE2StateSpace>()->getBounds(); | ||
| // std::cout << "OmplReedsSheppPlanner: Planner state space bounds: x = [" << bounds.low[0] << ", " << bounds.high[0] << "], y=[" |
There was a problem hiding this comment.
why do we need lines 56-59?
There was a problem hiding this comment.
The initialize() call is necessary to set the new state space boundaries in the planner, lines 57-59 were just for debugging to check that the state space is updated correctly.
There was a problem hiding this comment.
But I think we can just set the new boundaries. There is even a public method for that. We don't need to create the new state space from scratch (which is what these methods will do in the end). Did you ever try that?
There was a problem hiding this comment.
Adapted in db4a618. Is it correct to define the setStateSpaceBoundaries as virtual in the OmplPlanner header file and implement it in the source file? Would you solve it differently?
| double gridMapStateValidityThreshold_ = 0.5; | ||
| double gridMapUnsafeStateValidityThreshold_ = 0.5; | ||
| int gridMapMaxNumberOfUnsafeCells_ = 0; | ||
| double gridMapResolution_ = 0.2; |
There was a problem hiding this comment.
why do we need these parameters (resolution, map position etc.)? GridMapLazyStateValidator will receive a map anyway over ROS if I understand correctly. Should that be enough to initialize them?
There was a problem hiding this comment.
I have to think about it again. The original idea was that it is possible to define the resolution independent of the provided map (downsample it if needed, does not have to be in the node, could happen outside). The rest is probably just there because it was convenient in the beginning.
There was a problem hiding this comment.
Ok, finally tried that out. The issue is that the GridMapLazyStateValidator precomputes footprint points in its initialize function (GridMapLazyStateValidator::initialize()). This requires that a map is set and uses it geometry to calculate the footprint points. I think that was also the reason why I changed the validation function.
Could you explain what the point is behind the computeFootprintPoints function? Maybe we can set it up differently to avoid initializing some random map.
se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp
Outdated
Show resolved
Hide resolved
…r state to publish start and goal state
| @@ -129,6 +168,121 @@ bool isInCollision(const SE2state& state, const std::vector<Vertex>& footprint, | |||
| return false; | |||
| } | |||
|
|
|||
| // TODO Not working with updated state space bounds | |||
There was a problem hiding this comment.
what fails here? This approach that is commented out is significantly faster as opposed to using grid_map iterators. We should try to keep it!
There was a problem hiding this comment.
With the current implementation of isInCollision and isTraversable, I get the strange behavior that along the x-axis the state validator accepts all possible positions, along the y-axis only the ones that lie in the map.
Do not merge!
The original changes I hacked together to get things working, we can use this to start the discussion about how to implement this feature properly.