Only snap to GPS position when teaching

main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp

@@ -31,6 +31,7 @@
 
 #include "vtr_navigation_msgs/msg/annotate_route.hpp"
 #include "vtr_navigation_msgs/msg/update_waypoint.hpp"
+#include "vtr_navigation_msgs/msg/mission_command.hpp"
 #include "vtr_navigation_msgs/msg/graph_route.hpp"
 #include "vtr_navigation_msgs/msg/graph_state.hpp"
 #include "vtr_navigation_msgs/msg/graph_update.hpp"
@@ -69,6 +70,8 @@ class GraphMapServer : public tactic::Graph::Callback,
   using MoveGraphMsg = vtr_navigation_msgs::msg::MoveGraph;
   using AnnotateRouteMsg = vtr_navigation_msgs::msg::AnnotateRoute;
   using UpdateWaypointMsg = vtr_navigation_msgs::msg::UpdateWaypoint;
+  using MissionCommandMsg = vtr_navigation_msgs::msg::MissionCommand;
+  using GoalHandle = vtr_navigation_msgs::msg::GoalHandle;
 
   using VertexPtr = tactic::Graph::VertexPtr;
   using EdgePtr = tactic::Graph::EdgePtr;
@@ -113,6 +116,7 @@ class GraphMapServer : public tactic::Graph::Callback,
   void annotateRouteCallback(const AnnotateRouteMsg::ConstSharedPtr msg);
   void poseCallback(const NavSatFix::ConstSharedPtr msg);
   void updateWaypointCallback(const UpdateWaypointMsg::ConstSharedPtr msg);
+  void updateMissionCallback(const MissionCommandMsg::ConstSharedPtr msg);
 
  private:
   /// these functions are called with graph mutex locked
@@ -160,6 +164,8 @@ class GraphMapServer : public tactic::Graph::Callback,
   /** \brief Vertices and routes */
   GraphState graph_state_;
 
+  uint goal_;
+
   /**
    * \brief Cached robot persistent & target localization used after graph
    * relaxation and calibration
@@ -200,6 +206,7 @@ class GraphMapServer : public tactic::Graph::Callback,
   rclcpp::Subscription<MoveGraphMsg>::SharedPtr move_graph_sub_;
   rclcpp::Subscription<AnnotateRouteMsg>::SharedPtr annotate_route_sub_;
   rclcpp::Subscription<UpdateWaypointMsg>::SharedPtr update_waypoint_sub_;
+  rclcpp::Subscription<MissionCommandMsg>::SharedPtr mission_command_sub_;
 
   rclcpp::Subscription<NavSatFix>::SharedPtr pose_pub_;
 

main/src/vtr_navigation/src/graph_map_server.cpp

@@ -88,6 +88,7 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node,
   annotate_route_sub_ = node->create_subscription<AnnotateRouteMsg>("annotate_route", rclcpp::QoS(10), std::bind(&GraphMapServer::annotateRouteCallback, this, std::placeholders::_1), sub_opt);
   move_graph_sub_ = node->create_subscription<MoveGraphMsg>("move_graph", rclcpp::QoS(10), std::bind(&GraphMapServer::moveGraphCallback, this, std::placeholders::_1), sub_opt);
   update_waypoint_sub_ = node->create_subscription<UpdateWaypointMsg>("update_waypoint", rclcpp::QoS(10), std::bind(&GraphMapServer::updateWaypointCallback, this, std::placeholders::_1), sub_opt);
+  mission_command_sub_ = node->create_subscription<MissionCommandMsg>("mission_command", rclcpp::QoS(10), std::bind(&GraphMapServer::updateMissionCallback, this, std::placeholders::_1), sub_opt);
   // clang-format on
 
   pose_pub_ = node->create_subscription<NavSatFix>(pose_pub_topic_, rclcpp::QoS(10), std::bind(&GraphMapServer::poseCallback, this, std::placeholders::_1), sub_opt);
@@ -155,6 +156,11 @@ void GraphMapServer::followingRouteSrvCallback(
   response->following_route = following_route_;
 }
 
+void GraphMapServer::updateMissionCallback(
+    const MissionCommandMsg::ConstSharedPtr msg) {
+  goal_ = msg->goal_handle.type;
+}
+
 void GraphMapServer::annotateRouteCallback(
     const AnnotateRouteMsg::ConstSharedPtr msg) {
   CLOG(DEBUG, "navigation.graph_map_server")
@@ -242,7 +248,7 @@ void GraphMapServer::poseCallback(const NavSatFix::ConstSharedPtr msg) {
   auto prev_coords = gps_coords_[0];
   auto dist = haversineDist(prev_coords.second, msg->latitude, prev_coords.first, msg->longitude);
 
-  if (gps_coords_.size() >= 2 && !initial_pose_set_ && dist > dist_thres_) {
+  if (goal_ == GoalHandle::TEACH && gps_coords_.size() >= 2 && !initial_pose_set_ && dist > dist_thres_) {
     auto delta_lng = deltaLongToMetres(prev_coords.second, msg->latitude, prev_coords.first, msg->longitude);
     auto delta_lat = deltaLatToMetres(prev_coords.second, msg->latitude);