merge main and resolve conflict

ut-amrl · Jun 29, 2023 · 269665f · 269665f
2 parents 17a4558 + e54b078
commit 269665f
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 10 deletions.
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
@@ -783,6 +783,10 @@ string Navigation::GetNavStatus() {
   }
 }
 
+uint8_t Navigation::GetNavStatusUint8() {
+  return static_cast<uint8_t>(nav_state_);
+}
+
 vector<Vector2f> Navigation::GetPredictedCloud() {
   return fp_point_cloud_;
 }
@@ -882,7 +886,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_) < 

diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h
@@ -135,6 +135,7 @@ class Navigation {
   int GetNavState();
   float GetAngularVelocity();
   std::string GetNavStatus();
+  uint8_t GetNavStatusUint8();
   std::vector<Eigen::Vector2f> GetPredictedCloud();
   float GetCarrotDist();
   float GetObstacleMargin();
@@ -183,17 +184,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_;

diff --git a/src/navigation/navigation_main.cc b/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"
@@ -73,7 +73,7 @@
 #include "motion_primitives.h"
 #include "navigation.h"
 
-using actionlib_msgs::GoalStatus;
+using amrl_msgs::NavStatusMsg;
 using amrl_msgs::VisualizationMsg;
 using amrl_msgs::AckermannCurvatureDriveMsg;
 using math_util::DegToRad;
@@ -142,7 +142,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_;
@@ -321,8 +320,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);
 }