merge resolve

CMakeLists.txt

@@ -28,6 +28,7 @@ SET(ROS_BUILD_SHARED_LIBS false)
 LIST(APPEND CMAKE_PREFIX_PATH "/opt/libtorch")
 FIND_PACKAGE(Torch REQUIRED)
 FIND_PACKAGE(OpenCV REQUIRED)
+FIND_PACKAGE(zed_interfaces)
 
 IF(NOT ROS_BUILD_TYPE)
   SET(ROS_BUILD_TYPE Release)
@@ -52,6 +53,7 @@ ADD_SUBDIRECTORY(src/shared)
 INCLUDE_DIRECTORIES(src/shared)
 INCLUDE_DIRECTORIES(src/config_reader/include)
 INCLUDE_DIRECTORIES(src/third_party)
+INCLUDE_DIRECTORIES(/home/amrl_user/catkin_ws/devel/include/zed_interfaces)
 INCLUDE_DIRECTORIES(src)
 INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
 

manifest.xml

@@ -19,7 +19,7 @@
   <depend package="amrl_msgs"/>
   <depend package="cv_bridge"/>
   <depend package="image_transport"/>
-
+  <depend package="zed_interfaces"/>
 </package>
 
 

src/navigation/navigation.cc

@@ -779,6 +779,10 @@ string Navigation::GetNavStatus() {
   }
 }
 
+uint8_t Navigation::GetNavStatusUint8() {
+  return static_cast<uint8_t>(nav_state_);
+}
+
 vector<Vector2f> Navigation::GetPredictedCloud() {
   return fp_point_cloud_;
 }
@@ -878,7 +882,7 @@ bool Navigation::Run(const double& time,
     prev_state = nav_state_;
     if (nav_state_ == NavigationState::kGoto &&
         local_target_.squaredNorm() < Sq(params_.target_dist_tolerance) &&
-        robot_vel_.squaredNorm() < Sq(params_.target_dist_tolerance)) {
+        robot_vel_.squaredNorm() < Sq(params_.target_vel_tolerance)) {
       nav_state_ = NavigationState::kTurnInPlace;
     } else if (nav_state_ == NavigationState::kTurnInPlace &&
           AngleDist(robot_angle_, nav_goal_angle_) < 

src/navigation/navigation.h

@@ -134,6 +134,7 @@ class Navigation {
   Eigen::Vector2f GetVelocity();
   float GetAngularVelocity();
   std::string GetNavStatus();
+  uint8_t GetNavStatusUint8();
   std::vector<Eigen::Vector2f> GetPredictedCloud();
   float GetCarrotDist();
   float GetObstacleMargin();
@@ -182,17 +183,17 @@ class Navigation {
   // Publish a status message
   void PublishNavStatus(const Eigen::Vector2f& carrot);
 
-  // Current robot location.
+  // Current map frame robot location (LocalizationCallback).
   Eigen::Vector2f robot_loc_;
-  // Current robot orientation.
+  // Current map frame robot orientation (LocalizationCallback).
   float robot_angle_;
   // Current robot velocity.
   Eigen::Vector2f robot_vel_;
   // Current robot angular speed.
   float robot_omega_;
-  // Odometry-reported robot location.
+  // Current odometry frame robot location (OdometryCallback).
   Eigen::Vector2f odom_loc_;
-  // Odometry-reported robot angle.
+  // Current odometry frame robot angle (OdometryCallback).
   float odom_angle_;
   // Newest odometry message received.
   Odom latest_odom_msg_;

src/navigation/navigation_main.cc

@@ -38,7 +38,7 @@
 #include "config_reader/config_reader.h"
 #include "motion_primitives.h"
 #include "constant_curvature_arcs.h"
-#include "actionlib_msgs/GoalStatus.h"
+#include "amrl_msgs/NavStatusMsg.h"
 #include "amrl_msgs/Pose2Df.h"
 #include "glog/logging.h"
 #include "gflags/gflags.h"
@@ -152,7 +152,6 @@ ros::Publisher twist_drive_pub_;
 ros::Publisher viz_pub_;
 ros::Publisher map_lines_publisher_;
 ros::Publisher pose_marker_publisher_;
-ros::Publisher nav_status_pub_;
 ros::Publisher status_pub_;
 ros::Publisher fp_pcl_pub_;
 ros::Publisher path_pub_;
@@ -346,8 +345,9 @@ navigation::Twist ToTwist(geometry_msgs::TwistStamped twist_msg) {
 }
 
 void PublishNavStatus() {
-  GoalStatus status;
-  status.status = 1;
+  NavStatusMsg status;
+  status.stamp = ros::Time::now();
+  status.status = navigation_.GetNavStatusUint8();
 
   status_pub_.publish(status);
 }
@@ -834,7 +834,7 @@ int main(int argc, char** argv) {
       "ackermann_curvature_drive", 1);
   twist_drive_pub_ = n.advertise<geometry_msgs::Twist>(
       FLAGS_twist_drive_topic, 1);
-  status_pub_ = n.advertise<GoalStatus>("navigation_goal_status", 1);
+  status_pub_ = n.advertise<NavStatusMsg>("navigation_goal_status", 1);
   viz_pub_ = n.advertise<VisualizationMsg>("visualization", 1);
   viz_img_pub_ = it_.advertise("vis_image", 1);
   fp_pcl_pub_ = n.advertise<PointCloud>("forward_predicted_pcl", 1);
@@ -864,7 +864,7 @@ int main(int argc, char** argv) {
   ros::Subscriber goto_sub =
       n.subscribe("/move_base_simple/goal", 1, &GoToCallback);
   ros::Subscriber goto_amrl_sub =
-      n.subscribe("/move_base_simple/goal_amrl", 1, &GoToCallbackAMRL);
+      n.subscribe("set_nav_target", 1, &GoToCallbackAMRL);
   ros::Subscriber enabler_sub =
       n.subscribe(CONFIG_enable_topic, 1, &EnablerCallback);
   ros::Subscriber halt_sub =