Merge pull request #7 from SoonerRobotics/fixthecore

Nodes are now authoritative over their own state

autonav_ws/src/autonav_launch/launch/test.xml

@@ -1,5 +1,4 @@
 <launch>
-    <node pkg="autonav_shared" exec="synchronizer" output="screen" emulate_tty="true" />
     <node pkg="autonav_example_py" exec="example.py" output="screen" emulate_tty="true" />
     <node pkg="autonav_example_cpp" exec="autonav_example_cpp" output="screen" emulate_tty="true" />
 </launch>

autonav_ws/src/autonav_shared/CMakeLists.txt

@@ -21,9 +21,5 @@ target_include_directories(${PROJECT_NAME} PUBLIC
     $<INSTALL_INTERFACE:include>
 )
 
-ament_auto_add_executable(synchronizer
-    src/synchronizer.cpp
-)
-
 ament_python_install_package(${PROJECT_NAME})
 ament_auto_package()

autonav_ws/src/autonav_shared/autonav_shared/node.py

@@ -2,7 +2,6 @@
 from rclpy.node import Node as RclpyNode
 from autonav_shared.types import DeviceState, LogLevel, SystemState
 from autonav_msgs.msg import SystemState as SystemStateMsg, DeviceState as DeviceStateMsg, Performance, Log
-from autonav_msgs.srv import SetDeviceState, SetSystemState
 import sty
 import time
 import inspect
@@ -22,12 +21,11 @@ def __init__(self, name: str) -> None:
         # TODO: Setup all relevant publishers, subscribers, services, clients, etc
         self.system_state_sub = self.create_subscription(SystemStateMsg, "/autonav/shared/system", self.system_state_callback, 10)
         self.device_state_sub = self.create_subscription(DeviceStateMsg, "/autonav/shared/device", self.device_state_callback, 10)
+        self.system_state_pub = self.create_publisher(SystemStateMsg, "/autonav/shared/system", 10)
+        self.device_state_pub = self.create_publisher(DeviceStateMsg, "/autonav/shared/device", 10)
 
         self.performance_pub = self.create_publisher(Performance, "/autonav/shared/performance", 10)
         self.log_pub = self.create_publisher(Log, "/autonav/shared/log", 10)
-
-        self.set_device_state_client = self.create_client(SetDeviceState, "/autonav/shared/set_device_state")
-        self.set_system_state_client = self.create_client(SetSystemState, "/autonav/shared/set_system_state")
 
         self.set_device_state(DeviceState.WARMING)
 
@@ -86,84 +84,26 @@ def set_device_state(self, state: DeviceState) -> None:
         """
         Set the state of our device.
         """
-        msg = SetDeviceState.Request()
+        msg = DeviceStateMsg()
         msg.device = self.get_name()
         msg.state = state.value
-
-        while not self.set_device_state_client.wait_for_service(timeout_sec=1.0):
-            if not rclpy.ok():
-                self.log("Interrupted while waiting for the set_device_state service", LogLevel.FATAL)
-                return
-
-            self.log("Service set_device_state not available, waiting again...", LogLevel.WARN)
-
-        # Send the request, use the callback to handle the response
-        future = self.set_device_state_client.call_async(msg)
-        future.add_done_callback(self.set_device_state_callback)
-
-    def set_device_state_callback(self, future) -> None:
-        """
-        Callback for the set_device_state service.
-        """
-        try:
-            result = future.result()
-            if result is None or not result.ok:
-                self.log("Failed to set device state", LogLevel.ERROR)
-            # else:
-            #     self.log("Successfully set device state", LogLevel.DEBUG)
-        except Exception as e:
-            self.log(f"Failed to set device state: {e}", LogLevel.ERROR)
+        self.device_state_pub.publish(msg)
 
     def set_system_state(self, state: SystemState) -> None:
         """
         Set the state of the system.
         """
-        msg = SetSystemState.Request()
+        msg = SystemStateMsg()
         msg.state = state.value
         msg.mobility = self.mobility
-
-        while not self.set_system_state_client.wait_for_service(timeout_sec=1.0):
-            if not rclpy.ok():
-                self.log("Interrupted while waiting for the set_system_state service", LogLevel.FATAL)
-                return
-
-            self.log("Service set_system_state not available, waiting again...", LogLevel.WARN)
-
-        # Send the request, use the callback to handle the response
-        future = self.set_system_state_client.call_async(msg)
-        future.add_done_callback(self.set_system_state_callback)
+        self.system_state_pub.publish(msg)
 
     def set_mobility(self, mobility: bool) -> None:
         """
         Set the mobility of the system.
         """
-        msg = SetSystemState.Request()
-        msg.state = self.system_state.value
-        msg.mobility = mobility
-
-        while not self.set_system_state_client.wait_for_service(timeout_sec=1.0):
-            if not rclpy.ok():
-                self.log("Interrupted while waiting for the set_system_state service", LogLevel.FATAL)
-                return
-
-            self.log("Service set_system_state not available, waiting again...", LogLevel.WARN)
-
-        # Send the request, use the callback to handle the response
-        future = self.set_system_state_client.call_async(msg)
-        future.add_done_callback(self.set_system_state_callback)
-
-    def set_system_state_callback(self, future) -> None:
-        """
-        Callback for the set_system_state service.
-        """
-        try:
-            result = future.result()
-            if result is None or not result.ok:
-                self.log("Failed to set system state", LogLevel.ERROR)
-            else:
-                self.log("Successfully set system state", LogLevel.DEBUG)
-        except Exception as e:
-            self.log(f"Failed to set system state: {e}", LogLevel.ERROR)
+        self.mobility = mobility
+        self.set_system_state(self.system_state)
 
     def get_device_state(self, device: str = None) -> DeviceState:
         """

autonav_ws/src/autonav_shared/include/autonav_shared/node.hpp

@@ -97,10 +97,8 @@ namespace AutoNav
         // Publishers
         rclcpp::Publisher<autonav_msgs::msg::Performance>::SharedPtr performance_pub;
         rclcpp::Publisher<autonav_msgs::msg::Log>::SharedPtr log_pub;
-
-        // Clients
-        rclcpp::Client<autonav_msgs::srv::SetDeviceState>::SharedPtr set_device_state_client;
-        rclcpp::Client<autonav_msgs::srv::SetSystemState>::SharedPtr set_system_state_client;
+        rclcpp::Publisher<autonav_msgs::msg::DeviceState>::SharedPtr device_state_pub;
+        rclcpp::Publisher<autonav_msgs::msg::SystemState>::SharedPtr system_state_pub;
 
         // Functions
         void system_state_callback(const autonav_msgs::msg::SystemState::SharedPtr msg);

autonav_ws/src/autonav_shared/src/node.cpp

@@ -17,9 +17,8 @@ namespace AutoNav
 
         performance_pub = this->create_publisher<autonav_msgs::msg::Performance>("/autonav/shared/performance", 10);
         log_pub = this->create_publisher<autonav_msgs::msg::Log>("/autonav/shared/log", 10);
-
-        set_device_state_client = this->create_client<autonav_msgs::srv::SetDeviceState>("/autonav/shared/set_device_state");
-        set_system_state_client = this->create_client<autonav_msgs::srv::SetSystemState>("/autonav/shared/set_system_state");
+        device_state_pub = this->create_publisher<autonav_msgs::msg::DeviceState>("/autonav/shared/device", 10);
+        system_state_pub = this->create_publisher<autonav_msgs::msg::SystemState>("/autonav/shared/system", 10);
 
         set_device_state(AutoNav::DeviceState::WARMING);
     }
@@ -64,78 +63,20 @@ namespace AutoNav
 
     void Node::set_device_state(const std::string & device, AutoNav::DeviceState state)
     {
-        if (has_ownership)
-        {
-            // Assign the new state
-            device_states.insert_or_assign(device, state);
-            return;
-        }
-
-        auto request = std::make_shared<autonav_msgs::srv::SetDeviceState::Request>();
-        request->device = device;
-        request->state = static_cast<uint8_t>(state);
-
-        // Wait for the service to be available
-        while (!set_device_state_client->wait_for_service(std::chrono::seconds(1)))
-        {
-            if (!rclcpp::ok())
-            {
-                log("Interrupted while waiting for the set_device_state service", Logging::LogLevel::FATAL);
-                return;
-            }
-
-            log("Service set_device_state not available, waiting again...", Logging::LogLevel::WARN);
-        }
-
-        using ServiceResponseFuture = rclcpp::Client<autonav_msgs::srv::SetDeviceState>::SharedFuture;
-        auto response_received_callback = [this](ServiceResponseFuture future) {
-            auto response = future.get();
-            if (!response->ok)
-            {
-                log("Failed to update device state", Logging::LogLevel::ERROR);
-            } else {
-                log("Successfully updated device state", Logging::LogLevel::DEBUG);
-            }
-        };
-
-        auto future_result = set_device_state_client->async_send_request(request, response_received_callback);
+        // Publish the update
+        autonav_msgs::msg::DeviceState msg;
+        msg.device = device;
+        msg.state = static_cast<uint8_t>(state);
+        device_state_pub->publish(msg);
     }
 
     void Node::set_system_state(AutoNav::SystemState state, bool has_mobility)
     {
-        if (has_ownership)
-        {
-            system_state = state;
-            this->has_mobility = has_mobility;
-            return;
-        }
-
-        auto request = std::make_shared<autonav_msgs::srv::SetSystemState::Request>();
-        request->state = static_cast<uint8_t>(state);
-        request->mobility = has_mobility;
-
-        // Wait for the service to be available
-        while (!set_system_state_client->wait_for_service(std::chrono::seconds(1)))
-        {
-            if (!rclcpp::ok())
-            {
-                log("Interrupted while waiting for the set_system_state service", Logging::LogLevel::FATAL);
-                return;
-            }
-
-            log("Service set_system_state not available, waiting again...", Logging::LogLevel::WARN);
-        }
-
-        using ServiceResponseFuture = rclcpp::Client<autonav_msgs::srv::SetSystemState>::SharedFuture;
-        auto response_received_callback = [this](ServiceResponseFuture future) {
-            auto response = future.get();
-            if (!response->ok)
-            {
-                log("Failed to update system state", Logging::LogLevel::ERROR);
-            }
-        };
-
-        auto future_result = set_system_state_client->async_send_request(request, response_received_callback);
+        // Publish the update
+        autonav_msgs::msg::SystemState msg;
+        msg.state = static_cast<uint8_t>(state);
+        msg.mobility = has_mobility;
+        system_state_pub->publish(msg);
     }
 
     void Node::system_state_callback(const autonav_msgs::msg::SystemState::SharedPtr msg)
@@ -163,8 +104,8 @@ namespace AutoNav
         if (new_device || is_warming_up)
         {
             init();
-            device_states.insert_or_assign(msg->device, static_cast<AutoNav::DeviceState>(msg->state));
         }
+        device_states.insert_or_assign(msg->device, static_cast<AutoNav::DeviceState>(msg->state));
     }
 
     // TODO: Log to file