Fixed coordinate transform issues, need to fix: OdomToGPSUpdate, Path…

… cost params

config/navigation.lua

@@ -3,7 +3,7 @@ function deg2rad(deg)
 end
 
 OSMPlannerParameters = {
-  gps_topic = "/phone/gps_with_heading";
+  gps_topic = "/vectornav/gps_with_heading";
   gps_goals_topic = "/gps_goals";
   osrm_file = "osrm_texas_cbf_mld/texas-latest.osrm";
   osrm_path_resolution = 8; -- meters between GPS points
@@ -28,10 +28,10 @@ NavigationParameters = {
   max_angular_accel = 0.5;
   max_angular_decel = 0.5;
   max_angular_speed = 1.0;
-  carrot_dist = 10.0;
+  carrot_dist = 20.0;
   system_latency = 0.24;
   obstacle_margin = 0.15;
-  num_options = 31;
+  num_options = 62;
   robot_width = 0.44;
   robot_length = 0.5;
   base_link_offset = 0;
@@ -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 = 5; -- final goal distance will be half this (meters)
+  intermediate_goal_dist = 10; -- final goal distance will be half this (meters)
   max_inflation_radius = 1;
   min_inflation_radius = 0.3;
   local_costmap_resolution = 0.05;
@@ -58,7 +58,7 @@ NavigationParameters = {
   global_costmap_origin_x = -12.8;
   global_costmap_origin_y = 12.8;
   lidar_range_min = 0.1;
-  lidar_range_max = 10.0;
+  lidar_range_max = 25.6;
   replan_dist = 2;
   object_lifespan = 15;
   inflation_coeff = 8;

src/navigation/linear_evaluator.cc

@@ -19,44 +19,44 @@
 */
 //========================================================================
 
-#include <math.h>
+#include "linear_evaluator.h"
+
 #include <float.h>
+#include <math.h>
 
 #include <algorithm>
 #include <memory>
 #include <vector>
 
+#include "ackermann_motion_primitives.h"
+#include "constant_curvature_arcs.h"
+#include "eigen3/Eigen/Dense"
+#include "eigen3/Eigen/Geometry"
 #include "gflags/gflags.h"
 #include "math/line2d.h"
 #include "math/poses_2d.h"
-#include "eigen3/Eigen/Dense"
-#include "eigen3/Eigen/Geometry"
-
 #include "motion_primitives.h"
 #include "navigation_parameters.h"
-#include "constant_curvature_arcs.h"
-#include "ackermann_motion_primitives.h"
-#include "linear_evaluator.h"
 
-using std::min;
-using std::max;
-using std::vector;
-using std::shared_ptr;
-using pose_2d::Pose2Df;
 using Eigen::Vector2f;
 using navigation::MotionLimits;
+using pose_2d::Pose2Df;
+using std::max;
+using std::min;
+using std::shared_ptr;
+using std::vector;
 using namespace geometry;
 using namespace math_util;
 
-DEFINE_double(dw, 1, "Distance weight");
+DEFINE_double(dw, 0.5, "Distance weight");
 DEFINE_double(cw, -0.5, "Clearance weight");
-DEFINE_double(fw, -1, "Free path weight");
+DEFINE_double(fw, 0.5, "Free path weight");
 DEFINE_double(subopt, 1.5, "Max path increase for clearance");
 
 namespace motion_primitives {
 
 shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
-    const vector<shared_ptr<PathRolloutBase>>& paths) {
+    const vector<shared_ptr<PathRolloutBase>> &paths) {
   if (paths.size() == 0) return nullptr;
 
   // Check if there is any path with an obstacle-free path from the end to the
@@ -66,8 +66,8 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
   bool path_to_goal_exists = false;
   for (size_t i = 0; i < paths.size(); ++i) {
     const auto endpoint = paths[i]->EndPoint().translation;
-    clearance_to_goal[i] = StraightLineClearance(
-        Line2f(endpoint, local_target), point_cloud);
+    clearance_to_goal[i] =
+        StraightLineClearance(Line2f(endpoint, local_target), point_cloud);
     if (clearance_to_goal[i] > 0.0) {
       dist_to_goal[i] = (endpoint - local_target).norm();
       path_to_goal_exists = true;
@@ -79,8 +79,9 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
   float best_path_length = FLT_MAX;
   for (size_t i = 0; i < paths.size(); ++i) {
     if (paths[i]->Length() <= 0.0f) continue;
-    const float path_length = (path_to_goal_exists ?
-        (paths[i]->Length() + dist_to_goal[i]) : dist_to_goal[i]);
+    const float path_length =
+        (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i])
+                             : dist_to_goal[i]);
     if (path_length < best_path_length) {
       best_path_length = path_length;
       best = paths[i];
@@ -95,15 +96,14 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
 
   // Next try to find better paths.
   float best_cost = FLAGS_dw * (FLAGS_subopt * best_path_length) +
-      FLAGS_fw * best->Length() +
-      FLAGS_cw * best->Clearance();
+                    FLAGS_fw * best->Length() + FLAGS_cw * best->Clearance();
   for (size_t i = 0; i < paths.size(); ++i) {
     if (paths[i]->Length() <= 0.0f) continue;
-    const float path_length = (path_to_goal_exists ?
-        (paths[i]->Length() + dist_to_goal[i]) : dist_to_goal[i]);
-    const float cost = FLAGS_dw * path_length +
-      FLAGS_fw * paths[i]->Length() +
-      FLAGS_cw * paths[i]->Clearance();
+    const float path_length =
+        (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i])
+                             : dist_to_goal[i]);
+    const float cost = FLAGS_dw * path_length + FLAGS_fw * paths[i]->Length() +
+                       FLAGS_cw * paths[i]->Clearance();
     if (cost < best_cost) {
       best = paths[i];
       best_cost = cost;

src/navigation/navigation.cc

@@ -430,7 +430,6 @@ void Navigation::GetCompensatedOdomUTMTransform(Affine2f& T_tp_utm,
       min_dtime_idx = i;
     }
   }
-
   // Compute new reference odometry to utm transform
   const auto& sync_odom = odom_history_[min_dtime_idx];
   const auto& T_odom_utm = OdometryToUTMTransform(sync_odom, robot_gps_loc_);
@@ -444,8 +443,9 @@ Affine2f Navigation::OdometryToUTMTransform(
   // Assumes that odom and gps_loc correspond to the same time
   // T^odom_utm = T^base_utm * (T^base_odom)^-1
   const auto& T_base_odom = odom.toAffine2f();
-  const auto& utm_vec = gpsToGlobalCoord(gps_loc, gps_loc).cast<float>();
-  double utm_theta = DegToRad(gps_loc.heading);
+  const auto& utm_vec =
+      gpsToGlobalCoord(initial_gps_loc_, gps_loc).cast<float>();
+  double utm_theta = gpsToGlobalHeading(gps_loc);  // radians
 
   // Compute the transform from odom to gps
   const Affine2f T_utm =
@@ -474,6 +474,9 @@ void Navigation::UpdateRobotLocFromOdom(const Odom& msg) {
       Translation2f(robot_loc) * Rotation2Df(DegToRad(robot_gps_loc_.heading));
   const auto& T_new_utm = T_delta_utm * T_utm;
 
+  printf("Compensated robot loc: %f %f\n", T_new_utm.translation().x(),
+         T_new_utm.translation().y());  // Why is y infinity
+
   // Update robot_loc and robot_angle based on new odometry from last known gps
   // location
   robot_loc_ =
@@ -1325,8 +1328,17 @@ float Navigation::GetCarrotDist() { return params_.carrot_dist; }
 
 float Navigation::GetObstacleMargin() { return params_.obstacle_margin; }
 
-Eigen::Vector3f Navigation::GetRobotPose() {
-  return Eigen::Vector3f(robot_loc_.x(), robot_loc_.y(), robot_angle_);
+bool Navigation::GetRobotPose(Eigen::Vector3f& pose) {
+  if (!gps_initialized_) {
+    return false;
+  }
+  // Retrieve uncomponensated robot pose
+  Eigen::Vector2f robot_loc =
+      gpsToGlobalCoord(initial_gps_loc_, robot_gps_loc_).cast<float>();
+  float robot_angle = static_cast<float>(gpsToGlobalHeading(robot_gps_loc_));
+
+  pose = Eigen::Vector3f(robot_loc.x(), robot_loc.y(), robot_angle);
+  return true;
 }
 
 float Navigation::GetRobotWidth() { return params_.robot_width; }
@@ -1411,8 +1423,8 @@ void Navigation::ReplanAndSetNextNavGoal(bool replan) {
   nav_goal_loc_ =
       gpsToGlobalCoord(initial_gps_loc_, gps_nav_goals_loc_[gps_goal_index_])
           .cast<float>();
-  nav_goal_angle_ = gps_nav_goals_loc_[gps_goal_index_].heading;
-
+  nav_goal_angle_ = static_cast<float>(
+      gpsToGlobalHeading(gps_nav_goals_loc_[gps_goal_index_]));
   // Push next nav goal for visualization
 }
 
@@ -1675,8 +1687,15 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
     if (gps_goal_index_ == int(gps_nav_goals_loc_.size()) - 1) {
       goal_tolerance /= 2;
     }
+    printf("Current robot loc %f %f\n", robot_loc_.x(), robot_loc_.y());
+    const auto& metric_nav_goal_loc =
+        gpsToGlobalCoord(initial_gps_loc_, robot_gps_loc_);
+    printf("Nav goal loc %f %f\n", metric_nav_goal_loc.x(),
+           metric_nav_goal_loc.y());
+    printf("Distance to goal: %f\n", (robot_loc_ - nav_goal_loc_).norm());
     bool isGPSGoalReached = osm_planner_.isGoalReached(
         robot_gps_loc_, next_nav_goal_loc, params_.intermediate_goal_dist);
+    printf("IsGoalReached: %d\n", isGPSGoalReached);
     // bool is_goal_in_fov = isGoalInFOV(nav_goal_loc_);
     bool isGPSGoalStillValid = true;
     if (!plan_path_.empty()) {
@@ -1694,7 +1713,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
       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);
+        ReplanAndSetNextNavGoal(true);
       } else {
         nav_state_ = NavigationState::kStopped;
       }

src/navigation/navigation.h

@@ -194,7 +194,7 @@ class Navigation {
   std::vector<Eigen::Vector2f> GetPredictedCloud();
   float GetCarrotDist();
   float GetObstacleMargin();
-  Eigen::Vector3f GetRobotPose();
+  bool GetRobotPose(Eigen::Vector3f& pose);
   float GetRobotWidth();
   float GetRobotLength();
   const cv::Mat& GetVisualizationImage();

src/navigation/navigation_main.cc

@@ -83,9 +83,9 @@ using amrl_msgs::AckermannCurvatureDriveMsg;
 using amrl_msgs::GPSArrayMsg;
 using amrl_msgs::GPSMsg;
 using amrl_msgs::graphNavGPSSrv;
+using amrl_msgs::Localization2DMsg;
 using amrl_msgs::NavStatusMsg;
 using amrl_msgs::VisualizationMsg;
-using amrl_msgs::Localization2DMsg;
 using Eigen::Affine3f;
 using Eigen::Vector2f;
 using Eigen::Vector3f;
@@ -206,7 +206,7 @@ void OdometryCallback(const nav_msgs::Odometry& msg) {
   odom_ = OdomHandler(msg);
   navigation_.UpdateOdometry(odom_);
   // Compute global coordinate offset from odometry t'' to odometry t'
-  navigation_.UpdateRobotLocFromOdom(odom_);
+  // navigation_.UpdateRobotLocFromOdom(odom_);
 }
 
 void GPSCallback(const std_msgs::Float64MultiArray& msg) {
@@ -533,12 +533,17 @@ nav_msgs::Path CarrotToNavMsgsPath(const Vector2f& carrot) {
 
 void PublishLocalization() {
   // Publishes robot pose
+  Eigen::Vector3f robot_pose;
+  bool is_loc_initialized = navigation_.GetRobotPose(robot_pose);
+  if (!is_loc_initialized) {
+    return;
+  }
   Localization2DMsg loc_msg;
   loc_msg.header.stamp = ros::Time::now();
-  loc_msg.pose.x = navigation_.GetRobotPose().x();
-  loc_msg.pose.y = navigation_.GetRobotPose().y();
-  loc_msg.pose.theta = navigation_.GetRobotPose().z(); // theta
-  pose_marker_publisher_.publish(loc_msg);
+  loc_msg.pose.x = robot_pose.x();
+  loc_msg.pose.y = robot_pose.y();
+  loc_msg.pose.theta = robot_pose.z();  // theta
+  localization_pub_.publish(loc_msg);
 }
 
 void PublishPath() {
@@ -1091,6 +1096,7 @@ int main(int argc, char** argv) {
 
     // Publish Nav Status
     PublishNavStatus();
+    PublishLocalization();
     if (nav_succeeded) {
       if (!FLAGS_no_intermed) {
         // Publish Visualizations
@@ -1115,7 +1121,6 @@ int main(int argc, char** argv) {
         DrawTarget();
         DrawPathOptions();
       }
-      PublishLocalization();
       PublishVisualizationMarkers();
       PublishPath();
       PublishNextGPSGoal();