diff --git a/config/navigation.lua b/config/navigation.lua index c5206ba..10b19ce 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -17,6 +17,7 @@ NavigationParameters = { }; laser_frame = "base_link"; odom_topic = "/jackal_velocity_controller/odom"; + -- odom_topic = "/odometry/filtered"; localization_topic = "localization"; image_topic = "/camera/rgb/image_raw/compressed"; init_topic = "initialpose"; @@ -31,10 +32,10 @@ NavigationParameters = { carrot_dist = 20.0; system_latency = 0.24; obstacle_margin = 0.15; - num_options = 62; + num_options = 31; robot_width = 0.44; robot_length = 0.5; - base_link_offset = 0; + base_link_offset = 0.1; max_free_path_length = 10.0; max_clearance = 1.0; can_traverse_stairs = false; diff --git a/src/navigation/ackermann_motion_primitives.cc b/src/navigation/ackermann_motion_primitives.cc index 810dd25..db90aa6 100644 --- a/src/navigation/ackermann_motion_primitives.cc +++ b/src/navigation/ackermann_motion_primitives.cc @@ -19,30 +19,32 @@ */ //======================================================================== +#include "ackermann_motion_primitives.h" + #include #include #include #include -#include "math/poses_2d.h" +#include "config_reader/config_reader.h" +#include "constant_curvature_arcs.h" #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/Geometry" -#include "config_reader/config_reader.h" +#include "math/poses_2d.h" #include "motion_primitives.h" -#include "constant_curvature_arcs.h" -#include "ackermann_motion_primitives.h" -using std::min; +using Eigen::Vector2f; +using pose_2d::Pose2Df; using std::max; -using std::vector; +using std::min; using std::shared_ptr; -using pose_2d::Pose2Df; -using Eigen::Vector2f; +using std::vector; using namespace math_util; CONFIG_FLOAT(max_curvature, "AckermannSampler.max_curvature"); CONFIG_FLOAT(clearance_clip, "AckermannSampler.clearance_path_clip_fraction"); +CONFIG_FLOAT(clearance_padding, "AckermannSampler.clearance_padding"); namespace { // Epsilon value for handling limited numerical precision. @@ -51,8 +53,7 @@ const float kEpsilon = 1e-5; namespace motion_primitives { -AckermannSampler::AckermannSampler() { -} +AckermannSampler::AckermannSampler() {} void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) { ConstantCurvatureArc& path = *path_ptr; @@ -60,24 +61,18 @@ void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) { path.length = min(nav_params.max_free_path_length, local_target.x()); path.fpl = path.length; return; - } + } const float turn_radius = 1.0f / path.curvature; const float quarter_circle_dist = fabs(turn_radius) * M_PI_2; const Vector2f turn_center(0, turn_radius); const Vector2f target_radial = local_target - turn_center; - const Vector2f middle_radial = - fabs(turn_radius) * target_radial.normalized(); + const Vector2f middle_radial = fabs(turn_radius) * target_radial.normalized(); const float middle_angle = atan2(fabs(middle_radial.x()), fabs(middle_radial.y())); const float dist_closest_to_goal = middle_angle * fabs(turn_radius); - path.fpl = min({ - nav_params.max_free_path_length, - quarter_circle_dist}); - path.length = min({ - path.fpl, - dist_closest_to_goal - }); - const float stopping_dist = + path.fpl = min({nav_params.max_free_path_length, quarter_circle_dist}); + path.length = min({path.fpl, dist_closest_to_goal}); + const float stopping_dist = Sq(vel.x()) / (2.0 * nav_params.linear_limits.max_deceleration); path.length = max(path.length, stopping_dist); } @@ -86,29 +81,27 @@ vector> AckermannSampler::GetSamples(int n) { vector> samples; if (false) { samples = { - shared_ptr(new ConstantCurvatureArc(-0.1)), - shared_ptr(new ConstantCurvatureArc(0)), - shared_ptr(new ConstantCurvatureArc(0.1)), + shared_ptr(new ConstantCurvatureArc(-0.1)), + shared_ptr(new ConstantCurvatureArc(0)), + shared_ptr(new ConstantCurvatureArc(0.1)), }; return samples; } - const float max_domega = + const float max_domega = nav_params.dt * nav_params.angular_limits.max_acceleration; - const float max_dv = + const float max_dv = nav_params.dt * nav_params.linear_limits.max_acceleration; const float robot_speed = fabs(vel.x()); float c_min = -CONFIG_max_curvature; float c_max = CONFIG_max_curvature; if (robot_speed > max_dv + kEpsilon) { - c_min = max( - c_min, (ang_vel - max_domega) / (robot_speed - max_dv)); - c_max = min( - c_max, (ang_vel + max_domega) / (robot_speed - max_dv)); + c_min = max(c_min, (ang_vel - max_domega) / (robot_speed - max_dv)); + c_max = min(c_max, (ang_vel + max_domega) / (robot_speed - max_dv)); } const float dc = (c_max - c_min) / static_cast(n - 1); // printf("Options: %6.2f : %6.2f : %6.2f\n", c_min, dc, c_max); if (false) { - for (float c = c_min; c <= c_max; c+= dc) { + for (float c = c_min; c <= c_max; c += dc) { auto sample = new ConstantCurvatureArc(c); SetMaxPathLength(sample); CheckObstacles(sample); @@ -117,7 +110,7 @@ vector> AckermannSampler::GetSamples(int n) { } } else { const float dc = (2.0f * CONFIG_max_curvature) / static_cast(n - 1); - for (float c = -CONFIG_max_curvature; c <= CONFIG_max_curvature; c+= dc) { + for (float c = -CONFIG_max_curvature; c <= CONFIG_max_curvature; c += dc) { auto sample = new ConstantCurvatureArc(c); SetMaxPathLength(sample); CheckObstacles(sample); @@ -125,14 +118,15 @@ vector> AckermannSampler::GetSamples(int n) { samples.push_back(shared_ptr(sample)); } } - + return samples; } void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) { ConstantCurvatureArc& path = *path_ptr; // How much the robot's body extends in front of its base link frame. - const float l = 0.5 * nav_params.robot_length - nav_params.base_link_offset + nav_params.obstacle_margin; + const float l = 0.5 * nav_params.robot_length - nav_params.base_link_offset + + nav_params.obstacle_margin; // The robot's half-width. const float w = 0.5 * nav_params.robot_width + nav_params.obstacle_margin; if (fabs(path.curvature) < kEpsilon) { @@ -149,16 +143,16 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) { path.fpl = max(0.0f, path.fpl); path.length = min(path.fpl, path.length); - const float stopping_dist = - vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration); + const float stopping_dist = + vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration); if (path.fpl < stopping_dist) { path.length = 0; } - + return; } const float path_radius = 1.0 / path.curvature; - const Vector2f c(0, path_radius ); + const Vector2f c(0, path_radius); const float s = ((path_radius > 0.0) ? 1.0 : -1.0); const Vector2f inner_front_corner(l, s * w); const Vector2f outer_front_corner(l, -s * w); @@ -168,11 +162,11 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) { const float r3_sq = (outer_front_corner - c).squaredNorm(); float angle_min = M_PI; path.obstruction = Vector2f(-nav_params.max_free_path_length, 0); - // printf("%7.3f %7.3f %7.3f %7.3f\n", + // printf("%7.3f %7.3f %7.3f %7.3f\n", // path.curvature, sqrt(r1_sq), sqrt(r2_sq), sqrt(r3_sq)); // The x-coordinate of the rear margin. - const float x_min = -0.5 * nav_params.robot_length + - nav_params.base_link_offset - nav_params.obstacle_margin; + const float x_min = -0.5 * nav_params.robot_length + + nav_params.base_link_offset - nav_params.obstacle_margin; using std::isfinite; for (const Vector2f& p : point_cloud) { if (!isfinite(p.x()) || !isfinite(p.y()) || p.x() < 0.0f) continue; @@ -189,9 +183,9 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) { // path.curvature, sqrt(r_sq), r1, sqrt(r2_sq), sqrt(r3_sq)); if (r_sq < r1_sq || r_sq > r3_sq) continue; const float r = sqrt(r_sq); - const float theta = ((path.curvature > 0.0f) ? - atan2(p.x(), path_radius - p.y()) : - atan2(p.x(), p.y() - path_radius)); + const float theta = + ((path.curvature > 0.0f) ? atan2(p.x(), path_radius - p.y()) + : atan2(p.x(), p.y() - path_radius)); float alpha; if (r_sq < r2_sq) { // Point will hit the side of the robot first. @@ -221,18 +215,17 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) { angle_min = theta; } } - const float stopping_dist = + const float stopping_dist = vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration); if (path.length < stopping_dist) path.length = 0; path.length = max(0.0f, path.length); angle_min = min(angle_min, path.length * fabs(path.curvature)); path.clearance = nav_params.max_clearance; - for (const Vector2f& p : point_cloud) { - const float theta = ((path.curvature > 0.0f) ? - atan2(p.x(), path_radius - p.y()) : - atan2(p.x(), p.y() - path_radius)); + const float theta = + ((path.curvature > 0.0f) ? atan2(p.x(), path_radius - p.y()) + : atan2(p.x(), p.y() - path_radius)); if (theta < CONFIG_clearance_clip * angle_min && theta > 0.0) { const float r = (p - c).norm(); const float current_clearance = fabs(r - fabs(path_radius)); @@ -242,7 +235,7 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) { } } path.clearance = max(0.0f, path.clearance); - // printf("%7.3f %7.3f %7.3f \n", + // printf("%7.3f %7.3f %7.3f \n", // path.curvature, path.length, path.clearance); } diff --git a/src/navigation/linear_evaluator.cc b/src/navigation/linear_evaluator.cc index 70896d8..99e9862 100644 --- a/src/navigation/linear_evaluator.cc +++ b/src/navigation/linear_evaluator.cc @@ -48,9 +48,9 @@ using std::vector; using namespace geometry; using namespace math_util; -DEFINE_double(dw, 0.5, "Distance weight"); +DEFINE_double(dw, 1, "Distance weight"); DEFINE_double(cw, -0.5, "Clearance weight"); -DEFINE_double(fw, 0.5, "Free path weight"); +DEFINE_double(fw, -1, "Free path weight"); DEFINE_double(subopt, 1.5, "Max path increase for clearance"); namespace motion_primitives { @@ -128,4 +128,4 @@ void LinearEvaluator::SetSubOpt(const float &threshold) { FLAGS_subopt = threshold; } -} // namespace motion_primitives +} // namespace motion_primitives \ No newline at end of file diff --git a/src/navigation/linear_evaluator.h b/src/navigation/linear_evaluator.h index 6f50213..625a7e2 100644 --- a/src/navigation/linear_evaluator.h +++ b/src/navigation/linear_evaluator.h @@ -28,10 +28,11 @@ namespace motion_primitives { -struct LinearEvaluator : PathEvaluatorBase { +struct LinearEvaluator : PathEvaluatorBase { // Return the best path rollout from the provided set of paths. std::shared_ptr FindBest( const std::vector>& paths) override; + float GetPathCost(const std::shared_ptr& path); void SetClearanceWeight(const float& weight); void SetDistanceWeight(const float& weight); void SetFreePathWeight(const float& weight); @@ -40,5 +41,4 @@ struct LinearEvaluator : PathEvaluatorBase { } // namespace motion_primitives - #endif // LINEAR_EVALUATOR_H diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index ae0dc3c..498371b 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -80,7 +80,7 @@ using namespace motion_primitives; // Special test modes. DEFINE_bool(test_toc, false, "Run 1D time-optimal controller test"); DEFINE_bool(test_obstacle, false, "Run obstacle detection test"); -DEFINE_bool(test_avoidance, false, "Run obstacle avoidance test"); +DEFINE_bool(test_avoidance, true, "Run obstacle avoidance test"); DEFINE_bool(test_osm_planner, false, "Run OSM planner test"); DEFINE_bool(test_planner, false, "Run navigation planner test"); DEFINE_bool(test_latency, false, "Run Latency test"); @@ -940,6 +940,7 @@ bool Navigation::PlanStillValid() { } bool Navigation::IntermediatePlanStillValid() { + const bool kDebug = FLAGS_v > 1; if (plan_path_.size() < 2 || !intermediate_path_found_) return false; // TODO: Add parameter for when to look for new goal or use different @@ -947,12 +948,14 @@ bool Navigation::IntermediatePlanStillValid() { if ((nav_goal_loc_ - plan_path_[0].loc).norm() > sqrt(2 * params_.local_costmap_resolution) / 2 && (robot_loc_ - plan_path_[0].loc).norm() < 1) { + if (kDebug) printf("IntermediatePlanStillValid(): Goal too far\n"); return false; } Vector2f global_carrot; GetGlobalCarrot(global_carrot); if ((intermediate_goal_ - global_carrot).norm() > params_.replan_dist) { + if (kDebug) printf("IntermediatePlanStillValid(): Carrot too far\n"); return false; } @@ -964,6 +967,7 @@ bool Navigation::IntermediatePlanStillValid() { if (in_map && (costmap_.getCost(mx, my) == costmap_2d::LETHAL_OBSTACLE || costmap_.getCost(mx, my) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) { + if (kDebug) printf("IntermediatePlanStillValid(): Obstacle\n"); return false; } } @@ -1382,7 +1386,7 @@ Eigen::Vector2f Navigation::GetIntermediateGoal() { return intermediate_goal_; } int Navigation::GetNextGPSGlobalGoal(int start_goal_index) { if (gps_nav_goals_loc_.empty() || !gps_initialized_) return -1; - const bool kDebug = FLAGS_v > 0; + const bool kDebug = FLAGS_v > 1; // Never go back, Never surrender const auto& final_goal = gps_nav_goals_loc_.back(); for (int i = start_goal_index + 1; i < int(gps_nav_goals_loc_.size()); ++i) { @@ -1713,7 +1717,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, if (gps_goal_index_ + 1 < int(gps_nav_goals_loc_.size())) { if (kDebug) printf("Switching to next GPS Goal\n"); printf("Switching to next GPS Goal\n"); - ReplanAndSetNextNavGoal(true); + ReplanAndSetNextNavGoal(false); } else { nav_state_ = NavigationState::kStopped; } @@ -1760,7 +1764,6 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, // Switch between navigation states. NavigationState prev_state = nav_state_; - bool did_state_change = prev_state != nav_state_; do { prev_state = nav_state_; if (nav_state_ == NavigationState::kGoto && @@ -1776,16 +1779,16 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, switch (nav_state_) { case NavigationState::kStopped: { - if (kDebug && did_state_change) printf("\nNav complete\n"); + if (kDebug) printf("\nNav complete\n"); } break; case NavigationState::kPaused: { if (kDebug) printf("\nNav paused\n"); } break; case NavigationState::kGoto: { - if (kDebug && did_state_change) printf("\nNav Goto\n"); + if (kDebug) printf("\nNav Goto\n"); } break; case NavigationState::kTurnInPlace: { - if (kDebug && did_state_change) printf("\nNav TurnInPlace\n"); + if (kDebug) printf("\nNav TurnInPlace\n"); } break; case NavigationState::kOverride: { if (kDebug) printf("\nNav override\n"); diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index b386254..f081528 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -116,7 +116,7 @@ const string kOpenCVWindow = "Image window"; DEFINE_string(robot_config, "config/navigation.lua", "Robot config file"); DEFINE_string(maps_dir, kAmrlMapsDir, "Directory containing AMRL maps"); DEFINE_bool(no_joystick, true, "Whether to use a joystick or not"); -DEFINE_bool(no_intermed, true, +DEFINE_bool(no_intermed, false, "Whether to disable intermediate planning (will use legacy " "obstacle avoidance planner)"); @@ -206,7 +206,7 @@ void OdometryCallback(const nav_msgs::Odometry& msg) { odom_ = OdomHandler(msg); navigation_.UpdateOdometry(odom_); // Compute global coordinate offset from odometry t'' to odometry t' - // navigation_.UpdateRobotLocFromOdom(odom_); + navigation_.UpdateRobotLocFromOdom(odom_); } void GPSCallback(const std_msgs::Float64MultiArray& msg) { @@ -483,7 +483,7 @@ void SendCommand(Eigen::Vector2f vel, float ang_vel) { vel.setZero(); ang_vel = 0; } - + printf("Sending command: (%f, %f) %f\n", vel.x(), vel.y(), ang_vel); drive_msg.twist.angular.x = 0; drive_msg.twist.angular.y = 0; drive_msg.twist.angular.z = ang_vel;