Refine guardian node

Dockerfile

@@ -222,9 +222,6 @@ RUN pip3 install --ignore-installed \
     rosbags==0.10.4 \
     ultralytics
 
-# Install SAM2 seperately to avoid storage space errors on GitHub Actions
-RUN pip3 install sam2
-
 # Install Black for Python code formatting.
 RUN pip3 install black==24.10.0
 
@@ -236,6 +233,8 @@ RUN mim install 'mmdet>=3.0.0'
 RUN mim install "mmdet3d>=1.1.0"
 RUN mim install "mmpose>=1.1.0"
 
+# Install SAM2 seperately to avoid storage space errors on GitHub Actions
+RUN pip3 install sam2
 
 # install loop closure package "MapClosures"
 # this issue was addressed here: https://github.com/abetlen/llama-cpp-python/issues/707

docs/Miscellaneous/guardian_documentation.md

@@ -14,27 +14,22 @@ parent: Miscellaneous
 ## Overview
 The guardian node is a diagnostics node created to monitor the individual systems running within Navigator. It consolidates data from our various nodes and outputs overall system state, operating mode, and status.
 
+## Notes
+1. The guardian node currently tracks all nodes in the navigator stack; it does not track vehicle interface nodes. 
+2. The guardian node can be extended to handle more nodes by adding the node to one of the two watch lists and adding diagnostic publishing information to the node being added.
+3. Nodes publishing diagnostic information for the guardian node to handle should you the following format: "node_name, status, timestamp"
+
 ---
 
 ### In:
 - **/requested_mode** [*Mode*](../messages.md#mode)
   - Receives the requested operationg mode - disabled, manual control, or autonomous control.
 - **/node_statuses** [*DiagnosticStatus*](https://docs.ros2.org/galactic/api/diagnostic_msgs/msg/DiagnosticStatus.html)
   - Receives diagnostics information of nodes currently operating in the stack.
-- **/planning/path** [*Path*]()
-  - Receives a path from our [planning](../Planning/index.md) subsystem.
 - **/clock** [*Clock*](https://docs.ros2.org/galactic/api/rosgraph_msgs/msg/Clock.html)
 
 ### Out:
 - **/status** [*DiagnosticArray*](https://docs.ros2.org/galactic/api/diagnostic_msgs/msg/DiagnosticArray.html)
   - An array of diagnostic information on each running node on the watchlist, with a global status that reflects the overall state at the end of the array.
 - **/guardian/mode** [*Mode*](../messages.md#mode)
-  - What mode navigator needs to be in based on whether a safety event violation has triggered a disable of the auto or manual mode.
-
----
-
-### function(1)
-TODO
-
-### function(2)
-TODO
+  - What mode navigator needs to be in based on whether a safety event violation has triggered a disable of the auto or manual mode.

src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py

@@ -20,6 +20,7 @@
 from visualization_msgs.msg import Marker
 
 from navigator_msgs.msg import VehicleControl, VehicleSpeed
+from std_msgs.msg import String
 
 
 class Constants:
@@ -232,11 +233,15 @@ class PursePursuitController(Node):
 
     def __init__(self):
         super().__init__("pure_pursuit_controler")
+
+        self.diagnostic_publisher = self.create_publisher(String, '/node_status_info', 10)
+
+        self.diagnostic_pub_timer = self.create_timer(0.5, self.publish_diagnostics)
 
         self.vehicle_state = VehicleState()
         self.path = PursuitPath()
         self.target_waypoint = None
-        self.clock = Clock().clock
+        self.clock = 0.0
 
         self.route_subscriber = self.create_subscription(
             Path, "/planning/path", self.route_callback, 1
@@ -265,8 +270,16 @@ def __init__(self):
             0.1, self.visualize_waypoint_callback
         )
 
-    def clock_callback(self, msg: Clock):
-        self.clock = msg.clock
+    def clock_callback(self, msg):
+        self.clock = msg.clock.sec + (msg.clock.nanosec * 1e-9)
+
+
+    def publish_diagnostics(self):
+        diagnostic_msg = String()
+        diagnostic_msg.data = "pure_pursuit_controller, OK, " + str(self.clock)
+        self.diagnostic_publisher.publish(diagnostic_msg)
+
+
 
     def odometry_callback(self, msg: Odometry):
         self.vehicle_state.pose = msg.pose.pose
@@ -291,6 +304,7 @@ def stop_vehicle(self, steer: float, break_value: float):
         control_msg.brake = break_value
         control_msg.steer = steer
         self.command_publisher.publish(control_msg)
+        self.publish_diagnostics()
 
     def control_callback(self):
         """Calculate and publish the vehicle control commands based on the pure pursuit algorithm."""
@@ -338,6 +352,7 @@ def control_callback(self):
         control_msg.brake = brake
         control_msg.steer = steer
         self.command_publisher.publish(control_msg)
+        self.publish_diagnostics()
 
     def visualize_waypoint_callback(self):
         """Visualize the target waypoint in RVIZ."""
@@ -357,6 +372,7 @@ def visualize_waypoint_callback(self):
             color=ColorRGBA(a=0.3, g=1.0, b=1.0),
         )
         self.barrier_marker_pub.publish(radius_marker)
+        self.publish_diagnostics()
 
         arrow_marker = Marker(
             header=Header(frame_id="base_link", stamp=self.clock),
@@ -370,6 +386,7 @@ def visualize_waypoint_callback(self):
         )
 
         self.barrier_marker_pub.publish(arrow_marker)
+        self.publish_diagnostics()
 
     def visualize_path_callback(self):
         """Visualize the path in RVIZ."""
@@ -393,6 +410,7 @@ def visualize_path_callback(self):
             for x, y in path
         ]
 
+        self.publish_diagnostics()
         self.lookahead_path_publisher.publish(path_msg)
 
 

src/mapping/map_management/include/map_management/MapManager.hpp

@@ -57,7 +57,7 @@
 #include "navigator_msgs/srv/set_route.hpp"
 #include "rosgraph_msgs/msg/clock.hpp"
 #include "std_msgs/msg/float32.hpp"
-
+#include "std_msgs/msg/string.hpp"
 #include "map_management/RouteManager.hpp"
 
 #include "yaml-cpp/yaml.h"
@@ -77,6 +77,7 @@ using geometry_msgs::msg::PolygonStamped;
 using geometry_msgs::msg::PoseStamped;
 using geometry_msgs::msg::TransformStamped;
 using rosgraph_msgs::msg::Clock;
+using std_msgs::msg::String;
 
 using SptSolver = lemon::Dijkstra<lemon::SmartDigraph, lemon::SmartDigraph::ArcMap<double>>;
 using lemon::SmartDigraph;
@@ -117,6 +118,7 @@ namespace navigator
             void publishRefinedRoute();
             void publishSmoothRoute();
             void updateRouteWaypoints(Path::SharedPtr msg);
+            void publishDiagnostics();
             std::vector<odr::LaneKey> calculateRoute(odr::LaneKey start, odr::LaneKey end);
             lemon::SmartDigraph *g = nullptr;
 
@@ -144,6 +146,7 @@ namespace navigator
 
             void setPredeterminedRoute();
 
+            rclcpp::Publisher<String>::SharedPtr diagnostic_pub_;
             rclcpp::Publisher<OccupancyGrid>::SharedPtr drivable_grid_pub_;
             rclcpp::Publisher<OccupancyGrid>::SharedPtr junction_grid_pub_;
             rclcpp::Publisher<OccupancyGrid>::SharedPtr route_dist_grid_pub_;
@@ -162,11 +165,14 @@ namespace navigator
             rclcpp::TimerBase::SharedPtr route_distance_grid_pub_timer_;
             rclcpp::TimerBase::SharedPtr route_timer_;
             rclcpp::TimerBase::SharedPtr smooth_route_timer_;
+            rclcpp::TimerBase::SharedPtr diagnostic_pub_timer_;
 
             std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
             std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 
             Clock::SharedPtr clock_;
+            Clock::SharedPtr second_clock_;
+
             odr::OpenDriveMap *map_ = nullptr;
             std::vector<odr::LanePair> lane_polys_;
             std::vector<odr::Lane> lanes_in_route_;

src/mapping/map_management/src/MapManager.cpp

@@ -68,6 +68,7 @@ MapManagementNode::MapManagementNode() : Node("map_management_node")
     // Publishers and subscribers
     drivable_grid_pub_ = this->create_publisher<OccupancyGrid>("/grid/drivable", 10, parallel_pub_option);
     junction_grid_pub_ = this->create_publisher<OccupancyGrid>("/grid/junction", 10, parallel_pub_option);
+    diagnostic_pub_ = this->create_publisher<String>("/node_status_info", 10, parallel_pub_option);
     //route_dist_grid_pub_ = this->create_publisher<OccupancyGrid>("/grid/route_distance", 10);
     route_path_pub_ = this->create_publisher<Path>("/planning/smoothed_route", 10, parallel_pub_option);
     goal_pose_pub_ = this->create_publisher<PoseStamped>("/planning/goal_pose", 1, parallel_pub_option);
@@ -82,6 +83,7 @@ MapManagementNode::MapManagementNode() : Node("map_management_node")
     //route_timer_ = this->create_wall_timer(LOCAL_ROUTE_LS_FREQ, bind(&MapManagementNode::updateLocalRouteLinestring, this));
 
     smooth_route_timer_ = this->create_wall_timer(SMOOTH_ROUTE_LS_FREQ, bind(&MapManagementNode::publishSmoothRoute, this), mutex_group_);
+    diagnostic_pub_timer_ = this->create_wall_timer(500ms, bind(&MapManagementNode::publishDiagnostics, this), mutex_group_);
 
     tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
@@ -157,6 +159,13 @@ int main(int argc, char* argv[]) {
     return 0;
 }
 
+void MapManagementNode::publishDiagnostics()
+{
+    String msg;
+    msg.data = "map_manager, OK, " + std::to_string(this->clock_->clock.sec) + "." + std::to_string(this->clock_->clock.nanosec);
+    diagnostic_pub_->publish(msg);
+}
+
 /**
  * Given a global (possible large) route linestring:
  * 1. Find the point closest to the car.
@@ -219,6 +228,7 @@ void MapManagementNode::publishSmoothRoute()
             smoothed_route_msg_.poses[i].header.stamp = clock;
         }
         route_path_pub_->publish(smoothed_route_msg_);
+        publishDiagnostics();
     }
     // if the route has been defined, but the message hasn't been made
     else if(route_linestring_.size() > 0)
@@ -236,6 +246,7 @@ void MapManagementNode::publishSmoothRoute()
             smoothed_route_msg_.poses[i] = pose;
         }
         route_path_pub_->publish(smoothed_route_msg_);
+        publishDiagnostics();
     }
 }
 
@@ -816,6 +827,8 @@ void MapManagementNode::publishGrids(float top_dist, float bottom_dist, float si
     goal_pose.header.stamp = clock_->clock;
     goal_pose_pub_->publish(goal_pose);
 
+    publishDiagnostics();
+
     // std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
     // std::cout << "publishGrids(): " << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() << "[ms]" << std::endl;
 }
@@ -1040,6 +1053,7 @@ std::vector<odr::LaneKey> MapManagementNode::calculateRoute(odr::LaneKey start,
 void MapManagementNode::clockCb(Clock::SharedPtr msg)
 {
     this->clock_ = msg;
+    std::cout << "Clock received: " << this->clock_->clock.sec << std::endl;
 }
 
 /**

src/perception/drivable_surface_segmentation/depth_processing/depth_processing/depth_processing.py

@@ -3,21 +3,41 @@
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
 from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, qos_profile_sensor_data
+from std_msgs.msg import String
+from rosgraph_msgs.msg import Clock
 
 import numpy as np
 
 class DepthProcessingNode(Node):
     def __init__(self):
         super().__init__('depth_processing_node')
+
+        self.diagnostic_publisher = self.create_publisher(String, '/node_status_info', 10)
+
+        self.diagnostic_pub_timer = self.create_timer(0.5, self.publish_diagnostics)
+
         self.bridge = CvBridge()
 
         #subscribe to rosbag depth topics
         self.range_image_sub = self.create_subscription(Image, '/ouster/range_image', self.process_depth,qos_profile_sensor_data)
-
+        self.clock_sub = self.create_subscription(String, '/clock', self.clock_cb, 10)
         self.publisher = self.create_publisher(Image, '/processed_depth', 10)
 
+        self.clock = 0.0
+
         self.get_logger().info("Depth Processing Node Started (rosbag)")
 
+    def clock_cb(self, msg: String):
+        self.clock = msg.clock.sec + (msg.clock.nanosec * 1e-9)
+
+
+    def publish_diagnostics(self):
+        diagnostic_msg = String()
+        diagnostic_msg.data = "depth_processing, OK, " + str(self.clock)
+        self.diagnostic_publisher.publish(diagnostic_msg)
+
+
+
     def process_depth(self, msg):
         #ros2 image to opencv numpy array
         depth_image = self.bridge.imgmsg_to_cv2(msg, "passthrough")
@@ -34,6 +54,7 @@ def process_depth(self, msg):
         depth_msg = self.bridge.cv2_to_imgmsg(depth_image, encoding="32FC1")
         depth_msg.header = msg.header  # Keep original timestamps
         self.publisher.publish(depth_msg)
+        self.publish_diagnostics()
 
 
 def main(args=None):

src/perception/drivable_surface_segmentation/image_segmentation/image_segmentation/sam2/sam2/image_segmentation.py

@@ -6,17 +6,24 @@
 import numpy as np
 import torch
 import cv2
+from std_msgs.msg import String
+from rosgraph_msgs.msg import Clock
 
 from sam2.build_sam import build_sam2
 from sam2.sam2_image_predictor import SAM2ImagePredictor
 
 class ImageSegNode(Node):
     def __init__(self):
         super().__init__('image_seg_node')
+
+        self.diagnostic_publisher = self.create_publisher(String, '/node_status_info', 10)
 
-        self.signal_image_sub = self.create_subscription(Image, "/ouster/signal_image", self.image_callback, qos_profile_sensor_data)
+        self.diagnostic_pub_timer = self.create_timer(0.5, self.publish_diagnostics)
 
+        self.signal_image_sub = self.create_subscription(Image, "/ouster/signal_image", self.image_callback, qos_profile_sensor_data)
+        self.clock_sub = self.create_subscription(String, '/clock', self.clock_cb, 10)
         self.segmentation_pub = self.create_publisher(Image, "/segmentation_mask", 10)
+        self.clock = 0.0
 
         self.bridge = CvBridge()
 
@@ -28,6 +35,17 @@ def __init__(self):
 
         print("Started!")
 
+    def clock_cb(self, msg: String):
+        self.clock = msg.clock.sec + (msg.clock.nanosec * 1e-9)
+
+
+    def publish_diagnostics(self):
+        diagnostic_msg = String()
+        diagnostic_msg.data = "image_segmentation, OK, " + str(self.clock)
+        self.diagnostic_publisher.publish(diagnostic_msg)
+
+
+
     def image_callback(self, msg):
 
         #ros2 to opencv format
@@ -41,6 +59,7 @@ def image_callback(self, msg):
         mask_msg = self.bridge.cv2_to_imgmsg(segmentation_mask.astype(np.uint8), encoding="mono8")
         mask_msg.header = msg.header
         self.segmentation_pub.publish(mask_msg)
+        self.publish_diagnostics()
 
     def run_sam_segmentation(self, image):
         self.predictor.set_image(image)