Maanas Commit ROs2

mil_common/drivers/mil_pneumatic_actuator/CMakeLists.txt

@@ -1,18 +1,17 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(mil_pneumatic_actuator)
 
-find_package(catkin REQUIRED COMPONENTS
-  message_generation
-  rospy
-)
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-catkin_python_setup()
+# Set C++ standard
+set(CMAKE_CXX_STANDARD 14)
 
-add_service_files(
-  FILES
-  SetValve.srv
+# Generate ROS interfaces
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/SetValve.srv"
 )
 
-generate_messages()
-
-catkin_package(CATKIN_DEPENDS message_runtime)
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

mil_common/drivers/mil_pneumatic_actuator/launch/example.launch

@@ -1,52 +1,44 @@
-<launch>
-    <param name="/is_simulation" value="True" />
-    <node pkg="mil_pneumatic_actuator" type="pneumatic_actuator_node" name="actuator_driver" output="screen">
-        <param name="port" value="/dev/serial/by-id/usb-MIL_Data_Merge_Board_Ports_5_to_8_DMBP58-if00-port0"/>
-        <rosparam param="actuators">
-          # Stores information about each actuator.
-          # Each actuator can be up to 2 physical valves (such as an extend/retract for a piston)
-          # Actuators can be of type 'set' (open / close atomically) or 'pulse' (open for a short time, then close)
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import SetParameter
 
-          # Simplest configuration, sets up a 'set' actuator where true opens id 7 and false closes id 7
-          my_actuator: 7
-
-          # Example of a pulse actuator, will open 1 for 1 second then close
-          torpedo1:
-            type: 'pulse'
-            ports:
-              open_port:
-                id: 1
-                default: 0
-              close_port:
-                id: -1
-                default: 0
-            pulse_time: 1
-
-          # Example of a pulse actuator with 2 valves. When pulse, 3 opens and 4 closes, then both switch after 1 second
-          dropper:
-            type: 'pulse'
-            ports:
-              open_port:
-                # Drops
-                id: 3
-                default: 0
-              close_port:
-                # Reloads
-                id: 4
-                default: 1
-            pulse_time: 1
-
-          # Example of a 'set 'actuator with 2 valves. When set true, 6 closes and 5 opens. When false, 6 opens and 5 closes.
-          gripper:
-            type: 'set'
-            ports:
-              open_port:
-                id: 6
-                default: 1
-              close_port:
-                id: 5
-                default: 0
-            pulse_time: 1
-        </rosparam>
-    </node>
-</launch>
+def generate_launch_description():
+    return LaunchDescription([
+        SetParameter(name='/is_simulation', value='True'),
+        Node(
+            package='mil_pneumatic_actuator',
+            executable='pneumatic_actuator_node',
+            name='actuator_driver',
+            output='screen',
+            parameters=[{
+                'port': '/dev/serial/by-id/usb-MIL_Data_Merge_Board_Ports_5_to_8_DMBP58-if00-port0',
+                'actuators': {
+                    'my_actuator': 7,
+                    'torpedo1': {
+                        'type': 'pulse',
+                        'ports': {
+                            'open_port': {'id': 1, 'default': 0},
+                            'close_port': {'id': -1, 'default': 0}
+                        },
+                        'pulse_time': 1
+                    },
+                    'dropper': {
+                        'type': 'pulse',
+                        'ports': {
+                            'open_port': {'id': 3, 'default': 0},
+                            'close_port': {'id': 4, 'default': 1}
+                        },
+                        'pulse_time': 1
+                    },
+                    'gripper': {
+                        'type': 'set',
+                        'ports': {
+                            'open_port': {'id': 6, 'default': 1},
+                            'close_port': {'id': 5, 'default': 0}
+                        },
+                        'pulse_time': 1
+                    }
+                }
+            }]
+        )
+    ])

mil_common/drivers/mil_pneumatic_actuator/mil_pneumatic_actuator/board.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-import threading
+import threading #ros2
 from typing import Optional
 
 import serial
@@ -10,6 +10,8 @@
 
 lock = threading.Lock()
 
+import rclpy
+from rclpy.node import Node
 
 class PnuematicActuatorDriverError(Exception):
     """
@@ -65,7 +67,7 @@ def __init__(self):
         super().__init__(message)
 
 
-class PnuematicActuatorDriver:
+class PnuematicActuatorDriver(Node):
     """
     Allows high level ROS code to interface with Daniel's pneumatics board.
 
@@ -77,7 +79,6 @@ class PnuematicActuatorDriver:
     design documentation.
     """
 
-    # TODO: Add a function to try and reconnect to the serial port if we lose connection.
 
     def __init__(self, port: str, baud: int = 9600, simulated: bool = False):
         """
@@ -87,6 +88,8 @@ def __init__(self, port: str, baud: int = 9600, simulated: bool = False):
             simulated (bool): Whether to use a simulated actuator board class
                 or an interface to the physical board.
         """
+        super().__init__('pneumatic_actuator_driver')
+
         if simulated:
             self.ser = SimulatedPnuematicActuatorBoard()
         else:
@@ -205,6 +208,17 @@ def get_port(self, port: int) -> int:
         """
         Reads the data at a specific port.
 
+        Args:
+            port (int): The port to read from.
+
+        Raises:
+            PnuematicActuatorDriverResponseError: The expected response from the board
+                was not received.
+            PnuematicActuatorDriverChecksumError: The checksum expected and the checksum
+                received were not the same def get_port(self, port: int) -> int:
+        """
+        Reads the data at a specific port.
+
         Args:
             port (int): The port to read from.
 
@@ -236,3 +250,19 @@ def ping(self) -> int:
             int: The response from the board.
         """
         return self._send_request(Constants.PING_REQUEST, Constants.PING_RESPONSE)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = PnuematicActuatorDriver(port='/dev/ttyUSB0', baud=9600, simulated=False)
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

mil_common/drivers/mil_pneumatic_actuator/mil_pneumatic_actuator/simulated_board.py

@@ -1,17 +1,19 @@
 #!/usr/bin/env python3
-import rospy
+import rclpy
+from rclpy.node import Node
 from mil_misc_tools.serial_tools import SimulatedSerial
 
 from .constants import Constants
 
 
-class SimulatedPnuematicActuatorBoard(SimulatedSerial):
+class SimulatedPnuematicActuatorBoard(SimulatedSerial, Node):
     """
     A simulation of the pneumatic actuator board's serial protocol
     """
 
     def __init__(self, *args, **kwargs):
-        super().__init__()
+        Node.__init__(self, 'simulated_pneumatic_actuator_board')
+        SimulatedSerial.__init__(self)
 
     def write(self, data: bytes):
         """
@@ -20,16 +22,16 @@ def write(self, data: bytes):
         request = Constants.deserialize_packet(data)
         request = request[0]
         if request == Constants.PING_REQUEST:
-            # rospy.loginfo("Ping received")
+            # self.get_logger().info("Ping received")
             byte = Constants.PING_RESPONSE
-        elif request > 0x20 and request < 0x30:
-            rospy.loginfo(f"Open port {request - 0x20}")
+        elif 0x20 < request < 0x30:
+            self.get_logger().info(f"Open port {request - 0x20}")
             byte = Constants.OPEN_RESPONSE
-        elif request > 0x30 and request < 0x40:
-            rospy.loginfo(f"Close port {request - 0x30}")
+        elif 0x30 < request < 0x40:
+            self.get_logger().info(f"Close port {request - 0x30}")
             byte = Constants.CLOSE_RESPONSE
         else:
-            rospy.loginfo("Default")
+            self.get_logger().info("Default")
             byte = 0x00
         self.buffer += Constants.serialize_packet(byte)
         return len(data)