Repository: HKUST-Aerial-Robotics/spatiotemporal_semantic_corridor Branch: master Commit: e7c5d0989abe Files: 20 Total size: 142.1 KB Directory structure: gitextract_76jbbvj5/ ├── readme.md └── ssc_planner/ ├── CMakeLists.txt ├── inc/ │ └── ssc_planner/ │ ├── map_adapter.h │ ├── map_interface.h │ ├── ssc_map.h │ ├── ssc_map_utils.h │ ├── ssc_planner.h │ ├── ssc_server_ros.h │ └── ssc_visualizer.h ├── launch/ │ ├── test_ssc_smm.launch │ └── test_ssc_with_pubg.launch ├── package.xml ├── rviz/ │ └── ssc_config.rviz └── src/ ├── ssc_planner/ │ ├── map_adapter.cc │ ├── ssc_map.cc │ ├── ssc_planner.cc │ ├── ssc_server_ros.cc │ └── ssc_visualizer.cc ├── test_ssc_planner_with_smm.cc └── test_ssc_with_pubg.cc ================================================ FILE CONTENTS ================================================ ================================================ FILE: readme.md ================================================ # Spatio-temporal Semantic Corridor ## 0. News **21 Sept. 2020:** The whole dependencies and a playable demo can be found in: **https://github.com/HKUST-Aerial-Robotics/EPSILON** **31 August 2019:** The code for the ssc planner is available online! **3 July 2019:** Our paper is available online! * **Safe Trajectory Generation for Complex Urban Environments Using Spatio-temporal Semantic Corridor**, Wenchao Ding, Lu Zhang, Jing Chen and Shaojie Shen [IEEE Xplore](https://ieeexplore.ieee.org/document/8740885). *W. Ding and L. Zhang contributed equally to this project.* ``` @article{ding2019safe, title={Safe Trajectory Generation for Complex Urban Environments Using Spatio-temporal Semantic Corridor}, author={Ding, Wenchao and Zhang, Lu and Chen, Jing and Shen, Shaojie}, journal={IEEE Robotics and Automation Letters}, year={2019}, publisher={IEEE} } ``` **What Is Next:** The code for the dependencies of this planner is comming soon! ## 1. Introduction This is the project page for the paper **''Safe Trajectory Generation for Complex Urban Environments Using Spatio-temporal Semantic Corridor''** which is published at IEEE Robotics and Automation Letters (RA-L). This project contains (**already released**): * ssc_map: maintainer for the semantic elements in the spatio-temporal domain. * ssc_planner: planner for generating the semantic corridor in the spatio-temporal domain and optimizing safe and dynamically feasible trajectories. * ssc_server_ros: ros server which manages the replanning. * ssc_visualizer: visualizing the elements both in the spatio-temporal domain (in a separate rviz window) and in the global coordinate. The dependencies of this project includes (**comming soon**): * `common` package: an integration of various mathematical tools such as polynomial, spline, primitive, lane, trajectory, state, optimization solvers, etc. It provides many easy-to-use interfaces for mathematical modeling. * `phy_simulator` package: a configurable multi-agent simulator. It provides ground truth information and listens planner feedbacks. * `semantic_map_manager` package: map with semantic information for vehicle local planning. Each agent is capable of rendering its local planning map based on its configuration. * `vehicle_model` package: basic vehicle models and controllers. * `vehicle_msgs` package: ros communication messages and corresponding encoder and decoders. * `playgrounds` package: test cases/configurations/scenarios stored in json format. * `behavior_planner` package: mpdm behavior planner for on-road driving. It can provide a local reference lane based on navigation information and behavior decision. * `forward_simulator` package: forward simulation * `motion_predictor` package: surrounding vehicle motion prediction. * `route_planner` package: road-level route planner, a simple version. The dependencies will be released in another repo: **https://github.com/HKUST-Aerial-Robotics/HDJI_planning_core**. The overall structure is as follows: ![alt text](fig/overview.png) **Videos:** video ## 2. Prerequisites ## 3. Build ## 4. Usage ## 5. Demos ================================================ FILE: ssc_planner/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8) project(ssc_planner) set(CMAKE_VERBOSE_MAKEFILE "true") set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11 -march=native -g") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall") find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs sensor_msgs) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(OpenMP) if (OPENMP_FOUND) set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") endif() find_package(common REQUIRED) find_package(semantic_map_manager REQUIRED) find_package(behavior_planner REQUIRED) find_package(pubg_planner REQUIRED) #only for debugging purpose find_package(onlane_motion_planner REQUIRED) #if this catkin packge's header is used by other packages, use catkin_package to #declare the include directories of this package. catkin_package( # INCLUDE_DIRS include ) include_directories( inc ${catkin_INCLUDE_DIRS} ${common_INCLUDE_DIRS} ${semantic_map_manager_INCLUDE_DIRS} ${behavior_planner_INCLUDE_DIRS} ${pubg_planner_INCLUDE_DIRS} ${onlane_motion_planner_INCLUDE_DIRS} ) add_library(ssc_planner_lib STATIC src/ssc_planner/ssc_planner.cc src/ssc_planner/ssc_map.cc src/ssc_planner/map_adapter.cc ) target_link_libraries(ssc_planner_lib ${common_LIBRARIES} semantic_map_manager ) add_library(ssc_server_ros STATIC src/ssc_planner/ssc_server_ros.cc src/ssc_planner/ssc_visualizer.cc ) target_link_libraries(ssc_server_ros ssc_planner_lib semantic_map_ros ) add_executable(test_ssc_planner_with_smm src/test_ssc_planner_with_smm.cc ) target_link_libraries(test_ssc_planner_with_smm ${catkin_LIBRARIES} ssc_server_ros behavior_planner_ros onlane_server_ros ) add_executable(test_ssc_with_pubg src/test_ssc_with_pubg.cc ) target_link_libraries(test_ssc_with_pubg ${catkin_LIBRARIES} ssc_server_ros pubg_planner_ros onlane_server_ros ) #install the hearder files so that other packages can include. #install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}/ # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE #) ================================================ FILE: ssc_planner/inc/ssc_planner/map_adapter.h ================================================ /** * @file map_adapter.h * @author HKUST Aerial Robotics Group * @brief map adapter (inherits map interface) for ssc planner * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #ifndef _UTIL_SSC_PLANNER_INC_SSC_PLANNER_MAP_ADAPTER_H__ #define _UTIL_SSC_PLANNER_INC_SSC_PLANNER_MAP_ADAPTER_H__ #include "common/basics/semantics.h" #include "ssc_planner/map_interface.h" #include "semantic_map_manager/semantic_map_manager.h" namespace planning { class SscPlannerAdapter : public SscPlannerMapItf { public: using IntegratedMap = semantic_map_manager::SemanticMapManager; bool IsValid() override; ErrorType GetEgoVehicle(Vehicle* vehicle) override; ErrorType GetEgoState(State* state) override; ErrorType GetEgoReferenceLane(Lane* lane) override; ErrorType GetLocalNavigationLane(Lane* lane) override; ErrorType GetLaneByLaneId(const int lane_id, Lane* lane) override; ErrorType GetSemanticVehicleSet( SemanticVehicleSet* semantic_vehicle_set) override; ErrorType GetObstacleMap(GridMap2D* grid_map) override; ErrorType CheckIfCollision(const common::VehicleParam& vehicle_param, const State& state, bool* res) override; ErrorType GetForwardTrajectories( std::vector* behaviors, vec_E>* trajs) override; ErrorType GetEgoDiscretBehavior(LateralBehavior* lat_behavior) override; ErrorType GetForwardTrajectories( std::vector* behaviors, vec_E>* trajs, vec_E>>* sur_trajs) override; ErrorType GetObstacleGrids( std::set>* obs_grids) override; ErrorType GetSpeedLimit(vec_E* speed_limit_list) override; ErrorType GetTrafficLight( vec_E* traffic_light_list) override; ErrorType set_map(std::shared_ptr map); private: std::shared_ptr map_; bool is_valid_ = false; }; } // namespace planning #endif ================================================ FILE: ssc_planner/inc/ssc_planner/map_interface.h ================================================ /** * @file map_interface.h * @author HKUST Aerial Robotics Group * @brief map interface for ssc planner * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #ifndef _UTIL_SSC_PLANNER_INC_SSC_PLANNER_MAP_INTERFACE_H__ #define _UTIL_SSC_PLANNER_INC_SSC_PLANNER_MAP_INTERFACE_H__ #include #include #include "common/basics/basics.h" #include "common/basics/semantics.h" #include "common/lane/lane.h" #include "common/state/state.h" namespace planning { class SscPlannerMapItf { public: using ObstacleMapType = uint8_t; using State = common::State; using Lane = common::Lane; using Vehicle = common::Vehicle; using LateralBehavior = common::LateralBehavior; using Behavior = common::SemanticBehavior; using SemanticVehicleSet = common::SemanticVehicleSet; using GridMap2D = common::GridMapND; virtual bool IsValid() = 0; virtual ErrorType GetEgoVehicle(Vehicle* vehicle) = 0; virtual ErrorType GetEgoState(State* state) = 0; virtual ErrorType GetEgoReferenceLane(Lane* lane) = 0; virtual ErrorType GetLocalNavigationLane(Lane* lane) = 0; virtual ErrorType GetLaneByLaneId(const int lane_id, Lane* lane) = 0; virtual ErrorType GetSemanticVehicleSet( SemanticVehicleSet* semantic_vehicle_set) = 0; virtual ErrorType GetObstacleMap(GridMap2D* grid_map) = 0; virtual ErrorType CheckIfCollision(const common::VehicleParam& vehicle_param, const State& state, bool* res) = 0; virtual ErrorType GetForwardTrajectories( std::vector* behaviors, vec_E>* trajs) = 0; virtual ErrorType GetForwardTrajectories( std::vector* behaviors, vec_E>* trajs, vec_E>>* sur_trajs) = 0; virtual ErrorType GetEgoDiscretBehavior( LateralBehavior* lat_behavior) = 0; virtual ErrorType GetObstacleGrids( std::set>* obs_grids) = 0; virtual ErrorType GetSpeedLimit( vec_E* speed_limit_list) = 0; virtual ErrorType GetTrafficLight( vec_E* traffic_light_list) = 0; }; } // namespace planning #endif // _UTIL_SSC_PLANNER_INC_SSC_PLANNER_MAP_INTERFACE_H__ ================================================ FILE: ssc_planner/inc/ssc_planner/ssc_map.h ================================================ /** * @file ssc_map.h * @author HKUST Aerial Robotics Group * @brief maintain the spatial temporal map for the planner * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #ifndef _UTIL_SSC_PLANNER_INC_SSC_MAP_H_ #define _UTIL_SSC_PLANNER_INC_SSC_MAP_H_ #include #include #include #include #include #include #include #include #include #include #include #include #include "common/basics/semantics.h" #include "common/state/state.h" namespace planning { using ObstacleMapType = uint8_t; using SscMapDataType = uint8_t; class SscMap { public: using State = common::State; using Lane = common::Lane; using SemanticVehicleSet = common::SemanticVehicleSet; using GridMap3D = common::GridMapND; struct Config { std::array map_size = {{1000, 100, 51}}; // s, d, t std::array map_resolution = {{0.25, 0.2, 0.15}}; // m, m, s std::array axis_name = {{"s", "d", "t"}}; decimal_t s_back_len = 0.0; decimal_t kMaxLongitudinalVel = 30.0; decimal_t kMinLongitudinalVel = 0.0; decimal_t kMaxLateralVel = 2.0; decimal_t kMaxLongitudinalAcc = 2.0; decimal_t kMaxLongitudinalDecel = -3.0; // Avg. driver max decimal_t kMaxLateralAcc = 2.0; decimal_t kMaxLateralDecel = -2.0; }; SscMap() {} SscMap(const Config &config); ~SscMap() {} GridMap3D *p_3d_grid() const { return p_3d_grid_; } GridMap3D *p_3d_inflated_grid() const { return p_3d_inflated_grid_; } Config config() const { return config_; } vec_E driving_corridor_vec() const { return driving_corridor_vec_; } vec_E>> final_cubes_list() const { return final_cubes_list_; }; std::vector> semantic_voxel_set() const { return semantic_voxel_set_; } std::vector if_corridor_valid() const { return if_corridor_valid_; } void set_start_time(const decimal_t &t) { start_time_ = t; } void set_desired_fs(const common::FrenetState &fs) { desired_fs_ = fs; } void set_semantic_cube_set( const std::vector> &in) { semantic_cube_set_ = in; } void set_speed_limit_list(const vec_E &in) { speed_limit_list_ = in; } void set_traffic_light_list(const vec_E &in) { traffic_light_list_ = in; } ErrorType ConstructSscMap( const std::unordered_map> &sur_vehicle_trajs_fs, const vec_E &obstacle_grids); ErrorType InflateObstacleGrid(const common::VehicleParam ¶m); ErrorType ConstructCorridorsUsingInitialTrajectories( GridMap3D *p_grid, const vec_E> &trajs); ErrorType ConstructCorridorUsingInitialTrajectoryWithAssignedBehavior( GridMap3D *p_grid, const vec_E &trajs); ErrorType GetDtUsingTimeStamp(const decimal_t &time_stamp, decimal_t *dt) const; ErrorType ClearDrivingCorridor(); ErrorType GetFinalGlobalMetricCubesList(); private: bool CheckIfCubeIsFree(GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube) const; bool CheckIfPlaneIsFreeOnXAxis(GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube, const int &z) const; bool CheckIfPlaneIsFreeOnYAxis(GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube, const int &z) const; bool CheckIfPlaneIsFreeOnZAxis(GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube, const int &z) const; bool CheckIfCubeContainsSeed(const common::AxisAlignedsCubeNd &cube_a, const Vec3i &seed) const; bool CheckIfIntersectWithSemanticVoxels( const common::AxisAlignedsCubeNd &cube, std::unordered_map> *inters_on_sem_voxels, std::unordered_map> *inters_on_cube) const; bool CheckIfIntersectionsIgnorable( const std::unordered_map> &intersections) const; ErrorType GetSemanticVoxelSet(GridMap3D *p_grid); ErrorType GetInitialCubeUsingSeed(const vec_E &seeds, const int &i, common::AxisAlignedsCubeNd *cube); ErrorType GetInitialCubeUsingSeed( const Vec3i &seed_0, const Vec3i &seed_1, common::AxisAlignedsCubeNd *cube) const; ErrorType GetTimeCoveredCubeIndices( const common::DrivingCorridor3D *p_corridor, const int &start_id, const int &dir, const int &t_trans, std::vector *idx_list) const; ErrorType CorridorRelaxation(GridMap3D *p_grid, common::DrivingCorridor3D *p_corridor); ErrorType FillSemanticInfoInCorridor(GridMap3D *p_grid, common::DrivingCorridor3D *p_corridor); ErrorType InflateCubeIn3dGrid(GridMap3D *p_grid, const std::array &dir_disabled, const std::array &dir_step, common::AxisAlignedsCubeNd *cube); ErrorType GetInflationDirections(const bool &if_inter_with_sv, const bool &if_first_cube, const Vec3i &seed_0, const Vec3i &seed_1, std::array *dirs_disabled); bool InflateCubeOnXPosAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube); bool InflateCubeOnXNegAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube); bool InflateCubeOnYPosAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube); bool InflateCubeOnYNegAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube); bool InflateCubeOnZPosAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube); bool InflateCubeOnZNegAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube); ErrorType FillStaticPart(const vec_E &obs_grid_fs); ErrorType FillDynamicPart( const std::unordered_map> &sur_vehicle_trajs_fs); ErrorType FillMapWithFsVehicleTraj(const vec_E traj); common::GridMapND *p_3d_grid_; common::GridMapND *p_3d_inflated_grid_; std::unordered_map> inters_for_sem_voxels_; std::unordered_map> inters_for_cube_; Config config_; decimal_t start_time_; common::FrenetState desired_fs_; bool map_valid_ = false; std::vector> semantic_cube_set_; std::vector> semantic_voxel_set_; vec_E driving_corridor_vec_; std::vector if_corridor_valid_; vec_E>> final_cubes_list_; // Traffic signal vec_E speed_limit_list_; vec_E traffic_light_list_; }; } // namespace planning #endif // _UTIL_SSC_PLANNER_INC_SSC_MAP_H_ ================================================ FILE: ssc_planner/inc/ssc_planner/ssc_map_utils.h ================================================ /** * @file ssc_map_utils.h * @author HKUST Aerial Robotics Group * @brief utils for map maintenance * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #ifndef _UTIL_SSC_PLANNER_INC_SSC_MAP_UTILS_H__ #define _UTIL_SSC_PLANNER_INC_SSC_MAP_UTILS_H__ #include "ssc_planner/ssc_map.h" #include "common/basics/basics.h" #include "common/basics/semantics.h" #include "common/lane/lane.h" #include "common/state/frenet_state.h" #include "common/state/state.h" #include "common/state/state_transformer.h" namespace planning { class SscMapUtils { public: static ErrorType RetVehicleVerticesUsingState( const common::State& state, const common::VehicleParam& param, vec_E* vertices) { decimal_t angle = state.angle; decimal_t x = state.vec_position(0); decimal_t y = state.vec_position(1); decimal_t cos_theta = cos(angle); decimal_t sin_theta = sin(angle); decimal_t c_x = x + param.d_cr() * cos_theta; decimal_t c_y = y + param.d_cr() * sin_theta; decimal_t d_wx = param.width() / 2 * sin_theta; decimal_t d_wy = param.width() / 2 * cos_theta; decimal_t d_lx = param.length() / 2 * cos_theta; decimal_t d_ly = param.length() / 2 * sin_theta; // Counterclockwise from left-front vertex vertices->push_back(Vec2f(c_x - d_wx + d_lx, c_y + d_wy + d_ly)); // vertices->push_back(Vec2f(c_x - d_wx, c_y + d_wy)); vertices->push_back(Vec2f(c_x - d_wx - d_lx, c_y - d_ly + d_wy)); // vertices->push_back(Vec2f(c_x - d_lx, c_y - d_ly)); vertices->push_back(Vec2f(c_x + d_wx - d_lx, c_y - d_wy - d_ly)); // vertices->push_back(Vec2f(c_x + d_wx, c_y - d_wy)); vertices->push_back(Vec2f(c_x + d_wx + d_lx, c_y + d_ly - d_wy)); // vertices->push_back(Vec2f(c_x + d_lx, c_y + d_ly)); return kSuccess; } }; // SscMapUtils } // namespace planning #endif // _UTIL_SSC_PLANNER_INC_SSC_MAP_UTILS_H__ ================================================ FILE: ssc_planner/inc/ssc_planner/ssc_planner.h ================================================ /** * @file ssc_planner.h * @author HKUST Aerial Robotics Group * @brief planner using spatio-temporal corridor * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #ifndef _UTIL_SSC_PLANNER_INC_SSC_SEARCH_H_ #define _UTIL_SSC_PLANNER_INC_SSC_SEARCH_H_ #include #include #include #include #include "common/basics/basics.h" #include "common/interface/planner.h" #include "common/lane/lane.h" #include "common/spline/spline_generator.h" #include "common/state/frenet_state.h" #include "common/state/state.h" #include "common/state/state_transformer.h" #include "common/trajectory/trajectory.h" #include "ssc_planner/map_interface.h" #include "ssc_planner/ssc_map.h" namespace planning { class SscPlanner : public Planner { public: using ObstacleMapType = uint8_t; using SscMapDataType = uint8_t; using Lane = common::Lane; using State = common::State; using Vehicle = common::Vehicle; using LateralBehavior = common::LateralBehavior; using FrenetState = common::FrenetState; using Trajectory = common::Trajectory; using SemanticVehicleSet = common::SemanticVehicleSet; using GridMap2D = common::GridMapND; typedef common::BezierSpline<5, 2> BezierSpline; struct Config { decimal_t kSampleBezierStep{0.05}; decimal_t kMaxCurvature{0.3}; decimal_t kMaxTangentAcceleration{2.0}; decimal_t kMaxNormalAcceleration{2.0}; decimal_t kMaxTangentDeceleration{2.0}; decimal_t kMaxVelocity{28.0}; }; SscPlanner(); SscPlanner(const Config& cfg); /** * @brief setters */ ErrorType set_map_interface(SscPlannerMapItf* map_itf); ErrorType set_init_state(const State& state); /** * @brief getters */ SscMap* p_ssc_map() const { return p_ssc_map_; } FrenetState ego_frenet_state() const { return ego_frenet_state_; } Vehicle ego_vehicle() const { return ego_vehicle_; } vec_E> forward_trajs() const { return forward_trajs_; } vec_E qp_trajs() const { return qp_trajs_; } decimal_t time_origin() const { return time_origin_; } std::unordered_map> sur_vehicle_trajs_fs() const { return sur_vehicle_trajs_fs_; } vec_E> forward_trajs_fs() const { return forward_trajs_fs_; } vec_E ego_vehicle_contour_fs() const { return fs_ego_vehicle_.vertices; } Trajectory trajectory() const { return trajectory_; } /** * @brief Initialize the planner with config path */ std::string Name() override; /** * @brief Initialize the planner with config path */ ErrorType Init(const std::string config) override; /** * @brief Run one planning round with given states */ ErrorType RunOnce() override; private: ErrorType CorridorFeasibilityCheck( const vec_E>& cubes); /** * @brief transform all the states in a batch */ ErrorType StateTransformForInputData(); ErrorType RunQpOptimization(); /** * @brief transform all the states using openmp */ ErrorType StateTransformUsingOpenMp(const vec_E& global_state_vec, const vec_E& global_point_vec, vec_E* frenet_state_vec, vec_E* fs_point_vec) const; ErrorType StateTransformSingleThread(const vec_E& global_state_vec, const vec_E& global_point_vec, vec_E* frenet_state_vec, vec_E* fs_point_vec) const; ErrorType GetBezierSplineWithCurrentBehavior( const vec_E& trajs, const std::vector& behaviors, BezierSpline* bezier_spline); ErrorType BezierToTrajectory(const BezierSpline& bezier_spline, Trajectory* traj); ErrorType GetSemanticCubeList( const vec_E& speed_limit, const vec_E& traffic_light, std::vector>* cubes); Vehicle ego_vehicle_; LateralBehavior ego_behavior_; FrenetState ego_frenet_state_; Lane nav_lane_local_; decimal_t time_origin_{0.0}; State init_state_; bool has_init_state_ = false; GridMap2D grid_map_; std::set> obstacle_grids_; SemanticVehicleSet semantic_vehicle_set_; vec_E> forward_trajs_; std::vector forward_behaviors_; vec_E>> surround_forward_trajs_; vec_E obstacle_grids_fs_; // Initial solution for optimization common::FsVehicle fs_ego_vehicle_; vec_E> forward_trajs_fs_; std::unordered_map> sur_vehicle_trajs_fs_; vec_E>> surround_forward_trajs_fs_; vec_E qp_trajs_; std::vector valid_behaviors_; Trajectory trajectory_; common::StateTransformer stf_; // Map SscPlannerMapItf* map_itf_; bool map_valid_ = false; SscMap* p_ssc_map_; std::pair last_lane_s_; bool has_last_state_s_ = false; decimal_t global_s_offset_ = 0.0; TicToc timer_; Config config_; }; } // namespace planning #endif ================================================ FILE: ssc_planner/inc/ssc_planner/ssc_server_ros.h ================================================ /** * @file ssc_server_ros.h * @author HKUST Aerial Robotics Group * @brief planner server * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #ifndef _UTIL_SSC_PLANNER_INC_SSC_SERVER_ROS_H_ #define _UTIL_SSC_PLANNER_INC_SSC_SERVER_ROS_H_ #include "ros/ros.h" #include #include #include #include "semantic_map_manager/semantic_map_manager.h" #include "semantic_map_manager/visualizer.h" #include "ssc_planner/map_adapter.h" #include "ssc_planner/ssc_planner.h" #include "ssc_planner/ssc_visualizer.h" #include "common/basics/colormap.h" #include "common/basics/tic_toc.h" #include "common/lane/lane.h" #include "common/lane/lane_generator.h" #include "common/trajectory/trajectory.h" #include "common/visualization/common_visualization_util.h" #include "moodycamel/atomicops.h" #include "moodycamel/readerwriterqueue.h" #include #include "tf/tf.h" #include "tf/transform_datatypes.h" #include "vehicle_msgs/ControlSignal.h" #include "vehicle_msgs/encoder.h" #include "visualization_msgs/Marker.h" #include "visualization_msgs/MarkerArray.h" namespace planning { class SscPlannerServer { public: using SemanticMapManager = semantic_map_manager::SemanticMapManager; struct Config { int kInputBufferSize{100}; }; SscPlannerServer(ros::NodeHandle nh, int ego_id); SscPlannerServer(ros::NodeHandle nh, double work_rate, int ego_id); void PushSemanticMap(const SemanticMapManager &smm); void Init(); void Start(); private: void PlanCycleCallback(); void JoyCallback(const sensor_msgs::Joy::ConstPtr &msg); void Replan(); void PublishData(); void MainThread(); Config config_; bool is_replan_on_ = false; bool is_map_updated_ = false; common::Trajectory executing_traj_; common::Trajectory next_traj_; SscPlanner planner_; SscPlannerAdapter map_adapter_; TicToc time_profile_tool_; decimal_t global_init_stamp_{0.0}; common::VehicleControlSignal joy_ctrl_signal; // ros related ros::NodeHandle nh_; decimal_t work_rate_ = 20.0; int ego_id_; ros::Publisher ctrl_signal_pub_; ros::Publisher map_marker_pub_; ros::Publisher executing_traj_vis_pub_; ros::Subscriber joy_sub_; // input buffer moodycamel::ReaderWriterQueue *p_input_smm_buff_; SemanticMapManager last_smm_; semantic_map_manager::Visualizer *p_smm_vis_{nullptr}; SscVisualizer *p_ssc_vis_{nullptr}; int last_trajmk_cnt_{0}; }; } // namespace planning #endif // _UTIL_SSC_PLANNER_INC_SSC_SERVER_ROS_H__ ================================================ FILE: ssc_planner/inc/ssc_planner/ssc_visualizer.h ================================================ /** * @file ssc_visualizer.h * @author HKUST Aerial Robotics Group * @brief visualizer for the planner * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #ifndef _UTIL_SSC_PLANNER_INC_VISUALIZER_H_ #define _UTIL_SSC_PLANNER_INC_VISUALIZER_H_ #include #include #include #include #include #include #include "common/basics/basics.h" #include "common/basics/semantics.h" #include "common/primitive/frenet_primitive.h" #include "common/state/frenet_state.h" #include "common/state/state.h" #include "common/visualization/common_visualization_util.h" #include "ssc_planner/ssc_planner.h" namespace planning { class SscVisualizer { public: SscVisualizer(ros::NodeHandle nh, int node_id); ~SscVisualizer() {} void VisualizeDataWithStamp(const ros::Time &stamp, const SscPlanner &planner); private: void VisualizeSscMap(const ros::Time &stamp, const SscMap *p_ssc_map); void VisualizeEgoVehicleInSscSpace(const ros::Time &stamp, const vec_E &ego_vehicle_contour_fs, const decimal_t &ego_time); void VisualizeForwardTrajectoriesInSscSpace( const ros::Time &stamp, const vec_E> &trajs, const SscMap *p_ssc_map); void VisualizeQpTrajs(const ros::Time &stamp, const vec_E> &trajs); void VisualizeSurroundingVehicleTrajInSscSpace( const ros::Time &stamp, const std::unordered_map> &trajs, const SscMap *p_ssc_map); void VisualizeCorridorsInSscSpace( const ros::Time &stamp, const vec_E corridor_vec, const SscMap *p_ssc_map); void VisualizeSemanticVoxelSet( const ros::Time &stamp, const std::vector> &voxel_set, const SscMap *p_ssc_map); int last_traj_list_marker_cnt_ = 0; int last_surrounding_vehicle_marker_cnt_ = 0; ros::NodeHandle nh_; int node_id_; decimal_t start_time_; ros::Publisher ssc_map_pub_; ros::Publisher ego_vehicle_pub_; ros::Publisher forward_trajs_pub_; ros::Publisher sur_vehicle_trajs_pub_; ros::Publisher corridor_pub_; ros::Publisher qp_pub_; ros::Publisher semantic_voxel_set_pub_; int last_corridor_mk_cnt = 0; int last_qp_traj_mk_cnt = 0; int last_sur_vehicle_traj_mk_cnt = 0; int last_forward_traj_mk_cnt = 0; decimal_t marker_lifetime_{0.05}; }; // SscVisualizer } // namespace planning #endif // _UTIL_SSC_PLANNER_INC_VISUALIZER_H_ ================================================ FILE: ssc_planner/launch/test_ssc_smm.launch ================================================ ================================================ FILE: ssc_planner/launch/test_ssc_with_pubg.launch ================================================ ================================================ FILE: ssc_planner/package.xml ================================================ ssc_planner 0.0.0 ssc_planner HKUST_UAV_GROUP TODO catkin roscpp rospy common semantic_map_manager roscpp rospy common semantic_map_manager ================================================ FILE: ssc_planner/rviz/ssc_config.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Corridor1 - /Corridor1/Namespaces1 - /QpTraj1 Splitter Ratio: 0.5 Tree Height: 1203 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 5 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /vis/agent_0/ssc/map_vis Name: 3DGridMap Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true Marker Topic: /vis/agent_0/ssc/ego_fs_vis Name: EgoVehicle Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vis/agent_0/ssc/sur_vehicle_trajs_vis Name: SurVehicle Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /vis/agent_0/ssc/forward_trajs_vis Name: ForwardTrajs Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: false Marker Topic: /vis/agent_0/ssc/corridor_vis Name: Corridor Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /vis/agent_0/ssc/qp_vis Name: QpTraj Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /vis/agent_0/ssc/semantic_voxel_vis Name: SemCube Namespaces: "": true Queue Size: 100 Value: true Enabled: true Global Options: Background Color: 255; 255; 255 Default Light: true Fixed Frame: ssc_map Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 68.6894684 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 31.3054199 Y: 3.30344224 Z: 1.9960165 Focal Shape Fixed Size: false Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 Pitch: 0.44979769 Target Frame: ssc_map Value: Orbit (rviz) Yaw: 2.38508368 Saved: ~ Window Geometry: Displays: collapsed: true Height: 877 Hide Left Dock: true Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000016a00000542fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000542000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000327fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000327000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005a00000032700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1440 X: 2370 Y: 54 ================================================ FILE: ssc_planner/src/ssc_planner/map_adapter.cc ================================================ /** * @file map_adpater.cc * @author HKUST Aerial Robotics Group * @brief implementation for map adapter * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #include "ssc_planner/map_adapter.h" namespace planning { ErrorType SscPlannerAdapter::set_map(std::shared_ptr map) { map_ = map; // maintain a snapshop of the environment is_valid_ = true; return kSuccess; } bool SscPlannerAdapter::IsValid() { return is_valid_; } ErrorType SscPlannerAdapter::GetEgoVehicle(Vehicle* vehicle) { if (!is_valid_) return kWrongStatus; *vehicle = map_->ego_vehicle(); return kSuccess; } ErrorType SscPlannerAdapter::GetEgoState(State* state) { if (!is_valid_) return kWrongStatus; *state = map_->ego_vehicle().state(); return kSuccess; } ErrorType SscPlannerAdapter::GetLocalNavigationLane(Lane* lane) { if (!is_valid_) return kWrongStatus; auto ref_lane = map_->ego_behavior().navi_lane_local; if (!ref_lane.IsValid()) { printf("[GetEgoReferenceLane]No reference lane existing.\n"); return kWrongStatus; } *lane = ref_lane; return kSuccess; } ErrorType SscPlannerAdapter::GetForwardTrajectories( std::vector* behaviors, vec_E>* trajs) { if (!is_valid_) return kWrongStatus; if (map_->ego_behavior().forward_behaviors.size() < 1) return kWrongStatus; *behaviors = map_->ego_behavior().forward_behaviors; *trajs = map_->ego_behavior().forward_trajs; return kSuccess; } ErrorType SscPlannerAdapter::GetForwardTrajectories( std::vector* behaviors, vec_E>* trajs, vec_E>>* sur_trajs) { if (!is_valid_) return kWrongStatus; if (map_->ego_behavior().forward_behaviors.size() < 1) return kWrongStatus; *behaviors = map_->ego_behavior().forward_behaviors; *trajs = map_->ego_behavior().forward_trajs; *sur_trajs = map_->ego_behavior().surround_trajs; return kSuccess; } ErrorType SscPlannerAdapter::GetEgoDiscretBehavior( LateralBehavior* lat_behavior) { if (!is_valid_) return kWrongStatus; if (map_->ego_behavior().lat_behavior == common::LateralBehavior::kUndefined) return kWrongStatus; *lat_behavior = map_->ego_behavior().lat_behavior; return kSuccess; } ErrorType SscPlannerAdapter::GetLaneByLaneId(const int lane_id, Lane* lane) { if (!is_valid_) return kWrongStatus; auto semantic_lane_set = map_->semantic_lane_set(); auto it = semantic_lane_set.semantic_lanes.find(lane_id); if (it == semantic_lane_set.semantic_lanes.end()) { return kWrongStatus; } else { *lane = it->second.lane; } return kSuccess; } ErrorType SscPlannerAdapter::GetEgoReferenceLane(Lane* lane) { if (!is_valid_) return kWrongStatus; auto ref_lane = map_->ego_behavior().ref_lane; // auto ref_lane = map_->ego_behavior().navi_lane_local; if (!ref_lane.IsValid()) { printf("[GetEgoReferenceLane]No reference lane existing.\n"); return kWrongStatus; } *lane = ref_lane; return kSuccess; } ErrorType SscPlannerAdapter::GetSemanticVehicleSet( SemanticVehicleSet* semantic_vehicle_set) { if (!is_valid_) return kWrongStatus; *semantic_vehicle_set = map_->s_semantic_vehicles(); return kSuccess; } ErrorType SscPlannerAdapter::GetObstacleMap(GridMap2D* grid_map) { if (!is_valid_) return kWrongStatus; *grid_map = map_->obstacle_map(); return kSuccess; } ErrorType SscPlannerAdapter::CheckIfCollision( const common::VehicleParam& vehicle_param, const State& state, bool* res) { if (!is_valid_) return kWrongStatus; map_->CheckCollisionUsingStateAndVehicleParam(vehicle_param, state, res); return kSuccess; } ErrorType SscPlannerAdapter::GetObstacleGrids( std::set>* obs_grids) { if (!is_valid_) return kWrongStatus; *obs_grids = map_->obstacle_grids(); return kSuccess; } ErrorType SscPlannerAdapter::GetSpeedLimit( vec_E* speed_limit_list) { if (!is_valid_) return kWrongStatus; *speed_limit_list = map_->RetTrafficInfoSpeedLimit(); return kSuccess; } ErrorType SscPlannerAdapter::GetTrafficLight( vec_E* traffic_light_list) { if (!is_valid_) return kWrongStatus; *traffic_light_list = map_->RetTrafficInfoTrafficLight(); return kSuccess; } } // namespace planning ================================================ FILE: ssc_planner/src/ssc_planner/ssc_map.cc ================================================ /** * @file ssc_map.cc * @author HKUST Aerial Robotics Group * @brief implementation for ssc map * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #include "ssc_planner/ssc_map.h" #include "ssc_planner/ssc_map_utils.h" namespace planning { SscMap::SscMap(const SscMap::Config &config) : config_(config) { p_3d_grid_ = new common::GridMapND( config_.map_size, config_.map_resolution, config_.axis_name); p_3d_inflated_grid_ = new common::GridMapND( config_.map_size, config_.map_resolution, config_.axis_name); std::array map_origin = {}; map_origin[0] = 0; //-1 * config_.s_back_len; // s map_origin[1] = -1 * config_.map_size[1] * config_.map_resolution[1] / 2; // d map_origin[2] = 0.0; // t p_3d_grid_->set_origin(map_origin); p_3d_inflated_grid_->set_origin(map_origin); } ErrorType SscMap::GetDtUsingTimeStamp(const decimal_t &time_stamp, decimal_t *dt) const { *dt = time_stamp - start_time_; return kSuccess; } ErrorType SscMap::GetInitialCubeUsingSeed( const Vec3i &seed_0, const Vec3i &seed_1, common::AxisAlignedsCubeNd *cube) const { std::vector lb(3); std::vector ub(3); lb[0] = std::min(seed_0(0), seed_1(0)); lb[1] = std::min(seed_0(1), seed_1(1)); lb[2] = std::min(seed_0(2), seed_1(2)); ub[0] = std::max(seed_0(0), seed_1(0)); ub[1] = std::max(seed_0(1), seed_1(1)); ub[2] = std::max(seed_0(2), seed_1(2)); *cube = common::AxisAlignedsCubeNd(ub, lb); return kSuccess; } ErrorType SscMap::ConstructSscMap( const std::unordered_map> &sur_vehicle_trajs_fs, const vec_E &obstacle_grids) { p_3d_grid_->clear_data(); p_3d_inflated_grid_->clear_data(); FillStaticPart(obstacle_grids); FillDynamicPart(sur_vehicle_trajs_fs); return kSuccess; } ErrorType SscMap::GetInflationDirections(const bool &if_inter_with_sv, const bool &if_first_cube, const Vec3i &seed_0, const Vec3i &seed_1, std::array *dirs_disabled) { bool z_neg_disabled = true; if (if_first_cube) { z_neg_disabled = false; } if (if_inter_with_sv) { for (auto it = inters_for_sem_voxels_.begin(); it != inters_for_sem_voxels_.end(); ++it) { for (int j = 0; j < 6; ++j) { if (it->second[j]) { int dim = j / 2; if (seed_0[dim] < seed_1[dim]) { (*dirs_disabled)[2 * dim] = false; (*dirs_disabled)[2 * dim + 1] = true; } else { (*dirs_disabled)[2 * dim] = true; (*dirs_disabled)[2 * dim + 1] = false; } } } } (*dirs_disabled)[4] = false; (*dirs_disabled)[5] = z_neg_disabled; } else { (*dirs_disabled)[0] = false; (*dirs_disabled)[1] = false; (*dirs_disabled)[2] = false; (*dirs_disabled)[3] = false; (*dirs_disabled)[4] = false; (*dirs_disabled)[5] = z_neg_disabled; } return kSuccess; } ErrorType SscMap::ConstructCorridorsUsingInitialTrajectories( GridMap3D *p_grid, const vec_E> &trajs) { GetSemanticVoxelSet(p_grid); driving_corridor_vec_.clear(); int trajs_num = trajs.size(); if (trajs_num < 1) return kWrongStatus; // ~ Stage I: Get seeds vec_E> seeds_vec; for (int i = 0; i < trajs_num; ++i) { common::DrivingCorridor3D driving_corridor; vec_E traj_seeds; int num_states = static_cast(trajs[i].size()); if (num_states > 1) { bool first_seed_determined = false; for (int k = 0; k < num_states; ++k) { std::array p_w = {}; if (!first_seed_determined) { decimal_t s_0 = desired_fs_.vec_s[0]; decimal_t d_0 = desired_fs_.vec_ds[0]; decimal_t t_0 = 0.0; std::array p_w_0 = {s_0, d_0, t_0}; auto round_p_0 = p_grid->GetRoundedPosUsingGlobalPosition(p_w_0); decimal_t s_1 = trajs[i][k + 1].frenet_state.vec_s[0]; decimal_t d_1 = trajs[i][k + 1].frenet_state.vec_ds[0]; decimal_t t_1 = trajs[i][k + 1].frenet_state.time_stamp - start_time_; std::array p_w_1 = {s_1, d_1, t_1}; auto round_p_1 = p_grid->GetRoundedPosUsingGlobalPosition(p_w_1); if (round_p_1[2] <= 0) { continue; } if ((round_p_0[1] >= d_0 && d_0 >= round_p_1[1]) || (round_p_1[1] >= d_0 && d_0 >= round_p_0[1])) { p_w = p_w_0; } else { if (std::min(round_p_0[1], round_p_1[1]) > d_0) { p_w = p_w_0; p_w[1] -= p_grid->dims_resolution(1); } else if (std::max(round_p_0[1], round_p_1[1]) < d_0) { p_w = p_w_0; p_w[1] += p_grid->dims_resolution(1); } else { assert(false); } } first_seed_determined = true; } else { decimal_t s = trajs[i][k].frenet_state.vec_s[0]; decimal_t d = trajs[i][k].frenet_state.vec_ds[0]; decimal_t t = trajs[i][k].frenet_state.time_stamp - start_time_; p_w = {s, d, t}; } auto coord = p_grid->GetCoordUsingGlobalPosition(p_w); // * remove the states out of range if (!p_grid->CheckCoordInRange(coord)) { continue; } traj_seeds.push_back(Vec3i(coord[0], coord[1], coord[2])); } } if (!traj_seeds.empty()) { seeds_vec.push_back(traj_seeds); } } // ~ Stage II: Inflate cubes TicToc timer; for (const auto &seeds : seeds_vec) { common::DrivingCorridor3D driving_corridor; bool is_valid = true; auto seeds_num = static_cast(seeds.size()); if (seeds_num < 2) { driving_corridor.is_valid = false; driving_corridor_vec_.push_back(driving_corridor); is_valid = false; continue; } for (int i = 0; i < seeds_num; ++i) { if (i == 0) { common::AxisAlignedsCubeNd cube; GetInitialCubeUsingSeed(seeds[i], seeds[i + 1], &cube); if (!CheckIfCubeIsFree(p_grid, cube)) { printf("[error] Init cube is not free, seed id = %d\n", i); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(seeds[i]); driving_cube.seeds.push_back(seeds[i + 1]); driving_corridor.cubes.push_back(driving_cube); driving_corridor.is_valid = false; driving_corridor_vec_.push_back(driving_corridor); is_valid = false; break; } inters_for_sem_voxels_.clear(); inters_for_cube_.clear(); bool if_inter_with_sv = CheckIfIntersectWithSemanticVoxels( cube, &inters_for_sem_voxels_, &inters_for_cube_); // std::array dirs_disabled = { // {false, false, false, false, false, false}}; std::array dirs_disabled; GetInflationDirections(if_inter_with_sv, true, seeds[i], seeds[i + 1], &dirs_disabled); InflateCubeIn3dGrid(p_grid, dirs_disabled, {{20, 1, 10, 10, 1, 1}}, &cube); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(seeds[i]); driving_corridor.cubes.push_back(driving_cube); } else { if (CheckIfCubeContainsSeed(driving_corridor.cubes.back().cube, seeds[i])) { driving_corridor.cubes.back().seeds.push_back(seeds[i]); continue; } else { if (driving_corridor.cubes.back().seeds.size() <= 1) { printf("[Deprecated branch]\n"); printf("[SscQP]find one bad corridor break at %d with %d seeds.\n", i, static_cast(seeds.size())); driving_corridor.is_valid = false; driving_corridor.cubes.back().seeds.push_back(seeds[i]); driving_corridor_vec_.push_back(driving_corridor); is_valid = false; break; } else { // ~ Get the last seed in cube Vec3i seed_r = driving_corridor.cubes.back().seeds.back(); driving_corridor.cubes.back().seeds.pop_back(); // ~ Cut cube on time axis driving_corridor.cubes.back().cube.pos_ub[2] = seed_r(2); i = i - 1; common::AxisAlignedsCubeNd cube; GetInitialCubeUsingSeed(seeds[i], seeds[i + 1], &cube); if (!CheckIfCubeIsFree(p_grid, cube)) { printf("[error] Init cube is not free, seed id = %d\n", i); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(seeds[i]); driving_cube.seeds.push_back(seeds[i + 1]); driving_corridor.cubes.push_back(driving_cube); driving_corridor.is_valid = false; driving_corridor_vec_.push_back(driving_corridor); is_valid = false; break; } inters_for_sem_voxels_.clear(); inters_for_cube_.clear(); bool if_inter_with_sv = CheckIfIntersectWithSemanticVoxels( cube, &inters_for_sem_voxels_, &inters_for_cube_); if (if_inter_with_sv) { for (const auto &inter : inters_for_sem_voxels_) { printf("[Inflation]Ignored: %d -- %d, %d, %d, %d, %d, %d\n", inter.first, inter.second[0], inter.second[1], inter.second[2], inter.second[3], inter.second[4], inter.second[5]); } } // std::array dirs_disabled = { // {false, false, false, false, false, true}}; std::array dirs_disabled; GetInflationDirections(if_inter_with_sv, false, seeds[i], seeds[i + 1], &dirs_disabled); InflateCubeIn3dGrid(p_grid, dirs_disabled, {{20, 1, 10, 10, 1, 1}}, &cube); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(seeds[i]); driving_corridor.cubes.push_back(driving_cube); } } } } if (is_valid) { CorridorRelaxation(p_grid, &driving_corridor); FillSemanticInfoInCorridor(p_grid, &driving_corridor); driving_corridor.cubes.back().cube.pos_ub[2] = seeds.back()(2); driving_corridor.is_valid = true; driving_corridor_vec_.push_back(driving_corridor); } } GetFinalGlobalMetricCubesList(); printf("[Corr] Inflate cube cost = %lf\n", timer.toc()); std::cout << "[Corr] trajs.size() = " << trajs.size() << std::endl; return kSuccess; } ErrorType SscMap::ClearDrivingCorridor() { driving_corridor_vec_.clear(); return kSuccess; } ErrorType SscMap::ConstructCorridorUsingInitialTrajectoryWithAssignedBehavior( GridMap3D *p_grid, const vec_E &trajs) { GetSemanticVoxelSet(p_grid); // ~ Stage I: Get seeds vec_E traj_seeds; int num_states = static_cast(trajs.size()); if (num_states > 1) { bool first_seed_determined = false; for (int k = 0; k < num_states; ++k) { std::array p_w = {}; if (!first_seed_determined) { decimal_t s_0 = desired_fs_.vec_s[0]; decimal_t d_0 = desired_fs_.vec_ds[0]; decimal_t t_0 = 0.0; std::array p_w_0 = {s_0, d_0, t_0}; auto round_p_0 = p_grid->GetRoundedPosUsingGlobalPosition(p_w_0); decimal_t s_1 = trajs[k + 1].frenet_state.vec_s[0]; decimal_t d_1 = trajs[k + 1].frenet_state.vec_ds[0]; decimal_t t_1 = trajs[k + 1].frenet_state.time_stamp - start_time_; std::array p_w_1 = {s_1, d_1, t_1}; auto round_p_1 = p_grid->GetRoundedPosUsingGlobalPosition(p_w_1); if (round_p_1[2] <= 0) { continue; } if ((round_p_0[1] >= d_0 && d_0 >= round_p_1[1]) || (round_p_1[1] >= d_0 && d_0 >= round_p_0[1])) { p_w = p_w_0; } else { if (std::min(round_p_0[1], round_p_1[1]) > d_0) { p_w = p_w_0; p_w[1] -= p_grid->dims_resolution(1); } else if (std::max(round_p_0[1], round_p_1[1]) < d_0) { p_w = p_w_0; p_w[1] += p_grid->dims_resolution(1); } else { assert(false); } } // printf("[Seed] p_w = %lf, %lf, %lf\n", p_w[0], p_w[1], p_w[2]); first_seed_determined = true; } else { decimal_t s = trajs[k].frenet_state.vec_s[0]; decimal_t d = trajs[k].frenet_state.vec_ds[0]; decimal_t t = trajs[k].frenet_state.time_stamp - start_time_; p_w = {s, d, t}; } auto coord = p_grid->GetCoordUsingGlobalPosition(p_w); // * remove the states out of range if (!p_grid->CheckCoordInRange(coord)) { continue; } traj_seeds.push_back(Vec3i(coord[0], coord[1], coord[2])); } } // ~ Stage II: Inflate cubes common::DrivingCorridor3D driving_corridor; bool is_valid = true; auto seed_num = static_cast(traj_seeds.size()); if (seed_num < 2) { driving_corridor.is_valid = false; driving_corridor_vec_.push_back(driving_corridor); is_valid = false; return kWrongStatus; } for (int i = 0; i < seed_num; ++i) { if (i == 0) { common::AxisAlignedsCubeNd cube; GetInitialCubeUsingSeed(traj_seeds[i], traj_seeds[i + 1], &cube); if (!CheckIfCubeIsFree(p_grid, cube)) { printf("[error] Init cube is not free, seed id = %d\n", i); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(traj_seeds[i]); driving_cube.seeds.push_back(traj_seeds[i + 1]); driving_corridor.cubes.push_back(driving_cube); driving_corridor.is_valid = false; driving_corridor_vec_.push_back(driving_corridor); is_valid = false; break; } inters_for_sem_voxels_.clear(); inters_for_cube_.clear(); bool if_inter_with_sv = CheckIfIntersectWithSemanticVoxels( cube, &inters_for_sem_voxels_, &inters_for_cube_); // std::array dirs_disabled = { // {false, false, false, false, false, false}}; std::array dirs_disabled; GetInflationDirections(if_inter_with_sv, true, traj_seeds[i], traj_seeds[i + 1], &dirs_disabled); InflateCubeIn3dGrid(p_grid, dirs_disabled, {{20, 1, 10, 10, 1, 1}}, &cube); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(traj_seeds[i]); driving_corridor.cubes.push_back(driving_cube); } else { if (CheckIfCubeContainsSeed(driving_corridor.cubes.back().cube, traj_seeds[i])) { driving_corridor.cubes.back().seeds.push_back(traj_seeds[i]); continue; } else { if (driving_corridor.cubes.back().seeds.size() <= 1) { // ! CHECK: Still need this condition? printf("[Deprecated branch]\n"); printf("[SscQP]find one bad corridor break at %d with %d seeds.\n", i, static_cast(traj_seeds.size())); driving_corridor.is_valid = false; driving_corridor.cubes.back().seeds.push_back(traj_seeds[i]); driving_corridor_vec_.push_back(driving_corridor); is_valid = false; break; } else { // ~ Get the last seed in cube Vec3i seed_r = driving_corridor.cubes.back().seeds.back(); driving_corridor.cubes.back().seeds.pop_back(); // ~ Cut cube on time axis driving_corridor.cubes.back().cube.pos_ub[2] = seed_r(2); i = i - 1; common::AxisAlignedsCubeNd cube; GetInitialCubeUsingSeed(traj_seeds[i], traj_seeds[i + 1], &cube); if (!CheckIfCubeIsFree(p_grid, cube)) { printf("[error] Init cube is not free, seed id = %d\n", i); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(traj_seeds[i]); driving_cube.seeds.push_back(traj_seeds[i + 1]); driving_corridor.cubes.push_back(driving_cube); driving_corridor.is_valid = false; driving_corridor_vec_.push_back(driving_corridor); is_valid = false; break; } inters_for_sem_voxels_.clear(); inters_for_cube_.clear(); bool if_inter_with_sv = CheckIfIntersectWithSemanticVoxels( cube, &inters_for_sem_voxels_, &inters_for_cube_); if (if_inter_with_sv) { for (const auto &inter : inters_for_sem_voxels_) { printf("[Inflation]Ignored: %d -- %d, %d, %d, %d, %d, %d\n", inter.first, inter.second[0], inter.second[1], inter.second[2], inter.second[3], inter.second[4], inter.second[5]); } } // std::array dirs_disabled = { // {false, false, false, false, false, true}}; std::array dirs_disabled; GetInflationDirections(if_inter_with_sv, false, traj_seeds[i], traj_seeds[i + 1], &dirs_disabled); InflateCubeIn3dGrid(p_grid, dirs_disabled, {{20, 1, 10, 10, 1, 1}}, &cube); common::DrivingCube driving_cube; driving_cube.cube = cube; driving_cube.seeds.push_back(traj_seeds[i]); driving_corridor.cubes.push_back(driving_cube); } } } } if (is_valid) { CorridorRelaxation(p_grid, &driving_corridor); FillSemanticInfoInCorridor(p_grid, &driving_corridor); driving_corridor.cubes.back().cube.pos_ub[2] = traj_seeds.back()(2); driving_corridor.is_valid = true; driving_corridor_vec_.push_back(driving_corridor); } return kSuccess; } ErrorType SscMap::GetTimeCoveredCubeIndices( const common::DrivingCorridor3D *p_corridor, const int &start_idx, const int &dir, const int &t_trans, std::vector *idx_list) const { int dt = 0; int num_cube = p_corridor->cubes.size(); int idx = start_idx; while (idx < num_cube && idx >= 0) { dt += p_corridor->cubes[idx].cube.pos_ub[2] - p_corridor->cubes[idx].cube.pos_lb[2]; idx_list->push_back(idx); if (dir == 1) { ++idx; } else { --idx; } if (dt >= t_trans) { break; } } return kSuccess; } ErrorType SscMap::CorridorRelaxation(GridMap3D *p_grid, common::DrivingCorridor3D *p_corridor) { std::array margin = {{50, 10}}; int t_trans = 7; int num_cube = p_corridor->cubes.size(); for (int i = 0; i < num_cube - 1; ++i) { if (1) // ~ Enable s direction { int cube_0_lb = p_corridor->cubes[i].cube.pos_lb[0]; int cube_0_ub = p_corridor->cubes[i].cube.pos_ub[0]; int cube_1_lb = p_corridor->cubes[i + 1].cube.pos_lb[0]; int cube_1_ub = p_corridor->cubes[i + 1].cube.pos_ub[0]; if (abs(cube_0_ub - cube_1_lb) < margin[0]) { int room = margin[0] - abs(cube_0_ub - cube_1_lb); // ~ Upward std::vector up_idx_list; GetTimeCoveredCubeIndices(p_corridor, i + 1, 1, t_trans, &up_idx_list); // ~ Downward std::vector down_idx_list; GetTimeCoveredCubeIndices(p_corridor, i, 0, t_trans, &down_idx_list); for (const auto &idx : up_idx_list) { InflateCubeOnXNegAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } for (const auto &idx : down_idx_list) { InflateCubeOnXPosAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } } if (abs(cube_0_lb - cube_1_ub) < margin[0]) { int room = margin[0] - abs(cube_0_lb - cube_1_ub); // ~ Upward std::vector up_idx_list; GetTimeCoveredCubeIndices(p_corridor, i + 1, 1, t_trans, &up_idx_list); // ~ Downward std::vector down_idx_list; GetTimeCoveredCubeIndices(p_corridor, i, 0, t_trans, &down_idx_list); for (const auto &idx : up_idx_list) { InflateCubeOnXPosAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } for (const auto &idx : down_idx_list) { InflateCubeOnXNegAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } } } if (1) // ~ Enable d direction { int cube_0_lb = p_corridor->cubes[i].cube.pos_lb[1]; int cube_0_ub = p_corridor->cubes[i].cube.pos_ub[1]; int cube_1_lb = p_corridor->cubes[i + 1].cube.pos_lb[1]; int cube_1_ub = p_corridor->cubes[i + 1].cube.pos_ub[1]; if (abs(cube_0_ub - cube_1_lb) < margin[1]) { int room = margin[1] - abs(cube_0_ub - cube_1_lb); // ~ Upward std::vector up_idx_list; GetTimeCoveredCubeIndices(p_corridor, i + 1, 1, t_trans, &up_idx_list); // ~ Downward std::vector down_idx_list; GetTimeCoveredCubeIndices(p_corridor, i, 0, t_trans, &down_idx_list); for (const auto &idx : up_idx_list) { InflateCubeOnYNegAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } for (const auto &idx : down_idx_list) { InflateCubeOnYPosAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } } if (abs(cube_0_lb - cube_1_ub) < margin[1]) { int room = margin[1] - abs(cube_0_lb - cube_1_ub); // ~ Upward std::vector up_idx_list; GetTimeCoveredCubeIndices(p_corridor, i + 1, 1, t_trans, &up_idx_list); // ~ Downward std::vector down_idx_list; GetTimeCoveredCubeIndices(p_corridor, i, 0, t_trans, &down_idx_list); for (const auto idx : up_idx_list) { InflateCubeOnYPosAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } for (const auto idx : down_idx_list) { InflateCubeOnYNegAxis(p_grid, room, true, &(p_corridor->cubes[idx].cube)); } } } } return kSuccess; } ErrorType SscMap::FillSemanticInfoInCorridor( GridMap3D *p_grid, common::DrivingCorridor3D *p_corridor) { // ~ Use the first seed int num_cube = p_corridor->cubes.size(); for (int i = 0; i < num_cube; ++i) { for (int j = 0; j < static_cast(semantic_voxel_set_.size()); ++j) { if (CheckIfCubeContainsSeed(semantic_voxel_set_[j], p_corridor->cubes[i].seeds[0])) { p_corridor->cubes[i].cube.v_lb = semantic_voxel_set_[j].v_lb; p_corridor->cubes[i].cube.v_ub = semantic_voxel_set_[j].v_ub; break; } } } return kSuccess; } ErrorType SscMap::InflateObstacleGrid(const common::VehicleParam ¶m) { decimal_t s_p_inflate_len = param.length() / 2.0 - param.d_cr(); decimal_t s_n_inflate_len = param.length() - s_p_inflate_len; int num_s_p_inflate_grids = std::floor(s_p_inflate_len / config_.map_resolution[0]); int num_s_n_inflate_grids = std::floor(s_n_inflate_len / config_.map_resolution[0]); int num_d_inflate_grids = std::floor((param.width() - 0.5) / 2.0 / config_.map_resolution[1]); bool is_free = false; for (int i = 0; i < config_.map_size[0]; ++i) { for (int j = 0; j < config_.map_size[1]; ++j) { for (int k = 0; k < config_.map_size[2]; ++k) { std::array coord = {i, j, k}; p_3d_grid_->CheckIfEqualUsingCoordinate(coord, 0, &is_free); if (!is_free) { for (int s = -num_s_n_inflate_grids; s < num_s_p_inflate_grids; s++) { for (int d = -num_d_inflate_grids; d < num_d_inflate_grids; d++) { coord = {i + s, j + d, k}; p_3d_inflated_grid_->SetValueUsingCoordinate(coord, 100); } } } } } } return kSuccess; } ErrorType SscMap::InflateCubeIn3dGrid( GridMap3D *p_grid, const std::array &dir_disabled, const std::array &dir_step, common::AxisAlignedsCubeNd *cube) { bool x_p_finish = dir_disabled[0]; bool x_n_finish = dir_disabled[1]; bool y_p_finish = dir_disabled[2]; bool y_n_finish = dir_disabled[3]; bool z_p_finish = dir_disabled[4]; // bool z_n_finish = dir_disabled[5]; int x_p_step = dir_step[0]; int x_n_step = dir_step[1]; int y_p_step = dir_step[2]; int y_n_step = dir_step[3]; int z_p_step = dir_step[4]; // int z_n_step = dir_step[5]; // int s_idx_ini = cube->pos_lb[0]; // int d_idx_ini = cube->pos_lb[1]; int t_idx_ini = cube->pos_lb[2]; decimal_t t = t_idx_ini * p_grid->dims_resolution(2); decimal_t a_max = 4.7; decimal_t a_min = -4.7; decimal_t s_u = desired_fs_.vec_s[1] * t + 0.5 * a_max * t * t; decimal_t s_l = desired_fs_.vec_s[1] * t + 0.5 * a_min * t * t; int s_idx_u, s_idx_l; p_grid->GetCoordUsingGlobalMetricOnSingleDim(s_u, 0, &s_idx_u); p_grid->GetCoordUsingGlobalMetricOnSingleDim(s_l, 0, &s_idx_l); while (!(x_p_finish && x_n_finish && y_p_finish && y_n_finish)) { if (!x_p_finish) x_p_finish = InflateCubeOnXPosAxis(p_grid, x_p_step, false, cube); if (!x_n_finish) x_n_finish = InflateCubeOnXNegAxis(p_grid, x_n_step, false, cube); if (!y_p_finish) y_p_finish = InflateCubeOnYPosAxis(p_grid, y_p_step, false, cube); if (!y_n_finish) y_n_finish = InflateCubeOnYNegAxis(p_grid, y_n_step, false, cube); if (cube->pos_ub[0] > s_idx_u) x_p_finish = true; if (cube->pos_lb[0] < s_idx_l) x_n_finish = true; if (cube->pos_lb[0] < (p_grid->origin()[0] + 17) / config_.map_resolution[0]) { x_n_finish = true; } } // ~ No need to inflate along z-neg while (!z_p_finish) { if (!z_p_finish) z_p_finish = InflateCubeOnZPosAxis(p_grid, z_p_step, true, cube); if (cube->pos_ub[2] - cube->pos_lb[2] >= 1) { z_p_finish = true; // z_n_finish = true; } } return kSuccess; } bool SscMap::InflateCubeOnXPosAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube) { for (int i = 0; i < n_step; ++i) { int x = cube->pos_ub[0] + 1; if (!p_grid->CheckCoordInRangeOnSingleDim(x, 0)) { return true; } else { if (CheckIfPlaneIsFreeOnXAxis(p_grid, *cube, x)) { // The plane in 3D obstacle grid is free std::unordered_map> inter_res; std::unordered_map> inter_cube; // not used common::AxisAlignedsCubeNd cube_tmp = *cube; cube_tmp.pos_ub[0] = x; if (skip_semantic_cube || !CheckIfIntersectWithSemanticVoxels( cube_tmp, &inter_res, &inter_cube)) { // Without intersect with semantic voxels cube->pos_ub[0] = x; } else { // Intersect with semantic voxels if (CheckIfIntersectionsIgnorable(inter_res)) { // Intersections ignorable cube->pos_ub[0] = x; } else { // Intersections are not ignorable return true; } } } else { // The plane in 3D obstacle grid is not free, finish return true; } } } return false; } bool SscMap::InflateCubeOnXNegAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube) { for (int i = 0; i < n_step; ++i) { int x = cube->pos_lb[0] - 1; if (!p_grid->CheckCoordInRangeOnSingleDim(x, 0)) { return true; } else { if (CheckIfPlaneIsFreeOnXAxis(p_grid, *cube, x)) { // The plane in 3D obstacle grid is free std::unordered_map> inter_res; std::unordered_map> inter_cube; // not used common::AxisAlignedsCubeNd cube_tmp = *cube; cube_tmp.pos_lb[0] = x; if (skip_semantic_cube || !CheckIfIntersectWithSemanticVoxels( cube_tmp, &inter_res, &inter_cube)) { // Without intersect with semantic voxels cube->pos_lb[0] = x; } else { // Intersect with semantic voxels if (CheckIfIntersectionsIgnorable(inter_res)) { // Intersections ignorable cube->pos_lb[0] = x; } else { // Intersections are not ignorable return true; } } } else { return true; } } } return false; } bool SscMap::InflateCubeOnYPosAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube) { for (int i = 0; i < n_step; ++i) { int y = cube->pos_ub[1] + 1; if (!p_grid->CheckCoordInRangeOnSingleDim(y, 1)) { return true; } else { if (CheckIfPlaneIsFreeOnYAxis(p_grid, *cube, y)) { // The plane in 3D obstacle grid is free std::unordered_map> inter_res; std::unordered_map> inter_cube; // not used common::AxisAlignedsCubeNd cube_tmp = *cube; cube_tmp.pos_ub[1] = y; if (skip_semantic_cube || !CheckIfIntersectWithSemanticVoxels( cube_tmp, &inter_res, &inter_cube)) { // Without intersect with semantic voxels cube->pos_ub[1] = y; } else { // Intersect with semantic voxels if (CheckIfIntersectionsIgnorable(inter_res)) { // Intersections ignorable cube->pos_ub[1] = y; } else { // Intersections are not ignorable return true; } } } else { return true; } } } return false; } bool SscMap::InflateCubeOnYNegAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube) { for (int i = 0; i < n_step; ++i) { int y = cube->pos_lb[1] - 1; if (!p_grid->CheckCoordInRangeOnSingleDim(y, 1)) { return true; } else { if (CheckIfPlaneIsFreeOnYAxis(p_grid, *cube, y)) { // The plane in 3D obstacle grid is free std::unordered_map> inter_res; std::unordered_map> inter_cube; // not used common::AxisAlignedsCubeNd cube_tmp = *cube; cube_tmp.pos_lb[1] = y; if (skip_semantic_cube || !CheckIfIntersectWithSemanticVoxels( cube_tmp, &inter_res, &inter_cube)) { // Without intersect with semantic voxels cube->pos_lb[1] = y; } else { // Intersect with semantic voxels if (CheckIfIntersectionsIgnorable(inter_res)) { // Intersections ignorable cube->pos_lb[1] = y; } else { // Intersections are not ignorable return true; } } } else { return true; } } } return false; } bool SscMap::InflateCubeOnZPosAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube) { for (int i = 0; i < n_step; ++i) { int z = cube->pos_ub[2] + 1; if (!p_grid->CheckCoordInRangeOnSingleDim(z, 2)) { return true; } else { if (CheckIfPlaneIsFreeOnZAxis(p_grid, *cube, z)) { // The plane in 3D obstacle grid is free std::unordered_map> inter_res; std::unordered_map> inter_cube; // not used common::AxisAlignedsCubeNd cube_tmp = *cube; cube_tmp.pos_ub[2] = z; if (skip_semantic_cube || !CheckIfIntersectWithSemanticVoxels( cube_tmp, &inter_res, &inter_cube)) { // Without intersect with semantic voxels cube->pos_ub[2] = z; } else { // Intersect with semantic voxels if (CheckIfIntersectionsIgnorable(inter_res)) { // Intersections ignorable cube->pos_ub[2] = z; } else { // Intersections are not ignorable return true; } } } else { return true; } } } return false; } bool SscMap::InflateCubeOnZNegAxis(GridMap3D *p_grid, const int &n_step, const bool &skip_semantic_cube, common::AxisAlignedsCubeNd *cube) { for (int i = 0; i < n_step; ++i) { int z = cube->pos_lb[2] - 1; if (!p_grid->CheckCoordInRangeOnSingleDim(z, 2)) { return true; } else { if (CheckIfPlaneIsFreeOnZAxis(p_grid, *cube, z)) { // The plane in 3D obstacle grid is free std::unordered_map> inter_res; std::unordered_map> inter_cube; // not used common::AxisAlignedsCubeNd cube_tmp = *cube; cube_tmp.pos_lb[2] = z; if (skip_semantic_cube || !CheckIfIntersectWithSemanticVoxels( cube_tmp, &inter_res, &inter_cube)) { // Without intersect with semantic voxels cube->pos_lb[2] = z; } else { // Intersect with semantic voxels if (CheckIfIntersectionsIgnorable(inter_res)) { // Intersections ignorable cube->pos_lb[2] = z; } else { // Intersections are not ignorable return true; } } } else { return true; } } } return false; } ErrorType SscMap::GetSemanticVoxelSet(GridMap3D *p_grid) { // ~ Convert semantic cube (constraints) to voxel coordinate semantic_voxel_set_.clear(); int cube_num = semantic_cube_set_.size(); for (int i = 0; i < cube_num; ++i) { std::array p_ub = {{semantic_cube_set_[i].pos_ub[0], semantic_cube_set_[i].pos_ub[1], semantic_cube_set_[i].pos_ub[2]}}; auto coord_ub = p_grid->GetCoordUsingGlobalPosition(p_ub); std::array p_lb = {{semantic_cube_set_[i].pos_lb[0], semantic_cube_set_[i].pos_lb[1], semantic_cube_set_[i].pos_lb[2]}}; auto coord_lb = p_grid->GetCoordUsingGlobalPosition(p_lb); common::AxisAlignedsCubeNd cube( {coord_ub[0], coord_ub[1], coord_ub[2]}, {coord_lb[0], coord_lb[1], coord_lb[2]}); cube.v_lb = semantic_cube_set_[i].v_lb; cube.v_ub = semantic_cube_set_[i].v_ub; cube.a_lb = semantic_cube_set_[i].a_lb; cube.a_ub = semantic_cube_set_[i].a_ub; semantic_voxel_set_.push_back(cube); } return kSuccess; } bool SscMap::CheckIfCubeIsFree( GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube) const { int f0_min = cube.pos_lb[0]; int f0_max = cube.pos_ub[0]; int f1_min = cube.pos_lb[1]; int f1_max = cube.pos_ub[1]; int f2_min = cube.pos_lb[2]; int f2_max = cube.pos_ub[2]; int i, j, k; std::array coord; bool is_free; for (i = f0_min; i <= f0_max; ++i) { for (j = f1_min; j <= f1_max; ++j) { for (k = f2_min; k <= f2_max; ++k) { coord = {i, j, k}; p_grid->CheckIfEqualUsingCoordinate(coord, 0, &is_free); if (!is_free) { return false; } } } } return true; } bool SscMap::CheckIfPlaneIsFreeOnXAxis( GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube, const int &x) const { int f0_min = cube.pos_lb[1]; int f0_max = cube.pos_ub[1]; int f1_min = cube.pos_lb[2]; int f1_max = cube.pos_ub[2]; int i, j; std::array coord; bool is_free; for (i = f0_min; i <= f0_max; ++i) { for (j = f1_min; j <= f1_max; ++j) { coord = {x, i, j}; p_grid->CheckIfEqualUsingCoordinate(coord, 0, &is_free); if (!is_free) { return false; } } } return true; } bool SscMap::CheckIfPlaneIsFreeOnYAxis( GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube, const int &y) const { int f0_min = cube.pos_lb[0]; int f0_max = cube.pos_ub[0]; int f1_min = cube.pos_lb[2]; int f1_max = cube.pos_ub[2]; int i, j; std::array coord; bool is_free; for (i = f0_min; i <= f0_max; ++i) { for (j = f1_min; j <= f1_max; ++j) { coord = {i, y, j}; p_grid->CheckIfEqualUsingCoordinate(coord, 0, &is_free); if (!is_free) { return false; } } } return true; } bool SscMap::CheckIfPlaneIsFreeOnZAxis( GridMap3D *p_grid, const common::AxisAlignedsCubeNd &cube, const int &z) const { int f0_min = cube.pos_lb[0]; int f0_max = cube.pos_ub[0]; int f1_min = cube.pos_lb[1]; int f1_max = cube.pos_ub[1]; int i, j; std::array coord; bool is_free; for (i = f0_min; i <= f0_max; ++i) { for (j = f1_min; j <= f1_max; ++j) { coord = {i, j, z}; p_grid->CheckIfEqualUsingCoordinate(coord, 0, &is_free); if (!is_free) { return false; } } } return true; } bool SscMap::CheckIfCubeContainsSeed( const common::AxisAlignedsCubeNd &cube_a, const Vec3i &seed) const { for (int i = 0; i < 3; ++i) { if (cube_a.pos_lb[i] > seed(i) || cube_a.pos_ub[i] < seed(i)) { return false; } } return true; } bool SscMap::CheckIfIntersectWithSemanticVoxels( const common::AxisAlignedsCubeNd &cube, std::unordered_map> *inters_on_sem_voxels, std::unordered_map> *inters_on_cube) const { // ~ intersections: id -- results bool if_intersect = false; int num_semantic_voxels = semantic_voxel_set_.size(); for (int i = 0; i < num_semantic_voxels; ++i) { std::array inter_dim_a; std::array inter_dim_b; if (common::ShapeUtils::CheckIfAxisAlignedCubeNdIntersect( cube, semantic_voxel_set_[i], &inter_dim_a, &inter_dim_b)) { inters_on_sem_voxels->insert( std::pair>(i, inter_dim_b)); inters_on_cube->insert( std::pair>(i, inter_dim_a)); if_intersect = true; } } return if_intersect; } bool SscMap::CheckIfIntersectionsIgnorable( const std::unordered_map> &inters) const { for (const auto inter : inters) { int id = inter.first; auto it = inters_for_sem_voxels_.find(id); if (it == inters_for_sem_voxels_.end()) { return false; } else { for (int i = 0; i < 6; ++i) { if (it->second[i] != inter.second[i]) { return false; } } } } return true; } ErrorType SscMap::GetFinalGlobalMetricCubesList() { final_cubes_list_.clear(); if_corridor_valid_.clear(); for (const auto corridor : driving_corridor_vec_) { vec_E> cubes; if (!corridor.is_valid) { if_corridor_valid_.push_back(0); } else { if_corridor_valid_.push_back(1); for (int k = 0; k < static_cast(corridor.cubes.size()); ++k) { common::AxisAlignedsCubeNd cube; decimal_t x_lb, x_ub; decimal_t y_lb, y_ub; decimal_t z_lb, z_ub; p_3d_grid_->GetGlobalMetricUsingCoordOnSingleDim( corridor.cubes[k].cube.pos_lb[0], 0, &x_lb); p_3d_grid_->GetGlobalMetricUsingCoordOnSingleDim( corridor.cubes[k].cube.pos_ub[0], 0, &x_ub); p_3d_grid_->GetGlobalMetricUsingCoordOnSingleDim( corridor.cubes[k].cube.pos_lb[1], 1, &y_lb); p_3d_grid_->GetGlobalMetricUsingCoordOnSingleDim( corridor.cubes[k].cube.pos_ub[1], 1, &y_ub); p_3d_grid_->GetGlobalMetricUsingCoordOnSingleDim( corridor.cubes[k].cube.pos_lb[2], 2, &z_lb); p_3d_grid_->GetGlobalMetricUsingCoordOnSingleDim( corridor.cubes[k].cube.pos_ub[2], 2, &z_ub); cube.pos_lb.push_back(x_lb); cube.pos_lb.push_back(y_lb); cube.pos_lb.push_back(z_lb + start_time_); cube.pos_ub.push_back(x_ub); cube.pos_ub.push_back(y_ub); cube.pos_ub.push_back(z_ub + start_time_); cube.v_lb[0] = config_.kMinLongitudinalVel; cube.v_ub[0] = corridor.cubes[k].cube.v_ub[0]; cube.a_lb[0] = config_.kMaxLongitudinalDecel; cube.a_ub[0] = config_.kMaxLongitudinalAcc; cube.v_lb[1] = -config_.kMaxLateralAcc; cube.v_ub[1] = config_.kMaxLateralAcc; cube.a_lb[1] = config_.kMaxLateralDecel; cube.a_ub[1] = config_.kMaxLateralAcc; if (k == 0) { if (y_lb > desired_fs_.vec_ds[0] || y_ub < desired_fs_.vec_ds[0]) { printf("[Error] error, d = %lf, lb = %lf, ub = %lf\n", desired_fs_.vec_ds[0], y_lb, y_ub); assert(false); } } cubes.push_back(cube); } } final_cubes_list_.push_back(cubes); } return kSuccess; } ErrorType SscMap::FillStaticPart(const vec_E &obs_grid_fs) { for (int i = 0; i < static_cast(obs_grid_fs.size()); ++i) { if (obs_grid_fs[i](0) <= 0) { continue; } for (int k = 0; k < config_.map_size[2]; ++k) { std::array pt = {{obs_grid_fs[i](0), obs_grid_fs[i](1), (double)k * config_.map_resolution[2]}}; auto coord = p_3d_grid_->GetCoordUsingGlobalPosition(pt); if (p_3d_grid_->CheckCoordInRange(coord)) { p_3d_grid_->SetValueUsingCoordinate(coord, 100); } } } return kSuccess; } ErrorType SscMap::FillDynamicPart( const std::unordered_map> &sur_vehicle_trajs_fs) { for (auto it = sur_vehicle_trajs_fs.begin(); it != sur_vehicle_trajs_fs.end(); ++it) { FillMapWithFsVehicleTraj(it->second); } return kSuccess; } ErrorType SscMap::FillMapWithFsVehicleTraj( const vec_E traj) { if (traj.size() == 0) { printf("[SscMap] Trajectory is empty!"); return kWrongStatus; } for (int i = 0; i < static_cast(traj.size()); ++i) { bool is_valid = true; for (const auto v : traj[i].vertices) { if (v(0) <= 0) { is_valid = false; break; } } if (!is_valid) { continue; } decimal_t dt = traj[i].frenet_state.time_stamp - start_time_; int t_idx = 0; std::vector v_coord; std::array p_w; for (const auto v : traj[i].vertices) { p_w = {v(0), v(1), dt}; auto coord = p_3d_grid_->GetCoordUsingGlobalPosition(p_w); t_idx = coord[2]; if (!p_3d_grid_->CheckCoordInRange(coord)) { is_valid = false; break; } v_coord.push_back(common::Point2i(coord[0], coord[1])); } if (!is_valid) { continue; } std::vector> vv_coord_cv; std::vector v_coord_cv; common::ShapeUtils::GetCvPoint2iVecUsingCommonPoint2iVec(v_coord, &v_coord_cv); vv_coord_cv.push_back(v_coord_cv); int w = p_3d_grid_->dims_size()[0]; int h = p_3d_grid_->dims_size()[1]; int layer_offset = t_idx * w * h; cv::Mat layer_mat = cv::Mat(h, w, CV_MAKETYPE(cv::DataType::type, 1), p_3d_grid_->get_data_ptr() + layer_offset); cv::fillPoly(layer_mat, vv_coord_cv, 100); } return kSuccess; } } // namespace planning ================================================ FILE: ssc_planner/src/ssc_planner/ssc_planner.cc ================================================ /** * @file ssc_planner.cc * @author HKUST Aerial Robotics Group * @brief implementation for ssc planner * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #include "ssc_planner/ssc_planner.h" #include "ssc_planner/ssc_map_utils.h" #include #define USE_OPENMP 1 namespace planning { SscPlanner::SscPlanner() { p_ssc_map_ = new SscMap(SscMap::Config()); } SscPlanner::SscPlanner(const Config& cfg) : config_(cfg) { p_ssc_map_ = new SscMap(SscMap::Config()); } std::string SscPlanner::Name() { return std::string("ssc_planner"); } ErrorType SscPlanner::Init(const std::string config) { return kSuccess; } ErrorType SscPlanner::set_init_state(const State& state) { init_state_ = state; has_init_state_ = true; return kSuccess; } ErrorType SscPlanner::RunOnce() { if (map_itf_->GetEgoVehicle(&ego_vehicle_) != kSuccess) { printf("[SscPlanner]fail to get ego vehicle info.\n"); return kWrongStatus; } if (!has_init_state_) { init_state_ = ego_vehicle_.state(); } has_init_state_ = false; if (map_itf_->GetLocalNavigationLane(&nav_lane_local_) != kSuccess) { printf("[SscPlanner]fail to find ego lane.\n"); return kWrongStatus; } stf_ = common::StateTransformer(nav_lane_local_); common::FrenetState fs0; if (stf_.GetFrenetStateFromState(init_state_, &fs0) != kSuccess) { printf("[SscPlanner]fail to get init state frenet state.\n"); return kWrongStatus; } if (map_itf_->GetEgoDiscretBehavior(&ego_behavior_) != kSuccess) { printf("[SscPlanner]fail to get ego behavior.\n"); return kWrongStatus; } if (map_itf_->GetSemanticVehicleSet(&semantic_vehicle_set_) != kSuccess) { printf("[SscPlanner]fail to get semantic vehicle set.\n"); return kWrongStatus; } if (map_itf_->GetObstacleMap(&grid_map_) != kSuccess) { printf("[SscPlanner]fail to get obstacle map.\n"); return kWrongStatus; } if (map_itf_->GetObstacleGrids(&obstacle_grids_) != kSuccess) { printf("[SscPlanner]fail to get obstacle grids.\n"); return kWrongStatus; } if (map_itf_->GetForwardTrajectories(&forward_behaviors_, &forward_trajs_, &surround_forward_trajs_) != kSuccess) { printf("[SscPlanner]fail to get forward trajectories.\n"); return kWrongStatus; } // Traffic signal vec_E speed_limit_list; if (map_itf_->GetSpeedLimit(&speed_limit_list) != kSuccess) { printf("[SscPlanner]fail to get speed limit.\n"); return kWrongStatus; } // p_ssc_map_->set_speed_limit_list(speed_limit_list); vec_E traffic_light_list; if (map_itf_->GetTrafficLight(&traffic_light_list) != kSuccess) { printf("[SscPlanner]fail to get traffic light.\n"); return kWrongStatus; } // p_ssc_map_->set_traffic_light_list(traffic_light_list); TicToc timer_stf; if (StateTransformForInputData() != kSuccess) { return kWrongStatus; } printf("[SscPlanner] Transform time cost: %lf ms\n", timer_stf.toc()); std::vector> cubes; GetSemanticCubeList(speed_limit_list, traffic_light_list, &cubes); p_ssc_map_->set_semantic_cube_set(cubes); p_ssc_map_->set_desired_fs(fs0); if (!has_last_state_s_) { last_lane_s_ = std::make_pair(init_state_, fs0.vec_s[0]); global_s_offset_ = -fs0.vec_s[0]; has_last_state_s_ = true; } else { common::FrenetState new_fs0; stf_.GetFrenetStateFromState(last_lane_s_.first, &new_fs0); global_s_offset_ += last_lane_s_.second - new_fs0.vec_s[0]; last_lane_s_ = std::make_pair(init_state_, fs0.vec_s[0]); } // decimal_t global_s = global_s_offset_ + fs0.vec_s[0]; // decimal_t global_s_lb = std::floor(global_s / 0.25) * 0.25; // decimal_t local_s_lb = global_s_lb - global_s_offset_; p_ssc_map_->p_3d_grid()->clear_data(); p_ssc_map_->p_3d_inflated_grid()->clear_data(); std::array map_origin = {fs0.vec_s[0] - 20, -10.0, 0}; p_ssc_map_->p_3d_grid()->set_origin(map_origin); p_ssc_map_->p_3d_inflated_grid()->set_origin(map_origin); time_origin_ = init_state_.time_stamp; p_ssc_map_->set_start_time(time_origin_); // ~ For naive prediction // TicToc timer_con; // if (p_ssc_map_->ConstructSscMap(sur_vehicle_trajs_fs_, obstacle_grids_fs_)) // { // return kWrongStatus; // } // printf("[SscPlanner] ConstructSscMap time cost: %lf ms\n", // timer_con.toc()); // TicToc timer_infl; // p_ssc_map_->InflateObstacleGrid(ego_vehicle_.param()); // printf("[SscPlanner] InflateObstacleGrid time cost: %lf ms\n", // timer_infl.toc()); // TicToc timer_corridor; // if (p_ssc_map_->ConstructCorridorsUsingInitialTrajectories( // p_ssc_map_->p_3d_inflated_grid(), forward_trajs_fs_) != kSuccess) { // return kWrongStatus; // } // printf("[SscPlanner] Corridor generation time cost: %lf ms\n", // timer_corridor.toc()); // ~ For MPDM prediction p_ssc_map_->ClearDrivingCorridor(); int num_behaviors = forward_behaviors_.size(); for (int i = 0; i < num_behaviors; ++i) { if (p_ssc_map_->ConstructSscMap(surround_forward_trajs_fs_[i], obstacle_grids_fs_)) { return kWrongStatus; } if (p_ssc_map_->ConstructCorridorUsingInitialTrajectoryWithAssignedBehavior( p_ssc_map_->p_3d_inflated_grid(), forward_trajs_fs_[i]) != kSuccess) { return kWrongStatus; } } p_ssc_map_->GetFinalGlobalMetricCubesList(); if (RunQpOptimization() != kSuccess) { printf("[SscQP]fail to optimize qp trajectories.\n"); return kWrongStatus; } BezierSpline bezier_spline; if (GetBezierSplineWithCurrentBehavior(qp_trajs_, valid_behaviors_, &bezier_spline) != kSuccess) { printf("[SscQP]BezierToTrajectory: current behavior %d not valid.\n", static_cast(ego_behavior_)); return kWrongStatus; } if (BezierToTrajectory(bezier_spline, &trajectory_) != kSuccess) { printf("[SscQP]fail to transform bezier to trajectory.\n"); return kWrongStatus; } return kSuccess; } ErrorType SscPlanner::RunQpOptimization() { vec_E>> cube_list = p_ssc_map_->final_cubes_list(); std::vector if_corridor_valid = p_ssc_map_->if_corridor_valid(); if (cube_list.size() < 1) return kWrongStatus; if (cube_list.size() != forward_behaviors_.size()) { printf( "[SscQP]cube list %d not consist with behavior size: %d, forward traj " "%d, flag size %d\n", (int)cube_list.size(), (int)forward_behaviors_.size(), (int)forward_trajs_.size(), (int)if_corridor_valid.size()); return kWrongStatus; } qp_trajs_.clear(); valid_behaviors_.clear(); for (int i = 0; i < static_cast(cube_list.size()); i++) { if (if_corridor_valid[i] == 0) { printf("[error]**** for behavior %s has no valid corridor. ****\n", static_cast(forward_behaviors_[i])); continue; } auto fs_vehicle_traj = forward_trajs_fs_[i]; int num_states = static_cast(fs_vehicle_traj.size()); vec_E> start_constraints; start_constraints.push_back( Vecf<2>(ego_frenet_state_.vec_s[0], ego_frenet_state_.vec_dt[0])); start_constraints.push_back( Vecf<2>(ego_frenet_state_.vec_s[1], ego_frenet_state_.vec_dt[1])); start_constraints.push_back( Vecf<2>(ego_frenet_state_.vec_s[2], ego_frenet_state_.vec_dt[2])); // printf("[Inconsist]Start sd position (%lf, %lf).\n", // start_constraints[0](0), // start_constraints[0](1)); vec_E> end_constraints; end_constraints.push_back( Vecf<2>(fs_vehicle_traj[num_states - 1].frenet_state.vec_s[0], fs_vehicle_traj[num_states - 1].frenet_state.vec_dt[0])); end_constraints.push_back( Vecf<2>(fs_vehicle_traj[num_states - 1].frenet_state.vec_s[1], fs_vehicle_traj[num_states - 1].frenet_state.vec_dt[1])); common::SplineGenerator<5, 2> spline_generator; BezierSpline bezier_spline; if (CorridorFeasibilityCheck(cube_list[i]) != kSuccess) { printf("[SscQP]corridor not valid for optimization.\n"); continue; } std::vector ref_stamps; vec_E> ref_points; for (int n = 0; n < num_states; n++) { ref_stamps.push_back(fs_vehicle_traj[n].frenet_state.time_stamp); ref_points.push_back(Vecf<2>(fs_vehicle_traj[n].frenet_state.vec_s[0], fs_vehicle_traj[n].frenet_state.vec_dt[0])); } if (spline_generator.GetBezierSplineUsingCorridor( cube_list[i], start_constraints, end_constraints, ref_stamps, ref_points, &bezier_spline) != kSuccess) { printf("[error]******** solver error for behavior %d ********.\n", static_cast(forward_behaviors_[i])); for (auto& cube : cube_list[i]) { printf( "[error] x_lb = %f, x_ub = %f, y_lb = %f, y_ub = %f, z_lb = %f, " "z_ub = %f, d_z = %f\n", cube.pos_lb[0], cube.pos_ub[0], cube.pos_lb[1], cube.pos_ub[1], cube.pos_lb[2], cube.pos_ub[2], cube.pos_ub[2] - cube.pos_lb[2]); } printf("[error]Start sd velocity (%lf, %lf).\n", start_constraints[1](0), start_constraints[1](1)); printf("[error]Start sd acceleration (%lf, %lf).\n", start_constraints[2](0), start_constraints[2](1)); printf("[error]End sd position (%lf, %lf).\n", end_constraints[0](0), end_constraints[0](1)); printf("[error]End sd velocity (%lf, %lf).\n", end_constraints[1](0), end_constraints[1](1)); printf("[error]End state stamp: %lf.\n", fs_vehicle_traj[num_states - 1].frenet_state.time_stamp); continue; } // printf("[SscQP]spline begin stamp: %lf.\n", bezier_spline.begin()); qp_trajs_.push_back(bezier_spline); valid_behaviors_.push_back(forward_behaviors_[i]); } return kSuccess; } ErrorType SscPlanner::GetBezierSplineWithCurrentBehavior( const vec_E& trajs, const std::vector& behaviors, BezierSpline* bezier_spline) { int num_valid_behaviors = static_cast(behaviors.size()); if (num_valid_behaviors < 1) { return kWrongStatus; } bool find_exact_match_behavior = false; int index = 0; for (int i = 0; i < num_valid_behaviors; i++) { if (behaviors[i] == ego_behavior_) { find_exact_match_behavior = true; index = i; } } bool find_candidate_behavior = false; LateralBehavior candidate_bahavior = common::LateralBehavior::kLaneKeeping; if (!find_exact_match_behavior) { for (int i = 0; i < num_valid_behaviors; i++) { if (behaviors[i] == candidate_bahavior) { find_candidate_behavior = true; index = i; } } } if (!find_exact_match_behavior && !find_candidate_behavior) return kWrongStatus; // if (!find_exact_match_behavior) return kWrongStatus; *bezier_spline = trajs[index]; return kSuccess; } ErrorType SscPlanner::BezierToTrajectory(const BezierSpline& bezier_spline, Trajectory* traj) { using SplineType = Trajectory::SplineType; using SplineGeneratorType = Trajectory::SplineGeneratorType; const SplineGeneratorType generator; std::vector para; vec_E state_vec; FrenetState fs; State s; Vecf<2> pos, vel, acc; for (decimal_t t = bezier_spline.begin(); t < bezier_spline.end() + kEPS; t += config_.kSampleBezierStep) { bezier_spline.evaluate(t, 0, &pos); bezier_spline.evaluate(t, 1, &vel); bezier_spline.evaluate(t, 2, &acc); fs.Load(Vec3f(pos[0], vel[0], acc[0]), Vec3f(pos[1], vel[1], acc[1]), FrenetState::kInitWithDt); if (stf_.GetStateFromFrenetState(fs, &s) == kSuccess) { para.push_back(t); state_vec.push_back(s); } else { fs.Load(Vec3f(pos[0], 0.0, 0.0), Vec3f(pos[1], 0.0, 0.0), FrenetState::kInitWithDs); stf_.GetStateFromFrenetState(fs, &s); para.push_back(t); state_vec.push_back(s); } } SplineType spline; if (generator.GetSplineFromStateVec(para, state_vec, &spline) != kSuccess) { printf("[SscQP]fail to generate spline from state vec.\n"); return kWrongStatus; } *traj = Trajectory(spline); return kSuccess; } ErrorType SscPlanner::CorridorFeasibilityCheck( const vec_E>& cubes) { int num_cubes = static_cast(cubes.size()); if (num_cubes < 1) { printf("[SscQP]number of cubes not enough.\n"); return kWrongStatus; } for (int i = 1; i < num_cubes; i++) { if (cubes[i].pos_lb[2] != cubes[i - 1].pos_ub[2]) { printf("[SscQP]corridor error.\n"); printf( "[SscQP]Err- x_lb = %f, x_ub = %f, y_lb = %f, y_ub = %f, z_lb = %f, " "z_ub = %f\n", cubes[i - 1].pos_lb[0], cubes[i - 1].pos_ub[0], cubes[i - 1].pos_lb[1], cubes[i - 1].pos_ub[1], cubes[i - 1].pos_lb[2], cubes[i - 1].pos_ub[2]); printf( "[SscQP]Err- x_lb = %f, x_ub = %f, y_lb = %f, y_ub = %f, z_lb = %f, " "z_ub = %f\n", cubes[i].pos_lb[0], cubes[i].pos_ub[0], cubes[i].pos_lb[1], cubes[i].pos_ub[1], cubes[i].pos_lb[2], cubes[i].pos_ub[2]); return kWrongStatus; } } return kSuccess; } ErrorType SscPlanner::StateTransformForInputData() { vec_E global_state_vec; vec_E global_point_vec; int num_v; // ~ Stage I. Package states and points // * Ego vehicle state and vertices { global_state_vec.push_back(init_state_); vec_E v_vec; common::SemanticsUtils::GetVehicleVertices(ego_vehicle_.param(), init_state_, &v_vec); num_v = v_vec.size(); global_point_vec.insert(global_point_vec.end(), v_vec.begin(), v_vec.end()); } // * Ego forward simulation trajs states and vertices { common::VehicleParam ego_param = ego_vehicle_.param(); for (int i = 0; i < (int)forward_trajs_.size(); ++i) { if (forward_trajs_[i].size() < 1) continue; for (int k = 0; k < (int)forward_trajs_[i].size(); ++k) { // states State traj_state = forward_trajs_[i][k].state(); global_state_vec.push_back(traj_state); // vertices vec_E v_vec; SscMapUtils::RetVehicleVerticesUsingState(traj_state, ego_param, &v_vec); global_point_vec.insert(global_point_vec.end(), v_vec.begin(), v_vec.end()); } } } // * Surrounding vehicle trajs states and vertices { for (auto it = semantic_vehicle_set_.semantic_vehicles.begin(); it != semantic_vehicle_set_.semantic_vehicles.end(); ++it) { if (it->second.pred_traj.size() < 1) continue; common::VehicleParam v_param = it->second.vehicle.param(); for (int i = 0; i < (int)it->second.pred_traj.size(); ++i) { // states State traj_state = it->second.pred_traj[i]; global_state_vec.push_back(traj_state); // vertices vec_E v_vec; SscMapUtils::RetVehicleVerticesUsingState(traj_state, v_param, &v_vec); global_point_vec.insert(global_point_vec.end(), v_vec.begin(), v_vec.end()); } } } // * Surrounding vehicle trajs from MPDM { for (int i = 0; i < surround_forward_trajs_.size(); ++i) { for (auto it = surround_forward_trajs_[i].begin(); it != surround_forward_trajs_[i].end(); ++it) { for (int k = 0; k < it->second.size(); ++k) { // states State traj_state = it->second[k].state(); global_state_vec.push_back(traj_state); // vertices vec_E v_vec; SscMapUtils::RetVehicleVerticesUsingState( traj_state, it->second[k].param(), &v_vec); global_point_vec.insert(global_point_vec.end(), v_vec.begin(), v_vec.end()); } } } } // * Obstacle grids { for (auto it = obstacle_grids_.begin(); it != obstacle_grids_.end(); ++it) { Vec2f pt((*it)[0], (*it)[1]); global_point_vec.push_back(pt); } } vec_E frenet_state_vec(global_state_vec.size()); vec_E fs_point_vec(global_point_vec.size()); // ~ Stage II. Do transformation in multi-thread flavor #if USE_OPENMP TicToc timer_stf; StateTransformUsingOpenMp(global_state_vec, global_point_vec, &frenet_state_vec, &fs_point_vec); printf("[SscPlanner] OpenMp transform time cost: %lf ms\n", timer_stf.toc()); #else TicToc timer_stf; StateTransformSingleThread(global_state_vec, global_point_vec, &frenet_state_vec, &fs_point_vec); printf("[SscPlanner] Single thread transform time cost: %lf ms\n", timer_stf.toc()); #endif // ~ Stage III. Retrieve states and points int offset = 0; // * Ego vehicle state and vertices { fs_ego_vehicle_.frenet_state = frenet_state_vec[offset]; fs_ego_vehicle_.vertices.clear(); for (int i = 0; i < num_v; ++i) { fs_ego_vehicle_.vertices.push_back(fs_point_vec[offset * num_v + i]); } offset++; } // * Ego forward simulation trajs states and vertices { forward_trajs_fs_.clear(); if (forward_trajs_.size() < 1) return kWrongStatus; for (int j = 0; j < (int)forward_trajs_.size(); ++j) { if (forward_trajs_[j].size() < 1) assert(false); vec_E traj_fs; for (int k = 0; k < (int)forward_trajs_[j].size(); ++k) { common::FsVehicle fs_v; fs_v.frenet_state = frenet_state_vec[offset]; for (int i = 0; i < num_v; ++i) { fs_v.vertices.push_back(fs_point_vec[offset * num_v + i]); } traj_fs.emplace_back(fs_v); offset++; } forward_trajs_fs_.emplace_back(traj_fs); } } // * Surrounding vehicle trajs states and vertices { sur_vehicle_trajs_fs_.clear(); // if (semantic_vehicle_set_.semantic_vehicles.size() < 1) return // kWrongStatus; for (auto it = semantic_vehicle_set_.semantic_vehicles.begin(); it != semantic_vehicle_set_.semantic_vehicles.end(); ++it) { if (it->second.pred_traj.size() < 1) continue; int v_id = it->first; vec_E traj_fs; for (int k = 0; k < (int)it->second.pred_traj.size(); ++k) { common::FsVehicle fs_v; fs_v.frenet_state = frenet_state_vec[offset]; for (int i = 0; i < num_v; ++i) { fs_v.vertices.push_back(fs_point_vec[offset * num_v + i]); } traj_fs.emplace_back(fs_v); offset++; } sur_vehicle_trajs_fs_.insert( std::pair>(v_id, traj_fs)); } } // // ! Surrounding vehicle trajs from MPDM // { // for (int i = 0; i < surround_forward_trajs_.size(); ++i) { // for (auto it = surround_forward_trajs_[i].begin(); // it != surround_forward_trajs_[i].end(); ++it) { // for (int k = 0; k < it->second.size(); ++k) { // // states // State traj_state = it->second[k].state(); // global_state_vec.push_back(traj_state); // // vertices // vec_E v_vec; // SscMapUtils::RetVehicleVerticesUsingState( // traj_state, it->second[k].param(), &v_vec); // global_point_vec.insert(global_point_vec.end(), v_vec.begin(), // v_vec.end()); // } // } // } // } // * Surrounding vehicle trajs from MPDM { surround_forward_trajs_fs_.clear(); for (int j = 0; j < surround_forward_trajs_.size(); ++j) { std::unordered_map> sur_trajs; for (auto it = surround_forward_trajs_[j].begin(); it != surround_forward_trajs_[j].end(); ++it) { int v_id = it->first; vec_E traj_fs; for (int k = 0; k < it->second.size(); ++k) { common::FsVehicle fs_v; fs_v.frenet_state = frenet_state_vec[offset]; for (int i = 0; i < num_v; ++i) { fs_v.vertices.push_back(fs_point_vec[offset * num_v + i]); } traj_fs.emplace_back(fs_v); offset++; } sur_trajs.insert( std::pair>(v_id, traj_fs)); } surround_forward_trajs_fs_.emplace_back(sur_trajs); } } // * Obstacle grids { obstacle_grids_fs_.clear(); for (int i = 0; i < static_cast(obstacle_grids_.size()); ++i) { obstacle_grids_fs_.push_back(fs_point_vec[offset * num_v + i]); } } ego_frenet_state_ = fs_ego_vehicle_.frenet_state; return kSuccess; } ErrorType SscPlanner::StateTransformUsingOpenMp( const vec_E& global_state_vec, const vec_E& global_point_vec, vec_E* frenet_state_vec, vec_E* fs_point_vec) const { int state_num = global_state_vec.size(); int point_num = global_point_vec.size(); auto ptr_state_vec = frenet_state_vec->data(); auto ptr_point_vec = fs_point_vec->data(); printf("[OpenMp]Total number of queries: %d.\n", state_num + point_num); omp_set_num_threads(4); { #pragma omp parallel for for (int i = 0; i < state_num; ++i) { FrenetState fs; if (kSuccess != stf_.GetFrenetStateFromState(global_state_vec[i], &fs)) { fs.time_stamp = global_state_vec[i].time_stamp; } *(ptr_state_vec + i) = fs; } } { #pragma omp parallel for for (int i = 0; i < point_num; ++i) { Vec2f fs_pt; stf_.GetFrenetPointFromPoint(global_point_vec[i], &fs_pt); *(ptr_point_vec + i) = fs_pt; } } return kSuccess; } ErrorType SscPlanner::StateTransformSingleThread( const vec_E& global_state_vec, const vec_E& global_point_vec, vec_E* frenet_state_vec, vec_E* fs_point_vec) const { int state_num = global_state_vec.size(); int point_num = global_point_vec.size(); auto ptr_state_vec = frenet_state_vec->data(); auto ptr_point_vec = fs_point_vec->data(); { for (int i = 0; i < state_num; ++i) { FrenetState fs; stf_.GetFrenetStateFromState(global_state_vec[i], &fs); *(ptr_state_vec + i) = fs; } } { for (int i = 0; i < point_num; ++i) { Vec2f fs_pt; stf_.GetFrenetPointFromPoint(global_point_vec[i], &fs_pt); *(ptr_point_vec + i) = fs_pt; } } return kSuccess; } ErrorType SscPlanner::set_map_interface(SscPlannerMapItf* map_itf) { if (map_itf == nullptr) return kIllegalInput; map_itf_ = map_itf; map_valid_ = true; return kSuccess; } ErrorType SscPlanner::GetSemanticCubeList( const vec_E& speed_limit, const vec_E& traffic_light, std::vector>* cubes) { // ~ hard code some lateral boundaries (lane) // std::vector lat_boundary_pos = { // {-1.75 - 3.5, -1.75, 1.75, 1.75 + 3.5, 1.75 + 7}}; std::vector lat_boundary_pos = {}; // ~ Convert rules to frenet frame vec_E speed_limit_fs; for (const auto& rule : speed_limit) { Vec2f fp_0; stf_.GetFrenetPointFromPoint(rule.start_point(), &fp_0); Vec2f fp_1; stf_.GetFrenetPointFromPoint(rule.end_point(), &fp_1); common::SpeedLimit sl_fs(fp_0, fp_1, rule.vel_range()); speed_limit_fs.push_back(sl_fs); } int rule_num = speed_limit_fs.size(); std::set lon_boundary_pos_set; for (int i = 0; i < rule_num; ++i) { lon_boundary_pos_set.insert(speed_limit_fs[i].start_point()(0)); lon_boundary_pos_set.insert(speed_limit_fs[i].end_point()(0)); } lon_boundary_pos_set.insert(0); lon_boundary_pos_set.insert(p_ssc_map_->config().map_size[0] * p_ssc_map_->config().map_resolution[0]); std::vector lon_boundary_pos; lon_boundary_pos.assign(lon_boundary_pos_set.begin(), lon_boundary_pos_set.end()); // printf("[Corridor] Lon: "); // for (const auto& lon : lon_boundary_pos) { // printf("%f ", lon); // } // printf("\n"); // printf("[Corridor] Lat: "); // for (const auto& lat : lat_boundary_pos) { // printf("%f ", lat); // } // printf("\n"); for (int i = 0; i < static_cast(lon_boundary_pos.size()) - 1; ++i) { for (int j = 0; j < static_cast(lat_boundary_pos.size()) - 1; ++j) { std::vector ub = {lon_boundary_pos[i + 1], lat_boundary_pos[j + 1], 10}; std::vector lb = {lon_boundary_pos[i], lat_boundary_pos[j], 0}; common::AxisAlignedsCubeNd cube(ub, lb); cubes->push_back(cube); } } for (int i = 0; i < cubes->size(); ++i) { for (int j = 0; j < rule_num; ++j) { if ((*cubes)[i].pos_ub[0] <= speed_limit_fs[j].end_point()(0) && (*cubes)[i].pos_lb[0] >= speed_limit_fs[j].start_point()(0)) { (*cubes)[i].v_lb[0] = speed_limit_fs[j].vel_range()(0); (*cubes)[i].v_ub[0] = speed_limit_fs[j].vel_range()(1); } } } // printf("[Corridor]cubes->size() = %d\n", cubes->size()); // for (int i = 0; i < cubes->size(); ++i) { // printf("[Corridor] %d - s: [%f, %f], d: [%f, %f], v: [%f, %f]\n", i, // (*cubes)[i].pos_lb[0], (*cubes)[i].pos_ub[0], // (*cubes)[i].pos_lb[1], // (*cubes)[i].pos_ub[1], (*cubes)[i].v_lb[0], (*cubes)[i].v_ub[0]); // } return kSuccess; } } // namespace planning ================================================ FILE: ssc_planner/src/ssc_planner/ssc_server_ros.cc ================================================ /** * @file ssc_server.cc * @author HKUST Aerial Robotics Group * @brief implementation for ssc planner server * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #include "ssc_planner/ssc_server_ros.h" namespace planning { SscPlannerServer::SscPlannerServer(ros::NodeHandle nh, int ego_id) : nh_(nh), work_rate_(20.0), ego_id_(ego_id) { p_input_smm_buff_ = new moodycamel::ReaderWriterQueue( config_.kInputBufferSize); p_smm_vis_ = new semantic_map_manager::Visualizer(nh, ego_id); p_ssc_vis_ = new SscVisualizer(nh, ego_id); } SscPlannerServer::SscPlannerServer(ros::NodeHandle nh, double work_rate, int ego_id) : nh_(nh), work_rate_(work_rate), ego_id_(ego_id) { p_input_smm_buff_ = new moodycamel::ReaderWriterQueue( config_.kInputBufferSize); p_smm_vis_ = new semantic_map_manager::Visualizer(nh, ego_id); p_ssc_vis_ = new SscVisualizer(nh, ego_id); } void SscPlannerServer::PushSemanticMap(const SemanticMapManager& smm) { if (p_input_smm_buff_) p_input_smm_buff_->try_enqueue(smm); } void SscPlannerServer::PublishData() { using common::VisualizationUtil; auto current_time = ros::Time::now().toSec(); // smm visualization { p_smm_vis_->VisualizeDataWithStamp(ros::Time(current_time), last_smm_); p_smm_vis_->SendTfWithStamp(ros::Time(current_time), last_smm_); } // ssc visualization { TicToc timer; p_ssc_vis_->VisualizeDataWithStamp(ros::Time(current_time), planner_); printf("[ssc] ssc vis all time cost: %lf ms\n", timer.toc()); } // trajectory feedback { if (!executing_traj_.IsValid()) return; decimal_t plan_horizon = 1.0 / work_rate_; int num_cycles = std::round((current_time - executing_traj_.begin()) / plan_horizon); decimal_t ct = executing_traj_.begin() + num_cycles * plan_horizon; { common::State state; if (executing_traj_.GetState(ct, &state) == kSuccess) { // TODO: @(denny.ding) how to deal with low speed singularity decimal_t output_angle_diff = fabs(normalize_angle( state.angle - last_smm_.ego_vehicle().state().angle)); if (output_angle_diff > 0.1 || last_smm_.ego_vehicle().state().velocity < 0.1) { // ! this check will be done in the real controller later state.angle = last_smm_.ego_vehicle().state().angle; } vehicle_msgs::ControlSignal ctrl_msg; vehicle_msgs::Encoder::GetRosControlSignalFromControlSignal( common::VehicleControlSignal(state), ros::Time(ct), std::string("map"), &ctrl_msg); ctrl_signal_pub_.publish(ctrl_msg); } else { printf( "[SscPlannerServer]cannot evaluate state at %lf with begin %lf.\n", ct, executing_traj_.begin()); } } // trajectory visualization { visualization_msgs::MarkerArray traj_mk_arr; common::VisualizationUtil::GetMarkerArrayByTrajectory( executing_traj_, 0.1, Vecf<3>(0.3, 0.3, 0.3), common::cmap["magenta"], 0.5, &traj_mk_arr); int num_traj_mks = static_cast(traj_mk_arr.markers.size()); common::VisualizationUtil::FillHeaderIdInMarkerArray( ros::Time(current_time), std::string("/map"), last_trajmk_cnt_, &traj_mk_arr); last_trajmk_cnt_ = num_traj_mks; executing_traj_vis_pub_.publish(traj_mk_arr); } } } void SscPlannerServer::Init() { std::string traj_topic = std::string("/vis/agent_") + std::to_string(ego_id_) + std::string("/ssc/exec_traj"); joy_sub_ = nh_.subscribe("/joy", 10, &SscPlannerServer::JoyCallback, this); ctrl_signal_pub_ = nh_.advertise("ctrl", 20); map_marker_pub_ = nh_.advertise("ssc_map", 1); executing_traj_vis_pub_ = nh_.advertise(traj_topic, 1); } void SscPlannerServer::JoyCallback(const sensor_msgs::Joy::ConstPtr& msg) {} void SscPlannerServer::Start() { if (is_replan_on_) { return; } planner_.set_map_interface(&map_adapter_); printf("[SscPlannerServer]Planner server started.\n"); is_replan_on_ = true; std::thread(&SscPlannerServer::MainThread, this).detach(); } void SscPlannerServer::MainThread() { using namespace std::chrono; system_clock::time_point current_start_time{system_clock::now()}; system_clock::time_point next_start_time{current_start_time}; const milliseconds interval{static_cast(1000.0 / work_rate_)}; while (true) { current_start_time = system_clock::now(); next_start_time = current_start_time + interval; PlanCycleCallback(); std::this_thread::sleep_until(next_start_time); } } void SscPlannerServer::PlanCycleCallback() { if (!is_replan_on_) { return; } // printf("input buffer size: %d.\n", p_input_smm_buff_->size_approx()); while (p_input_smm_buff_->try_dequeue(last_smm_)) { is_map_updated_ = true; } if (!is_map_updated_) return; // Update map auto map_ptr = std::make_shared(last_smm_); map_adapter_.set_map(map_ptr); PublishData(); auto current_time = ros::Time::now().toSec(); printf("[PlanCycleCallback]>>>>>>>current time %lf.\n", current_time); if (!executing_traj_.IsValid()) { // Init planning if (planner_.RunOnce() != kSuccess) { printf("[SscPlannerServer]Initial planning failed.\n"); return; } executing_traj_ = planner_.trajectory(); global_init_stamp_ = executing_traj_.begin(); printf("[SscPlannerServer]init plan success with stamp: %lf.\n", global_init_stamp_); return; } if (current_time > executing_traj_.end()) { printf("[SscPlannerServer]Current time %lf out of [%lf, %lf].\n", current_time, executing_traj_.begin(), executing_traj_.end()); printf("[SscPlannerServer]Mission complete.\n"); // is_replan_on_ = false; executing_traj_ = common::Trajectory(); next_traj_ = common::Trajectory(); return; } // NOTE: comment this block if you want to disable replan { if (!next_traj_.IsValid()) { Replan(); return; } if (next_traj_.IsValid()) { executing_traj_ = next_traj_; next_traj_ = common::Trajectory(); Replan(); return; } } } void SscPlannerServer::Replan() { if (!is_replan_on_) return; if (!executing_traj_.IsValid()) return; decimal_t plan_horizon = 1.0 / work_rate_; common::State desired_state; decimal_t cur_time = ros::Time::now().toSec(); int num_cycles_exec = std::round((executing_traj_.begin() - global_init_stamp_) / plan_horizon); int num_cycles_ahead = cur_time > executing_traj_.begin() ? std::floor((cur_time - global_init_stamp_) / plan_horizon) + 1 : num_cycles_exec + 1; decimal_t t = global_init_stamp_ + plan_horizon * num_cycles_ahead; printf( "[SscPlannerServer]init stamp: %lf, plan horizon: %lf, num cycles %d.\n", global_init_stamp_, plan_horizon, num_cycles_ahead); printf( "[SscPlannerServer]Replan at cur time %lf with executing traj begin " "time: %lf to actual t: %lf.\n", cur_time, executing_traj_.begin(), t); if (executing_traj_.GetState(t, &desired_state) != kSuccess) { printf("[SscPlannerServer]Cannot get desired state at %lf.\n", t); return; } desired_state.time_stamp = t; // TODO: (@denny.ding)remove this code! decimal_t output_angle_diff = fabs(normalize_angle( desired_state.angle - last_smm_.ego_vehicle().state().angle)); if (output_angle_diff > 0.1 || last_smm_.ego_vehicle().state().velocity < 0.3) { // ! this check will be done in the real controller later desired_state.angle = last_smm_.ego_vehicle().state().angle; } planner_.set_init_state(desired_state); printf("[SscPlannerServer]desired state time: %lf.\n", t); time_profile_tool_.tic(); if (planner_.RunOnce() != kSuccess) { printf("[Summary]Ssc planner core failed in %lf ms.\n", time_profile_tool_.toc()); return; } printf("[Summary]Ssc planner succeed in %lf ms.\n", time_profile_tool_.toc()); next_traj_ = planner_.trajectory(); printf("[SscPlannerServer]desired t: %lf, actual t: %lf.\n", t, next_traj_.begin()); } } // namespace planning ================================================ FILE: ssc_planner/src/ssc_planner/ssc_visualizer.cc ================================================ /** * @file ssc_visualizer.cc * @author HKUST Aerial Robotics Group * @brief implementation for ssc visualizer * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #include "ssc_planner/ssc_visualizer.h" #include "ssc_planner/ssc_map_utils.h" namespace planning { SscVisualizer::SscVisualizer(ros::NodeHandle nh, int node_id) : nh_(nh), node_id_(node_id) { std::cout << "node_id_ = " << node_id_ << std::endl; std::string ssc_map_vis_topic = std::string("/vis/agent_") + std::to_string(node_id_) + std::string("/ssc/map_vis"); std::string ego_vehicle_vis_topic = std::string("/vis/agent_") + std::to_string(node_id_) + std::string("/ssc/ego_fs_vis"); std::string forward_trajs_vis_topic = std::string("/vis/agent_") + std::to_string(node_id_) + std::string("/ssc/forward_trajs_vis"); std::string sur_vehicle_trajs_vis_topic = std::string("/vis/agent_") + std::to_string(node_id_) + std::string("/ssc/sur_vehicle_trajs_vis"); std::string corridor_vis_topic = std::string("/vis/agent_") + std::to_string(node_id_) + std::string("/ssc/corridor_vis"); std::string sem_voxel_vis_topic = std::string("/vis/agent_") + std::to_string(node_id_) + std::string("/ssc/semantic_voxel_vis"); std::string qp_vis_topic = std::string("/vis/agent_") + std::to_string(node_id_) + std::string("/ssc/qp_vis"); ssc_map_pub_ = nh_.advertise(ssc_map_vis_topic, 1); qp_pub_ = nh_.advertise(qp_vis_topic, 1); ego_vehicle_pub_ = nh_.advertise(ego_vehicle_vis_topic, 1); forward_trajs_pub_ = nh_.advertise( forward_trajs_vis_topic, 1); sur_vehicle_trajs_pub_ = nh_.advertise( sur_vehicle_trajs_vis_topic, 1); corridor_pub_ = nh_.advertise(corridor_vis_topic, 1); semantic_voxel_set_pub_ = nh_.advertise(sem_voxel_vis_topic, 1); } void SscVisualizer::VisualizeDataWithStamp(const ros::Time &stamp, const SscPlanner &planner) { start_time_ = planner.time_origin(); std::cout << "[SscMapTime] stamp = " << stamp << std::endl; VisualizeSscMap(stamp, planner.p_ssc_map()); VisualizeEgoVehicleInSscSpace(stamp, planner.ego_vehicle_contour_fs(), planner.ego_vehicle().state().time_stamp); VisualizeForwardTrajectoriesInSscSpace(stamp, planner.forward_trajs_fs(), planner.p_ssc_map()); VisualizeSurroundingVehicleTrajInSscSpace( stamp, planner.sur_vehicle_trajs_fs(), planner.p_ssc_map()); VisualizeCorridorsInSscSpace( stamp, planner.p_ssc_map()->driving_corridor_vec(), planner.p_ssc_map()); VisualizeQpTrajs(stamp, planner.qp_trajs()); VisualizeSemanticVoxelSet(stamp, planner.p_ssc_map()->semantic_voxel_set(), planner.p_ssc_map()); } void SscVisualizer::VisualizeQpTrajs( const ros::Time &stamp, const vec_E> &trajs) { if (trajs.size() < 1) { printf("[SscQP]No valid qp trajs.\n"); return; } int id = 0; visualization_msgs::MarkerArray traj_mk_arr; for (int i = 0; i < static_cast(trajs.size()); i++) { visualization_msgs::Marker traj_mk; traj_mk.type = visualization_msgs::Marker::LINE_STRIP; traj_mk.action = visualization_msgs::Marker::MODIFY; traj_mk.id = id++; Vecf<2> pos; for (decimal_t t = trajs[i].begin(); t < trajs[i].end() + kEPS; t += 0.02) { if (trajs[i].evaluate(t, 0, &pos) == kSuccess) { geometry_msgs::Point pt; pt.x = pos[0]; pt.y = pos[1]; pt.z = t - start_time_; traj_mk.points.push_back(pt); } } common::VisualizationUtil::FillScaleColorInMarker( Vec3f(0.2, 0.2, 0.2), common::cmap.at("magenta"), &traj_mk); traj_mk_arr.markers.push_back(traj_mk); } int num_markers = static_cast(traj_mk_arr.markers.size()); common::VisualizationUtil::FillHeaderIdInMarkerArray( stamp, std::string("ssc_map"), last_qp_traj_mk_cnt, &traj_mk_arr); qp_pub_.publish(common::VisualizationUtil::GetDeletionMarkerArray()); qp_pub_.publish(traj_mk_arr); last_qp_traj_mk_cnt = num_markers; } void SscVisualizer::VisualizeSscMap(const ros::Time &stamp, const SscMap *p_ssc_map) { visualization_msgs::MarkerArray map_marker_arr; visualization_msgs::Marker map_marker; common::VisualizationUtil::GetRosMarkerCubeListUsingGripMap3D( p_ssc_map->p_3d_grid(), stamp, "ssc_map", Vec3f(0, 0, 0), &map_marker); decimal_t s_len = p_ssc_map->config().map_resolution[0] * p_ssc_map->config().map_size[0]; // decimal_t d_len = // p_ssc_map->config().map_resolution[1] * p_ssc_map->config().map_size[1]; // decimal_t t_len = // p_ssc_map->config().map_resolution[2] * p_ssc_map->config().map_size[2]; decimal_t x = s_len / 2 - p_ssc_map->config().s_back_len; decimal_t y = 0; // decimal_t z = t_len / 2; std::array aabb_coord = {x, y, 0}; std::array aabb_len = {s_len, 3.5, 0.01}; common::AxisAlignedBoundingBoxND<3> map_aabb(aabb_coord, aabb_len); visualization_msgs::Marker map_aabb_marker; map_aabb_marker.header.frame_id = "ssc_map"; map_aabb_marker.header.stamp = stamp; map_aabb_marker.id = 1; common::VisualizationUtil::GetRosMarkerCubeUsingAxisAlignedBoundingBox3D( map_aabb, common::ColorARGB(0.2, 1, 0, 1), &map_aabb_marker); map_marker_arr.markers.push_back(map_marker); map_marker_arr.markers.push_back(map_aabb_marker); ssc_map_pub_.publish(map_marker_arr); } void SscVisualizer::VisualizeEgoVehicleInSscSpace( const ros::Time &stamp, const vec_E &ego_vehicle_contour_fs, const decimal_t &ego_time) { if (ego_vehicle_contour_fs.size() == 0) return; visualization_msgs::Marker ego_vehicle_marker; common::ColorARGB color(0.8, 1.0, 0.0, 0.0); decimal_t dt = ego_time - start_time_; vec_E contour = ego_vehicle_contour_fs; contour.push_back(contour.front()); common::VisualizationUtil::GetRosMarkerLineStripUsing2DofVecWithOffsetZ( contour, color, Vec3f(0.3, 0.3, 0.3), dt, 0, &ego_vehicle_marker); ego_vehicle_marker.header.frame_id = "ssc_map"; ego_vehicle_marker.header.stamp = stamp; ego_vehicle_pub_.publish(ego_vehicle_marker); } void SscVisualizer::VisualizeForwardTrajectoriesInSscSpace( const ros::Time &stamp, const vec_E> &trajs, const SscMap *p_ssc_map) { if (trajs.size() < 1) return; visualization_msgs::MarkerArray trajs_markers; int id_cnt = 0; // common::ColorARGB color = common::cmap.at("gold"); // color.a = 0.4; for (int i = 0; i < static_cast(trajs.size()); ++i) { if (trajs[i].size() < 1) continue; for (int k = 0; k < static_cast(trajs[i].size()); ++k) { visualization_msgs::Marker vehicle_marker; vec_E contour = trajs[i][k].vertices; bool is_valid = true; for (const auto v : contour) { if (v(0) <= 0) { is_valid = false; break; } } if (!is_valid) { continue; } common::ColorARGB color = common::GetJetColorByValue( static_cast(k), static_cast(trajs[i].size() - 1), 0.0); contour.push_back(contour.front()); decimal_t dt = trajs[i][k].frenet_state.time_stamp - start_time_; common::VisualizationUtil::GetRosMarkerLineStripUsing2DofVecWithOffsetZ( contour, color, Vec3f(0.1, 0.1, 0.1), dt, id_cnt++, &vehicle_marker); trajs_markers.markers.push_back(vehicle_marker); } } int num_markers = static_cast(trajs_markers.markers.size()); common::VisualizationUtil::FillHeaderIdInMarkerArray( stamp, std::string("ssc_map"), last_forward_traj_mk_cnt, &trajs_markers); forward_trajs_pub_.publish( common::VisualizationUtil::GetDeletionMarkerArray()); forward_trajs_pub_.publish(trajs_markers); last_forward_traj_mk_cnt = num_markers; } void SscVisualizer::VisualizeSurroundingVehicleTrajInSscSpace( const ros::Time &stamp, const std::unordered_map> &trajs, const SscMap *p_ssc_map) { if (trajs.size() < 1) return; visualization_msgs::MarkerArray trajs_markers; int id_cnt = 0; for (auto it = trajs.begin(); it != trajs.end(); ++it) { if (it->second.size() < 1) continue; for (int k = 0; k < static_cast(it->second.size()); ++k) { visualization_msgs::Marker vehicle_marker; common::ColorARGB color = common::GetJetColorByValue( static_cast(k), static_cast(it->second.size() - 1), 0.0); vec_E contour = it->second[k].vertices; bool is_valid = true; for (const auto v : contour) { if (v(0) <= 0) { is_valid = false; break; } } if (!is_valid) { continue; } contour.push_back(contour.front()); decimal_t dt = it->second[k].frenet_state.time_stamp - start_time_; common::VisualizationUtil::GetRosMarkerLineStripUsing2DofVecWithOffsetZ( contour, color, Vec3f(0.1, 0.1, 0.1), dt, id_cnt++, &vehicle_marker); vehicle_marker.header.frame_id = "ssc_map"; vehicle_marker.header.stamp = stamp; trajs_markers.markers.push_back(vehicle_marker); } } int num_markers = static_cast(trajs_markers.markers.size()); common::VisualizationUtil::FillHeaderIdInMarkerArray( stamp, std::string("ssc_map"), last_sur_vehicle_traj_mk_cnt, &trajs_markers); sur_vehicle_trajs_pub_.publish( common::VisualizationUtil::GetDeletionMarkerArray()); sur_vehicle_trajs_pub_.publish(trajs_markers); last_sur_vehicle_traj_mk_cnt = num_markers; } void SscVisualizer::VisualizeCorridorsInSscSpace( const ros::Time &stamp, const vec_E corridor_vec, const SscMap *p_ssc_map) { if (corridor_vec.size() < 1) return; visualization_msgs::MarkerArray corridor_vec_marker; int id_cnt = 0; int ns_cnt = 0; for (const auto &corridor : corridor_vec) { // if (corridor.is_valid) continue; int cube_cnt = 0; for (const auto &driving_cube : corridor.cubes) { // * Show seeds common::ColorARGB color = common::GetJetColorByValue( cube_cnt, corridor.cubes.size() - 1, -kEPS); for (const auto &seed : driving_cube.seeds) { visualization_msgs::Marker seed_marker; decimal_t s_x, s_y, s_z; p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim(seed(0), 0, &s_x); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim(seed(1), 1, &s_y); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim(seed(2), 2, &s_z); common::VisualizationUtil::GetRosMarkerSphereUsingPoint( Vec3f(s_x, s_y, s_z), color, Vec3f(0.3, 0.3, 0.3), id_cnt++, &seed_marker); seed_marker.ns = std::to_string(ns_cnt); corridor_vec_marker.markers.push_back(seed_marker); } // * Show driving cube decimal_t x_max, x_min; decimal_t y_max, y_min; decimal_t z_max, z_min; p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( driving_cube.cube.pos_ub[0], 0, &x_max); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( driving_cube.cube.pos_lb[0], 0, &x_min); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( driving_cube.cube.pos_ub[1], 1, &y_max); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( driving_cube.cube.pos_lb[1], 1, &y_min); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( driving_cube.cube.pos_ub[2], 2, &z_max); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( driving_cube.cube.pos_lb[2], 2, &z_min); decimal_t dx = x_max - x_min; // + res_x; decimal_t dy = y_max - y_min; // + res_y; decimal_t dz = z_max - z_min; // + res_z; decimal_t x = x_min + dx / 2.0; // - res_x / 2; decimal_t y = y_min + dy / 2.0; // - res_y / 2; decimal_t z = z_min + dz / 2.0; // - res_z / 2; std::array aabb_coord = {x, y, z}; std::array aabb_len = {dx, dy, dz}; common::AxisAlignedBoundingBoxND<3> map_aabb(aabb_coord, aabb_len); visualization_msgs::Marker map_aabb_marker; map_aabb_marker.id = id_cnt++; map_aabb_marker.ns = std::to_string(ns_cnt); common::VisualizationUtil::GetRosMarkerCubeUsingAxisAlignedBoundingBox3D( map_aabb, common::ColorARGB(0.15, 0.3, 1.0, 0.3), &map_aabb_marker); corridor_vec_marker.markers.push_back(map_aabb_marker); ++cube_cnt; } ++ns_cnt; } int num_markers = static_cast(corridor_vec_marker.markers.size()); common::VisualizationUtil::FillHeaderIdInMarkerArray( ros::Time::now(), std::string("ssc_map"), last_corridor_mk_cnt, &corridor_vec_marker); corridor_pub_.publish(common::VisualizationUtil::GetDeletionMarkerArray()); corridor_pub_.publish(corridor_vec_marker); last_corridor_mk_cnt = num_markers; } void SscVisualizer::VisualizeSemanticVoxelSet( const ros::Time &stamp, const std::vector> &voxel_set, const SscMap *p_ssc_map) { if (voxel_set.size() < 1) return; visualization_msgs::MarkerArray voxel_set_marker; int id_cnt = 0; for (const auto &voxel : voxel_set) { decimal_t x_max, x_min; decimal_t y_max, y_min; decimal_t z_max, z_min; p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( voxel.pos_ub[0], 0, &x_max); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( voxel.pos_lb[0], 0, &x_min); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( voxel.pos_ub[1], 1, &y_max); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( voxel.pos_lb[1], 1, &y_min); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( voxel.pos_ub[2], 2, &z_max); p_ssc_map->p_3d_grid()->GetGlobalMetricUsingCoordOnSingleDim( voxel.pos_lb[2], 2, &z_min); decimal_t dx = x_max - x_min; decimal_t dy = y_max - y_min; decimal_t dz = z_max - z_min; decimal_t x = x_min + dx / 2.0; decimal_t y = y_min + dy / 2.0; decimal_t z = z_min + dz / 2.0; std::array aabb_coord = {x, y, z}; std::array aabb_len = {dx, dy, dz}; common::AxisAlignedBoundingBoxND<3> voxel_aabb(aabb_coord, aabb_len); visualization_msgs::Marker voxel_marker; voxel_marker.header.frame_id = "ssc_map"; voxel_marker.header.stamp = ros::Time::now(); voxel_marker.id = id_cnt++; common::VisualizationUtil::GetRosMarkerCubeUsingAxisAlignedBoundingBox3D( voxel_aabb, common::ColorARGB(0.1, 1.0, 1.0, 1.0), &voxel_marker); voxel_set_marker.markers.push_back(voxel_marker); } semantic_voxel_set_pub_.publish( common::VisualizationUtil::GetDeletionMarkerArray()); semantic_voxel_set_pub_.publish(voxel_set_marker); } } // namespace planning ================================================ FILE: ssc_planner/src/test_ssc_planner_with_smm.cc ================================================ /** * @file test_ssc_planner_with_smm.cc * @author HKUST Aerial Robotics Group * @brief test ssc planner with mpdm behavior planner * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #include #include #include #include #include "semantic_map_manager/data_renderer.h" #include "semantic_map_manager/ros_adapter.h" #include "semantic_map_manager/semantic_map_manager.h" #include "semantic_map_manager/visualizer.h" #include "behavior_planner/behavior_server_ros.h" #include "onlane_motion_planner/onlane_server_ros.h" #include "ssc_planner/ssc_server_ros.h" DECLARE_BACKWARD; double ssc_planner_work_rate = 20.0; double bp_work_rate = 20.0; double onlane_mp_work_rate = 20.0; planning::SscPlannerServer* p_ssc_server_{nullptr}; planning::BehaviorPlannerServer* p_bp_server_{nullptr}; int BehaviorUpdateCallback( const semantic_map_manager::SemanticMapManager& smm) { if (p_ssc_server_) p_ssc_server_->PushSemanticMap(smm); return 0; } int SemanticMapUpdateCallback( const semantic_map_manager::SemanticMapManager& smm) { if (p_bp_server_) p_bp_server_->PushSemanticMap(smm); return 0; } int main(int argc, char** argv) { ros::init(argc, argv, "~"); ros::NodeHandle nh("~"); int ego_id; if (!nh.getParam("ego_id", ego_id)) { ROS_ERROR("Failed to get param %d", ego_id); assert(false); } std::string agent_config_path; if (!nh.getParam("agent_config_path", agent_config_path)) { ROS_ERROR("Failed to get param %s", agent_config_path.c_str()); assert(false); } semantic_map_manager::SemanticMapManager semantic_map_manager( ego_id, agent_config_path); semantic_map_manager::RosAdapter smm_ros_adapter(nh, &semantic_map_manager); smm_ros_adapter.BindMapUpdateCallback(SemanticMapUpdateCallback); double desired_vel; nh.param("desired_vel", desired_vel, 6.0); // Declare bp p_bp_server_ = new planning::BehaviorPlannerServer(nh, bp_work_rate, ego_id); p_bp_server_->set_user_desired_velocity(desired_vel); p_bp_server_->BindBehaviorUpdateCallback(BehaviorUpdateCallback); p_bp_server_->set_autonomous_level(3); p_ssc_server_ = new planning::SscPlannerServer(nh, ssc_planner_work_rate, ego_id); p_ssc_server_->Init(); p_bp_server_->Init(); smm_ros_adapter.Init(); p_bp_server_->Start(); p_ssc_server_->Start(); // TicToc timer; ros::Rate rate(100); while (ros::ok()) { ros::spinOnce(); rate.sleep(); } return 0; } ================================================ FILE: ssc_planner/src/test_ssc_with_pubg.cc ================================================ /** * @file test_ssc_with_pubg.cc * @author HKUST Aerial Robotics Group * @brief test ssc planner with pubg behavior planner * @version 0.1 * @date 2019-02 * @copyright Copyright (c) 2019 */ #include #include #include #include #include "semantic_map_manager/data_renderer.h" #include "semantic_map_manager/ros_adapter.h" #include "semantic_map_manager/semantic_map_manager.h" #include "semantic_map_manager/visualizer.h" #include "pubg_planner/pubg_server_ros.h" #include "onlane_motion_planner/onlane_server_ros.h" #include "ssc_planner/ssc_server_ros.h" DECLARE_BACKWARD; double ssc_planner_work_rate = 20.0; double bp_work_rate = 20.0; double onlane_mp_work_rate = 20.0; planning::SscPlannerServer* p_ssc_server_{nullptr}; planning::PubgPlannerServer* p_bp_server_{nullptr}; int BehaviorUpdateCallback( const semantic_map_manager::SemanticMapManager& smm) { if (p_ssc_server_) p_ssc_server_->PushSemanticMap(smm); return 0; } int SemanticMapUpdateCallback( const semantic_map_manager::SemanticMapManager& smm) { if (p_bp_server_) p_bp_server_->PushSemanticMap(smm); return 0; } int main(int argc, char** argv) { ros::init(argc, argv, "~"); ros::NodeHandle nh("~"); int ego_id; if (!nh.getParam("ego_id", ego_id)) { ROS_ERROR("Failed to get param %d", ego_id); assert(false); } std::string agent_config_path; if (!nh.getParam("agent_config_path", agent_config_path)) { ROS_ERROR("Failed to get param %s", agent_config_path.c_str()); assert(false); } semantic_map_manager::SemanticMapManager semantic_map_manager( ego_id, agent_config_path); semantic_map_manager::RosAdapter smm_ros_adapter(nh, &semantic_map_manager); smm_ros_adapter.BindMapUpdateCallback(SemanticMapUpdateCallback); double desired_vel; nh.param("desired_vel", desired_vel, 6.0); // Declare bp p_bp_server_ = new planning::PubgPlannerServer(nh, bp_work_rate, ego_id); p_bp_server_->set_user_desired_velocity(desired_vel); p_bp_server_->BindBehaviorUpdateCallback(BehaviorUpdateCallback); p_bp_server_->set_autonomous_level(3); p_ssc_server_ = new planning::SscPlannerServer(nh, ssc_planner_work_rate, ego_id); p_ssc_server_->Init(); p_bp_server_->Init(); smm_ros_adapter.Init(); p_bp_server_->Start(); p_ssc_server_->Start(); // TicToc timer; ros::Rate rate(100); while (ros::ok()) { ros::spinOnce(); rate.sleep(); } return 0; }