├── figs ├── RSR.png ├── CCC_path.png ├── rosgraph.png ├── bicycle_model.png ├── vehicle_model.png ├── arc_length_calc.png ├── arc_length_algorithm.png ├── hybrid_astar_result1.png ├── hybrid_astar_result2.png ├── hybrid_astar_result3.png ├── hybrid_astar_result4.png ├── hybrid_astar_result5.png ├── hybrid_astar_result6.png ├── hybrid_astar_result7.png ├── reeds-shepp-curve-words.png └── compute_tangents_via_the_vector_method.png ├── maps ├── map_demo.png ├── map_large.png ├── map_maze.png ├── map_dead_end.png ├── map_parking.png └── map.yaml ├── launch ├── graph_search_node.launch └── config.rviz ├── src ├── hybrid_a_star_node.cpp ├── graph_search_utils.cpp ├── a_star.cpp └── hybrid_a_star.cpp ├── include ├── graph_search_utils.hpp ├── a_star.hpp └── hybrid_a_star.hpp ├── package.xml ├── CMakeLists.txt └── README.md /figs/RSR.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/RSR.png -------------------------------------------------------------------------------- /figs/CCC_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/CCC_path.png -------------------------------------------------------------------------------- /figs/rosgraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/rosgraph.png -------------------------------------------------------------------------------- /maps/map_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/maps/map_demo.png -------------------------------------------------------------------------------- /maps/map_large.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/maps/map_large.png -------------------------------------------------------------------------------- /maps/map_maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/maps/map_maze.png -------------------------------------------------------------------------------- /maps/map_dead_end.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/maps/map_dead_end.png -------------------------------------------------------------------------------- /maps/map_parking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/maps/map_parking.png -------------------------------------------------------------------------------- /figs/bicycle_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/bicycle_model.png -------------------------------------------------------------------------------- /figs/vehicle_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/vehicle_model.png -------------------------------------------------------------------------------- /figs/arc_length_calc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/arc_length_calc.png -------------------------------------------------------------------------------- /figs/arc_length_algorithm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/arc_length_algorithm.png -------------------------------------------------------------------------------- /figs/hybrid_astar_result1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/hybrid_astar_result1.png -------------------------------------------------------------------------------- /figs/hybrid_astar_result2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/hybrid_astar_result2.png -------------------------------------------------------------------------------- /figs/hybrid_astar_result3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/hybrid_astar_result3.png -------------------------------------------------------------------------------- /figs/hybrid_astar_result4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/hybrid_astar_result4.png -------------------------------------------------------------------------------- /figs/hybrid_astar_result5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/hybrid_astar_result5.png -------------------------------------------------------------------------------- /figs/hybrid_astar_result6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/hybrid_astar_result6.png -------------------------------------------------------------------------------- /figs/hybrid_astar_result7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/hybrid_astar_result7.png -------------------------------------------------------------------------------- /figs/reeds-shepp-curve-words.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/reeds-shepp-curve-words.png -------------------------------------------------------------------------------- /figs/compute_tangents_via_the_vector_method.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stoneman404/hybrid-a-star/HEAD/figs/compute_tangents_via_the_vector_method.png -------------------------------------------------------------------------------- /maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map_large.png 2 | resolution: 0.5 3 | origin: [0.0, 0.0, 0.0] 4 | occupied_thresh: 0.1 5 | free_thresh: 0.05 6 | negate: 0 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/graph_search_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/hybrid_a_star_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | int main(int argc, char **argv) { 7 | ros::init(argc, argv, "hybrid_a_star_planner"); 8 | ros::NodeHandle nh; 9 | std::unique_ptr hybrid_a_star = 10 | std::make_unique(nh); 11 | ros::Rate loop_rate(10); 12 | while (ros::ok()) { 13 | ros::spinOnce(); 14 | loop_rate.sleep(); 15 | ros::Time begin = ros::Time::now(); 16 | hybrid_a_star->RunOnce(); 17 | ros::Time end = ros::Time::now(); 18 | ros::Duration d(end - begin); 19 | ROS_WARN("[RUNONCE Time: %lf ms]", d.toSec() * 1000.0); 20 | } 21 | return 0; 22 | } -------------------------------------------------------------------------------- /include/graph_search_utils.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CATKIN_WS_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_GRAPH_SEARCH_UTILS_HPP_ 3 | #define CATKIN_WS_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_GRAPH_SEARCH_UTILS_HPP_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace planning { 10 | 11 | bool InBoudary(int index_x, int index_y, 12 | nav_msgs::OccupancyGrid::Ptr grid_map); 13 | void Index2Pose(int index_x, int index_y, 14 | nav_msgs::OccupancyGrid::Ptr grid_map, 15 | double *x, double *y); 16 | void Pose2Index(double x, double y, 17 | nav_msgs::OccupancyGrid::Ptr grid_map, 18 | int *index_x, int *index_y); 19 | int CalcIndex(int index_x, int index_y, int width, int height); 20 | bool CheckPose2d(double x, double y, 21 | nav_msgs::OccupancyGrid::Ptr grid_map); 22 | double NormalizeAngle(const double &angle); 23 | double CalcAngleDist(double from, double to); 24 | bool CheckPose3d(double base_x, double base_y, double base_phi, 25 | double adc_length, double adc_width, 26 | double axle_ref_x, double expand_dist, 27 | const nav_msgs::OccupancyGrid::Ptr &grid_map); 28 | bool CheckCircleRobotPose(double x, double y, double raidus, 29 | nav_msgs::OccupancyGrid::Ptr grid_map); 30 | } 31 | 32 | #endif //CATKIN_WS_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_GRAPH_SEARCH_UTILS_HPP_ 33 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graph_search_planner 4 | 0.0.0 5 | The graph_search_planner package 6 | 7 | 8 | 9 | 10 | ldh 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | nav_msgs 54 | roscpp 55 | rospy 56 | std_msgs 57 | tf 58 | 59 | geometry_msgs 60 | nav_msgs 61 | roscpp 62 | rospy 63 | std_msgs 64 | geometry_msgs 65 | nav_msgs 66 | roscpp 67 | rospy 68 | 69 | std_msgs 70 | tf 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/graph_search_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "graph_search_utils.hpp" 2 | 3 | namespace planning { 4 | bool InBoudary(int index_x, int index_y, nav_msgs::OccupancyGrid::Ptr grid_map) { 5 | return index_x >= 0 && index_x < grid_map->info.width && index_y >= 0 6 | && index_y < grid_map->info.height; 7 | } 8 | 9 | void Index2Pose(int index_x, int index_y, 10 | nav_msgs::OccupancyGrid::Ptr grid_map, 11 | double *x, double *y) { 12 | const double theta = tf::getYaw(grid_map->info.origin.orientation); 13 | const double robot_x = index_x * grid_map->info.resolution; 14 | const double robot_y = index_y * grid_map->info.resolution; 15 | // const double relative_x = std::cos(theta) * robot_x - std::sin(theta) * robot_y; 16 | // const double relative_y = std::sin(theta) * robot_x + std::cos(theta) * robot_y; 17 | const double relative_x = robot_x; 18 | const double relative_y = robot_y; 19 | *x = grid_map->info.origin.position.x + relative_x; 20 | *y = grid_map->info.origin.position.y + relative_y; 21 | } 22 | 23 | void Pose2Index(double x, double y, 24 | nav_msgs::OccupancyGrid::Ptr grid_map, 25 | int *index_x, int *index_y) { 26 | // double origin_x = grid_map->info.origin.position.x; 27 | // double origin_y = grid_map->info.origin.position.y; 28 | // double origin_theta = tf::getYaw(grid_map->info.origin.orientation); 29 | // double dx = x - origin_x; 30 | // double dy = y - origin_y; 31 | // const double robot_x = std::cos(origin_theta) * dx + std::sin(origin_theta) * dy; 32 | // const double robot_y = -std::sin(origin_theta) * dx + std::cos(origin_theta) * dy; 33 | *index_x = static_cast(std::round(x / grid_map->info.resolution)); 34 | *index_y = static_cast(std::round(y / grid_map->info.resolution)); 35 | } 36 | 37 | int CalcIndex(int index_x, int index_y, int width, int height) { 38 | int index = index_y * width + index_x; 39 | return index; 40 | } 41 | 42 | bool CheckPose2d(double x, double y, nav_msgs::OccupancyGrid::Ptr grid_map) { 43 | int index_x, index_y; 44 | Pose2Index(x, y, grid_map, &index_x, &index_y); 45 | if (!InBoudary(index_x, index_y, grid_map)) { 46 | return false; 47 | } 48 | int index = CalcIndex(index_x, index_y, grid_map->info.width, grid_map->info.height); 49 | return grid_map->data[index] < 0.1; 50 | } 51 | 52 | double NormalizeAngle(const double &angle) { 53 | double a = std::fmod(angle + M_PI, 2.0 * M_PI); 54 | if (a < 0.0) { 55 | a += (2.0 * M_PI); 56 | } 57 | return a - M_PI; 58 | } 59 | 60 | double CalcAngleDist(double from, double to) { 61 | return NormalizeAngle(to - from); 62 | } 63 | 64 | bool CheckPose3d(double base_x, double base_y, double base_phi, 65 | 66 | double adc_length, double adc_width, 67 | double axle_ref_x, double expand_dist, 68 | const nav_msgs::OccupancyGrid::Ptr &grid_map) { 69 | 70 | const double adc_expand_length = adc_length + expand_dist; 71 | const double adc_expand_width = adc_width + expand_dist; 72 | const double axle2back = 0.5 * adc_expand_length - axle_ref_x; 73 | if (axle2back < 0.0) { 74 | return false; 75 | } 76 | const double left = -1.0 * axle2back; 77 | const double right = adc_expand_length - 1.0 * axle2back; 78 | const double top = adc_expand_width / 2.0; 79 | const double bottom = -adc_expand_width / 2.0; 80 | const double sin_phi = std::sin(base_phi); 81 | const double cos_phi = std::cos(base_phi); 82 | double xy_resolution = std::min(0.5, static_cast(grid_map->info.resolution)); 83 | for (double x = left; x < right; x += xy_resolution) { 84 | for (double y = top; y > bottom; y -= xy_resolution) { 85 | double map_x = x * cos_phi - y * sin_phi + base_x; 86 | double map_y = x * sin_phi + y * cos_phi + base_y; 87 | if (!CheckPose2d(map_x, map_y, grid_map)) { 88 | return false; 89 | } 90 | } 91 | } 92 | return true; 93 | } 94 | 95 | bool CheckCircleRobotPose(double x, double y, double raidus, 96 | nav_msgs::OccupancyGrid::Ptr grid_map) { 97 | for (double theta = -M_PI; theta < M_PI; theta += 1.0 / 6.0 * M_PI) { 98 | for (double r = 0.0; r < raidus; r += grid_map->info.resolution) { 99 | double robot_x = x + r * std::cos(theta); 100 | double robot_y = y + r * std::sin(theta); 101 | if (!CheckPose2d(robot_x, robot_y, grid_map)) { 102 | return false; 103 | } 104 | } 105 | } 106 | return true; 107 | } 108 | 109 | } 110 | 111 | -------------------------------------------------------------------------------- /include/a_star.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PLANNING_ALGORITHM_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_A_STAR_HPP_ 2 | #define PLANNING_ALGORITHM_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_A_STAR_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "graph_search_utils.hpp" 13 | #include 14 | 15 | namespace planning { 16 | 17 | class Node2d { 18 | public: 19 | Node2d( 20 | double x, double y, 21 | nav_msgs::OccupancyGrid::Ptr grid_map) { 22 | Pose2Index(x, y, std::move(grid_map), &index_x_, &index_y_); 23 | index_ = std::to_string(index_x_) + "_" + std::to_string(index_y_); 24 | } 25 | Node2d(int x, int y) { 26 | index_x_ = x; 27 | index_y_ = y; 28 | index_ = std::to_string(index_x_) + "_" + std::to_string(index_y_); 29 | } 30 | //////////////////// setter ///////////////////////////// 31 | void SetHCost(double h) { 32 | hcost_ = h; 33 | fcost_ = hcost_ + gcost_; 34 | } 35 | void SetFcost(double f) { fcost_ = f; } 36 | void SetGcost(double g) { 37 | gcost_ = g; 38 | fcost_ = gcost_ + hcost_; 39 | } 40 | void SetPreNode(std::shared_ptr pre_node) { 41 | pre_node_ = std::move(pre_node); 42 | } 43 | 44 | /////////////////// getter /////////////////////// 45 | double GetHCost() const { return hcost_; } 46 | double GetFCost() const { return fcost_; } 47 | double GetGCost() const { return gcost_; } 48 | int GetIndexX() const { return index_x_; } 49 | int GetIndexY() const { return index_y_; } 50 | const std::string &GetIndex() const { return index_; } 51 | std::shared_ptr GetPreNode() const { return pre_node_; } 52 | /////////////// operator //////////////////////// 53 | bool operator==(const Node2d &other) const { 54 | return other.GetIndex() == index_; 55 | } 56 | 57 | private: 58 | double gcost_ = 0.0; 59 | double fcost_ = 0.0; 60 | double hcost_ = 0.0; 61 | int index_x_ = 0; 62 | int index_y_ = 0; 63 | std::string index_; 64 | std::shared_ptr pre_node_ = nullptr; 65 | }; 66 | 67 | struct AStarResult { 68 | std::vector x; 69 | std::vector y; 70 | double cost = 0.0; 71 | }; 72 | 73 | class AStar { 74 | public: 75 | 76 | AStar() = default; 77 | ~AStar() = default; 78 | void SetVhicleParams(double length, double width, double axle_ref_x) { 79 | length_ = length; 80 | width_ = width; 81 | axle_ref_x_ = axle_ref_x; 82 | } 83 | bool SearchPath(double sx, double sy, double ex, double ey, AStarResult *result); 84 | 85 | void SetMap(nav_msgs::OccupancyGrid::Ptr grid_map) { 86 | grid_map_ = std::move(grid_map); 87 | has_map_ = true; 88 | xy_resolution_ = grid_map_->info.resolution; 89 | } 90 | /** 91 | * check the dp map to get the cost 92 | * @param sx: the curruent position x 93 | * @param sy: the current position y 94 | * @return 95 | */ 96 | double CheckDpMap(double sx, double sy); 97 | /** 98 | * 99 | * @param ex: the end position x 100 | * @param ey: the end position y 101 | * @return true or false 102 | */ 103 | bool GenerateDpMap(double ex, double ey); 104 | 105 | protected: 106 | 107 | /** 108 | * Euclid Distance, as heuristic 109 | * @param x1 110 | * @param y1 111 | * @param x2 112 | * @param y2 113 | * @return 114 | */ 115 | double EuclidDistance(std::shared_ptr cur, std::shared_ptr next) { 116 | double h; 117 | const int cur_index_x = cur->GetIndexX(); 118 | const int cur_index_y = cur->GetIndexY(); 119 | const int next_index_x = next->GetIndexX(); 120 | const int next_index_y = next->GetIndexY(); 121 | double cur_x, cur_y, next_x, next_y; 122 | Index2Pose(cur_index_x, cur_index_y, grid_map_, &cur_x, &cur_y); 123 | Index2Pose(next_index_x, next_index_y, grid_map_, &next_x, &next_y); 124 | h = std::hypot(next_x - cur_x, next_y - cur_y); 125 | double tie_breaker = 1; 126 | return tie_breaker * h; 127 | } 128 | 129 | double MahDistance(std::shared_ptr cur, std::shared_ptr next) { 130 | double h; 131 | const int cur_index_x = cur->GetIndexX(); 132 | const int cur_index_y = cur->GetIndexY(); 133 | const int next_index_x = next->GetIndexX(); 134 | const int next_index_y = next->GetIndexY(); 135 | double cur_x, cur_y, next_x, next_y; 136 | Index2Pose(cur_index_x, cur_index_y, grid_map_, &cur_x, &cur_y); 137 | Index2Pose(next_index_x, next_index_y, grid_map_, &next_x, &next_y); 138 | h = std::fabs(next_x - cur_x) + std::fabs(next_y - cur_y); 139 | return h; 140 | } 141 | 142 | double EdgeCost(std::shared_ptr cur, std::shared_ptr next) { 143 | double cost; 144 | const int cur_index_x = cur->GetIndexX(); 145 | const int cur_index_y = cur->GetIndexY(); 146 | const int next_index_x = next->GetIndexX(); 147 | const int next_index_y = next->GetIndexY(); 148 | double cur_x, cur_y, next_x, next_y; 149 | Index2Pose(cur_index_x, cur_index_y, grid_map_, &cur_x, &cur_y); 150 | Index2Pose(next_index_x, next_index_y, grid_map_, &next_x, &next_y); 151 | cost = std::hypot(next_x - cur_x, next_y - cur_y); 152 | return cost; 153 | } 154 | 155 | bool VerifyNode2d(std::shared_ptr node); 156 | 157 | std::vector> GenerateNextNodes(std::shared_ptr node); 158 | 159 | bool TracePath(AStarResult *astar_path); 160 | 161 | struct cmp { 162 | bool operator()(const std::shared_ptr left, const std::shared_ptr right) const { 163 | return left->GetFCost() >= right->GetFCost(); 164 | } 165 | }; 166 | 167 | private: 168 | double length_{}; 169 | double width_{}; 170 | double axle_ref_x_{}; 171 | double xy_resolution_{}; 172 | double node_radius_{}; 173 | std::shared_ptr start_node_ = nullptr; 174 | std::shared_ptr goal_node_ = nullptr; 175 | std::shared_ptr final_node_ = nullptr; 176 | std::unordered_map> dp_map_; 177 | bool has_map_ = false; 178 | nav_msgs::OccupancyGrid::Ptr grid_map_; 179 | 180 | }; 181 | 182 | } 183 | #endif //PLANNING_ALGORITHM_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_A_STAR_HPP_ 184 | -------------------------------------------------------------------------------- /launch/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 462 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.699999988079071 56 | Class: rviz/Map 57 | Color Scheme: map 58 | Draw Behind: false 59 | Enabled: true 60 | Name: Map 61 | Topic: /map 62 | Unreliable: false 63 | Use Timestamp: false 64 | Value: true 65 | - Alpha: 1 66 | Axes Length: 1 67 | Axes Radius: 0.10000000149011612 68 | Class: rviz/PoseWithCovariance 69 | Color: 255; 25; 0 70 | Covariance: 71 | Orientation: 72 | Alpha: 0.5 73 | Color: 255; 255; 127 74 | Color Style: Unique 75 | Frame: Local 76 | Offset: 1 77 | Scale: 1 78 | Value: true 79 | Position: 80 | Alpha: 0.30000001192092896 81 | Color: 204; 51; 204 82 | Scale: 1 83 | Value: true 84 | Value: true 85 | Enabled: true 86 | Head Length: 0.30000001192092896 87 | Head Radius: 0.10000000149011612 88 | Name: PoseWithCovariance 89 | Shaft Length: 1 90 | Shaft Radius: 0.05000000074505806 91 | Shape: Arrow 92 | Topic: /initialpose 93 | Unreliable: false 94 | Value: true 95 | - Alpha: 1 96 | Axes Length: 1 97 | Axes Radius: 0.10000000149011612 98 | Class: rviz/Pose 99 | Color: 255; 25; 0 100 | Enabled: true 101 | Head Length: 0.30000001192092896 102 | Head Radius: 0.10000000149011612 103 | Name: Pose 104 | Shaft Length: 1 105 | Shaft Radius: 0.05000000074505806 106 | Shape: Arrow 107 | Topic: /move_base_simple/goal 108 | Unreliable: false 109 | Value: true 110 | - Alpha: 1 111 | Axes Length: 1 112 | Axes Radius: 0.10000000149011612 113 | Class: rviz/PoseWithCovariance 114 | Color: 255; 25; 0 115 | Covariance: 116 | Orientation: 117 | Alpha: 0.5 118 | Color: 255; 255; 127 119 | Color Style: Unique 120 | Frame: Local 121 | Offset: 1 122 | Scale: 1 123 | Value: true 124 | Position: 125 | Alpha: 0.30000001192092896 126 | Color: 204; 51; 204 127 | Scale: 1 128 | Value: true 129 | Value: true 130 | Enabled: true 131 | Head Length: 0.30000001192092896 132 | Head Radius: 0.10000000149011612 133 | Name: PoseWithCovariance 134 | Shaft Length: 1 135 | Shaft Radius: 0.05000000074505806 136 | Shape: Arrow 137 | Topic: /initialpose 138 | Unreliable: false 139 | Value: true 140 | - Class: rviz/MarkerArray 141 | Enabled: true 142 | Marker Topic: /visualized_path 143 | Name: MarkerArray 144 | Namespaces: 145 | "": true 146 | Queue Size: 100 147 | Value: true 148 | - Class: rviz/MarkerArray 149 | Enabled: true 150 | Marker Topic: /visualized_vehicle 151 | Name: MarkerArray 152 | Namespaces: 153 | "": true 154 | Queue Size: 100 155 | Value: true 156 | Enabled: true 157 | Global Options: 158 | Background Color: 48; 48; 48 159 | Default Light: true 160 | Fixed Frame: map 161 | Frame Rate: 30 162 | Name: root 163 | Tools: 164 | - Class: rviz/Interact 165 | Hide Inactive Objects: true 166 | - Class: rviz/MoveCamera 167 | - Class: rviz/Select 168 | - Class: rviz/FocusCamera 169 | - Class: rviz/Measure 170 | - Class: rviz/SetInitialPose 171 | Theta std deviation: 0.2617993950843811 172 | Topic: /initialpose 173 | X std deviation: 0.5 174 | Y std deviation: 0.5 175 | - Class: rviz/SetGoal 176 | Topic: /move_base_simple/goal 177 | - Class: rviz/PublishPoint 178 | Single click: true 179 | Topic: /clicked_point 180 | Value: true 181 | Views: 182 | Current: 183 | Angle: 0.004999999888241291 184 | Class: rviz/TopDownOrtho 185 | Enable Stereo Rendering: 186 | Stereo Eye Separation: 0.05999999865889549 187 | Stereo Focal Distance: 1 188 | Swap Stereo Eyes: false 189 | Value: false 190 | Invert Z Axis: false 191 | Name: Current View 192 | Near Clip Distance: 0.009999999776482582 193 | Scale: 19.26283836364746 194 | Target Frame: 195 | Value: TopDownOrtho (rviz) 196 | X: 14.053326606750488 197 | Y: 12.734590530395508 198 | Saved: ~ 199 | Window Geometry: 200 | Displays: 201 | collapsed: false 202 | Height: 759 203 | Hide Left Dock: false 204 | Hide Right Dock: false 205 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000259fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000259000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056d0000003efc0100000002fb0000000800540069006d006501000000000000056d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002fc0000025900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 206 | Selection: 207 | collapsed: false 208 | Time: 209 | collapsed: false 210 | Tool Properties: 211 | collapsed: false 212 | Views: 213 | collapsed: false 214 | Width: 1389 215 | X: 218 216 | Y: 66 217 | -------------------------------------------------------------------------------- /include/hybrid_a_star.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PLANNING_ALGORITHM_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_HYBRID_A_STAR_HPP_ 2 | #define PLANNING_ALGORITHM_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_HYBRID_A_STAR_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "a_star.hpp" 17 | namespace planning { 18 | 19 | struct HybridAStarResult { 20 | std::vector x; 21 | std::vector y; 22 | std::vector phi; 23 | std::vector v; 24 | std::vector a; 25 | std::vector steer; 26 | std::vector accumulated_s; 27 | }; 28 | 29 | class Node3d { 30 | public: 31 | 32 | Node3d(double x, double y, double phi); 33 | Node3d(double x, double y, double phi, 34 | nav_msgs::OccupancyGrid::Ptr grid_map, 35 | double phi_resolution); 36 | Node3d(const std::vector &traversed_x, 37 | const std::vector &traversed_y, 38 | const std::vector &traversed_phi, 39 | nav_msgs::OccupancyGrid::Ptr grid_map, 40 | double phi_resolution); 41 | ~Node3d() = default; 42 | bool operator==(const Node3d &other) const; 43 | ///////////////////////// getter //////////////////////// 44 | double GetX() const { return x_; } 45 | double GetY() const { return y_; } 46 | double GetPhi() const { return phi_; } 47 | double GetGCost() const { return gcost_; } 48 | double GetFCost() const { return fcost_; } 49 | double GetHCost() const { return hcost_; } 50 | bool GetDirection() const { return direction_; } 51 | double GetSteering() const { return steering_; } 52 | std::string GetIndex() const { return index_; } 53 | int GetIndexX() const { return index_x_; } 54 | int GetIndexY() const { return index_y_; } 55 | int GetIndexPhi() const { return index_phi_; } 56 | std::vector GetXs() const { return traversed_x_; } 57 | std::vector GetYs() const { return traversed_y_; } 58 | std::vector GetPhis() const { return traversed_phi_; } 59 | size_t GetStepSize() const { return step_size_; } 60 | std::shared_ptr GetPreNode() const { return pre_node_; } 61 | //////////////////// setter /////////////////////// 62 | void SetGCost(double g) { 63 | gcost_ = g; 64 | fcost_ = gcost_ + hcost_; 65 | } 66 | void SetHCost(double h) { 67 | hcost_ = h; 68 | fcost_ = hcost_ + gcost_; 69 | } 70 | void SetDirection(bool dir) { this->direction_ = dir; } 71 | void SetSteering(double steering) { this->steering_ = steering; } 72 | void SetPreNode(std::shared_ptr pre_node) { this->pre_node_ = std::move(pre_node); } 73 | bool IsInRange(const std::shared_ptr &goal, double analytic_expand_range) const { 74 | double dis = std::hypot(goal->GetY() - y_, goal->GetX() - x_); 75 | return dis <= analytic_expand_range; 76 | } 77 | 78 | private: 79 | double x_ = 0.0; 80 | double y_ = 0.0; 81 | double phi_ = 0.0; 82 | int index_x_ = 0; 83 | int index_y_ = 0; 84 | int index_phi_ = 0; 85 | std::string index_; 86 | std::vector traversed_x_; 87 | std::vector traversed_y_; 88 | std::vector traversed_phi_; 89 | size_t step_size_ = 1; 90 | double hcost_{}; 91 | double gcost_{}; 92 | double fcost_{}; 93 | std::shared_ptr pre_node_ = nullptr; 94 | // true move forward, false, move backward 95 | bool direction_ = true; 96 | double steering_ = 0.0; 97 | 98 | }; 99 | 100 | class HybridAStar { 101 | public: 102 | explicit HybridAStar(const ros::NodeHandle &nh); 103 | ~HybridAStar() = default; 104 | void RunOnce(); 105 | 106 | protected: 107 | std::shared_ptr ReedsSheppShot(const std::shared_ptr &node, double *len); 108 | std::shared_ptr DubinShot(const std::shared_ptr &node, double *len); 109 | bool Search(double sx, double sy, double sphi, double ex, 110 | double ey, double ephi, HybridAStarResult *result); 111 | bool TrajectoryPartition(const HybridAStarResult *result, 112 | std::vector *parition_results) const; 113 | // anlytic expansion node 114 | bool AnalyticExpansion(const std::shared_ptr ¤t_node); 115 | // check the 3d node is valid 116 | bool CheckNode3d(const std::shared_ptr &node); 117 | // generate next node according to index 118 | std::shared_ptr NextNodeGenerator( 119 | const std::shared_ptr ¤t_node, size_t index); 120 | // heuristic with obstacle 121 | double HoloWithObstacleHeuristic(const std::shared_ptr &next_node); 122 | // non holonmoic heuristic without obstacle 123 | double NonHoloWithoutObstacleHeuristic(const std::shared_ptr &next_node) const; 124 | // calculate edge cost 125 | double EdgeCost(const std::shared_ptr ¤t_node, 126 | const std::shared_ptr &next_node) const; 127 | 128 | struct cmp { 129 | bool operator()(const std::shared_ptr &left, 130 | const std::shared_ptr &right) const; 131 | }; 132 | 133 | void SetGoal(const geometry_msgs::PoseStamped::Ptr &goal); 134 | void SetStart(const geometry_msgs::PoseWithCovarianceStamped::Ptr &start); 135 | void SetMap(const nav_msgs::OccupancyGrid::Ptr &grid_map); 136 | bool LoadResult(HybridAStarResult *result); 137 | static nav_msgs::Path HybridAStarResult2NavPath(const HybridAStarResult &result); 138 | static nav_msgs::Path AStarResult2NavPath(const AStarResult &astar_result); 139 | void VisualizedPath(const nav_msgs::Path &path); 140 | 141 | private: 142 | bool has_grid_map_ = false; 143 | bool valid_start_ = false; 144 | bool valid_goal_ = false; 145 | bool enable_backward_ = false; 146 | bool has_dp_map_ = false; 147 | geometry_msgs::PoseWithCovarianceStamped start_pose_; 148 | geometry_msgs::PoseStamped goal_pose_; 149 | ros::Publisher ha_star_path_publisher_; 150 | ros::Publisher visulized_path_publisher_; 151 | ros::Publisher visualized_vehicle_publisher_; 152 | ros::Subscriber grid_map_subscriber_; 153 | ros::Subscriber goal_pose_subscriber_; 154 | ros::Subscriber init_pose_subscriber_; 155 | int next_node_num_ = 0; 156 | double step_size_ = 0.0; 157 | double max_steer_angle_ = 0.0; 158 | double adc_width_ = 0.0; 159 | double adc_length_ = 0.0; 160 | double wheel_base_ = 0.0; 161 | double xy_resolution_ = 0.0; 162 | double phi_resolution_ = 0.0; 163 | double delta_steer_ = 0.0; 164 | double traj_forward_penalty_ = 0.0; 165 | double traj_backward_penalty_ = 0.0; 166 | double traj_gear_switch_penalty_ = 0.0; 167 | double traj_steer_penalty_ = 0.0; 168 | double traj_steer_change_penalty_ = 0.0; 169 | double delta_t_ = 0.1; 170 | double traj_step_length_penalty_ = 0.0; 171 | // double max_cost_ = 1000.0; 172 | double max_cost_ = 1.0; 173 | double anlytic_expand_distance_ = 0.0; 174 | std::shared_ptr start_node_ = nullptr; 175 | std::shared_ptr end_node_ = nullptr; 176 | std::shared_ptr final_node_ = nullptr; 177 | nav_msgs::OccupancyGrid::Ptr grid_map_; 178 | std::unique_ptr heuristic_generator_; 179 | // std::priority_queue, std::vector>, cmp> open_pq_; 180 | boost::heap::binomial_heap, boost::heap::compare> priority_queue_; 181 | std::unordered_map< 182 | std::string, boost::heap::binomial_heap, boost::heap::compare>::handle_type> 183 | open_set_handles_; 184 | std::unordered_map> open_set_; 185 | std::unordered_map> closed_set_; 186 | ros::NodeHandle nh_; 187 | }; 188 | 189 | } 190 | 191 | #endif //PLANNING_ALGORITHM_SRC_GRAPH_SEARCH_PLANNER_INCLUDE_HYBRID_A_STAR_HPP_ 192 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(graph_search_planner) 3 | 4 | #add_compile_options(-std=c++14) 5 | add_compile_options(-std=c++14 -O3 -Werror -Wall -Wno-unused -Wno-sign-compare) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | nav_msgs 13 | roscpp 14 | rospy 15 | std_msgs 16 | tf 17 | # ompl 18 | ) 19 | find_package(ompl REQUIRED) 20 | find_package(Eigen3 REQUIRED) 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a exec_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # geometry_msgs# nav_msgs# std_msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES graph_search_planner 112 | # CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | include 124 | ${catkin_INCLUDE_DIRS} 125 | ${OMPL_INCLUDE_DIRS} 126 | ${Eigen3_INCLUDE_DIRS} 127 | ) 128 | 129 | ## Declare a C++ library 130 | # add_library(${PROJECT_NAME} 131 | # src/${PROJECT_NAME}/graph_search_planner.cpp 132 | # ) 133 | 134 | ## Add cmake target dependencies of the library 135 | ## as an example, code may need to be generated before libraries 136 | ## either from message generation or dynamic reconfigure 137 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Declare a C++ executable 140 | ## With catkin_make all packages are built within a single CMake context 141 | ## The recommended prefix ensures that target names across packages don't collide 142 | add_executable(${PROJECT_NAME}_node 143 | src/graph_search_utils.cpp 144 | src/hybrid_a_star.cpp 145 | src/hybrid_a_star_node.cpp 146 | src/a_star.cpp 147 | ) 148 | 149 | ## Rename C++ executable without prefix 150 | ## The above recommended prefix causes long target names, the following renames the 151 | ## target back to the shorter version for ease of user use 152 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 153 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 154 | 155 | ## Add cmake target dependencies of the executable 156 | ## same as for the library above 157 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 158 | 159 | ## Specify libraries to link a library or executable target against 160 | target_link_libraries(${PROJECT_NAME}_node 161 | ${catkin_LIBRARIES} 162 | ompl 163 | Eigen3::Eigen 164 | ) 165 | 166 | ############# 167 | ## Install ## 168 | ############# 169 | 170 | 171 | install(DIRECTORY launch/ 172 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 173 | 174 | # all install targets should use catkin DESTINATION variables 175 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 176 | 177 | ## Mark executable scripts (Python etc.) for installation 178 | ## in contrast to setup.py, you can choose the destination 179 | # install(PROGRAMS 180 | # scripts/my_python_script 181 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark executables for installation 185 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 186 | # install(TARGETS ${PROJECT_NAME}_node 187 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 188 | # ) 189 | 190 | ## Mark libraries for installation 191 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 192 | # install(TARGETS ${PROJECT_NAME} 193 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 194 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 195 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 196 | # ) 197 | 198 | ## Mark cpp header files for installation 199 | # install(DIRECTORY include/${PROJECT_NAME}/ 200 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 201 | # FILES_MATCHING PATTERN "*.h" 202 | # PATTERN ".svn" EXCLUDE 203 | # ) 204 | 205 | ## Mark other files for installation (e.g. launch and bag files, etc.) 206 | # install(FILES 207 | # # myfile1 208 | # # myfile2 209 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 210 | # ) 211 | 212 | ############# 213 | ## Testing ## 214 | ############# 215 | 216 | ## Add gtest based cpp test target and link libraries 217 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_graph_search_planner.cpp) 218 | # if(TARGET ${PROJECT_NAME}-test) 219 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 220 | # endif() 221 | 222 | ## Add folders to be run by python nosetests 223 | # catkin_add_nosetests(test) 224 | -------------------------------------------------------------------------------- /src/a_star.cpp: -------------------------------------------------------------------------------- 1 | #include "a_star.hpp" 2 | #include "graph_search_utils.hpp" 3 | #include 4 | #include 5 | namespace planning { 6 | 7 | std::vector> AStar::GenerateNextNodes( 8 | std::shared_ptr node) { 9 | std::vector> next_nodes; 10 | int cur_index_x = node->GetIndexX(); 11 | int cur_index_y = node->GetIndexY(); 12 | double current_x, current_y; 13 | Index2Pose(cur_index_x, cur_index_y, grid_map_, ¤t_x, ¤t_y); 14 | double xy_resolution = grid_map_->info.resolution; 15 | std::shared_ptr up = std::make_shared(cur_index_x, cur_index_y + 1); 16 | // up->SetGcost(node->GetGCost() + xy_resolution); 17 | std::shared_ptr down = std::make_shared(cur_index_x, cur_index_y - 1); 18 | down->SetGcost(node->GetGCost() + xy_resolution); 19 | std::shared_ptr right = std::make_shared(cur_index_x + 1, cur_index_y); 20 | // right->SetGcost(node->GetGCost() + xy_resolution); 21 | auto left = std::make_shared(cur_index_x - 1, cur_index_y); 22 | // left->SetGcost(node->GetGCost() + xy_resolution); 23 | auto up_right = std::make_shared(cur_index_x + 1, cur_index_y + 1); 24 | // up_right->SetGcost(node->GetGCost() + std::sqrt(2.0) * xy_resolution); 25 | auto up_left = std::make_shared(cur_index_x - 1, cur_index_y + 1); 26 | // up_left->SetGcost(node->GetGCost() + std::sqrt(2.0) * xy_resolution); 27 | auto down_right = std::make_shared(cur_index_x + 1, cur_index_y - 1); 28 | // up_left->SetGcost(node->GetGCost() + std::sqrt(2.0) * xy_resolution); 29 | auto down_left = std::make_shared(cur_index_x - 1, cur_index_y - 1); 30 | // down_left->SetGcost(node->GetGCost() + std::sqrt(2.0) * xy_resolution); 31 | next_nodes.emplace_back(up); 32 | next_nodes.emplace_back(down); 33 | next_nodes.emplace_back(right); 34 | next_nodes.emplace_back(left); 35 | next_nodes.emplace_back(up_right); 36 | next_nodes.emplace_back(up_left); 37 | next_nodes.emplace_back(down_right); 38 | next_nodes.emplace_back(down_left); 39 | return next_nodes; 40 | } 41 | 42 | // we treat the robot in a star algorithm as a point robot, 43 | // because it only provides heuristic for hybrid a star 44 | bool AStar::VerifyNode2d(std::shared_ptr node) { 45 | int index_x = node->GetIndexX(); 46 | int index_y = node->GetIndexY(); 47 | double x, y; 48 | 49 | // double raidus = std::hypot(0.5 * length_ + axle_ref_x_, 0.5 * width_); 50 | double raidus = 0.5 * length_ + 0.2; 51 | Index2Pose(index_x, index_y, grid_map_, &x, &y); 52 | // return CheckPose2d(x, y, grid_map_); 53 | return CheckCircleRobotPose(x, y, raidus, grid_map_); 54 | } 55 | 56 | bool AStar::TracePath(AStarResult *astar_path) { 57 | if (astar_path == nullptr) { 58 | ROS_FATAL("[AStar::TracePath]: the input pointer is nullptr"); 59 | return false; 60 | } 61 | if (final_node_ == nullptr) { 62 | ROS_FATAL("[AStar::TracePath]: the final node is nullptr, cannot find path"); 63 | return false; 64 | } 65 | std::vector xs, ys; 66 | auto current_node = final_node_; 67 | double x, y; 68 | while (current_node->GetPreNode() != nullptr) { 69 | 70 | Index2Pose( 71 | current_node->GetIndexX(), 72 | current_node->GetIndexY(), 73 | grid_map_, 74 | &x, &y); 75 | std::cout << " index_x : " << current_node->GetIndexX() 76 | << " index_y: " << current_node->GetIndexY() 77 | << " x: " << x << " y : " << y << std::endl; 78 | 79 | xs.push_back(x); 80 | ys.push_back(y); 81 | current_node = current_node->GetPreNode(); 82 | } 83 | double x_start, y_start; 84 | Index2Pose( 85 | current_node->GetIndexX(), 86 | current_node->GetIndexY(), 87 | grid_map_, 88 | &x_start, 89 | &y_start); 90 | xs.push_back(x_start); 91 | ys.push_back(y_start); 92 | std::reverse(xs.begin(), xs.end()); 93 | std::reverse(ys.begin(), ys.end()); 94 | astar_path->x = std::move(xs); 95 | astar_path->y = std::move(ys); 96 | double len = 0.0; 97 | for (size_t i = 1; i < astar_path->x.size(); ++i) { 98 | len += std::hypot(astar_path->x[i] - astar_path->x[i - 1], astar_path->y[i] - astar_path->y[i - 1]); 99 | } 100 | astar_path->cost = len; 101 | return true; 102 | } 103 | 104 | bool AStar::SearchPath(double sx, double sy, double ex, double ey, AStarResult *result) { 105 | if (result == nullptr) { 106 | ROS_FATAL("[AStar::SearchPath], the input pointer is nullptr"); 107 | return false; 108 | } 109 | result->x.clear(); 110 | result->y.clear(); 111 | boost::heap::binomial_heap, boost::heap::compare> priority_queue; 112 | std::unordered_map, 113 | boost::heap::compare < cmp >> ::handle_type > open_set_handles; 114 | std::unordered_map> open_set; 115 | std::unordered_map> close_set; 116 | std::shared_ptr start_node = std::make_shared(sx, sy, grid_map_); 117 | std::shared_ptr end_node = std::make_shared(ex, ey, grid_map_); 118 | 119 | if (!VerifyNode2d(start_node)) { 120 | ROS_FATAL("[AStar::SearchPath], the start node is not collision free"); 121 | return false; 122 | } 123 | if (!VerifyNode2d(end_node)) { 124 | ROS_FATAL("[AStar::SearchPath], the end node is not collision free"); 125 | return false; 126 | } 127 | final_node_ = nullptr; 128 | auto handle = priority_queue.emplace(start_node); 129 | open_set_handles.emplace(start_node->GetIndex(), handle); 130 | 131 | while (!priority_queue.empty()) { 132 | std::shared_ptr current_node = priority_queue.top(); 133 | priority_queue.pop(); 134 | open_set_handles.erase(current_node->GetIndex()); 135 | open_set.erase(current_node->GetIndex()); 136 | 137 | if (*(current_node) == *(end_node)) { 138 | final_node_ = current_node; 139 | break; 140 | } 141 | close_set.emplace(current_node->GetIndex(), current_node); 142 | std::vector> next_nodes = GenerateNextNodes(current_node); 143 | for (auto &next_node : next_nodes) { 144 | if (!VerifyNode2d(next_node)) { 145 | continue; 146 | } 147 | if (close_set.find(next_node->GetIndex()) != close_set.end()) { 148 | continue; 149 | } 150 | double tentative_g = current_node->GetGCost() + EdgeCost(current_node, next_node); 151 | if (open_set.find(next_node->GetIndex()) == open_set.end()) { 152 | next_node->SetHCost(EuclidDistance(next_node, end_node)); 153 | next_node->SetPreNode(current_node); 154 | next_node->SetGcost(tentative_g); 155 | open_set.emplace(next_node->GetIndex(), next_node); 156 | auto next_handle = priority_queue.emplace(next_node); 157 | open_set_handles.emplace(next_node->GetIndex(), next_handle); 158 | } else if (open_set[next_node->GetIndex()]->GetGCost() > tentative_g) { 159 | // open_set[next_node->GetIndex()] = next_node; 160 | open_set[next_node->GetIndex()]->SetGcost(tentative_g); 161 | open_set[next_node->GetIndex()]->SetPreNode(current_node); 162 | priority_queue.update(open_set_handles[next_node->GetIndex()], open_set[next_node->GetIndex()]); 163 | } 164 | } 165 | } 166 | 167 | if (final_node_ == nullptr) { 168 | // ROS_FATAL("[AStar::SearchPath]:, the final node is nullptr, Failed to find Path"); 169 | return false; 170 | } 171 | this->TracePath(result); 172 | return true; 173 | } 174 | 175 | bool AStar::GenerateDpMap(double ex, double ey) { 176 | auto end_node = std::make_shared(ex, ey, grid_map_); 177 | if (!VerifyNode2d(end_node)) { 178 | ROS_FATAL("[AStar::GenerateDpMap], the end node is not collision free"); 179 | return false; 180 | } 181 | dp_map_.clear(); 182 | 183 | boost::heap::binomial_heap, boost::heap::compare> priority_queue; 184 | std::unordered_map, boost::heap::compare < cmp >> ::handle_type > open_set_handles; 186 | std::unordered_map> open_set; 187 | 188 | end_node->SetPreNode(nullptr); 189 | end_node->SetFcost(0.0); 190 | auto handle = priority_queue.emplace(end_node); 191 | open_set.emplace(end_node->GetIndex(), end_node); 192 | open_set_handles.emplace(end_node->GetIndex(), handle); 193 | 194 | while (!priority_queue.empty()) { 195 | auto current_node = priority_queue.top(); 196 | priority_queue.pop(); 197 | open_set.erase(current_node->GetIndex()); 198 | open_set_handles.erase(current_node->GetIndex()); 199 | dp_map_.emplace(current_node->GetIndex(), current_node); 200 | std::vector> next_nodes = this->GenerateNextNodes(current_node); 201 | for (auto &next_node : next_nodes) { 202 | if (!VerifyNode2d(next_node)) { 203 | continue; 204 | } 205 | // dp_map_ acts as closed set 206 | if (dp_map_.find(next_node->GetIndex()) != dp_map_.end()) { 207 | continue; 208 | } 209 | double tentative_f = current_node->GetFCost() + EdgeCost(current_node, next_node); 210 | if (open_set.find(next_node->GetIndex()) == open_set.end()) { 211 | next_node->SetPreNode(current_node); 212 | next_node->SetFcost(tentative_f); 213 | open_set.emplace(next_node->GetIndex(), next_node); 214 | auto next_handle = priority_queue.emplace(next_node); 215 | open_set_handles.emplace(next_node->GetIndex(), next_handle); 216 | } else if (open_set[next_node->GetIndex()]->GetFCost() > tentative_f) { 217 | open_set[next_node->GetIndex()]->SetFcost(tentative_f); 218 | open_set[next_node->GetIndex()]->SetPreNode(current_node); 219 | priority_queue.update(open_set_handles[next_node->GetIndex()], open_set[next_node->GetIndex()]); 220 | } 221 | } 222 | } 223 | return true; 224 | } 225 | 226 | double AStar::CheckDpMap(double sx, double sy) { 227 | int index_x, index_y; 228 | Pose2Index(sx, sy, grid_map_, &index_x, &index_y); 229 | std::string index = std::to_string(index_x) + "_" + std::to_string(index_y); 230 | if (dp_map_.find(index) != dp_map_.end()) { 231 | return dp_map_[index]->GetFCost(); 232 | } else { 233 | return std::numeric_limits::infinity(); 234 | } 235 | } 236 | 237 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # hybrid-a-star 2 | 3 | ## 依赖 4 | 5 | - Ubuntu 18.04 6 | - ROS Melodic 7 | 8 | ## Ros Graph 9 | 10 | ![](figs/rosgraph.png) 11 | 12 | 13 | 14 | ## 车辆模型: 15 | 16 | ![](figs/vehicle_model.png) 17 | 18 | $(x,y)$: 车辆定位位置，在后轴中心处 19 | 20 | $\theta$: 车辆的朝向 21 | 22 | $L$: 轴距 23 | 24 | $\rho$: 转弯半径 25 | 26 | 27 | $$ 28 | \begin{bmatrix} 29 | \dot{x} \\ 30 | \dot{y} \\ 31 | \dot{\theta} 32 | \end{bmatrix} = 33 | v \begin{bmatrix} 34 | \cos\theta \\ 35 | \sin\theta \\ 36 | \frac{\tan{\phi}}{L} 37 | \end{bmatrix} 38 | $$ 39 | 40 | 41 | 更新方程: 42 | 43 | 44 | $$ 45 | \begin{bmatrix} 46 | x_{k+1} \\ 47 | y_{k+1} \\ 48 | \theta_{k+1} 49 | \end{bmatrix} 50 | = 51 | \begin{bmatrix} 52 | x_k + \Delta s\cos\theta \\ 53 | y_k + \Delta s\sin\theta \\ 54 | \theta_k + \frac{\Delta s}{L} * \tan\phi_k 55 | \end{bmatrix} 56 | $$ 57 | 58 | 59 | $\Delta s = v\Delta t$: 机器人运动距离, 60 | 61 | ## Dubin Curve 62 | 63 | 假设车辆的起点为 $q_I$, 终点为$q_F$, 那么求解最短路径可以写成求解如下优化问题： 64 | $$ 65 | \min L(q,u) = \min \int_0^{t_F} \sqrt{\dot{x}(t)^2 + \dot{y}(t)^2} \text{d}t 66 | $$ 67 | 假设车辆运行速度恒定，那么，（3）式中的代价函数 $L(q, u)$ 只与 $t_F$ 相关。 68 | 69 | 已经证明，给定任意的起点和终点，Dubin car的最短路径总是可以表示为不超过三种的motion primitives. 70 | 71 | Dubin car 的action 只有 $u\in\{-1, 0, 1\}$. 并且可以证明，只有六种motion primitives 是最优的。 72 | $$ 73 | \{L_\alpha R_\beta L_\gamma, R_\alpha L_\beta R_\gamma,L_\alpha S_d L_\gamma,L_\alpha S_dR_\gamma,R_\alpha S_d L_\gamma,R_\alpha S_d R_\gamma\} 74 | $$ 75 | 其中 $\alpha\in [0, 2\pi), \gamma \in [0, 2\pi), \beta \in (\pi, 2\pi), d \geq 0$. 76 | 77 | 78 | 79 | ### 向量法计算切线 80 | 81 | ![](figs/compute_tangents_via_the_vector_method.png) 82 | 83 | 1. 给定两个圆$C_1, C_2$, 圆心分别是$p_1, p_2$; 84 | 85 | 2. 连接$p_1, p_2$, 构造向量$\vec{V_1}$, 向量的模长为$D$; 86 | 87 | 3. 绘制两个圆的公切线, 如上图所示为$\vec{V_2} = p_{\text{opt}2} - p_{\text{opt}1}$. 88 | 89 | 4. 绘制$\vec{V_2}$ 的法向量，$\hat{n}$, 为单位向量。 90 | 91 | 5. 向量之间的关系： 92 | 93 | - $\vec{V_2} \cdot \hat{n} = 0$, 94 | 95 | - $\hat{n} \cdot \hat{n} = 1$ 96 | 97 | - 修改$\vec{V_1}, 使得 $$\vec{V_1} ,\vec{V_2}$ 平行： $\vec{V_1^t} = \vec{V_1} + (r_2 - r_1)\hat{n}$. 98 | 99 | - 所以 100 | $$ 101 | \hat{n} \cdot \vec{V_1^t} = \hat{n} \cdot (\vec{V_1} + (r_2 - r_1)\cdot \hat{n}) = 0 102 | $$ 103 | 得到 104 | $$ 105 | \hat{n} \cdot \vec{V_1} = r_1 - r_2 106 | $$ 107 | 两边同除以$D$,得到 108 | $$ 109 | \hat{V_1} \cdot \hat{n} = \frac{r_1 - r_2}{D} 110 | $$ 111 | 其中 $\hat{V_1} = \vec{V_1} /D$. 112 | 113 | 令$c = \frac{r_1 -r_2}{D}$, 为 $\vec{V_1}, \hat{n}$ 的夹角的余弦值。 114 | 115 | 因此可以从$\hat{V_1}$ 旋转$\arccos c$ 得到$\hat{n}$. 116 | 117 | 假设$\hat{n} = [n_x, n_y]^T$, $\hat{V_1} = [v_{1x}, v_{1y}]^T$. 118 | 119 | 因此有 120 | $$ 121 | n_x = v_{1x} c - v_{1y} \sqrt{1-c^2} \\ 122 | n_y = v_{1x} \sqrt{1-c^2} + v_{1y} c 123 | $$ 124 | 那么如计算切点$p_{\text{opt}1,2}$ 呢？ 125 | $$ 126 | p_{\text{opt}1,x} - x_1 = r_1 \times n_x \\ 127 | p_{\text{opt}2,y} - y_1 = r_1 \times n_y 128 | $$ 129 | $x_1, y_1$ 为$C_1$ 的圆心。 130 | 131 | 同理可以计算$p_{\text{opt}2}$. 132 | 133 | 134 | 135 | ### 计算弧长 136 | 137 | ![](figs/arc_length_calc.png) 138 | 139 | 给定一个圆$C_1$, 圆心坐标为$p_1$. 半径为$r_1$, 计算从$p_2$到$p_3$ 的弧长。圆的方向为$d$, 表示"left"/"right".上图所示的方向为"right". 140 | 141 | ![](figs/arc_length_algorithm.png) 142 | 143 | ### CSC Path 144 | 145 | 在有了上面的基础，可以计算CSC path了。这里以计算$RSR$ 路径为例子。 146 | 147 | 起点坐标为$[x_1, y_1,\theta_1]^T$, 终点的坐标为$[x_2, y_2, \theta_2]^T$, 因此起点圆的方向$d = \text{"right"}$, 终点圆的方向为$d=\text{"right"}$. 148 | 149 | 因此可以构造起点圆$C_1$ 位于起点的右边，圆心坐标为$p_1 = [x_{p_1}, y_{p_1}]^T$ 150 | $$ 151 | [x_{p_1}, y_{p_1}]^T = [x_1 + r_\min \cos(\theta_1 - \frac{\pi}{2}), y_1 + r_\min \sin(\theta_1 - \frac{\pi}{2})]^T 152 | $$ 153 | 154 | 155 | 同理，可以构造终点圆心坐标为$p_2 = [x_{p_2}, y_{p_2}]^T$ 156 | $$ 157 | [x_{p_2}, y_{p_2}]^T = [x_2 + r_\min \cos(\theta_2 - \frac{\pi}{2}), y_2 + r_\min \sin(\theta_2 - \frac{\pi}{2})]^T 158 | $$ 159 | 如下图所示 160 | 161 | ![](figs/RSR.png) 162 | 163 | 注意到，只有一组切线是有效的，计算出$p_{\text{op1}},p_{\text{op2}}$, 就可以计算出整个路径。其他类似计算。 164 | 165 | ### CCC Path 166 | 167 | ![](figs/CCC_path.png) 168 | 169 | 如果起点和终点的距离$d \le 4r_{\min}$, CCC path is valid. 170 | 171 | 计算CCC path, 除了构造起点圆和终点圆外，必须构造第三个圆，如上图中的$C_3$. 172 | 173 | $|\overline{p_1p_2}| = D = \sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}$, $|\overline{p_1p_3}| =|\overline{p_2p_3}| = 2r_{\min}$. 174 | 175 | $\theta = \angle {p_2p_1p_3} = \arccos \frac{D}{4r_{\min}}$ 176 | 177 | $\vec{V_1} = p2 - p1$, 如果是$LRL$ 路径，$C_3$ 应当在$C_1$ 的左侧，那么，$\vec{V_1}$ 应当向右旋转，指向$p_3$, 如上图所示。 178 | 179 | 所以$p_3 - p_1$的角度为 $\alpha = \text{atan2}(\vec{V_1}) - \theta $ 180 | 181 | 如果是RLR 路径，那么$\alpha = \text{atan2}(\vec{V_1}) + \theta $ 182 | 183 | 可以计算$p_3$ 的坐标为 184 | $$ 185 | [x_1 + 2r_\min\cos(\alpha), y_1 + 2r_\min \sin(\alpha)]^T 186 | $$ 187 | 进一步，可以计算$p_{\text{pt1}}, p_{\text{pt2}}$, 从而得到CCC path. 188 | 189 | ## Reeds-Shepp Curves 190 | 191 | 区别于Dubin Car, Reeds-Shepp Car允许车辆前向运动，也允许车辆后向运动，车辆模型仍然采用简单的后轴为中心的汽车模型，假设速度恒定为1, Reed-Shepp Car运动方程如下 192 | $$ 193 | \dot{x} = u_1\cos\theta \\ 194 | \dot{y} = u_1 \sin\theta \\ 195 | \dot{\theta} = u_1u_2 196 | $$ 197 | 其中，$u_1\in\{-1,1\}, u_2 \in[-\tan\phi_\max, \tan\phi_\max]$.当$u_1 =1$的时候，表示车辆前向运动，当$u_1=-1$,表示车辆向后运动。 198 | 199 | Reeds Shepp Car从起点到终点的最短路径，一定是下面的words中的其中之一。word 中的"|"表示车辆运动朝向从正向转为反向，或者从反向转为正向。 200 | $$ 201 | {C|C|C, CC|C, C|CC, CSC, CC_\beta|C_\beta,C|C_\beta C_\beta|C,C|C_{\pi/2}SC,CSC_{\pi/2}|C, C|C_{\pi/2}SC_{\pi/2}|C} 202 | $$ 203 | 204 | 205 | 每个word 都由$L^+, L^-, R^+,R^-,S^+,S^-$这六种primitives 组成。Reeds-Shepp曲线的word所有组合不超过48种，所有组合如下 图所示。 206 | 207 | ![](figs/reeds-shepp-curve-words.png) 208 | 209 | 210 | 211 | 根据车辆运动学模型的微分方程如下： 212 | $$ 213 | \begin{array}{cl} \dot{x}(t) &= V(t)\cos\bigl(\psi(t)\bigr) \quad\\ \dot{y}(t) &= 214 | V(t)\sin\bigl(\psi(t)\bigr) \quad\\ \dot{\psi}(t) &= \dfrac{V(t)}{R(t)} \end{array} 215 | $$ 216 | 217 | 218 | 为了更好的表示路径，对车辆行驶路径进行归一化处理，简化路径的表示方法。即限制$V(t)=±1$、$|ψ˙(t)|=1$，所以车辆只能在自身方向ψ(t)上以速度|V(t)=1|前进或后退，并且变化率ψ˙(t)≤1**rad/s**。对于车辆的转弯半径可以通过缩放变换到1，即车辆绕单位圆行驶。这样表示的好处在于车辆行驶的弧长与变化的角度一致。如果直线行驶，则$\dot{\psi} = 0$ 219 | 220 | 积分形式为 221 | $$ 222 | \begin{array}{cl} 223 | x(t) &= x(0) + \int^t_0V(\tau)\cos\bigl(\psi(\tau)\bigr)d\tau\\ 224 | y(t) &= y(0) + \int^t_0V(\tau)\sin\bigl(\psi(\tau)\bigr)d\tau\\ 225 | \psi(t) &= \psi(0) + \int^t_0\dot{\psi}(\tau)d\tau 226 | \end{array} 227 | $$ 228 | 229 | 230 | ### 可行路径 231 | 232 | 根据(16)积分可以得到，起始坐标表示为$(x,y,\psi)$, 经过时间t, 根据积分可以得到 233 | $$ 234 | x(t) = x + \sin\tau|^{\psi+t}_\psi \\ 235 | y(t) = y - \cos\tau|^{\psi+t}_\psi \\ 236 | \psi(t) = t 237 | $$ 238 | 对应的路径如下 239 | $$ 240 | \begin{array}{cl} 241 | L_t^+(x,y,\psi) &= \ \bigl(x + \sin(\psi+t)- \sin(\psi),y - \cos(\psi+t)+\cos(\psi),\psi + t\bigr)\\ 242 | R_t^+(x,y,\psi) &= \ \bigl(x - \sin(\psi-t)+ \sin(\psi),y + \cos(\psi-t)-\cos(\psi),\psi - t\bigr)\\ 243 | S_t^+(x,y,\psi) &= \ (x+t\cos(\psi),y + t\sin(\psi),\psi) 244 | \end{array} 245 | $$ 246 | 247 | $$ 248 | \begin{array}{cl} 249 | L_t^-(x,y,\psi) &= \ \bigl(x - \sin(\psi-t)+ \sin(\psi),y + \cos(\psi-t)-\cos(\psi),\psi - t\bigr)\\ 250 | R_t^-(x,y,\psi) &= \ \bigl(x + \sin(\psi+t)- \sin(\psi),y - \cos(\psi+t)+\cos(\psi),\psi + t\bigr)\\ 251 | S_t^-(x,y,\psi) &= \ (x-t\cos(\psi),y - t\sin(\psi),\psi) 252 | \end{array} 253 | $$ 254 | 255 | ### 位置姿态统一 256 | 257 | 由于车辆的起始位置和终点位置无法穷举，所以一般在计算路径之前，需要将车辆的位置和姿态作归一化： 258 | 259 | 假设起始姿态为$q_i = (x_1, y_1, \psi_1)$, 目标姿态为 $q_g = (x_2, y_2, \psi_2)$, 车辆转弯半径为$r = \rho$. 260 | 261 | 平移 262 | 263 | 首先将向量$\vec{q_iq_g}$ 平移到原点$(0,0)$, 平移向量为$(-x_1,-y_1)$. 平移后的向量为 264 | $$ 265 | \vec{q_iq_g}= 266 | \left[ 267 | \begin{array}{cl} 268 | x_2-x_1\\ 269 | y_2-y_1 270 | \end{array}\right]= 271 | \left[ 272 | \begin{array}{cl} 273 | x\\ 274 | y 275 | \end{array} 276 | \right]\tag{9} 277 | $$ 278 | 应用旋转矩阵，将车辆的起始点朝向转到x轴正向，对应得旋转角度为$\psi_1$，旋转矩阵如下： 279 | $$ 280 | \vec{q_iq_g}= 281 | \left[ 282 | \begin{array}{cl} 283 | x\\ 284 | y 285 | \end{array} 286 | \right]= 287 | \left[ 288 | \begin{array}{cl} 289 | \cos(\psi_1) &\sin(\psi_1)\\ 290 | -\sin(\psi_1) &\cos(\psi_1) 291 | \end{array} 292 | \right] 293 | \left[ 294 | \begin{array}{cl} 295 | x\\ 296 | y 297 | \end{array} 298 | \right]= 299 | \left[ 300 | \begin{array}{cl} 301 | x\cos(\psi_1) + y\sin(\psi_1)\\ 302 | -x\sin(\psi_1)+y\cos(\psi_1) 303 | \end{array} 304 | \right] 305 | $$ 306 | 307 | 308 | 通过上述旋转后，目标位置朝向为$\psi = \psi_2 - \psi_1$ 309 | 310 | 对于车辆的转弯半径，将车辆的转弯半径缩放到1，得到的最终的车辆姿态为 311 | $$ 312 | q_g= 313 | \left[ 314 | \begin{array}{cl} 315 | \bigl(x\cos(\psi_1) + y\sin(\psi_1)\bigr)/\rho\\ 316 | \bigl(-x\sin(\psi_1)+y\cos(\psi_1)\bigr)/\rho\\ 317 | \psi_2-\psi_1 318 | \end{array} 319 | \right] 320 | $$ 321 | 322 | ### 类型转换 323 | 324 | 上述48个字段中的某些方程的最小值拥有两个解，所以实际公式将达到68个。凭经验观察，似乎只要48个表达式在任何情况下都需要，一些表达式无疑是方程的虚假解。在可以考虑的48种不同的字符模式中，由于存在一些基本变换，没有必要给出所有的形式。下面列出3种常见的转换模式： 325 | 326 | **时间变换(timeflip)** 327 | 328 | 时间变换通过交换字母上标符号+和−，即车取反向的行进方向。也就是说，$L^-R^+S^+L^+$表达式可以通过$L^+R^- S^-L^-$表达式通过时间变换获得。其中+,-交换。如果原始路径从(0,0,0)到 $(x,y,ψ)$，显而易见时间变换的路径将从$(0,0,0)$到$(−x,y,−ψ)$。因此，如果一个路径表达式$L^+_tR^-_{-\pi/2}S^-_uL^-_v$从点$(0,0,0)$到$(−x,y,−ψ)$，查找的合适弧长为t、u、v。这就等效于路径表达式 $L^-_tR^+_{\pi/2}S^+_uL^+_v$从点$(0,0,0)$到$(x,y,ψ)$ 查找相应的弧长。所以，通过时间变换将上述列表中第一个字母符号为”−“的字段消除。 329 | 330 | **反射变换(reflect)** 331 | 332 | 反射变换通过交换字母L和R，即取反车辆转向。也就是说，一个路径表达式$R^+L^−S^−R^−$的解可以通过反射变换从路径表达式$L^+R^−S^−L^−$的解中获得，即沿着该路径交换L和R。相应的参考路径由$(x,y,ψ)$变为$(x,−y,−ψ)$。 333 | 334 | 假设已知路径$L^+_tR^-_{-\pi/2}S^-_uL^-_v$解的表达式，从点$(0,0,0) \to (x,-y,-\psi)$的最优弧长为t、u、v。 335 | $$ 336 | \begin{array}{cl} 337 | &L^+_tR^-_{-\pi/2}S^-_uL^-_v :(0,0,0) \to (x,-y,-\psi)\\ 338 | \end{array} 339 | $$ 340 | 341 | 342 | 则从点$(0,0,0) \to (x,y,\psi)$的最优弧长为t、u、v，对应路径$R^+_tL^-_{-\pi/2}S^-_uR^-_v$的解， 343 | $$ 344 | \begin{array}{cl} 345 | &R^+_tL^-_{-\pi/2}S^-_uR^-_v :(0,0,0) \to (x,y,\psi)\\ 346 | \end{array} 347 | $$ 348 | 349 | 350 | 所以，通过反射变换可以将上述列表中以$L^+$开头的字段消除。 351 | 352 | **逆向变换(backwards)** 353 | 354 | 逆向变换通过将原路径按照相反方向行走。也就是说，路径$L^-S^-R^-L^+$的公式可以使用逆向变换从路径$L^+R^-S^-L^-$的公式中获得，即按照相反的顺序运动。逆向变换将目标点$(x,y,\psi)$转化为$\bigl(x\cos(\psi)+y\sin(\psi),x\sin(\psi)-y\cos(\psi),\psi\bigr)$。所以可以通过公式$L^+R^-S^-L^-$到达点$\bigl(x\cos(\psi)+y\sin(\psi),x\sin(\psi)-y\cos(\psi),\psi\bigr)$，从而获得$L^-S^-R^-L^+$到达点$(x,y,\psi)$的解。通过这个变换可以消除上述列表中一些字段，最终保留9个字段讨论如下。具体计算过程见论文8.1-8.11. 355 | 356 | ## Heuristic 357 | 358 | $$ 359 | h = \max(h_\text{nonholo},h_\text{holo} ) 360 | $$ 361 | 362 | ### Non-holonomic Without Obstacle 363 | 364 | 其中，$h_\text{nonholo}$ 为忽略障碍物，考虑车辆非完整约束的情况下，从当前节点计算出来的路径长度，用Reeds-Sheep 长度表示。 365 | 366 | ### Holonomic With Obstacle 367 | 368 | $h_\text{holo}$ 为不考虑机器人的非完整约束，考虑环境中的障碍物计算的heuristic. 由A* 算法搜索出来的从当前节点到终点的路径长度。 369 | 370 | ## Results 371 | 372 | ![](figs/hybrid_astar_result1.png) 373 | 374 | ![](figs/hybrid_astar_result3.png) 375 | 376 | ![](figs/hybrid_astar_result4.png) 377 | 378 | ![](figs/hybrid_astar_result7.png) 379 | 380 | ## Future Works 381 | 382 | - [ ] path smooth 383 | 384 | - [ ] assign velocity profile 385 | 386 | -------------------------------------------------------------------------------- /src/hybrid_a_star.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace planning { 13 | 14 | Node3d::Node3d(double x, double y, double phi) { 15 | x_ = x; 16 | y_ = y; 17 | phi_ = phi; 18 | } 19 | 20 | Node3d::Node3d(double x, double y, double phi, 21 | nav_msgs::OccupancyGrid::Ptr grid_map, 22 | double phi_resolution) { 23 | x_ = x; 24 | y_ = y; 25 | phi_ = phi; 26 | Pose2Index(x, y, std::move(grid_map), &index_x_, &index_y_); 27 | index_phi_ = static_cast((phi_ - (-M_PI)) / phi_resolution); 28 | index_ = std::to_string(index_x_) + "_" + std::to_string(index_y_) + "_" + std::to_string(index_phi_); 29 | traversed_x_.push_back(x); 30 | traversed_y_.push_back(y); 31 | traversed_phi_.push_back(phi); 32 | step_size_ = traversed_x_.size(); 33 | 34 | } 35 | 36 | Node3d::Node3d(const std::vector &traversed_x, 37 | const std::vector &traversed_y, 38 | const std::vector &traversed_phi, 39 | nav_msgs::OccupancyGrid::Ptr grid_map, 40 | double phi_resolution) { 41 | ROS_ASSERT(traversed_x.size() == traversed_y.size()); 42 | ROS_ASSERT(traversed_x.size() == traversed_phi.size()); 43 | traversed_x_ = traversed_x; 44 | traversed_y_ = traversed_y; 45 | traversed_phi_ = traversed_phi; 46 | x_ = traversed_x_.back(); 47 | y_ = traversed_y_.back(); 48 | phi_ = traversed_phi_.back(); 49 | Pose2Index(x_, y_, std::move(grid_map), &index_x_, &index_y_); 50 | index_phi_ = static_cast((phi_ - (-M_PI)) / phi_resolution); 51 | index_ = std::to_string(index_x_) + "_" 52 | + std::to_string(index_y_) + "_" + std::to_string(index_phi_); 53 | step_size_ = traversed_x_.size(); 54 | } 55 | 56 | bool Node3d::operator==(const planning::Node3d &other) const { 57 | return other.GetIndex() == index_; 58 | } 59 | 60 | /////////////////////////// hybrid a star class //////////////// 61 | HybridAStar::HybridAStar(const ros::NodeHandle &nh) { 62 | nh_ = nh; 63 | ha_star_path_publisher_ = nh_.advertise("/ha_path", 1); 64 | visualized_vehicle_publisher_ = nh_.advertise("/visualized_vehicle", 1); 65 | visulized_path_publisher_ = nh_.advertise("/visualized_path", 1); 66 | 67 | nh_.param("/next_node_num", next_node_num_, 10); 68 | nh_.param("/max_steer_angle", max_steer_angle_, M_PI / 5.0); 69 | nh_.param("/adc_width", adc_width_, 1.5); 70 | nh_.param("/adc_length", adc_length_, 3.0); 71 | nh_.param("/phi_resolution", phi_resolution_, 0.1); 72 | nh_.param("/traj_forward_penalty", traj_forward_penalty_, .5); 73 | nh_.param("/traj_backward_penalty", traj_backward_penalty_, 0.8); 74 | nh_.param("/traj_gear_switch_penalty", traj_gear_switch_penalty_, 1.); 75 | nh_.param("/traj_steer_penalty", traj_steer_penalty_, 0.1); 76 | nh_.param("/traj_steer_change_penalty", traj_steer_change_penalty_, .5); 77 | nh_.param("/traj_step_length_penalty", traj_step_length_penalty_, 0.1); 78 | nh_.param("/step_size", step_size_, 0.7); 79 | nh_.param("/wheel_base", wheel_base_, 1.6); 80 | nh_.param("/enable_backward", enable_backward_, true); 81 | nh_.param("/delta_t", delta_t_, 0.4); 82 | nh_.param("/anlytic_expand_distance", anlytic_expand_distance_, 10); 83 | 84 | grid_map_subscriber_ = nh_.subscribe("/map", 10, &HybridAStar::SetMap, this); 85 | init_pose_subscriber_ = nh_.subscribe("/initialpose", 10, &HybridAStar::SetStart, this); 86 | goal_pose_subscriber_ = nh_.subscribe("/move_base_simple/goal", 10, &HybridAStar::SetGoal, this); 87 | heuristic_generator_ = std::make_unique(); 88 | } 89 | 90 | void HybridAStar::RunOnce() { 91 | if (!has_grid_map_ || !valid_start_ || !valid_goal_) { 92 | ROS_FATAL("[HybridAStar::RunOnce: has_grid_map_ : %s, " 93 | "valid_start_: %s, valid_goal_: %s]", 94 | has_grid_map_ ? "true" : "false", 95 | valid_start_ ? "true" : "false", 96 | valid_goal_ ? "true" : "false"); 97 | return; 98 | } 99 | 100 | HybridAStarResult result; 101 | // AStarResult result; 102 | double sx = start_pose_.pose.pose.position.x; 103 | double sy = start_pose_.pose.pose.position.y; 104 | double sphi = tf::getYaw(start_pose_.pose.pose.orientation); 105 | double ex = goal_pose_.pose.position.x; 106 | double ey = goal_pose_.pose.position.y; 107 | double ephi = tf::getYaw(goal_pose_.pose.orientation); 108 | ros::Time t0 = ros::Time::now(); 109 | // auto plan_result = heuristic_generator_->SearchPath(sx, sy, ex, ey, &result); 110 | auto plan_result = this->Search(sx, sy, sphi, ex, ey, ephi, &result); 111 | ros::Time t1 = ros::Time::now(); 112 | ROS_WARN(" SearchTime: %lf ms", (t1 - t0).toSec() * 1000.0); 113 | if (!plan_result) { 114 | ROS_FATAL("[HybridAStar::RunOnce]: HybridAStar Search Failed"); 115 | return; 116 | } 117 | ROS_WARN("RunOnce, the plan_result: %s", plan_result ? "true" : "false"); 118 | ROS_WARN("RunOnce, result size: %zu", result.y.size()); 119 | nav_msgs::Path published_path = HybridAStar::HybridAStarResult2NavPath(result); 120 | // nav_msgs::Path published_path = this->AStarResult2NavPath(result); 121 | ha_star_path_publisher_.publish(published_path); 122 | VisualizedPath(published_path); 123 | 124 | } 125 | 126 | void HybridAStar::VisualizedPath(const nav_msgs::Path &path) { 127 | visualization_msgs::Marker pathNode; 128 | visualization_msgs::MarkerArray pathNodes; 129 | pathNode.action = visualization_msgs::Marker::DELETE; 130 | visualization_msgs::Marker pathVehicle; 131 | visualization_msgs::MarkerArray pathVehicles; 132 | pathVehicle.header.frame_id = "/map"; 133 | pathVehicle.header.stamp = ros::Time::now(); 134 | pathVehicle.lifetime = ros::Duration(1.0); 135 | pathVehicle.type = visualization_msgs::Marker::CUBE; 136 | pathVehicle.scale.x = adc_length_; 137 | pathVehicle.scale.y = adc_width_; 138 | pathVehicle.scale.z = 1; 139 | pathVehicle.color.a = 0.1; 140 | 141 | pathNode.action = visualization_msgs::Marker::ADD; 142 | pathNode.lifetime = ros::Duration(1.0); 143 | 144 | pathNode.header.frame_id = "/map"; 145 | pathNode.header.stamp = ros::Time::now(); 146 | pathNode.type = visualization_msgs::Marker::SPHERE; 147 | pathNode.scale.x = 0.1; 148 | pathNode.scale.y = 0.1; 149 | pathNode.scale.z = 0.1; 150 | pathNode.color.a = 1.0; 151 | pathNode.color.r = 1.0; 152 | pathNode.color.g = 0.5; 153 | pathNode.color.b = 0.5; 154 | pathNodes.markers.clear(); 155 | pathVehicles.markers.clear(); 156 | // geometry_msgs::Point pt; 157 | for (size_t i = 0; i < path.poses.size(); ++i) { 158 | pathNode.id = i; 159 | pathVehicle.id = i; 160 | pathVehicle.pose = path.poses[i].pose; 161 | pathNode.pose.position.x = path.poses[i].pose.position.x; 162 | pathNode.pose.position.y = path.poses[i].pose.position.y; 163 | pathNode.pose.orientation = path.poses[i].pose.orientation; 164 | pathNodes.markers.push_back(pathNode); 165 | pathVehicles.markers.push_back(pathVehicle); 166 | } 167 | visualized_vehicle_publisher_.publish(pathVehicles); 168 | visulized_path_publisher_.publish(pathNodes); 169 | 170 | } 171 | 172 | nav_msgs::Path HybridAStar::AStarResult2NavPath(const planning::AStarResult &astar_result) { 173 | size_t result_size = astar_result.x.size(); 174 | nav_msgs::Path path; 175 | path.header.frame_id = "/path"; 176 | path.header.stamp = ros::Time::now(); 177 | path.poses.resize(result_size); 178 | for (size_t i = 0; i < result_size; ++i) { 179 | const double x = (astar_result).x[i]; 180 | const double y = (astar_result).y[i]; 181 | path.poses[i].pose.position.x = x; 182 | path.poses[i].pose.position.y = y; 183 | } 184 | return path; 185 | } 186 | 187 | nav_msgs::Path HybridAStar::HybridAStarResult2NavPath(const planning::HybridAStarResult &result) { 188 | size_t result_size = result.x.size(); 189 | nav_msgs::Path path; 190 | path.header.frame_id = "/path"; 191 | path.header.stamp = ros::Time::now(); 192 | path.poses.resize(result_size); 193 | for (size_t i = 0; i < result_size; ++i) { 194 | const double x = result.x[i]; 195 | const double y = result.y[i]; 196 | const double phi = result.phi[i]; 197 | path.poses[i].pose.position.x = x; 198 | path.poses[i].pose.position.y = y; 199 | tf::Quaternion q; 200 | q.setRPY(0.0, 0.0, phi); 201 | geometry_msgs::Quaternion quaternion; 202 | 203 | tf::quaternionTFToMsg(q, quaternion); 204 | path.poses[i].pose.orientation = quaternion; 205 | } 206 | return path; 207 | } 208 | 209 | bool HybridAStar::Search(double sx, double sy, 210 | double sphi, double ex, 211 | double ey, double ephi, 212 | planning::HybridAStarResult *result) { 213 | if (result == nullptr) { 214 | ROS_ERROR("[HybridAStar::Search] : the input pointer is nullptr"); 215 | return false; 216 | } 217 | result->x.clear(); 218 | result->y.clear(); 219 | result->phi.clear(); 220 | result->steer.clear(); 221 | result->a.clear(); 222 | result->accumulated_s.clear(); 223 | result->v.clear(); 224 | start_node_.reset( 225 | new Node3d(sx, sy, sphi, grid_map_, phi_resolution_)); 226 | start_node_->SetDirection(true); 227 | open_set_.clear(); 228 | priority_queue_ = decltype(priority_queue_)(); 229 | open_set_handles_ = decltype(open_set_handles_)(); 230 | closed_set_.clear(); 231 | final_node_ = nullptr; 232 | if (!CheckNode3d(start_node_)) { 233 | ROS_ERROR("[HybridAStar::Search], the start node is not collision free"); 234 | return false; 235 | } 236 | if (!CheckNode3d(end_node_)) { 237 | ROS_ERROR("[HybridAStar::Search], the end node is not collision free"); 238 | return false; 239 | } 240 | open_set_.emplace(start_node_->GetIndex(), start_node_); 241 | auto handle = priority_queue_.emplace(start_node_); 242 | open_set_handles_.emplace(start_node_->GetIndex(), handle); 243 | int expand_node_num = 0; 244 | double tie_breaker = 1.01; 245 | int cnt = 0; 246 | while (!priority_queue_.empty()) { 247 | cnt++; 248 | const auto current_node = priority_queue_.top(); 249 | priority_queue_.pop(); 250 | open_set_.erase(current_node->GetIndex()); 251 | open_set_handles_.erase(current_node->GetIndex()); 252 | ros::Time t0 = ros::Time::now(); 253 | if (current_node->IsInRange(end_node_, anlytic_expand_distance_)) { 254 | if (AnalyticExpansion(current_node)) { 255 | break; 256 | } 257 | } 258 | if (*(current_node) == *(end_node_)) { 259 | final_node_ = current_node; 260 | break; 261 | } 262 | closed_set_.emplace(current_node->GetIndex(), current_node); 263 | t0 = ros::Time::now(); 264 | for (size_t i = 0; i < next_node_num_; ++i) { 265 | std::shared_ptr next_node = NextNodeGenerator(current_node, i); 266 | if (next_node == nullptr) { 267 | continue; 268 | } 269 | if (closed_set_.find(next_node->GetIndex()) != closed_set_.end()) { 270 | continue; 271 | } 272 | bool check_result = CheckNode3d(next_node); 273 | if (!check_result) { 274 | continue; 275 | } 276 | double tentative_gcost = current_node->GetGCost() + EdgeCost(current_node, next_node); 277 | if (open_set_.find(next_node->GetIndex()) == open_set_.end()) { 278 | expand_node_num++; 279 | next_node->SetGCost(tentative_gcost); 280 | 281 | next_node->SetHCost(tie_breaker * std::max(HoloWithObstacleHeuristic(next_node), NonHoloWithoutObstacleHeuristic(next_node))); 282 | 283 | next_node->SetPreNode(current_node); 284 | open_set_.emplace(next_node->GetIndex(), next_node); 285 | auto next_handle = priority_queue_.emplace(next_node); 286 | open_set_handles_.emplace(next_node->GetIndex(), next_handle); 287 | } else if (open_set_[next_node->GetIndex()]->GetGCost() > tentative_gcost) { 288 | open_set_[next_node->GetIndex()]->SetGCost(tentative_gcost); 289 | open_set_[next_node->GetIndex()]->SetPreNode(current_node); 290 | priority_queue_.update(open_set_handles_[next_node->GetIndex()], open_set_[next_node->GetIndex()]); 291 | } 292 | } 293 | if (cnt > 100000) { 294 | return false; 295 | } 296 | } 297 | 298 | if (final_node_ == nullptr) { 299 | ROS_ERROR("[Search], the final node_ is nullptr"); 300 | return false; 301 | } 302 | return this->LoadResult(result); 303 | } 304 | 305 | bool HybridAStar::LoadResult(planning::HybridAStarResult *result) { 306 | if (result == nullptr) { 307 | return false; 308 | } 309 | std::shared_ptr current_node = final_node_; 310 | std::vector hybrid_a_x; 311 | std::vector hybrid_a_y; 312 | std::vector hybrid_a_phi; 313 | while (current_node->GetPreNode() != nullptr) { 314 | std::vector x = current_node->GetXs(); 315 | std::vector y = current_node->GetYs(); 316 | std::vector phi = current_node->GetPhis(); 317 | if (x.empty() || y.empty() || phi.empty()) { 318 | return false; 319 | } 320 | if (x.size() != y.size() || x.size() != phi.size()) { 321 | return false; 322 | } 323 | std::reverse(x.begin(), x.end()); 324 | std::reverse(y.begin(), y.end()); 325 | std::reverse(phi.begin(), phi.end()); 326 | x.pop_back(); 327 | y.pop_back(); 328 | phi.pop_back(); 329 | hybrid_a_x.insert(hybrid_a_x.end(), x.begin(), x.end()); 330 | hybrid_a_y.insert(hybrid_a_y.end(), y.begin(), y.end()); 331 | hybrid_a_phi.insert(hybrid_a_phi.end(), phi.begin(), phi.end()); 332 | current_node = current_node->GetPreNode(); 333 | } 334 | hybrid_a_x.push_back(current_node->GetX()); 335 | hybrid_a_y.push_back(current_node->GetY()); 336 | hybrid_a_phi.push_back(current_node->GetPhi()); 337 | std::reverse(hybrid_a_x.begin(), hybrid_a_x.end()); 338 | std::reverse(hybrid_a_y.begin(), hybrid_a_y.end()); 339 | std::reverse(hybrid_a_phi.begin(), hybrid_a_phi.end()); 340 | (*result).x = hybrid_a_x; 341 | (*result).y = hybrid_a_y; 342 | (*result).phi = hybrid_a_phi; 343 | 344 | std::vector partitioned_results; 345 | if (!TrajectoryPartition(result, &partitioned_results)) { 346 | return false; 347 | } 348 | HybridAStarResult stitched_result; 349 | for (const auto &res : partitioned_results) { 350 | std::copy(res.x.begin(), res.x.end() - 1, 351 | std::back_inserter(stitched_result.x)); 352 | std::copy(res.y.begin(), res.y.end() - 1, 353 | std::back_inserter(stitched_result.y)); 354 | std::copy(res.phi.begin(), res.phi.end() - 1, 355 | std::back_inserter(stitched_result.phi)); 356 | std::copy(res.v.begin(), res.v.end() - 1, 357 | std::back_inserter(stitched_result.v)); 358 | std::copy(res.a.begin(), res.a.end(), 359 | std::back_inserter(stitched_result.a)); 360 | std::copy(res.steer.begin(), res.steer.end(), 361 | std::back_inserter(stitched_result.steer)); 362 | } 363 | stitched_result.x.push_back(partitioned_results.back().x.back()); 364 | stitched_result.y.push_back(partitioned_results.back().y.back()); 365 | stitched_result.phi.push_back(partitioned_results.back().phi.back()); 366 | stitched_result.v.push_back(partitioned_results.back().v.back()); 367 | if (stitched_result.x.size() != stitched_result.y.size() || 368 | stitched_result.x.size() != stitched_result.v.size() || 369 | stitched_result.x.size() != stitched_result.phi.size()) { 370 | return false; 371 | } 372 | if (stitched_result.a.size() != stitched_result.steer.size() || 373 | stitched_result.x.size() - stitched_result.a.size() != 1) { 374 | return false; 375 | } 376 | *result = stitched_result; 377 | 378 | return true; 379 | } 380 | 381 | std::shared_ptr HybridAStar::NextNodeGenerator( 382 | const std::shared_ptr ¤t_node, 383 | size_t next_node_index) { 384 | double steering = 0.0; 385 | double traveled_distance = 0.0; 386 | double step_size = std::sqrt(2) * grid_map_->info.resolution; 387 | if (next_node_index < static_cast(next_node_num_) / 2) { 388 | steering = 389 | -max_steer_angle_ + 390 | (2 * max_steer_angle_ / (static_cast(next_node_num_) / 2 - 1)) * 391 | static_cast(next_node_index); 392 | traveled_distance = step_size_; 393 | } else { 394 | size_t index = next_node_index - next_node_num_ / 2; 395 | steering = 396 | -max_steer_angle_ + 397 | (2 * max_steer_angle_ / (static_cast(next_node_num_) / 2 - 1)) * 398 | static_cast(index); 399 | traveled_distance = -step_size_; 400 | } 401 | double arc = std::sqrt(2) * grid_map_->info.resolution; 402 | std::vector intermediate_x; 403 | std::vector intermediate_y; 404 | std::vector intermediate_phi; 405 | double last_x = current_node->GetX(); 406 | double last_y = current_node->GetY(); 407 | double last_phi = current_node->GetPhi(); 408 | intermediate_x.push_back(last_x); 409 | intermediate_y.push_back(last_y); 410 | intermediate_phi.push_back(last_phi); 411 | for (size_t i = 0; i < arc / step_size_; ++i) { 412 | const double next_x = last_x + traveled_distance * std::cos(last_phi); 413 | const double next_y = last_y + traveled_distance * std::sin(last_phi); 414 | const double next_phi = NormalizeAngle( 415 | last_phi + 416 | traveled_distance / wheel_base_ * std::tan(steering)); 417 | intermediate_x.push_back(next_x); 418 | intermediate_y.push_back(next_y); 419 | intermediate_phi.push_back(next_phi); 420 | last_x = next_x; 421 | last_y = next_y; 422 | last_phi = next_phi; 423 | } 424 | std::shared_ptr next_node = std::make_shared( 425 | intermediate_x, intermediate_y, intermediate_phi, 426 | grid_map_, phi_resolution_); 427 | 428 | next_node->SetSteering(steering); 429 | next_node->SetDirection(traveled_distance > 0.0); 430 | return next_node; 431 | } 432 | 433 | double HybridAStar::EdgeCost(const std::shared_ptr ¤t_node, 434 | const std::shared_ptr &next_node) const { 435 | 436 | double edge_cost = static_cast((next_node->GetStepSize() - 1)) * step_size_ * traj_step_length_penalty_; 437 | 438 | if (next_node->GetDirection()) { 439 | edge_cost += traj_forward_penalty_ * (next_node->GetStepSize() - 1) * step_size_; 440 | } else { 441 | edge_cost += traj_backward_penalty_ * (next_node->GetStepSize() - 1) * step_size_; 442 | } 443 | if (current_node->GetDirection() != next_node->GetDirection()) { 444 | edge_cost += traj_gear_switch_penalty_; 445 | } 446 | if (std::fabs(next_node->GetSteering()) > phi_resolution_) { 447 | edge_cost += traj_steer_penalty_ * std::fabs(next_node->GetSteering()); 448 | } 449 | if (std::fabs(next_node->GetSteering() - current_node->GetSteering()) > phi_resolution_) { 450 | edge_cost += traj_steer_change_penalty_ * 451 | std::fabs(next_node->GetSteering() - current_node->GetSteering()); 452 | } 453 | return edge_cost; 454 | 455 | } 456 | 457 | double HybridAStar::HoloWithObstacleHeuristic( 458 | const std::shared_ptr &next_node) { 459 | if (!has_dp_map_) { 460 | return 0.0; 461 | } 462 | return (heuristic_generator_->CheckDpMap( 463 | next_node->GetX(), 464 | next_node->GetY())); 465 | // return h; 466 | } 467 | 468 | double HybridAStar::NonHoloWithoutObstacleHeuristic( 469 | const std::shared_ptr &next_node) const { 470 | 471 | ////////////////// dubin cost ////////////////////// 472 | double non_holo_heu = 0.0; 473 | double dubin_cost = 0.0; 474 | double reedsShepp_cost = 0.0; 475 | const double min_r = 1.0 / (std::tan(max_steer_angle_) / wheel_base_); 476 | if (!enable_backward_) { 477 | ompl::base::DubinsStateSpace dubin_path(min_r); 478 | auto dubin_start = (ompl::base::SE2StateSpace::StateType *) dubin_path.allocState(); 479 | auto dubin_end = (ompl::base::SE2StateSpace::StateType *) dubin_path.allocState(); 480 | dubin_start->setXY(next_node->GetX(), next_node->GetY()); 481 | dubin_start->setYaw(next_node->GetPhi()); 482 | dubin_end->setXY(end_node_->GetX(), end_node_->GetY()); 483 | dubin_end->setYaw(end_node_->GetPhi()); 484 | dubin_cost = dubin_path.distance(dubin_start, dubin_end); 485 | non_holo_heu = dubin_cost; 486 | } else { 487 | /////////////////////////////reedsShepp cost //////////// 488 | ompl::base::ReedsSheppStateSpace reeds_shepp_path(min_r); 489 | auto rs_start = (ompl::base::ReedsSheppStateSpace::StateType *) reeds_shepp_path.allocState(); 490 | auto rs_end = (ompl::base::ReedsSheppStateSpace::StateType *) reeds_shepp_path.allocState(); 491 | rs_start->setXY(next_node->GetX(), next_node->GetY()); 492 | rs_start->setYaw(next_node->GetPhi()); 493 | rs_end->setXY(end_node_->GetX(), end_node_->GetY()); 494 | rs_end->setYaw(end_node_->GetPhi()); 495 | reedsShepp_cost = reeds_shepp_path.distance(rs_start, rs_end); 496 | // ROS_WARN("rs cost : %lf, dubin cost : %lf", reedsShepp_cost, dubin_cost); 497 | non_holo_heu = reedsShepp_cost; 498 | } 499 | 500 | return non_holo_heu; 501 | } 502 | 503 | bool HybridAStar::CheckNode3d(const std::shared_ptr &node) { 504 | size_t node_step_size = node->GetStepSize(); 505 | auto traversed_x = node->GetXs(); 506 | auto traversed_y = node->GetYs(); 507 | auto traversed_phi = node->GetPhis(); 508 | size_t check_start_index = 0; 509 | if (node_step_size > 1) { 510 | check_start_index = 1; 511 | } 512 | for (size_t i = check_start_index; i < node_step_size; ++i) { 513 | const double x = traversed_x[i]; 514 | const double y = traversed_y[i]; 515 | const double phi = traversed_phi[i]; 516 | if (!CheckPose3d(x, y, phi, adc_length_, 517 | adc_width_, wheel_base_ / 2.0, 0.5, grid_map_)) { 518 | return false; 519 | } 520 | } 521 | return true; 522 | } 523 | 524 | bool HybridAStar::AnalyticExpansion(const std::shared_ptr ¤t_node) { 525 | if (current_node == nullptr) { 526 | ROS_ERROR("[HybridAStar::AnalyticExpansion], " 527 | "the input current node is nullptr"); 528 | return false; 529 | } 530 | double dubin_length = 0.0; 531 | double reedsSheep_length = 0.0; 532 | if (enable_backward_) { 533 | auto final_node = this->ReedsSheppShot( 534 | current_node, &reedsSheep_length); 535 | if (final_node == nullptr) { 536 | return false; 537 | } else { 538 | final_node_ = std::move(final_node); 539 | final_node_->SetPreNode(current_node); 540 | closed_set_.emplace(final_node_->GetIndex(), final_node_); 541 | return true; 542 | } 543 | } else { 544 | auto final_node = this->DubinShot(current_node, &dubin_length); 545 | if (final_node == nullptr) { 546 | return false; 547 | } else { 548 | final_node_ = std::move(final_node); 549 | final_node_->SetPreNode(current_node); 550 | closed_set_.emplace(final_node_->GetIndex(), final_node_); 551 | return true; 552 | } 553 | } 554 | } 555 | 556 | std::shared_ptr HybridAStar::DubinShot(const std::shared_ptr &node, double *len) { 557 | if (node == nullptr) { 558 | return nullptr; 559 | } 560 | const double min_r = 1.0 / (std::tan(max_steer_angle_) / wheel_base_); 561 | ompl::base::StateSpacePtr space(std::make_shared(min_r)); 562 | ompl::base::ScopedState<> from(space), to(space), s(space); 563 | from[0] = node->GetX(); 564 | from[1] = node->GetY(); 565 | from[2] = node->GetPhi(); 566 | to[0] = end_node_->GetX(); 567 | to[1] = end_node_->GetY(); 568 | to[2] = end_node_->GetPhi(); 569 | std::vector reals; 570 | *len = space->distance(from(), to()); 571 | double ratio = step_size_ / *len; 572 | double t = 0.0; 573 | std::vector traversed_x, traversed_y, traversed_phi; 574 | while (t < 1.0 + ratio) { 575 | space->interpolate(from(), to(), t, s()); 576 | reals = s.reals(); 577 | // std::cout << "x: " << reals[0] << ", y: " << reals[1] << ", z: " << reals[2] << std::endl; 578 | traversed_x.push_back(reals[0]); 579 | traversed_y.push_back(reals[1]); 580 | traversed_phi.push_back(reals[2]); 581 | t += ratio; 582 | } 583 | auto final_node = 584 | std::make_shared( 585 | traversed_x, 586 | traversed_y, 587 | traversed_phi, 588 | grid_map_, 589 | phi_resolution_); 590 | if (!CheckNode3d(final_node)) { 591 | return nullptr; 592 | } 593 | return final_node; 594 | } 595 | 596 | std::shared_ptr HybridAStar::ReedsSheppShot( 597 | const std::shared_ptr &node, double *len) { 598 | 599 | if (node == nullptr) { 600 | return nullptr; 601 | } 602 | const double min_r = 1.0 / (std::tan(max_steer_angle_) / wheel_base_); 603 | ompl::base::StateSpacePtr space(std::make_shared(min_r)); 604 | ompl::base::ScopedState<> from(space), to(space), s(space); 605 | from[0] = node->GetX(); 606 | from[1] = node->GetY(); 607 | from[2] = node->GetPhi(); 608 | to[0] = end_node_->GetX(); 609 | to[1] = end_node_->GetY(); 610 | to[2] = end_node_->GetPhi(); 611 | std::vector reals; 612 | *len = space->distance(from(), to()); 613 | double delta_t = 0.1; 614 | double ratio = step_size_ / *len; 615 | double t = 0.0; 616 | std::vector traversed_x, traversed_y, traversed_phi; 617 | while (t < 1.0 + ratio) { 618 | space->interpolate(from(), to(), t, s()); 619 | reals = s.reals(); 620 | // std::cout << "x: " << reals[0] << ", y: " << reals[1] << ", z: " << reals[2] << std::endl; 621 | 622 | traversed_x.push_back(reals[0]); 623 | traversed_y.push_back(reals[1]); 624 | traversed_phi.push_back(reals[2]); 625 | t += ratio; 626 | } 627 | auto final_node = 628 | std::make_shared( 629 | traversed_x, 630 | traversed_y, 631 | traversed_phi, 632 | grid_map_, 633 | phi_resolution_); 634 | if (!CheckNode3d(final_node)) { 635 | return nullptr; 636 | } 637 | return final_node; 638 | } 639 | 640 | bool HybridAStar::TrajectoryPartition( 641 | const planning::HybridAStarResult *result, 642 | std::vector *parition_results) const { 643 | if (result == nullptr || parition_results == nullptr) { 644 | return false; 645 | } 646 | const auto &x = result->x; 647 | const auto &y = result->y; 648 | const auto &phi = result->phi; 649 | if (x.size() != y.size() || x.size() != phi.size()) { 650 | return false; 651 | } 652 | size_t horizon = x.size(); 653 | parition_results->clear(); 654 | parition_results->emplace_back(); 655 | auto *current_traj = &(parition_results->back()); 656 | double heading_angle = phi.front(); 657 | double tracking_angle = std::atan2(y[1] - y[0], x[1] - x[0]); 658 | bool current_gear = std::fabs(CalcAngleDist(tracking_angle, heading_angle)) < M_PI_2; 659 | for (size_t i = 0; i < horizon - 1; ++i) { 660 | heading_angle = phi[i]; 661 | tracking_angle = std::atan2(y[i + 1] - y[i], x[i + 1] - x[i]); 662 | bool gear = std::fabs(CalcAngleDist(tracking_angle, heading_angle)) < M_PI_2; 663 | if (gear != current_gear) { 664 | current_traj->x.push_back(x[i]); 665 | current_traj->y.push_back(y[i]); 666 | current_traj->phi.push_back(phi[i]); 667 | parition_results->emplace_back(); 668 | current_traj = &(parition_results->back()); 669 | current_gear = gear; 670 | } 671 | current_traj->x.push_back(x[i]); 672 | current_traj->y.push_back(y[i]); 673 | current_traj->phi.push_back(phi[i]); 674 | } 675 | current_traj->x.push_back(x.back()); 676 | current_traj->y.push_back(y.back()); 677 | current_traj->phi.push_back(phi.back()); 678 | for (auto &traj : *parition_results) { 679 | if (traj.x.size() < 2 || traj.y.size() < 2 || traj.phi.size() < 2) { 680 | return false; 681 | } 682 | size_t x_size = traj.x.size(); 683 | traj.v.push_back(0.0); 684 | 685 | for (size_t i = 1; i + 1 < x_size; ++i) { 686 | double discrete_v = (((traj.x[i + 1] - traj.x[i]) / delta_t_) * 687 | std::cos(traj.phi[i]) + 688 | ((traj.x[i] - traj.x[i - 1]) / delta_t_) * 689 | std::cos(traj.phi[i])) / 690 | 2.0 + 691 | (((traj.y[i + 1] - traj.y[i]) / delta_t_) * 692 | std::sin(traj.phi[i]) + 693 | ((traj.y[i] - traj.y[i - 1]) / delta_t_) * 694 | std::sin(traj.phi[i])) / 695 | 2.0; 696 | traj.v.push_back(discrete_v); 697 | } 698 | traj.v.push_back(0.0); 699 | for (size_t i = 0; i < x_size - 1; ++i) { 700 | const double discrete_a = (traj.v[i + 1] - traj.v[i]) / delta_t_; 701 | traj.a.push_back(discrete_a); 702 | } 703 | for (size_t i = 0; i + 1 < x_size; ++i) { 704 | double discrete_steer = (traj.phi[i + 1] - traj.phi[i]) * 705 | wheel_base_ / step_size_; 706 | if (traj.v[i] > 0.0) { 707 | discrete_steer = std::atan(discrete_steer); 708 | } else { 709 | discrete_steer = std::atan(-discrete_steer); 710 | } 711 | traj.steer.push_back(discrete_steer); 712 | } 713 | 714 | } 715 | return true; 716 | } 717 | void HybridAStar::SetGoal(const geometry_msgs::PoseStamped_>::Ptr &goal) { 718 | if (!has_grid_map_) { 719 | return; 720 | } 721 | bool new_goal = true; 722 | if (std::hypot(goal->pose.position.x - goal_pose_.pose.position.x, 723 | goal->pose.position.y - goal_pose_.pose.position.y) < grid_map_->info.resolution && 724 | CalcAngleDist(tf::getYaw(goal->pose.orientation), tf::getYaw(goal_pose_.pose.orientation)) < phi_resolution_) { 725 | new_goal = false; 726 | } 727 | if (!CheckPose3d( 728 | goal->pose.position.x, 729 | goal->pose.position.y, 730 | tf::getYaw(goal->pose.orientation), 731 | adc_length_, adc_width_, 732 | wheel_base_ / 2.0, 2.0 * grid_map_->info.resolution, grid_map_)) { 733 | valid_goal_ = false; 734 | return; 735 | } 736 | goal_pose_ = *goal; 737 | 738 | valid_goal_ = true; 739 | // heuristic_lookup_table_.clear(); 740 | 741 | if (valid_goal_ && has_grid_map_) { 742 | heuristic_generator_->SetMap(grid_map_); 743 | heuristic_generator_->SetVhicleParams(adc_length_, adc_width_, wheel_base_ / 2.0); 744 | if (!heuristic_generator_->GenerateDpMap(goal_pose_.pose.position.x, goal_pose_.pose.position.y)) { 745 | has_dp_map_ = false; 746 | return; 747 | } 748 | has_dp_map_ = true; 749 | std::vector xs{goal_pose_.pose.position.x}; 750 | std::vector ys{goal_pose_.pose.position.y}; 751 | std::vector phis{tf::getYaw(goal_pose_.pose.orientation)}; 752 | end_node_.reset(new Node3d(xs,ys,phis, 753 | grid_map_, phi_resolution_)); 754 | } 755 | } 756 | 757 | void HybridAStar::SetStart(const geometry_msgs::PoseWithCovarianceStamped_>::Ptr &start) { 758 | if (!has_grid_map_) { 759 | return; 760 | } 761 | if (!CheckPose3d(start->pose.pose.position.x, start->pose.pose.position.y, tf::getYaw(start->pose.pose.orientation), 762 | adc_length_, adc_width_, wheel_base_ / 2.0, 2.0 * grid_map_->info.resolution, grid_map_)) { 763 | valid_start_ = false; 764 | return; 765 | } 766 | start_pose_ = *start; 767 | valid_start_ = true; 768 | } 769 | 770 | void HybridAStar::SetMap(const nav_msgs::OccupancyGrid_>::Ptr &grid_map) { 771 | grid_map_ = grid_map; 772 | std::cout << "grid_map_: width : " << grid_map_->info.width << 773 | " height: " << grid_map_->info.height << std::endl; 774 | has_grid_map_ = true; 775 | } 776 | 777 | bool HybridAStar::cmp::operator()(const std::shared_ptr &left, const std::shared_ptr &right) const { 778 | return left->GetFCost() >= right->GetFCost(); 779 | } 780 | } --------------------------------------------------------------------------------