Skip to content

Commit

Permalink
[Feature] Added working gps goal->local target and fov checker
Browse files Browse the repository at this point in the history
artzha committed Nov 11, 2024
1 parent 9e51f14 commit b31dd46
Showing 7 changed files with 176 additions and 128 deletions.
60 changes: 30 additions & 30 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -95,36 +95,36 @@ TARGET_LINK_LIBRARIES(streetmap_test
${LibOSRM_LIBRARIES}
${LibOSRM_DEPENDENT_LIBRARIES})

ADD_EXECUTABLE(navigation_tests
src/navigation/navigation_tests.cc
src/navigation/motion_primitives.cc
)
TARGET_LINK_LIBRARIES(navigation_tests
gtest
gtest_main
${libs}
${LibOSRM_LIBRARIES}
${LibOSRM_DEPENDENT_LIBRARIES})

ROSBUILD_ADD_EXECUTABLE(social_nav
src/navigation/social_nav.cc
src/navigation/social_main.cc
src/navigation/motion_primitives.cc
src/navigation/ackermann_motion_primitives.cc
src/navigation/constant_curvature_arcs.cc
src/navigation/image_tiler.cc
src/navigation/linear_evaluator.cc
src/navigation/deep_cost_map_evaluator.cc
src/navigation/image_based_evaluator.cc
src/navigation/navigation.cc)
TARGET_LINK_LIBRARIES(social_nav
shared_library
${libs}
${TORCH_LIBRARIES}
${OpenCV_LIBS}
${LibOSRM_LIBRARIES}
${LibOSRM_DEPENDENT_LIBRARIES})
# ADD_EXECUTABLE(navigation_tests
# src/navigation/navigation_tests.cc
# src/navigation/motion_primitives.cc
# )
# TARGET_LINK_LIBRARIES(navigation_tests
# gtest
# gtest_main
# ${libs}
# ${LibOSRM_LIBRARIES}
# ${LibOSRM_DEPENDENT_LIBRARIES})

# ROSBUILD_ADD_EXECUTABLE(social_nav
# src/navigation/social_nav.cc
# src/navigation/social_main.cc
# src/navigation/motion_primitives.cc
# src/navigation/ackermann_motion_primitives.cc
# src/navigation/constant_curvature_arcs.cc
# src/navigation/image_tiler.cc
# src/navigation/linear_evaluator.cc
# src/navigation/deep_cost_map_evaluator.cc
# src/navigation/image_based_evaluator.cc
# src/navigation/navigation.cc)
# TARGET_LINK_LIBRARIES(social_nav
# shared_library
# ${libs}
# ${TORCH_LIBRARIES}
# ${OpenCV_LIBS}
# ${LibOSRM_LIBRARIES}
# ${LibOSRM_DEPENDENT_LIBRARIES})


add_dependencies(navigation ${catkin_EXPORTED_TARGETS})
add_dependencies(social_nav ${catkin_EXPORTED_TARGETS})
# add_dependencies(social_nav ${catkin_EXPORTED_TARGETS})
8 changes: 4 additions & 4 deletions config/navigation.lua
Original file line number Diff line number Diff line change
@@ -52,10 +52,10 @@ NavigationParameters = {
local_costmap_resolution = 0.05;
local_costmap_size = 20;
global_costmap_resolution = 0.1;
global_costmap_size_x = 100;
global_costmap_size_y = 100;
global_costmap_origin_x = -10;
global_costmap_origin_y = 0;
global_costmap_size_x = 128;
global_costmap_size_y = 256;
global_costmap_origin_x = -12.8;
global_costmap_origin_y = 12.8;
lidar_range_min = 0.1;
lidar_range_max = 10.0;
replan_dist = 2;
184 changes: 103 additions & 81 deletions src/navigation/navigation.cc
Original file line number Diff line number Diff line change
@@ -387,6 +387,7 @@ void Navigation::UpdateOdometry(const Odom& msg) {
PruneLatencyQueue();
if (!odom_initialized_) {
starting_loc_ = Vector2f(msg.position.x(), msg.position.y());
initial_odom_msg_ = msg;
odom_initialized_ = true;
}
}
@@ -571,19 +572,6 @@ vector<int> Navigation::GlobalPlan(const Vector2f& initial,
}

vector<GPSPoint> Navigation::GlobalPlan(const GPSPoint& inital, const vector<GPSPoint>& goals) {
// for (size_t i = 0; i + 1 < gps_nav_goals_loc_.size(); i += 2) {
// printf("Start %lf %lf\n", gps_nav_goals_loc_[i].lat,
// gps_nav_goals_loc_[i].lon);
// printf("End %lf %lf\n", gps_nav_goals_loc_[i + 1].lat,
// gps_nav_goals_loc_[i + 1].lon);
// const auto& gps_route =
// osm_planner_.plan(gps_nav_goals_loc_[i], gps_nav_goals_loc_[i + 1]);
// printf("Route: %d\n", int(gps_route.size()));
// for (const auto& p : gps_route) {
// printf("%lf %lf %lf\n", p.time, p.lat, p.lon);
// }
// }
// }
vector<GPSPoint> path;
GPSPoint start = inital;
for (const auto& subgoal : goals) {
@@ -596,6 +584,9 @@ vector<GPSPoint> Navigation::GlobalPlan(const GPSPoint& inital, const vector<GPS
return path;
}

void Navigation::UpdateLocalCostmap(const costmap_2d::Costmap2D& costmap) {
costmap_ = costmap;
}

vector<GraphDomain::State> Navigation::Plan(const Vector2f& initial,
const Vector2f& end) {
@@ -1226,29 +1217,51 @@ vector<ObstacleCost> Navigation::GetGlobalCostmapObstacles() {

Eigen::Vector2f Navigation::GetIntermediateGoal() { return intermediate_goal_; }

void Navigation::GetGlobalGoal() {
if (!gps_initialized_ || gps_nav_goals_loc_.empty()) return;
if (gps_goal_index_ < 0) return;

// 1 Replan global path in UTM frame from current position
const auto& next_goal = gps_nav_goals_loc_[gps_goal_index_];
const auto& gps_route = osm_planner_.plan(robot_gps_loc_, next_goal);

// 2 Convert global path from UTM to local frame
for (const auto& p : gps_route) {
std::tuple<double, double> local_coords = gpsToGlobalCoord(
initial_gps_loc_.lat, initial_gps_loc_.lon, p.lat, p.lon);

if (FLAGS_v > 2) {
printf("|----- BEGIN GetGlobalGoal() -----|\n");
printf("GPS Subgoal: %lf %lf\n", p.lat, p.lon);
printf("Local: %lf %lf\n", std::get<0>(local_coords),
std::get<1>(local_coords));
printf("|----- END GetGlobalGoal() -----|\n");
}
void Navigation::PlanIntermediate(const Vector2f& initial, const Vector2f& end) {
// TODO: Implement Intermediate planner that uses costmap

// Initialize Planning Domain A*

// Add start and end states to planning domain

// Run A* on planning domain

// Get path from A*

// Set intermediate_path_found_ to true if path found
intermediate_path_found_ = true;
// local_target_ =
// plan_path_ =
}

bool Navigation::isGoalInFOV(const Vector2f& local_goal) {
const float angle_to_goal = atan2(local_goal.y(), local_goal.x());
const float min_angle = -params_.local_fov / 2; // in radians
const float max_angle = params_.local_fov / 2; // in radians
return angle_to_goal >= min_angle && angle_to_goal <= max_angle;
}

void Navigation::UpdateRobotLocFromOdom() {
if (!odom_initialized_) {
return;
}

// 3 Save global path in local frame
Eigen::Vector2f initial_odom_loc_ = Vector2f(initial_odom_msg_.position.x(), initial_odom_msg_.position.y());
float initial_odom_angle_ = 2.0f * atan2f(initial_odom_msg_.orientation.z(),
initial_odom_msg_.orientation.w());
Eigen::Affine2f T_initial_to_odom = Eigen::Translation2f(initial_odom_loc_) *
Eigen::Rotation2Df(initial_odom_angle_);
Eigen::Affine2f T_robot_to_odom = Eigen::Translation2f(odom_loc_) *
Eigen::Rotation2Df(odom_angle_);

Eigen::Affine2f T_robot_to_initial = T_initial_to_odom.inverse() * T_robot_to_odom;

// Update robot_loc_ and robot_angle_
robot_loc_ = T_robot_to_initial.translation();
// Normalize angle to [-pi, pi]
robot_angle_ = std::atan2(
std::sin(Eigen::Rotation2Df(T_robot_to_initial.rotation()).angle()),
std::cos(Eigen::Rotation2Df(T_robot_to_initial.rotation()).angle()));
}

bool Navigation::Run(const double& time, Vector2f& cmd_vel,
@@ -1266,6 +1279,10 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
if (kDebug) printf("GPS not initialized\n");
return false;
}
if (gps_nav_goals_loc_.empty() or gps_goal_index_ < 0) {
if (kDebug) printf("No GPS goals\n");
return false;
}

ForwardPredict(time + params_.system_latency);
if (FLAGS_test_toc) {
@@ -1287,7 +1304,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
LatencyTest(cmd_vel, cmd_angle_vel);
return true;
}
return true;

if (params_.do_intermed) {
int cell_inflation_size =
std::ceil(params_.max_inflation_radius / costmap_.getResolution());
@@ -1474,67 +1491,72 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
}
}

gps_goal_index_ = 1; // TODO: remove this for debuggin
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_gps_goal_reached = osm_planner_.isGoalReached(
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_path_invalid = (!params_.do_intermed && !PlanStillValid()) ||
(params_.do_intermed &&
(!PlanStillValid() || !IntermediatePlanStillValid()));

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_gps_goal_reached) {
if (is_goal_reached) {
if (!does_next_goal_exist) {
nav_state_ = NavigationState::kStopped;
} else {
gps_goal_index_++;
osm_planner_.updateGlobalGoal(robot_gps_loc_, gps_nav_goals_loc_,
gps_goal_index_);
// Update global plan

PlanIntermediate(robot_loc_, local_subgoal);
initial_odom_msg_ = latest_odom_msg_;
nav_state_ = NavigationState::kGoto;
}
nav_state_ = NavigationState::kStopped;
} else if (is_path_invalid) {
// Replan intermediate goal if local plan is infeasible
plan_path_ = Plan(robot_loc_, nav_goal_loc_);
bool is_plan_valid = PlanStillValid();
// If global plan is still not valid, enter recovery behavior
if (!is_plan_valid) {
} 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 {
nav_state_ = NavigationState::kGoto;
}
} else if (!is_path_invalid) {
} else if (is_path_invalid) {
// Update plan_path_ for GetLocalCarrot
PlanIntermediate(robot_loc_, local_subgoal);
initial_odom_msg_ = latest_odom_msg_;
nav_state_ = NavigationState::kGoto;
} else {
nav_state_ = NavigationState::kStopped;
nav_state_ = NavigationState::kGoto;
}

// Update robot_loc_ based on odometry
UpdateRobotLocFromOdom();

// Before switching states, update the local target
if (nav_state_ == NavigationState::kGoto) {
// Get next local bev carrot
Vector2f carrot(0, 0);
bool foundCarrot = GetLocalCarrot(carrot);
if (!foundCarrot) {
Halt(cmd_vel, cmd_angle_vel);
return false;
}
// Local Navigation
local_target_ = Rotation2Df(-robot_angle_) * (carrot - robot_loc_);
}
return true;
// // Before switching states we need to update the local target.
// if (nav_state_ == NavigationState::kGoto ||
// nav_state_ == NavigationState::kOverride) {
// printf("Updating local target\n");
// // Recompute global plan as necessary.
// if ((!params_.do_intermed && !PlanStillValid()) ||
// (params_.do_intermed &&
// (!PlanStillValid() || !IntermediatePlanStillValid()))) {
// if (kDebug) printf("Replanning\n");
// plan_path_ = Plan(robot_loc_, nav_goal_loc_);
// }
// if (nav_state_ == NavigationState::kGoto) {
// // Get Carrot and check if done
// Vector2f carrot(0, 0);
// bool foundCarrot = GetLocalCarrot(carrot);
// if (!foundCarrot) {
// Halt(cmd_vel, cmd_angle_vel);
// return false;
// }
// // Local Navigation
// local_target_ = Rotation2Df(-robot_angle_) * (carrot - robot_loc_);
// }
// }

switch (nav_state_) {
case NavigationState::kStopped: {
8 changes: 5 additions & 3 deletions src/navigation/navigation.h
Original file line number Diff line number Diff line change
@@ -122,17 +122,19 @@ class Navigation {
float* free_path_length,
float* clearance,
Eigen::Vector2f* obstruction);
bool isGoalInFOV(const Eigen::Vector2f& local_goal);
void UpdateGPS(const GPSPoint& msg);
void SetGPSNavGoals(const vector<GPSPoint>& goals);
void SetNavGoal(const Eigen::Vector2f& loc, float angle);
void ResetNavGoals();
void updateGlobalNavGoal();
void UpdateLocalCostmap(const costmap_2d::Costmap2D& costmap);
void SetOverride(const Eigen::Vector2f& loc, float angle);
void Resume();
bool PlanStillValid();
bool IntermediatePlanStillValid();

void Plan(Eigen::Vector2f goal_loc);
void PlanIntermediate(const Eigen::Vector2f& initial, const Eigen::Vector2f& end);
std::vector<GraphDomain::State> Plan(const Eigen::Vector2f& initial,
const Eigen::Vector2f& end);
std::vector<int> GlobalPlan(const Eigen::Vector2f& initial,
@@ -192,6 +194,7 @@ class Navigation {
std::vector<ObstacleCost> GetGlobalCostmapObstacles();

Eigen::Vector2f GetIntermediateGoal();
void UpdateRobotLocFromOdom();

private:

@@ -247,6 +250,7 @@ class Navigation {
// Current odometry frame robot angle (OdometryCallback).
float odom_angle_;
// Newest odometry message received.
Odom initial_odom_msg_;
Odom latest_odom_msg_;
// Newest image received.
cv::Mat latest_image_;
@@ -340,8 +344,6 @@ class Navigation {
//
bool intermediate_path_found_;



Eigen::Vector2f intermediate_goal_;

};
36 changes: 33 additions & 3 deletions src/navigation/navigation_main.cc
Original file line number Diff line number Diff line change
@@ -61,6 +61,7 @@
#include "sensor_msgs/NavSatFix.h"
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
#include <nav_msgs/OccupancyGrid.h>
#include "nav_msgs/Odometry.h"
#include "nav_msgs/Path.h"
#include "ros/ros.h"
@@ -116,7 +117,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, false, "Whether to disable intermediate planning (will use legacy obstacle avoidance planner)");
DEFINE_bool(no_intermed, true, "Whether to disable intermediate planning (will use legacy obstacle avoidance planner)");

CONFIG_STRING(image_topic, "NavigationParameters.image_topic");
CONFIG_STRINGLIST(laser_topics, "NavigationParameters.laser_topics");
@@ -213,6 +214,35 @@ void GPSCallback(const std_msgs::Float64MultiArray& msg) {
navigation_.UpdateGPS(loc);
}

void LocalCostmapCallback(const nav_msgs::OccupancyGrid& msg) {
// Set up the Costmap2D dimensions, resolution, and origin based on the OccupancyGrid message
unsigned int width = msg.info.width;
unsigned int height = msg.info.height;
double resolution = msg.info.resolution;
double origin_x = msg.info.origin.position.x;
double origin_y = msg.info.origin.position.y;

costmap_2d::Costmap2D costmap(width, height, resolution, origin_x, origin_y);
for (unsigned int y = 0; y < height; ++y) {
for (unsigned int x = 0; x < width; ++x) {
// Index in OccupancyGrid's data array
unsigned int index = x + y * width;
int8_t cell_value = msg.data[index];
// Map OccupancyGrid values to continuous float values for Costmap2D (e.g., scale 0-100 to 0.0-1.0)
float normalized_value;
if (cell_value == -1) {
normalized_value = costmap_2d::NO_INFORMATION;
} else {
normalized_value = static_cast<float>(cell_value) / 100.0f;
}
costmap.setCost(x, y, static_cast<unsigned char>(normalized_value * 255.0f));
}
}
navigation_.UpdateLocalCostmap(costmap);

ROS_INFO("Costmap2D created with width: %d, height: %d, resolution: %f", width, height, resolution);
}

void RetrieveTransform(const std_msgs::Header& msg,
Eigen::Affine3f& frame_tf) {
static tf::TransformListener tf_listener;
@@ -1030,8 +1060,8 @@ int main(int argc, char** argv) {
n.subscribe("nav_override", 1, &OverrideCallback);
ros::Subscriber gps_pos_sub =
n.subscribe(CONFIG_gps_topic, 1, &GPSCallback);
// ros::Subscriber gps_goals_sub =
// n.subscribe(CONFIG_gps_goals_topic, 1, &GoToGPSGoalCallback);
ros::Subscriber local_costmap_sub =
n.subscribe("local_costmap", 1, &LocalCostmapCallback);

std_msgs::Header viz_img_header; // empty viz_img_header
viz_img_header.stamp = ros::Time::now(); // time
6 changes: 0 additions & 6 deletions src/navigation/osm_planner.h
Original file line number Diff line number Diff line change
@@ -119,12 +119,6 @@ class OSMPlanner {
return distance < threshold;
}

void updateGlobalGoal(const GPSPoint &current, vector<GPSPoint> &goals,
int goal_index) {
// TODO: Implement business logic for updating next global goal given
// current and goal_index
}

private:
std::unique_ptr<osrm::OSRM> osrm;
};
2 changes: 1 addition & 1 deletion src/shared
Submodule shared updated 1 files
+42 −0 math/gps_util.h

0 comments on commit b31dd46

Please sign in to comment.