diff --git a/config/navigation.lua b/config/navigation.lua index d8e61c5..bfd3815 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -25,7 +25,7 @@ NavigationParameters = { dt = 0.060; max_linear_accel = 0.5; max_linear_decel = 0.5; - max_linear_speed = 0.5; + max_linear_speed = 0.75; max_angular_accel = 0.5; max_angular_decel = 0.5; max_angular_speed = 1.0; @@ -44,7 +44,7 @@ NavigationParameters = { target_dist_tolerance = 0.1; target_vel_tolerance = 0.1; target_angle_tolerance = 0.05; - local_fov = deg2rad(90); + local_fov = deg2rad(180); use_kinect = true; camera_calibration_path = "config/camera_calibration_kinect.yaml"; model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt"; diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index fa7d553..552f4d2 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -333,6 +333,29 @@ void Navigation::SetNavGoal(const Vector2f& loc, float angle) { if (FLAGS_v > 0) printf("SetNavGoal(): %f %f %f\n", loc.x(), loc.y(), angle); } +MissionStatus Navigation::GetMissionStatus() { + static int64_t prev_mission_id = -1; + if (prev_mission_id != mission_status_.mission_id) { + mission_status_.goals = gps_nav_goals_loc_; + mission_status_.goals_reached.clear(); + prev_mission_id = mission_status_.mission_id; + mission_status_.next_goal_id = -1; + } + mission_status_.time = ros::Time::now().toSec(); + mission_status_.status = GetNavStatusUint8(); + mission_status_.next_goal_id = gps_goal_index_; + + int num_goals_reached = int(mission_status_.goals_reached.size()); + for (int i = num_goals_reached; i < gps_goal_index_; i++) { + const auto& waypoint = + GPSPoint(mission_status_.time, gps_nav_goals_loc_[i].lat, + gps_nav_goals_loc_[i].lon); + mission_status_.goals_reached.emplace_back(waypoint); + } + + return mission_status_; +} + void Navigation::SetGPSNavGoals(const vector& goals) { nav_state_ = NavigationState::kGoto; // Skip gps subgoal update if the goal is the same as the current goal @@ -342,6 +365,7 @@ void Navigation::SetGPSNavGoals(const vector& goals) { gps_nav_goals_loc_ = goals; gps_goal_index_ = GetNextGPSGlobalGoal(0); plan_path_.clear(); + mission_status_.mission_id++; if (FLAGS_v > 0) printf("SetGPSNavGoals(): %d\n", int(gps_nav_goals_loc_.size())); @@ -1206,7 +1230,8 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) { Eigen::Vector2f prev_local_target = local_target_; Eigen::Vector2f temp_target; GetCarrot(temp_target, false, params_.recovery_carrot_dist); - // Eigen::Vector2f temp_target = GetPathGoal(params_.recovery_carrot_dist); + // Eigen::Vector2f temp_target = + // GetPathGoal(params_.recovery_carrot_dist); local_target_ = Rotation2Df(-robot_angle_) * (temp_target - robot_loc_); TurnInPlace(vel_cmd, ang_vel_cmd); local_target_ = prev_local_target; @@ -1443,6 +1468,7 @@ int Navigation::GetNextGPSGlobalGoal(int start_goal_index) { printf("Checking subgoal %d\n", i); printf("Robot dist to final goal: %f\n", robot_to_final_goal); printf("Subgoal dist to final goal: %f\n", subgoal_to_final_goal); + printf("Robot dist to subgoal: %f\n", robot_to_subgoal); } if (robot_to_final_goal < subgoal_to_final_goal || robot_to_subgoal < params_.intermediate_goal_tolerance) { @@ -1476,7 +1502,9 @@ void Navigation::ReplanAndSetNextNavGoal(bool replan) { const auto& route = this->GlobalPlan(robot_gps_loc_, goals); const auto& map_route = this->GPSRouteToMap(route); this->SetGPSNavGoals(route); // Updates gps_goal_index_ - } else { // Only update next goal if not replanning + // Republish new route + throw std::runtime_error("Replanning not implemented"); + } else { // Only update next goal if not replanning gps_goal_index_ = GetNextGPSGlobalGoal(gps_goal_index_); } @@ -1545,8 +1573,8 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, costmap_obstacles_.clear(); // True if distance is less than replan inflation size - // Assign different value for points within inflation size but farther than - // replan size + // Assign different value for points within inflation size but farther + // than replan size unordered_map inflation_cells; unordered_set obstacle_cells; @@ -1793,7 +1821,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, * 1. If goal is invalid, replan global path. * 2. If goal is still valid, update next global goal */ - bool isGPSGoalValid = PlanStillValid(); + bool isGPSGoalValid = true; // PlanStillValid(); ReplanAndSetNextNavGoal(!isGPSGoalValid); /** Run intermediate planner */ diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h index 5c7729a..1ec8b99 100644 --- a/src/navigation/navigation.h +++ b/src/navigation/navigation.h @@ -31,6 +31,7 @@ #include "amrl_msgs/AckermannCurvatureDriveMsg.h" #include "amrl_msgs/GPSMsg.h" +#include "amrl_msgs/GPSNavStatusMsg.h" #include "amrl_msgs/Localization2DMsg.h" #include "amrl_msgs/VisualizationMsg.h" #include "config_reader/config_reader.h" @@ -89,6 +90,17 @@ struct Odom { EIGEN_MAKE_ALIGNED_OPERATOR_NEW; }; +struct MissionStatus { + double time; + uint8_t status; + int64_t mission_id; + int64_t next_goal_id; + std::vector goals; + std::vector goals_reached; + + MissionStatus() : time(0), status(0), mission_id(-1), next_goal_id(-1) {} +}; + struct SeenObstacle { Eigen::Vector2f location; std::time_t last_seen; @@ -189,6 +201,7 @@ class Navigation { Eigen::Vector2f GetOverrideTarget(); Eigen::Vector2f GetVelocity(); float GetAngularVelocity(); + MissionStatus GetMissionStatus(); std::string GetNavStatus(); uint8_t GetNavStatusUint8(); std::vector GetPredictedCloud(); @@ -282,6 +295,7 @@ class Navigation { int gps_goal_index_; std::vector gps_nav_goals_loc_; GPSTranslator gps_translator_; + MissionStatus mission_status_; NavigationState nav_state_; diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index ac8b821..d83c028 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -39,6 +39,7 @@ #include "amrl_msgs/GPSArrayMsg.h" #include "amrl_msgs/GPSMsg.h" #include "amrl_msgs/Localization2DMsg.h" +#include "amrl_msgs/MissionStatusMsg.h" #include "amrl_msgs/NavStatusMsg.h" #include "amrl_msgs/Pose2Df.h" #include "amrl_msgs/VisualizationMsg.h" @@ -84,6 +85,7 @@ using amrl_msgs::GPSArrayMsg; using amrl_msgs::GPSMsg; using amrl_msgs::graphNavGPSSrv; using amrl_msgs::Localization2DMsg; +using amrl_msgs::MissionStatusMsg; using amrl_msgs::NavStatusMsg; using amrl_msgs::VisualizationMsg; using Eigen::Affine3f; @@ -96,6 +98,7 @@ using math_util::DegToRad; using math_util::RadToDeg; using motion_primitives::ConstantCurvatureArc; using motion_primitives::PathRolloutBase; +using navigation::MissionStatus; using navigation::MotionLimits; using navigation::Navigation; using navigation::PathOption; @@ -174,6 +177,7 @@ ros::Publisher path_pub_; ros::Publisher carrot_pub_; ros::Publisher next_gps_goal_pub_; ros::Publisher localization_pub_; +ros::Publisher mission_status_pub_; image_transport::Publisher viz_img_pub_; // Messages @@ -466,6 +470,32 @@ navigation::Twist ToTwist(geometry_msgs::TwistStamped twist_msg) { return twist; } +void PublishMissionStatus() { + MissionStatus status = navigation_.GetMissionStatus(); + MissionStatusMsg status_msg; + status_msg.stamp = ros::Time(status.time); + status_msg.status = status.status; + status_msg.mission_id = status.mission_id; + status_msg.next_goal_id = status.next_goal_id; + + for (size_t i = 0; i < status.goals.size(); ++i) { + GPSMsg goal_msg; + goal_msg.header.stamp = ros::Time(status.goals[i].time); + goal_msg.latitude = status.goals[i].lat; + goal_msg.longitude = status.goals[i].lon; + status_msg.goals.data.push_back(goal_msg); + + if (i < status.goals_reached.size()) { + GPSMsg goal_reached_msg; + goal_reached_msg.header.stamp = ros::Time(status.goals_reached[i].time); + goal_reached_msg.latitude = status.goals_reached[i].lat; + goal_reached_msg.longitude = status.goals_reached[i].lon; + status_msg.goals_reached.data.push_back(goal_reached_msg); + } + } + mission_status_pub_.publish(status_msg); +} + void PublishNavStatus() { NavStatusMsg status; status.stamp = ros::Time::now(); @@ -1020,6 +1050,8 @@ int main(int argc, char** argv) { n.advertise("ackermann_curvature_drive", 1); twist_drive_pub_ = n.advertise(FLAGS_twist_drive_topic, 1); + mission_status_pub_ = n.advertise( + "/navigation/mission_status", 1, true); // Only publish status_pub_ = n.advertise("navigation_goal_status", 1); viz_pub_ = n.advertise("visualization", 1); viz_img_pub_ = it_.advertise("vis_image", 1); @@ -1097,6 +1129,7 @@ int main(int argc, char** argv) { // Publish Nav Status PublishNavStatus(); + PublishMissionStatus(); PublishLocalization(); if (nav_succeeded) { if (!FLAGS_no_intermed) {