Add science module drill control

.gitmodules

@@ -9,3 +9,7 @@
 [submodule "src/third-party/zed-ros2-wrapper"]
 	path = src/third-party/zed-ros2-wrapper
 	url = https://github.com/stereolabs/zed-ros2-wrapper.git
+[submodule "src/third-party/micro_ros_setup"]
+	path = src/third-party/micro_ros_setup
+	url = https://github.com/micro-ROS/micro_ros_setup.git
+	branch = humble

rosdep-keys.txt

@@ -77,4 +77,11 @@ boost
 joint_trajectory_controller
 diff_drive_controller
 joint_state_publisher_gui
-libjsoncpp
+libjsoncpp
+flex
+bison
+libncurses-dev
+usbutils
+tinyxml2
+clang-tidy
+python3-vcstool

src/HW-Devices/ros_phoenix_humble/launch/science.launch.py

@@ -0,0 +1,31 @@
+import launch
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    """Generate launch description with multiple components."""
+    container = ComposableNodeContainer(
+        name="PhoenixContainer",
+        namespace="",
+        package="ros_phoenix",
+        executable="phoenix_container",
+        parameters=[{"interface": "can0"}],
+        composable_node_descriptions=[
+            ComposableNode(
+                package="ros_phoenix",
+                plugin="ros_phoenix::TalonSRX",
+                name="drill",
+                parameters=[{"id": 15}],
+            ),
+            ComposableNode(
+                package="ros_phoenix",
+                plugin="ros_phoenix::TalonSRX",
+                name="elevator",
+                parameters=[{"id": 14}],
+            ),
+        ],
+        output="screen",
+    )
+
+    return launch.LaunchDescription([container])

src/HW-Devices/ros_roboclaw/ros_roboclaw/drill_roboclaw_node.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+import rclpy
+from .roboclaw_library import Roboclaw
+from rclpy.node import Node
+from std_msgs.msg import Float32
+
+
+class RoboClaw(Node):
+    def __init__(self):
+        super().__init__("drill_roboclaw_node")
+        self.get_logger().info("Drill RoboClaw node has been started.")
+        self.declare_parameter("port", "/dev/ttyACM1")
+        self.declare_parameter("baud_rate", 115200)
+        port = self.get_parameter("port").get_parameter_value().string_value
+        baud_rate = self.get_parameter("baud_rate").get_parameter_value().integer_value
+        self.controller = Roboclaw(port, baud_rate)
+        self.address = 0x80  # Default address for the RoboClaw
+        if not self.controller.Open():
+            self.get_logger().error(f"Failed to connect to RoboClaw on {port}.")
+            raise RuntimeError("Could not open serial connection to RoboClaw.")
+        self.get_logger().info(f"Connected to RoboClaw on {port} at {baud_rate} baud.")
+        self.cmd_sub = self.create_subscription(
+            Float32, "drill/set", self.cmd_callback, 10
+        )
+        (rc, version) = self.controller.ReadVersion(self.address)
+        if rc:
+            self.get_logger().info(f"Version: {version}")
+        else:
+            self.get_logger().error("Failed to read version from RoboClaw.")
+        self.telemetry_timer = self.create_timer(10.0, self.telemetry_callback)
+        self.watchdog_flag = False
+        self.log_flag = True
+        self.watchdog_timer = self.create_timer(0.5, self.watchdog_callback)
+
+    def telemetry_callback(self):
+        (okE, err) = self.controller.ReadError(self.address)
+        if okE:
+            if err:
+                self.get_logger().error(f"Error code: {err}")
+        else:
+            self.get_logger().error("Failed to read error code from RoboClaw.")
+
+    def watchdog_callback(self):
+        if self.watchdog_flag:
+            self.watchdog_flag = False
+            self.log_flag = True
+            return
+        if self.log_flag:
+            self.get_logger().warn(
+                "Watchdog timeout: no command received in the last 0.5 seconds. Stopping motor."
+            )
+        self.log_flag = False
+        if not self.controller.DutyM1(self.address, 0):
+            self.get_logger().error("Failed to send watchdog command to RoboClaw.")
+
+    def cmd_callback(self, msg):
+        duty_cycle = msg.data
+        if duty_cycle < -1.0 or duty_cycle > 1.0:
+            self.get_logger().error(
+                f"Received invalid duty cycle {duty_cycle}. Must be between -1.0 and 1.0."
+            )
+            return
+        duty_cycle_int16 = int(
+            duty_cycle * 32767
+        )  # Scale from -1.0 to 1.0 range to -32767 to 32767
+        if not self.controller.DutyM1(self.address, duty_cycle_int16):
+            self.get_logger().error(
+                f"Failed to set duty cycle {duty_cycle_int16} on RoboClaw."
+            )
+        self.watchdog_flag = True
+
+
+def main(args=None):
+    """
+    Initializes the ROS 2 python library, starts this node, and enters the
+    ROS 2 spin loop to process incoming messages and trigger callbacks.
+    """
+    rclpy.init(args=args)
+    node = RoboClaw()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/HW-Devices/ros_roboclaw/ros_roboclaw/elevator_roboclaw_node.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+import rclpy
+from .roboclaw_library import Roboclaw
+from rclpy.node import Node
+from std_msgs.msg import Float32
+
+
+class RoboClaw(Node):
+    def __init__(self):
+        super().__init__("elevator_roboclaw_node")
+        self.get_logger().info("Elevator RoboClaw node has been started.")
+        self.declare_parameter("port", "/dev/ttyACM0")
+        self.declare_parameter("baud_rate", 115200)
+        port = self.get_parameter("port").get_parameter_value().string_value
+        baud_rate = self.get_parameter("baud_rate").get_parameter_value().integer_value
+        self.controller = Roboclaw(port, baud_rate)
+        self.address = 0x80  # Default address for the RoboClaw
+        if not self.controller.Open():
+            self.get_logger().error(f"Failed to connect to RoboClaw on {port}.")
+            raise RuntimeError("Could not open serial connection to RoboClaw.")
+        self.get_logger().info(f"Connected to RoboClaw on {port} at {baud_rate} baud.")
+        self.cmd_sub = self.create_subscription(
+            Float32, "elevator/set", self.cmd_callback, 10
+        )
+        (rc, version) = self.controller.ReadVersion(self.address)
+        if rc:
+            self.get_logger().info(f"Version: {version}")
+        else:
+            self.get_logger().error("Failed to read version from RoboClaw.")
+        self.telemetry_timer = self.create_timer(10.0, self.telemetry_callback)
+        self.watchdog_flag = False
+        self.log_flag = True
+        self.watchdog_timer = self.create_timer(0.5, self.watchdog_callback)
+
+    def telemetry_callback(self):
+        (okE, err) = self.controller.ReadError(self.address)
+        if okE:
+            if err:
+                self.get_logger().error(f"Error code: {err}")
+        else:
+            self.get_logger().error("Failed to read error code from RoboClaw.")
+
+    def watchdog_callback(self):
+        if self.watchdog_flag:
+            self.watchdog_flag = False
+            self.log_flag = True
+            return
+        if self.log_flag:
+            self.get_logger().warn(
+                "Watchdog timeout: no command received in the last 0.5 seconds. Stopping motor."
+            )
+        self.log_flag = False
+        if not self.controller.DutyM1(self.address, 0):
+            self.get_logger().error("Failed to send watchdog command to RoboClaw.")
+
+    def cmd_callback(self, msg):
+        duty_cycle = msg.data
+        if duty_cycle < -1.0 or duty_cycle > 1.0:
+            self.get_logger().error(
+                f"Received invalid duty cycle {duty_cycle}. Must be between -1.0 and 1.0."
+            )
+            return
+        duty_cycle_int16 = int(
+            duty_cycle * 32767
+        )  # Scale from -1.0 to 1.0 range to -32767 to 32767
+        if not self.controller.DutyM1(self.address, duty_cycle_int16):
+            self.get_logger().error(
+                f"Failed to set duty cycle {duty_cycle_int16} on RoboClaw."
+            )
+        self.watchdog_flag = True
+
+
+def main(args=None):
+    """
+    Initializes the ROS 2 python library, starts this node, and enters the
+    ROS 2 spin loop to process incoming messages and trigger callbacks.
+    """
+    rclpy.init(args=args)
+    node = RoboClaw()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/HW-Devices/ros_roboclaw/setup.py

@@ -18,6 +18,10 @@
     license="Apache-2.0",
     tests_require=["pytest"],
     entry_points={
-        "console_scripts": ["roboclaw_node = ros_roboclaw.roboclaw_node:main"],
+        "console_scripts": [
+            "roboclaw_node = ros_roboclaw.roboclaw_node:main",
+            "drill_roboclaw_node = ros_roboclaw.drill_roboclaw_node:main",
+            "elevator_roboclaw_node = ros_roboclaw.elevator_roboclaw_node:main",
+        ],
     },
 )

src/Teleop-Control/joystick_control/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ros_phoenix REQUIRED)
 find_package(interfaces REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
 
 set(CMAKE_CXX_STANDARD 14)
 
@@ -28,6 +29,10 @@ add_executable(arm
   src/arm.cpp
 )
 
+add_executable(science
+  src/science.cpp
+)
+
 ament_target_dependencies(drive
   rclcpp
   geometry_msgs
@@ -48,9 +53,18 @@ ament_target_dependencies(arm
   std_srvs
 )
 
+ament_target_dependencies(science
+  rclcpp
+  joy
+  ros_phoenix
+  interfaces
+  std_msgs
+)
+
 install(TARGETS
   drive
   arm
+  science
   DESTINATION lib/${PROJECT_NAME})
 
 # Install header files

src/Teleop-Control/joystick_control/config/pxn.yaml

@@ -22,4 +22,58 @@ arm_teleop_node:
     max_wrist_speed: 1.0
     max_act1_speed: 1.0
     max_act2_speed: 1.0
-    max_elbow_yaw_speed: 1.0
+    max_elbow_yaw_speed: 1.0
+science_teleop_node:
+  ros__parameters:
+    drill_button: 0
+    drill_elevation_axis: 1
+    servo_control_axis: 5
+    servo1:
+      name: "pump1"
+      button: 12
+      max_value: 65535
+      default_value: 0
+      min_value: 0
+    servo2:
+      name: "pump2"
+      button: 13
+      max_value: 65535
+      default_value: 0
+      min_value: 0
+    servo3:
+      name: "pump3"
+      button: 14
+      max_value: 65535
+      default_value: 0
+      min_value: 0
+    servo4:
+      name: "pump4"
+      button: 15
+      max_value: 65535
+      default_value: 0
+      min_value: 0
+    servo5:
+      name: "sample_servo"
+      button: 16
+      max_value: 6881
+      default_value: 2949
+      min_value: 2949
+    servo6:
+      name: "ion_exchange_servo"
+      button: 17
+      max_value: 65535
+      default_value: 0
+      min_value: 0
+    servo7:
+      name: "servo7"
+      button: 2
+      max_value: 65535
+      default_value: 0
+      min_value: 0
+    servo8:
+      name: "servo8"
+      button: 1
+      max_value: 65535
+      default_value: 0
+      min_value: 0
+

src/Teleop-Control/joystick_control/include/drive.hpp

@@ -11,8 +11,8 @@ class drive : public rclcpp::Node {
   void drive_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
 
 private:
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub;
-  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
+  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
 
   void declare_parameters();
   void load_parameters();

src/Teleop-Control/joystick_control/include/science.hpp

@@ -0,0 +1,41 @@
+#ifndef SCIENCE_HPP
+#define SCIENCE_HPP
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include "std_msgs/msg/u_int32.hpp"
+
+class science : public rclcpp::Node {
+public:
+  science();
+  void science_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
+
+private:
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr drill_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr elevator_pub_;
+  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
+
+  std::vector<int> servo_values_;
+  std::vector<rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr> servo_pubs_;
+
+  void update_servo_value(int servo_index,
+                          const sensor_msgs::msg::Joy &joystickMsg);
+  void declare_parameters();
+  void load_parameters();
+
+  // Parameters
+  int kDrillButton;
+  int kDrillElevationAxis;
+  int kServoControlAxis;
+
+  std::vector<std::string> kServoNames;
+  std::vector<int> kServoButtons;
+  std::vector<int> kServoMaxValues;
+  std::vector<int> kServoMinValues;
+  std::vector<int> kServoDefaultValues;
+
+  bool initialized_;
+};
+
+#endif // SCIENCE_HPP