From 982e2ae8fd526fce50e1debda004ba0ceef00dab Mon Sep 17 00:00:00 2001 From: artzha Date: Mon, 2 Dec 2024 17:05:31 -0600 Subject: [PATCH] Testing in progress, something wrong with pc visualization and local target --- config/navigation.lua | 4 +- src/navigation/navigation.cc | 95 ++++++++++--------------------- src/navigation/navigation.h | 2 + src/navigation/navigation_main.cc | 12 ++++ src/shared | 2 +- 5 files changed, 47 insertions(+), 68 deletions(-) diff --git a/config/navigation.lua b/config/navigation.lua index b77aac8..dc51583 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -28,7 +28,7 @@ NavigationParameters = { max_angular_accel = 0.5; max_angular_decel = 0.5; max_angular_speed = 1.0; - carrot_dist = 3.5; + carrot_dist = 10.0; system_latency = 0.24; obstacle_margin = 0.15; num_options = 31; @@ -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 = 12; + intermediate_goal_dist = 5; -- final goal distance will be half this (meters) max_inflation_radius = 1; min_inflation_radius = 0.3; local_costmap_resolution = 0.05; diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index cce4b3c..b42c79d 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -33,6 +33,7 @@ #include #include "ackermann_motion_primitives.h" +#include "amrl_msgs/GPSMsg.h" #include "amrl_msgs/NavStatusMsg.h" #include "amrl_msgs/Pose2Df.h" #include "astar.h" @@ -53,6 +54,7 @@ #include "simple_queue.h" using json = nlohmann::json; +using amrl_msgs::GPSMsg; using Eigen::Affine2f; using Eigen::Rotation2Df; using Eigen::Translation2f; @@ -341,6 +343,16 @@ void Navigation::SetGPSNavGoals(const vector& goals) { printf("SetGPSNavGoals(): %d\n", int(gps_nav_goals_loc_.size())); } +bool Navigation::GetNextGPSGoal(GPSMsg& goal_msg) { + if (gps_nav_goals_loc_.empty()) return false; + + goal_msg = GPSMsg(); + goal_msg.latitude = gps_nav_goals_loc_[gps_goal_index_].lat; + goal_msg.longitude = gps_nav_goals_loc_[gps_goal_index_].lon; + goal_msg.heading = gps_nav_goals_loc_[gps_goal_index_].heading; + return true; +} + void Navigation::ResetNavGoals() { nav_state_ = NavigationState::kStopped; nav_goal_loc_ = robot_loc_; @@ -994,7 +1006,7 @@ bool Navigation::GetLocalCarrot(Vector2f& carrot) { } bool Navigation::GetCarrot(Vector2f& carrot, bool global, float carrot_dist) { - const bool kDebug = FLAGS_v > 1; + const bool kDebug = FLAGS_v > 2; vector plan_path = plan_path_; if (global) { plan_path = global_plan_path_; @@ -1357,8 +1369,8 @@ int Navigation::GetNextGPSGlobalGoal(int start_goal_index) { const bool kDebug = FLAGS_v > 0; // Never go back, Never surrender const auto& final_goal = gps_nav_goals_loc_.back(); - for (int i = start_goal_index; i < int(gps_nav_goals_loc_.size()); ++i) { - const auto& subgoal = gps_nav_goals_loc_[i]; + for (int i = start_goal_index + 1; i < int(gps_nav_goals_loc_.size()); ++i) { + GPSPoint subgoal = gps_nav_goals_loc_[i]; double robot_to_final_goal = gpsDistance(robot_gps_loc_, final_goal); double subgoal_to_final_goal = gpsDistance(subgoal, final_goal); if (kDebug) { @@ -1396,6 +1408,8 @@ void Navigation::ReplanAndSetNextNavGoal(bool replan) { gpsToGlobalCoord(initial_gps_loc_, gps_nav_goals_loc_[gps_goal_index_]) .cast(); nav_goal_angle_ = gps_nav_goals_loc_[gps_goal_index_].heading; + + // Push next nav goal for visualization } void Navigation::UpdateRobotLocFromOdom() { @@ -1425,7 +1439,7 @@ void Navigation::UpdateRobotLocFromOdom() { bool Navigation::Run(const double& time, Vector2f& cmd_vel, float& cmd_angle_vel) { - const bool kDebug = FLAGS_v > 0; + const bool kDebug = FLAGS_v > 1; if (!initialized_) { if (kDebug) printf("Parameters and maps not initialized\n"); return false; @@ -1650,63 +1664,6 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, } } - /** BEGIN REFERENCE CODE FOR GPS NAV TO LOCAL */ - // // gps_goal_index_ = 1; // TODO: remove this for debugging - // printf("Current goal index %d\n", gps_goal_index_); - // Eigen::Affine2f T_relutm_local = gpsToLocal( - // robot_gps_loc_.lat, robot_gps_loc_.lon, - // robot_gps_loc_.heading, - // gps_nav_goals_loc_[gps_goal_index_].lat, - // gps_nav_goals_loc_[gps_goal_index_].lon, - // gps_nav_goals_loc_[gps_goal_index_].heading); - // Vector2f local_subgoal(T_relutm_local.translation().x(), - // T_relutm_local.translation().y()); - // // Before switches states, we need to update the nav targets - // bool is_goal_reached = osm_planner_.isGoalReached( - // robot_gps_loc_, gps_nav_goals_loc_[gps_goal_index_]); - // bool does_next_goal_exist = - // gps_goal_index_ + 1 < int(gps_nav_goals_loc_.size()); - // bool is_goal_in_fov = isGoalInFOV(local_subgoal); - // if (kDebug) { - // printf("Is goal in fov: %d\n", is_goal_in_fov); - // printf("Goal Relative Local: %f %f\n", T_relutm_local.translation().x(), - // T_relutm_local.translation().y()); - // printf("Heading: %f\n", - // Eigen::Rotation2Df(T_relutm_local.rotation()).angle() * 180 / - // M_PI); - // } - // bool is_path_invalid = global_plan_path_.empty(); - - // printf("is_goal_reached: %d\n", is_goal_reached); - // printf("does_next_goal_exist: %d\n", does_next_goal_exist); - // printf("is_goal_in_fov: %d\n", is_goal_in_fov); - // // Update navigation states - // if (is_goal_reached) { - // if (!does_next_goal_exist) { - // nav_state_ = NavigationState::kStopped; - // } else { - // gps_goal_index_++; - // PlanIntermediate(robot_loc_, local_subgoal); - // initial_odom_msg_ = latest_odom_msg_; - // UpdateRobotLocFromOdom(); - // nav_state_ = NavigationState::kGoto; - // } - // } else if (!is_goal_in_fov) { - // // float goal_angle = atan2(local_subgoal.y(), local_subgoal.x()); - // // TODO: Modify to do turn in place in direction of goal - // nav_state_ = NavigationState::kTurnInPlace; - // } else if (is_path_invalid) { - // // Update plan_path_ for GetLocalCarrot - // PlanIntermediate(robot_loc_, local_subgoal); - // initial_odom_msg_ = latest_odom_msg_; - // UpdateRobotLocFromOdom(); - // nav_state_ = NavigationState::kGoto; - // } else { - // nav_state_ = NavigationState::kGoto; - // } - /** END REFERENCE CODE FOR GPS NAV TO LOCAL */ - // return true; - if (!gps_nav_goals_loc_.empty()) { // Keep iterating to next waypoint until // one gets you closer to the goal GPSPoint next_nav_goal_loc = gps_nav_goals_loc_[gps_goal_index_]; @@ -1732,6 +1689,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, if (kDebug) printf("GPS Goal reached\n"); 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); } else { nav_state_ = NavigationState::kStopped; @@ -1764,17 +1722,24 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, // Get Carrot and check if done (global coordinates utm) Vector2f carrot(0, 0); bool foundCarrot = GetLocalCarrot(carrot); + printf("Local carrot %f %f\n", carrot.x(), carrot.y()); + printf("Next GPS goal %f %f\n", nav_goal_loc_.x(), nav_goal_loc_.y()); + printf("Current Robot loc %f %f\n", robot_loc_.x(), robot_loc_.y()); if (!foundCarrot) { Halt(cmd_vel, cmd_angle_vel); return false; } - // Local Navigation + // Local Navigation (Convert global to local coordinates) local_target_ = Rotation2Df(-robot_angle_) * (carrot - robot_loc_); + // Swap x=y y=-x + // local_target_ = Vector2f(local_target_.y(), -local_target_.x()); + printf("Local target %f %f\n", local_target_.x(), local_target_.y()); } } // 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 && @@ -1790,16 +1755,16 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, switch (nav_state_) { case NavigationState::kStopped: { - if (kDebug) printf("\nNav complete\n"); + if (kDebug && did_state_change) printf("\nNav complete\n"); } break; case NavigationState::kPaused: { if (kDebug) printf("\nNav paused\n"); } break; case NavigationState::kGoto: { - if (kDebug) printf("\nNav Goto\n"); + if (kDebug && did_state_change) printf("\nNav Goto\n"); } break; case NavigationState::kTurnInPlace: { - if (kDebug) printf("\nNav TurnInPlace\n"); + if (kDebug && did_state_change) printf("\nNav TurnInPlace\n"); } break; case NavigationState::kOverride: { if (kDebug) printf("\nNav override\n"); diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h index cb00380..93f56c4 100644 --- a/src/navigation/navigation.h +++ b/src/navigation/navigation.h @@ -30,6 +30,7 @@ #include #include "amrl_msgs/AckermannCurvatureDriveMsg.h" +#include "amrl_msgs/GPSMsg.h" #include "amrl_msgs/Localization2DMsg.h" #include "amrl_msgs/VisualizationMsg.h" #include "config_reader/config_reader.h" @@ -139,6 +140,7 @@ class Navigation { const GPSPoint& gps_loc); void UpdateRobotLocFromOdom(const Odom& msg); int GetNextGPSGlobalGoal(int start_goal_index); + bool GetNextGPSGoal(amrl_msgs::GPSMsg& goal_msg); void Plan(Eigen::Vector2f goal_loc); void PlanIntermediate(const Eigen::Vector2f& initial, diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index 6233a28..d6311e2 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -171,6 +171,7 @@ ros::Publisher status_pub_; ros::Publisher fp_pcl_pub_; ros::Publisher path_pub_; ros::Publisher carrot_pub_; +ros::Publisher next_gps_goal_pub_; image_transport::Publisher viz_img_pub_; // Messages @@ -572,6 +573,14 @@ void PublishPath() { } } +void PublishNextGPSGoal() { + GPSMsg goal_msg; + bool is_goal_valid = navigation_.GetNextGPSGoal(goal_msg); + if (is_goal_valid) { + next_gps_goal_pub_.publish(goal_msg); + } +} + void DrawTarget() { const float carrot_dist = navigation_.GetCarrotDist(); const Eigen::Vector2f target = navigation_.GetTarget(); @@ -999,6 +1008,8 @@ int main(int argc, char** argv) { fp_pcl_pub_ = n.advertise("forward_predicted_pcl", 1); path_pub_ = n.advertise("trajectory", 1); carrot_pub_ = n.advertise("carrot", 1, true); + next_gps_goal_pub_ = n.advertise( + "next_gps_goal", 1, true); // Only publish if there is a goal // Messages local_viz_msg_ = @@ -1093,6 +1104,7 @@ int main(int argc, char** argv) { } PublishVisualizationMarkers(); PublishPath(); + PublishNextGPSGoal(); local_viz_msg_.header.stamp = ros::Time::now(); global_viz_msg_.header.stamp = ros::Time::now(); viz_pub_.publish(local_viz_msg_); diff --git a/src/shared b/src/shared index 956f648..f138093 160000 --- a/src/shared +++ b/src/shared @@ -1 +1 @@ -Subproject commit 956f64866045b045750fbdbcb84f4366ebd3eb38 +Subproject commit f1380937bad222aef6f978cee2e465debfb039d1