diff --git a/CMakeLists.txt b/CMakeLists.txt index 7fa0292..e0273e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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}) \ No newline at end of file +# add_dependencies(social_nav ${catkin_EXPORTED_TARGETS}) \ No newline at end of file diff --git a/config/navigation.lua b/config/navigation.lua index 33bbda3..0dd6798 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -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; diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index 31d8448..85b7adb 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -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 Navigation::GlobalPlan(const Vector2f& initial, } vector Navigation::GlobalPlan(const GPSPoint& inital, const vector& 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 path; GPSPoint start = inital; for (const auto& subgoal : goals) { @@ -596,6 +584,9 @@ vector Navigation::GlobalPlan(const GPSPoint& inital, const vector Navigation::Plan(const Vector2f& initial, const Vector2f& end) { @@ -1226,29 +1217,51 @@ vector 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 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: { diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h index 092029d..4b85646 100644 --- a/src/navigation/navigation.h +++ b/src/navigation/navigation.h @@ -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& 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 Plan(const Eigen::Vector2f& initial, const Eigen::Vector2f& end); std::vector GlobalPlan(const Eigen::Vector2f& initial, @@ -192,6 +194,7 @@ class Navigation { std::vector 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_; }; diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index 768b1a6..bb52f03 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -61,6 +61,7 @@ #include "sensor_msgs/NavSatFix.h" #include "visualization_msgs/Marker.h" #include "visualization_msgs/MarkerArray.h" +#include #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(cell_value) / 100.0f; + } + costmap.setCost(x, y, static_cast(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 diff --git a/src/navigation/osm_planner.h b/src/navigation/osm_planner.h index 40311e9..98b1c8a 100644 --- a/src/navigation/osm_planner.h +++ b/src/navigation/osm_planner.h @@ -119,12 +119,6 @@ class OSMPlanner { return distance < threshold; } - void updateGlobalGoal(const GPSPoint ¤t, vector &goals, - int goal_index) { - // TODO: Implement business logic for updating next global goal given - // current and goal_index - } - private: std::unique_ptr osrm; }; diff --git a/src/shared b/src/shared index f2f5f6a..811ada3 160000 --- a/src/shared +++ b/src/shared @@ -1 +1 @@ -Subproject commit f2f5f6a6cc055b2bc10e78172d4377d4b2b4594c +Subproject commit 811ada32da8536ab7e5cafd3924d9969f9404fd0