diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index 3b7e57f..31dfb8e 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -26,6 +26,7 @@ #include #include +#include namespace Modelec { @@ -60,6 +61,7 @@ namespace Modelec void PCBCallback(const std_msgs::msg::String::SharedPtr msg); rclcpp::Publisher::SharedPtr odo_pos_publisher_; + rclcpp::Subscription::SharedPtr odo_get_pos_sub_; rclcpp::Publisher::SharedPtr odo_speed_publisher_; rclcpp::Publisher::SharedPtr odo_tof_publisher_; rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; @@ -84,7 +86,7 @@ namespace Modelec rclcpp::Subscription::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> tof_promises_; diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 2977791..57a1ea4 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -81,6 +81,17 @@ namespace Modelec odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); + odo_get_pos_sub_ = this->create_subscription( + "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( "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() diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 7d00291..d0a1e98 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -9,6 +9,7 @@ #include #include +#include #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& waypoints) const; void SendGoTo(); @@ -156,5 +159,7 @@ namespace Modelec rclcpp::Publisher::SharedPtr spawn_pub_; rclcpp::Service::SharedPtr ask_spawn_srv_; + rclcpp::Publisher::SharedPtr odo_get_pos_pub_; + }; } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 973e3bb..479b4df 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -102,6 +102,9 @@ namespace Modelec spawn_pub_->publish(s); } }); + + odo_get_pos_pub_ = node_->create_publisher( + "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()) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 5e1f87f..970b988 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -119,6 +119,8 @@ namespace Modelec { auto now = this->now(); + nav_->Update(); + switch (state_) { case State::INIT: