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