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..d179853 100644 --- a/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml +++ b/car_demo/car_demo/config/reeds_shepp_planner_ros.yaml @@ -1,21 +1,46 @@ 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: + 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: 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 + # 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 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 + grid_map_pub_topic: /occupancy_map_ros/map_out + planner_ros: nav_msgs_path_topic: ompl_rs_planner_ros/nav_msgs_path planning_service_name: ompl_rs_planner_ros/planning_service - path_msg_topic: ompl_rs_planner_ros/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 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_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_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/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/GridMapLazyStateValidator.hpp b/se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp index 74cb82c..cdecd9f 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, TRAVERSABILITY_ITERATOR = 2, ROBUST_TRAVERSABILITY = 3 }; + 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/GridMapStateValidator.hpp b/se2_planning/include/se2_planning/GridMapStateValidator.hpp index 9929cad..7250e75 100644 --- a/se2_planning/include/se2_planning/GridMapStateValidator.hpp +++ b/se2_planning/include/se2_planning/GridMapStateValidator.hpp @@ -47,9 +47,9 @@ class GridMapStateValidator : public StateValidator { bool isFootprintInitialized_ = false; bool isLayerNameInitialized_ = false; - std::string obstacleLayerName_ = ""; grid_map::GridMap gridMap_; RobotFootprint nominalFootprint_; + std::string obstacleLayerName_ = ""; private: mutable RobotFootprint currentFootprint_; 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/include/se2_planning/OmplPlanner.hpp b/se2_planning/include/se2_planning/OmplPlanner.hpp index 5a60cbe..99e3c91 100644 --- a/se2_planning/include/se2_planning/OmplPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplPlanner.hpp @@ -24,6 +24,21 @@ 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 setStateValidator(std::unique_ptr stateValidator) final; + const StateValidator& getStateValidator() const final; + void lockStateValidator() final; + void unlockStateValidator() 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; void setMaxPlanningDuration(double T); void getOmplPath(ompl::geometric::PathGeometric* omplPath) const; @@ -35,15 +50,18 @@ class OmplPlanner : public Planner { void setOmplPlanner(ompl::base::PlannerPtr 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; ompl::base::StateSpacePtr stateSpace_; + std::unique_ptr bounds_; ompl::geometric::SimpleSetupPtr simpleSetup_; 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 ad5485a..ddd5fc2 100644 --- a/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp +++ b/se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp @@ -34,11 +34,8 @@ struct ReedsSheppPath : public Path { }; struct OmplReedsSheppPlannerParameters { + double boundariesMargin_ = 1.0; double turningRadius_ = 10.0; - 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,22 +51,21 @@ 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 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(); - 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; int getDistanceSignAt(const ompl::geometric::PathGeometric& path, unsigned int id) const; - 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..e70a599 100644 --- a/se2_planning/include/se2_planning/Planner.hpp +++ b/se2_planning/include/se2_planning/Planner.hpp @@ -7,7 +7,9 @@ #pragma once +#include "se2_planning/Map.hpp" #include "se2_planning/State.hpp" +#include "se2_planning/StateValidator.hpp" #include namespace se2_planning { @@ -24,8 +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 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 5e69e0b..b6f3ae3 100644 --- a/se2_planning/include/se2_planning/StateValidator.hpp +++ b/se2_planning/include/se2_planning/StateValidator.hpp @@ -8,15 +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 void lock(); + virtual void unlock(); + + private: + boost::mutex mtx_; }; class SE2stateValidator : public StateValidator { 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/GridMapLazyStateValidator.cpp b/se2_planning/src/GridMapLazyStateValidator.cpp index 04b21fd..f67767f 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"); @@ -51,6 +79,7 @@ void GridMapLazyStateValidator::initialize() { if (isUseEarlyStoppingHeuristic_) { addExtraPointsForEarlyStopping(nominalFootprint_, &nominalFootprintPoints_, seed_); } + isInitializeCalled_ = true; } @@ -63,8 +92,21 @@ 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()); - return !isInCollision(se2state, nominalFootprintPoints_, gridMap_, obstacleLayerName_); + switch (stateValidityCheckingMethod_) { + 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_, + 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 +128,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_; @@ -104,14 +146,13 @@ 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) { - 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 +161,6 @@ bool isInCollision(const SE2state& state, const std::vector& footprint, continue; } - const double collisionThreshold = 0.1; if (occupancy > collisionThreshold) { return true; } @@ -129,6 +169,126 @@ bool isInCollision(const SE2state& state, const std::vector& footprint, return false; } +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) { + 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 +329,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..a6cd3f8 100644 --- a/se2_planning/src/GridMapStateValidator.cpp +++ b/se2_planning/src/GridMapStateValidator.cpp @@ -25,6 +25,10 @@ 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"); @@ -32,9 +36,6 @@ void GridMapStateValidator::setGridMap(const grid_map::GridMap& gridMap) { gridMap_ = gridMap; isGridMapInitialized_ = true; } -bool GridMapStateValidator::isInitialized() const { - return isGridMapInitialized_ && isFootprintInitialized_ && isLayerNameInitialized_; -} void GridMapStateValidator::setFootprint(const RobotFootprint& footprint) { nominalFootprint_ = footprint; @@ -59,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; 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*/ diff --git a/se2_planning/src/OmplPlanner.cpp b/se2_planning/src/OmplPlanner.cpp index df3aae0..19d8984 100644 --- a/se2_planning/src/OmplPlanner.cpp +++ b/se2_planning/src/OmplPlanner.cpp @@ -32,9 +32,53 @@ 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); +} + +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(); +} + +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() { simpleSetup_->clear(); simpleSetup_->setStartAndGoalStates(*startState_, *goalState_); + // 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. if (!simpleSetup_->solve(maxPlanningDuration_)) { std::cout << "OmplPlanner: Solve failed" << std::endl; return false; @@ -47,12 +91,13 @@ bool OmplPlanner::plan() { return true; } + bool OmplPlanner::reset() { simpleSetup_->clear(); return true; } + bool OmplPlanner::initialize() { - initializeStateSpace(); if (stateSpace_ == nullptr) { std::cerr << "OmplPlanner:: state space is nullptr" << std::endl; return false; @@ -66,6 +111,19 @@ bool OmplPlanner::initialize() { return true; } +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]; + } +} + +const ompl::base::RealVectorBounds& OmplPlanner::getStateSpaceBoundaries() const { + return *bounds_; +} + void OmplPlanner::setMaxPlanningDuration(double T) { maxPlanningDuration_ = T; } diff --git a/se2_planning/src/OmplReedsSheppPlanner.cpp b/se2_planning/src/OmplReedsSheppPlanner.cpp index adcc1d2..8ffb061 100644 --- a/se2_planning/src/OmplReedsSheppPlanner.cpp +++ b/se2_planning/src/OmplReedsSheppPlanner.cpp @@ -42,45 +42,81 @@ 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 to zero + ompl::base::RealVectorBounds bounds(reedsSheppStateSpaceDim_); + 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() { - bool result = BASE::plan(); + // 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; } -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_); + +const ompl::base::RealVectorBounds& OmplReedsSheppPlanner::getStateSpaceBoundaries() const { + return stateSpace_->as()->getBounds(); } -void OmplReedsSheppPlanner::setStateValidator(std::unique_ptr stateValidator) { - stateValidator_ = std::move(stateValidator); +void OmplReedsSheppPlanner::setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) { + BASE::setStateSpaceBoundaries(bounds); + stateSpace_->as()->setBounds(bounds); } -const StateValidator& OmplReedsSheppPlanner::getStateValidator() const { - return *stateValidator_; +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(); + s->setX(rsState->x_); + s->setY(rsState->y_); + s->setYaw(rsState->yaw_); + return stateSpace_->satisfiesBounds(s); } -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 { + (void)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); } @@ -191,7 +227,6 @@ ReedsSheppState convert(const ompl::base::State* s) { retState.x_ = rsState->getX(); retState.y_ = rsState->getY(); retState.yaw_ = rsState->getYaw(); - return retState; } diff --git a/se2_planning/src/StateValidator.cpp b/se2_planning/src/StateValidator.cpp index 74d38cb..9c523a1 100644 --- a/se2_planning/src/StateValidator.cpp +++ b/se2_planning/src/StateValidator.cpp @@ -20,4 +20,11 @@ bool SE2stateValidator::isStateValid(const State& state) const { return true; } +void StateValidator::lock() { + mtx_.lock(); +} + +void StateValidator::unlock() { + mtx_.unlock(); +} } // namespace se2_planning diff --git a/se2_planning/src/planning_benchmark.cpp b/se2_planning/src/planning_benchmark.cpp index c547b5a..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" @@ -18,6 +19,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(); @@ -26,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); @@ -64,8 +62,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 +72,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 +83,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)); @@ -92,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/CMakeLists.txt b/se2_planning_ros/CMakeLists.txt index 8987cf8..f6e311f 100644 --- a/se2_planning_ros/CMakeLists.txt +++ b/se2_planning_ros/CMakeLists.txt @@ -6,9 +6,11 @@ add_compile_options(-Wall -Wextra -Wpedantic) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(SRC_FILES + src/GridMapLazyStateValidatorRos.cpp src/OmplReedsSheppPlannerRos.cpp - src/loaders.cpp + src/OccupancyMapRos.cpp src/PlannerRos.cpp + src/loaders.cpp src/common.cpp ) @@ -18,6 +20,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..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,21 +1,44 @@ 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: + 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 +map_ros: + layer_name: elevation + grid_map_sub_topic: /se2_grid_map_generator_node/grid_map + grid_map_pub_topic: /occupancy_map_ros/map_out + planner_ros: nav_msgs_path_topic: ompl_rs_planner_ros/nav_msgs_path planning_service_name: ompl_rs_planner_ros/planning_service - path_msg_topic: ompl_rs_planner_ros/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 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..11f76d8 --- /dev/null +++ b/se2_planning_ros/include/se2_planning_ros/GridMapLazyStateValidatorRos.hpp @@ -0,0 +1,74 @@ +/* + * 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 { + +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); + + GridMapLazyStateValidatorRosParameters parameters_; + ros::NodeHandlePtr nh_; + ros::Subscriber mapSubscriber_; + ros::Publisher mapPublisher_; +}; + +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/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 ef56ddc..6b7d5c7 100644 --- a/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp +++ b/se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp @@ -14,13 +14,14 @@ #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 { 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; }; @@ -36,17 +37,21 @@ 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 initializeStateSpaceBoundaryMarker(); bool planningService(PlanningService::Request& req, PlanningService::Response& res) override; - ros::Publisher pathNavMsgsPublisher_; - ros::Publisher pathPublisher_; - OmplReedsSheppPlannerRosParameters parameters_; - ros::ServiceServer planningService_; + const int reedsSheppStateSpaceDim_ = 2; int planSeqNumber_ = -1; + ros::Time planTimeStamp_{0.0}; + OmplReedsSheppPlannerRosParameters parameters_; + visualization_msgs::Marker stateSpaceBoundaryMarker_; }; } /* namespace se2_planning */ diff --git a/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp b/se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp index bee2680..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 { @@ -24,8 +25,22 @@ 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; + + 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; + virtual void publishPathNavMsgs() const; + virtual void publishStartState() const; + virtual void publishGoalState() const; + virtual void publishStateSpaceBoundaryMarker(); protected: using PlanningService = se2_navigation_msgs::RequestPathSrv; @@ -36,6 +51,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/include/se2_planning_ros/loaders.hpp b/se2_planning_ros/include/se2_planning_ros/loaders.hpp index 9262cd6..2f808da 100644 --- a/se2_planning_ros/include/se2_planning_ros/loaders.hpp +++ b/se2_planning_ros/include/se2_planning_ros/loaders.hpp @@ -6,14 +6,19 @@ */ #pragma once + #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); 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..f31b0b2 --- /dev/null +++ b/se2_planning_ros/src/GridMapLazyStateValidatorRos.cpp @@ -0,0 +1,81 @@ +/* + * 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) {} + +void GridMapLazyStateValidatorRos::setParameters(const GridMapLazyStateValidatorRosParameters& parameters) { + parameters_ = parameters; +} + +void GridMapLazyStateValidatorRos::initialize() { + BASE::initialize(); + initRos(); +} + +void GridMapLazyStateValidatorRos::mapCb(const grid_map_msgs::GridMap& msg) { + grid_map::GridMap newMap; + grid_map::GridMapRosConverter::fromMessage(msg, newMap); + + if (newMap.exists(obstacleLayerName_)) { + BASE::lock(); + BASE::setGridMap(newMap); + BASE::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/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/OmplReedsSheppPlannerRos.cpp b/se2_planning_ros/src/OmplReedsSheppPlannerRos.cpp index 4b26c09..9008aaf 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(); + initializeStateSpaceBoundaryMarker(); return result; } bool OmplReedsSheppPlannerRos::plan() { @@ -40,32 +40,93 @@ 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_->getStateValidator().isInitialized()) { + ROS_WARN_STREAM("State validator has not been initialized yet. Abort planning."); + 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; + + // 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); setGoalState(goal); + + // Block update of state validator obstacle map during planning + planner_->lockStateValidator(); + planner_->lockMap(); + + // 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 + if (!planner_->as()->satisfiesStateSpaceBoundaries(start)) { + 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. Enlarged state space boundaries."); + } + + // 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)) { + ROS_WARN_STREAM("Start state (x: " << start.x_ << ", y: " << start.y_ << ", yaw: " << start.yaw_ + << ") not valid. Start planning anyway."); + } + if (!planner_->getStateValidator().isStateValid(goal)) { + ROS_WARN_STREAM("Goal state (x: " << goal.x_ << ", y: " << goal.y_ << ", yaw: " << goal.yaw_ << ") not valid. Start planning anyway."); + } + + publishStartState(); + publishGoalState(); + publishStateSpaceBoundaryMarker(); + + planner_->unlockMap(); + planner_->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; } void OmplReedsSheppPlannerRos::initRos() { + planningService_ = nh_->advertiseService(parameters_.planningServiceName_, &OmplReedsSheppPlannerRos::planningService, this); 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 { - 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 = ros::Time::now(); - 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 { @@ -73,10 +134,64 @@ 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::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_; + ReedsSheppState startState{}; + getStartingState(&startState); + startPose.pose = se2_planning::convert(startState); + startPublisher_.publish(startPose); +} + +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() { + // Set marker info. + // TODO(christoph): This should be extracted from the map reference frame!!! + stateSpaceBoundaryMarker_.header.frame_id = parameters_.pathFrame_; + stateSpaceBoundaryMarker_.header.stamp = planTimeStamp_; + + // Set positions of markers. + const auto bounds = planner_->as()->getStateSpaceBoundaries(); + 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 + stateSpaceBoundaryMarker_.points[4].x = stateSpaceBoundaryMarker_.points[0].x; + stateSpaceBoundaryMarker_.points[4].y = stateSpaceBoundaryMarker_.points[0].y; + + stateSpacePublisher_.publish(stateSpaceBoundaryMarker_); +} + } /* namespace se2_planning */ diff --git a/se2_planning_ros/src/PlannerRos.cpp b/se2_planning_ros/src/PlannerRos.cpp index 411e05e..6038eb6 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,26 +38,65 @@ 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); } +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(); +} + +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 73f883a..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(); } @@ -50,13 +47,73 @@ 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(); 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; +} + +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 == "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', 'traversability_iterator" + "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; +} + void loadRRTstarParameters(const std::string& filename, RRTstarParameters* parameters) { YAML::Node basenode = YAML::LoadFile(filename); diff --git a/se2_planning_ros/src/se2_planner_node.cpp b/se2_planning_ros/src/se2_planner_node.cpp index 44614db..d080e9e 100644 --- a/se2_planning_ros/src/se2_planner_node.cpp +++ b/se2_planning_ros/src/se2_planner_node.cpp @@ -7,29 +7,57 @@ #include +#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" -int main(int argc, char** argv) { - using namespace se2_planning; +using namespace se2_planning; +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); + const auto stateValidatorRosParameters = loadGridMapLazyStateValidatorRosParameters(filename); + const auto mapRosParameters = loadOccupancyMapRosParameters(filename); + + // 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_), + stateValidatorRosParameters.gridMapResolution_, + grid_map::Position(stateValidatorRosParameters.gridMapPositionX_, stateValidatorRosParameters.gridMapPositionY_)); + gridMap.add(stateValidatorRosParameters.gridMapObstacleLayerName_, stateValidatorRosParameters.gridMapDefaultValue_); + + // 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); 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); 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_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 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);