From 745d2d03243a75005ac29f66d6f57e50c4a9b7a6 Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 5 Apr 2021 17:54:22 +0200 Subject: [PATCH 01/24] Add functionality to update map (and the planner state space boundaries) online --- .../config/reeds_shepp_planner_ros.yaml | 30 ++- .../car_demo/launch/demo_autonomous.launch | 3 +- car_demo/car_demo/rviz/demo_autonomous.rviz | 44 ++++- car_demo/car_demo/src/PriusControllerRos.cpp | 2 +- se2_navigation_msgs/msg/PathRequestMsg.msg | 1 + .../GridMapLazyStateValidator.hpp | 40 +++- .../include/se2_planning/OmplPlanner.hpp | 1 + .../se2_planning/OmplReedsSheppPlanner.hpp | 6 + .../include/se2_planning/StateValidator.hpp | 4 + .../src/GridMapLazyStateValidator.cpp | 174 +++++++++++++++++- se2_planning/src/GridMapStateValidator.cpp | 9 + se2_planning/src/OmplPlanner.cpp | 8 + se2_planning/src/OmplReedsSheppPlanner.cpp | 47 +++++ se2_planning/src/StateValidator.cpp | 11 ++ se2_planning/src/planning_benchmark.cpp | 19 +- se2_planning_ros/CMakeLists.txt | 3 + .../reeds_shepp_planner_ros_example.yaml | 31 +++- .../GridMapLazyStateValidatorRos.hpp | 81 ++++++++ .../OmplReedsSheppPlannerRos.hpp | 11 ++ .../include/se2_planning_ros/loaders.hpp | 3 + se2_planning_ros/launch/se2_planner.launch | 3 + se2_planning_ros/package.xml | 5 +- .../src/GridMapLazyStateValidatorRos.cpp | 92 +++++++++ .../src/OmplReedsSheppPlannerRos.cpp | 131 ++++++++++++- se2_planning_ros/src/loaders.cpp | 43 +++++ se2_planning_ros/src/se2_planner_node.cpp | 65 ++++++- se2_planning_rviz/rviz/default.rviz | 42 ++++- .../visualization_helpers.hpp | 4 + .../src/visualization_helpers.cpp | 21 +++ 29 files changed, 872 insertions(+), 62 deletions(-) create mode 100644 se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp create mode 100644 se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp diff --git a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml index 025bf60..8ba0ad8 100644 --- a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml +++ b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml @@ -5,17 +5,41 @@ state_space: y_lower: -100.0 y_upper: 100.0 turning_radius: 10.0 - + +state_validator_ros: + grid_map_frame: odom + 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 + grid_map_state_validity_threshold: 0.1 + grid_map_unsafe_state_validity_threshold: 0.1 + grid_map_max_number_of_unsafe_cells: 0 + # grid map dimensions/geometry (only for init, can probably be removed) + grid_map_resolution: 0.04 + grid_map_length: 20.0 + grid_map_width: 20.0 + grid_map_position_x: 5.0 + grid_map_position_y: 0.0 + grid_map_default_value: 0.0 + # Robot footprint for collision checking + # TODO adapt footprint + robot_footprint_length_forward: 0.40 + robot_footprint_length_backward: 0.40 + robot_footprint_width_left: 0.35 + robot_footprint_width_right: 0.35 + 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/nav_msgs_path + 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 - ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners + ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners, not implemented yet max_planning_time: 1.0 ompl_planners: diff --git a/car_demo/car_demo/launch/demo_autonomous.launch b/car_demo/car_demo/launch/demo_autonomous.launch index 3e79287..53da405 100644 --- a/car_demo/car_demo/launch/demo_autonomous.launch +++ b/car_demo/car_demo/launch/demo_autonomous.launch @@ -28,7 +28,8 @@ - + + diff --git a/car_demo/car_demo/rviz/demo_autonomous.rviz b/car_demo/car_demo/rviz/demo_autonomous.rviz index 97330bf..50e5cf0 100644 --- a/car_demo/car_demo/rviz/demo_autonomous.rviz +++ b/car_demo/car_demo/rviz/demo_autonomous.rviz @@ -1,12 +1,13 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 85 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /RobotModel1 + - /Marker1 Splitter Ratio: 0.5 Tree Height: 341 - Class: rviz/Selection @@ -567,6 +568,35 @@ Visualization Manager: Topic: /se2_planner_node/ompl_rs_planner_ros/nav_msgs_path Unreliable: false Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /se2_planner_node/state_space + Name: Marker + Namespaces: + state_space: true + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /se2_planner_node/state_validator/grid_map + Unreliable: false + Use Rainbow: true + Value: true Enabled: true Global Options: Background Color: 186; 189; 182 @@ -602,18 +632,18 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: -0.24793481826782227 - Y: 1.0106052160263062 - Z: -0.06408524513244629 + X: 4.376317977905273 + Y: -2.549678325653076 + Z: 5.444847106933594 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.1503983736038208 + Pitch: 0.4153985381126404 Target Frame: Value: Orbit (rviz) - Yaw: 2.2054100036621094 + Yaw: 2.095402956008911 Saved: ~ Window Geometry: Displays: @@ -634,7 +664,7 @@ Window Geometry: collapsed: true Width: 1853 X: 1987 - Y: 27 + Y: 34 back_camera: collapsed: false front_camera: diff --git a/car_demo/car_demo/src/PriusControllerRos.cpp b/car_demo/car_demo/src/PriusControllerRos.cpp index 00340df..a7d4450 100644 --- a/car_demo/car_demo/src/PriusControllerRos.cpp +++ b/car_demo/car_demo/src/PriusControllerRos.cpp @@ -217,7 +217,7 @@ void PriusControllerRos::initRos() controllerCommandService_ = nh_->advertiseService("/prius/controller_command_service", &PriusControllerRos::controllerCommandService, this); - pathSub_ = nh_->subscribe("/se2_planner_node/ompl_rs_planner_ros/path", 1, + pathSub_ = nh_->subscribe("/se2_planner_node/ompl_rs_planner_ros/path_msgs_path", 1, &PriusControllerRos::pathCallback, this); } diff --git a/se2_navigation_msgs/msg/PathRequestMsg.msg b/se2_navigation_msgs/msg/PathRequestMsg.msg index 75aeea0..635345f 100644 --- a/se2_navigation_msgs/msg/PathRequestMsg.msg +++ b/se2_navigation_msgs/msg/PathRequestMsg.msg @@ -1,2 +1,3 @@ +std_msgs/Header header geometry_msgs/Pose startingPose geometry_msgs/Pose goalPose diff --git a/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp b/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp index 74cb82c..938e6c0 100644 --- a/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp +++ b/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp @@ -15,6 +15,8 @@ namespace se2_planning { +enum StateValidityCheckingMethod : int { COLLISION = 0, TRAVERSABILITY = 1, ROBUST_TRAVERSABILITY = 2 }; + class GridMapLazyStateValidator : public GridMapStateValidator { using BASE = GridMapStateValidator; @@ -23,32 +25,56 @@ class GridMapLazyStateValidator : public GridMapStateValidator { ~GridMapLazyStateValidator() override = default; bool isStateValid(const State& state) const final; - void initialize() final; + void initialize() override; bool isInitialized() const final; void setIsUseRandomizedStrategy(bool value); bool getIsUseRandomizedStrategy() const; void setIsUseEarlyStoppingHeuristic(bool value); - bool setIsUseEarlyStoppingHeuristic() const; + bool getIsUseEarlyStoppingHeuristic() const; void setSeed(int value); int getSeed() const; + void setStateValidityCheckingMethod(StateValidityCheckingMethod value); + StateValidityCheckingMethod getStateValidityCheckingMethod() const; + + void setStateValidityThreshold(double value); + double getStateValidityThreshold() const; + + void setUnsafeStateValidityThreshold(double value); + double getUnsafeStateValidityThreshold() const; + + void setMaxNumberOfUnsafeCells(int value); + int getMaxNumberOfUnsafeCells() const; + private: std::vector nominalFootprintPoints_; bool isInitializeCalled_ = false; bool isUseRandomizedStrategy_ = false; bool isUseEarlyStoppingHeuristic_ = false; + StateValidityCheckingMethod stateValidityCheckingMethod_ = StateValidityCheckingMethod::COLLISION; + double minStateValidityThreshold_ = 0.5; + double unsafeStateValidityThreshold_ = 0.5; + int maxNumberOfUnsafeCells_ = 0; int seed_ = 0; }; void computeFootprintPoints(const grid_map::GridMap& gridMap, const RobotFootprint& footprint, std::vector* footprintPoints); -bool isInCollision(const SE2state& state, const std::vector& nominalFootprintPoints, const grid_map::GridMap& gridMap, - const std::string& obstacleLayer); +bool isInCollision(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, + const std::string& obstacleLayer, const double collisionThreshold); +// bool isTraversable(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, +// const std::string& traversabilityLayer, const double traversabilityThreshold); +bool isTraversableIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap, + const std::string& traversabilityLayer, const double traversabilityThreshold); +bool isTraversableRobustIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap, + const std::string& traversabilityLayer, const double minTraversabilityThreshold, + const double unsafeTraversabilityThreshold, const int maxNumberOfUnsafeCells); void addExtraPointsForEarlyStopping(const RobotFootprint& footprint, std::vector* points, int seed); -std::unique_ptr createGridMapLazyStateValidator(const grid_map::GridMap& gridMap, - const RobotFootprint& footprint, - const std::string& obstacleLayer); +std::unique_ptr createGridMapLazyStateValidator( + const grid_map::GridMap& gridMap, const RobotFootprint& footprint, const std::string& obstacleLayer, + const StateValidityCheckingMethod& stateValidityCheckingMethod, const double stateValidityThreshold, + const double unsafeStateValidityThreshold, const int maxNumberOfUnsafeCells); } /* namespace se2_planning */ diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index 5a60cbe..9b7f211 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -33,6 +33,7 @@ class OmplPlanner : public Planner { void getInterpolatedPath(Path* interpolatedPath, unsigned int numPoints) const; ompl::geometric::SimpleSetupPtr getSimpleSetup() const; void setOmplPlanner(ompl::base::PlannerPtr planner); + virtual void updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds); protected: virtual void initializeStateSpace() = 0; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index ad5485a..adf8623 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -56,6 +56,12 @@ class OmplReedsSheppPlanner final : public OmplPlanner { void setParameters(const OmplReedsSheppPlannerParameters& parameters); void setStateValidator(std::unique_ptr stateValidator); const StateValidator& getStateValidator() const; + void lockStateValidator(); + void unlockStateValidator(); + bool isLocked() const; + void updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) override; + bool satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state); + const ompl::base::RealVectorBounds& getStateSpaceBoundaries(); private: void createDefaultStateSpace(); diff --git a/se2_planning/include/se2_planning/StateValidator.hpp b/se2_planning/include/se2_planning/StateValidator.hpp index 5e69e0b..9191115 100644 --- a/se2_planning/include/se2_planning/StateValidator.hpp +++ b/se2_planning/include/se2_planning/StateValidator.hpp @@ -17,6 +17,10 @@ class StateValidator { virtual bool isStateValid(const State& state) const = 0; virtual void initialize(); virtual bool isInitialized() const; + virtual bool isLocked() const; + virtual void lock(); + virtual void unlock(); + bool isLocked_ = false; }; class SE2stateValidator : public StateValidator { diff --git a/se2_planning/src/GridMapLazyStateValidator.cpp b/se2_planning/src/GridMapLazyStateValidator.cpp index 04b21fd..5f1fb80 100644 --- a/se2_planning/src/GridMapLazyStateValidator.cpp +++ b/se2_planning/src/GridMapLazyStateValidator.cpp @@ -26,7 +26,7 @@ bool GridMapLazyStateValidator::getIsUseRandomizedStrategy() const { void GridMapLazyStateValidator::setIsUseEarlyStoppingHeuristic(bool value) { isUseEarlyStoppingHeuristic_ = value; } -bool GridMapLazyStateValidator::setIsUseEarlyStoppingHeuristic() const { +bool GridMapLazyStateValidator::getIsUseEarlyStoppingHeuristic() const { return isUseEarlyStoppingHeuristic_; } @@ -37,6 +37,34 @@ int GridMapLazyStateValidator::getSeed() const { return seed_; } +void GridMapLazyStateValidator::setStateValidityCheckingMethod(StateValidityCheckingMethod value) { + stateValidityCheckingMethod_ = value; +} +StateValidityCheckingMethod GridMapLazyStateValidator::getStateValidityCheckingMethod() const { + return stateValidityCheckingMethod_; +} + +void GridMapLazyStateValidator::setStateValidityThreshold(double value) { + minStateValidityThreshold_ = value; +} +double GridMapLazyStateValidator::getStateValidityThreshold() const { + return minStateValidityThreshold_; +} + +void GridMapLazyStateValidator::setUnsafeStateValidityThreshold(double value) { + unsafeStateValidityThreshold_ = value; +} +double GridMapLazyStateValidator::getUnsafeStateValidityThreshold() const { + return unsafeStateValidityThreshold_; +} + +void GridMapLazyStateValidator::setMaxNumberOfUnsafeCells(int value) { + maxNumberOfUnsafeCells_ = value; +} +int GridMapLazyStateValidator::getMaxNumberOfUnsafeCells() const { + return maxNumberOfUnsafeCells_; +} + void GridMapLazyStateValidator::initialize() { if (!isGridMapInitialized_) { throw std::runtime_error("Initialize the grid map first"); @@ -64,7 +92,17 @@ bool GridMapLazyStateValidator::isStateValid(const State& state) const { } /* Optimistic and assumes no obstacles */ const auto se2state = *(state.as()); - return !isInCollision(se2state, nominalFootprintPoints_, gridMap_, obstacleLayerName_); + switch (stateValidityCheckingMethod_) { + case StateValidityCheckingMethod::COLLISION: + return !isInCollision(se2state, nominalFootprintPoints_, gridMap_, obstacleLayerName_, minStateValidityThreshold_); + case StateValidityCheckingMethod::TRAVERSABILITY: + return isTraversableIterator(se2state, nominalFootprint_, gridMap_, obstacleLayerName_, minStateValidityThreshold_); + case StateValidityCheckingMethod::ROBUST_TRAVERSABILITY: + return isTraversableRobustIterator(se2state, nominalFootprint_, gridMap_, obstacleLayerName_, minStateValidityThreshold_, + unsafeStateValidityThreshold_, maxNumberOfUnsafeCells_); + default: + throw std::runtime_error("Invalid StateValidityCheckingMethod set."); + } } void computeFootprintPoints(const grid_map::GridMap& gridMap, const RobotFootprint& footprint, std::vector* footprintPoints) { @@ -86,7 +124,7 @@ void computeFootprintPoints(const grid_map::GridMap& gridMap, const RobotFootpri } bool isInCollision(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, - const std::string& obstacleLayer) { + const std::string& obstacleLayer, const double collisionThreshold) { const double Cos = std::cos(state.yaw_); const double Sin = std::sin(state.yaw_); const double dx = state.x_; @@ -111,7 +149,8 @@ bool isInCollision(const SE2state& state, const std::vector& footprint, gridMap.getIndex(position, id); occupancy = data(bindToRange(id.x(), 0, nRows), bindToRange(id.y(), 0, nCols)); } catch (const std::out_of_range& e) { - return true; + continue; // TODO at the moment return traversable for out of bounds case + // return true; // would assume all points out of bounds to be intraversable } // ignore nans since they might come from faulty @@ -120,7 +159,6 @@ bool isInCollision(const SE2state& state, const std::vector& footprint, continue; } - const double collisionThreshold = 0.1; if (occupancy > collisionThreshold) { return true; } @@ -129,6 +167,121 @@ bool isInCollision(const SE2state& state, const std::vector& footprint, return false; } +// TODO Not working with updated state space bounds +// bool isTraversable(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, +// const std::string& traversabilityLayer, const double traversabilityThreshold) { +// const double Cos = std::cos(state.yaw_); +// const double Sin = std::sin(state.yaw_); +// const double dx = state.x_; +// const double dy = state.y_; +// auto transformOperator = [Cos, Sin, dx, dy](const Vertex& v) -> Vertex { +// return Vertex{Cos * v.x_ - Sin * v.y_ + dx, Sin * v.x_ + Cos * v.y_ + dy}; +// }; +// const auto& data = gridMap.get(traversabilityLayer); +// for (const auto& vertex : footprint) { +// double traversability = 1.0; +// try { +// // Translate and rotate to current state +// const auto v = transformOperator(vertex); +// grid_map::Index id; +// gridMap.getIndex(grid_map::Position(v.x_, v.y_), id); +// traversability = data(id.x(), id.y()); +// } catch (const std::out_of_range& e) { +// continue; // TODO at the moment return traversable for out of bounds case +// } +// +// // ignore nans since they might come from faulty +// // perception pipeline +// if (std::isnan(traversability)) { +// continue; +// } +// +// if (traversability < traversabilityThreshold) { +// return false; +// } +// } +// +// return true; +//} + +bool isTraversableIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap, + const std::string& traversabilityLayer, const double traversabilityThreshold) { + RobotFootprint currentFootprint; + currentFootprint = footprint; // assign memory/size, otherwise segfault + // Translate and rotate to current state + footprintAtPose(footprint, state, ¤tFootprint); + grid_map::Polygon footprintPolygon; + footprintPolygon = toPolygon(currentFootprint); + footprintPolygon.setFrameId(gridMap.getFrameId()); + footprintPolygon.setTimestamp(gridMap.getTimestamp()); + + const auto& data = gridMap.get(traversabilityLayer); + for (grid_map::PolygonIterator iterator(gridMap, footprintPolygon); !iterator.isPastEnd(); ++iterator) { + double traversability = 1.0; + try { + const grid_map::Index idx(*iterator); + traversability = data(idx.x(), idx.y()); + } catch (const std::out_of_range& e) { + continue; // TODO at the moment return traversable for out of bounds case + } + + // ignore nans since they might come from faulty + // perception pipeline + if (std::isnan(traversability)) { + continue; + } + + if (traversability < traversabilityThreshold) { + return false; + } + } + + return true; +} + +bool isTraversableRobustIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap, + const std::string& traversabilityLayer, const double minTraversabilityThreshold, + const double unsafeTraversabilityThreshold, const int maxNumberOfUnsafeCells) { + RobotFootprint currentFootprint; + currentFootprint = footprint; // assign memory/size, otherwise segfault + // Translate and rotate to current state + footprintAtPose(footprint, state, ¤tFootprint); + grid_map::Polygon footprintPolygon; + footprintPolygon = toPolygon(currentFootprint); + footprintPolygon.setFrameId(gridMap.getFrameId()); + footprintPolygon.setTimestamp(gridMap.getTimestamp()); + + int numberOfUnsafeCells = 0; + + const auto& data = gridMap.get(traversabilityLayer); + for (grid_map::PolygonIterator iterator(gridMap, footprintPolygon); !iterator.isPastEnd(); ++iterator) { + double traversability = 1.0; + try { + const grid_map::Index idx(*iterator); + traversability = data(idx.x(), idx.y()); + } catch (const std::out_of_range& e) { + continue; // TODO at the moment return traversable for out of bounds case + } + + // ignore nans since they might come from faulty + // perception pipeline + if (std::isnan(traversability)) { + continue; + } + + if (traversability < minTraversabilityThreshold) { + return false; + } else if (traversability < unsafeTraversabilityThreshold) { + numberOfUnsafeCells++; + if (numberOfUnsafeCells > maxNumberOfUnsafeCells) { + return false; + } + } + } + + return true; +} + void divideAndAdd(std::vector* vertices, int depth, const Vertex& pLeft, const Vertex& pRight) { if (depth <= 0) { return; @@ -169,12 +322,17 @@ void addExtraPointsForEarlyStopping(const RobotFootprint& footprint, std::vector points->insert(points->begin(), extraPoints.begin(), extraPoints.end()); } -std::unique_ptr createGridMapLazyStateValidator(const grid_map::GridMap& gridMap, - const RobotFootprint& footprint, - const std::string& obstacleLayer) { +std::unique_ptr createGridMapLazyStateValidator( + const grid_map::GridMap& gridMap, const RobotFootprint& footprint, const std::string& obstacleLayer, + const StateValidityCheckingMethod& stateValidityCheckingMethod, const double stateValidityThreshold, + const double unsafeStateValidityThreshold, const int maxNumberOfUnsafeCells) { std::unique_ptr validator = std::make_unique(); validator->setGridMap(gridMap); validator->setObstacleLayerName(obstacleLayer); + validator->setStateValidityCheckingMethod(stateValidityCheckingMethod); + validator->setStateValidityThreshold(stateValidityThreshold); + validator->setUnsafeStateValidityThreshold(unsafeStateValidityThreshold); + validator->setMaxNumberOfUnsafeCells(maxNumberOfUnsafeCells); validator->setFootprint(footprint); validator->initialize(); return std::move(validator); diff --git a/se2_planning/src/GridMapStateValidator.cpp b/se2_planning/src/GridMapStateValidator.cpp index bfeff0d..ce87a18 100644 --- a/se2_planning/src/GridMapStateValidator.cpp +++ b/se2_planning/src/GridMapStateValidator.cpp @@ -30,6 +30,15 @@ void GridMapStateValidator::setGridMap(const grid_map::GridMap& gridMap) { throw std::runtime_error("Grid map has no layers"); } gridMap_ = gridMap; + // TODO does not work correctly, boundaries of traversability layer get set to nan!? Problem because elevation_mapping_cupy + // has strange resolution?! + // Convert resolution, optionally only copy relevant layer + // gridMap_.setFrameId(gridMap.getFrameId()); + // gridMap_.setTimestamp(gridMap.getTimestamp()); + // gridMap_.setGeometry(gridMap.getLength(), gridMap.getResolution(), gridMap.getPosition()); + // if (!gridMap_.addDataFrom(gridMap, true, true, true)) { + // throw std::runtime_error("Grid map add data failed."); + // } isGridMapInitialized_ = true; } bool GridMapStateValidator::isInitialized() const { diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index df3aae0..984549f 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -35,6 +35,10 @@ void OmplPlanner::getPath(Path* path) const { bool OmplPlanner::plan() { simpleSetup_->clear(); simpleSetup_->setStartAndGoalStates(*startState_, *goalState_); + // TODO see https://ompl.kavrakilab.org/genericPlanning.html for continue planning + // The ompl::base::Planner::solve() method can be called repeatedly with different + // allowed time durations until a solution is found. The planning process continues + // with the available data structures when sequential calls to ompl::base::Planner::solve() are made. if (!simpleSetup_->solve(maxPlanningDuration_)) { std::cout << "OmplPlanner: Solve failed" << std::endl; return false; @@ -66,6 +70,10 @@ bool OmplPlanner::initialize() { return true; } +void OmplPlanner::updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) { + throw std::runtime_error("Not implemented"); +} + void OmplPlanner::setMaxPlanningDuration(double T) { maxPlanningDuration_ = T; } diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index adcc1d2..fdeb52a 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -53,6 +53,12 @@ void OmplReedsSheppPlanner::createDefaultStateSpace() { setStateSpaceBoundaries(); } bool OmplReedsSheppPlanner::plan() { + initialize(); + ompl::base::StateSpacePtr space = simpleSetup_->getStateSpace(); + auto bounds = space->as()->getBounds(); + // std::cout << "OmplReedsSheppPlanner: Planner state space bounds: x = [" << bounds.low[0] << ", " << bounds.high[0] << "], y=[" + // << bounds.low[1] << ", " << bounds.high[1] << "]" << std::endl; + bool result = BASE::plan(); *interpolatedPath_ = interpolatePath(*path_, parameters_.pathSpatialResolution_); return result; @@ -63,6 +69,35 @@ void OmplReedsSheppPlanner::setStateSpaceBoundaries() { bounds_->high[0] = parameters_.xUpperBound_; bounds_->high[1] = parameters_.yUpperBound_; stateSpace_->as()->setBounds(*bounds_); + // std::cout << "OmplReedsSheppPlanner: Set state space bounds: x = [" << bounds_->low[0] << ", " << bounds_->high[0] << "], y=[" + // << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl; +} +const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundaries() { + return stateSpace_->as()->getBounds(); +} + +void OmplReedsSheppPlanner::updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) { + bounds_->low[0] = bounds.low[0]; + bounds_->low[1] = bounds.low[1]; + bounds_->high[0] = bounds.high[0]; + bounds_->high[1] = bounds.high[1]; + parameters_.xLowerBound_ = bounds_->low[0]; + parameters_.yLowerBound_ = bounds_->low[1]; + parameters_.xUpperBound_ = bounds_->high[0]; + parameters_.yUpperBound_ = bounds_->high[1]; + stateSpace_->as()->setBounds(*bounds_); + // std::cout << "OmplReedsSheppPlanner: Update state space bounds: x = [" << bounds_->low[0] << ", " << bounds_->high[0] << "], y=[" + // << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl; +} + +bool OmplReedsSheppPlanner::satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) { + ompl::base::ScopedStatePtr stateOmpl(std::make_shared >(stateSpace_)); + auto s = ((*stateOmpl)())->as(); + auto rsState = state.as(); + s->setX(rsState->x_); + s->setY(rsState->y_); + s->setYaw(rsState->yaw_); + return stateSpace_->satisfiesBounds(s); } void OmplReedsSheppPlanner::setStateValidator(std::unique_ptr stateValidator) { @@ -73,6 +108,18 @@ const StateValidator& OmplReedsSheppPlanner::getStateValidator() const { return *stateValidator_; } +void OmplReedsSheppPlanner::lockStateValidator() { + stateValidator_->lock(); +} + +void OmplReedsSheppPlanner::unlockStateValidator() { + stateValidator_->unlock(); +} + +bool OmplReedsSheppPlanner::isLocked() const { + return stateValidator_->isLocked(); +} + bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) { const ReedsSheppState rsState = se2_planning::convert(state); return stateValidator_->isStateValid(rsState); diff --git a/se2_planning/src/StateValidator.cpp b/se2_planning/src/StateValidator.cpp index 74d38cb..304d417 100644 --- a/se2_planning/src/StateValidator.cpp +++ b/se2_planning/src/StateValidator.cpp @@ -20,4 +20,15 @@ bool SE2stateValidator::isStateValid(const State& state) const { return true; } +bool StateValidator::isLocked() const { + return isLocked_; +} + +void StateValidator::lock() { + isLocked_ = true; +} + +void StateValidator::unlock() { + isLocked_ = false; +} } // namespace se2_planning diff --git a/se2_planning/src/planning_benchmark.cpp b/se2_planning/src/planning_benchmark.cpp index c547b5a..d3e7c11 100644 --- a/se2_planning/src/planning_benchmark.cpp +++ b/se2_planning/src/planning_benchmark.cpp @@ -18,6 +18,10 @@ const int numTestCases = 500; const std::string benchmarkLayer = "occupancy"; +const se2_planning::StateValidityCheckingMethod stateValidityCheckingMethod = se2_planning::StateValidityCheckingMethod::COLLISION; +const double collisionThreshold = 0.1; +const double unsafeTraversabilityThreshold = 0.1; +const int maxNumberOfUnsafeCells = 10; void setCostThreshold(se2_planning::OmplReedsSheppPlanner* planner) { auto si = planner->getSimpleSetup()->getSpaceInformation(); @@ -64,8 +68,9 @@ void runNormalValidator(se2_planning::OmplReedsSheppPlanner& planner, const grid void runLazyValidator(se2_planning::OmplReedsSheppPlanner& planner, const grid_map::GridMap& gridMap) { // create state validator - auto plannerStateValidator = - se2_planning::createGridMapLazyStateValidator(gridMap, se2_planning::computeFootprint(1.0, 0.0, 0.5, 0.5), benchmarkLayer); + auto plannerStateValidator = se2_planning::createGridMapLazyStateValidator( + gridMap, se2_planning::computeFootprint(1.0, 0.0, 0.5, 0.5), benchmarkLayer, stateValidityCheckingMethod, collisionThreshold, + unsafeTraversabilityThreshold, maxNumberOfUnsafeCells); planner.setStateValidator(std::move(plannerStateValidator)); std::cout << "benchmarking lazy validator " << std::endl; runPlanner(planner); @@ -73,8 +78,9 @@ void runLazyValidator(se2_planning::OmplReedsSheppPlanner& planner, const grid_m void runLazyRandomValidator(se2_planning::OmplReedsSheppPlanner& planner, const grid_map::GridMap& gridMap) { // create state validator - auto plannerStateValidator = - se2_planning::createGridMapLazyStateValidator(gridMap, se2_planning::computeFootprint(1.0, 0.0, 0.5, 0.5), benchmarkLayer); + auto plannerStateValidator = se2_planning::createGridMapLazyStateValidator( + gridMap, se2_planning::computeFootprint(1.0, 0.0, 0.5, 0.5), benchmarkLayer, stateValidityCheckingMethod, collisionThreshold, + unsafeTraversabilityThreshold, maxNumberOfUnsafeCells); plannerStateValidator->setIsUseRandomizedStrategy(true); planner.setStateValidator(std::move(plannerStateValidator)); std::cout << "benchmarking lazy random validator " << std::endl; @@ -83,8 +89,9 @@ void runLazyRandomValidator(se2_planning::OmplReedsSheppPlanner& planner, const void runLazyRandomEarlyStoppingValidator(se2_planning::OmplReedsSheppPlanner& planner, const grid_map::GridMap& gridMap) { // create state validator - auto plannerStateValidator = - se2_planning::createGridMapLazyStateValidator(gridMap, se2_planning::computeFootprint(1.0, 0.0, 0.5, 0.5), benchmarkLayer); + auto plannerStateValidator = se2_planning::createGridMapLazyStateValidator( + gridMap, se2_planning::computeFootprint(1.0, 0.0, 0.5, 0.5), benchmarkLayer, stateValidityCheckingMethod, collisionThreshold, + unsafeTraversabilityThreshold, maxNumberOfUnsafeCells); plannerStateValidator->setIsUseRandomizedStrategy(true); plannerStateValidator->setIsUseEarlyStoppingHeuristic(true); planner.setStateValidator(std::move(plannerStateValidator)); diff --git a/se2_planning_ros/CMakeLists.txt b/se2_planning_ros/CMakeLists.txt index 8987cf8..bc65159 100644 --- a/se2_planning_ros/CMakeLists.txt +++ b/se2_planning_ros/CMakeLists.txt @@ -7,6 +7,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(SRC_FILES src/OmplReedsSheppPlannerRos.cpp + src/GridMapLazyStateValidatorRos.cpp src/loaders.cpp src/PlannerRos.cpp src/common.cpp @@ -18,6 +19,8 @@ set(CATKIN_PACKAGE_DEPENDENCIES nav_msgs tf2 se2_navigation_msgs + grid_map_ros + visualization_msgs ) find_package(catkin REQUIRED diff --git a/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml b/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml index 142ed8c..a785fe0 100644 --- a/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml +++ b/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml @@ -5,17 +5,40 @@ state_space: y_lower: -1000.0 y_upper: 1000.0 turning_radius: 10.0 - + +state_validator_ros: + grid_map_frame: odom + 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: obstacle + grid_map_state_validity_checking_method: collision + grid_map_state_validity_threshold: 0.1 + grid_map_unsafe_state_validity_threshold: 0.1 + grid_map_max_number_of_unsafe_cells: 0 + # grid map dimensions/geometry + grid_map_resolution: 0.1 + grid_map_length: 20.0 + grid_map_width: 20.0 + grid_map_position_x: 5.0 + grid_map_position_y: 0.0 + grid_map_default_value: 0.0 + # Robot footprint for collision checking + robot_footprint_length_forward: 0.75 + robot_footprint_length_backward: 0.75 + robot_footprint_width_left: 0.50 + robot_footprint_width_right: 0.50 + 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/nav_msgs_path - path_frame: map + 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 - ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners + ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners, not implemented yet max_planning_time: 1.0 ompl_planners: diff --git a/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp b/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp new file mode 100644 index 0000000..df5d2e7 --- /dev/null +++ b/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp @@ -0,0 +1,81 @@ +/* + * GridMapLazyStateValidatorRos.hpp + * + * Created on: Jul 20, 2020 + * Author: meyerc + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include "se2_planning/GridMapLazyStateValidator.hpp" +#include "se2_planning_ros/OmplReedsSheppPlannerRos.hpp" + +#include + +namespace se2_planning { + +using WriteLock = boost::unique_lock; +using ReadLock = boost::shared_lock; + +struct GridMapLazyStateValidatorRosParameters { + std::string gridMapFrame_ = "map"; + std::string gridMapMsgSubTopic_ = "state_validator_ros/traversability_map_in"; + std::string gridMapMsgPubTopic_ = "state_validator_ros/traversability_map_out"; + std::string gridMapObstacleLayerName_ = "traversability"; // redundant, already defined by GridMapStateValidator + StateValidityCheckingMethod gridMapStateValidityCheckingMethod_ = + StateValidityCheckingMethod::TRAVERSABILITY; // redundant, already defined by GridMapStateValidator + double gridMapStateValidityThreshold_ = 0.5; + double gridMapUnsafeStateValidityThreshold_ = 0.5; + int gridMapMaxNumberOfUnsafeCells_ = 0; + double gridMapResolution_ = 0.2; + double gridMapLength_ = 20.0; + double gridMapWidth_ = 20.0; + double gridMapPositionX_ = 0.0; + double gridMapPositionY_ = 0.0; + double gridMapDefaultValue_ = 0.0; + double robotFootPrintLengthForward_ = 0.75; + double robotFootPrintLengthBackward_ = 0.75; + double robotFootPrintWidthLeft_ = 0.5; + double robotFootPrintWidthRight_ = 0.5; +}; + +class GridMapLazyStateValidatorRos : public GridMapLazyStateValidator { + using BASE = GridMapLazyStateValidator; + + public: + explicit GridMapLazyStateValidatorRos(ros::NodeHandlePtr nh); + ~GridMapLazyStateValidatorRos() override = default; + + void initialize() override; + void setParameters(const GridMapLazyStateValidatorRosParameters& parameters); + void publishMap(const grid_map::GridMap& map) const; + + private: + void initRos(); + void mapCb(const grid_map_msgs::GridMap& msg); + + ros::NodeHandlePtr nh_; + GridMapLazyStateValidatorRosParameters parameters_; + + //! Grid map data. + ros::Subscriber mapSubscriber_; + ros::Publisher mapPublisher_; + bool newMapAvailable_ = false; + boost::shared_mutex gridMapMutex_; +}; + +geometry_msgs::Polygon convert(const RobotFootprint& footprint); + +std::unique_ptr createGridMapLazyStateValidatorRos(const ros::NodeHandlePtr nh, + const GridMapLazyStateValidatorRosParameters& params, + const grid_map::GridMap& gridMap, + const RobotFootprint& footprint); + +} /* namespace se2_planning */ diff --git a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp index ef56ddc..91c3200 100644 --- a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp @@ -14,6 +14,7 @@ #include "se2_navigation_msgs/RequestPathSrv.h" #include "se2_planning/OmplReedsSheppPlanner.hpp" #include "se2_planning_ros/PlannerRos.hpp" +#include "visualization_msgs/Marker.h" namespace se2_planning { @@ -23,6 +24,7 @@ struct OmplReedsSheppPlannerRosParameters { std::string planningSerivceName_ = "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 { @@ -40,13 +42,22 @@ class OmplReedsSheppPlannerRos : public PlannerRos { private: void initRos(); void publishPathNavMsgs() const; + void publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const; + void initializeStateSpaceMarker(); + void publishStateSpaceMarker(); bool planningService(PlanningService::Request& req, PlanningService::Response& res) override; ros::Publisher pathNavMsgsPublisher_; ros::Publisher pathPublisher_; + ros::Publisher startPublisher_; + ros::Publisher goalPublisher_; + ros::Publisher stateSpacePublisher_; + visualization_msgs::Marker stateSpaceMarker_; OmplReedsSheppPlannerRosParameters parameters_; ros::ServiceServer planningService_; int planSeqNumber_ = -1; + ros::Time planTimeStamp_; + const int reedsSheppStateSpaceDim_ = 2; }; } /* namespace se2_planning */ diff --git a/se2_planning_ros/include/se2_planning_ros/loaders.hpp b/se2_planning_ros/include/se2_planning_ros/loaders.hpp index 9262cd6..6b4ab5b 100644 --- a/se2_planning_ros/include/se2_planning_ros/loaders.hpp +++ b/se2_planning_ros/include/se2_planning_ros/loaders.hpp @@ -6,14 +6,17 @@ */ #pragma once + #include "se2_planning/OmplReedsSheppPlanner.hpp" #include "se2_planning/ompl_planner_creators.hpp" +#include "se2_planning_ros/GridMapLazyStateValidatorRos.hpp" #include "se2_planning_ros/OmplReedsSheppPlannerRos.hpp" namespace se2_planning { OmplReedsSheppPlannerParameters loadOmplReedsSheppPlannerParameters(const std::string& filename); OmplReedsSheppPlannerRosParameters loadOmplReedsSheppPlannerRosParameters(const std::string& filename); +GridMapLazyStateValidatorRosParameters loadGridMapLazyStateValidatorRosParameters(const std::string& filename); void loadOmplPlannerParameters(const std::string& plannerName, const std::string& filename, OmplPlannerParameters* params); void loadOmplPlannerParameters(OmplPlanners type, const std::string& filename, OmplPlannerParameters* params); diff --git a/se2_planning_ros/launch/se2_planner.launch b/se2_planning_ros/launch/se2_planner.launch index 98b193c..cec6e0f 100644 --- a/se2_planning_ros/launch/se2_planner.launch +++ b/se2_planning_ros/launch/se2_planner.launch @@ -14,10 +14,13 @@ + + + diff --git a/se2_planning_ros/package.xml b/se2_planning_ros/package.xml index 280f05e..2be1e14 100644 --- a/se2_planning_ros/package.xml +++ b/se2_planning_ros/package.xml @@ -15,6 +15,9 @@ tf2 yaml-cpp se2_navigation_msgs + se2_planning_rviz - + + grid_map_ros + visualization_msgs diff --git a/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp b/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp new file mode 100644 index 0000000..c6cb047 --- /dev/null +++ b/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp @@ -0,0 +1,92 @@ +/* + * GridMapLazyStateValidatorRos.cpp + * + * Created on: Jul 20, 2020 + * Author: meyerc + */ + +#include "se2_planning_ros/GridMapLazyStateValidatorRos.hpp" + +#include + +namespace se2_planning { + +GridMapLazyStateValidatorRos::GridMapLazyStateValidatorRos(ros::NodeHandlePtr nh) : BASE(), nh_(nh), newMapAvailable_(false) {} + +void GridMapLazyStateValidatorRos::setParameters(const GridMapLazyStateValidatorRosParameters& parameters) { + parameters_ = parameters; +} + +void GridMapLazyStateValidatorRos::initialize() { + BASE::initialize(); + initRos(); +} + +void GridMapLazyStateValidatorRos::mapCb(const grid_map_msgs::GridMap& msg) { + // TODO Also replace using the same gridMapMutex_? Issue that we require to set it in OMPLReedsSheppPlanner which only + // has access to StateValidator class => add general state validator mutex? + if (isLocked()) { + ROS_INFO_STREAM("Planner is running, grid map for state validator can not be updated!"); + return; + } else { + grid_map::GridMap newMap; + grid_map::GridMapRosConverter::fromMessage(msg, newMap); + + if (newMap.exists(obstacleLayerName_)) { + WriteLock writeLock(gridMapMutex_); + setGridMap(newMap); + newMapAvailable_ = true; + if (!isGridMapInitialized_) { + isGridMapInitialized_ = true; + } + writeLock.unlock(); + publishMap(getGridMap()); + } else { + ROS_ERROR("GlobalMap: No traversability layer found to load!"); + } + } +} + +void GridMapLazyStateValidatorRos::initRos() { + // Visualize map used in state validator in rviz + mapPublisher_ = nh_->advertise(parameters_.gridMapMsgPubTopic_, 1); + // Input topic for grid map + mapSubscriber_ = nh_->subscribe(parameters_.gridMapMsgSubTopic_, 1, &GridMapLazyStateValidatorRos::mapCb, this); +} + +void GridMapLazyStateValidatorRos::publishMap(const grid_map::GridMap& map) const { + grid_map_msgs::GridMap msg; + grid_map::GridMapRosConverter::toMessage(map, msg); + mapPublisher_.publish(msg); +} + +geometry_msgs::Polygon convert(const RobotFootprint& footprint) { + geometry_msgs::Polygon polygon; + for (const auto& vertex : footprint.vertex_) { + geometry_msgs::Point32 point; + point.x = vertex.x_; + point.y = vertex.y_; + point.z = 0.0; + polygon.points.push_back(point); + } + return polygon; +} + +std::unique_ptr createGridMapLazyStateValidatorRos(const ros::NodeHandlePtr nh, + const GridMapLazyStateValidatorRosParameters& params, + const grid_map::GridMap& gridMap, + const RobotFootprint& footprint) { + std::unique_ptr validator = std::make_unique(nh); + validator->setGridMap(gridMap); + validator->setObstacleLayerName(params.gridMapObstacleLayerName_); + validator->setStateValidityCheckingMethod(params.gridMapStateValidityCheckingMethod_); + validator->setStateValidityThreshold(params.gridMapStateValidityThreshold_); + validator->setUnsafeStateValidityThreshold(params.gridMapUnsafeStateValidityThreshold_); + validator->setMaxNumberOfUnsafeCells(params.gridMapMaxNumberOfUnsafeCells_); + validator->setFootprint(footprint); + validator->setParameters(params); + validator->initialize(); + return std::move(validator); +} + +} /* namespace se2_planning */ diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index 4b26c09..51e41e8 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -14,9 +14,7 @@ namespace se2_planning { -OmplReedsSheppPlannerRos::OmplReedsSheppPlannerRos(ros::NodeHandlePtr nh) : BASE(nh) { - initRos(); -} +OmplReedsSheppPlannerRos::OmplReedsSheppPlannerRos(ros::NodeHandlePtr nh) : BASE(nh) {} void OmplReedsSheppPlannerRos::setParameters(const OmplReedsSheppPlannerRosParameters& parameters) { parameters_ = parameters; @@ -24,6 +22,8 @@ void OmplReedsSheppPlannerRos::setParameters(const OmplReedsSheppPlannerRosParam bool OmplReedsSheppPlannerRos::initialize() { bool result = BASE::initialize(); + initRos(); + initializeStateSpaceMarker(); return result; } bool OmplReedsSheppPlannerRos::plan() { @@ -40,14 +40,76 @@ bool OmplReedsSheppPlannerRos::plan() { } bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, PlanningService::Response& res) { + // TODO Grid map in state validator is currently initialized to some default values, this check is therefore useless + if (!planner_->as()->getStateValidator().isInitialized()) { + ROS_WARN_STREAM("State validator has not been initialized yet. Abort planning."); + res.status = false; + return true; + } + + planTimeStamp_ = req.pathRequest.header.stamp; + const auto start = se2_planning::convert(req.pathRequest.startingPose); const auto goal = se2_planning::convert(req.pathRequest.goalPose); setStartingState(start); setGoalState(goal); + + // Block update of state validator obstacle map during planning + planner_->as()->lockStateValidator(); + // TODO move from se2_planner_node here (introduces dependency but makes state space update more consistent: + // adapt state space boundaries from grid map + // planner_->as()->getStateValidator().as + + // Adapt state space boundaries (larger than grid map, state validity checking assumes area out of bounds to be + // traversable) to contain initial and goal state otherwise RRTstar fails + // TODO expose stateSpaceBoundsMargin_ as param => depends on footprint size? + // TODO move to OMPLReedsSheppPlanner.cpp? + const double stateSpaceBoundsMargin_ = parameters_.stateSpaceBoundsMargin_; + if (!planner_->as()->satisfiesStateSpaceBounds(start)) { + ROS_DEBUG("Initial state not in grid map. Enlarge state space boundaries."); + const auto bounds = planner_->as()->getStateSpaceBoundaries(); + ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_); + newBounds.high[0] = std::max(bounds.high[0], start.x_ + stateSpaceBoundsMargin_); + newBounds.high[1] = std::max(bounds.high[1], start.y_ + stateSpaceBoundsMargin_); + newBounds.low[0] = std::min(bounds.low[0], start.x_ - stateSpaceBoundsMargin_); + newBounds.low[1] = std::min(bounds.low[1], start.y_ - stateSpaceBoundsMargin_); + planner_->as()->updateStateSpaceBounds(newBounds); + } + if (!planner_->as()->satisfiesStateSpaceBounds(goal)) { + ROS_DEBUG("Goal state not in grid map. Enlarge state space boundaries."); + const auto bounds = planner_->as()->getStateSpaceBoundaries(); + ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_); + newBounds.high[0] = std::max(bounds.high[0], goal.x_ + stateSpaceBoundsMargin_); + newBounds.high[1] = std::max(bounds.high[1], goal.y_ + stateSpaceBoundsMargin_); + newBounds.low[0] = std::min(bounds.low[0], goal.x_ - stateSpaceBoundsMargin_); + newBounds.low[1] = std::min(bounds.low[1], goal.y_ - stateSpaceBoundsMargin_); + planner_->as()->updateStateSpaceBounds(newBounds); + } + + // TODO move to detach? Better to publish this info for debugging before checking validity of states. + publishStartGoalMsgs(start, goal); + publishStateSpaceMarker(); + + // Use state validator only after lock mutex is active and state space is updated + // Checks only for non-traversable terrain not for state space bounds + if (!planner_->as()->getStateValidator().isStateValid(start)) { + ROS_WARN_STREAM("Start state (x: " << start.x_ << ", y: " << start.y_ << ", yaw: " << start.yaw_ + << ") not valid. Start planning anyway."); + } + if (!planner_->as()->getStateValidator().isStateValid(goal)) { + ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Start planning anyway."); + // ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Abort planning."); + // planner_->as()->unlockStateValidator(); + // return false; + } + bool result = plan(); + planner_->as()->unlockStateValidator(); res.status = result; + // Always return true for function, if something fails, set res.status = false, otherwise service call fails with + // error that service not callable return true; } @@ -55,6 +117,9 @@ void OmplReedsSheppPlannerRos::initRos() { pathNavMsgsPublisher_ = nh_->advertise(parameters_.pathNavMsgTopic_, 1, true); planningService_ = nh_->advertiseService(parameters_.planningSerivceName_, &OmplReedsSheppPlannerRos::planningService, this); pathPublisher_ = nh_->advertise(parameters_.pathMsgTopic_, 1); + startPublisher_ = nh_->advertise("start", 1); + goalPublisher_ = nh_->advertise("goal", 1); + stateSpacePublisher_ = nh_->advertise("state_space", 1); } void OmplReedsSheppPlannerRos::publishPathNavMsgs() const { @@ -62,7 +127,7 @@ void OmplReedsSheppPlannerRos::publishPathNavMsgs() const { planner_->as()->getInterpolatedPath(&rsPath, parameters_.pathNavMsgResolution_); nav_msgs::Path msg = se2_planning::copyAllPoints(rsPath); msg.header.frame_id = parameters_.pathFrame_; - msg.header.stamp = ros::Time::now(); + msg.header.stamp = planTimeStamp_; msg.header.seq = planSeqNumber_; pathNavMsgsPublisher_.publish(msg); ROS_INFO_STREAM("Publishing ReedsShepp path nav msg, num states: " << msg.poses.size()); @@ -73,10 +138,66 @@ void OmplReedsSheppPlannerRos::publishPath() const { planner_->getPath(&rsPath); se2_navigation_msgs::Path msg = se2_planning::convert(rsPath); msg.header_.frame_id = parameters_.pathFrame_; - msg.header_.stamp = ros::Time::now(); + msg.header_.stamp = planTimeStamp_; msg.header_.seq = planSeqNumber_; pathPublisher_.publish(se2_navigation_msgs::convert(msg)); ROS_INFO_STREAM("Publishing ReedsShepp path, num states: " << rsPath.numPoints()); } +void OmplReedsSheppPlannerRos::publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const { + geometry_msgs::PoseStamped startPose; + startPose.header.frame_id = parameters_.pathFrame_; + startPose.header.stamp = planTimeStamp_; + startPose.pose = se2_planning::convert(start); + startPublisher_.publish(startPose); + geometry_msgs::PoseStamped goalPose; + goalPose.header.frame_id = parameters_.pathFrame_; + goalPose.header.stamp = planTimeStamp_; + goalPose.pose = se2_planning::convert(goal); + goalPublisher_.publish(goalPose); +} + +void OmplReedsSheppPlannerRos::initializeStateSpaceMarker() { + // TODO expose as params? + double lineWidth_ = 0.01; + std_msgs::ColorRGBA color_; + // No transparency + color_.a = 1; + // Black + color_.r = 0; + color_.g = 0; + color_.b = 0; + // Init marker + int nVertices_ = 5; + stateSpaceMarker_.ns = "state_space"; + stateSpaceMarker_.lifetime = ros::Duration(); + stateSpaceMarker_.action = visualization_msgs::Marker::ADD; + stateSpaceMarker_.type = visualization_msgs::Marker::LINE_STRIP; + stateSpaceMarker_.scale.x = lineWidth_; + stateSpaceMarker_.points.resize(nVertices_); // Initialized to [0.0, 0.0, 0.0] + stateSpaceMarker_.colors.resize(nVertices_, color_); +} + +void OmplReedsSheppPlannerRos::publishStateSpaceMarker() { + // Set marker info. + stateSpaceMarker_.header.frame_id = parameters_.pathFrame_; + stateSpaceMarker_.header.stamp = planTimeStamp_; + + // Set positions of markers. + const auto bounds = planner_->as()->getStateSpaceBoundaries(); + stateSpaceMarker_.points[0].x = bounds.low[0]; + stateSpaceMarker_.points[0].y = bounds.low[1]; + stateSpaceMarker_.points[1].x = bounds.high[0]; + stateSpaceMarker_.points[1].y = bounds.low[1]; + stateSpaceMarker_.points[2].x = bounds.high[0]; + stateSpaceMarker_.points[2].y = bounds.high[1]; + stateSpaceMarker_.points[3].x = bounds.low[0]; + stateSpaceMarker_.points[3].y = bounds.high[1]; + // Close the rectangle with the fifth point + stateSpaceMarker_.points[4].x = stateSpaceMarker_.points[0].x; + stateSpaceMarker_.points[4].y = stateSpaceMarker_.points[0].y; + + stateSpacePublisher_.publish(stateSpaceMarker_); +} + } /* namespace se2_planning */ diff --git a/se2_planning_ros/src/loaders.cpp b/se2_planning_ros/src/loaders.cpp index 73f883a..8425065 100644 --- a/se2_planning_ros/src/loaders.cpp +++ b/se2_planning_ros/src/loaders.cpp @@ -53,6 +53,49 @@ OmplReedsSheppPlannerRosParameters loadOmplReedsSheppPlannerRosParameters(const parameters.planningSerivceName_ = node["planning_service_name"].as(); parameters.pathNavMsgResolution_ = node["nav_msg_path_spatial_resolution"].as(); parameters.pathMsgTopic_ = node["path_msg_topic"].as(); + parameters.stateSpaceBoundsMargin_ = node["state_space_bounds_margin"].as(); + + return parameters; +} + +GridMapLazyStateValidatorRosParameters loadGridMapLazyStateValidatorRosParameters(const std::string& filename) { + YAML::Node basenode = YAML::LoadFile(filename); + + if (basenode.IsNull()) { + throw std::runtime_error("GridMapLazyStateValidatorRosParameters::loadParameters loading failed"); + } + + GridMapLazyStateValidatorRosParameters parameters; + auto node = basenode["state_validator_ros"]; + parameters.gridMapFrame_ = node["grid_map_frame"].as(); + parameters.gridMapObstacleLayerName_ = node["grid_map_obstacle_layer_name"].as(); + auto stateValidityCheckingMethodName = node["grid_map_state_validity_checking_method"].as(); + if (stateValidityCheckingMethodName == "collision") { + parameters.gridMapStateValidityCheckingMethod_ = StateValidityCheckingMethod::COLLISION; + } else if (stateValidityCheckingMethodName == "traversability") { + parameters.gridMapStateValidityCheckingMethod_ = StateValidityCheckingMethod::TRAVERSABILITY; + } else if (stateValidityCheckingMethodName == "robust_traversability") { + parameters.gridMapStateValidityCheckingMethod_ = StateValidityCheckingMethod::ROBUST_TRAVERSABILITY; + } else { + throw std::runtime_error( + "Invalid value for StateValidityCheckingMethod. Valid values are 'collision', 'traversability'" + "or 'robust_traversability'"); + } + parameters.gridMapStateValidityThreshold_ = node["grid_map_state_validity_threshold"].as(); + parameters.gridMapUnsafeStateValidityThreshold_ = node["grid_map_unsafe_state_validity_threshold"].as(); + parameters.gridMapMaxNumberOfUnsafeCells_ = node["grid_map_max_number_of_unsafe_cells"].as(); + parameters.gridMapMsgSubTopic_ = node["grid_map_msg_sub_topic"].as(); + parameters.gridMapMsgPubTopic_ = node["grid_map_msg_pub_topic"].as(); + parameters.gridMapResolution_ = node["grid_map_resolution"].as(); + parameters.gridMapLength_ = node["grid_map_length"].as(); + parameters.gridMapWidth_ = node["grid_map_width"].as(); + parameters.gridMapPositionX_ = node["grid_map_position_x"].as(); + parameters.gridMapPositionY_ = node["grid_map_position_y"].as(); + parameters.gridMapDefaultValue_ = node["grid_map_default_value"].as(); + parameters.robotFootPrintLengthForward_ = node["robot_footprint_length_forward"].as(); + parameters.robotFootPrintLengthBackward_ = node["robot_footprint_length_backward"].as(); + parameters.robotFootPrintWidthLeft_ = node["robot_footprint_width_left"].as(); + parameters.robotFootPrintWidthRight_ = node["robot_footprint_width_right"].as(); return parameters; } diff --git a/se2_planning_ros/src/se2_planner_node.cpp b/se2_planning_ros/src/se2_planner_node.cpp index 44614db..09c40b0 100644 --- a/se2_planning_ros/src/se2_planner_node.cpp +++ b/se2_planning_ros/src/se2_planner_node.cpp @@ -7,29 +7,76 @@ #include +#include +#include "se2_planning/OmplReedsSheppPlanner.hpp" +#include "se2_planning_ros/GridMapLazyStateValidatorRos.hpp" #include "se2_planning_ros/loaders.hpp" -int main(int argc, char** argv) { - using namespace se2_planning; +using namespace se2_planning; + +// Make planner global to have access to it in the gridMapCallback, +// required to update planner bounds in dependence of grid map +auto planner = std::make_shared(); + +void gridMapCallback(const grid_map_msgs::GridMap& msg) { + // Update planner bounds when new map is published, placed here due to class structure + grid_map::GridMap map; + grid_map::GridMapRosConverter::fromMessage(msg, map); + // Grid map is symmetric around position + grid_map::Position mapPosition; + mapPosition = map.getPosition(); + grid_map::Length mapLength; + mapLength = map.getLength(); + ompl::base::RealVectorBounds bounds(2); + bounds.low[0] = mapPosition.x() - mapLength.x() / 2.0; + bounds.low[1] = mapPosition.y() - mapLength.y() / 2.0; + bounds.high[0] = mapPosition.x() + mapLength.x() / 2.0; + bounds.high[1] = mapPosition.y() + mapLength.y() / 2.0; + + if (!planner->isLocked()) { + planner->updateStateSpaceBounds(bounds); + ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", " + << mapLength.y()); + } else { + ROS_DEBUG_STREAM("OMPL State Space Update: Planner is locked. Not updating state space bounds."); + } +} +int main(int argc, char** argv) { ros::init(argc, argv, "se2_planner_node"); ros::NodeHandlePtr nh(new ros::NodeHandle("~")); + // Load planner parameters std::string filename = nh->param("/ompl_planner_ros/parameter_path", "ompl_rs_planner_ros/nav_msgs_path"); const auto plannerParameters = loadOmplReedsSheppPlannerParameters(filename); const auto plannerRosParameters = loadOmplReedsSheppPlannerRosParameters(filename); - auto planner = std::make_shared(); + const auto stateValidatorRosParameters = loadGridMapLazyStateValidatorRosParameters(filename); planner->setParameters(plannerParameters); + + // Create initial grid map + grid_map::GridMap gridMap; + gridMap.setFrameId(stateValidatorRosParameters.gridMapFrame_); + gridMap.setGeometry(grid_map::Length(stateValidatorRosParameters.gridMapLength_, stateValidatorRosParameters.gridMapWidth_), + stateValidatorRosParameters.gridMapResolution_, + grid_map::Position(stateValidatorRosParameters.gridMapPositionX_, stateValidatorRosParameters.gridMapPositionY_)); + gridMap.add(stateValidatorRosParameters.gridMapObstacleLayerName_, stateValidatorRosParameters.gridMapDefaultValue_); + + // Set grid map state validator + auto validator = se2_planning::createGridMapLazyStateValidatorRos( + nh, stateValidatorRosParameters, gridMap, + se2_planning::computeFootprint( + stateValidatorRosParameters.robotFootPrintLengthForward_, stateValidatorRosParameters.robotFootPrintLengthBackward_, + stateValidatorRosParameters.robotFootPrintWidthLeft_, stateValidatorRosParameters.robotFootPrintWidthRight_)); + planner->setStateValidator(std::move(validator)); + + // Setup ROS interface and start node se2_planning::OmplReedsSheppPlannerRos plannerRos(nh); plannerRos.setPlanningStrategy(planner); plannerRos.setParameters(plannerRosParameters); plannerRos.initialize(); - OmplPlannerParameters plannerOmplParameters; - const std::string plannerName = plannerParameters.omplPlannerName_; - loadOmplPlannerParameters(plannerName, filename, &plannerOmplParameters); - auto omplPlanner = createPlanner(planner->getSimpleSetup()->getSpaceInformation(), plannerName); - setPlannerParameters(plannerOmplParameters, plannerName, omplPlanner); - planner->setOmplPlanner(omplPlanner); + + // Callback to update OMPL planner bounds when map changes + ros::Subscriber mapSub = nh->subscribe(stateValidatorRosParameters.gridMapMsgSubTopic_, 1, gridMapCallback); ros::spin(); diff --git a/se2_planning_rviz/rviz/default.rviz b/se2_planning_rviz/rviz/default.rviz index 3a335e9..b4c29a7 100644 --- a/se2_planning_rviz/rviz/default.rviz +++ b/se2_planning_rviz/rviz/default.rviz @@ -1,14 +1,15 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 85 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Path1 + - /GridMap1 Splitter Ratio: 0.5 - Tree Height: 699 + Tree Height: 658 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -97,6 +98,27 @@ Visualization Manager: Show Visual Aids: false Update Topic: /se2_planning_markers/update Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: traversability + Color Transformer: GridMapLayer + Enabled: true + Height Layer: traversability + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /se2_grid_map_test_node/grid_map + Unreliable: false + Use Rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -132,18 +154,18 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 8.55838680267334 - Y: -4.175713539123535 - Z: -1.6773569583892822 + X: -2.0674149990081787 + Y: -4.780889987945557 + Z: -3.0755908489227295 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.094796895980835 + Pitch: 0.6997972726821899 Target Frame: Value: Orbit (rviz) - Yaw: 3.7990670204162598 + Yaw: 1.7190651893615723 Saved: ~ Window Geometry: Displays: @@ -153,7 +175,7 @@ Window Geometry: Hide Right Dock: false PlanningPanel: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000024300000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000363000001780100001cfa000000000100000002fb0000001a0050006c0061006e006e0069006e006700500061006e0065006c0100000000ffffffff0000019500fffffffb000000100044006900730070006c0061007900730100000000000002f60000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003df0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000024300000352fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000004700000352000001ce01000023fa000000010100000002fb0000001a0050006c0061006e006e0069006e006700500061006e0065006c0100000000ffffffff0000019b00fffffffb000000100044006900730070006c0061007900730100000000000002f60000016100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000352fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004700000352000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000040fc0100000002fb0000000800540069006d006501000000000000073d0000037100fffffffb0000000800540069006d00650100000000000004500000000000000000000003dd0000035200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -163,5 +185,5 @@ Window Geometry: Views: collapsed: false Width: 1853 - X: 1987 - Y: 27 + X: 455 + Y: 25 diff --git a/se2_visualization_ros/include/se2_visualization_ros/visualization_helpers.hpp b/se2_visualization_ros/include/se2_visualization_ros/visualization_helpers.hpp index 65b9572..7893786 100644 --- a/se2_visualization_ros/include/se2_visualization_ros/visualization_helpers.hpp +++ b/se2_visualization_ros/include/se2_visualization_ros/visualization_helpers.hpp @@ -26,6 +26,7 @@ #pragma once +#include #include #include #include @@ -74,6 +75,9 @@ void drawSphere(const Eigen::Vector3d& p, const Color& color, double diameter, v void drawSphereList(const std::vector& points, const Color& color, double diameter, visualization_msgs::Marker* marker); +void drawFootprint(const Eigen::Vector3d& center, const Eigen::Vector4d& orientation, const geometry_msgs::Polygon& footprint, + const Color& color, const double& scale, visualization_msgs::Marker* marker); + geometry_msgs::Quaternion toQuaternion(double roll, double pitch, double yaw); } /*namespace se2_visualization_ros */ diff --git a/se2_visualization_ros/src/visualization_helpers.cpp b/se2_visualization_ros/src/visualization_helpers.cpp index a6dc1c9..374409e 100644 --- a/se2_visualization_ros/src/visualization_helpers.cpp +++ b/se2_visualization_ros/src/visualization_helpers.cpp @@ -122,6 +122,27 @@ void drawSphereList(const std::vector& points, const Color marker->points = points; } +void drawFootprint(const Eigen::Vector3d& center, const Eigen::Vector4d& orientation, const geometry_msgs::Polygon& footprint, + const Color& color, const double& scale, visualization_msgs::Marker* marker) { + marker->color = color; + marker->scale.x = scale; + marker->action = visualization_msgs::Marker::ADD; + marker->type = visualization_msgs::Marker::LINE_STRIP; + marker->pose.position = createPoint(center.x(), center.y(), center.z()); + + // set a unit quaternion such that rviz doesn't complain + marker->pose.orientation.x = orientation.x(); + marker->pose.orientation.y = orientation.y(); + marker->pose.orientation.z = orientation.z(); + marker->pose.orientation.w = orientation.w(); + + // Need five points for a rectangle to result in four lines + for (const auto& vertex : footprint.points) { + marker->points.push_back(createPoint(vertex.x, vertex.y, vertex.z)); + } + marker->points.push_back(createPoint(footprint.points[0].x, footprint.points[0].y, footprint.points[0].z)); +} + void drawAxes(const Eigen::Vector3d& p, const Eigen::Quaterniond& q, double scale, double line_width, visualization_msgs::Marker* marker) { marker->colors.resize(6); marker->points.resize(6); From 51f0bace18ff73ef6c4ef0899163ed8b0206534f Mon Sep 17 00:00:00 2001 From: meychr Date: Thu, 13 May 2021 21:07:16 +0200 Subject: [PATCH 02/24] Make class member of StateValidator private --- se2_planning/include/se2_planning/StateValidator.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/se2_planning/include/se2_planning/StateValidator.hpp b/se2_planning/include/se2_planning/StateValidator.hpp index 9191115..3111520 100644 --- a/se2_planning/include/se2_planning/StateValidator.hpp +++ b/se2_planning/include/se2_planning/StateValidator.hpp @@ -20,6 +20,8 @@ class StateValidator { virtual bool isLocked() const; virtual void lock(); virtual void unlock(); + + private: bool isLocked_ = false; }; From c8ad75dfd72956b0eca0707dfb0dbfb1532d5eed Mon Sep 17 00:00:00 2001 From: meychr Date: Thu, 13 May 2021 21:24:59 +0200 Subject: [PATCH 03/24] Make some class functions constant --- se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp | 4 ++-- se2_planning/src/OmplReedsSheppPlanner.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index adf8623..01060c2 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -60,8 +60,8 @@ class OmplReedsSheppPlanner final : public OmplPlanner { void unlockStateValidator(); bool isLocked() const; void updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) override; - bool satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state); - const ompl::base::RealVectorBounds& getStateSpaceBoundaries(); + bool satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) const; + const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const; private: void createDefaultStateSpace(); diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index fdeb52a..91ca100 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -72,7 +72,7 @@ void OmplReedsSheppPlanner::setStateSpaceBoundaries() { // std::cout << "OmplReedsSheppPlanner: Set state space bounds: x = [" << bounds_->low[0] << ", " << bounds_->high[0] << "], y=[" // << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl; } -const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundaries() { +const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundaries() const { return stateSpace_->as()->getBounds(); } @@ -90,7 +90,7 @@ void OmplReedsSheppPlanner::updateStateSpaceBounds(const ompl::base::RealVectorB // << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl; } -bool OmplReedsSheppPlanner::satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) { +bool OmplReedsSheppPlanner::satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) const { ompl::base::ScopedStatePtr stateOmpl(std::make_shared >(stateSpace_)); auto s = ((*stateOmpl)())->as(); auto rsState = state.as(); From 0d0bcc061f20563fb01b03194bbd5181cd9bb672 Mon Sep 17 00:00:00 2001 From: meychr Date: Thu, 13 May 2021 21:46:46 +0200 Subject: [PATCH 04/24] Unify name of state space boundaries to StateSpaceBoundaries --- .../include/se2_planning/OmplPlanner.hpp | 2 +- .../se2_planning/OmplReedsSheppPlanner.hpp | 4 +- se2_planning/src/OmplPlanner.cpp | 2 +- se2_planning/src/OmplReedsSheppPlanner.cpp | 4 +- .../OmplReedsSheppPlannerRos.hpp | 7 +- .../src/OmplReedsSheppPlannerRos.cpp | 71 +++++++++---------- se2_planning_ros/src/se2_planner_node.cpp | 2 +- 7 files changed, 46 insertions(+), 46 deletions(-) diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index 9b7f211..c970366 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -33,7 +33,7 @@ class OmplPlanner : public Planner { void getInterpolatedPath(Path* interpolatedPath, unsigned int numPoints) const; ompl::geometric::SimpleSetupPtr getSimpleSetup() const; void setOmplPlanner(ompl::base::PlannerPtr planner); - virtual void updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds); + virtual void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds); protected: virtual void initializeStateSpace() = 0; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index 01060c2..88a11ef 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -59,8 +59,8 @@ class OmplReedsSheppPlanner final : public OmplPlanner { void lockStateValidator(); void unlockStateValidator(); bool isLocked() const; - void updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) override; - bool satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) const; + void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override; + bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const; const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const; private: diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index 984549f..ce77397 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -70,7 +70,7 @@ bool OmplPlanner::initialize() { return true; } -void OmplPlanner::updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) { +void OmplPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) { throw std::runtime_error("Not implemented"); } diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index 91ca100..bf171e7 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -76,7 +76,7 @@ const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundari return stateSpace_->as()->getBounds(); } -void OmplReedsSheppPlanner::updateStateSpaceBounds(const ompl::base::RealVectorBounds& bounds) { +void OmplReedsSheppPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) { bounds_->low[0] = bounds.low[0]; bounds_->low[1] = bounds.low[1]; bounds_->high[0] = bounds.high[0]; @@ -90,7 +90,7 @@ void OmplReedsSheppPlanner::updateStateSpaceBounds(const ompl::base::RealVectorB // << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl; } -bool OmplReedsSheppPlanner::satisfiesStateSpaceBounds(const se2_planning::ReedsSheppState& state) const { +bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const { ompl::base::ScopedStatePtr stateOmpl(std::make_shared >(stateSpace_)); auto s = ((*stateOmpl)())->as(); auto rsState = state.as(); diff --git a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp index 91c3200..59d694d 100644 --- a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp @@ -43,8 +43,8 @@ class OmplReedsSheppPlannerRos : public PlannerRos { void initRos(); void publishPathNavMsgs() const; void publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const; - void initializeStateSpaceMarker(); - void publishStateSpaceMarker(); + void initializeStateSpaceBoundaryMarker(); + void publishStateSpaceBoundaryMarker(); bool planningService(PlanningService::Request& req, PlanningService::Response& res) override; ros::Publisher pathNavMsgsPublisher_; @@ -52,7 +52,8 @@ class OmplReedsSheppPlannerRos : public PlannerRos { ros::Publisher startPublisher_; ros::Publisher goalPublisher_; ros::Publisher stateSpacePublisher_; - visualization_msgs::Marker stateSpaceMarker_; + + visualization_msgs::Marker stateSpaceBoundaryMarker_; OmplReedsSheppPlannerRosParameters parameters_; ros::ServiceServer planningService_; int planSeqNumber_ = -1; diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index 51e41e8..c7577bf 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -23,7 +23,7 @@ void OmplReedsSheppPlannerRos::setParameters(const OmplReedsSheppPlannerRosParam bool OmplReedsSheppPlannerRos::initialize() { bool result = BASE::initialize(); initRos(); - initializeStateSpaceMarker(); + initializeStateSpaceBoundaryMarker(); return result; } bool OmplReedsSheppPlannerRos::plan() { @@ -65,7 +65,7 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl // TODO expose stateSpaceBoundsMargin_ as param => depends on footprint size? // TODO move to OMPLReedsSheppPlanner.cpp? const double stateSpaceBoundsMargin_ = parameters_.stateSpaceBoundsMargin_; - if (!planner_->as()->satisfiesStateSpaceBounds(start)) { + if (!planner_->as()->satisfiesStateSpaceBoundaries(start)) { ROS_DEBUG("Initial state not in grid map. Enlarge state space boundaries."); const auto bounds = planner_->as()->getStateSpaceBoundaries(); ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_); @@ -73,9 +73,9 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl newBounds.high[1] = std::max(bounds.high[1], start.y_ + stateSpaceBoundsMargin_); newBounds.low[0] = std::min(bounds.low[0], start.x_ - stateSpaceBoundsMargin_); newBounds.low[1] = std::min(bounds.low[1], start.y_ - stateSpaceBoundsMargin_); - planner_->as()->updateStateSpaceBounds(newBounds); + planner_->as()->updateStateSpaceBoundaries(newBounds); } - if (!planner_->as()->satisfiesStateSpaceBounds(goal)) { + if (!planner_->as()->satisfiesStateSpaceBoundaries(goal)) { ROS_DEBUG("Goal state not in grid map. Enlarge state space boundaries."); const auto bounds = planner_->as()->getStateSpaceBoundaries(); ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_); @@ -83,12 +83,12 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl newBounds.high[1] = std::max(bounds.high[1], goal.y_ + stateSpaceBoundsMargin_); newBounds.low[0] = std::min(bounds.low[0], goal.x_ - stateSpaceBoundsMargin_); newBounds.low[1] = std::min(bounds.low[1], goal.y_ - stateSpaceBoundsMargin_); - planner_->as()->updateStateSpaceBounds(newBounds); + planner_->as()->updateStateSpaceBoundaries(newBounds); } // TODO move to detach? Better to publish this info for debugging before checking validity of states. publishStartGoalMsgs(start, goal); - publishStateSpaceMarker(); + publishStateSpaceBoundaryMarker(); // Use state validator only after lock mutex is active and state space is updated // Checks only for non-traversable terrain not for state space bounds @@ -157,47 +157,46 @@ void OmplReedsSheppPlannerRos::publishStartGoalMsgs(const ReedsSheppState& start goalPublisher_.publish(goalPose); } -void OmplReedsSheppPlannerRos::initializeStateSpaceMarker() { - // TODO expose as params? - double lineWidth_ = 0.01; - std_msgs::ColorRGBA color_; +void OmplReedsSheppPlannerRos::initializeStateSpaceBoundaryMarker() { + double lineWidth = 0.01; + std_msgs::ColorRGBA color; // No transparency - color_.a = 1; + color.a = 1; // Black - color_.r = 0; - color_.g = 0; - color_.b = 0; + color.r = 0; + color.g = 0; + color.b = 0; // Init marker - int nVertices_ = 5; - stateSpaceMarker_.ns = "state_space"; - stateSpaceMarker_.lifetime = ros::Duration(); - stateSpaceMarker_.action = visualization_msgs::Marker::ADD; - stateSpaceMarker_.type = visualization_msgs::Marker::LINE_STRIP; - stateSpaceMarker_.scale.x = lineWidth_; - stateSpaceMarker_.points.resize(nVertices_); // Initialized to [0.0, 0.0, 0.0] - stateSpaceMarker_.colors.resize(nVertices_, color_); + int nVertices = 5; + stateSpaceBoundaryMarker_.ns = "state_space"; + stateSpaceBoundaryMarker_.lifetime = ros::Duration(); + stateSpaceBoundaryMarker_.action = visualization_msgs::Marker::ADD; + stateSpaceBoundaryMarker_.type = visualization_msgs::Marker::LINE_STRIP; + stateSpaceBoundaryMarker_.scale.x = lineWidth; + stateSpaceBoundaryMarker_.points.resize(nVertices); // Initialized to [0.0, 0.0, 0.0] + stateSpaceBoundaryMarker_.colors.resize(nVertices, color); } -void OmplReedsSheppPlannerRos::publishStateSpaceMarker() { +void OmplReedsSheppPlannerRos::publishStateSpaceBoundaryMarker() { // Set marker info. - stateSpaceMarker_.header.frame_id = parameters_.pathFrame_; - stateSpaceMarker_.header.stamp = planTimeStamp_; + stateSpaceBoundaryMarker_.header.frame_id = parameters_.pathFrame_; + stateSpaceBoundaryMarker_.header.stamp = planTimeStamp_; // Set positions of markers. const auto bounds = planner_->as()->getStateSpaceBoundaries(); - stateSpaceMarker_.points[0].x = bounds.low[0]; - stateSpaceMarker_.points[0].y = bounds.low[1]; - stateSpaceMarker_.points[1].x = bounds.high[0]; - stateSpaceMarker_.points[1].y = bounds.low[1]; - stateSpaceMarker_.points[2].x = bounds.high[0]; - stateSpaceMarker_.points[2].y = bounds.high[1]; - stateSpaceMarker_.points[3].x = bounds.low[0]; - stateSpaceMarker_.points[3].y = bounds.high[1]; + stateSpaceBoundaryMarker_.points[0].x = bounds.low[0]; + stateSpaceBoundaryMarker_.points[0].y = bounds.low[1]; + stateSpaceBoundaryMarker_.points[1].x = bounds.high[0]; + stateSpaceBoundaryMarker_.points[1].y = bounds.low[1]; + stateSpaceBoundaryMarker_.points[2].x = bounds.high[0]; + stateSpaceBoundaryMarker_.points[2].y = bounds.high[1]; + stateSpaceBoundaryMarker_.points[3].x = bounds.low[0]; + stateSpaceBoundaryMarker_.points[3].y = bounds.high[1]; // Close the rectangle with the fifth point - stateSpaceMarker_.points[4].x = stateSpaceMarker_.points[0].x; - stateSpaceMarker_.points[4].y = stateSpaceMarker_.points[0].y; + stateSpaceBoundaryMarker_.points[4].x = stateSpaceBoundaryMarker_.points[0].x; + stateSpaceBoundaryMarker_.points[4].y = stateSpaceBoundaryMarker_.points[0].y; - stateSpacePublisher_.publish(stateSpaceMarker_); + stateSpacePublisher_.publish(stateSpaceBoundaryMarker_); } } /* namespace se2_planning */ diff --git a/se2_planning_ros/src/se2_planner_node.cpp b/se2_planning_ros/src/se2_planner_node.cpp index 09c40b0..0e8a79c 100644 --- a/se2_planning_ros/src/se2_planner_node.cpp +++ b/se2_planning_ros/src/se2_planner_node.cpp @@ -34,7 +34,7 @@ void gridMapCallback(const grid_map_msgs::GridMap& msg) { bounds.high[1] = mapPosition.y() + mapLength.y() / 2.0; if (!planner->isLocked()) { - planner->updateStateSpaceBounds(bounds); + planner->updateStateSpaceBoundaries(bounds); ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", " << mapLength.y()); } else { From 07efb89095633faa799e4fc9c3cb22ef214cc003 Mon Sep 17 00:00:00 2001 From: meychr Date: Thu, 13 May 2021 21:54:28 +0200 Subject: [PATCH 05/24] Reorder member variables --- .../se2_planning_ros/OmplReedsSheppPlannerRos.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp index 59d694d..2ae96fa 100644 --- a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp @@ -53,12 +53,13 @@ class OmplReedsSheppPlannerRos : public PlannerRos { ros::Publisher goalPublisher_; ros::Publisher stateSpacePublisher_; - visualization_msgs::Marker stateSpaceBoundaryMarker_; - OmplReedsSheppPlannerRosParameters parameters_; ros::ServiceServer planningService_; - int planSeqNumber_ = -1; - ros::Time planTimeStamp_; + const int reedsSheppStateSpaceDim_ = 2; + int planSeqNumber_ = -1; + ros::Time planTimeStamp_{0.0}; + OmplReedsSheppPlannerRosParameters parameters_; + visualization_msgs::Marker stateSpaceBoundaryMarker_; }; } /* namespace se2_planning */ From b492b7f042e01679df736c94c6bbc122c9958d2d Mon Sep 17 00:00:00 2001 From: meychr Date: Fri, 14 May 2021 21:12:24 +0200 Subject: [PATCH 06/24] Implement getStartingState and getGoalState for OmplReedsSheppPlanner --- se2_planning/include/se2_planning/OmplPlanner.hpp | 3 +++ .../include/se2_planning/OmplReedsSheppPlanner.hpp | 1 + se2_planning/src/OmplPlanner.cpp | 10 ++++++++++ se2_planning/src/OmplReedsSheppPlanner.cpp | 7 ++++++- 4 files changed, 20 insertions(+), 1 deletion(-) diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index c970366..5044c20 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -24,6 +24,8 @@ class OmplPlanner : public Planner { void setStartingState(const State& startingState) final; void setGoalState(const State& goalState) final; void getPath(Path* path) const final; + void getStartingState(State* startingState) const final; + void getGoalState(State* goalState) const final; void setMaxPlanningDuration(double T); void getOmplPath(ompl::geometric::PathGeometric* omplPath) const; @@ -39,6 +41,7 @@ class OmplPlanner : public Planner { virtual void initializeStateSpace() = 0; virtual bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) = 0; virtual ompl::base::ScopedStatePtr convert(const State& state) const = 0; + virtual void convert(const ompl::base::ScopedStatePtr omplState, State* state) const = 0; virtual void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const = 0; ompl::base::StateSpacePtr stateSpace_; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index 88a11ef..5621a63 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -69,6 +69,7 @@ class OmplReedsSheppPlanner final : public OmplPlanner { void setStateSpaceBoundaries(); bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) final; ompl::base::ScopedStatePtr convert(const State& state) const final; + void convert(const ompl::base::ScopedStatePtr omplState, State* state) const final; void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const final; int getDistanceSignAt(const ompl::geometric::PathGeometric& path, unsigned int id) const; diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index ce77397..9916589 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -32,6 +32,14 @@ void OmplPlanner::getPath(Path* path) const { convert(*interpolatedPath_, path); } +void OmplPlanner::getStartingState(State* startingState) const { + convert(startState_, startingState); +} + +void OmplPlanner::getGoalState(State* goalState) const { + convert(goalState_, goalState); +} + bool OmplPlanner::plan() { simpleSetup_->clear(); simpleSetup_->setStartAndGoalStates(*startState_, *goalState_); @@ -51,10 +59,12 @@ bool OmplPlanner::plan() { return true; } + bool OmplPlanner::reset() { simpleSetup_->clear(); return true; } + bool OmplPlanner::initialize() { initializeStateSpace(); if (stateSpace_ == nullptr) { diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index bf171e7..3cb47e2 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -124,10 +124,16 @@ bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ReedsSheppState rsState = se2_planning::convert(state); return stateValidator_->isStateValid(rsState); } + ompl::base::ScopedStatePtr OmplReedsSheppPlanner::convert(const State& state) const { return se2_planning::convert(*(state.as()), simpleSetup_->getSpaceInformation()); } +void OmplReedsSheppPlanner::convert(const ompl::base::ScopedStatePtr omplState, State* state) const { + auto reedsSheppState = state->as(); + *reedsSheppState = se2_planning::convert(omplState->get()); +} + void OmplReedsSheppPlanner::convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const { se2_planning::convert(pathOmpl, stateSpace_, path); } @@ -238,7 +244,6 @@ ReedsSheppState convert(const ompl::base::State* s) { retState.x_ = rsState->getX(); retState.y_ = rsState->getY(); retState.yaw_ = rsState->getYaw(); - return retState; } From 85723b842a65413c8e1fee98f81cf119f68a8b4c Mon Sep 17 00:00:00 2001 From: meychr Date: Fri, 14 May 2021 21:22:54 +0200 Subject: [PATCH 07/24] Move ROS publishers and services to base class PlannerRos, use Planner state to publish start and goal state --- .../OmplReedsSheppPlannerRos.hpp | 15 +--- .../include/se2_planning_ros/PlannerRos.hpp | 11 +++ .../src/OmplReedsSheppPlannerRos.cpp | 80 ++++++++++--------- se2_planning_ros/src/PlannerRos.cpp | 23 ++++++ 4 files changed, 82 insertions(+), 47 deletions(-) diff --git a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp index 2ae96fa..2fec50c 100644 --- a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp @@ -38,23 +38,16 @@ class OmplReedsSheppPlannerRos : public PlannerRos { bool plan() override; void setParameters(const OmplReedsSheppPlannerRosParameters& parameters); void publishPath() const final; + void publishPathNavMsgs() const final; + void publishStartState() const final; + void publishGoalState() const final; + void publishStateSpaceBoundaryMarker() final; private: void initRos(); - void publishPathNavMsgs() const; - void publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const; void initializeStateSpaceBoundaryMarker(); - void publishStateSpaceBoundaryMarker(); bool planningService(PlanningService::Request& req, PlanningService::Response& res) override; - ros::Publisher pathNavMsgsPublisher_; - ros::Publisher pathPublisher_; - ros::Publisher startPublisher_; - ros::Publisher goalPublisher_; - ros::Publisher stateSpacePublisher_; - - ros::ServiceServer planningService_; - const int reedsSheppStateSpaceDim_ = 2; int planSeqNumber_ = -1; ros::Time planTimeStamp_{0.0}; diff --git a/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp index bee2680..c4288fd 100644 --- a/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp @@ -26,6 +26,10 @@ class PlannerRos : public Planner { void setPlanningStrategy(std::shared_ptr planner); virtual void publishPath() const; + virtual void publishPathNavMsgs() const; + virtual void publishStartState() const; + virtual void publishGoalState() const; + virtual void publishStateSpaceBoundaryMarker(); protected: using PlanningService = se2_navigation_msgs::RequestPathSrv; @@ -36,6 +40,13 @@ class PlannerRos : public Planner { ros::NodeHandlePtr nh_; std::shared_ptr planner_; + ros::ServiceServer planningService_; + + ros::Publisher pathNavMsgsPublisher_; + ros::Publisher pathPublisher_; + ros::Publisher startPublisher_; + ros::Publisher goalPublisher_; + ros::Publisher stateSpacePublisher_; }; } /* namespace se2_planning*/ diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index c7577bf..bb96cc2 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -87,7 +87,8 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl } // TODO move to detach? Better to publish this info for debugging before checking validity of states. - publishStartGoalMsgs(start, goal); + publishStartState(); + publishGoalState(); publishStateSpaceBoundaryMarker(); // Use state validator only after lock mutex is active and state space is updated @@ -114,23 +115,32 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl } void OmplReedsSheppPlannerRos::initRos() { - pathNavMsgsPublisher_ = nh_->advertise(parameters_.pathNavMsgTopic_, 1, true); planningService_ = nh_->advertiseService(parameters_.planningSerivceName_, &OmplReedsSheppPlannerRos::planningService, this); + pathNavMsgsPublisher_ = nh_->advertise(parameters_.pathNavMsgTopic_, 1, true); pathPublisher_ = nh_->advertise(parameters_.pathMsgTopic_, 1); startPublisher_ = nh_->advertise("start", 1); goalPublisher_ = nh_->advertise("goal", 1); stateSpacePublisher_ = nh_->advertise("state_space", 1); } -void OmplReedsSheppPlannerRos::publishPathNavMsgs() const { - ReedsSheppPath rsPath; - planner_->as()->getInterpolatedPath(&rsPath, parameters_.pathNavMsgResolution_); - nav_msgs::Path msg = se2_planning::copyAllPoints(rsPath); - msg.header.frame_id = parameters_.pathFrame_; - msg.header.stamp = planTimeStamp_; - msg.header.seq = planSeqNumber_; - pathNavMsgsPublisher_.publish(msg); - ROS_INFO_STREAM("Publishing ReedsShepp path nav msg, num states: " << msg.poses.size()); +void OmplReedsSheppPlannerRos::initializeStateSpaceBoundaryMarker() { + double lineWidth = 0.01; + std_msgs::ColorRGBA color; + // No transparency + color.a = 1; + // Black + color.r = 0; + color.g = 0; + color.b = 0; + // Init marker + int nVertices = 5; + stateSpaceBoundaryMarker_.ns = "state_space"; + stateSpaceBoundaryMarker_.lifetime = ros::Duration(); + stateSpaceBoundaryMarker_.action = visualization_msgs::Marker::ADD; + stateSpaceBoundaryMarker_.type = visualization_msgs::Marker::LINE_STRIP; + stateSpaceBoundaryMarker_.scale.x = lineWidth; + stateSpaceBoundaryMarker_.points.resize(nVertices); // Initialized to [0.0, 0.0, 0.0] + stateSpaceBoundaryMarker_.colors.resize(nVertices, color); } void OmplReedsSheppPlannerRos::publishPath() const { @@ -144,37 +154,35 @@ void OmplReedsSheppPlannerRos::publishPath() const { ROS_INFO_STREAM("Publishing ReedsShepp path, num states: " << rsPath.numPoints()); } -void OmplReedsSheppPlannerRos::publishStartGoalMsgs(const ReedsSheppState& start, const ReedsSheppState& goal) const { +void OmplReedsSheppPlannerRos::publishPathNavMsgs() const { + ReedsSheppPath rsPath; + planner_->as()->getInterpolatedPath(&rsPath, parameters_.pathNavMsgResolution_); + nav_msgs::Path msg = se2_planning::copyAllPoints(rsPath); + msg.header.frame_id = parameters_.pathFrame_; + msg.header.stamp = planTimeStamp_; + msg.header.seq = planSeqNumber_; + pathNavMsgsPublisher_.publish(msg); + ROS_INFO_STREAM("Publishing ReedsShepp path nav msg, num states: " << msg.poses.size()); +} + +void OmplReedsSheppPlannerRos::publishStartState() const { geometry_msgs::PoseStamped startPose; startPose.header.frame_id = parameters_.pathFrame_; startPose.header.stamp = planTimeStamp_; - startPose.pose = se2_planning::convert(start); + ReedsSheppState startState{}; + getStartingState(&startState); + startPose.pose = se2_planning::convert(startState); startPublisher_.publish(startPose); - geometry_msgs::PoseStamped goalPose; - goalPose.header.frame_id = parameters_.pathFrame_; - goalPose.header.stamp = planTimeStamp_; - goalPose.pose = se2_planning::convert(goal); - goalPublisher_.publish(goalPose); } -void OmplReedsSheppPlannerRos::initializeStateSpaceBoundaryMarker() { - double lineWidth = 0.01; - std_msgs::ColorRGBA color; - // No transparency - color.a = 1; - // Black - color.r = 0; - color.g = 0; - color.b = 0; - // Init marker - int nVertices = 5; - stateSpaceBoundaryMarker_.ns = "state_space"; - stateSpaceBoundaryMarker_.lifetime = ros::Duration(); - stateSpaceBoundaryMarker_.action = visualization_msgs::Marker::ADD; - stateSpaceBoundaryMarker_.type = visualization_msgs::Marker::LINE_STRIP; - stateSpaceBoundaryMarker_.scale.x = lineWidth; - stateSpaceBoundaryMarker_.points.resize(nVertices); // Initialized to [0.0, 0.0, 0.0] - stateSpaceBoundaryMarker_.colors.resize(nVertices, color); +void OmplReedsSheppPlannerRos::publishGoalState() const { + geometry_msgs::PoseStamped startPose; + startPose.header.frame_id = parameters_.pathFrame_; + startPose.header.stamp = planTimeStamp_; + ReedsSheppState goalState{}; + getStartingState(&goalState); + startPose.pose = se2_planning::convert(goalState); + startPublisher_.publish(startPose); } void OmplReedsSheppPlannerRos::publishStateSpaceBoundaryMarker() { diff --git a/se2_planning_ros/src/PlannerRos.cpp b/se2_planning_ros/src/PlannerRos.cpp index 411e05e..8b8004d 100644 --- a/se2_planning_ros/src/PlannerRos.cpp +++ b/se2_planning_ros/src/PlannerRos.cpp @@ -15,6 +15,22 @@ void PlannerRos::publishPath() const { throw std::runtime_error("Publish path not implemented"); } +void PlannerRos::publishPathNavMsgs() const { + throw std::runtime_error("Publish path nav msgs not implemented"); +} + +void PlannerRos::publishStartState() const { + throw std::runtime_error("Publish start state not implemented"); +} + +void PlannerRos::publishGoalState() const { + throw std::runtime_error("Publish goal state not implemented"); +} + +void PlannerRos::publishStateSpaceBoundaryMarker() { + throw std::runtime_error("Publish state space boundary marker not implemented"); +} + void PlannerRos::setPlanningStrategy(std::shared_ptr planner) { planner_ = planner; } @@ -22,24 +38,31 @@ void PlannerRos::setPlanningStrategy(std::shared_ptr planner) { void PlannerRos::setStartingState(const State& startingState) { planner_->setStartingState(startingState); } + void PlannerRos::setGoalState(const State& goalState) { planner_->setGoalState(goalState); } + bool PlannerRos::plan() { return planner_->plan(); } + void PlannerRos::getPath(Path* path) const { planner_->getPath(path); } + bool PlannerRos::reset() { return planner_->reset(); } + bool PlannerRos::initialize() { return planner_->initialize(); } + void PlannerRos::getStartingState(State* startingState) const { planner_->getStartingState(startingState); } + void PlannerRos::getGoalState(State* goalState) const { planner_->getGoalState(goalState); } From f24ce1366dcf780596b55a7e06134b61064c92ef Mon Sep 17 00:00:00 2001 From: meychr Date: Fri, 14 May 2021 21:24:18 +0200 Subject: [PATCH 08/24] Make isStateValid constant --- se2_planning/include/se2_planning/OmplPlanner.hpp | 2 +- se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp | 2 +- se2_planning/src/OmplReedsSheppPlanner.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index 5044c20..64544d3 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -39,7 +39,7 @@ class OmplPlanner : public Planner { protected: virtual void initializeStateSpace() = 0; - virtual bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) = 0; + virtual bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const = 0; virtual ompl::base::ScopedStatePtr convert(const State& state) const = 0; virtual void convert(const ompl::base::ScopedStatePtr omplState, State* state) const = 0; virtual void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const = 0; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index 5621a63..73e5ed2 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -67,7 +67,7 @@ class OmplReedsSheppPlanner final : public OmplPlanner { void createDefaultStateSpace(); void initializeStateSpace() final; void setStateSpaceBoundaries(); - bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) final; + bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const final; ompl::base::ScopedStatePtr convert(const State& state) const final; void convert(const ompl::base::ScopedStatePtr omplState, State* state) const final; void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const final; diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index 3cb47e2..a97d2f8 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -120,7 +120,7 @@ bool OmplReedsSheppPlanner::isLocked() const { return stateValidator_->isLocked(); } -bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) { +bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const { const ReedsSheppState rsState = se2_planning::convert(state); return stateValidator_->isStateValid(rsState); } From 11075fe30536618325fc410ab131c0ab31fdb4c9 Mon Sep 17 00:00:00 2001 From: meychr Date: Fri, 14 May 2021 22:12:14 +0200 Subject: [PATCH 09/24] Fix typo --- .../include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp | 2 +- se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp | 2 +- se2_planning_ros/src/loaders.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp index 2fec50c..1575c02 100644 --- a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp @@ -21,7 +21,7 @@ namespace se2_planning { struct OmplReedsSheppPlannerRosParameters { std::string pathFrame_ = "map"; std::string pathNavMsgTopic_ = "ompl_rs_planner_ros/nav_msgs_path"; - std::string planningSerivceName_ = "ompl_rs_planner_ros/planning_service"; + 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; diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index bb96cc2..df8167c 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -115,7 +115,7 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl } void OmplReedsSheppPlannerRos::initRos() { - planningService_ = nh_->advertiseService(parameters_.planningSerivceName_, &OmplReedsSheppPlannerRos::planningService, this); + planningService_ = nh_->advertiseService(parameters_.planningServiceName_, &OmplReedsSheppPlannerRos::planningService, this); pathNavMsgsPublisher_ = nh_->advertise(parameters_.pathNavMsgTopic_, 1, true); pathPublisher_ = nh_->advertise(parameters_.pathMsgTopic_, 1); startPublisher_ = nh_->advertise("start", 1); diff --git a/se2_planning_ros/src/loaders.cpp b/se2_planning_ros/src/loaders.cpp index 8425065..0ea61a3 100644 --- a/se2_planning_ros/src/loaders.cpp +++ b/se2_planning_ros/src/loaders.cpp @@ -50,7 +50,7 @@ OmplReedsSheppPlannerRosParameters loadOmplReedsSheppPlannerRosParameters(const auto node = basenode["planner_ros"]; parameters.pathNavMsgTopic_ = node["nav_msgs_path_topic"].as(); parameters.pathFrame_ = node["path_frame"].as(); - parameters.planningSerivceName_ = node["planning_service_name"].as(); + parameters.planningServiceName_ = node["planning_service_name"].as(); parameters.pathNavMsgResolution_ = node["nav_msg_path_spatial_resolution"].as(); parameters.pathMsgTopic_ = node["path_msg_topic"].as(); parameters.stateSpaceBoundsMargin_ = node["state_space_bounds_margin"].as(); From 8c0bc04c133f48fb54ed0649a771297a21539357 Mon Sep 17 00:00:00 2001 From: meychr Date: Fri, 14 May 2021 22:15:50 +0200 Subject: [PATCH 10/24] Move StateValidator functions to Planner interface --- .../include/se2_planning/OmplPlanner.hpp | 7 ++++++ .../se2_planning/OmplReedsSheppPlanner.hpp | 6 ----- se2_planning/include/se2_planning/Planner.hpp | 7 ++++++ se2_planning/src/OmplPlanner.cpp | 22 ++++++++++++++++++- se2_planning/src/OmplReedsSheppPlanner.cpp | 20 ----------------- .../include/se2_planning_ros/PlannerRos.hpp | 6 +++++ .../src/OmplReedsSheppPlannerRos.cpp | 15 +++++-------- se2_planning_ros/src/PlannerRos.cpp | 20 +++++++++++++++++ 8 files changed, 67 insertions(+), 36 deletions(-) diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index 64544d3..04fb422 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -27,6 +27,12 @@ class OmplPlanner : public Planner { void getStartingState(State* startingState) const final; void getGoalState(State* goalState) const final; + void setStateValidator(std::unique_ptr stateValidator) final; + const StateValidator& getStateValidator() const final; + void lockStateValidator() final; + void unlockStateValidator() final; + bool isLocked() const final; + void setMaxPlanningDuration(double T); void getOmplPath(ompl::geometric::PathGeometric* omplPath) const; void getOmplInterpolatedPath(ompl::geometric::PathGeometric* omplPath, double spatialResolution) const; @@ -48,6 +54,7 @@ class OmplPlanner : public Planner { ompl::geometric::SimpleSetupPtr simpleSetup_; ompl::base::ScopedStatePtr startState_, goalState_; std::unique_ptr path_, interpolatedPath_; + std::unique_ptr stateValidator_; private: double maxPlanningDuration_ = 1.0; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index 73e5ed2..eba55f6 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -54,11 +54,6 @@ class OmplReedsSheppPlanner final : public OmplPlanner { bool initialize() final; bool plan() final; void setParameters(const OmplReedsSheppPlannerParameters& parameters); - void setStateValidator(std::unique_ptr stateValidator); - const StateValidator& getStateValidator() const; - void lockStateValidator(); - void unlockStateValidator(); - bool isLocked() const; void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override; bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const; const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const; @@ -76,7 +71,6 @@ class OmplReedsSheppPlanner final : public OmplPlanner { std::unique_ptr bounds_; const int reedsSheppStateSpaceDim_ = 2; OmplReedsSheppPlannerParameters parameters_; - std::unique_ptr stateValidator_; }; std::string toString(ReedsSheppPathSegment::Direction direction); diff --git a/se2_planning/include/se2_planning/Planner.hpp b/se2_planning/include/se2_planning/Planner.hpp index bba9af7..8e4d287 100644 --- a/se2_planning/include/se2_planning/Planner.hpp +++ b/se2_planning/include/se2_planning/Planner.hpp @@ -8,6 +8,7 @@ #pragma once #include "se2_planning/State.hpp" +#include "se2_planning/StateValidator.hpp" #include namespace se2_planning { @@ -27,6 +28,12 @@ class Planner { virtual void getStartingState(State* startingState) const { throw std::runtime_error("Planner: getStartingState() not implemented"); } virtual void getGoalState(State* goalState) const { throw std::runtime_error("Planner: getGoalState() not implemented"); } + virtual void setStateValidator(std::unique_ptr stateValidator) = 0; + virtual const StateValidator& getStateValidator() const { throw std::runtime_error("Planner: getStateValidator() not implemented"); }; + virtual void lockStateValidator() = 0; + virtual void unlockStateValidator() = 0; + virtual bool isLocked() const = 0; + template const T* as() const { BOOST_CONCEPT_ASSERT((boost::Convertible)); diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index 9916589..9e8c3dc 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -40,10 +40,30 @@ void OmplPlanner::getGoalState(State* goalState) const { convert(goalState_, goalState); } +void OmplPlanner::setStateValidator(std::unique_ptr stateValidator) { + stateValidator_ = std::move(stateValidator); +} + +const StateValidator& OmplPlanner::getStateValidator() const { + return *stateValidator_; +} + +void OmplPlanner::lockStateValidator() { + stateValidator_->lock(); +} + +void OmplPlanner::unlockStateValidator() { + stateValidator_->unlock(); +} + +bool OmplPlanner::isLocked() const { + return stateValidator_->isLocked(); +} + bool OmplPlanner::plan() { simpleSetup_->clear(); simpleSetup_->setStartAndGoalStates(*startState_, *goalState_); - // TODO see https://ompl.kavrakilab.org/genericPlanning.html for continue planning + // TODO see https://ompl.kavrakilab.org/genericPlanning.html on how to continue planning // The ompl::base::Planner::solve() method can be called repeatedly with different // allowed time durations until a solution is found. The planning process continues // with the available data structures when sequential calls to ompl::base::Planner::solve() are made. diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index a97d2f8..44652e8 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -100,26 +100,6 @@ bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::Re return stateSpace_->satisfiesBounds(s); } -void OmplReedsSheppPlanner::setStateValidator(std::unique_ptr stateValidator) { - stateValidator_ = std::move(stateValidator); -} - -const StateValidator& OmplReedsSheppPlanner::getStateValidator() const { - return *stateValidator_; -} - -void OmplReedsSheppPlanner::lockStateValidator() { - stateValidator_->lock(); -} - -void OmplReedsSheppPlanner::unlockStateValidator() { - stateValidator_->unlock(); -} - -bool OmplReedsSheppPlanner::isLocked() const { - return stateValidator_->isLocked(); -} - bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const { const ReedsSheppState rsState = se2_planning::convert(state); return stateValidator_->isStateValid(rsState); diff --git a/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp index c4288fd..a86d1a4 100644 --- a/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp @@ -24,6 +24,12 @@ class PlannerRos : public Planner { void getStartingState(State* startingState) const override; void getGoalState(State* goalState) const override; + void setStateValidator(std::unique_ptr stateValidator) override; + const StateValidator& getStateValidator() const override; + void lockStateValidator() override; + void unlockStateValidator() override; + bool isLocked() const override; + void setPlanningStrategy(std::shared_ptr planner); virtual void publishPath() const; virtual void publishPathNavMsgs() const; diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index df8167c..61e4642 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -41,7 +41,7 @@ bool OmplReedsSheppPlannerRos::plan() { bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, PlanningService::Response& res) { // TODO Grid map in state validator is currently initialized to some default values, this check is therefore useless - if (!planner_->as()->getStateValidator().isInitialized()) { + if (!planner_->getStateValidator().isInitialized()) { ROS_WARN_STREAM("State validator has not been initialized yet. Abort planning."); res.status = false; return true; @@ -55,10 +55,10 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl setGoalState(goal); // Block update of state validator obstacle map during planning - planner_->as()->lockStateValidator(); + planner_->lockStateValidator(); // TODO move from se2_planner_node here (introduces dependency but makes state space update more consistent: // adapt state space boundaries from grid map - // planner_->as()->getStateValidator().as + // planner_->getStateValidator().as // Adapt state space boundaries (larger than grid map, state validity checking assumes area out of bounds to be // traversable) to contain initial and goal state otherwise RRTstar fails @@ -93,19 +93,16 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl // Use state validator only after lock mutex is active and state space is updated // Checks only for non-traversable terrain not for state space bounds - if (!planner_->as()->getStateValidator().isStateValid(start)) { + if (!planner_->getStateValidator().isStateValid(start)) { ROS_WARN_STREAM("Start state (x: " << start.x_ << ", y: " << start.y_ << ", yaw: " << start.yaw_ << ") not valid. Start planning anyway."); } - if (!planner_->as()->getStateValidator().isStateValid(goal)) { + if (!planner_->getStateValidator().isStateValid(goal)) { ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Start planning anyway."); - // ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Abort planning."); - // planner_->as()->unlockStateValidator(); - // return false; } bool result = plan(); - planner_->as()->unlockStateValidator(); + planner_->unlockStateValidator(); res.status = result; diff --git a/se2_planning_ros/src/PlannerRos.cpp b/se2_planning_ros/src/PlannerRos.cpp index 8b8004d..4157a6e 100644 --- a/se2_planning_ros/src/PlannerRos.cpp +++ b/se2_planning_ros/src/PlannerRos.cpp @@ -67,4 +67,24 @@ void PlannerRos::getGoalState(State* goalState) const { planner_->getGoalState(goalState); } +void PlannerRos::setStateValidator(std::unique_ptr stateValidator) { + planner_->setStateValidator(std::move(stateValidator)); +} + +const StateValidator& PlannerRos::getStateValidator() const { + return planner_->getStateValidator(); +} + +void PlannerRos::lockStateValidator() { + planner_->lockStateValidator(); +} + +void PlannerRos::unlockStateValidator() { + planner_->unlockStateValidator(); +} + +bool PlannerRos::isLocked() const { + return planner_->isLocked(); +} + } // namespace se2_planning From 007285952a3d5745ce01cced10cffdc8e6569acd Mon Sep 17 00:00:00 2001 From: meychr Date: Wed, 19 May 2021 20:21:40 +0200 Subject: [PATCH 11/24] Adapt se2_grid_map_generator config and add TODOs to fix frames --- se2_grid_map_generator/config/default.yaml | 2 +- se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp | 2 ++ se2_planning_ros/src/se2_planner_node.cpp | 2 ++ se2_planning_rviz/src/PlanningPanel.cpp | 2 ++ 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/se2_grid_map_generator/config/default.yaml b/se2_grid_map_generator/config/default.yaml index 3f04a1b..6e61418 100644 --- a/se2_grid_map_generator/config/default.yaml +++ b/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 diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index 61e4642..445062a 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/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_; diff --git a/se2_planning_ros/src/se2_planner_node.cpp b/se2_planning_ros/src/se2_planner_node.cpp index 0e8a79c..0408f0b 100644 --- a/se2_planning_ros/src/se2_planner_node.cpp +++ b/se2_planning_ros/src/se2_planner_node.cpp @@ -19,6 +19,8 @@ using namespace se2_planning; auto planner = std::make_shared(); 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); diff --git a/se2_planning_rviz/src/PlanningPanel.cpp b/se2_planning_rviz/src/PlanningPanel.cpp index 2b88c21..988e85c 100644 --- a/se2_planning_rviz/src/PlanningPanel.cpp +++ b/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 From db4a618de5f69d9b9b0bee5375f1217ca36002a6 Mon Sep 17 00:00:00 2001 From: meychr Date: Wed, 19 May 2021 20:37:29 +0200 Subject: [PATCH 12/24] Set state space bounds changes --- .../include/se2_planning/OmplPlanner.hpp | 5 +- .../se2_planning/OmplReedsSheppPlanner.hpp | 10 ++-- se2_planning/src/OmplPlanner.cpp | 10 ++-- se2_planning/src/OmplReedsSheppPlanner.cpp | 50 ++++++------------- .../src/OmplReedsSheppPlannerRos.cpp | 7 ++- se2_planning_ros/src/se2_planner_node.cpp | 20 +++----- 6 files changed, 43 insertions(+), 59 deletions(-) diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index 04fb422..072bec8 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -33,6 +33,8 @@ class OmplPlanner : public Planner { void unlockStateValidator() final; bool isLocked() const final; + virtual void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds); + void setMaxPlanningDuration(double T); void getOmplPath(ompl::geometric::PathGeometric* omplPath) const; void getOmplInterpolatedPath(ompl::geometric::PathGeometric* omplPath, double spatialResolution) const; @@ -41,16 +43,15 @@ class OmplPlanner : public Planner { void getInterpolatedPath(Path* interpolatedPath, unsigned int numPoints) const; ompl::geometric::SimpleSetupPtr getSimpleSetup() const; void setOmplPlanner(ompl::base::PlannerPtr planner); - virtual void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds); protected: - virtual void initializeStateSpace() = 0; virtual bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const = 0; virtual ompl::base::ScopedStatePtr convert(const State& state) const = 0; virtual void convert(const ompl::base::ScopedStatePtr omplState, State* state) const = 0; virtual void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const = 0; ompl::base::StateSpacePtr stateSpace_; + std::unique_ptr bounds_; ompl::geometric::SimpleSetupPtr simpleSetup_; ompl::base::ScopedStatePtr startState_, goalState_; std::unique_ptr path_, interpolatedPath_; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index eba55f6..e8981c6 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -35,10 +35,14 @@ struct ReedsSheppPath : public Path { struct OmplReedsSheppPlannerParameters { double turningRadius_ = 10.0; + + // TODO(christoph): Do we need these parameters? Should be provided by the map. If no map available, no planning + // possible...? double xLowerBound_ = -1000.0; double xUpperBound_ = 1000.0; double yLowerBound_ = -1000.0; double yUpperBound_ = 1000.0; + double pathSpatialResolution_ = 0.05; double maxPlanningTime_ = 1.0; std::string omplPlannerName_ = "RRTstar"; @@ -54,21 +58,19 @@ class OmplReedsSheppPlanner final : public OmplPlanner { bool initialize() final; bool plan() final; void setParameters(const OmplReedsSheppPlannerParameters& parameters); - void updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override; + void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override; bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const; const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const; private: void createDefaultStateSpace(); - void initializeStateSpace() final; - void setStateSpaceBoundaries(); bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const final; ompl::base::ScopedStatePtr convert(const State& state) const final; void convert(const ompl::base::ScopedStatePtr omplState, State* state) const final; void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const final; int getDistanceSignAt(const ompl::geometric::PathGeometric& path, unsigned int id) const; - std::unique_ptr bounds_; + // TODO(christoph): Can we move bounds_ to OmplPlanner? Can we remove it altogether? const int reedsSheppStateSpaceDim_ = 2; OmplReedsSheppPlannerParameters parameters_; }; diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index 9e8c3dc..1ea99f5 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -86,7 +86,6 @@ bool OmplPlanner::reset() { } bool OmplPlanner::initialize() { - initializeStateSpace(); if (stateSpace_ == nullptr) { std::cerr << "OmplPlanner:: state space is nullptr" << std::endl; return false; @@ -100,8 +99,13 @@ bool OmplPlanner::initialize() { return true; } -void OmplPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) { - throw std::runtime_error("Not implemented"); +void OmplPlanner::setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) { + for (size_t idx = 0; idx < bounds.low.size(); idx++) { + bounds_->low[idx] = bounds.low[idx]; + } + for (size_t idx = 0; idx < bounds.high.size(); idx++) { + bounds_->high[idx] = bounds.high[idx]; + } } void OmplPlanner::setMaxPlanningDuration(double T) { diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index 44652e8..7c25ffe 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -42,52 +42,33 @@ bool OmplReedsSheppPlanner::initialize() { return true; } -void OmplReedsSheppPlanner::initializeStateSpace() { - // todo maybe not really needed - createDefaultStateSpace(); -} - void OmplReedsSheppPlanner::createDefaultStateSpace() { + // Allocate abstracted objects stateSpace_.reset(new ompl::base::ReedsSheppStateSpace(parameters_.turningRadius_)); bounds_ = std::make_unique(reedsSheppStateSpaceDim_); - setStateSpaceBoundaries(); + // Initialize bounds with parameters + ompl::base::RealVectorBounds bounds(reedsSheppStateSpaceDim_); + bounds.low[0] = parameters_.xLowerBound_; + bounds.low[1] = parameters_.yLowerBound_; + bounds.high[0] = parameters_.xUpperBound_; + bounds.high[1] = parameters_.yUpperBound_; + setStateSpaceBoundaries(bounds); } bool OmplReedsSheppPlanner::plan() { - initialize(); - ompl::base::StateSpacePtr space = simpleSetup_->getStateSpace(); - auto bounds = space->as()->getBounds(); - // std::cout << "OmplReedsSheppPlanner: Planner state space bounds: x = [" << bounds.low[0] << ", " << bounds.high[0] << "], y=[" - // << bounds.low[1] << ", " << bounds.high[1] << "]" << std::endl; - - bool result = BASE::plan(); + // Call to initialize is required s.t. OMPL planner is updated with latest state space boundaries + // initialize(); + const bool result = BASE::plan(); *interpolatedPath_ = interpolatePath(*path_, parameters_.pathSpatialResolution_); return result; } -void OmplReedsSheppPlanner::setStateSpaceBoundaries() { - bounds_->low[0] = parameters_.xLowerBound_; - bounds_->low[1] = parameters_.yLowerBound_; - bounds_->high[0] = parameters_.xUpperBound_; - bounds_->high[1] = parameters_.yUpperBound_; - stateSpace_->as()->setBounds(*bounds_); - // std::cout << "OmplReedsSheppPlanner: Set state space bounds: x = [" << bounds_->low[0] << ", " << bounds_->high[0] << "], y=[" - // << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl; -} + const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundaries() const { return stateSpace_->as()->getBounds(); } -void OmplReedsSheppPlanner::updateStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) { - bounds_->low[0] = bounds.low[0]; - bounds_->low[1] = bounds.low[1]; - bounds_->high[0] = bounds.high[0]; - bounds_->high[1] = bounds.high[1]; - parameters_.xLowerBound_ = bounds_->low[0]; - parameters_.yLowerBound_ = bounds_->low[1]; - parameters_.xUpperBound_ = bounds_->high[0]; - parameters_.yUpperBound_ = bounds_->high[1]; - stateSpace_->as()->setBounds(*bounds_); - // std::cout << "OmplReedsSheppPlanner: Update state space bounds: x = [" << bounds_->low[0] << ", " << bounds_->high[0] << "], y=[" - // << bounds_->low[1] << ", " << bounds_->high[1] << "]" << std::endl; +void OmplReedsSheppPlanner::setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) { + BASE::setStateSpaceBoundaries(bounds); + stateSpace_->as()->setBounds(bounds); } bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const { @@ -101,6 +82,7 @@ bool OmplReedsSheppPlanner::satisfiesStateSpaceBoundaries(const se2_planning::Re } bool OmplReedsSheppPlanner::isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const { + (void)si; const ReedsSheppState rsState = se2_planning::convert(state); return stateValidator_->isStateValid(rsState); } diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index 445062a..65691d3 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -57,13 +57,13 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl // Block update of state validator obstacle map during planning planner_->lockStateValidator(); + // TODO move from se2_planner_node here (introduces dependency but makes state space update more consistent: // adapt state space boundaries from grid map // planner_->getStateValidator().as // Adapt state space boundaries (larger than grid map, state validity checking assumes area out of bounds to be // traversable) to contain initial and goal state otherwise RRTstar fails - // TODO expose stateSpaceBoundsMargin_ as param => depends on footprint size? // TODO move to OMPLReedsSheppPlanner.cpp? const double stateSpaceBoundsMargin_ = parameters_.stateSpaceBoundsMargin_; if (!planner_->as()->satisfiesStateSpaceBoundaries(start)) { @@ -74,7 +74,7 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl newBounds.high[1] = std::max(bounds.high[1], start.y_ + stateSpaceBoundsMargin_); newBounds.low[0] = std::min(bounds.low[0], start.x_ - stateSpaceBoundsMargin_); newBounds.low[1] = std::min(bounds.low[1], start.y_ - stateSpaceBoundsMargin_); - planner_->as()->updateStateSpaceBoundaries(newBounds); + planner_->as()->setStateSpaceBoundaries(newBounds); } if (!planner_->as()->satisfiesStateSpaceBoundaries(goal)) { ROS_DEBUG("Goal state not in grid map. Enlarge state space boundaries."); @@ -84,10 +84,9 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl newBounds.high[1] = std::max(bounds.high[1], goal.y_ + stateSpaceBoundsMargin_); newBounds.low[0] = std::min(bounds.low[0], goal.x_ - stateSpaceBoundsMargin_); newBounds.low[1] = std::min(bounds.low[1], goal.y_ - stateSpaceBoundsMargin_); - planner_->as()->updateStateSpaceBoundaries(newBounds); + planner_->as()->setStateSpaceBoundaries(newBounds); } - // TODO move to detach? Better to publish this info for debugging before checking validity of states. publishStartState(); publishGoalState(); publishStateSpaceBoundaryMarker(); diff --git a/se2_planning_ros/src/se2_planner_node.cpp b/se2_planning_ros/src/se2_planner_node.cpp index 0408f0b..9db3422 100644 --- a/se2_planning_ros/src/se2_planner_node.cpp +++ b/se2_planning_ros/src/se2_planner_node.cpp @@ -24,24 +24,20 @@ void gridMapCallback(const grid_map_msgs::GridMap& msg) { // Update planner bounds when new map is published, placed here due to class structure grid_map::GridMap map; grid_map::GridMapRosConverter::fromMessage(msg, map); + // Grid map is symmetric around position - grid_map::Position mapPosition; - mapPosition = map.getPosition(); - grid_map::Length mapLength; - mapLength = map.getLength(); + const grid_map::Position mapPosition = map.getPosition(); + const grid_map::Length mapLength = map.getLength(); ompl::base::RealVectorBounds bounds(2); bounds.low[0] = mapPosition.x() - mapLength.x() / 2.0; bounds.low[1] = mapPosition.y() - mapLength.y() / 2.0; bounds.high[0] = mapPosition.x() + mapLength.x() / 2.0; bounds.high[1] = mapPosition.y() + mapLength.y() / 2.0; - - if (!planner->isLocked()) { - planner->updateStateSpaceBoundaries(bounds); - ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", " - << mapLength.y()); - } else { - ROS_DEBUG_STREAM("OMPL State Space Update: Planner is locked. Not updating state space bounds."); - } + planner->lockStateValidator(); + planner->setStateSpaceBoundaries(bounds); + planner->unlockStateValidator(); + ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", " + << mapLength.y()); } int main(int argc, char** argv) { From 27359400ec8b2de8bcbb6ce132a2b9c7cc71a16d Mon Sep 17 00:00:00 2001 From: meychr Date: Wed, 19 May 2021 20:39:30 +0200 Subject: [PATCH 13/24] Fix formatting --- se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index 65691d3..4c9be40 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -57,7 +57,7 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl // Block update of state validator obstacle map during planning planner_->lockStateValidator(); - + // TODO move from se2_planner_node here (introduces dependency but makes state space update more consistent: // adapt state space boundaries from grid map // planner_->getStateValidator().as From 6b3863b007d3a0c171128158402fe14c14bda38a Mon Sep 17 00:00:00 2001 From: meychr Date: Wed, 19 May 2021 20:54:57 +0200 Subject: [PATCH 14/24] Move getStateSpaceBoundaries to OmplPlanner --- se2_planning/include/se2_planning/OmplPlanner.hpp | 1 + se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp | 2 +- se2_planning/src/OmplPlanner.cpp | 4 ++++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index 072bec8..e50bccb 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -34,6 +34,7 @@ class OmplPlanner : public Planner { bool isLocked() const final; virtual void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds); + virtual const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const; void setMaxPlanningDuration(double T); void getOmplPath(ompl::geometric::PathGeometric* omplPath) const; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index e8981c6..c66f90c 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -59,8 +59,8 @@ class OmplReedsSheppPlanner final : public OmplPlanner { bool plan() final; void setParameters(const OmplReedsSheppPlannerParameters& parameters); void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override; + const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const override; bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const; - const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const; private: void createDefaultStateSpace(); diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index 1ea99f5..eea3f1e 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -108,6 +108,10 @@ void OmplPlanner::setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bo } } +const ompl::base::RealVectorBounds& OmplPlanner::getStateSpaceBoundaries() const { + return *bounds_; +} + void OmplPlanner::setMaxPlanningDuration(double T) { maxPlanningDuration_ = T; } From 4704095182896dedf2986d13d1df587fdad90da5 Mon Sep 17 00:00:00 2001 From: meychr Date: Wed, 19 May 2021 22:00:18 +0200 Subject: [PATCH 15/24] Remove comment --- se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index c66f90c..462ab53 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -70,7 +70,6 @@ class OmplReedsSheppPlanner final : public OmplPlanner { void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const final; int getDistanceSignAt(const ompl::geometric::PathGeometric& path, unsigned int id) const; - // TODO(christoph): Can we move bounds_ to OmplPlanner? Can we remove it altogether? const int reedsSheppStateSpaceDim_ = 2; OmplReedsSheppPlannerParameters parameters_; }; From 808bd6533ce6d3f7f8e9f2d69d328d1903928591 Mon Sep 17 00:00:00 2001 From: meychr Date: Sat, 29 May 2021 21:58:03 +0200 Subject: [PATCH 16/24] Reorder code and remove TODO --- .../se2_planning/GridMapStateValidator.hpp | 4 ++-- se2_planning/src/GridMapLazyStateValidator.cpp | 1 + se2_planning/src/GridMapStateValidator.cpp | 18 +++++------------- 3 files changed, 8 insertions(+), 15 deletions(-) diff --git a/se2_planning/include/se2_planning/GridMapStateValidator.hpp b/se2_planning/include/se2_planning/GridMapStateValidator.hpp index 9929cad..ceb9491 100644 --- a/se2_planning/include/se2_planning/GridMapStateValidator.hpp +++ b/se2_planning/include/se2_planning/GridMapStateValidator.hpp @@ -47,11 +47,11 @@ class GridMapStateValidator : public StateValidator { bool isFootprintInitialized_ = false; bool isLayerNameInitialized_ = false; - std::string obstacleLayerName_ = ""; grid_map::GridMap gridMap_; RobotFootprint nominalFootprint_; + std::string obstacleLayerName_ = ""; - private: +private: mutable RobotFootprint currentFootprint_; }; diff --git a/se2_planning/src/GridMapLazyStateValidator.cpp b/se2_planning/src/GridMapLazyStateValidator.cpp index 5f1fb80..5b244a3 100644 --- a/se2_planning/src/GridMapLazyStateValidator.cpp +++ b/se2_planning/src/GridMapLazyStateValidator.cpp @@ -91,6 +91,7 @@ bool GridMapLazyStateValidator::isStateValid(const State& state) const { return true; } /* Optimistic and assumes no obstacles */ + // TODO(christoph): Should we create separate classes for each validator method? => can have different constructors const auto se2state = *(state.as()); switch (stateValidityCheckingMethod_) { case StateValidityCheckingMethod::COLLISION: diff --git a/se2_planning/src/GridMapStateValidator.cpp b/se2_planning/src/GridMapStateValidator.cpp index ce87a18..a6cd3f8 100644 --- a/se2_planning/src/GridMapStateValidator.cpp +++ b/se2_planning/src/GridMapStateValidator.cpp @@ -25,25 +25,17 @@ bool GridMapStateValidator::isStateValid(const State& state) const { return !isInCollision(polygon, gridMap_, obstacleLayerName_); } +bool GridMapStateValidator::isInitialized() const { + return isGridMapInitialized_ && isFootprintInitialized_ && isLayerNameInitialized_; +} + void GridMapStateValidator::setGridMap(const grid_map::GridMap& gridMap) { if (gridMap.getLayers().empty()) { throw std::runtime_error("Grid map has no layers"); } gridMap_ = gridMap; - // TODO does not work correctly, boundaries of traversability layer get set to nan!? Problem because elevation_mapping_cupy - // has strange resolution?! - // Convert resolution, optionally only copy relevant layer - // gridMap_.setFrameId(gridMap.getFrameId()); - // gridMap_.setTimestamp(gridMap.getTimestamp()); - // gridMap_.setGeometry(gridMap.getLength(), gridMap.getResolution(), gridMap.getPosition()); - // if (!gridMap_.addDataFrom(gridMap, true, true, true)) { - // throw std::runtime_error("Grid map add data failed."); - // } isGridMapInitialized_ = true; } -bool GridMapStateValidator::isInitialized() const { - return isGridMapInitialized_ && isFootprintInitialized_ && isLayerNameInitialized_; -} void GridMapStateValidator::setFootprint(const RobotFootprint& footprint) { nominalFootprint_ = footprint; @@ -68,7 +60,7 @@ const std::string& GridMapStateValidator::getObstacleLayerName() const { bool isInCollision(const grid_map::Polygon& polygon, const grid_map::GridMap& gridMap, const std::string& obstacleLayer) { /* - * anything different thatn 0.0 means that + * anything different than 0.0 means that * the space is not free, this is just so that numerics are a bit better */ const double collisionThreshold = 0.1; From 21e338521e8e411ed9bdc910e98111b671a82179 Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 21:40:31 +0200 Subject: [PATCH 17/24] Fix format --- se2_planning/include/se2_planning/GridMapStateValidator.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/se2_planning/include/se2_planning/GridMapStateValidator.hpp b/se2_planning/include/se2_planning/GridMapStateValidator.hpp index ceb9491..7250e75 100644 --- a/se2_planning/include/se2_planning/GridMapStateValidator.hpp +++ b/se2_planning/include/se2_planning/GridMapStateValidator.hpp @@ -51,7 +51,7 @@ class GridMapStateValidator : public StateValidator { RobotFootprint nominalFootprint_; std::string obstacleLayerName_ = ""; -private: + private: mutable RobotFootprint currentFootprint_; }; From 5c1afb2f8dea6532679662f597e303021031e19d Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 21:42:15 +0200 Subject: [PATCH 18/24] Implement map interface and occupancy map & adapt height map --- se2_planning/CMakeLists.txt | 11 +++-- se2_planning/include/se2_planning/Bounds.hpp | 22 ++++++++++ .../include/se2_planning/HeightMap.hpp | 15 ++++--- se2_planning/include/se2_planning/Map.hpp | 33 +++++++++++++++ .../include/se2_planning/OccupancyMap.hpp | 32 +++++++++++++++ se2_planning/src/Bounds.cpp | 16 ++++++++ se2_planning/src/HeightMap.cpp | 21 +++++----- se2_planning/src/Map.cpp | 38 ++++++++++++++++++ se2_planning/src/OccupancyMap.cpp | 40 +++++++++++++++++++ 9 files changed, 204 insertions(+), 24 deletions(-) create mode 100644 se2_planning/include/se2_planning/Bounds.hpp create mode 100644 se2_planning/include/se2_planning/Map.hpp create mode 100644 se2_planning/include/se2_planning/OccupancyMap.hpp create mode 100644 se2_planning/src/Bounds.cpp create mode 100644 se2_planning/src/Map.cpp create mode 100644 se2_planning/src/OccupancyMap.cpp diff --git a/se2_planning/CMakeLists.txt b/se2_planning/CMakeLists.txt index 44d13b5..13ca62f 100644 --- a/se2_planning/CMakeLists.txt +++ b/se2_planning/CMakeLists.txt @@ -6,14 +6,17 @@ add_compile_options(-Wall -Wextra -Wpedantic) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(SRC_FILES - src/ompl_planner_creators.cpp + src/Bounds.cpp src/GridMapLazyStateValidator.cpp src/GridMapStateValidator.cpp + src/HeightMap.cpp + src/Map.cpp + src/OccupancyMap.cpp src/OmplPlanner.cpp src/OmplReedsSheppPlanner.cpp src/State.cpp src/StateValidator.cpp - src/HeightMap.cpp + src/ompl_planner_creators.cpp ) set(CATKIN_PACKAGE_DEPENDENCIES @@ -32,7 +35,7 @@ if(NOT EIGEN3_FOUND) set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) endif() -find_package(Boost REQUIRED) +find_package(Boost REQUIRED COMPONENTS thread) find_package(ompl REQUIRED) catkin_package( @@ -88,8 +91,8 @@ add_executable(planning_benchmark src/planning_benchmark.cpp ) target_link_libraries(planning_benchmark - ${catkin_LIBRARIES} ${PROJECT_NAME} + ${catkin_LIBRARIES} ${OMPL_LIBRARIES} test_helpers ) diff --git a/se2_planning/include/se2_planning/Bounds.hpp b/se2_planning/include/se2_planning/Bounds.hpp new file mode 100644 index 0000000..7ce8a32 --- /dev/null +++ b/se2_planning/include/se2_planning/Bounds.hpp @@ -0,0 +1,22 @@ +/* + * Bounds.hpp + * + * Created on: May 29, 2021 + * Author: meyerc + */ + +#pragma once +#include +#include + +namespace se2_planning { + +struct Bounds { + Bounds() = default; + Bounds(XYstate low, XYstate high); + ~Bounds() = default; + XYstate low_ = {0.0, 0.0}; + XYstate high_ = {0.0, 0.0}; +}; + +} // namespace se2_planning diff --git a/se2_planning/include/se2_planning/HeightMap.hpp b/se2_planning/include/se2_planning/HeightMap.hpp index 169def7..50e00c0 100644 --- a/se2_planning/include/se2_planning/HeightMap.hpp +++ b/se2_planning/include/se2_planning/HeightMap.hpp @@ -7,22 +7,21 @@ #pragma once #include "grid_map_core/GridMap.hpp" +#include "se2_planning/Map.hpp" namespace se2_planning { -class HeightMap { +class HeightMap : public Map { public: HeightMap() = default; - virtual ~HeightMap() = default; - - bool isInitialized() const; - - double getHeightAt(double x, double y) const; + ~HeightMap() override = default; + bool isInitialized() const override; + void initializeBounds(const Bounds& bounds) override; + Bounds getBounds() const override; + double getValueAt(double x, double y) const override; void setGridMap(const grid_map::GridMap& gm, const std::string& heightLayer); - const grid_map::GridMap& getGridMap() const; - private: grid_map::GridMap impl_; std::string heightLayer_ = ""; diff --git a/se2_planning/include/se2_planning/Map.hpp b/se2_planning/include/se2_planning/Map.hpp new file mode 100644 index 0000000..b52127a --- /dev/null +++ b/se2_planning/include/se2_planning/Map.hpp @@ -0,0 +1,33 @@ +/* + * Map.hpp + * + * Created on: May 16, 2021 + * Author: meyerc + */ + +#pragma once + +#include "se2_planning/Bounds.hpp" +#include "se2_planning/State.hpp" + +#include + +namespace se2_planning { + +class Map { + public: + Map() = default; + virtual ~Map() = default; + + virtual bool isInitialized() const = 0; + virtual void initializeBounds(const Bounds& bounds) = 0; + virtual Bounds getBounds() const = 0; + virtual double getValueAt(double x, double y) const = 0; + virtual void lock(); + virtual void unlock(); + + private: + boost::mutex mtx_; +}; + +} /* namespace se2_planning */ diff --git a/se2_planning/include/se2_planning/OccupancyMap.hpp b/se2_planning/include/se2_planning/OccupancyMap.hpp new file mode 100644 index 0000000..9c44bce --- /dev/null +++ b/se2_planning/include/se2_planning/OccupancyMap.hpp @@ -0,0 +1,32 @@ +/* + * OccupancyMap.hpp + * + * Created on: May 16, 2021 + * Author: meyerc + */ + +#pragma once + +#include "grid_map_core/GridMap.hpp" +#include "se2_planning/Map.hpp" + +namespace se2_planning { + +class OccupancyMap : public Map { + public: + OccupancyMap() = default; + ~OccupancyMap() override = default; + + bool isInitialized() const override; + void initializeBounds(const Bounds& bounds) override; + Bounds getBounds() const override; + double getValueAt(double x, double y) const override; + void setGridMap(const grid_map::GridMap& gm, const std::string& occupancyLayer); + + private: + grid_map::GridMap impl_; + std::string occupancyLayer_ = ""; + bool isInitialized_ = false; +}; + +} /* namespace se2_planning */ diff --git a/se2_planning/src/Bounds.cpp b/se2_planning/src/Bounds.cpp new file mode 100644 index 0000000..8ea3c6c --- /dev/null +++ b/se2_planning/src/Bounds.cpp @@ -0,0 +1,16 @@ +/* + * Bounds.cpp + * + * Created on: May 29, 2021 + * Author: meyerc + */ + +#include "se2_planning/Bounds.hpp" + +#include + +namespace se2_planning { + +Bounds::Bounds(XYstate low, XYstate high) : low_(low), high_(high) {} + +} /* namespace se2_planning */ diff --git a/se2_planning/src/HeightMap.cpp b/se2_planning/src/HeightMap.cpp index d33fafc..194c66c 100644 --- a/se2_planning/src/HeightMap.cpp +++ b/se2_planning/src/HeightMap.cpp @@ -5,13 +5,6 @@ * Author: jelavice */ -/* - * HeightMap.hpp - * - * Created on: Feb 17, 2021 - * Author: jelavice - */ - #include "se2_planning/HeightMap.hpp" namespace se2_planning { @@ -20,7 +13,15 @@ bool HeightMap::isInitialized() const { return isInitialized_; } -double HeightMap::getHeightAt(double x, double y) const { +void HeightMap::initializeBounds(const Bounds& bounds) { + (void)bounds; +} + +Bounds HeightMap::getBounds() const { + return Bounds{}; +} + +double HeightMap::getValueAt(double x, double y) const { return impl_.atPosition(heightLayer_, grid_map::Position(x, y)); } @@ -30,8 +31,4 @@ void HeightMap::setGridMap(const grid_map::GridMap& gm, const std::string& heigh isInitialized_ = true; } -const grid_map::GridMap& HeightMap::getGridMap() const { - return impl_; -} - } /* namespace se2_planning*/ diff --git a/se2_planning/src/Map.cpp b/se2_planning/src/Map.cpp new file mode 100644 index 0000000..27082f1 --- /dev/null +++ b/se2_planning/src/Map.cpp @@ -0,0 +1,38 @@ +/* + * Map.cpp + * + * Created on: May 30, 2021 + * Author: meyerc + */ + +#include "se2_planning/Map.hpp" + +namespace se2_planning { + +bool Map::isInitialized() const { + return true; +} + +void Map::initializeBounds(const Bounds& bounds) { + (void)bounds; + throw std::runtime_error("Planner: initializeBounds() not implemented"); +} + +Bounds Map::getBounds() const { + throw std::runtime_error("Planner: getBounds() not implemented"); +} + +double Map::getValueAt(double x, double y) const { + (void)x; + (void)y; + throw std::runtime_error("Planner: getValueAt() not implemented"); +} + +void Map::lock() { + mtx_.lock(); +} + +void Map::unlock() { + mtx_.unlock(); +} +} // namespace se2_planning diff --git a/se2_planning/src/OccupancyMap.cpp b/se2_planning/src/OccupancyMap.cpp new file mode 100644 index 0000000..a57f866 --- /dev/null +++ b/se2_planning/src/OccupancyMap.cpp @@ -0,0 +1,40 @@ +/* + * OccupancyMap.cpp + * + * Created on: May 16, 2021 + * Author: meyerc + */ + +#include "se2_planning/OccupancyMap.hpp" + +namespace se2_planning { + +bool OccupancyMap::isInitialized() const { + return isInitialized_; +} + +void OccupancyMap::initializeBounds(const Bounds& bounds) { + // TODO(christoph): When grid map is set, bounds are set anyways, do we need a separate init option? Can lead to + // mismatch between bounds and map. + (void)bounds; +} + +Bounds OccupancyMap::getBounds() const { + const auto position = impl_.getPosition(); + const auto length = impl_.getLength(); + const Bounds bounds = {{position.x() - length.x() / 2.0, position.y() - length.y() / 2.0}, + {position.x() + length.x() / 2.0, position.y() + length.y() / 2.0}}; + return bounds; +} + +double OccupancyMap::getValueAt(double x, double y) const { + return impl_.atPosition(occupancyLayer_, grid_map::Position(x, y)); +} + +void OccupancyMap::setGridMap(const grid_map::GridMap& gm, const std::string& occupancyLayer) { + occupancyLayer_ = occupancyLayer; + impl_ = gm; + isInitialized_ = true; +} + +} /* namespace se2_planning*/ From fc5707788019f994fd8cbe491e2ef3960abf2804 Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 21:46:10 +0200 Subject: [PATCH 19/24] Add ROS class for occupancy map --- .../config/reeds_shepp_planner_ros.yaml | 5 +- se2_planning_ros/CMakeLists.txt | 5 +- .../reeds_shepp_planner_ros_example.yaml | 5 +- .../se2_planning_ros/OccupancyMapRos.hpp | 45 +++++++++++++++++ .../OmplReedsSheppPlannerRos.hpp | 1 - .../include/se2_planning_ros/loaders.hpp | 2 + se2_planning_ros/src/OccupancyMapRos.cpp | 49 +++++++++++++++++++ se2_planning_ros/src/loaders.cpp | 17 ++++++- 8 files changed, 123 insertions(+), 6 deletions(-) create mode 100644 se2_planning_ros/include/se2_planning_ros/OccupancyMapRos.hpp create mode 100644 se2_planning_ros/src/OccupancyMapRos.cpp diff --git a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml index 8ba0ad8..78db2c2 100644 --- a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml +++ b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml @@ -28,6 +28,10 @@ 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 @@ -35,7 +39,6 @@ planner_ros: 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 diff --git a/se2_planning_ros/CMakeLists.txt b/se2_planning_ros/CMakeLists.txt index bc65159..f6e311f 100644 --- a/se2_planning_ros/CMakeLists.txt +++ b/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 ) diff --git a/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml b/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml index a785fe0..45a5570 100644 --- a/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml +++ b/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml @@ -27,6 +27,10 @@ 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 @@ -34,7 +38,6 @@ planner_ros: 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 diff --git a/se2_planning_ros/include/se2_planning_ros/OccupancyMapRos.hpp b/se2_planning_ros/include/se2_planning_ros/OccupancyMapRos.hpp new file mode 100644 index 0000000..7db5499 --- /dev/null +++ b/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 +#include +#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 createOccupancyMapRos(const ros::NodeHandlePtr nh, const OccupancyMapRosParameters& params); + +} /* namespace se2_planning*/ diff --git a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp index 1575c02..6b7d5c7 100644 --- a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp +++ b/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 { diff --git a/se2_planning_ros/include/se2_planning_ros/loaders.hpp b/se2_planning_ros/include/se2_planning_ros/loaders.hpp index 6b4ab5b..2f808da 100644 --- a/se2_planning_ros/include/se2_planning_ros/loaders.hpp +++ b/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); diff --git a/se2_planning_ros/src/OccupancyMapRos.cpp b/se2_planning_ros/src/OccupancyMapRos.cpp new file mode 100644 index 0000000..3023b85 --- /dev/null +++ b/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(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 createOccupancyMapRos(const ros::NodeHandlePtr nh, const OccupancyMapRosParameters& params) { + std::unique_ptr map = std::make_unique(nh); + map->setParameters(params); + map->init(); + return map; +} + +} // namespace se2_planning diff --git a/se2_planning_ros/src/loaders.cpp b/se2_planning_ros/src/loaders.cpp index 0ea61a3..7b25bc6 100644 --- a/se2_planning_ros/src/loaders.cpp +++ b/se2_planning_ros/src/loaders.cpp @@ -53,7 +53,22 @@ OmplReedsSheppPlannerRosParameters loadOmplReedsSheppPlannerRosParameters(const parameters.planningServiceName_ = node["planning_service_name"].as(); parameters.pathNavMsgResolution_ = node["nav_msg_path_spatial_resolution"].as(); parameters.pathMsgTopic_ = node["path_msg_topic"].as(); - parameters.stateSpaceBoundsMargin_ = node["state_space_bounds_margin"].as(); + + 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(); + parameters.gridMapMsgSubTopic_ = node["grid_map_sub_topic"].as(); + parameters.gridMapMsgPubTopic_ = node["grid_map_pub_topic"].as(); return parameters; } From 632a7f67e10852168798b91e0a05d011ab618b27 Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 22:17:03 +0200 Subject: [PATCH 20/24] Integrate new map for state space boundaries update, adapt tests --- .../config/reeds_shepp_planner_ros.yaml | 5 +- .../include/se2_planning/OmplPlanner.hpp | 7 ++- .../se2_planning/OmplReedsSheppPlanner.hpp | 10 +--- se2_planning/include/se2_planning/Planner.hpp | 17 +++++-- .../include/se2_planning/StateValidator.hpp | 6 ++- se2_planning/src/OmplPlanner.cpp | 16 ++++++- se2_planning/src/OmplReedsSheppPlanner.cpp | 35 +++++++++++--- se2_planning/src/StateValidator.cpp | 8 +--- se2_planning/src/planning_benchmark.cpp | 23 ++++----- .../test/OmplReedsSheppPlannerTest.cpp | 8 +++- se2_planning/test/test_helpers.cpp | 45 +++++++++--------- se2_planning/test/test_helpers.hpp | 9 ++-- .../reeds_shepp_planner_ros_example.yaml | 5 +- .../include/se2_planning_ros/PlannerRos.hpp | 7 ++- .../src/OmplReedsSheppPlannerRos.cpp | 41 ++++++---------- se2_planning_ros/src/PlannerRos.cpp | 16 ++++++- se2_planning_ros/src/loaders.cpp | 9 ++-- se2_planning_ros/src/se2_planner_node.cpp | 47 ++++++------------- 18 files changed, 164 insertions(+), 150 deletions(-) diff --git a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml index 78db2c2..4cb5ca9 100644 --- a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml +++ b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml @@ -1,9 +1,6 @@ state_space: - x_lower: -100.0 - x_upper: 100.0 - y_lower: -100.0 - y_upper: 100.0 + boundaries_margin: 3.0 turning_radius: 10.0 state_validator_ros: diff --git a/se2_planning/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index e50bccb..99e3c91 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -31,7 +31,11 @@ class OmplPlanner : public Planner { const StateValidator& getStateValidator() const final; void lockStateValidator() final; void unlockStateValidator() final; - bool isLocked() const final; + + void setMap(std::unique_ptr Map) final; + const Map& getMap() const final; + void lockMap() final; + void unlockMap() final; virtual void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds); virtual const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const; @@ -57,6 +61,7 @@ class OmplPlanner : public Planner { ompl::base::ScopedStatePtr startState_, goalState_; std::unique_ptr path_, interpolatedPath_; std::unique_ptr stateValidator_; + std::unique_ptr map_; private: double maxPlanningDuration_ = 1.0; diff --git a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp index 462ab53..ddd5fc2 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -34,15 +34,8 @@ struct ReedsSheppPath : public Path { }; struct OmplReedsSheppPlannerParameters { + double boundariesMargin_ = 1.0; double turningRadius_ = 10.0; - - // TODO(christoph): Do we need these parameters? Should be provided by the map. If no map available, no planning - // possible...? - double xLowerBound_ = -1000.0; - double xUpperBound_ = 1000.0; - double yLowerBound_ = -1000.0; - double yUpperBound_ = 1000.0; - double pathSpatialResolution_ = 0.05; double maxPlanningTime_ = 1.0; std::string omplPlannerName_ = "RRTstar"; @@ -61,6 +54,7 @@ class OmplReedsSheppPlanner final : public OmplPlanner { void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override; const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const override; bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const; + void extendStateSpaceBoundaries(const se2_planning::ReedsSheppState& state, const double margin); private: void createDefaultStateSpace(); diff --git a/se2_planning/include/se2_planning/Planner.hpp b/se2_planning/include/se2_planning/Planner.hpp index 8e4d287..e70a599 100644 --- a/se2_planning/include/se2_planning/Planner.hpp +++ b/se2_planning/include/se2_planning/Planner.hpp @@ -7,6 +7,7 @@ #pragma once +#include "se2_planning/Map.hpp" #include "se2_planning/State.hpp" #include "se2_planning/StateValidator.hpp" @@ -25,14 +26,24 @@ class Planner { virtual bool reset() { throw std::runtime_error("Planner: reset() not implemented"); } virtual bool initialize() { throw std::runtime_error("Planner: initialize() not implemented"); } - virtual void getStartingState(State* startingState) const { throw std::runtime_error("Planner: getStartingState() not implemented"); } - virtual void getGoalState(State* goalState) const { throw std::runtime_error("Planner: getGoalState() not implemented"); } + virtual void getStartingState(State* startingState) const { + (void)startingState; + throw std::runtime_error("Planner: getStartingState() not implemented"); + } + virtual void getGoalState(State* goalState) const { + (void)goalState; + throw std::runtime_error("Planner: getGoalState() not implemented"); + } virtual void setStateValidator(std::unique_ptr stateValidator) = 0; virtual const StateValidator& getStateValidator() const { throw std::runtime_error("Planner: getStateValidator() not implemented"); }; virtual void lockStateValidator() = 0; virtual void unlockStateValidator() = 0; - virtual bool isLocked() const = 0; + + virtual void setMap(std::unique_ptr Map) = 0; + virtual const Map& getMap() const { throw std::runtime_error("Planner: getMap() not implemented"); }; + virtual void lockMap() = 0; + virtual void unlockMap() = 0; template const T* as() const { diff --git a/se2_planning/include/se2_planning/StateValidator.hpp b/se2_planning/include/se2_planning/StateValidator.hpp index 3111520..b6f3ae3 100644 --- a/se2_planning/include/se2_planning/StateValidator.hpp +++ b/se2_planning/include/se2_planning/StateValidator.hpp @@ -8,21 +8,23 @@ #pragma once #include "se2_planning/State.hpp" +#include + namespace se2_planning { class StateValidator { public: StateValidator() = default; virtual ~StateValidator() = default; + virtual bool isStateValid(const State& state) const = 0; virtual void initialize(); virtual bool isInitialized() const; - virtual bool isLocked() const; virtual void lock(); virtual void unlock(); private: - bool isLocked_ = false; + boost::mutex mtx_; }; class SE2stateValidator : public StateValidator { diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index eea3f1e..19d8984 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -56,8 +56,20 @@ void OmplPlanner::unlockStateValidator() { stateValidator_->unlock(); } -bool OmplPlanner::isLocked() const { - return stateValidator_->isLocked(); +void OmplPlanner::setMap(std::unique_ptr Map) { + map_ = std::move(Map); +} + +const Map& OmplPlanner::getMap() const { + return *map_; +} + +void OmplPlanner::lockMap() { + map_->lock(); +} + +void OmplPlanner::unlockMap() { + map_->unlock(); } bool OmplPlanner::plan() { diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index 7c25ffe..8ffb061 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -46,17 +46,38 @@ void OmplReedsSheppPlanner::createDefaultStateSpace() { // Allocate abstracted objects stateSpace_.reset(new ompl::base::ReedsSheppStateSpace(parameters_.turningRadius_)); bounds_ = std::make_unique(reedsSheppStateSpaceDim_); - // Initialize bounds with parameters + // Initialize bounds to zero ompl::base::RealVectorBounds bounds(reedsSheppStateSpaceDim_); - bounds.low[0] = parameters_.xLowerBound_; - bounds.low[1] = parameters_.yLowerBound_; - bounds.high[0] = parameters_.xUpperBound_; - bounds.high[1] = parameters_.yUpperBound_; setStateSpaceBoundaries(bounds); } + +void OmplReedsSheppPlanner::extendStateSpaceBoundaries(const se2_planning::ReedsSheppState& state, const double margin) { + const auto bounds = getStateSpaceBoundaries(); + ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_); + newBounds.high[0] = std::max(bounds.high[0], state.x_ + margin); + newBounds.high[1] = std::max(bounds.high[1], state.y_ + margin); + newBounds.low[0] = std::min(bounds.low[0], state.x_ - margin); + newBounds.low[1] = std::min(bounds.low[1], state.y_ - margin); + setStateSpaceBoundaries(newBounds); +} + bool OmplReedsSheppPlanner::plan() { - // Call to initialize is required s.t. OMPL planner is updated with latest state space boundaries - // initialize(); + // Update state space boundaries with values from map + const auto bounds = map_->getBounds(); + ompl::base::RealVectorBounds realVectorBounds(reedsSheppStateSpaceDim_); + realVectorBounds.low[0] = bounds.low_.x_; + realVectorBounds.low[1] = bounds.low_.y_; + realVectorBounds.high[0] = bounds.high_.x_; + realVectorBounds.high[1] = bounds.high_.y_; + setStateSpaceBoundaries(realVectorBounds); + // Extend state space boundaries from the map to always include start and goal with an additional margin + ReedsSheppState startState{}; + getStartingState(&startState); + extendStateSpaceBoundaries(startState, parameters_.boundariesMargin_); + ReedsSheppState goalState{}; + getGoalState(&goalState); + extendStateSpaceBoundaries(goalState, parameters_.boundariesMargin_); + // Plan const bool result = BASE::plan(); *interpolatedPath_ = interpolatePath(*path_, parameters_.pathSpatialResolution_); return result; diff --git a/se2_planning/src/StateValidator.cpp b/se2_planning/src/StateValidator.cpp index 304d417..9c523a1 100644 --- a/se2_planning/src/StateValidator.cpp +++ b/se2_planning/src/StateValidator.cpp @@ -20,15 +20,11 @@ bool SE2stateValidator::isStateValid(const State& state) const { return true; } -bool StateValidator::isLocked() const { - return isLocked_; -} - void StateValidator::lock() { - isLocked_ = true; + mtx_.lock(); } void StateValidator::unlock() { - isLocked_ = false; + mtx_.unlock(); } } // namespace se2_planning diff --git a/se2_planning/src/planning_benchmark.cpp b/se2_planning/src/planning_benchmark.cpp index d3e7c11..c2e1db1 100644 --- a/se2_planning/src/planning_benchmark.cpp +++ b/se2_planning/src/planning_benchmark.cpp @@ -11,6 +11,7 @@ #include "grid_map_core/iterators/GridMapIterator.hpp" #include "ompl/base/objectives/PathLengthOptimizationObjective.h" #include "se2_planning/GridMapLazyStateValidator.hpp" +#include "se2_planning/OccupancyMap.hpp" #include "se2_planning/OmplReedsSheppPlanner.hpp" #include "test_helpers.hpp" @@ -30,13 +31,6 @@ void setCostThreshold(se2_planning::OmplReedsSheppPlanner* planner) { planner->getSimpleSetup()->setOptimizationObjective(optimizationObjective); } -void createRectangularStateSpace(double stateBound, se2_planning::OmplReedsSheppPlannerParameters* parameters) { - parameters->xLowerBound_ = -stateBound; - parameters->xUpperBound_ = stateBound; - parameters->yLowerBound_ = -stateBound; - parameters->yUpperBound_ = stateBound; -} - void runPlanner(se2_planning::OmplReedsSheppPlanner& planner) { const se2_planning::ReedsSheppState start(0.0, -10.0, 0.0); const se2_planning::ReedsSheppState goal(0.0, 10.0, 0.0); @@ -99,27 +93,26 @@ void runLazyRandomEarlyStoppingValidator(se2_planning::OmplReedsSheppPlanner& pl runPlanner(planner); } -int main(int argc, char** argv) { +int main() { namespace test = se2_planning_test; ompl::msg::setLogLevel(ompl::msg::LogLevel::LOG_NONE); - const std::string testLayer = "occupancy"; // create environment grid_map::GridMap gridMap; gridMap.setGeometry(grid_map::Length(40.0, 40.0), 0.1); - gridMap.add(testLayer, 0.0); + gridMap.add(benchmarkLayer, 0.0); std::function isAnObstacle = [](double x, double y) { return test::isInsideRectangle(x, y, -10.0, 0.0, 40.0, 2.0) || test::isInsideRectangle(x, y, 5.0, 5.0, 5.0, 5.0); }; - test::addObstacles(isAnObstacle, testLayer, &gridMap); + test::addObstacles(isAnObstacle, benchmarkLayer, &gridMap); // setup planner - se2_planning::OmplReedsSheppPlannerParameters parameters; - parameters.maxPlanningTime_ = 10.0; - parameters.turningRadius_ = 1.0; - createRectangularStateSpace(20.0, ¶meters); + se2_planning::OmplReedsSheppPlannerParameters parameters = se2_planning_test::createDefaultParams(); se2_planning::OmplReedsSheppPlanner planner; planner.setParameters(parameters); planner.initialize(); + auto map = std::make_unique(); + map->setGridMap(gridMap, benchmarkLayer); + planner.setMap(std::move(map)); setCostThreshold(&planner); runNormalValidator(planner, gridMap); diff --git a/se2_planning/test/OmplReedsSheppPlannerTest.cpp b/se2_planning/test/OmplReedsSheppPlannerTest.cpp index 1a437aa..0e64e9c 100644 --- a/se2_planning/test/OmplReedsSheppPlannerTest.cpp +++ b/se2_planning/test/OmplReedsSheppPlannerTest.cpp @@ -26,8 +26,12 @@ TEST(Planning, OmplReedsSheppPlanner) ompl::RNG::setSeed(seed); ompl::msg::setLogLevel(ompl::msg::LogLevel::LOG_NONE); - se2_planning::OmplReedsSheppPlannerParameters parameters = - createRectangularStateSpaceWithDefaultParams(10.0); + se2_planning::OmplReedsSheppPlannerParameters parameters = createDefaultParams(); + + std::function noObstacles = [](double x, double y) { + return false; + }; + grid_map::GridMap map = createGridMap(20, 20, 0.1, noObstacles) se2_planning::OmplReedsSheppPlanner planner; setupPlanner(parameters, &planner); diff --git a/se2_planning/test/test_helpers.cpp b/se2_planning/test/test_helpers.cpp index 3d59826..9ac3fd7 100644 --- a/se2_planning/test/test_helpers.cpp +++ b/se2_planning/test/test_helpers.cpp @@ -60,6 +60,18 @@ se2_planning::SE2state randomState(const grid_map::GridMap &gm, double margin) return s; } +se2_planning::ReedsSheppState randomReedsSheppState(const grid_map::GridMap &gm, double margin) +{ + se2_planning::ReedsSheppState s; + const double l = gm.getLength()(0) / 2.0 - margin; + const double w = gm.getLength()(1) / 2.0 - margin; + s.x_ = randomNumber(-l, l); + s.y_ = randomNumber(-w, w); + s.yaw_ = randomNumber(-M_PI, M_PI); + return s; +} + + grid_map::GridMap createGridMap(double length, double width, double resolution, std::function isAnObstacle) { @@ -93,16 +105,6 @@ bool isStartAndGoalStateOK(const se2_planning::ReedsSheppPath &path, return firstState == start && lastState == goal; } -se2_planning::ReedsSheppState randomState( - const se2_planning::OmplReedsSheppPlannerParameters ¶meters) -{ - se2_planning::ReedsSheppState s; - s.x_ = randomNumber(0.8 * parameters.xLowerBound_, 0.8 * parameters.xUpperBound_); - s.y_ = randomNumber(0.8 * parameters.yLowerBound_, 0.8 * parameters.yUpperBound_); - s.yaw_ = randomNumber(-M_PI, M_PI); - return s; -} - void setCostThreshold(se2_planning::OmplReedsSheppPlanner *planner) { auto si = planner->getSimpleSetup()->getSpaceInformation(); @@ -112,17 +114,12 @@ void setCostThreshold(se2_planning::OmplReedsSheppPlanner *planner) planner->getSimpleSetup()->setOptimizationObjective(optimizationObjective); } -se2_planning::OmplReedsSheppPlannerParameters createRectangularStateSpaceWithDefaultParams( - double stateBound) -{ - se2_planning::OmplReedsSheppPlannerParameters parameters; - parameters.xLowerBound_ = -stateBound; - parameters.xUpperBound_ = stateBound; - parameters.yLowerBound_ = -stateBound; - parameters.yUpperBound_ = stateBound; - parameters.maxPlanningTime_ = 10.0; - parameters.turningRadius_ = 1.0; - return parameters; +se2_planning::OmplReedsSheppPlannerParameters createDefaultParams(){ + se2_planning::OmplReedsSheppPlannerParameters params{}; + params.maxPlanningTime_ = 10.0; + params.turningRadius_ = 1.0; + params.boundariesMargin_ = 1.0; + return params; } void setupPlanner(const se2_planning::OmplReedsSheppPlannerParameters ¶meters, @@ -135,10 +132,10 @@ void setupPlanner(const se2_planning::OmplReedsSheppPlannerParameters ¶meter bool simplePlanBetweenRandomStartAndGoalTest( se2_planning::OmplReedsSheppPlanner &planner, - const se2_planning::OmplReedsSheppPlannerParameters ¶meters) + const se2_planning::OmplReedsSheppPlannerParameters ¶meters, const grid_map::GridMap &map) { - const auto start = randomState(parameters); - const auto goal = randomState(parameters); + const se2_planning::ReedsSheppState start = randomReedsSheppState(map, parameters.boundariesMargin_); + const se2_planning::ReedsSheppState goal = randomReedsSheppState(map, parameters.boundariesMargin_); planner.setStartingState(start); planner.setGoalState(goal); const bool status = planner.plan(); diff --git a/se2_planning/test/test_helpers.hpp b/se2_planning/test/test_helpers.hpp index 62698d0..21273a4 100644 --- a/se2_planning/test/test_helpers.hpp +++ b/se2_planning/test/test_helpers.hpp @@ -26,6 +26,7 @@ void addObstacles(std::function isObstacle, const std::str grid_map::GridMap *map); bool isInsideRectangle(double _x, double _y, double x0, double y0, double xLength, double yLength); se2_planning::SE2state randomState(const grid_map::GridMap &gm, double margin); +se2_planning::ReedsSheppState randomReedsSheppState(const grid_map::GridMap &gm, double margin); grid_map::GridMap createGridMap(double length, double width, double resolution, std::function isAnObstacle); @@ -36,19 +37,15 @@ bool isStartAndGoalStateOK(const se2_planning::ReedsSheppPath &path, const se2_planning::ReedsSheppState &start, const se2_planning::ReedsSheppState &goal); -se2_planning::ReedsSheppState randomState( - const se2_planning::OmplReedsSheppPlannerParameters ¶meters); - void setCostThreshold(se2_planning::OmplReedsSheppPlanner *planner); -se2_planning::OmplReedsSheppPlannerParameters createRectangularStateSpaceWithDefaultParams( - double stateBound); +se2_planning::OmplReedsSheppPlannerParameters createDefaultParams(); void setupPlanner(const se2_planning::OmplReedsSheppPlannerParameters ¶meters, se2_planning::OmplReedsSheppPlanner *planner); bool simplePlanBetweenRandomStartAndGoalTest( se2_planning::OmplReedsSheppPlanner &planner, - const se2_planning::OmplReedsSheppPlannerParameters ¶meters); + const se2_planning::OmplReedsSheppPlannerParameters ¶meters, const grid_map::GridMap &map); } /* namespace se2_planning_test */ diff --git a/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml b/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml index 45a5570..df28577 100644 --- a/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml +++ b/se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml @@ -1,9 +1,6 @@ state_space: - x_lower: -1000.0 - x_upper: 1000.0 - y_lower: -1000.0 - y_upper: 1000.0 + boundaries_margin: 1.0 turning_radius: 10.0 state_validator_ros: diff --git a/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp index a86d1a4..d87776e 100644 --- a/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp @@ -9,6 +9,7 @@ #include #include "se2_navigation_msgs/RequestPathSrv.h" +#include "se2_planning/Map.hpp" #include "se2_planning/Planner.hpp" namespace se2_planning { @@ -28,7 +29,11 @@ class PlannerRos : public Planner { const StateValidator& getStateValidator() const override; void lockStateValidator() override; void unlockStateValidator() override; - bool isLocked() const override; + + void setMap(std::unique_ptr Map) override; + const Map& getMap() const override; + void lockMap() override; + void unlockMap() override; void setPlanningStrategy(std::shared_ptr planner); virtual void publishPath() const; diff --git a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index 4c9be40..9008aaf 100644 --- a/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp +++ b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp @@ -46,6 +46,11 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl res.status = false; return true; } + if (!planner_->getMap().isInitialized()) { + ROS_WARN_STREAM("Map for state space boundaries has not been initialized yet. Abort planning."); + res.status = false; + return true; + } planTimeStamp_ = req.pathRequest.header.stamp; @@ -57,40 +62,20 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl // Block update of state validator obstacle map during planning planner_->lockStateValidator(); + planner_->lockMap(); - // TODO move from se2_planner_node here (introduces dependency but makes state space update more consistent: - // adapt state space boundaries from grid map - // planner_->getStateValidator().as + // Triggers actual planning + bool result = plan(); // Adapt state space boundaries (larger than grid map, state validity checking assumes area out of bounds to be // traversable) to contain initial and goal state otherwise RRTstar fails - // TODO move to OMPLReedsSheppPlanner.cpp? - const double stateSpaceBoundsMargin_ = parameters_.stateSpaceBoundsMargin_; if (!planner_->as()->satisfiesStateSpaceBoundaries(start)) { - ROS_DEBUG("Initial state not in grid map. Enlarge state space boundaries."); - const auto bounds = planner_->as()->getStateSpaceBoundaries(); - ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_); - newBounds.high[0] = std::max(bounds.high[0], start.x_ + stateSpaceBoundsMargin_); - newBounds.high[1] = std::max(bounds.high[1], start.y_ + stateSpaceBoundsMargin_); - newBounds.low[0] = std::min(bounds.low[0], start.x_ - stateSpaceBoundsMargin_); - newBounds.low[1] = std::min(bounds.low[1], start.y_ - stateSpaceBoundsMargin_); - planner_->as()->setStateSpaceBoundaries(newBounds); + ROS_DEBUG("Initial state not in grid map. Enlarged state space boundaries."); } if (!planner_->as()->satisfiesStateSpaceBoundaries(goal)) { - ROS_DEBUG("Goal state not in grid map. Enlarge state space boundaries."); - const auto bounds = planner_->as()->getStateSpaceBoundaries(); - ompl::base::RealVectorBounds newBounds(reedsSheppStateSpaceDim_); - newBounds.high[0] = std::max(bounds.high[0], goal.x_ + stateSpaceBoundsMargin_); - newBounds.high[1] = std::max(bounds.high[1], goal.y_ + stateSpaceBoundsMargin_); - newBounds.low[0] = std::min(bounds.low[0], goal.x_ - stateSpaceBoundsMargin_); - newBounds.low[1] = std::min(bounds.low[1], goal.y_ - stateSpaceBoundsMargin_); - planner_->as()->setStateSpaceBoundaries(newBounds); + ROS_DEBUG("Goal state not in grid map. Enlarged state space boundaries."); } - publishStartState(); - publishGoalState(); - publishStateSpaceBoundaryMarker(); - // Use state validator only after lock mutex is active and state space is updated // Checks only for non-traversable terrain not for state space bounds if (!planner_->getStateValidator().isStateValid(start)) { @@ -101,7 +86,11 @@ bool OmplReedsSheppPlannerRos::planningService(PlanningService::Request& req, Pl ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Start planning anyway."); } - bool result = plan(); + publishStartState(); + publishGoalState(); + publishStateSpaceBoundaryMarker(); + + planner_->unlockMap(); planner_->unlockStateValidator(); res.status = result; diff --git a/se2_planning_ros/src/PlannerRos.cpp b/se2_planning_ros/src/PlannerRos.cpp index 4157a6e..6038eb6 100644 --- a/se2_planning_ros/src/PlannerRos.cpp +++ b/se2_planning_ros/src/PlannerRos.cpp @@ -83,8 +83,20 @@ void PlannerRos::unlockStateValidator() { planner_->unlockStateValidator(); } -bool PlannerRos::isLocked() const { - return planner_->isLocked(); +void PlannerRos::setMap(std::unique_ptr Map) { + planner_->setMap(std::move(Map)); +} + +const Map& PlannerRos::getMap() const { + return planner_->getMap(); +} + +void PlannerRos::lockMap() { + planner_->lockMap(); +} + +void PlannerRos::unlockMap() { + planner_->unlockMap(); } } // namespace se2_planning diff --git a/se2_planning_ros/src/loaders.cpp b/se2_planning_ros/src/loaders.cpp index 7b25bc6..01d4b1e 100644 --- a/se2_planning_ros/src/loaders.cpp +++ b/se2_planning_ros/src/loaders.cpp @@ -22,10 +22,7 @@ OmplReedsSheppPlannerParameters loadOmplReedsSheppPlannerParameters(const std::s OmplReedsSheppPlannerParameters parameters; { auto node = basenode["state_space"]; - parameters.xLowerBound_ = node["x_lower"].as(); - parameters.xUpperBound_ = node["x_upper"].as(); - parameters.yLowerBound_ = node["y_lower"].as(); - parameters.yUpperBound_ = node["y_upper"].as(); + parameters.boundariesMargin_ = node["boundaries_margin"].as(); parameters.turningRadius_ = node["turning_radius"].as(); } @@ -89,11 +86,13 @@ GridMapLazyStateValidatorRosParameters loadGridMapLazyStateValidatorRosParameter parameters.gridMapStateValidityCheckingMethod_ = StateValidityCheckingMethod::COLLISION; } else if (stateValidityCheckingMethodName == "traversability") { parameters.gridMapStateValidityCheckingMethod_ = StateValidityCheckingMethod::TRAVERSABILITY; + } else if (stateValidityCheckingMethodName == "traversability_iterator") { + parameters.gridMapStateValidityCheckingMethod_ = StateValidityCheckingMethod::TRAVERSABILITY_ITERATOR; } else if (stateValidityCheckingMethodName == "robust_traversability") { parameters.gridMapStateValidityCheckingMethod_ = StateValidityCheckingMethod::ROBUST_TRAVERSABILITY; } else { throw std::runtime_error( - "Invalid value for StateValidityCheckingMethod. Valid values are 'collision', 'traversability'" + "Invalid value for StateValidityCheckingMethod. Valid values are 'collision', 'traversability', 'traversability_iterator" "or 'robust_traversability'"); } parameters.gridMapStateValidityThreshold_ = node["grid_map_state_validity_threshold"].as(); diff --git a/se2_planning_ros/src/se2_planner_node.cpp b/se2_planning_ros/src/se2_planner_node.cpp index 9db3422..d080e9e 100644 --- a/se2_planning_ros/src/se2_planner_node.cpp +++ b/se2_planning_ros/src/se2_planner_node.cpp @@ -10,36 +10,12 @@ #include #include "se2_planning/OmplReedsSheppPlanner.hpp" #include "se2_planning_ros/GridMapLazyStateValidatorRos.hpp" +#include "se2_planning_ros/OccupancyMapRos.hpp" +#include "se2_planning_ros/OmplReedsSheppPlannerRos.hpp" #include "se2_planning_ros/loaders.hpp" using namespace se2_planning; -// Make planner global to have access to it in the gridMapCallback, -// required to update planner bounds in dependence of grid map -auto planner = std::make_shared(); - -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); - - // Grid map is symmetric around position - const grid_map::Position mapPosition = map.getPosition(); - const grid_map::Length mapLength = map.getLength(); - ompl::base::RealVectorBounds bounds(2); - bounds.low[0] = mapPosition.x() - mapLength.x() / 2.0; - bounds.low[1] = mapPosition.y() - mapLength.y() / 2.0; - bounds.high[0] = mapPosition.x() + mapLength.x() / 2.0; - bounds.high[1] = mapPosition.y() + mapLength.y() / 2.0; - planner->lockStateValidator(); - planner->setStateSpaceBoundaries(bounds); - planner->unlockStateValidator(); - ROS_DEBUG_STREAM("OMPL State Space Update: pos: " << mapPosition.x() << ", " << mapPosition.y() << ", length: " << mapLength.x() << ", " - << mapLength.y()); -} - int main(int argc, char** argv) { ros::init(argc, argv, "se2_planner_node"); ros::NodeHandlePtr nh(new ros::NodeHandle("~")); @@ -49,9 +25,11 @@ int main(int argc, char** argv) { const auto plannerParameters = loadOmplReedsSheppPlannerParameters(filename); const auto plannerRosParameters = loadOmplReedsSheppPlannerRosParameters(filename); const auto stateValidatorRosParameters = loadGridMapLazyStateValidatorRosParameters(filename); - planner->setParameters(plannerParameters); + const auto mapRosParameters = loadOccupancyMapRosParameters(filename); - // Create initial grid map + // Create initial grid map for state validator in planner + // TODO(christoph): Currently necessary because GridMapLazyStateValidator expects that a grid map is set when calling + // initialize function (precomputes footprint points based on grid map size) grid_map::GridMap gridMap; gridMap.setFrameId(stateValidatorRosParameters.gridMapFrame_); gridMap.setGeometry(grid_map::Length(stateValidatorRosParameters.gridMapLength_, stateValidatorRosParameters.gridMapWidth_), @@ -59,13 +37,21 @@ int main(int argc, char** argv) { grid_map::Position(stateValidatorRosParameters.gridMapPositionX_, stateValidatorRosParameters.gridMapPositionY_)); gridMap.add(stateValidatorRosParameters.gridMapObstacleLayerName_, stateValidatorRosParameters.gridMapDefaultValue_); - // Set grid map state validator + // Setup state validator auto validator = se2_planning::createGridMapLazyStateValidatorRos( nh, stateValidatorRosParameters, gridMap, se2_planning::computeFootprint( stateValidatorRosParameters.robotFootPrintLengthForward_, stateValidatorRosParameters.robotFootPrintLengthBackward_, stateValidatorRosParameters.robotFootPrintWidthLeft_, stateValidatorRosParameters.robotFootPrintWidthRight_)); + + // Setup map for state space boundaries in planner + auto map = se2_planning::createOccupancyMapRos(nh, mapRosParameters); + + // Setup planner + auto planner = std::make_shared(); + planner->setParameters(plannerParameters); planner->setStateValidator(std::move(validator)); + planner->setMap(std::move(map)); // Setup ROS interface and start node se2_planning::OmplReedsSheppPlannerRos plannerRos(nh); @@ -73,9 +59,6 @@ int main(int argc, char** argv) { plannerRos.setParameters(plannerRosParameters); plannerRos.initialize(); - // Callback to update OMPL planner bounds when map changes - ros::Subscriber mapSub = nh->subscribe(stateValidatorRosParameters.gridMapMsgSubTopic_, 1, gridMapCallback); - ros::spin(); return 0; From db5da4d149f10c160801f32f26815cd9bc0025c8 Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 22:17:51 +0200 Subject: [PATCH 21/24] Uncomment isTraversable again --- .../GridMapLazyStateValidator.hpp | 6 +- .../src/GridMapLazyStateValidator.cpp | 90 ++++++++++--------- 2 files changed, 51 insertions(+), 45 deletions(-) diff --git a/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp b/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp index 938e6c0..cdecd9f 100644 --- a/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp +++ b/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp @@ -15,7 +15,7 @@ namespace se2_planning { -enum StateValidityCheckingMethod : int { COLLISION = 0, TRAVERSABILITY = 1, ROBUST_TRAVERSABILITY = 2 }; +enum StateValidityCheckingMethod : int { COLLISION = 0, TRAVERSABILITY = 1, TRAVERSABILITY_ITERATOR = 2, ROBUST_TRAVERSABILITY = 3 }; class GridMapLazyStateValidator : public GridMapStateValidator { using BASE = GridMapStateValidator; @@ -64,8 +64,8 @@ class GridMapLazyStateValidator : public GridMapStateValidator { void computeFootprintPoints(const grid_map::GridMap& gridMap, const RobotFootprint& footprint, std::vector* footprintPoints); bool isInCollision(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, const std::string& obstacleLayer, const double collisionThreshold); -// bool isTraversable(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, -// const std::string& traversabilityLayer, const double traversabilityThreshold); +bool isTraversable(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, + const std::string& traversabilityLayer, const double traversabilityThreshold); bool isTraversableIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap, const std::string& traversabilityLayer, const double traversabilityThreshold); bool isTraversableRobustIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap, diff --git a/se2_planning/src/GridMapLazyStateValidator.cpp b/se2_planning/src/GridMapLazyStateValidator.cpp index 5b244a3..f67767f 100644 --- a/se2_planning/src/GridMapLazyStateValidator.cpp +++ b/se2_planning/src/GridMapLazyStateValidator.cpp @@ -79,6 +79,7 @@ void GridMapLazyStateValidator::initialize() { if (isUseEarlyStoppingHeuristic_) { addExtraPointsForEarlyStopping(nominalFootprint_, &nominalFootprintPoints_, seed_); } + isInitializeCalled_ = true; } @@ -97,6 +98,8 @@ bool GridMapLazyStateValidator::isStateValid(const State& state) const { case StateValidityCheckingMethod::COLLISION: return !isInCollision(se2state, nominalFootprintPoints_, gridMap_, obstacleLayerName_, minStateValidityThreshold_); case StateValidityCheckingMethod::TRAVERSABILITY: + return isTraversable(se2state, nominalFootprintPoints_, gridMap_, obstacleLayerName_, minStateValidityThreshold_); + case StateValidityCheckingMethod::TRAVERSABILITY_ITERATOR: return isTraversableIterator(se2state, nominalFootprint_, gridMap_, obstacleLayerName_, minStateValidityThreshold_); case StateValidityCheckingMethod::ROBUST_TRAVERSABILITY: return isTraversableRobustIterator(se2state, nominalFootprint_, gridMap_, obstacleLayerName_, minStateValidityThreshold_, @@ -143,12 +146,10 @@ bool isInCollision(const SE2state& state, const std::vector& footprint, try { const auto v = transformOperator(vertex); const auto position = grid_map::Position(v.x_, v.y_); - if (!gridMap.isInside(position)) { - return true; // treat space outside the map as being in collision - } - grid_map::Index id; - gridMap.getIndex(position, id); - occupancy = data(bindToRange(id.x(), 0, nRows), bindToRange(id.y(), 0, nCols)); + grid_map::Index idx; + gridMap.getIndex(position, idx); + // TODO(christoph): What is the value for out of the map? Not working correctly for x-direction + occupancy = data(bindToRange(idx.x(), 0, nRows), bindToRange(idx.y(), 0, nCols)); } catch (const std::out_of_range& e) { continue; // TODO at the moment return traversable for out of bounds case // return true; // would assume all points out of bounds to be intraversable @@ -168,42 +169,47 @@ bool isInCollision(const SE2state& state, const std::vector& footprint, return false; } -// TODO Not working with updated state space bounds -// bool isTraversable(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, -// const std::string& traversabilityLayer, const double traversabilityThreshold) { -// const double Cos = std::cos(state.yaw_); -// const double Sin = std::sin(state.yaw_); -// const double dx = state.x_; -// const double dy = state.y_; -// auto transformOperator = [Cos, Sin, dx, dy](const Vertex& v) -> Vertex { -// return Vertex{Cos * v.x_ - Sin * v.y_ + dx, Sin * v.x_ + Cos * v.y_ + dy}; -// }; -// const auto& data = gridMap.get(traversabilityLayer); -// for (const auto& vertex : footprint) { -// double traversability = 1.0; -// try { -// // Translate and rotate to current state -// const auto v = transformOperator(vertex); -// grid_map::Index id; -// gridMap.getIndex(grid_map::Position(v.x_, v.y_), id); -// traversability = data(id.x(), id.y()); -// } catch (const std::out_of_range& e) { -// continue; // TODO at the moment return traversable for out of bounds case -// } -// -// // ignore nans since they might come from faulty -// // perception pipeline -// if (std::isnan(traversability)) { -// continue; -// } -// -// if (traversability < traversabilityThreshold) { -// return false; -// } -// } -// -// return true; -//} +bool isTraversable(const SE2state& state, const std::vector& footprint, const grid_map::GridMap& gridMap, + const std::string& traversabilityLayer, const double traversabilityThreshold) { + const double Cos = std::cos(state.yaw_); + const double Sin = std::sin(state.yaw_); + const double dx = state.x_; + const double dy = state.y_; + + auto transformOperator = [Cos, Sin, dx, dy](const Vertex& v) -> Vertex { + return Vertex{Cos * v.x_ - Sin * v.y_ + dx, Sin * v.x_ + Cos * v.y_ + dy}; + }; + + const auto& data = gridMap.get(traversabilityLayer); + const int nRows = data.rows(); + const int nCols = data.cols(); + for (const auto& vertex : footprint) { + double traversability = 1.0; + try { + // Translate and rotate to current state + const auto v = transformOperator(vertex); + const auto position = grid_map::Position(v.x_, v.y_); + grid_map::Index idx; + gridMap.getIndex(position, idx); + // TODO(christoph): What is the value for out of the map? Not working correctly. + traversability = data(bindToRange(idx.x(), 0, nRows), bindToRange(idx.y(), 0, nCols)); + } catch (const std::out_of_range& e) { + continue; // TODO at the moment return traversable for out of bounds case + } + + // ignore nans since they might come from faulty + // perception pipeline + if (std::isnan(traversability)) { + continue; + } + + if (traversability < traversabilityThreshold) { + return false; + } + } + + return true; +} bool isTraversableIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap, const std::string& traversabilityLayer, const double traversabilityThreshold) { From 80db12126c1ef5d722773cb8fe2e3740224c303e Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 22:18:42 +0200 Subject: [PATCH 22/24] Make GridMapLazyStateValidatorRos similar to OccupancyMapRos --- .../GridMapLazyStateValidatorRos.hpp | 9 +----- .../src/GridMapLazyStateValidatorRos.cpp | 31 ++++++------------- 2 files changed, 11 insertions(+), 29 deletions(-) diff --git a/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp b/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp index df5d2e7..11f76d8 100644 --- a/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp @@ -21,9 +21,6 @@ namespace se2_planning { -using WriteLock = boost::unique_lock; -using ReadLock = boost::shared_lock; - struct GridMapLazyStateValidatorRosParameters { std::string gridMapFrame_ = "map"; std::string gridMapMsgSubTopic_ = "state_validator_ros/traversability_map_in"; @@ -61,14 +58,10 @@ class GridMapLazyStateValidatorRos : public GridMapLazyStateValidator { void initRos(); void mapCb(const grid_map_msgs::GridMap& msg); - ros::NodeHandlePtr nh_; GridMapLazyStateValidatorRosParameters parameters_; - - //! Grid map data. + ros::NodeHandlePtr nh_; ros::Subscriber mapSubscriber_; ros::Publisher mapPublisher_; - bool newMapAvailable_ = false; - boost::shared_mutex gridMapMutex_; }; geometry_msgs::Polygon convert(const RobotFootprint& footprint); diff --git a/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp b/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp index c6cb047..f31b0b2 100644 --- a/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp +++ b/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp @@ -11,7 +11,7 @@ namespace se2_planning { -GridMapLazyStateValidatorRos::GridMapLazyStateValidatorRos(ros::NodeHandlePtr nh) : BASE(), nh_(nh), newMapAvailable_(false) {} +GridMapLazyStateValidatorRos::GridMapLazyStateValidatorRos(ros::NodeHandlePtr nh) : BASE(), nh_(nh) {} void GridMapLazyStateValidatorRos::setParameters(const GridMapLazyStateValidatorRosParameters& parameters) { parameters_ = parameters; @@ -23,27 +23,16 @@ void GridMapLazyStateValidatorRos::initialize() { } void GridMapLazyStateValidatorRos::mapCb(const grid_map_msgs::GridMap& msg) { - // TODO Also replace using the same gridMapMutex_? Issue that we require to set it in OMPLReedsSheppPlanner which only - // has access to StateValidator class => add general state validator mutex? - if (isLocked()) { - ROS_INFO_STREAM("Planner is running, grid map for state validator can not be updated!"); - return; - } else { - grid_map::GridMap newMap; - grid_map::GridMapRosConverter::fromMessage(msg, newMap); + grid_map::GridMap newMap; + grid_map::GridMapRosConverter::fromMessage(msg, newMap); - if (newMap.exists(obstacleLayerName_)) { - WriteLock writeLock(gridMapMutex_); - setGridMap(newMap); - newMapAvailable_ = true; - if (!isGridMapInitialized_) { - isGridMapInitialized_ = true; - } - writeLock.unlock(); - publishMap(getGridMap()); - } else { - ROS_ERROR("GlobalMap: No traversability layer found to load!"); - } + if (newMap.exists(obstacleLayerName_)) { + BASE::lock(); + BASE::setGridMap(newMap); + BASE::unlock(); + publishMap(getGridMap()); + } else { + ROS_ERROR("GlobalMap: No traversability layer found to load!"); } } From ad3095d7da217f008e0e8ae5d95ed038eab53aa4 Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 22:19:16 +0200 Subject: [PATCH 23/24] Change config file to use elevation for collision checking in state validator --- car_demo/car_demo/config/reeds_shepp_planner_ros.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml index 4cb5ca9..f9280cd 100644 --- a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml +++ b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml @@ -7,8 +7,8 @@ state_validator_ros: grid_map_frame: odom 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 + grid_map_obstacle_layer_name: elevation + grid_map_state_validity_checking_method: collision # see GridMapLazyStateValidator.hpp for list of available methods grid_map_state_validity_threshold: 0.1 grid_map_unsafe_state_validity_threshold: 0.1 grid_map_max_number_of_unsafe_cells: 0 From d07c57978453678eff84e64801c544d86ce13d0e Mon Sep 17 00:00:00 2001 From: meychr Date: Mon, 7 Jun 2021 22:19:51 +0200 Subject: [PATCH 24/24] Increase footprint size in car_demo --- car_demo/car_demo/config/reeds_shepp_planner_ros.yaml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml index f9280cd..d179853 100644 --- a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml +++ b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml @@ -20,11 +20,12 @@ state_validator_ros: grid_map_position_y: 0.0 grid_map_default_value: 0.0 # Robot footprint for collision checking - # TODO adapt footprint - robot_footprint_length_forward: 0.40 - robot_footprint_length_backward: 0.40 - robot_footprint_width_left: 0.35 - robot_footprint_width_right: 0.35 + # TODO adapt footprint for car + robot_footprint_length_forward: 1.75 + robot_footprint_length_backward: 1.75 + robot_footprint_width_left: 1.0 + robot_footprint_width_right: 1.0 + map_ros: layer_name: elevation grid_map_sub_topic: /se2_grid_map_generator_node/grid_map