ask pos

src/modelec_com/include/modelec_com/pcb_odo_interface.hpp

@@ -26,6 +26,7 @@
 #include <modelec_interfaces/srv/odometry_add_waypoint.hpp>
 
 #include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/empty.hpp>
 
 namespace Modelec
 {
@@ -60,6 +61,7 @@ namespace Modelec
         void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
 
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_get_pos_sub_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
@@ -84,7 +86,7 @@ namespace Modelec
         rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr start_odo_sub_;
         bool start_odo_ = false;
 
-        rclcpp::TimerBase::SharedPtr odo_get_pos_timer_;
+        // rclcpp::TimerBase::SharedPtr odo_get_pos_timer_;
 
         // Promises and mutexes to synchronize service responses asynchronously
         std::queue<std::promise<long>> tof_promises_;

src/modelec_com/src/pcb_odo_interface.cpp

@@ -81,6 +81,17 @@ namespace Modelec
         odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
             "odometry/position", 10);
 
+        odo_get_pos_sub_ = this->create_subscription<std_msgs::msg::Empty>(
+            "odometry/get/pos", 30, [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                if (isOk)
+                {
+                    RCLCPP_INFO(this->get_logger(), "Requesting position from PCB");
+
+                    GetPos();
+                }
+            });
+
         odo_speed_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometrySpeed>(
             "odometry/speed", 10);
 
@@ -183,7 +194,7 @@ namespace Modelec
                 }
             });
 
-        odo_get_pos_timer_ = this->create_wall_timer(
+        /*odo_get_pos_timer_ = this->create_wall_timer(
             std::chrono::milliseconds(500),
             [this]()
             {
@@ -193,7 +204,7 @@ namespace Modelec
 
                     GetPos();
                 }
-            });
+            });*/
     }
 
     PCBOdoInterface::~PCBOdoInterface()

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

@@ -9,6 +9,7 @@
 #include <std_srvs/srv/empty.hpp>
 
 #include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/empty.hpp>
 
 #include "deposite_zone.hpp"
 #include "pathfinding.hpp"
@@ -36,6 +37,8 @@ namespace Modelec
 
         int GetTeamId() const;
 
+        void Update();
+
         // void SendWaypoint() const;
         // void SendWaypoint(const std::vector<WaypointMsg>& waypoints) const;
         void SendGoTo();
@@ -156,5 +159,7 @@ namespace Modelec
         rclcpp::Publisher<modelec_interfaces::msg::Spawn>::SharedPtr spawn_pub_;
         rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_spawn_srv_;
 
+        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr odo_get_pos_pub_;
+
     };
 }

src/modelec_strat/src/navigation_helper.cpp

@@ -102,6 +102,9 @@ namespace Modelec
                     spawn_pub_->publish(s);
                 }
             });
+
+        odo_get_pos_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
+            "odometry/get/pos", 30);
     }
 
     void NavigationHelper::ReInit()
@@ -124,6 +127,12 @@ namespace Modelec
         return team_id_;
     }
 
+    void NavigationHelper::Update()
+    {
+        std_msgs::msg::Empty empty_msg;
+        odo_get_pos_pub_->publish(empty_msg);
+    }
+
     void NavigationHelper::SendGoTo()
     {
         while (!waypoint_queue_.empty())

src/modelec_strat/src/strat_fms.cpp

@@ -119,6 +119,8 @@ namespace Modelec
     {
         auto now = this->now();
 
+        nav_->Update();
+
         switch (state_)
         {
         case State::INIT: