[Feature] Added service based waypoint planner with continuous odomet…

…ry integration

CMakeLists.txt

@@ -80,6 +80,7 @@ ROSBUILD_ADD_EXECUTABLE(navigation
                         src/navigation/image_tiler.cc
                         src/navigation/deep_cost_map_evaluator.cc
                         src/navigation/image_based_evaluator.cc
+                        src/navigation/carrot_service.cc
                         )
 TARGET_LINK_LIBRARIES(navigation 
     shared_library 

config/navigation.lua

@@ -49,6 +49,7 @@ NavigationParameters = {
   camera_calibration_path = "config/camera_calibration_kinect.yaml";
   model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt";
   evaluator_type = "linear";
+  carrot_planner_type = "service"; -- geometric, service
   intermediate_goal_tolerance = 10; -- final goal distance will be half this (meters)
   max_inflation_radius = 1;
   min_inflation_radius = 0.3;

src/navigation/carrot_base.h

@@ -0,0 +1,29 @@
+#ifndef CARROT_BASE_H
+#define CARROT_BASE_H
+
+#include <vector>
+
+#include "eigen3/Eigen/Dense"
+#include "navigation_types.h"
+
+using navigation::CarrotPlan;
+using navigation::Odom;
+
+class CarrotBase {
+ public:
+  // Virtual destructor for proper cleanup of derived classes
+  virtual ~CarrotBase() {}
+
+  /**
+   * Pure virtual function to be implemented by derived classes.
+   * @param local_carrot: The target waypoint in local coordinates.
+   * @return A struct containing the computed carrot plan.
+   */
+  virtual struct CarrotPlan GetCarrot(const Eigen::Vector2f& local_carrot,
+                                      const Odom& odom) = 0;
+
+ protected:
+  Eigen::Vector2f current_carrot_;  // Current carrot location
+};
+
+#endif

src/navigation/carrot_geometric.h

@@ -0,0 +1,67 @@
+//========================================================================
+//  This software is free: you can redistribute it and/or modify
+//  it under the terms of the GNU Lesser General Public License Version 3,
+//  as published by the Free Software Foundation.
+//
+//  This software is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU Lesser General Public License for more details.
+//
+//  You should have received a copy of the GNU Lesser General Public License
+//  Version 3 in the file COPYING that came with this distribution.
+//  If not, see <http://www.gnu.org/licenses/>.
+//========================================================================
+/*!
+\file    carrot_service.h
+\brief   Carrot server client interface for local navigation.
+\author  Arthur Zhang (C) 2024
+*/
+//========================================================================
+#ifndef API_CARROT_H
+#define API_CARROT_H
+
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+
+#include <condition_variable>
+#include <mutex>
+#include <vector>
+
+#include "amrl_msgs/CarrotPlannerSrv.h"
+#include "carrot_base.h"  // Include the parent class header
+
+using navigation::CarrotPlan;
+using navigation::Odom;
+
+class CarrotService : public CarrotBase {
+ public:
+  // Constructor
+  explicit CarrotService(const std::string& service_name);
+
+  // Overridden plan function
+  CarrotPlan GetCarrot(const Eigen::Vector2f& local_carrot,
+                       const Odom& odom) override;
+
+ private:
+  ros::NodeHandle nh_;                 // ROS node handle
+  ros::ServiceClient service_client_;  // ROS service client
+
+  // Thread-safe shared state
+  CarrotPlan latest_carrot_plan_;  // Latest carrot plan
+  Odom latest_odom_;               // Latest odometry message
+  bool has_carrot_ = false;        // Indicates if a carrot exists
+  bool service_request_ongoing_ =
+      false;                    // Indicates if a service request is ongoing
+  std::mutex mutex_;            // Mutex for thread-safe access
+  std::condition_variable cv_;  // Condition variable for blocking
+
+  // Helper functions
+  void TransformCarrot(const Odom& odom, CarrotPlan& carrot_plan);
+  void RequestCarrotUpdate(const Eigen::Vector2f& local_waypoint,
+                           const Odom& odom);
+};
+
+#endif  // API_CARROT_H

src/navigation/carrot_service.cc

@@ -0,0 +1,111 @@
+#include "carrot_service.h"
+
+#include <stdexcept>
+#include <thread>
+
+using amrl_msgs::CarrotPlannerSrv;
+using Eigen::Affine2f;
+using Eigen::Vector2f;
+using navigation::CarrotPlan;
+using navigation::Odom;
+using std::vector;
+
+// Constructor
+CarrotService::CarrotService(const std::string& service_name) {
+  // Initialize the ROS service client
+  service_client_ = nh_.serviceClient<CarrotPlannerSrv>(service_name);
+}
+
+CarrotPlan CarrotService::GetCarrot(const Vector2f& local_waypoint,
+                                    const Odom& odom) {
+  // Initiate a service request if no request is ongoing
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!service_request_ongoing_) {
+      service_request_ongoing_ = true;
+      std::thread(&CarrotService::RequestCarrotUpdate, this, local_waypoint,
+                  odom)
+          .detach();
+    }
+  }
+
+  // Transform the current path to the current odom frame
+  TransformCarrot(odom, latest_carrot_plan_);
+
+  return latest_carrot_plan_;
+}
+
+void CarrotService::TransformCarrot(const Odom& odom, CarrotPlan& carrot_plan) {
+  // Block until a carrot exists
+  std::unique_lock<std::mutex> lock(mutex_);
+  cv_.wait(lock, [this] { return has_carrot_; });
+
+  // Transformations at time t0 and t1
+  Eigen::Affine2f T_baset0_odom =
+      latest_odom_.toAffine2f();  // Transformation from local @ t' to odom
+  Eigen::Affine2f T_baset1_odom =
+      odom.toAffine2f();  // Transformation from local @ t'' to odom
+
+  // Compute the relative transformation: local @ t' -> odom -> local @ t''
+  Eigen::Affine2f T_baset0_baset1 = T_baset1_odom.inverse() * T_baset0_odom;
+
+  // Transform the path to the local frame at t''
+  for (const auto& point : latest_carrot_plan_.path) {
+    // Transform local frame @ t' to odom frame, then to local frame @ t''
+    carrot_plan.path.emplace_back(T_baset0_baset1 * point);
+  }
+  carrot_plan.path_idx = carrot_plan.path.size() - 1;
+}
+
+void CarrotService::RequestCarrotUpdate(const Vector2f& local_waypoint,
+                                        const Odom& odom) {
+  try {
+    CarrotPlannerSrv srv;
+
+    // Prepare service request
+    srv.request.carrot.header.stamp = ros::Time::now();
+    srv.request.carrot.header.frame_id = "map";
+    srv.request.carrot.pose.position.x = local_waypoint.x();
+    srv.request.carrot.pose.position.y = local_waypoint.y();
+    srv.request.carrot.pose.position.z = 0.0;
+    srv.request.carrot.pose.orientation.x = 0.0;
+    srv.request.carrot.pose.orientation.y = 0.0;
+    srv.request.carrot.pose.orientation.z = 0.0;
+    srv.request.carrot.pose.orientation.w = 1.0;
+
+    // Call the service
+    if (service_client_.call(srv)) {
+      if (!srv.response.path.poses.empty()) {
+        CarrotPlan new_plan;
+
+        // Extract path
+        for (const auto& pose : srv.response.path.poses) {
+          new_plan.path.emplace_back(
+              Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y));
+        }
+
+        // Update shared carrot plan
+        {
+          std::lock_guard<std::mutex> lock(mutex_);
+          latest_carrot_plan_ = new_plan;
+          latest_odom_ = odom;
+          has_carrot_ = true;
+        }
+        // printf("Service call successful\n");
+        cv_.notify_all();
+      } else {
+        ROS_ERROR("Carrot planner service failed to compute a valid plan");
+      }
+    } else {
+      ROS_ERROR("Failed to call carrot planner service");
+    }
+  } catch (const std::exception& e) {
+    ROS_ERROR("Exception in carrot planner service: %s", e.what());
+  }
+
+  // Mark service request as completed
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    service_request_ongoing_ = false;
+  }
+}

src/navigation/carrot_service.h

@@ -0,0 +1,67 @@
+//========================================================================
+//  This software is free: you can redistribute it and/or modify
+//  it under the terms of the GNU Lesser General Public License Version 3,
+//  as published by the Free Software Foundation.
+//
+//  This software is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU Lesser General Public License for more details.
+//
+//  You should have received a copy of the GNU Lesser General Public License
+//  Version 3 in the file COPYING that came with this distribution.
+//  If not, see <http://www.gnu.org/licenses/>.
+//========================================================================
+/*!
+\file    carrot_service.h
+\brief   Carrot server client interface for local navigation.
+\author  Arthur Zhang (C) 2024
+*/
+//========================================================================
+#ifndef API_CARROT_H
+#define API_CARROT_H
+
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+
+#include <condition_variable>
+#include <mutex>
+#include <vector>
+
+#include "amrl_msgs/CarrotPlannerSrv.h"
+#include "carrot_base.h"  // Include the parent class header
+
+using navigation::CarrotPlan;
+using navigation::Odom;
+
+class CarrotService : public CarrotBase {
+ public:
+  // Constructor
+  explicit CarrotService(const std::string& service_name);
+
+  // Overridden plan function
+  CarrotPlan GetCarrot(const Eigen::Vector2f& local_carrot,
+                       const Odom& odom) override;
+
+ private:
+  ros::NodeHandle nh_;                 // ROS node handle
+  ros::ServiceClient service_client_;  // ROS service client
+
+  // Thread-safe shared state
+  CarrotPlan latest_carrot_plan_;  // Latest carrot plan
+  Odom latest_odom_;               // Latest odometry message
+  bool has_carrot_ = false;        // Indicates if a carrot exists
+  bool service_request_ongoing_ =
+      false;                    // Indicates if a service request is ongoing
+  std::mutex mutex_;            // Mutex for thread-safe access
+  std::condition_variable cv_;  // Condition variable for blocking
+
+  // Helper functions
+  void TransformCarrot(const Odom& odom, CarrotPlan& carrot_plan);
+  void RequestCarrotUpdate(const Eigen::Vector2f& local_waypoint,
+                           const Odom& odom);
+};
+
+#endif  // API_CARROT_H

src/navigation/navigation.cc

@@ -221,10 +221,22 @@ void Navigation::Initialize(const NavigationParameters& params,
   } else if (params_.evaluator_type == "linear") {
     evaluator = (PathEvaluatorBase*)new LinearEvaluator();
   } else {
-    printf("Uknown evaluator type %s\n", params_.evaluator_type.c_str());
+    printf("Unknown evaluator type %s\n", params_.evaluator_type.c_str());
     exit(1);
   }
   evaluator_ = std::unique_ptr<PathEvaluatorBase>(evaluator);
+
+  CarrotBase* carrot_planner = nullptr;
+  if (params_.carrot_planner_type == "service") {
+    carrot_planner = new CarrotService("/navigation/carrot_planner");
+  } else if (params_.carrot_planner_type == "geometric") {
+    printf("Geometric carrot planner is built in\n");
+  } else {
+    printf("Umknown carrot planner type %s\n",
+           params_.carrot_planner_type.c_str());
+    exit(1);
+  }
+  carrot_planner_ = std::unique_ptr<CarrotBase>(carrot_planner);
 }
 
 void Navigation::InitializeOSM(const OSMPlannerParameters& params) {
@@ -1050,17 +1062,29 @@ bool Navigation::GetLocalCarrotHeading(Vector2f& carrot) {
   Vector2f T_goal_map = nav_goal_loc_;
   Vector2f T_base_map = robot_loc_;
 
-  // Compute current odometry estimated location in map frame
-  // Affine2f T_odom_map =
-  //     OdometryToUTMTransform(initial_odom_msg_, initial_gps_loc_);
-  // Affine2f T_base_odom = Translation2f(odom_loc_) *
-  // Rotation2Df(odom_angle_); Affine2f T_base_map = T_odom_map *
-  // T_base_odom;
-
   // Goal carrot is a point on the carrot circle around robot
   Vector2f goal_vec_norm = (T_goal_map - T_base_map).normalized();
   // carrot = T_odom_map.translation() + goal_vec_norm * params_.carrot_dist;
-  carrot = T_base_map + goal_vec_norm * params_.carrot_dist;
+  Vector2f local_carrot = goal_vec_norm * params_.carrot_dist;
+
+  if (carrot_planner_ != nullptr) {
+    // Correct for heading to align with local frame
+    local_carrot = Rotation2Df(-robot_angle_) * local_carrot;
+
+    const CarrotPlan& plan =
+        carrot_planner_->GetCarrot(local_carrot, latest_odom_msg_);
+    if (plan.path.empty()) return false;
+    local_carrot =
+        plan.path[plan.path_idx];  // override local carrot with service planner
+
+    // Rotate back to global frame
+    local_carrot = Rotation2Df(robot_angle_) * local_carrot;
+
+    printf("GetLocalCarrotHeading(): Global carrot from planner %f %f\n",
+           local_carrot.x(), local_carrot.y());
+  }
+  carrot = T_base_map + local_carrot;  // Transform carrot to global frame
+
   return true;
 }