From 34b4c2cee90c546b3c6132b114e18596b24dfec3 Mon Sep 17 00:00:00 2001 From: artzha Date: Tue, 3 Dec 2024 17:08:32 -0600 Subject: [PATCH] Fixed coordinate transform issues, need to fix: OdomToGPSUpdate, Path cost params --- config/navigation.lua | 10 +++--- src/navigation/linear_evaluator.cc | 52 +++++++++++++++--------------- src/navigation/navigation.cc | 35 +++++++++++++++----- src/navigation/navigation.h | 2 +- src/navigation/navigation_main.cc | 19 +++++++---- 5 files changed, 71 insertions(+), 47 deletions(-) diff --git a/config/navigation.lua b/config/navigation.lua index dc51583..c5206ba 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -3,7 +3,7 @@ function deg2rad(deg) end OSMPlannerParameters = { - gps_topic = "/phone/gps_with_heading"; + gps_topic = "/vectornav/gps_with_heading"; gps_goals_topic = "/gps_goals"; osrm_file = "osrm_texas_cbf_mld/texas-latest.osrm"; osrm_path_resolution = 8; -- meters between GPS points @@ -28,10 +28,10 @@ NavigationParameters = { max_angular_accel = 0.5; max_angular_decel = 0.5; max_angular_speed = 1.0; - carrot_dist = 10.0; + carrot_dist = 20.0; system_latency = 0.24; obstacle_margin = 0.15; - num_options = 31; + num_options = 62; robot_width = 0.44; robot_length = 0.5; base_link_offset = 0; @@ -47,7 +47,7 @@ NavigationParameters = { camera_calibration_path = "config/camera_calibration_kinect.yaml"; model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt"; evaluator_type = "linear"; - intermediate_goal_dist = 5; -- final goal distance will be half this (meters) + intermediate_goal_dist = 10; -- final goal distance will be half this (meters) max_inflation_radius = 1; min_inflation_radius = 0.3; local_costmap_resolution = 0.05; @@ -58,7 +58,7 @@ NavigationParameters = { global_costmap_origin_x = -12.8; global_costmap_origin_y = 12.8; lidar_range_min = 0.1; - lidar_range_max = 10.0; + lidar_range_max = 25.6; replan_dist = 2; object_lifespan = 15; inflation_coeff = 8; diff --git a/src/navigation/linear_evaluator.cc b/src/navigation/linear_evaluator.cc index b9809e1..70896d8 100644 --- a/src/navigation/linear_evaluator.cc +++ b/src/navigation/linear_evaluator.cc @@ -19,44 +19,44 @@ */ //======================================================================== -#include +#include "linear_evaluator.h" + #include +#include #include #include #include +#include "ackermann_motion_primitives.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 "eigen3/Eigen/Dense" -#include "eigen3/Eigen/Geometry" - #include "motion_primitives.h" #include "navigation_parameters.h" -#include "constant_curvature_arcs.h" -#include "ackermann_motion_primitives.h" -#include "linear_evaluator.h" -using std::min; -using std::max; -using std::vector; -using std::shared_ptr; -using pose_2d::Pose2Df; using Eigen::Vector2f; 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; -DEFINE_double(dw, 1, "Distance weight"); +DEFINE_double(dw, 0.5, "Distance weight"); DEFINE_double(cw, -0.5, "Clearance weight"); -DEFINE_double(fw, -1, "Free path weight"); +DEFINE_double(fw, 0.5, "Free path weight"); DEFINE_double(subopt, 1.5, "Max path increase for clearance"); namespace motion_primitives { shared_ptr LinearEvaluator::FindBest( - const vector>& paths) { + 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 @@ -66,8 +66,8 @@ shared_ptr LinearEvaluator::FindBest( 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); + 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; @@ -79,8 +79,9 @@ shared_ptr LinearEvaluator::FindBest( float best_path_length = FLT_MAX; 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 path_length = + (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i]) + : dist_to_goal[i]); if (path_length < best_path_length) { best_path_length = path_length; best = paths[i]; @@ -95,15 +96,14 @@ 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(); + FLAGS_fw * best->Length() + FLAGS_cw * best->Clearance(); 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 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(); if (cost < best_cost) { best = paths[i]; best_cost = cost; diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index da96731..ae0dc3c 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -430,7 +430,6 @@ void Navigation::GetCompensatedOdomUTMTransform(Affine2f& T_tp_utm, min_dtime_idx = i; } } - // Compute new reference odometry to utm transform const auto& sync_odom = odom_history_[min_dtime_idx]; const auto& T_odom_utm = OdometryToUTMTransform(sync_odom, robot_gps_loc_); @@ -444,8 +443,9 @@ Affine2f Navigation::OdometryToUTMTransform( // Assumes that odom and gps_loc correspond to the same time // T^odom_utm = T^base_utm * (T^base_odom)^-1 const auto& T_base_odom = odom.toAffine2f(); - const auto& utm_vec = gpsToGlobalCoord(gps_loc, gps_loc).cast(); - double utm_theta = DegToRad(gps_loc.heading); + const auto& utm_vec = + gpsToGlobalCoord(initial_gps_loc_, gps_loc).cast(); + double utm_theta = gpsToGlobalHeading(gps_loc); // radians // Compute the transform from odom to gps const Affine2f T_utm = @@ -474,6 +474,9 @@ 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 + // Update robot_loc and robot_angle based on new odometry from last known gps // location robot_loc_ = @@ -1325,8 +1328,17 @@ float Navigation::GetCarrotDist() { return params_.carrot_dist; } float Navigation::GetObstacleMargin() { return params_.obstacle_margin; } -Eigen::Vector3f Navigation::GetRobotPose() { - return Eigen::Vector3f(robot_loc_.x(), robot_loc_.y(), robot_angle_); +bool Navigation::GetRobotPose(Eigen::Vector3f& pose) { + if (!gps_initialized_) { + return false; + } + // Retrieve uncomponensated robot pose + Eigen::Vector2f robot_loc = + gpsToGlobalCoord(initial_gps_loc_, robot_gps_loc_).cast(); + float robot_angle = static_cast(gpsToGlobalHeading(robot_gps_loc_)); + + pose = Eigen::Vector3f(robot_loc.x(), robot_loc.y(), robot_angle); + return true; } float Navigation::GetRobotWidth() { return params_.robot_width; } @@ -1411,8 +1423,8 @@ void Navigation::ReplanAndSetNextNavGoal(bool replan) { nav_goal_loc_ = gpsToGlobalCoord(initial_gps_loc_, gps_nav_goals_loc_[gps_goal_index_]) .cast(); - nav_goal_angle_ = gps_nav_goals_loc_[gps_goal_index_].heading; - + nav_goal_angle_ = static_cast( + gpsToGlobalHeading(gps_nav_goals_loc_[gps_goal_index_])); // Push next nav goal for visualization } @@ -1675,8 +1687,15 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, if (gps_goal_index_ == int(gps_nav_goals_loc_.size()) - 1) { goal_tolerance /= 2; } + printf("Current robot loc %f %f\n", robot_loc_.x(), robot_loc_.y()); + const auto& metric_nav_goal_loc = + gpsToGlobalCoord(initial_gps_loc_, robot_gps_loc_); + printf("Nav goal loc %f %f\n", metric_nav_goal_loc.x(), + metric_nav_goal_loc.y()); + printf("Distance to goal: %f\n", (robot_loc_ - nav_goal_loc_).norm()); bool isGPSGoalReached = osm_planner_.isGoalReached( robot_gps_loc_, next_nav_goal_loc, params_.intermediate_goal_dist); + printf("IsGoalReached: %d\n", isGPSGoalReached); // bool is_goal_in_fov = isGoalInFOV(nav_goal_loc_); bool isGPSGoalStillValid = true; if (!plan_path_.empty()) { @@ -1694,7 +1713,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(false); + ReplanAndSetNextNavGoal(true); } else { nav_state_ = NavigationState::kStopped; } diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h index f516ac7..1af0f34 100644 --- a/src/navigation/navigation.h +++ b/src/navigation/navigation.h @@ -194,7 +194,7 @@ class Navigation { std::vector GetPredictedCloud(); float GetCarrotDist(); float GetObstacleMargin(); - Eigen::Vector3f GetRobotPose(); + bool GetRobotPose(Eigen::Vector3f& pose); float GetRobotWidth(); float GetRobotLength(); const cv::Mat& GetVisualizationImage(); diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index 8899432..b386254 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -83,9 +83,9 @@ using amrl_msgs::AckermannCurvatureDriveMsg; using amrl_msgs::GPSArrayMsg; using amrl_msgs::GPSMsg; using amrl_msgs::graphNavGPSSrv; +using amrl_msgs::Localization2DMsg; using amrl_msgs::NavStatusMsg; using amrl_msgs::VisualizationMsg; -using amrl_msgs::Localization2DMsg; using Eigen::Affine3f; using Eigen::Vector2f; using Eigen::Vector3f; @@ -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) { @@ -533,12 +533,17 @@ nav_msgs::Path CarrotToNavMsgsPath(const Vector2f& carrot) { void PublishLocalization() { // Publishes robot pose + Eigen::Vector3f robot_pose; + bool is_loc_initialized = navigation_.GetRobotPose(robot_pose); + if (!is_loc_initialized) { + return; + } Localization2DMsg loc_msg; loc_msg.header.stamp = ros::Time::now(); - loc_msg.pose.x = navigation_.GetRobotPose().x(); - loc_msg.pose.y = navigation_.GetRobotPose().y(); - loc_msg.pose.theta = navigation_.GetRobotPose().z(); // theta - pose_marker_publisher_.publish(loc_msg); + loc_msg.pose.x = robot_pose.x(); + loc_msg.pose.y = robot_pose.y(); + loc_msg.pose.theta = robot_pose.z(); // theta + localization_pub_.publish(loc_msg); } void PublishPath() { @@ -1091,6 +1096,7 @@ int main(int argc, char** argv) { // Publish Nav Status PublishNavStatus(); + PublishLocalization(); if (nav_succeeded) { if (!FLAGS_no_intermed) { // Publish Visualizations @@ -1115,7 +1121,6 @@ int main(int argc, char** argv) { DrawTarget(); DrawPathOptions(); } - PublishLocalization(); PublishVisualizationMarkers(); PublishPath(); PublishNextGPSGoal();