Merge pull request #8 from purduerov/heartbeat

Heartbeat

.devcontainer/Dockerfile

@@ -0,0 +1,7 @@
+# Specify base image
+FROM ros:humble
+ARG DEBIAN_FRONTEND=noninteractive
+
+# Install system dependencies
+RUN apt-get -y update
+RUN apt-get -y install python3-pip

.devcontainer/devcontainer.json

@@ -0,0 +1,27 @@
+{
+    "name": "X17-Core Dev Container",
+    "build": {
+        "dockerfile": "Dockerfile",
+        "context": ".."
+    },
+    "customizations": {
+        "vscode": {
+            "extensions": [
+                "ms-iot.vscode-ros",          // ROS extension
+                "ms-python.python",           // Python extension (if needed)
+                "ms-azuretools.vscode-docker" // Docker extension
+            ],
+            "settings": {
+                "terminal.integrated.shell.linux": "/bin/bash"
+            }
+        }
+    },
+    "mounts": [
+        "source=${localWorkspaceFolder}/,target=/workspace,type=bind"
+    ],
+    "runArgs": [
+        "--network", "bridge"     // Use host network
+    ],
+    "remoteUser": "root",
+    "postCreateCommand": "echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc && echo 'export ROS_DOMAIN_ID=69' >> /root/.bashrc"
+}

.github/workflows/python_linter.yml

@@ -36,9 +36,7 @@ jobs:
       run: |
         git config --local user.name "GitHub Action"
         git config --local user.email "action@github.com"
-        git add .
-        git commit -m "Auto-format code using Black"
-        git push
+        git diff --exit-code || (git add . && git commit -m "Auto-format code using Black" && git push)
       env:
         GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}  # Use GitHub token for authentication
 

.gitignore

@@ -3,3 +3,5 @@ build
 install
 utils
 log
+.vscode
+!.vscode/settings.json

.vscode/settings.json

@@ -0,0 +1,4 @@
+{
+    "python.analysis.extraPaths": ["utils"],
+    "python.autoComplete.extraPaths": ["utils"]
+}

rov_launch/launch/core_launch.yaml

@@ -1,6 +1,6 @@
 launch:
   - include:
       file: $(find-pkg-share thrust_control)/launch/thrust_control_launch.yaml
-      file: $(find-pkg-share sensors)/launch/sensors_launch.yaml
-      file: $(find-pkg-share tools)/launch/tools_launch.yaml
-      file: $(find-pkg-share videos)/launch/videos_launch.yaml
+      # file: $(find-pkg-share sensors)/launch/sensors_launch.yaml
+      # file: $(find-pkg-share tools)/launch/tools_launch.yaml
+      # file: $(find-pkg-share videos)/launch/videos_launch.yaml

thrust_control/src/ROV_main.py

@@ -3,6 +3,7 @@
 from rclpy.node import Node
 
 from shared_msgs.msg import ThrustCommandMsg, RovVelocityCommand, ImuVelocityCommand
+from utils.heartbeat_helper import HeartbeatHelper
 
 
 class ROVMainNode(Node):
@@ -18,6 +19,9 @@ class ROVMainNode(Node):
     def __init__(self):
         super().__init__("ROV_main")
 
+        # Heartbeat
+        self.heartbeat_helper = HeartbeatHelper(self)
+
         self.controller_sub = self.create_subscription(
             RovVelocityCommand, "/rov_velocity", self._controller_input, 10
         )

thrust_control/src/thrust_control.py

@@ -1,6 +1,7 @@
 #! /usr/bin/python3
 import rclpy
 from rclpy.node import Node
+from utils.heartbeat_helper import HeartbeatHelper
 
 
 from shared_msgs.msg import (
@@ -48,6 +49,9 @@ def __init__(self):
         super().__init__("thrust_control")
         self.tm = ThrustMapper()
 
+        # Setup heartbeat
+        self.heartbeat_helper = HeartbeatHelper(self, 1.0, "thrust_control")
+
         # initialize publishers
         self.thrust_pub = self.create_publisher(FinalThrustMsg, "final_thrust", 10)
         self.status_pub = self.create_publisher(ThrustStatusMsg, "thrust_status", 10)

thrust_control/src/thrust_to_spi.py

@@ -8,6 +8,7 @@
 import RPi.GPIO as GPIO
 
 from shared_msgs.msg import FinalThrustMsg, ToolsMotorMsg
+from utils.heartbeat_helper import HeartbeatHelper
 
 
 def invert_thrust(thrust_value):
@@ -37,6 +38,9 @@ class ThrustToSPINode(Node):
     def __init__(self, bus=0, device=1, mode=0, speed=50000, bits_per_word=8):
         super().__init__("thrust_to_spi")
 
+        # Setup heartbeat
+        self.heartbeat_helper = HeartbeatHelper(self, "thrust_to_spi")
+
         # initialize logger
         logger = self.get_logger().info("INITIALIZED")
 

utils/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(utils)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  utils/heartbeat_helper.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

utils/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>utils</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="akahl@purdue.edu">adam</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>rclpy</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_generators</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

utils/utils/heartbeat_helper.py

@@ -0,0 +1,31 @@
+#! /usr/bin/python3
+
+from rclpy.node import Node
+from std_msgs.msg import Header
+
+class HeartbeatHelper:
+    def __init__(self, node: Node, topic_name="heartbeat", timer_period=5.0):
+        """This class is used to publish a heartbeat message on a given topic at a given interval"""
+        self.timer_period = timer_period
+        self.node = node
+        self.publisher = self.node.create_publisher(Header, topic_name, 10)
+        self.timer = self.node.create_timer(timer_period, self.publish_heartbeat)
+
+    def publish_heartbeat(self):
+        """Publish a heartbeat message on the topic"""
+        self.node.get_logger().info("Publishing heartbeat from {}".format(self.node.get_name()))
+        msg = Header()
+        msg.stamp = self.node.get_clock().now().to_msg()
+        msg.frame_id = self.node.get_name()
+        self.publisher.publish(msg)
+
+    def stop_heartbeat(self):
+        """Stop the heartbeat timer"""
+        self.timer.cancel()
+        self.node.destroy_publisher(self.publisher)
+        self.node.get_logger().info(f'Heartbeat stopped for {self.node.get_name()}')
+
+    def start_heartbeat(self):
+        """Start the heartbeat timer"""
+        self.timer = self.node.create_timer(self.timer_period, self.publish_heartbeat)
+        self.node.get_logger().info(f'Heartbeat started for {self.node.get_name()}')