From ba2f48c98095e8075cec257ad37241df15eeda9f Mon Sep 17 00:00:00 2001 From: mr_winter <913982779@qq.com> Date: Mon, 10 Jun 2024 20:16:44 +0800 Subject: [PATCH] fix bugs: core dump in getLookAheadPoint function --- .../sample_planner/include/rrt_star.h | 2 +- .../sample_planner/include/sample_planner.h | 23 +++---- .../sample_planner/src/quick_informed_rrt.cpp | 58 ++++++++-------- .../sample_planner/src/rrt_star.cpp | 8 +-- .../sample_planner/src/sample_planner.cpp | 6 ++ .../local_planner/src/local_planner.cpp | 66 +++++++++++-------- .../lqr_planner/src/lqr_planner.cpp | 4 -- src/core/utils/include/nodes.h | 9 ++- src/core/utils/src/nodes.cpp | 4 +- .../config/costmap/global_costmap_params.yaml | 6 +- .../config/planner/sample_planner_params.yaml | 4 +- .../include/robots/start_robots.launch.xml | 2 +- src/user_config/user_config.yaml | 2 +- 13 files changed, 105 insertions(+), 89 deletions(-) diff --git a/src/core/global_planner/sample_planner/include/rrt_star.h b/src/core/global_planner/sample_planner/include/rrt_star.h index 5f3a256..14c834f 100644 --- a/src/core/global_planner/sample_planner/include/rrt_star.h +++ b/src/core/global_planner/sample_planner/include/rrt_star.h @@ -51,7 +51,7 @@ class RRTStar : public RRT * @param node sample node * @return nearest node */ - Node _findNearestPoint(std::unordered_map list, Node& node); + Node _findNearestPoint(std::unordered_map& list, Node& node); protected: double r_; // optimization radius diff --git a/src/core/global_planner/sample_planner/include/sample_planner.h b/src/core/global_planner/sample_planner/include/sample_planner.h index 0f3dd4f..3c2dfff 100644 --- a/src/core/global_planner/sample_planner/include/sample_planner.h +++ b/src/core/global_planner/sample_planner/include/sample_planner.h @@ -127,17 +127,18 @@ class SamplePlanner : public nav_core::BaseGlobalPlanner ros::ServiceServer make_plan_srv_; // planning service private: - double tolerance_; // tolerance - bool is_outline_; // whether outline the boudary of map - double factor_; // obstacle inflation factor - bool is_expand_; // whether publish expand map or not - int sample_points_; // random sample points - double sample_max_d_; // max distance between sample points - double opt_r_; // optimization raidus - double prior_set_r_; // radius of priority sample circles - int rewire_threads_n_; // threads number of rewire process - double step_ext_d_; // increased distance of adaptive extend step size - double t_freedom_; // freedom of t distribution + double tolerance_; // tolerance + bool is_outline_; // whether outline the boudary of map + double factor_; // obstacle inflation factor + bool is_expand_; // whether publish expand map or not + int sample_points_; // random sample points + double sample_max_d_; // max distance between sample points + double opt_r_; // optimization raidus + double prior_set_r_; // radius of priority sample circles + int rewire_threads_n_; // threads number of rewire process + double step_ext_d_; // increased distance of adaptive extend step size + double t_freedom_; // freedom of t distribution + std::vector history_plan_; // history plan }; } // namespace sample_planner #endif \ No newline at end of file diff --git a/src/core/global_planner/sample_planner/src/quick_informed_rrt.cpp b/src/core/global_planner/sample_planner/src/quick_informed_rrt.cpp index b4dc069..9899831 100644 --- a/src/core/global_planner/sample_planner/src/quick_informed_rrt.cpp +++ b/src/core/global_planner/sample_planner/src/quick_informed_rrt.cpp @@ -26,14 +26,15 @@ namespace global_planner * @param costmap the environment for path planning * @param max_dist max distance between sample points */ -QuickInformedRRT::QuickInformedRRT(costmap_2d::Costmap2D* costmap, int sample_num, double max_dist, double r, double r_set, int n_threads, double d_extend, double t_freedom) +QuickInformedRRT::QuickInformedRRT(costmap_2d::Costmap2D* costmap, int sample_num, double max_dist, double r, + double r_set, int n_threads, double d_extend, double t_freedom) : InformedRRT(costmap, sample_num, max_dist, r) { set_r_ = r_set; - rewire_threads_ = n_threads; - step_extend_d_ = d_extend; + rewire_threads_ = n_threads; + step_extend_d_ = d_extend; recover_max_dist_ = max_dist; - t_distr_freedom_ = t_freedom; + t_distr_freedom_ = t_freedom; } /** @@ -68,7 +69,7 @@ bool QuickInformedRRT::plan(const Node& start, const Node& goal, std::vector path) std::mt19937 eng(rd()); std::student_t_distribution<> t_distr(t_distr_freedom_); - if(std::abs(t_distr(eng)) < mu && path.size() != 0) // sample in priority circles + if (std::abs(t_distr(eng)) < mu && path.size() != 0) // sample in priority circles { - int wc = rand() % path.size(); - std::uniform_real_distribution p(-set_r_, set_r_); - int cx = path[wc].x_ + p(eng); - int cy = path[wc].y_ + p(eng); - return Node(cx, cy, 0, 0, grid2Index(cx, cy), 0); + int wc = rand() % path.size(); + std::uniform_real_distribution p(-set_r_, set_r_); + int cx = path[wc].x_ + p(eng); + int cy = path[wc].y_ + p(eng); + return Node(cx, cy, 0, 0, grid2Index(cx, cy), 0); } - else // ellipse sample + else // ellipse sample { if (c_best_ < std::numeric_limits::max()) { @@ -159,15 +161,15 @@ Node QuickInformedRRT::_generateRandomNode(int mu, std::vector path) std::uniform_real_distribution p(-1, 1); while (true) { - x = p(eng); - y = p(eng); - if (x * x + y * y < 1) + x = p(eng); + y = p(eng); + if (x * x + y * y < 1) break; } // transform to ellipse Node temp = _transform(x, y); if (temp.id_ < map_size_ - 1) - return temp; + return temp; } } else @@ -220,26 +222,28 @@ Node QuickInformedRRT::_findNearestPoint(std::unordered_map list, Nod // parallel rewire optimization std::vector v_iters; - for (auto p : sample_list_) { + for (auto p : sample_list_) + { v_iters.push_back(p.first); } -# pragma omp parallel for num_threads(rewire_threads_) +#pragma omp parallel for num_threads(rewire_threads_) for (int i = 0; i < v_iters.size(); i++) { - auto &p = sample_list_[v_iters[i]]; + auto& p = sample_list_[v_iters[i]]; // inside the optimization circle double new_dist = helper::dist(p, new_node); - if (new_dist > r_) continue; - + if (new_dist > r_) + continue; + double cost = p.g_ + new_dist; // update new sample node's cost and parent if (new_node.g_ > cost) { if (!_isAnyObstacleInPath(new_node, p)) { - // other thread may update new_node.g_ -# pragma omp critical + // other thread may update new_node.g_ +#pragma omp critical if (new_node.g_ > cost) { new_node.pid_ = p.id_; diff --git a/src/core/global_planner/sample_planner/src/rrt_star.cpp b/src/core/global_planner/sample_planner/src/rrt_star.cpp index 384d782..b6756ac 100644 --- a/src/core/global_planner/sample_planner/src/rrt_star.cpp +++ b/src/core/global_planner/sample_planner/src/rrt_star.cpp @@ -79,7 +79,6 @@ bool RRTStar::plan(const Node& start, const Node& goal, std::vector& path, path = _convertClosedListToPath(sample_list_, start, goal); optimized = true; } - iteration++; } @@ -92,12 +91,11 @@ bool RRTStar::plan(const Node& start, const Node& goal, std::vector& path, * @param node sample node * @return nearest node */ -Node RRTStar::_findNearestPoint(std::unordered_map list, Node& node) +Node RRTStar::_findNearestPoint(std::unordered_map& list, Node& node) { Node nearest_node, new_node(node); double min_dist = std::numeric_limits::max(); - - for (const auto p : list) + for (const auto& p : list) { // calculate distance double new_dist = helper::dist(p.second, new_node); @@ -128,7 +126,7 @@ Node RRTStar::_findNearestPoint(std::unordered_map list, Node& node) if (!_isAnyObstacleInPath(new_node, nearest_node)) { // rewire optimization - for (auto p : sample_list_) + for (auto& p : sample_list_) { // inside the optimization circle double new_dist = helper::dist(p.second, new_node); diff --git a/src/core/global_planner/sample_planner/src/sample_planner.cpp b/src/core/global_planner/sample_planner/src/sample_planner.cpp index 2d84cf2..2b83b24 100644 --- a/src/core/global_planner/sample_planner/src/sample_planner.cpp +++ b/src/core/global_planner/sample_planner/src/sample_planner.cpp @@ -198,12 +198,18 @@ bool SamplePlanner::makePlan(const geometry_msgs::PoseStamped& start, const geom geometry_msgs::PoseStamped goalCopy = goal; goalCopy.header.stamp = ros::Time::now(); plan.push_back(goalCopy); + history_plan_ = plan; } else ROS_ERROR("Failed to get a plan from path when a legal path was found. This shouldn't happen."); } + else if (history_plan_.size() > 0) { + plan = history_plan_; + ROS_WARN("Using history path."); + } else ROS_ERROR("Failed to get a path."); + // publish expand zone if (is_expand_) _publishExpand(expand); diff --git a/src/core/local_planner/local_planner/src/local_planner.cpp b/src/core/local_planner/local_planner/src/local_planner.cpp index f3e362b..7ff627c 100644 --- a/src/core/local_planner/local_planner/src/local_planner.cpp +++ b/src/core/local_planner/local_planner/src/local_planner.cpp @@ -251,17 +251,14 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS return helper::dist(ps, robot_pose_global) >= lookahead_dist; }); - std::vector::const_iterator prev_pose_it; - std::vector::const_iterator pprev_pose_it; // If the no pose is not far enough, take the last pose if (goal_pose_it == prune_plan.end()) { goal_pose_it = std::prev(prune_plan.end()); - prev_pose_it = std::prev(goal_pose_it); - pprev_pose_it = std::prev(prev_pose_it); pt.point.x = goal_pose_it->pose.position.x; pt.point.y = goal_pose_it->pose.position.y; kappa = 0; + theta = atan2(pt.point.y - ry, pt.point.x - rx); } else { @@ -270,13 +267,20 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS // This can be found with a closed form for the intersection of a segment and a circle // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle, // and goal_pose is guaranteed to be outside the circle. - prev_pose_it = std::prev(goal_pose_it); - pprev_pose_it = std::prev(prev_pose_it); - - double px = prev_pose_it->pose.position.x; - double py = prev_pose_it->pose.position.y; + double px, py; double gx = goal_pose_it->pose.position.x; double gy = goal_pose_it->pose.position.y; + if (goal_pose_it == prune_plan.begin()) + { + px = rx; + py = ry; + } + else + { + auto prev_pose_it = std::prev(goal_pose_it); + px = prev_pose_it->pose.position.x; + py = prev_pose_it->pose.position.y; + } // transform to the robot frame so that the circle centers at (0,0) std::pair prev_p(px - rx, py - ry); @@ -286,30 +290,36 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS pt.point.x = i_points[0].first + rx; pt.point.y = i_points[0].second + ry; - double ax = pprev_pose_it->pose.position.x; - double ay = pprev_pose_it->pose.position.y; - double bx = prev_pose_it->pose.position.x; - double by = prev_pose_it->pose.position.y; - double cx = goal_pose_it->pose.position.x; - double cy = goal_pose_it->pose.position.y; - - double a = std::hypot(bx - cx, by - cy); - double b = std::hypot(cx - ax, cy - ay); - double c = std::hypot(ax - bx, ay - by); - - double cosB = (a * a + c * c - b * b) / (2 * a * c); - double sinB = std::sin(std::acos(cosB)); - - double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); - kappa = std::copysign(2 * sinB / b, cross); + auto next_pose_it = std::next(goal_pose_it); + if (next_pose_it != prune_plan.end()) + { + double ax = px; + double ay = py; + double bx = gx; + double by = gy; + double cx = next_pose_it->pose.position.x; + double cy = next_pose_it->pose.position.y; + double a = std::hypot(bx - cx, by - cy); + double b = std::hypot(cx - ax, cy - ay); + double c = std::hypot(ax - bx, ay - by); + + double cosB = (a * a + c * c - b * b) / (2 * a * c); + double sinB = std::sin(std::acos(cosB)); + + double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); + kappa = std::copysign(2 * sinB / b, cross); + } + else + { + kappa = 0.0; + } + theta = atan2(gy - py, gx - px); } + if (std::isnan(kappa)) kappa = 0.0; pt.header.frame_id = goal_pose_it->header.frame_id; pt.header.stamp = goal_pose_it->header.stamp; - - theta = atan2(goal_pose_it->pose.position.y - prev_pose_it->pose.position.y, - goal_pose_it->pose.position.x - prev_pose_it->pose.position.x); } } // namespace local_planner \ No newline at end of file diff --git a/src/core/local_planner/lqr_planner/src/lqr_planner.cpp b/src/core/local_planner/lqr_planner/src/lqr_planner.cpp index 1502c3d..494b095 100644 --- a/src/core/local_planner/lqr_planner/src/lqr_planner.cpp +++ b/src/core/local_planner/lqr_planner/src/lqr_planner.cpp @@ -227,10 +227,6 @@ bool LQRPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) Eigen::Vector3d s_d(lookahead_pt.point.x, lookahead_pt.point.y, theta_trj); // desired state Eigen::Vector2d u_r(vt, vt * kappa); // refered input Eigen::Vector2d u = _lqrControl(s, s_d, u_r); - // ROS_WARN("[INPUT] vt: %f, kappa: %f, s_x: %f, s_y: %f, s_t: %f, s_d_x: %f, s_d_y: %f, s_d_t: %f", vt, kappa, - // s[0], - // s[1], s[2], s_d[0], s_d[1], s_d[2]); - // ROS_WARN("ux: %f, uy: %f", u[0], u[1]); cmd_vel.linear.x = linearRegularization(base_odom, u[0]); cmd_vel.angular.z = angularRegularization(base_odom, u[1]); diff --git a/src/core/utils/include/nodes.h b/src/core/utils/include/nodes.h index 482f336..01c4eb8 100644 --- a/src/core/utils/include/nodes.h +++ b/src/core/utils/include/nodes.h @@ -7,9 +7,9 @@ * @date: 2023-07-21 * @version: 2.1 * - * Copyright (c) 2024, Yang Haodong. + * Copyright (c) 2024, Yang Haodong. * All rights reserved. - * + * * -------------------------------------------------------- * * ******************************************************** @@ -40,7 +40,8 @@ class Node * @param pid node's parent's id */ Node(int x = 0, int y = 0, double g = 0.0, double h = 0.0, int id = 0, int pid = 0); - + Node(const Node& n); + /** * @brief Overloading operator + for Node class * @param n another Node @@ -204,6 +205,4 @@ class LNode : public Node std::multimap::iterator open_it; // iterator }; - - #endif // NODES_H \ No newline at end of file diff --git a/src/core/utils/src/nodes.cpp b/src/core/utils/src/nodes.cpp index fca26bd..6fbcc0c 100644 --- a/src/core/utils/src/nodes.cpp +++ b/src/core/utils/src/nodes.cpp @@ -28,7 +28,9 @@ Node::Node(int x, int y, double g, double h, int id, int pid) : x_(x), y_(y), g_(g), h_(h), id_(id), pid_(pid) { } - +Node::Node(const Node& n) : x_(n.x_), y_(n.y_), g_(n.g_), h_(n.h_), id_(n.id_), pid_(n.pid_) +{ +} /** * @brief Overloading operator + for Node class * @param n another Node diff --git a/src/sim_env/config/costmap/global_costmap_params.yaml b/src/sim_env/config/costmap/global_costmap_params.yaml index d82641e..dd092c3 100644 --- a/src/sim_env/config/costmap/global_costmap_params.yaml +++ b/src/sim_env/config/costmap/global_costmap_params.yaml @@ -5,7 +5,7 @@ global_costmap: update_frequency: 1.0 publish_frequency: 1.0 transform_tolerance: 0.5 - cost_scaling_factor: 1.2 + cost_scaling_factor: 2.0 inflation_radius: 0.8 ### Sensor management parameters ### @@ -19,7 +19,7 @@ global_costmap: data_type: LaserScan, clearing: true, marking: true, - max_obstacle_height: 2.0, + max_obstacle_height: 0.8, min_obstacle_height: 0.0, obstacle_range: 2.5, raytrace_range: 3.0, @@ -28,7 +28,7 @@ global_costmap: inflation_layer: enabled: true - cost_scaling_factor: 1.2 # exponential rate at which the obstacle cost drops off (default: 10) + cost_scaling_factor: 2.0 # exponential rate at which the obstacle cost drops off (default: 10) inflation_radius: 0.8 # max. distance from an obstacle at which costs are incurred for planning paths. static_map: diff --git a/src/sim_env/config/planner/sample_planner_params.yaml b/src/sim_env/config/planner/sample_planner_params.yaml index 6bde80b..fe00ed0 100644 --- a/src/sim_env/config/planner/sample_planner_params.yaml +++ b/src/sim_env/config/planner/sample_planner_params.yaml @@ -2,9 +2,9 @@ SamplePlanner: # random sample points sample_points: 1500 #1000 for informed RRT*, 3000 for others # max distance between sample points - sample_max_d: 40.0 + sample_max_d: 30.0 # optimization radius - optimization_r: 40.0 + optimization_r: 20.0 # offset of transform from world(x,y) to grid map(x,y) convert_offset: 0.0 # error tolerance diff --git a/src/sim_env/launch/include/robots/start_robots.launch.xml b/src/sim_env/launch/include/robots/start_robots.launch.xml index 3229747..d147410 100644 --- a/src/sim_env/launch/include/robots/start_robots.launch.xml +++ b/src/sim_env/launch/include/robots/start_robots.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/src/user_config/user_config.yaml b/src/user_config/user_config.yaml index 323881d..79ac6a2 100644 --- a/src/user_config/user_config.yaml +++ b/src/user_config/user_config.yaml @@ -4,7 +4,7 @@ rviz_file: "sim_env.rviz" robots_config: - robot1_type: "turtlebot3_waffle" - robot1_global_planner: "rrt_connect" + robot1_global_planner: "informed_rrt" robot1_local_planner: "rpp" robot1_x_pos: "0.0" robot1_y_pos: "0.0"