Removed usage of faulty gps goal distance check

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";

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<GPSPoint>& 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<GPSPoint>& 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<uint32_t, unsigned char> inflation_cells;
     unordered_set<uint32_t> 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 */

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<GPSPoint> goals;
+  std::vector<GPSPoint> 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<Eigen::Vector2f> GetPredictedCloud();
@@ -282,6 +295,7 @@ class Navigation {
   int gps_goal_index_;
   std::vector<GPSPoint> gps_nav_goals_loc_;
   GPSTranslator gps_translator_;
+  MissionStatus mission_status_;
 
   NavigationState nav_state_;
 

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<AckermannCurvatureDriveMsg>("ackermann_curvature_drive", 1);
   twist_drive_pub_ =
       n.advertise<geometry_msgs::Twist>(FLAGS_twist_drive_topic, 1);
+  mission_status_pub_ = n.advertise<MissionStatusMsg>(
+      "/navigation/mission_status", 1, true);  // Only publish
   status_pub_ = n.advertise<NavStatusMsg>("navigation_goal_status", 1);
   viz_pub_ = n.advertise<VisualizationMsg>("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) {