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