Added old controller code

README.md

@@ -1,11 +1,4 @@
-# (At least) teach the robot to drive remotely
+# Temporary FT24 control repo
-
-## Start Script
-The whole system (modules for robot control, remote control, lidar drivers and ROS interface, as well as SLAM & navigation) can be launched by executing `start.bash`, which is located at the root of the project directory.
-
-`start.bash` creates a tmux session with a separate window for each module, plus an additional window running `htop` for monitoring performance. In each window, the appropriate launch file for the respective module is executed.
-
-The benefits of this modular setup lie in separation of the outputs of the different modules, enabling easier troubleshooting. Additionally, this allows terminating or restarting modules independently. For example, if the controller loses connection, this would be evident in the remote control window and the responsible module could be restarted without bringing the entire system down.
 
 ## Robot Control
 The robot control is implemented in the `ft24_control` package and can be launched with \
@@ -15,11 +8,8 @@ This launch file brings up the `ros2_control`-based nodes that serve as the inte
 ## Remote Control
 The launch file `ros2 launch src/dcaitirobot/launch/remote_control_launch.py` launches the nodes required to interface the PS3 controller and publish its commands as `Twist` messages in ROS.
 
-## Lidar
+## Simulation Start
-`ros2 launch velodyne velodyne-all-nodes-VLP16-launch.py` is provided by the `velodyne` package and brings up the velodyne driver as well as nodes required to publish ROS2 messages of type `PointCloud2` and `LaserScan`.
+For the Gazebo simulation, run:
-
+```
-**Note**\
+ros2 launch ft24_control launch_sim.py
-In order to bring the lidar to the same subnet as the Jetson within the provided router, we modified the lidar's IP from `192.168.4.200` to `192.168.1.200`.
+```
-
-## SLAM and Nav2
-`ros2 launch src/dcaiti_navigation/launch/start_nav2.py` will start `slam_toolbox` (plus several nodes/transforms required for `slam_toolbox` to work with our lidar output), multiple `Nav2` packages as well `explore_lite` which provides autonomous exploration support for `Nav2`.

src/ft24_control/CMakeLists.txt

@@ -42,6 +42,8 @@ ament_python_install_package(${PROJECT_NAME})
 # Install Python executables
 install(PROGRAMS
   src/imu_covariance_adder.py
+  src/joy.py
+  src/twist.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/ft24_control/src/joy.py

@@ -0,0 +1,276 @@
+from math import atan2, degrees, sqrt
+from struct import Struct
+from time import sleep
+from typing import List
+
+import rclpy
+from rclpy.context import Context
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from sensor_msgs.msg import Joy
+
+
+class JSEvent(object):
+    """A joystick even class (struct)
+
+        struct js_event_t
+        {
+            uint32_t time;
+            int16_t value;
+            uint8_t type;
+            uint8_t id;
+        };
+
+    Takes in raw bytes from a file.read(JS_EVENT_SIZE) call.
+
+    Also defines AXIS and BUTTON ids
+    """
+
+    JS_EVENT_SIZE = 8  # 64 bits is 8 bytes
+    EVENT_BUTTON = 0x01
+    EVENT_AXIS = 0x02
+    EVENT_INIT = 0x80
+    MAX_AXIS_COUNT = 27
+    MAX_BUTTON_COUNT = 18
+    AXIS_COUNT = 26
+    BUTTON_COUNT = 17
+
+    AXIS_LEFT_STICK_HORIZONTAL = 0
+    AXIS_LEFT_STICK_VERTICAL = 1
+    AXIS_RIGHT_STICK_HORIZONTAL = 2
+    AXIS_RIGHT_STICK_VERTICAL = 3
+    AXIS_DPAD_UP = 8
+    AXIS_DPAD_RIGHT = 9
+    AXIS_DPAD_DOWN = 10
+    AXIS_DPAD_LEFT = 11  # who knows what the left value should actually be...
+    AXIS_LEFT_TRIGGER = 12
+    AXIS_RIGHT_TRIGGER = 13
+    AXIS_LEFT_BUMPER = 14
+    AXIS_RIGHT_BUMPER = 15
+    AXIS_TRIANGLE = 16
+    AXIS_CIRCLE = 17
+    AXIS_X = 18
+    AXIS_SQUARE = 19
+    AXIS_ACCEL_X = 23  # note: left is positive, right is negative
+    AXIS_ACCEL_Y = 24  # note: back is positive, forward is negative
+    AXIS_ACCEL_Z = 25  # note: can't tell what sign is what
+
+    BUTTON_SELECT = 0
+    BUTTON_LEFT_JOYSTICK = 1
+    BUTTON_RIGHT_JOYSTICK = 2
+    BUTTON_START = 3
+    BUTTON_DPAD_UP = 4
+    BUTTON_DPAD_RIGHT = 5
+    BUTTON_DPAD_DOWN = 6
+    BUTTON_DPAD_LEFT = 7
+    BUTTON_LEFT_TRIGGER = 8
+    BUTTON_RIGHT_TRIGGER = 9
+    BUTTON_LEFT_BUMPER = 10
+    BUTTON_RIGHT_BUMPER = 11
+    BUTTON_TRIANGLE = 12
+    BUTTON_CIRCLE = 13
+    BUTTON_X = 14
+    BUTTON_SQUARE = 15
+    BUTTON_PS3 = 16
+
+    def __init__(self, event_struct):
+        # c.f. https://docs.python.org/3/library/struct.html#format-characters
+        s = Struct("< I h B B")
+        self.time, self.value, self.type, self.id = s.unpack(event_struct)
+        # also c.f. Struct.pack()
+
+        # ignore non-input events
+        self.type = self.type & ~self.EVENT_INIT
+
+    def __repr__(self):
+        struct = "struct js_event_t\n{"
+        # struct += '\n\tuint32_t time : {}'.format(self.time)
+        struct += "\n\tint16_t value : {}".format(self.value)
+        struct += "\n\tuint8_t type  : {}".format(hex(self.type))
+        struct += "\n\tuint8_t id    : {}".format(self.id)
+        struct += "\n};\n"
+        return struct
+
+
+class Joystick(object):
+    def __init__(self, js_file="/dev/input/js1"):
+        # note names beginning with `__` are enforced as private
+        self.file = open(js_file, "rb")
+        # since we're using ID's as indices, account for 0 based indices
+        self.__axis_values = [0] * (JSEvent.MAX_AXIS_COUNT + 1)
+        # since we're using ID's as indices, account for 0 based indices
+        self.__button_values = [0] * (JSEvent.MAX_BUTTON_COUNT + 1)
+
+        self.update_id = -1
+        self.AxisUpdate = False
+        self.ButtonUpdate = False
+        self.event = None
+
+    def __del__(self):
+        self.file.close()
+
+    def __enter__(self):
+        return self
+
+    # __enter__ and __exit__ allow the context manager syntax
+    # with Joystick() as js:
+    # ...
+    def __exit__(self, exc_type, exc_value, traceback):
+        self.file.close()
+
+    def Update(self):
+        self.event = JSEvent(self.file.read(JSEvent.JS_EVENT_SIZE))
+
+        if self.event.type == JSEvent.EVENT_AXIS:
+            self.update_id = self.event.id
+            self.AxisUpdate = True
+            self.ButtonUpdate = False
+            self.__axis_values[self.event.id] = self.event.value
+
+        if self.event.type == JSEvent.EVENT_BUTTON:
+            self.update_id = self.event.id
+            self.AxisUpdate = False
+            self.ButtonUpdate = True
+            self.__button_values[self.event.id] = self.event.value
+
+    def getButtonState(self, button_id):
+        return self.__button_values[button_id]
+
+    def getAxisState(self, axis_id):
+        return self.__axis_values[axis_id]
+
+
+class JoystickController(Joystick):
+    """An implementation specific controller that inherits a basic Joystick. Adds the capability to
+    check for specific updates that you care about and then do something based on that.
+    """
+
+    def __init__(self, js_file="/dev/input/js0"):
+        Joystick.__init__(self, js_file)
+        # direction gives the direction of travel. Defined in terms of r and theta
+        self.direction_vector = (0, 0)
+        # rotation tells the robot to rotate x degrees
+        self.rotation_vector = (0, 0)
+
+    def hasUpdate(self):
+        return self.ButtonUpdate or self.AxisUpdate
+
+    def performUpdates(self):
+        care_about_buttons = [
+            JSEvent.BUTTON_SELECT,
+            JSEvent.BUTTON_LEFT_JOYSTICK,
+            JSEvent.BUTTON_RIGHT_JOYSTICK,
+            JSEvent.BUTTON_START,
+            JSEvent.BUTTON_DPAD_UP,
+            JSEvent.BUTTON_DPAD_RIGHT,
+            JSEvent.BUTTON_DPAD_DOWN,
+            JSEvent.BUTTON_DPAD_LEFT,
+            JSEvent.BUTTON_LEFT_TRIGGER,
+            JSEvent.BUTTON_RIGHT_TRIGGER,
+            JSEvent.BUTTON_LEFT_BUMPER,
+            JSEvent.BUTTON_RIGHT_BUMPER,
+            JSEvent.BUTTON_TRIANGLE,
+            JSEvent.BUTTON_CIRCLE,
+            JSEvent.BUTTON_X,
+            JSEvent.BUTTON_SQUARE,
+            JSEvent.BUTTON_PS3,
+        ]
+        care_about_axes = [
+            JSEvent.AXIS_LEFT_STICK_HORIZONTAL,
+            JSEvent.AXIS_LEFT_STICK_VERTICAL,
+            JSEvent.AXIS_RIGHT_STICK_HORIZONTAL,
+            JSEvent.AXIS_RIGHT_STICK_VERTICAL,
+            JSEvent.AXIS_DPAD_UP,
+            JSEvent.AXIS_DPAD_LEFT,
+            JSEvent.AXIS_DPAD_RIGHT,
+            JSEvent.AXIS_DPAD_DOWN,
+            JSEvent.AXIS_LEFT_TRIGGER,
+            JSEvent.AXIS_RIGHT_TRIGGER,
+            JSEvent.AXIS_LEFT_BUMPER,
+            JSEvent.AXIS_RIGHT_BUMPER,
+            JSEvent.AXIS_TRIANGLE,
+            JSEvent.AXIS_CIRCLE,
+            JSEvent.AXIS_X,
+            JSEvent.AXIS_SQUARE,
+            JSEvent.AXIS_ACCEL_X,
+            JSEvent.AXIS_ACCEL_Y,
+            JSEvent.AXIS_ACCEL_Z,
+        ]
+
+        if self.ButtonUpdate and self.update_id in care_about_buttons:
+            self.__button_update()
+
+        if self.AxisUpdate and self.update_id in care_about_axes:
+            self.__axis_update()
+
+    def __button_update(self):
+        pass
+
+    def __axis_update(self):
+        pass
+   
+class JoyNode(Node):
+    def __init__(
+        self,
+        node_name: str,
+        *,
+        context: Context = None,
+        cli_args: List[str] = None,
+        namespace: str = None,
+        use_global_arguments: bool = True,
+        enable_rosout: bool = True,
+        start_parameter_services: bool = True,
+        parameter_overrides: List[Parameter] = None,
+        allow_undeclared_parameters: bool = False,
+        automatically_declare_parameters_from_overrides: bool = False,
+    ) -> None:
+        super().__init__(
+            node_name,
+            context=context,
+            cli_args=cli_args,
+            namespace=namespace,
+            use_global_arguments=use_global_arguments,
+            enable_rosout=enable_rosout,
+            start_parameter_services=start_parameter_services,
+            parameter_overrides=parameter_overrides,
+            allow_undeclared_parameters=allow_undeclared_parameters,
+            automatically_declare_parameters_from_overrides=automatically_declare_parameters_from_overrides,
+        )
+
+        self.pub = self.create_publisher(Joy, "/joy", 1)
+
+    def run(self):
+        msg = Joy()
+        msg.axes = [0.0, 0.0]
+        msg.buttons = [0]
+        with JoystickController("/dev/input/js0") as controller:
+            while rclpy.ok():
+                controller.Update()
+                sleep(0.0001)
+
+                if controller.hasUpdate():
+                    hor = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
+                    ver = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)
+
+                    button_change_mode = controller.getButtonState(JSEvent.BUTTON_CIRCLE) # it says circle but it actually is dpad up
+                    self.get_logger().info(str(button_change_mode))
+
+                    hor /= 2**15
+                    ver /= 2**15
+
+                    msg.axes[JSEvent.AXIS_LEFT_STICK_HORIZONTAL] = hor
+                    msg.axes[JSEvent.AXIS_LEFT_STICK_VERTICAL] = ver
+                    msg.buttons[0] = button_change_mode
+
+                    self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = JoyNode("test")
+    node.run()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/ft24_control/src/twist.py

@@ -0,0 +1,106 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from sensor_msgs.msg import Joy
+
+import numpy as np
+
+SCALE_LINEAR_VELOCITY = 0.2
+SCALE_ANGULAR_VELOCITY = 0.7
+PUBLISH_FREQUENCY_HZ = 30
+
+SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
+
+
+class TwistPublisher(Node):
+    """
+    This node serves as a bridge between the Joy messages published from the PS3 controller
+    interface in joy.py and the Twist messages expected by ros_control.
+    """
+    def __init__(
+        self,
+        smooth_weights=np.ones(5),
+        linear_scale=1.0,
+        angular_scale=1.0,
+        publish_frequency_hz=30,
+    ):
+        super().__init__("twist_publisher")
+
+        self.mode = "manual"
+
+        self.linear_window = np.zeros(smooth_weights.shape[0])
+        self.angular_window = np.zeros(smooth_weights.shape[0])
+        self.linear_recent = 0.0
+        self.angular_recent = 0.0
+
+        self.linear_out = 0.0
+        self.angular_out = 0.0
+
+        self.weights = smooth_weights / smooth_weights.sum()
+        self.window_size = smooth_weights.shape[0]
+
+        self.linear_scale = linear_scale
+        self.angular_scale = angular_scale
+
+        self.publisher_2 = self.create_publisher(Twist, "/cmd_vel_joystick", 1)
+
+        self.create_subscription(Joy, "/joy", self.joy_callback, 1)
+        self.timer = self.create_timer(1 / publish_frequency_hz, self.timer_callback)
+
+    def joy_callback(self, joy_message):
+        x, y = joy_message.axes[:2]
+        self.linear_recent = -y * SCALE_LINEAR_VELOCITY
+        self.angular_recent = -x * SCALE_ANGULAR_VELOCITY
+
+        if joy_message.buttons[0]:
+            self.mode = "manual" if self.mode == "auto" else "auto"
+            self.get_logger().info(f"Switching to {self.mode!r} mode")
+
+  
+    def update_smooth_window(self):
+        self.linear_window = np.append(self.linear_window, self.linear_recent)[
+            -self.window_size :
+        ]
+        self.angular_window = np.append(self.angular_window, self.angular_recent)[
+            -self.window_size :
+        ]
+
+    def calc_smooth_speed(self):
+        self.linear_out = np.average(self.linear_window, weights=self.weights)
+        self.angular_out = np.average(self.angular_window, weights=self.weights)
+
+    def timer_callback(self):
+        
+        if self.mode != "manual":
+            return
+
+        msg = Twist()
+
+        self.update_smooth_window()
+        self.calc_smooth_speed()
+
+        msg.linear.x = self.linear_out
+        msg.angular.z = self.angular_out
+        self.publisher_2.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = TwistPublisher(
+        SMOOTH_WEIGHTS,
+        SCALE_LINEAR_VELOCITY,
+        SCALE_ANGULAR_VELOCITY,
+        PUBLISH_FREQUENCY_HZ,
+    )
+
+    rclpy.spin(minimal_publisher)
+
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()