From 8aa385f053521c2884e52b2da905fe1625ceb54b Mon Sep 17 00:00:00 2001 From: artzha Date: Mon, 9 Dec 2024 12:13:57 -0600 Subject: [PATCH] [Improvement] Added exponential clearance separation for trajectory selection --- config/navigation.lua | 17 +- src/navigation/ackermann_motion_primitives.cc | 6 +- src/navigation/ackermann_motion_primitives.h | 6 +- src/navigation/constant_curvature_arcs.cc | 43 ++-- src/navigation/constant_curvature_arcs.h | 20 +- src/navigation/linear_evaluator.cc | 34 ++- src/navigation/linear_evaluator.cc.ckp | 232 ++++++++++++++++++ src/navigation/linear_evaluator.h | 4 +- src/navigation/linear_evaluator.h.bkp | 53 ++++ src/navigation/motion_primitives.h | 6 + src/navigation/navigation.cc | 160 +++++++++++- src/navigation/navigation_main.cc | 4 +- src/navigation/navigation_parameters.h | 108 ++++---- 13 files changed, 574 insertions(+), 119 deletions(-) create mode 100644 src/navigation/linear_evaluator.cc.ckp create mode 100644 src/navigation/linear_evaluator.h.bkp diff --git a/config/navigation.lua b/config/navigation.lua index 10b19ce..6efa310 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -16,8 +16,8 @@ NavigationParameters = { -- "/kinect_laserscan", }; laser_frame = "base_link"; - odom_topic = "/jackal_velocity_controller/odom"; - -- odom_topic = "/odometry/filtered"; + -- odom_topic = "/jackal_velocity_controller/odom"; + odom_topic = "/odometry/filtered"; localization_topic = "localization"; image_topic = "/camera/rgb/image_raw/compressed"; init_topic = "initialpose"; @@ -35,9 +35,10 @@ NavigationParameters = { num_options = 31; robot_width = 0.44; robot_length = 0.5; + robot_wheelbase = 0.26; base_link_offset = 0.1; - max_free_path_length = 10.0; - max_clearance = 1.0; + max_free_path_length = 5.0; + max_clearance = 1.0; -- was 1.0 can_traverse_stairs = false; use_map_speed = true; target_dist_tolerance = 0.1; @@ -69,5 +70,11 @@ NavigationParameters = { AckermannSampler = { max_curvature = 2.5; - clearance_path_clip_fraction = 0.8; + clearance_path_clip_fraction = 0.05; }; + +-- LinearEvaluator = { +-- distance_weight = 1.0; +-- free_path_weight = 1.0; +-- clearance_weight = 1.0; +-- } \ No newline at end of file diff --git a/src/navigation/ackermann_motion_primitives.cc b/src/navigation/ackermann_motion_primitives.cc index db90aa6..9cacec6 100644 --- a/src/navigation/ackermann_motion_primitives.cc +++ b/src/navigation/ackermann_motion_primitives.cc @@ -44,7 +44,6 @@ 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. @@ -226,13 +225,16 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) { 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) { + // if (theta < CONFIG_clearance_clip * angle_min && theta > 0.0) { + if (theta > 0.0) { const float r = (p - c).norm(); const float current_clearance = fabs(r - fabs(path_radius)); if (path.clearance > current_clearance) { path.clearance = current_clearance; } } + + // } } path.clearance = max(0.0f, path.clearance); // printf("%7.3f %7.3f %7.3f \n", diff --git a/src/navigation/ackermann_motion_primitives.h b/src/navigation/ackermann_motion_primitives.h index 68ebb9b..d253317 100644 --- a/src/navigation/ackermann_motion_primitives.h +++ b/src/navigation/ackermann_motion_primitives.h @@ -19,9 +19,6 @@ */ //======================================================================== -#ifndef ACKERMANN_MOTION_PRIMITIVES_H -#define ACKERMANN_MOTION_PRIMITIVES_H - #include #include @@ -30,6 +27,9 @@ #include "math/poses_2d.h" #include "motion_primitives.h" +#ifndef ACKERMANN_MOTION_PRIMITIVES_H +#define ACKERMANN_MOTION_PRIMITIVES_H + namespace motion_primitives { // Path rollout sampler. diff --git a/src/navigation/constant_curvature_arcs.cc b/src/navigation/constant_curvature_arcs.cc index 163a281..e14afe9 100644 --- a/src/navigation/constant_curvature_arcs.cc +++ b/src/navigation/constant_curvature_arcs.cc @@ -19,18 +19,18 @@ */ //======================================================================== +#include "constant_curvature_arcs.h" + #include #include #include -#include "math/poses_2d.h" -#include "math/math_util.h" #include "eigen3/Eigen/Dense" - +#include "math/math_util.h" +#include "math/poses_2d.h" #include "motion_primitives.h" #include "navigation_parameters.h" -#include "constant_curvature_arcs.h" using Eigen::Vector2f; using pose_2d::Pose2Df; @@ -39,32 +39,33 @@ using navigation::MotionLimits; namespace motion_primitives { -float ConstantCurvatureArc::Length() const { - return length; -} +float ConstantCurvatureArc::Length() const { return length; } -float ConstantCurvatureArc::FPL() const { - return fpl; -} +float ConstantCurvatureArc::FPL() const { return fpl; } -float ConstantCurvatureArc::AngularLength() const { - return angular_length; -} +float ConstantCurvatureArc::AngularLength() const { return angular_length; } -float ConstantCurvatureArc::Clearance() const { - return clearance; +float ConstantCurvatureArc::Clearance() const { return clearance; } + +void ConstantCurvatureArc::SetLength(const float& new_length) { + length = new_length; +} +void ConstantCurvatureArc::SetFPL(const float& new_fpl) { fpl = new_fpl; } +void ConstantCurvatureArc::SetAngularLength(const float& new_angular_length) { + angular_length = new_angular_length; +} +void ConstantCurvatureArc::SetClearance(const float& new_clearance) { + clearance = new_clearance; } void ConstantCurvatureArc::GetControls(const MotionLimits& linear_limits, const MotionLimits& angular_limits, - const float dt, - const Vector2f& vel, - const float ang_vel, - Vector2f& vel_cmd, + const float dt, const Vector2f& vel, + const float ang_vel, Vector2f& vel_cmd, float& ang_vel_cmd) const { vel_cmd.y() = 0; - vel_cmd.x() = Run1DTimeOptimalControl( - linear_limits, 0, vel.x(), length, 0, dt); + vel_cmd.x() = + Run1DTimeOptimalControl(linear_limits, 0, vel.x(), length, 0, dt); ang_vel_cmd = vel_cmd.x() * curvature; } diff --git a/src/navigation/constant_curvature_arcs.h b/src/navigation/constant_curvature_arcs.h index 9df00d7..f6133c9 100644 --- a/src/navigation/constant_curvature_arcs.h +++ b/src/navigation/constant_curvature_arcs.h @@ -19,13 +19,11 @@ */ //======================================================================== - #include #include -#include "math/poses_2d.h" #include "eigen3/Eigen/Dense" - +#include "math/poses_2d.h" #include "motion_primitives.h" #ifndef CONSTANT_CURVATURE_ARCS_H @@ -48,12 +46,18 @@ struct ConstantCurvatureArc : PathRolloutBase { // Clearance along path. float Clearance() const override; + // Setters + void SetLength(const float& new_length) override; + void SetFPL(const float& new_fpl) override; + void SetAngularLength(const float& new_angular_length) override; + void SetClearance(const float& new_clearance) override; + // Default constructor. ConstantCurvatureArc() : curvature(0), length(0), angular_length(0) {} // Explicit constructor from curvature. - explicit ConstantCurvatureArc(float curvature) : - curvature(curvature), length(0), angular_length(0) {} + explicit ConstantCurvatureArc(float curvature) + : curvature(curvature), length(0), angular_length(0) {} // The pose of the robot at the end of the path rollout. pose_2d::Pose2Df EndPoint() const override; @@ -63,10 +67,8 @@ struct ConstantCurvatureArc : PathRolloutBase { // The pose of the robot at the end of the path rollout. void GetControls(const navigation::MotionLimits& linear_limits, const navigation::MotionLimits& angular_limits, - const float dt, - const Eigen::Vector2f& linear_vel, - const float angular_vel, - Eigen::Vector2f& vel_cmd, + const float dt, const Eigen::Vector2f& linear_vel, + const float angular_vel, Eigen::Vector2f& vel_cmd, float& ang_vel_cmd) const override; float curvature; diff --git a/src/navigation/linear_evaluator.cc b/src/navigation/linear_evaluator.cc index 99e9862..3e27cc8 100644 --- a/src/navigation/linear_evaluator.cc +++ b/src/navigation/linear_evaluator.cc @@ -48,10 +48,24 @@ using std::vector; using namespace geometry; using namespace math_util; -DEFINE_double(dw, 1, "Distance weight"); -DEFINE_double(cw, -0.5, "Clearance weight"); -DEFINE_double(fw, -1, "Free path weight"); -DEFINE_double(subopt, 1.5, "Max path increase for clearance"); +// DEFINE_double(dw, 1, "Distance weight"); +// DEFINE_double(cw, -3.0, "Clearance weight"); +// DEFINE_double(fw, -1, "Free path weight"); +// DEFINE_double(subopt, 1.5, "Max path increase for clearance"); + +// // Somewhat good paramters! +// DEFINE_double(dw, 1.0, "Distance weight"); +// DEFINE_double(cw, 1.5, "Clearance weight"); +// DEFINE_double(fw, -2.0, "Free path weight"); +// DEFINE_double(subopt, 0.0, "Max path increase for clearance"); +// DEFINE_double(cw_beta, 5.0, "Clearance weight beta"); + +// Somewhat good paramters! +DEFINE_double(dw, 1.0, "Distance weight"); +DEFINE_double(cw, 6.0, "Clearance weight"); +DEFINE_double(fw, -2.0, "Free path weight"); +DEFINE_double(subopt, 0.0, "Max path increase for clearance"); +DEFINE_double(cw_beta, 5.0, "Clearance weight beta"); namespace motion_primitives { @@ -95,15 +109,15 @@ shared_ptr LinearEvaluator::FindBest( } // Next try to find better paths. - float best_cost = FLAGS_dw * (FLAGS_subopt * best_path_length) + - FLAGS_fw * best->Length() + FLAGS_cw * best->Clearance(); + float best_cost = FLAGS_dw * (best_path_length) + FLAGS_fw * best->Length() + + FLAGS_cw * ClearanceCost(best); for (size_t i = 0; i < paths.size(); ++i) { if (paths[i]->Length() <= 0.0f) continue; const float path_length = (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i]) : dist_to_goal[i]); - const float cost = FLAGS_dw * path_length + FLAGS_fw * paths[i]->Length() + - FLAGS_cw * paths[i]->Clearance(); + const float cost = FLAGS_cw * ClearanceCost(paths[i]) + + FLAGS_dw * path_length + FLAGS_fw * paths[i]->Length(); if (cost < best_cost) { best = paths[i]; best_cost = cost; @@ -112,6 +126,10 @@ shared_ptr LinearEvaluator::FindBest( return best; } +float LinearEvaluator::ClearanceCost(const shared_ptr &path) { + return FLAGS_cw * exp(-FLAGS_cw_beta * path->Clearance()); +} + void LinearEvaluator::SetClearanceWeight(const float &weight) { FLAGS_cw = weight; } diff --git a/src/navigation/linear_evaluator.cc.ckp b/src/navigation/linear_evaluator.cc.ckp new file mode 100644 index 0000000..a6434ce --- /dev/null +++ b/src/navigation/linear_evaluator.cc.ckp @@ -0,0 +1,232 @@ +//======================================================================== +// This software is free: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License Version 3, +// as published by the Free Software Foundation. +// +// This software is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// Version 3 in the file COPYING that came with this distribution. +// If not, see . +//======================================================================== +/*! +\file linear_evaluator.h +\brief Path rollout evaluator using a linear weighted cost. +\author Kavan Sikand, (C) 2020 +*/ +//======================================================================== + +#include "linear_evaluator.h" + +#include +#include + +#include +#include +#include + +#include "ackermann_motion_primitives.h" +#include "config_reader/config_reader.h" +#include "constant_curvature_arcs.h" +#include "eigen3/Eigen/Dense" +#include "eigen3/Eigen/Geometry" +#include "gflags/gflags.h" +#include "math/line2d.h" +#include "math/poses_2d.h" +#include "motion_primitives.h" +#include "navigation_parameters.h" + +using Eigen::Vector2f; +using motion_primitives::PathRolloutBase; +using navigation::MotionLimits; +using pose_2d::Pose2Df; +using std::max; +using std::min; +using std::shared_ptr; +using std::vector; +using namespace geometry; +using namespace math_util; + +// CONFIG_FLOAT(pw, "LinearEvaluator.path_weight"); // +CONFIG_FLOAT( + dw, "LinearEvaluator.distance_weight"); // Less distance to goal is better +CONFIG_FLOAT(fw, "LinearEvaluator.free_path_weight"); // +CONFIG_FLOAT(cw, + "LinearEvaluator.clearance_weight"); // More clearance is better + +namespace motion_primitives { +// Function to normalize paths +void LinearEvaluator::NormalizePaths( + const vector>& paths, + const vector& dist_to_goal, + vector>& norm_paths, + vector& norm_dist_to_goal) { + if (paths.empty() || dist_to_goal.empty()) { + return; + } + + // Step 1: Compute min and max values for each property + auto [min_length_it, max_length_it] = std::minmax_element( + paths.begin(), paths.end(), [](const auto& lhs, const auto& rhs) { + return lhs->Length() < rhs->Length(); + }); + + auto [min_fpl_it, max_fpl_it] = std::minmax_element( + paths.begin(), paths.end(), + [](const auto& lhs, const auto& rhs) { return lhs->FPL() < rhs->FPL(); }); + + auto [min_clearance_it, max_clearance_it] = std::minmax_element( + paths.begin(), paths.end(), [](const auto& lhs, const auto& rhs) { + return lhs->Clearance() < rhs->Clearance(); + }); + + auto [min_dist_it, max_dist_it] = + std::minmax_element(dist_to_goal.begin(), dist_to_goal.end()); + + float min_length = (*min_length_it)->Length(); + float max_length = (*max_length_it)->Length(); + float min_fpl = (*min_fpl_it)->FPL(); + float max_fpl = (*max_fpl_it)->FPL(); + float min_clearance = (*min_clearance_it)->Clearance(); + float max_clearance = (*max_clearance_it)->Clearance(); + float min_dist_to_goal = *min_dist_it; + float max_dist_to_goal = *max_dist_it; + + // Step 2: Normalize values + norm_paths = paths; + norm_dist_to_goal.resize(dist_to_goal.size()); + + for (size_t i = 0; i < paths.size(); ++i) { + // Normalize Length + float normalized_length = (paths[i]->Length() - min_length) / + (max_length - min_length + kEpsilon); + norm_paths[i]->SetLength(normalized_length); + + // Normalize FPL + if (max_fpl != min_fpl) { + float normalized_fpl = + (paths[i]->FPL() - min_fpl) / (max_fpl - min_fpl + kEpsilon); + norm_paths[i]->SetFPL(normalized_fpl); + } else { + norm_paths[i]->SetFPL(0); // Default to 0 if min == max + } + + // Normalize Clearance + float normalized_clearance = (paths[i]->Clearance() - min_clearance) / + (max_clearance - min_clearance + kEpsilon); + norm_paths[i]->SetClearance(normalized_clearance); + + // Normalize Distance to Goal (if you want to modify dist_to_goal) + norm_dist_to_goal[i] = (dist_to_goal[i] - min_dist_to_goal) / + (max_dist_to_goal - min_dist_to_goal + kEpsilon); + } +} + +shared_ptr LinearEvaluator::FindBest( + const vector>& paths) { + if (paths.size() == 0) return nullptr; + + // Check if there is any path with an obstacle-free path from the end to the + // local target. + vector clearance_to_goal(paths.size(), 0.0); + vector dist_to_goal(paths.size(), FLT_MAX); + bool path_to_goal_exists = false; + for (size_t i = 0; i < paths.size(); ++i) { + const auto endpoint = paths[i]->EndPoint().translation; + clearance_to_goal[i] = + StraightLineClearance(Line2f(endpoint, local_target), point_cloud); + if (clearance_to_goal[i] > 0.0) { + dist_to_goal[i] = (endpoint - local_target).norm(); + path_to_goal_exists = true; + } + } + + // First find the shortest path. + shared_ptr best = nullptr; + float best_path_length = FLT_MAX; + int best_path_idx = -1; + for (size_t i = 0; i < paths.size(); ++i) { + if (paths[i]->AngularLength() <= 0.0f) continue; + const float path_length = + (path_to_goal_exists ? (paths[i]->AngularLength() + dist_to_goal[i]) + : dist_to_goal[i]); + if (path_length < best_path_length) { + best_path_length = path_length; + best_path_idx = i; + } + } + + if (best_path_idx == -1) { + printf("No valid path found\n"); + // No valid paths! + return nullptr; + } + // vector> norm_paths; + // vector norm_dist_to_goal; + // NormalizePaths(paths, dist_to_goal, norm_paths, norm_dist_to_goal); + best = paths[best_path_idx]; + + // Next try to find better paths. + float best_cost = EvaluatePath(best, dist_to_goal[best_path_idx]); + for (size_t i = 0; i < paths.size(); ++i) { + if (paths[i]->Length() <= 0.0f) continue; + // const float path_length = + // (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i]) + // : dist_to_goal[i]); + const float cost = EvaluatePath(paths[i], dist_to_goal[i]); + if (cost < best_cost) { + best = paths[i]; + best_cost = cost; + } + + if (FLAGS_v > 0) { + const float dist_to_target = + (local_target - best->EndPoint().translation).norm(); + cout << "Best path has free path length: " << best->FPL() + << " and clearance: " << best->Clearance() + << " and distance to target: " << dist_to_target + << " and cost: " << best_cost << endl; + } + } + return best; +} + +float LinearEvaluator::EvaluatePath(const shared_ptr& path, + const float dist_to_goal) { + const float clearance = path->Clearance(); // good + const float free_path_length = path->Length(); // good + const float dist_to_target = dist_to_goal; + + const float score = CONFIG_cw * clearance - CONFIG_fw * free_path_length - + CONFIG_dw * dist_to_target; + + if (FLAGS_v > 1) { + cout << ", Free path length: " << path->FPL() << ", End point: (" + << path->EndPoint().translation << ")" + << ", Clearance: " << path->Clearance() + << ", dist(target): " << dist_to_target << ", Score: " << score + << endl; + } + return score; +} + +void LinearEvaluator::SetClearanceWeight(const float& weight) { + // CONFIG_cw = weight; +} + +void LinearEvaluator::SetDistanceWeight(const float& weight) { + // CONFIG_dw = weight; +} + +void LinearEvaluator::SetFreePathWeight(const float& weight) { + // CONFIG_fw = weight; +} + +// void LinearEvaluator::SetSubOpt(const float &threshold) { +// FLAGS_subopt = threshold; +// } + +} // namespace motion_primitives diff --git a/src/navigation/linear_evaluator.h b/src/navigation/linear_evaluator.h index 625a7e2..e6cbd7c 100644 --- a/src/navigation/linear_evaluator.h +++ b/src/navigation/linear_evaluator.h @@ -32,7 +32,7 @@ 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); + float ClearanceCost(const std::shared_ptr& path); void SetClearanceWeight(const float& weight); void SetDistanceWeight(const float& weight); void SetFreePathWeight(const float& weight); @@ -41,4 +41,4 @@ struct LinearEvaluator : PathEvaluatorBase { } // namespace motion_primitives -#endif // LINEAR_EVALUATOR_H +#endif // LINEAR_EVALUATOR_H \ No newline at end of file diff --git a/src/navigation/linear_evaluator.h.bkp b/src/navigation/linear_evaluator.h.bkp new file mode 100644 index 0000000..4baa251 --- /dev/null +++ b/src/navigation/linear_evaluator.h.bkp @@ -0,0 +1,53 @@ +//======================================================================== +// This software is free: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License Version 3, +// as published by the Free Software Foundation. +// +// This software is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// Version 3 in the file COPYING that came with this distribution. +// If not, see . +//======================================================================== +/*! +\file linear_evaluator.h +\brief Path rollout evaluator using a linear weighted cost. +\author Kavan Sikand, (C) 2020 +*/ +//======================================================================== + +#include + +#include + +#include "motion_primitives.h" + +#ifndef LINEAR_EVALUATOR_H +#define LINEAR_EVALUATOR_H + +namespace motion_primitives { + +struct LinearEvaluator : PathEvaluatorBase { + // Normalize paths from 0 to 1 for each metric. + void NormalizePaths( + const std::vector>& paths, + const std::vector& dist_to_goal, + std::vector>& norm_paths, + std::vector& norm_dist_to_goal); + // Return the best path rollout from the provided set of paths. + std::shared_ptr FindBest( + const std::vector>& paths) override; + float EvaluatePath(const std::shared_ptr& path, + const float dist_to_goal); + void SetClearanceWeight(const float& weight); + void SetDistanceWeight(const float& weight); + void SetFreePathWeight(const float& weight); + // void SetSubOpt(const float& threshold); +}; + +} // namespace motion_primitives + +#endif // LINEAR_EVALUATOR_H diff --git a/src/navigation/motion_primitives.h b/src/navigation/motion_primitives.h index 7c15c90..63d4751 100644 --- a/src/navigation/motion_primitives.h +++ b/src/navigation/motion_primitives.h @@ -53,6 +53,12 @@ struct PathRolloutBase { // $\dot{\theta}(t)$ is the instantaneous angular velocity. virtual float AngularLength() const = 0; + // Setters + virtual void SetLength(const float& new_length) = 0; + virtual void SetFPL(const float& new_fpl) = 0; + virtual void SetAngularLength(const float& new_angular_length) = 0; + virtual void SetClearance(const float& new_clearance) = 0; + // The pose of the robot at the end of the path rollout. virtual pose_2d::Pose2Df EndPoint() const = 0; diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index 498371b..ba3f5b0 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -86,6 +86,8 @@ DEFINE_bool(test_planner, false, "Run navigation planner test"); DEFINE_bool(test_latency, false, "Run Latency test"); DEFINE_double(test_dist, 0.5, "Test distance"); DEFINE_string(test_log_file, "", "Log test results to file"); +DEFINE_int32(max_unsolvable_iterations, 3, + "Max unsolvable obstacle avoidance iterations"); DEFINE_double(max_curvature, 2.0, "Maximum curvature of turning"); @@ -474,8 +476,8 @@ void Navigation::UpdateRobotLocFromOdom(const Odom& msg) { Translation2f(robot_loc) * Rotation2Df(DegToRad(robot_gps_loc_.heading)); const auto& T_new_utm = T_delta_utm * T_utm; - printf("Compensated robot loc: %f %f\n", T_new_utm.translation().x(), - T_new_utm.translation().y()); // Why is y infinity + // printf("Compensated robot loc: %f %f\n", T_new_utm.translation().x(), + // T_new_utm.translation().y()); // Why is y infinity // Update robot_loc and robot_angle based on new odometry from last known gps // location @@ -522,6 +524,129 @@ void Navigation::UpdateCommandHistory(Twist twist) { } } +// void Navigation::ForwardPredict(double time) { +// if (FLAGS_v > 1) { +// cout << "================ [Navigation] FORWARD PREDICT ================" +// << endl; +// cout << "forward time: " << time << endl; +// } + +// Eigen::Affine2f fp_odom_tf = +// Eigen::Translation2f(odom_loc_) * Eigen::Rotation2Df(odom_angle_); +// Eigen::Affine2f fp_local_tf = Eigen::Affine2f::Identity(); +// for (const Twist& cmd : command_history_) { +// const float cmd_v = cmd.linear.x(); +// const float cmd_omega = cmd.linear.x() * cmd.angular.z(); +// // Assume constant velocity and omega over the time interval +// // want to compute distance traveled (vel * dt) and arc length (ang_vel * +// // dt) and the value of the linear and angular velocity to use gives us the +// // area of the trapezoid in the v-t space when we multiply by dt +// const float v_mid = (robot_vel_.norm() + cmd_v) / 2.0f; +// const float omega_mid = (robot_omega_ + cmd_omega) / 2.0f; + +// // Forward Predict Odometry +// if (cmd.time >= +// t_odometry_ - +// params_.dt) { // start forward integrating from command before our +// // most recent odometry (we are still executing the +// // command in the timestep before the odom message) +// // we know that this gives us the last command because the controller +// runs +// // at a fixed frequency and you generate one command per controller +// // iteration every dt + +// // in every iteration besides the first command in the control history, +// // t_odometry_ < cmd.time +// const double dt = +// (t_odometry_ > cmd.time) // find time that we are executing the +// // previous command for +// ? min(params_.dt - (t_odometry_ - cmd.time), params_.dt) +// : min( +// time - cmd.time, +// params_.dt); // upper bound on how long we execute a +// command + +// // see trapezoid comment above for v_mid and omega_mid +// const float dtheta = omega_mid * dt; +// const float ds = v_mid * dt; +// // Translation coeff for exponential map of se2 -- lie algebra dead +// // reckoning / autonomous error equation: does not estimate future state +// // based on past state Special matrix that tells you how much the angular +// // rate of rotation affects the translation because we integrate the +// // linear velocity and have angular rotation unless we are moving +// // straight, we accummulate error by just using the linear velocity (the +// // tangent -- it is a linear approximation) especially if our curvature +// is +// // high a smaller dt may not necessarily decrease error with the linear +// // velocity approximation; we could increase and decrease our error +// // between steps +// Eigen::Matrix2f V = Eigen::Matrix2f::Identity(); +// if (fabs(dtheta) > kEpsilon) { +// V << sin(dtheta) / dtheta, (cos(dtheta) - 1) / dtheta, +// (1 - cos(dtheta)) / dtheta, sin(dtheta) / dtheta; +// } +// // Exponential map of translation part of se2 (in local frame) +// Eigen::Vector2f dloc = V * Eigen::Vector2f(ds, 0); +// // Update odom_tf in odom frame +// fp_odom_tf = +// fp_odom_tf * Eigen::Translation2f(dloc) * Eigen::Rotation2Df(dtheta); +// // Update odom_loc_ and odom_angle_ in odom frame +// fp_odom_tf = +// fp_odom_tf * Eigen::Translation2f(dloc) * Eigen::Rotation2Df(dtheta); +// odom_loc_ = fp_odom_tf.translation(); +// odom_angle_ = atan2(fp_odom_tf(1, 0), fp_odom_tf(0, 0)); + +// // Update robot_vel_ and robot_omega_ +// robot_vel_ = Eigen::Rotation2Df(odom_angle_) * Vector2f(cmd_v, 0); +// robot_omega_ = cmd_omega; +// t_odometry_ = cmd.time; // keep track of time of command we last +// executed + +// if (FLAGS_v > 1) { +// cout << "dtheta " << dtheta << ", ds " << ds << endl; +// cout << "V \n" << V << endl; +// cout << "odom_loc_: " << odom_loc_.transpose() +// << ", odom_angle_: " << odom_angle_ << endl; +// } +// } + +// // Forward Predict Point Cloud +// if (cmd.time >= t_point_cloud_ - params_.dt) { +// const double dt = +// (t_point_cloud_ > cmd.time) +// ? min(params_.dt - (t_point_cloud_ - cmd.time), +// params_.dt) : min(time - cmd.time, params_.dt); +// // cout << "dt " << dt << endl; +// // cout << "params_.dt " << params_.dt << endl; +// // cout << "t_point_cloud_ " << t_point_cloud_ << " cmd.time " << +// cmd.time +// // << endl; +// const float dtheta = omega_mid * dt; +// const float ds = v_mid * dt; +// // Translation coeff for exponential map of se2 +// Eigen::Matrix2f V = Eigen::Matrix2f::Identity(); +// if (fabs(dtheta) > kEpsilon) { +// V << sin(dtheta) / dtheta, (cos(dtheta) - 1) / dtheta, +// (1 - cos(dtheta)) / dtheta, sin(dtheta) / dtheta; +// } +// // cout << "V" << V << endl; +// // Exponential map of translation part of se2 (in local frame) +// Eigen::Vector2f dloc = V * Eigen::Vector2f(ds, 0); +// // Update local_tf in local frame +// fp_local_tf = +// fp_local_tf * Eigen::Translation2f(dloc) * +// Eigen::Rotation2Df(dtheta); +// } +// } + +// // Forward Predict Point Cloud +// Eigen::Affine2f inv_fp_local_tf = fp_local_tf.inverse(); +// fp_point_cloud_.resize(point_cloud_.size()); +// for (size_t i = 0; i < point_cloud_.size(); i++) { +// fp_point_cloud_[i] = inv_fp_local_tf * point_cloud_[i]; +// } +// } + void Navigation::ForwardPredict(double t) { if (command_history_.empty()) { robot_vel_ = Vector2f(0, 0); @@ -538,11 +663,10 @@ void Navigation::ForwardPredict(double t) { } printf("Predict: %f %f\n", t - t_odometry_, t - t_point_cloud_); } - const auto& latest_odom_msg = odom_history_.back(); odom_loc_ = - Vector2f(latest_odom_msg.position.x(), latest_odom_msg.position.y()); - odom_angle_ = 2.0f * atan2f(latest_odom_msg.orientation.z(), - latest_odom_msg.orientation.w()); + Vector2f(latest_odom_msg_.position.x(), latest_odom_msg_.position.y()); + odom_angle_ = 2.0f * atan2f(latest_odom_msg_.orientation.z(), + latest_odom_msg_.orientation.w()); using Eigen::Affine2f; using Eigen::Rotation2Df; using Eigen::Translation2f; @@ -601,7 +725,8 @@ void Navigation::LatencyTest(Vector2f& cmd_vel, float& cmd_angle_vel) { } void Navigation::ObstAvTest(Vector2f& cmd_vel, float& cmd_angle_vel) { - const Vector2f kTarget(4, 0); + const static Vector2f kTarget(4, 0); + // Transform target to local frame local_target_ = kTarget; RunObstacleAvoidance(cmd_vel, cmd_angle_vel); } @@ -1131,6 +1256,7 @@ DEFINE_double(tx, 0.4, "Test obstacle point - X"); DEFINE_double(ty, -0.38, "Test obstacle point - Y"); void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { + static int num_unsolvable_iter = 0; static CumulativeFunctionTimer function_timer_(__FUNCTION__); CumulativeFunctionTimer::Invocation invoke(&function_timer_); const bool debug = FLAGS_v > 1; @@ -1147,13 +1273,15 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { local_target, fp_point_cloud_, latest_image_); auto paths = sampler_->GetSamples(params_.num_options); if (debug) { - printf("%lu options\n", paths.size()); + printf("%lu options (fpl, length, curvature, clearance, dist_to_goal)\n", + paths.size()); int i = 0; for (auto p : paths) { + float dist_to_goal = (p->EndPoint().translation - local_target).norm(); ConstantCurvatureArc arc = *reinterpret_cast(p.get()); - printf("%3d: %7.5f %7.3f %7.3f\n", i++, arc.curvature, arc.length, - arc.curvature); + printf("%3d: %7.5f %7.3f %7.3f %7.3f %7.3f\n", i++, arc.fpl, arc.length, + arc.curvature, arc.clearance, dist_to_goal); } } if (paths.size() == 0) { @@ -1163,8 +1291,10 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { return; } auto best_path = evaluator_->FindBest(paths); - if (best_path == nullptr) { + if (best_path == nullptr && + num_unsolvable_iter >= FLAGS_max_unsolvable_iterations) { if (debug) printf("No best path found\n"); + // No valid path found! Eigen::Vector2f prev_local_target = local_target_; Eigen::Vector2f temp_target; @@ -1174,8 +1304,14 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { TurnInPlace(vel_cmd, ang_vel_cmd); local_target_ = prev_local_target; return; + } else if (best_path == nullptr) { + num_unsolvable_iter++; + // Simply execute last best path + best_path = best_option_; + if (debug) printf("No best path found, iter %d\n", num_unsolvable_iter); + } else { + num_unsolvable_iter = 0; } - ang_vel_cmd = 0; vel_cmd = {0, 0}; diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index f081528..9d51197 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -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; @@ -876,6 +876,7 @@ void LoadConfig(navigation::NavigationParameters* params) { NATURALNUM_PARAM(num_options); REAL_PARAM(robot_width); REAL_PARAM(robot_length); + REAL_PARAM(robot_wheelbase); REAL_PARAM(base_link_offset); REAL_PARAM(max_free_path_length); REAL_PARAM(max_clearance); @@ -922,6 +923,7 @@ void LoadConfig(navigation::NavigationParameters* params) { params->num_options = CONFIG_num_options; params->robot_width = CONFIG_robot_width; params->robot_length = CONFIG_robot_length; + params->robot_wheelbase = CONFIG_robot_wheelbase; params->base_link_offset = CONFIG_base_link_offset; params->max_free_path_length = CONFIG_max_free_path_length; params->max_clearance = CONFIG_max_clearance; diff --git a/src/navigation/navigation_parameters.h b/src/navigation/navigation_parameters.h index 8b58738..1b772cb 100644 --- a/src/navigation/navigation_parameters.h +++ b/src/navigation/navigation_parameters.h @@ -20,6 +20,7 @@ //======================================================================== #include + #include "opencv2/opencv.hpp" #ifndef NAVIGATION_PARAMETERS_H @@ -38,17 +39,12 @@ struct MotionLimits { // NOTE: Must be positive! float max_speed; - MotionLimits() : - max_acceleration(0), - max_deceleration(0), - max_speed(0) {} - - MotionLimits(float max_acceleration, - float max_deceleration, - float max_speed) : - max_acceleration(max_acceleration), - max_deceleration(max_deceleration), - max_speed(max_speed) {} + MotionLimits() : max_acceleration(0), max_deceleration(0), max_speed(0) {} + + MotionLimits(float max_acceleration, float max_deceleration, float max_speed) + : max_acceleration(max_acceleration), + max_deceleration(max_deceleration), + max_speed(max_speed) {} }; struct OSMPlannerParameters { @@ -57,11 +53,11 @@ struct OSMPlannerParameters { std::string gps_topic; std::string gps_goals_topic; - OSMPlannerParameters() : - osrm_file(""), - osrm_path_resolution(20.0), - gps_topic(""), - gps_goals_topic("") {} + OSMPlannerParameters() + : osrm_file(""), + osrm_path_resolution(20.0), + gps_topic(""), + gps_goals_topic("") {} }; struct NavigationParameters { @@ -86,6 +82,8 @@ struct NavigationParameters { float robot_width; // Length of the robot. float robot_length; + // Wheelbase length of robot + float robot_wheelbase; // Location of the base link w.r.t. the center of the robot. // Negative values indicate that the base link is closer to the rear of the // robot, for example on a car with ackermann steering, with its base link @@ -139,7 +137,7 @@ struct NavigationParameters { // How long an object should stay in the costmap if not continuously observed float object_lifespan; - + // Coefficient for exponential inflation cost float inflation_coeff; // Weight for distance cost vs. inflation cost @@ -147,50 +145,48 @@ struct NavigationParameters { // Distance of carrot when using turn in place recovery float recovery_carrot_dist; - cv::Mat K; cv::Mat D; cv::Mat H; // Default constructor, just set defaults. - NavigationParameters() : - dt(0.025), - linear_limits(0.5, 0.5, 0.5), - angular_limits(0.5, 0.5, 1.0), - intermediate_goal_dist(5), - system_latency(0.24), - obstacle_margin(0.15), - num_options(41), - robot_width(0.44), - robot_length(0.5), - base_link_offset(0), - max_free_path_length(10.0), - max_clearance(1.0), - can_traverse_stairs(false), - use_map_speed(true), - target_dist_tolerance(0.1), - target_vel_tolerance(0.1), - target_angle_tolerance(0.05), - use_kinect(true), - evaluator_type("cost_map"), - carrot_dist(2), - max_inflation_radius(1), - local_costmap_resolution(0.1), - local_costmap_size(20), - min_inflation_radius(0.3), - global_costmap_resolution(0.1), - global_costmap_size_x(100), - global_costmap_size_y(100), - global_costmap_origin_x(-50), - global_costmap_origin_y(-50), - lidar_range_min(0.1), - lidar_range_max(10), - replan_dist(2), - object_lifespan(5), - inflation_coeff(5), - distance_weight(2), - recovery_carrot_dist(0.5){ - } + NavigationParameters() + : dt(0.025), + linear_limits(0.5, 0.5, 0.5), + angular_limits(0.5, 0.5, 1.0), + intermediate_goal_dist(5), + system_latency(0.24), + obstacle_margin(0.15), + num_options(41), + robot_width(0.44), + robot_length(0.5), + base_link_offset(0), + max_free_path_length(10.0), + max_clearance(1.0), + can_traverse_stairs(false), + use_map_speed(true), + target_dist_tolerance(0.1), + target_vel_tolerance(0.1), + target_angle_tolerance(0.05), + use_kinect(true), + evaluator_type("cost_map"), + carrot_dist(2), + max_inflation_radius(1), + local_costmap_resolution(0.1), + local_costmap_size(20), + min_inflation_radius(0.3), + global_costmap_resolution(0.1), + global_costmap_size_x(100), + global_costmap_size_y(100), + global_costmap_origin_x(-50), + global_costmap_origin_y(-50), + lidar_range_min(0.1), + lidar_range_max(10), + replan_dist(2), + object_lifespan(5), + inflation_coeff(5), + distance_weight(2), + recovery_carrot_dist(0.5) {} }; } // namespace navigation