diff --git a/.gitignore b/.gitignore
index fabc1cf..24c5d9c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -21,3 +21,6 @@ Zone.Identifier
deps/songs
autonav_ws/src/scr/include/scr/websocketpp/**/*:Zone.Identifier
+
+# python
+**/venv
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_launch/launch/manual24.xml b/autonav_ws/src/autonav_launch/launch/manual24.xml
new file mode 100644
index 0000000..a8b5c45
--- /dev/null
+++ b/autonav_ws/src/autonav_launch/launch/manual24.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_launch/launch/manual24sim.xml b/autonav_ws/src/autonav_launch/launch/manual24sim.xml
new file mode 100644
index 0000000..73f17ac
--- /dev/null
+++ b/autonav_ws/src/autonav_launch/launch/manual24sim.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_launch/launch/manual25.xml b/autonav_ws/src/autonav_launch/launch/manual25.xml
new file mode 100644
index 0000000..8cfab62
--- /dev/null
+++ b/autonav_ws/src/autonav_launch/launch/manual25.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_launch/launch/manual25sim.xml b/autonav_ws/src/autonav_launch/launch/manual25sim.xml
new file mode 100644
index 0000000..416cedf
--- /dev/null
+++ b/autonav_ws/src/autonav_launch/launch/manual25sim.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_manual/CMakeLists.txt b/autonav_ws/src/autonav_manual/CMakeLists.txt
new file mode 100644
index 0000000..857a221
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/CMakeLists.txt
@@ -0,0 +1,73 @@
+cmake_minimum_required(VERSION 3.8)
+
+project(autonav_manual)
+
+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(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(autonav_msgs REQUIRED)
+find_package(autonav_shared REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package( REQUIRED)
+
+# msgs and srvs
+#rosidl_generate_interfaces(${PROJECT_NAME}
+ # add message types here
+ #"path_to_message/message.msg"
+#)
+
+# C++
+
+# Inlcude Cpp "include" directory
+include_directories(include)
+
+# Create Cpp executables
+#add_executable(executable_name path_to_executable/executable.cpp)
+#ament_target_dependencies(executable_name rclcpp other_dependencies)
+
+# Install Cpp executables
+install(TARGETS
+ # install executables by name
+ # executable_name
+ DESTINATION lib/${PROJECT_NAME}
+ )
+
+# Python
+
+ # Use only if not using rosidl_generate_interfaces
+ # Install Python modules
+ #ament_python_install_package(${PROJECT_NAME})
+
+ # Install Python programs
+ install(PROGRAMS
+ # add programs in format:
+ #/path_to_program/program.py
+ src/controller_input.py
+ src/controller_listener.py
+ src/manual_24.py
+ src/manual_25.py
+ src/motormessage_listener.py
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+
+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()
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_manual/LICENSE b/autonav_ws/src/autonav_manual/LICENSE
new file mode 100644
index 0000000..30e8e2e
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/LICENSE
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
diff --git a/autonav_ws/src/autonav_manual/package.xml b/autonav_ws/src/autonav_manual/package.xml
new file mode 100644
index 0000000..891f7df
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/package.xml
@@ -0,0 +1,34 @@
+
+
+
+ autonav_manual
+ 2025.0.1
+ Contains nodes for reading, using, and debugging controller input
+ tony
+ MIT
+
+ ament_cmake
+ ament_cmake_python
+
+ rclcpp
+ rclpy
+ autonav_msgs
+ autonav_shared
+
+ rosidl_default_generators
+
+ rosidl_default_runtime
+ evdev
+ std_msgs
+ time
+ ros2launch
+
+ rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_manual/src/controller_input.py b/autonav_ws/src/autonav_manual/src/controller_input.py
new file mode 100644
index 0000000..f9e3d06
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/src/controller_input.py
@@ -0,0 +1,240 @@
+#!/usr/bin/env python3
+
+import rclpy
+import evdev
+from evdev import ecodes
+import time
+from autonav_shared.node import Node
+from autonav_shared.types import LogLevel, DeviceState, SystemState
+from std_msgs.msg import String
+from autonav_msgs.msg import ControllerInput
+
+JOY_MIN = 0
+JOY_MAX = 65535
+
+TRIGGER_MIN = 0
+TRIGGER_MAX = 1023
+
+# install the bluez bluetooth drivers for ubuntu:
+# sudo apt install bluez
+# enable UserspaceHID=true in /etc/bluetooth/input.conf
+# robot ip: 192.168.1.79
+
+class ControllerInputNode(Node):
+ def __init__(self):
+ super().__init__('controller_input')
+
+
+ def init(self):
+ self.set_device_state(DeviceState.WARMING)
+
+ self.timer_period_s = 0.1
+ self.publisher = self.create_publisher(ControllerInput, '/autonav/controller_input', 10)
+
+ self.controller = self.get_controller()
+ self.stick_names = ["ABS_X", "ABS_Y", "ABS_Z", "ABS_RZ"]
+ self.dpad_names = ["ABS_HAT0Y", "ABS_HAT0X"]
+ self.button_names = ["BTN_NORTH", "BTN_EAST", "BTN_SOUTH", "BTN_WEST"]\
+
+ self.controller_state = {
+ "ABS_BRAKE": 0.0,
+ "ABS_GAS": 0.0,
+ "ABS_HAT0X": 0.0,
+ "ABS_HAT0Y": 0.0,
+ "ABS_RZ": 0.0,
+ "ABS_X": 0.0,
+ "ABS_Y": 0.0,
+ "ABS_Z": 0.0,
+ "BTN_EAST": 0.0,
+ "BTN_MODE": 0.0,
+ "BTN_NORTH": 0.0,
+ "BTN_SELECT": 0.0,
+ "BTN_START": 0.0,
+ "BTN_SOUTH": 0.0,
+ "BTN_TL": 0.0,
+ "BTN_TR": 0.0,
+ "BTN_WEST": 0.0,
+ "KEY_RECORD": 0.0,
+ "wildcard": 0.0
+ }
+
+ self.get_inputs_loop()
+
+
+ def get_controller(self):
+ devices = [evdev.InputDevice(path) for path in evdev.list_devices()]
+ controller = None
+
+ for device in devices:
+ if device.name == 'Xbox Wireless Controller':
+ controller = evdev.InputDevice(device.path)
+ self.get_logger().info(f"assigned controller: \nName: {device.name} \nPath: {device.path} \nBluetooth MAC address: {device.uniq}\n" +
+ f"\nController state will be published on update and every {self.timer_period_s} seconds")
+
+ if controller is not None:
+ self.set_device_state(DeviceState.OPERATING)
+
+ return controller
+
+
+ def normalize(self, input, output_start, output_end, input_start, input_end, deadzone_percent):
+ output = output_start + ((output_end - output_start) / (input_end - input_start)) * (input - input_start)
+
+ if abs(output) < (deadzone_percent/100.0):
+ return 0.0
+ else:
+ return output
+
+
+ def get_inputs_loop(self):
+ last_callback_time_s = time.time()
+ while True:
+ last_callback_time_s = self.clock_routine(last_callback_time_s)
+
+ try: # handle a disconnection
+ event = self.controller.read_one()
+
+ if event is None:
+ continue
+
+ if event.type in ecodes.bytype:
+ codename = ecodes.bytype[event.type][event.code]
+ else:
+ codename = "wildcard"
+
+ value = 0.0
+
+ if ecodes.EV[event.type] == "EV_SYN": # we don't care about these
+ continue
+
+ if ecodes.EV[event.type] == "EV_MSC": # we don't care about these
+ continue
+
+ if ecodes.EV[event.type] == "EV_ABS": # joysticks, dpad, or triggers
+ if codename in self.stick_names:
+ value = self.normalize(event.value, -1, 1, JOY_MIN, JOY_MAX, 10.0)
+ elif codename in self.dpad_names:
+ value = float(event.value)
+ else: #trigger
+ value = self.normalize(event.value, 0, 1, TRIGGER_MIN, TRIGGER_MAX, 10.0)
+
+ elif ecodes.EV[event.type] == "EV_KEY": # buttons
+ value = float(event.value)
+
+ matches = None
+
+ # codenames for the controller right thumb buttons are an array of possible names, match for one of them
+ if type(codename) == list:
+ matches = list(set(codename) & set(self.button_names))
+ if matches is not None:
+ button_name = self.xbox_swap_N_W(matches[0])
+ else:
+ button_name = "wildcard"
+ else:
+ button_name = codename
+
+ self.update_controller_state(button_name, value)
+
+ msg = self.construct_controller_state_message()
+
+ # self.get_logger().info(f"publishing controller state:\n{str(self.controller_state)}")
+ self.publisher.publish(msg)
+
+ except OSError as e: # first disconnect
+ #raise(e) # debug
+ last_callback_time_s = self.reconnect(last_callback_time_s)
+
+ except AttributeError as e: # after the controller InputDevice object is destroyed
+ #raise(e) # debug
+ last_callback_time_s = self.reconnect(last_callback_time_s)
+
+ else:
+ pass
+
+
+ def clock_routine(self, last_callback_time_s):
+ current_time_s = time.time()
+ time_delta_s = current_time_s - last_callback_time_s
+ if time_delta_s > self.timer_period_s:
+ self.controller_state_timer_callback()
+ last_callback_time_s = time.time()
+
+ return last_callback_time_s
+
+
+ def controller_state_timer_callback(self):
+ msg = self.construct_controller_state_message()
+ # print(f"publishing: {str(self.controller_state)}")
+ self.publisher.publish(msg)
+
+
+ def construct_controller_state_message(self):
+ message = ControllerInput()
+
+ message.btn_north = self.controller_state["BTN_NORTH"]
+ message.btn_east = self.controller_state["BTN_EAST"]
+ message.btn_south = self.controller_state["BTN_SOUTH"]
+ message.btn_west = self.controller_state["BTN_WEST"]
+ message.btn_start = self.controller_state["BTN_START"]
+ message.btn_select = self.controller_state["BTN_SELECT"]
+ message.btn_mode = self.controller_state["BTN_MODE"]
+ message.btn_tr = self.controller_state["BTN_TR"]
+ message.btn_tl = self.controller_state["BTN_TL"]
+ message.key_record = self.controller_state["KEY_RECORD"]
+ message.abs_hat0x = self.controller_state["ABS_HAT0X"]
+ message.abs_hat0y = self.controller_state["ABS_HAT0Y"]
+ message.abs_x = self.controller_state["ABS_X"]
+ message.abs_y = self.controller_state["ABS_Y"]
+ message.abs_z = self.controller_state["ABS_Z"]
+ message.abs_rz = self.controller_state["ABS_RZ"]
+ message.abs_gas = self.controller_state["ABS_GAS"]
+ message.abs_brake = self.controller_state["ABS_BRAKE"]
+ message.wildcard = self.controller_state["wildcard"]
+
+ return message
+
+
+ def update_controller_state(self, key, value):
+ self.controller_state[key] = value
+
+
+ def reconnect(self, last_callback_time_s):
+ self.set_device_state(DeviceState.ERROR)
+
+ if self.system_state != SystemState.AUTONOMOUS:
+ self.set_system_state(SystemState.DISABLED)
+
+ self.controller_state = dict.fromkeys(self.controller_state, 0.0)
+ last_callback_time_s = self.clock_routine(last_callback_time_s)
+
+ time.sleep(self.timer_period_s)
+
+ self.get_logger().info("attempting to reconnect...")
+ self.controller = None
+ self.controller = self.get_controller()
+
+ if self.controller is not None:
+ self.get_logger().info("reconnected!")
+
+ return last_callback_time_s
+
+
+ def xbox_swap_N_W(self, BTN_DIR):
+ if BTN_DIR == "BTN_NORTH":
+ return "BTN_WEST"
+ elif BTN_DIR == "BTN_WEST":
+ return "BTN_NORTH"
+ else:
+ return BTN_DIR
+
+
+def main():
+ rclpy.init()
+ node = ControllerInputNode()
+ rclpy.spin(node)
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
+
diff --git a/autonav_ws/src/autonav_manual/src/controller_listener.py b/autonav_ws/src/autonav_manual/src/controller_listener.py
new file mode 100644
index 0000000..a0c4876
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/src/controller_listener.py
@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from autonav_msgs.msg import ControllerInput
+
+
+class ControllerListener(Node):
+
+ def __init__(self):
+ super().__init__('controller_subscriber')
+
+ self.controller_state = {}
+
+ self.subscription = self.create_subscription(
+ ControllerInput,
+ '/autonav/controller_input',
+ self.listener_callback,
+ 10)
+
+ self.subscription # prevent unused variable warning
+
+
+ def listener_callback(self, msg):
+ self.deserialize_controller_state(msg)
+ self.get_logger().info(f"I heard: {str(self.controller_state)}")
+
+
+ def deserialize_controller_state(self, msg):
+ attributes = [n for n in dir(msg) if not (n.startswith('__') or n.startswith('_'))]
+ attributes.remove('SLOT_TYPES')
+ attributes.remove('get_fields_and_field_types')
+ for attribute in attributes:
+ self.controller_state[attribute] = getattr(msg, attribute)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ controller_listener = ControllerListener()
+
+ rclpy.spin(controller_listener)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ controller_listener.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_manual/src/manual_24.py b/autonav_ws/src/autonav_manual/src/manual_24.py
new file mode 100644
index 0000000..c920a02
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/src/manual_24.py
@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+
+import rclpy
+import json
+from autonav_msgs.msg import ControllerInput, MotorInput
+from types import SimpleNamespace
+import numpy as np
+from autonav_shared.node import Node
+from autonav_shared.types import LogLevel, DeviceState, SystemState
+from autonav_msgs.msg import ControllerInput
+
+class Manual24Config:
+ def __init__(self):
+ self.max_forward_speed = 3
+ self.max_angular_speed = np.pi
+
+
+class Manual24Node(Node):
+ def __init__(self):
+ super().__init__('manual_24')
+ self.write_config(Manual24Config())
+
+ def init(self):
+ # self.max_forward_speed = 1
+ self.max_forward_speed = self.config.max_forward_speed
+ # self.max_angular_speed = np.pi/4
+ self.max_angular_speed = self.config.max_angular_speed
+
+ self.controller_state = {}
+
+ self.set_device_state(DeviceState.WARMING)
+
+ self.controllerSubscriber = self.create_subscription(
+ ControllerInput,
+ '/autonav/controller_input',
+ self.input_callback,
+ 10)
+
+ self.motorPublisher = self.create_publisher(
+ MotorInput,
+ '/autonav/MotorInput',
+ 10
+ )
+
+ self.controllerSubscriber # prevent unused variable warning
+
+
+ def get_default_config(self):
+ return Manual24Config()
+
+
+ def input_callback(self, msg):
+ self.set_device_state(DeviceState.OPERATING)
+ self.deserialize_controller_state(msg)
+ # self.log(f"I heard: {str(self.controller_state)}")
+
+ self.change_system_state()
+ self.compose_motorinput_message()
+
+
+ def deserialize_controller_state(self, msg):
+ attributes = [n for n in dir(msg) if not (n.startswith('__') or n.startswith('_'))]
+ attributes.remove('SLOT_TYPES')
+ attributes.remove('get_fields_and_field_types')
+ for attribute in attributes:
+ self.controller_state[attribute] = getattr(msg, attribute)
+
+
+ def normalize(self, input, output_start, output_end, input_start, input_end):
+ return output_start + ((output_end - output_start) / (input_end - input_start)) * (input - input_start)
+
+
+ def change_system_state(self):
+ new_system_state = self.system_state
+ if self.controller_state['btn_east'] == 1.0:
+ new_system_state = SystemState.SHUTDOWN
+ self.log(f'Setting system state to {new_system_state}')
+ self.set_system_state(new_system_state)
+
+ elif self.controller_state['btn_start'] == 1.0:
+ new_system_state = SystemState.MANUAL
+ self.log(f'Setting system state to {new_system_state}')
+ self.set_system_state(new_system_state)
+
+ elif self.controller_state['btn_select'] == 1.0:
+ new_system_state = SystemState.DISABLED
+ self.log(f'Setting system state to {new_system_state}')
+ self.set_system_state(new_system_state)
+
+
+ def compose_motorinput_message(self):
+ forward_velocity = 0.0
+ angular_velocity = 0.0
+ if self.system_state == SystemState.MANUAL:
+ forward_velocity = self.normalize(self.controller_state["abs_gas"] - self.controller_state["abs_brake"], -self.max_forward_speed, self.max_forward_speed, -1, 1)
+ angular_velocity = self.normalize(self.controller_state["abs_x"], self.max_angular_speed, -self.max_angular_speed, -1, 1)
+
+ motor_msg = MotorInput()
+ motor_msg.forward_velocity = forward_velocity
+ motor_msg.angular_velocity = angular_velocity
+
+ self.motorPublisher.publish(motor_msg)
+
+
+def main(args=None):
+ rclpy.init()
+ node = Manual24Node()
+ rclpy.spin(node)
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_manual/src/manual_25.py b/autonav_ws/src/autonav_manual/src/manual_25.py
new file mode 100644
index 0000000..f719b91
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/src/manual_25.py
@@ -0,0 +1,178 @@
+#!/usr/bin/env python3
+
+import rclpy
+import json
+from autonav_msgs.msg import ControllerInput, MotorInput, MotorFeedback
+from types import SimpleNamespace
+import numpy as np
+from autonav_shared.node import Node
+from autonav_shared.types import LogLevel, DeviceState, SystemState
+from autonav_msgs.msg import ControllerInput
+from enum import IntEnum
+import time
+
+class ControllerMode(IntEnum):
+ LOCAL = 0
+ GLOBAL = 1
+
+
+class Manual25Config:
+ def __init__(self):
+ self.max_forward_speed = 3
+ self.max_sideways_speed = 3
+ self.max_angular_speed = np.pi
+ self.odom_fudge_factor = 1
+
+
+class Manual25Node(Node):
+ def __init__(self):
+ super().__init__('manual25_node')
+ self.write_config(Manual25Config())
+
+
+ def init(self):
+ self.mode = ControllerMode.LOCAL
+ self.orientation = 0
+ self.last_time = 0
+ self.delta_t = 0
+ self.new_time = time.time()
+ # self.max_forward_speed = 1
+ self.max_forward_speed = self.config.max_forward_speed
+ self.max_sideways_speed = self.config.max_sideways_speed
+ # self.max_angular_speed = np.pi/4
+ self.max_angular_speed = self.config.max_angular_speed
+ self.odom_fudge_factor = self.config.odom_fudge_factor
+
+ self.controller_state = {}
+
+ self.set_device_state(DeviceState.WARMING)
+
+ self.controllerSubscriber = self.create_subscription(
+ ControllerInput,
+ '/autonav/controller_input',
+ self.input_callback,
+ 10
+ )
+
+ self.motorSubscription = self.create_subscription(
+ MotorFeedback,
+ '/autonav/MotorFeedback',
+ self.on_motor_feedback,
+ 10
+ )
+
+ self.motorPublisher = self.create_publisher(
+ MotorInput,
+ '/autonav/MotorInput',
+ 10
+ )
+
+ self.controllerSubscriber # prevent unused variable warning
+
+
+ def input_callback(self, msg):
+ self.set_device_state(DeviceState.OPERATING)
+ self.deserialize_controller_state(msg)
+ # self.log(f"I heard: {str(self.controller_state)}")
+
+ self.change_controller_mode()
+ self.change_system_state()
+
+ self.log(f"orientation: {self.orientation}")
+ # local vs. global toggle
+
+ if self.mode == ControllerMode.LOCAL:
+ self.compose_motorinput_message_local()
+ elif self.mode == ControllerMode.GLOBAL:
+ self.compose_motorinput_message_global()
+
+
+
+ def deserialize_controller_state(self, msg):
+ attributes = [n for n in dir(msg) if not (n.startswith('__') or n.startswith('_'))]
+ attributes.remove('SLOT_TYPES')
+ attributes.remove('get_fields_and_field_types')
+ for attribute in attributes:
+ self.controller_state[attribute] = getattr(msg, attribute)
+
+
+ def normalize(self, input, output_start, output_end, input_start, input_end):
+ return output_start + ((output_end - output_start) / (input_end - input_start)) * (input - input_start)
+
+
+ def change_controller_mode(self):
+ if self.controller_state["btn_north"] == 1.0:
+ self.orientation = 0
+ self.mode = ControllerMode.GLOBAL
+ self.log(f'changed controller mode to {self.mode}')
+ elif self.controller_state["btn_south"] == 1.0:
+ self.orientation = 0
+ self.mode = ControllerMode.LOCAL
+ self.log(f'changed controller mode to {self.mode}')
+
+
+ def change_system_state(self):
+ new_system_state = self.system_state
+ if self.controller_state['btn_east'] == 1.0:
+ new_system_state = SystemState.SHUTDOWN
+ self.log(f'Setting system state to {new_system_state}')
+ self.set_system_state(new_system_state)
+
+ elif self.controller_state['btn_start'] == 1.0:
+ new_system_state = SystemState.MANUAL
+ self.log(f'Setting system state to {new_system_state}')
+ self.set_system_state(new_system_state)
+
+ elif self.controller_state['btn_select'] == 1.0:
+ new_system_state = SystemState.DISABLED
+ self.log(f'Setting system state to {new_system_state}')
+ self.set_system_state(new_system_state)
+
+
+ def compose_motorinput_message_local(self):
+ forward_velocity = 0.0
+ sideways_velocity = 0.0
+ angular_velocity = 0.0
+ if self.system_state == SystemState.MANUAL:
+ forward_velocity = self.normalize(self.controller_state["abs_y"], -self.max_forward_speed, self.max_forward_speed, 1.0, -1.0)
+ sideways_velocity = self.normalize(self.controller_state["abs_x"], -self.max_sideways_speed, self.max_sideways_speed, -1.0, 1.0)
+ angular_velocity = self.normalize(self.controller_state["abs_z"], -self.max_angular_speed, self.max_angular_speed, 1.0, -1.0)
+
+ motor_msg = MotorInput()
+ motor_msg.forward_velocity = forward_velocity
+ motor_msg.sideways_velocity = sideways_velocity
+ motor_msg.angular_velocity = angular_velocity
+
+ self.motorPublisher.publish(motor_msg)
+
+ # https://math.stackexchange.com/questions/2895880/inversion-of-rotation-matrix
+ def compose_motorinput_message_global(self):
+ forward_velocity = 0.0
+ sideways_velocity = 0.0
+ angular_velocity = 0.0
+ if self.system_state == SystemState.MANUAL:
+ forward_velocity = self.normalize(self.controller_state["abs_y"], -self.max_forward_speed, self.max_forward_speed, 1.0, -1.0)
+ sideways_velocity = self.normalize(self.controller_state["abs_x"], -self.max_sideways_speed, self.max_sideways_speed, -1.0, 1.0)
+ angular_velocity = self.normalize(self.controller_state["abs_z"], -self.max_angular_speed, self.max_angular_speed, 1.0, -1.0)
+
+ motor_msg = MotorInput()
+ motor_msg.forward_velocity = forward_velocity * np.cos(self.orientation) + sideways_velocity * np.sin(self.orientation)
+ motor_msg.sideways_velocity = -1 * sideways_velocity * np.cos(self.orientation) + forward_velocity * np.sin(self.orientation)
+ motor_msg.angular_velocity = angular_velocity
+
+ self.motorPublisher.publish(motor_msg)
+
+
+ def on_motor_feedback(self, msg:MotorFeedback):
+ delta_theta = msg.delta_theta * self.odom_fudge_factor
+ self.orientation += delta_theta
+
+def main(args=None):
+ rclpy.init()
+ node = Manual25Node()
+ rclpy.spin(node)
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_manual/src/motormessage_listener.py b/autonav_ws/src/autonav_manual/src/motormessage_listener.py
new file mode 100644
index 0000000..bc88e29
--- /dev/null
+++ b/autonav_ws/src/autonav_manual/src/motormessage_listener.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from autonav_msgs.msg import MotorInput
+
+
+class ControllerListener(Node):
+
+ def __init__(self):
+ super().__init__('motor_subscriber')
+
+ self.controller_state = {}
+
+ self.subscription = self.create_subscription(
+ MotorInput,
+ '/autonav/MotorInput',
+ self.listener_callback,
+ 10)
+
+ self.subscription # prevent unused variable warning
+
+
+ def listener_callback(self, msg):
+ self.get_logger().info(f"I heard: {msg.forward_velocity}, {msg.sideways_velocity}, {msg.angular_velocity}")
+
+
+ def deserialize_controller_state(self, msg):
+ attributes = [n for n in dir(msg) if not (n.startswith('__') or n.startswith('_'))]
+ attributes.remove('SLOT_TYPES')
+ attributes.remove('get_fields_and_field_types')
+ for attribute in attributes:
+ self.controller_state[attribute] = getattr(msg, attribute)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ controller_listener = ControllerListener()
+
+ rclpy.spin(controller_listener)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ controller_listener.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_msgs/CMakeLists.txt b/autonav_ws/src/autonav_msgs/CMakeLists.txt
index f660835..99c6eed 100644
--- a/autonav_ws/src/autonav_msgs/CMakeLists.txt
+++ b/autonav_ws/src/autonav_msgs/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(builtin_interfaces REQUIRED)
set(msg_files
"msg/AudibleFeedback.msg"
"msg/Conbus.msg"
+ "msg/ControllerInput.msg"
"msg/DeviceState.msg"
"msg/GPSFeedback.msg"
"msg/IMUData.msg"
diff --git a/autonav_ws/src/autonav_msgs/msg/ControllerInput.msg b/autonav_ws/src/autonav_msgs/msg/ControllerInput.msg
new file mode 100644
index 0000000..ceede29
--- /dev/null
+++ b/autonav_ws/src/autonav_msgs/msg/ControllerInput.msg
@@ -0,0 +1,19 @@
+float64 btn_north
+float64 btn_east
+float64 btn_south
+float64 btn_west
+float64 btn_start
+float64 btn_select
+float64 btn_mode
+float64 btn_tr
+float64 btn_tl
+float64 key_record
+float64 abs_hat0x
+float64 abs_hat0y
+float64 abs_x
+float64 abs_y
+float64 abs_z
+float64 abs_rz
+float64 abs_gas
+float64 abs_brake
+float64 wildcard
\ No newline at end of file