Skip to content

Commit

Permalink
Testing in progress, something wrong with pc visualization and local …
Browse files Browse the repository at this point in the history 
…target
  • Loading branch information
@artzha
artzha committed Dec 2, 2024
1 parent e27da9a commit 982e2ae
Show file tree
Hide file tree
Showing 5 changed files with 47 additions and 68 deletions.
4 changes: 2 additions & 2 deletions config/navigation.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down
95 changes: 30 additions & 65 deletions src/navigation/navigation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include <unordered_map>

#include "ackermann_motion_primitives.h"
#include "amrl_msgs/GPSMsg.h"
#include "amrl_msgs/NavStatusMsg.h"
#include "amrl_msgs/Pose2Df.h"
#include "astar.h"
Expand All @@ -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;
Expand Down Expand Up @@ -341,6 +343,16 @@ void Navigation::SetGPSNavGoals(const vector<GPSPoint>& 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_;
Expand Down Expand Up @@ -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<GraphDomain::State> plan_path = plan_path_;
if (global) {
plan_path = global_plan_path_;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -1396,6 +1408,8 @@ void Navigation::ReplanAndSetNextNavGoal(bool replan) {
gpsToGlobalCoord(initial_gps_loc_, gps_nav_goals_loc_[gps_goal_index_])
.cast<float>();
nav_goal_angle_ = gps_nav_goals_loc_[gps_goal_index_].heading;

// Push next nav goal for visualization
}

void Navigation::UpdateRobotLocFromOdom() {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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_];
Expand All @@ -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;
Expand Down Expand Up @@ -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 &&
Expand All @@ -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");
Expand Down
2 changes: 2 additions & 0 deletions src/navigation/navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <vector>

#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"
Expand Down Expand Up @@ -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,
Expand Down
12 changes: 12 additions & 0 deletions src/navigation/navigation_main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -999,6 +1008,8 @@ int main(int argc, char** argv) {
fp_pcl_pub_ = n.advertise<PointCloud>("forward_predicted_pcl", 1);
path_pub_ = n.advertise<nav_msgs::Path>("trajectory", 1);
carrot_pub_ = n.advertise<nav_msgs::Path>("carrot", 1, true);
next_gps_goal_pub_ = n.advertise<GPSMsg>(
"next_gps_goal", 1, true); // Only publish if there is a goal

// Messages
local_viz_msg_ =
Expand Down Expand Up @@ -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_);
Expand Down
2 changes: 1 addition & 1 deletion src/shared
Submodule shared updated 1 files
+4 −4 math/gps_util.h

0 comments on commit 982e2ae

Please sign in to comment.