Formatting code changes and fixed compile issues with deep cost map

config/navigation.lua

@@ -37,14 +37,14 @@ NavigationParameters = {
   robot_length = 0.5;
   robot_wheelbase = 0.26;
   base_link_offset = 0.1;
-  max_free_path_length = 10.0;
+  max_free_path_length = 8.0;
   max_clearance = 1.0; -- was 1.0
   can_traverse_stairs = false;
   use_map_speed = true;
   target_dist_tolerance = 0.1;
   target_vel_tolerance = 0.1;
   target_angle_tolerance = 0.05;
-  local_fov = deg2rad(200);
+  local_fov = deg2rad(120);
   use_kinect = true;
   camera_calibration_path = "config/camera_calibration_kinect.yaml";
   model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt";

config/navigation_carrot.lua

@@ -50,7 +50,7 @@ NavigationParameters = {
   model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt";
   evaluator_type = "linear";
   carrot_planner_type = "service"; -- geometric, service
-  intermediate_goal_tolerance = 15; -- final goal distance will be half this (meters)
+  intermediate_goal_tolerance = 10; -- final goal distance will be half this (meters)
   max_inflation_radius = 1;
   min_inflation_radius = 0.3;
   local_costmap_resolution = 0.05;

src/navigation/deep_cost_map_evaluator_service.h

@@ -19,91 +19,95 @@
 */
 //========================================================================
 
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+#include <opencv2/opencv.hpp>
 #include <string>
 #include <vector>
-#include <mutex>
-#include <memory>
-#include <condition_variable>
 
-#include "ros/ros.h"
 #include "amrl_msgs/CostmapSrv.h"
-
-#include "motion_primitives.h"
 #include "image_based_evaluator.h"
-#include "shared/util/timer.h"
+#include "motion_primitives.h"
 #include "navigation_parameters.h"
 #include "navigation_types.h"
-
-#include <cv_bridge/cv_bridge.h>
-#include <opencv2/opencv.hpp>
-#include <image_transport/image_transport.h>
+#include "ros/ros.h"
+#include "shared/util/timer.h"
 
 #ifndef DEEP_COST_MAP_EVALUATOR_SERVICE_H
 #define DEEP_COST_MAP_EVALUATOR_SERVICE_H
 
 namespace motion_primitives {
 
 class DeepCostMapEvaluatorService : public PathEvaluatorBase {
-  public:
-    DeepCostMapEvaluatorService(const navigation::NavigationParameters& params);
+ public:
+  DeepCostMapEvaluatorService(const navigation::NavigationParameters& params);
 
-    std::shared_ptr<PathRolloutBase> FindBest(
+  std::shared_ptr<PathRolloutBase> FindBest(
       const std::vector<std::shared_ptr<PathRolloutBase>>& paths) override;
 
-    /**
-     * Generates an RGB image to visualize the scalar image generated by
-     * GetScalarCostImage
-     */
-    cv::Mat3b GetRGBCostImage(const cv::Mat1f& scalar_cost_map);
-
-    // Calls ros service to obtain costmap image, transforms to odom frame that was passed in
-    void UpdateMap(const navigation::Odom& odom);
-
-    std::vector<float> GetLearnedPathCosts() const override;
-
-    /**
-     * Annotates the latest_costmap_ with intermediate points along each path
-     * option with colors corresponding to the relative cost of the path.
-     *
-     * Implicitly uses scalar costmap image and path_costs_ vector, intended to
-     * be called inside of FindBest after the cost of each path ahs been
-     * calculated. Overwrites the latest_vis_image_ image.
-     */
-    void DrawPathCosts(const std::vector<std::shared_ptr<PathRolloutBase>>& paths,
+  /**
+   * Generates an RGB image to visualize the scalar image generated by
+   * GetScalarCostImage
+   */
+  cv::Mat3b GetRGBCostImage(const cv::Mat1f& scalar_cost_map);
+
+  // Calls ros service to obtain costmap image, transforms to odom frame that
+  // was passed in
+  void UpdateMap(const navigation::Odom& odom);
+
+  std::vector<float> GetLearnedPathCosts() const override;
+
+  /**
+   * Annotates the latest_costmap_ with intermediate points along each path
+   * option with colors corresponding to the relative cost of the path.
+   *
+   * Implicitly uses scalar costmap image and path_costs_ vector, intended to
+   * be called inside of FindBest after the cost of each path ahs been
+   * calculated. Overwrites the latest_vis_image_ image.
+   */
+  void DrawPathCosts(const std::vector<std::shared_ptr<PathRolloutBase>>& paths,
                      std::shared_ptr<PathRolloutBase> best_path);
 
-    cv::Mat3b latest_vis_image_;  // Latest visualization image
-  private:
-    ros::NodeHandle nh_;                 // ROS node handle
-    ros::ServiceClient service_client_;  // ROS service client
-    navigation::NavigationParameters params_;
-    std::mutex mutex_;            // Mutex for thread-safe access
-    std::condition_variable cv_;  // Condition variable for blocking
-    bool service_request_ongoing_;  // Indicates if a service request is ongoing
-
-    std::vector<float> learned_path_costs_;  // Learned cost of each path
-    std::vector<float> path_costs_;  // Cost of each path
-    cv::Mat1f latest_costmap_;      // Latest cost map
-    cv::Mat1f prev_costmap_;       // Last up to date cost map
-    navigation::Odom prev_odom_;              // Last up to date odometry message
-    bool has_map_;          // Indicates if a map exists
-
-    // Computes nonlinear clearance weight
-    float ClearanceCost(const std::shared_ptr<PathRolloutBase> &path);
-
-    // Called by UpdateMap to update map to local frame
-    void UpdateMapToLocalFrame(const cv::Mat1f& costmap, const navigation::Odom& prev_odom, const navigation::Odom& odom);
-
-    // Returns sum of costs along each path
-    std::vector<float> GetLearnedCosts(
-      const std::vector<std::shared_ptr<PathRolloutBase>>& paths, const cv::Mat1f& cost_map);
-
-    // Updates costmap
-    void RequestMapUpdate(const navigation::Odom& odom);
-
-    // Maps cost to colormap
-    cv::Vec3b GetColorFromCost(float cost);
-
+  cv::Mat3b latest_vis_image_;  // Latest visualization image
+ private:
+  ros::NodeHandle nh_;                 // ROS node handle
+  ros::ServiceClient service_client_;  // ROS service client
+  navigation::NavigationParameters params_;
+  std::mutex mutex_;              // Mutex for thread-safe access
+  std::condition_variable cv_;    // Condition variable for blocking
+  bool service_request_ongoing_;  // Indicates if a service request is ongoing
+
+  std::vector<float> learned_path_costs_;  // Learned cost of each path
+  std::vector<float> path_costs_;          // Cost of each path
+  cv::Mat1f latest_costmap_;               // Latest cost map
+  cv::Mat1f prev_costmap_;                 // Last up to date cost map
+  navigation::Odom prev_odom_;             // Last up to date odometry message
+  bool has_map_;                           // Indicates if a map exists
+
+  // Computes nonlinear clearance weight
+  float ClearanceCost(const std::shared_ptr<PathRolloutBase>& path);
+
+  // Called by UpdateMap to update map to local frame
+  void UpdateMapToLocalFrame(const cv::Mat1f& costmap,
+                             const navigation::Odom& prev_odom,
+                             const navigation::Odom& odom);
+
+  float LearnedCost(float cost);
+
+  // Returns sum of costs along each path
+  std::vector<float> GetLearnedCosts(
+      const std::vector<std::shared_ptr<PathRolloutBase>>& paths,
+      const cv::Mat1f& cost_map);
+
+  // Updates costmap
+  void RequestMapUpdate(const navigation::Odom& odom);
+
+  // Maps cost to colormap
+  cv::Vec3b GetColorFromCost(float cost);
 };
 
 }  // namespace motion_primitives

src/navigation/navigation_main.cc

@@ -144,7 +144,7 @@ CONFIG_STRING(enable_topic, "NavigationParameters.enable_topic");
 DEFINE_string(map, "UT_Campus", "Name of navigation map file");
 DEFINE_string(twist_drive_topic, "navigation/cmd_vel", "Drive Command Topic");
 DEFINE_bool(debug_images, false, "Show debug images");
-DEFINE_bool(simulate, true, "Simulate robot");
+DEFINE_bool(simulate, false, "Simulate robot");
 
 // DECLARE_int32(v);
 
@@ -655,7 +655,8 @@ void PublishPath() {
     CarrotPlan carrot_plan;
     bool foundCarrotPlan = navigation_.GetCarrotPlan(carrot_plan);
     if (foundCarrotPlan) {
-      ros_visualization::CarrotPlanToMarkerArray(carrot_plan_pub_, "base_link", carrot_plan);
+      ros_visualization::CarrotPlanToMarkerArray(carrot_plan_pub_, "base_link",
+                                                 carrot_plan);
     }
 
     bool foundGlobalCarrot = navigation_.GetGlobalCarrot(carrot);
@@ -1135,7 +1136,8 @@ int main(int argc, char** argv) {
 
   // ROS path publishers
   fox_path_pub_ = n.advertise<MarkerArray>("/navigation/path_rollouts", 1);
-  carrot_plan_pub_ = n.advertise<MarkerArray>("/navigation/carrot_path_rollout", 1);
+  carrot_plan_pub_ =
+      n.advertise<MarkerArray>("/navigation/carrot_path_rollout", 1);
 
   // Messages
   local_viz_msg_ =
@@ -1182,9 +1184,11 @@ int main(int argc, char** argv) {
   // std_msgs::Header viz_img_header;          // empty viz_img_header
   // viz_img_header.stamp = ros::Time::now();  // time
   // cv_bridge::CvImage viz_img;
-  // if (params.evaluator_type == "cost_map" || params.evaluator_type == "cost_map_service") {
+  // if (params.evaluator_type == "cost_map" || params.evaluator_type ==
+  // "cost_map_service") {
   //   viz_img =
-  //       cv_bridge::CvImage(viz_img_header, sensor_msgs::image_encodings::RGB8,
+  //       cv_bridge::CvImage(viz_img_header,
+  //       sensor_msgs::image_encodings::RGB8,
   //                          navigation_.GetVisualizationImage());
   // }
 
@@ -1247,7 +1251,8 @@ int main(int argc, char** argv) {
       global_viz_msg_.header.stamp = ros::Time::now();
       viz_pub_.publish(local_viz_msg_);
       viz_pub_.publish(global_viz_msg_);
-      if (params.evaluator_type == "cost_map" || params.evaluator_type == "cost_map_service") {
+      if (params.evaluator_type == "cost_map" ||
+          params.evaluator_type == "cost_map_service") {
         cv_bridge::CvImage viz_img;
         viz_img.header.stamp = ros::Time::now();
         viz_img.encoding = sensor_msgs::image_encodings::BGR8;