From 5b523222df1d5c2b2eb4005ba5f71e914d4ff078 Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Thu, 3 Oct 2024 19:27:58 -0500 Subject: [PATCH 1/6] moving manual package into the ws --- .../src/autonav_launch/launch/manual24.xml | 16 ++ .../src/autonav_launch/launch/manual24sim.xml | 14 + autonav_ws/src/xb_manual_pkg/CMakeLists.txt | 71 +++++ autonav_ws/src/xb_manual_pkg/LICENSE | 17 ++ autonav_ws/src/xb_manual_pkg/package.xml | 34 +++ .../src/xb_manual_pkg/src/controller_input.py | 251 ++++++++++++++++++ .../xb_manual_pkg/src/controller_listener.py | 52 ++++ autonav_ws/src/xb_manual_pkg/src/manual_24.py | 133 ++++++++++ .../src/motormessage_listener.py | 51 ++++ 9 files changed, 639 insertions(+) create mode 100644 autonav_ws/src/autonav_launch/launch/manual24.xml create mode 100644 autonav_ws/src/autonav_launch/launch/manual24sim.xml create mode 100644 autonav_ws/src/xb_manual_pkg/CMakeLists.txt create mode 100644 autonav_ws/src/xb_manual_pkg/LICENSE create mode 100644 autonav_ws/src/xb_manual_pkg/package.xml create mode 100644 autonav_ws/src/xb_manual_pkg/src/controller_input.py create mode 100644 autonav_ws/src/xb_manual_pkg/src/controller_listener.py create mode 100644 autonav_ws/src/xb_manual_pkg/src/manual_24.py create mode 100644 autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py 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..9c1de03 --- /dev/null +++ b/autonav_ws/src/autonav_launch/launch/manual24.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ 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..64b2ca1 --- /dev/null +++ b/autonav_ws/src/autonav_launch/launch/manual24sim.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/autonav_ws/src/xb_manual_pkg/CMakeLists.txt b/autonav_ws/src/xb_manual_pkg/CMakeLists.txt new file mode 100644 index 0000000..9230640 --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.8) + +project(xb_manual_pkg) + +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) +# 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/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/xb_manual_pkg/LICENSE b/autonav_ws/src/xb_manual_pkg/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/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/xb_manual_pkg/package.xml b/autonav_ws/src/xb_manual_pkg/package.xml new file mode 100644 index 0000000..031ffe2 --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/package.xml @@ -0,0 +1,34 @@ + + + + xb_manual_pkg + 0.0.0 + TODO: Package description + tony + MIT + + ament_cmake + ament_cmake_python + + rclcpp + rclpy + autonav_msgs + scr + + 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/xb_manual_pkg/src/controller_input.py b/autonav_ws/src/xb_manual_pkg/src/controller_input.py new file mode 100644 index 0000000..d9089d5 --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/src/controller_input.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python3 + +import rclpy +import evdev +import time +from scr.node import Node +from scr.states import DeviceStateEnum, SystemStateEnum +from evdev import ecodes +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.get_logger().info("HERE") + + self.set_device_state(DeviceStateEnum.STANDBY) + + self.get_logger().info("HERE") + + 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(DeviceStateEnum.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(DeviceStateEnum.ERRORED) + + if self.system_state != SystemStateEnum.AUTONOMOUS: + self.set_system_state(SystemStateEnum.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() + # controller_input = ControllerInputNode() + + # rclpy.spin(controller_input) + + # controller_input.destroy_node() + # rclpy.shutdown() + rclpy.init() + node = ControllerInputNode() + Node.run_node(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() + diff --git a/autonav_ws/src/xb_manual_pkg/src/controller_listener.py b/autonav_ws/src/xb_manual_pkg/src/controller_listener.py new file mode 100644 index 0000000..a0c4876 --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/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/xb_manual_pkg/src/manual_24.py b/autonav_ws/src/xb_manual_pkg/src/manual_24.py new file mode 100644 index 0000000..1a4b6b1 --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/src/manual_24.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 + +import rclpy +from autonav_msgs.msg import ControllerInput, MotorInput +from scr.node import Node +from scr.states import DeviceStateEnum, SystemStateEnum, SystemModeEnum +from scr.states import SystemStateEnum +import json +from types import SimpleNamespace + +#!/usr/bin/env python3 + +import rclpy +import numpy as np +from scr.node import Node +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') + + def init(self): + self.config = self.get_default_config() + # 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(DeviceStateEnum.STANDBY) + + self.get_logger().info("HERE") + + 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 config_updated(self, jsonObject): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + + + def get_default_config(self): + return Manual24Config() + + + def input_callback(self, msg): + self.set_device_state(DeviceStateEnum.OPERATING) + self.deserialize_controller_state(msg) + self.get_logger().info(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 = SystemStateEnum.SHUTDOWN + + elif self.controller_state['btn_start'] == 1.0: + new_system_state = SystemStateEnum.MANUAL + + elif self.controller_state['btn_select'] == 1.0: + new_system_state = SystemStateEnum.DISABLED + + self.get_logger().info(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 == SystemStateEnum.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(args=args) + + # controller_listener = Manual24() + + # 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() + rclpy.init() + node = Manual24Node() + Node.run_node(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py b/autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py new file mode 100644 index 0000000..f3b03d7 --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/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.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 From c4c6264a73d4590ae105e5def8c8d03f52c8b710 Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Tue, 8 Oct 2024 19:55:15 -0500 Subject: [PATCH 2/6] manual24 ported fully, manual 25 robot local controls implemented. Probably doing the global transformation wrong at this point but can't test --- .../src/autonav_launch/launch/manual24.xml | 5 +- .../src/autonav_launch/launch/manual24sim.xml | 6 +- .../src/autonav_launch/launch/manual25.xml | 15 ++ .../src/autonav_launch/launch/manual25sim.xml | 14 ++ autonav_ws/src/autonav_msgs/CMakeLists.txt | 1 + .../src/autonav_msgs/msg/ControllerInput.msg | 19 ++ autonav_ws/src/xb_manual_pkg/CMakeLists.txt | 2 + autonav_ws/src/xb_manual_pkg/package.xml | 2 +- .../src/xb_manual_pkg/src/controller_input.py | 25 +-- autonav_ws/src/xb_manual_pkg/src/manual_24.py | 38 ++-- autonav_ws/src/xb_manual_pkg/src/manual_25.py | 169 ++++++++++++++++++ .../src/motormessage_listener.py | 2 +- 12 files changed, 246 insertions(+), 52 deletions(-) create mode 100644 autonav_ws/src/autonav_launch/launch/manual25.xml create mode 100644 autonav_ws/src/autonav_launch/launch/manual25sim.xml create mode 100644 autonav_ws/src/autonav_msgs/msg/ControllerInput.msg create mode 100644 autonav_ws/src/xb_manual_pkg/src/manual_25.py diff --git a/autonav_ws/src/autonav_launch/launch/manual24.xml b/autonav_ws/src/autonav_launch/launch/manual24.xml index 9c1de03..3be9967 100644 --- a/autonav_ws/src/autonav_launch/launch/manual24.xml +++ b/autonav_ws/src/autonav_launch/launch/manual24.xml @@ -1,7 +1,6 @@ - - + @@ -9,7 +8,7 @@ - + diff --git a/autonav_ws/src/autonav_launch/launch/manual24sim.xml b/autonav_ws/src/autonav_launch/launch/manual24sim.xml index 64b2ca1..cb81892 100644 --- a/autonav_ws/src/autonav_launch/launch/manual24sim.xml +++ b/autonav_ws/src/autonav_launch/launch/manual24sim.xml @@ -2,13 +2,13 @@ - - + + - + \ 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..5551edb --- /dev/null +++ b/autonav_ws/src/autonav_launch/launch/manual25.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ 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..1531313 --- /dev/null +++ b/autonav_ws/src/autonav_launch/launch/manual25sim.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ 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 ab9ff5b..87145ff 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 diff --git a/autonav_ws/src/xb_manual_pkg/CMakeLists.txt b/autonav_ws/src/xb_manual_pkg/CMakeLists.txt index 9230640..fe414f9 100644 --- a/autonav_ws/src/xb_manual_pkg/CMakeLists.txt +++ b/autonav_ws/src/xb_manual_pkg/CMakeLists.txt @@ -12,6 +12,7 @@ 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) @@ -51,6 +52,7 @@ install(TARGETS src/controller_input.py src/controller_listener.py src/manual_24.py + src/manual_25.py src/motormessage_listener.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/autonav_ws/src/xb_manual_pkg/package.xml b/autonav_ws/src/xb_manual_pkg/package.xml index 031ffe2..f7307d3 100644 --- a/autonav_ws/src/xb_manual_pkg/package.xml +++ b/autonav_ws/src/xb_manual_pkg/package.xml @@ -13,7 +13,7 @@ rclcpp rclpy autonav_msgs - scr + autonav_shared rosidl_default_generators diff --git a/autonav_ws/src/xb_manual_pkg/src/controller_input.py b/autonav_ws/src/xb_manual_pkg/src/controller_input.py index d9089d5..3da0644 100644 --- a/autonav_ws/src/xb_manual_pkg/src/controller_input.py +++ b/autonav_ws/src/xb_manual_pkg/src/controller_input.py @@ -2,10 +2,10 @@ import rclpy import evdev -import time -from scr.node import Node -from scr.states import DeviceStateEnum, SystemStateEnum 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 @@ -28,7 +28,7 @@ def __init__(self): def init(self): self.get_logger().info("HERE") - self.set_device_state(DeviceStateEnum.STANDBY) + self.set_device_state(DeviceState.WARMING) self.get_logger().info("HERE") @@ -76,7 +76,7 @@ def get_controller(self): f"\nController state will be published on update and every {self.timer_period_s} seconds") if controller is not None: - self.set_device_state(DeviceStateEnum.OPERATING) + self.set_device_state(DeviceState.OPERATING) return controller @@ -203,10 +203,10 @@ def update_controller_state(self, key, value): def reconnect(self, last_callback_time_s): - self.set_device_state(DeviceStateEnum.ERRORED) + self.set_device_state(DeviceState.ERROR) - if self.system_state != SystemStateEnum.AUTONOMOUS: - self.set_system_state(SystemStateEnum.DISABLED) + 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) @@ -233,16 +233,9 @@ def xbox_swap_N_W(self, BTN_DIR): def main(): - # rclpy.init() - # controller_input = ControllerInputNode() - - # rclpy.spin(controller_input) - - # controller_input.destroy_node() - # rclpy.shutdown() rclpy.init() node = ControllerInputNode() - Node.run_node(node) + rclpy.spin(node) rclpy.shutdown() diff --git a/autonav_ws/src/xb_manual_pkg/src/manual_24.py b/autonav_ws/src/xb_manual_pkg/src/manual_24.py index 1a4b6b1..9d74087 100644 --- a/autonav_ws/src/xb_manual_pkg/src/manual_24.py +++ b/autonav_ws/src/xb_manual_pkg/src/manual_24.py @@ -1,18 +1,12 @@ #!/usr/bin/env python3 import rclpy -from autonav_msgs.msg import ControllerInput, MotorInput -from scr.node import Node -from scr.states import DeviceStateEnum, SystemStateEnum, SystemModeEnum -from scr.states import SystemStateEnum import json +from autonav_msgs.msg import ControllerInput, MotorInput from types import SimpleNamespace - -#!/usr/bin/env python3 - -import rclpy import numpy as np -from scr.node import Node +from autonav_shared.node import Node +from autonav_shared.types import LogLevel, DeviceState, SystemState from autonav_msgs.msg import ControllerInput class Manual24Config: @@ -33,7 +27,7 @@ def init(self): self.controller_state = {} - self.set_device_state(DeviceStateEnum.STANDBY) + self.set_device_state(DeviceState.WARMING) self.get_logger().info("HERE") @@ -61,11 +55,10 @@ def get_default_config(self): def input_callback(self, msg): - self.set_device_state(DeviceStateEnum.OPERATING) + self.set_device_state(DeviceState.OPERATING) self.deserialize_controller_state(msg) self.get_logger().info(f"I heard: {str(self.controller_state)}") - self.change_system_state() self.compose_motorinput_message() @@ -85,13 +78,13 @@ def normalize(self, input, output_start, output_end, input_start, input_end): def change_system_state(self): new_system_state = self.system_state if self.controller_state['btn_east'] == 1.0: - new_system_state = SystemStateEnum.SHUTDOWN + new_system_state = SystemState.SHUTDOWN elif self.controller_state['btn_start'] == 1.0: - new_system_state = SystemStateEnum.MANUAL + new_system_state = SystemState.MANUAL elif self.controller_state['btn_select'] == 1.0: - new_system_state = SystemStateEnum.DISABLED + new_system_state = SystemState.DISABLED self.get_logger().info(f'Setting system state to {new_system_state}') self.set_system_state(new_system_state) @@ -100,7 +93,7 @@ def change_system_state(self): def compose_motorinput_message(self): forward_velocity = 0.0 angular_velocity = 0.0 - if self.system_state == SystemStateEnum.MANUAL: + 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) @@ -112,20 +105,9 @@ def compose_motorinput_message(self): def main(args=None): - # rclpy.init(args=args) - - # controller_listener = Manual24() - - # 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() rclpy.init() node = Manual24Node() - Node.run_node(node) + rclpy.spin(node) rclpy.shutdown() diff --git a/autonav_ws/src/xb_manual_pkg/src/manual_25.py b/autonav_ws/src/xb_manual_pkg/src/manual_25.py new file mode 100644 index 0000000..b412a9e --- /dev/null +++ b/autonav_ws/src/xb_manual_pkg/src/manual_25.py @@ -0,0 +1,169 @@ +#!/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 +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 + + +class Manual25Node(Node): + def __init__(self): + super().__init__('manual_25') + + + def init(self): + self.config = self.get_default_config() + self.mode = ControllerMode.LOCAL + self.orientation = 0 + self.last_time = 0 + self.delta_t = 0 + + # 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.controller_state = {} + + self.set_device_state(DeviceState.WARMING) + + self.get_logger().info("HERE") + + 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 config_updated(self, jsonObject): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + + + def get_default_config(self): + return Manual25Config() + + + def input_callback(self, msg): + self.new_time = time.clock() + self.set_device_state(DeviceState.OPERATING) + self.deserialize_controller_state(msg) + self.get_logger().info(f"I heard: {str(self.controller_state)}") + + self.changed_controller_mode() + self.change_system_state() + + # local vs. global toggle + + self.delta_t = self.new_time - self.last_time + if self.mode == ControllerMode.LOCAL: + self.compose_motorinput_message_local() + elif self.mode == ControllerMode.GLOBAL: + self.compose_motorinput_message_global() + + self.last_time = self.new_time + + 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 changed_controller_mode(self): + new_controller_mode = ControllerMode.GLOBAL if self.controller_state["btn_select"] == 1.0 else ControllerMode.LOCAL + self.mode = new_controller_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 + + elif self.controller_state['btn_start'] == 1.0: + new_system_state = SystemState.MANUAL + + elif self.controller_state['btn_select'] == 1.0: + new_system_state = SystemState.DISABLED + + self.get_logger().info(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) + self.orientation += angular_velocity * self.delta_t + + + 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 * abs(np.cos(self.orientation)) + sideways_velocity * abs(np.sin(self.orientation)) + motor_msg.sideways_velocity = sideways_velocity * abs(np.cos(self.orientation)) - forward_velocity * abs(np.sin(self.orientation)) + motor_msg.angular_velocity = angular_velocity + + self.motorPublisher.publish(motor_msg) + self.orientation += angular_velocity * self.delta_t + + +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/xb_manual_pkg/src/motormessage_listener.py b/autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py index f3b03d7..bc88e29 100644 --- a/autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py +++ b/autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py @@ -22,7 +22,7 @@ def __init__(self): def listener_callback(self, msg): - self.get_logger().info(f"I heard: {msg.forward_velocity}, {msg.angular_velocity}") + self.get_logger().info(f"I heard: {msg.forward_velocity}, {msg.sideways_velocity}, {msg.angular_velocity}") def deserialize_controller_state(self, msg): From c17971fbf4db1fce517c316089a2982d592ec95b Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Thu, 10 Oct 2024 19:17:40 -0500 Subject: [PATCH 3/6] manual for swerve done in theory --- .gitignore | 3 +++ .../src/autonav_launch/launch/manual25.xml | 2 +- .../src/autonav_launch/launch/manual25sim.xml | 2 +- autonav_ws/src/xb_manual_pkg/src/manual_25.py | 25 +++++++++++-------- 4 files changed, 20 insertions(+), 12 deletions(-) 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/manual25.xml b/autonav_ws/src/autonav_launch/launch/manual25.xml index 5551edb..0001b2e 100644 --- a/autonav_ws/src/autonav_launch/launch/manual25.xml +++ b/autonav_ws/src/autonav_launch/launch/manual25.xml @@ -5,7 +5,7 @@ - + diff --git a/autonav_ws/src/autonav_launch/launch/manual25sim.xml b/autonav_ws/src/autonav_launch/launch/manual25sim.xml index 1531313..997cf9f 100644 --- a/autonav_ws/src/autonav_launch/launch/manual25sim.xml +++ b/autonav_ws/src/autonav_launch/launch/manual25sim.xml @@ -7,7 +7,7 @@ - + diff --git a/autonav_ws/src/xb_manual_pkg/src/manual_25.py b/autonav_ws/src/xb_manual_pkg/src/manual_25.py index b412a9e..197d891 100644 --- a/autonav_ws/src/xb_manual_pkg/src/manual_25.py +++ b/autonav_ws/src/xb_manual_pkg/src/manual_25.py @@ -72,17 +72,18 @@ def get_default_config(self): def input_callback(self, msg): - self.new_time = time.clock() + self.new_time = time.time() self.set_device_state(DeviceState.OPERATING) self.deserialize_controller_state(msg) - self.get_logger().info(f"I heard: {str(self.controller_state)}") + # self.get_logger().info(f"I heard: {str(self.controller_state)}") - self.changed_controller_mode() + self.change_controller_mode() self.change_system_state() # local vs. global toggle self.delta_t = self.new_time - self.last_time + if self.mode == ControllerMode.LOCAL: self.compose_motorinput_message_local() elif self.mode == ControllerMode.GLOBAL: @@ -102,10 +103,14 @@ 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 changed_controller_mode(self): - new_controller_mode = ControllerMode.GLOBAL if self.controller_state["btn_select"] == 1.0 else ControllerMode.LOCAL - self.mode = new_controller_mode - + def change_controller_mode(self): + if self.controller_state["btn_north"] == 1.0: + self.mode = ControllerMode.GLOBAL + self.get_logger().info(f'changed controller mode to {self.mode}') + elif self.controller_state["btn_south"] == 1.0: + self.mode = ControllerMode.LOCAL + self.get_logger().info(f'changed controller mode to {self.mode}') + def change_system_state(self): new_system_state = self.system_state @@ -139,7 +144,7 @@ def compose_motorinput_message_local(self): self.motorPublisher.publish(motor_msg) self.orientation += angular_velocity * self.delta_t - + # https://math.stackexchange.com/questions/2895880/inversion-of-rotation-matrix def compose_motorinput_message_global(self): forward_velocity = 0.0 sideways_velocity = 0.0 @@ -150,8 +155,8 @@ def compose_motorinput_message_global(self): 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 * abs(np.cos(self.orientation)) + sideways_velocity * abs(np.sin(self.orientation)) - motor_msg.sideways_velocity = sideways_velocity * abs(np.cos(self.orientation)) - forward_velocity * abs(np.sin(self.orientation)) + 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) From 6db486fab6ff52ee2840c1247a92fc1b77d21b3f Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Thu, 31 Oct 2024 20:04:08 -0500 Subject: [PATCH 4/6] added odometry to global space control for swerve, added config --- autonav_ws/src/xb_manual_pkg/src/manual_25.py | 60 +++++++++++-------- 1 file changed, 34 insertions(+), 26 deletions(-) diff --git a/autonav_ws/src/xb_manual_pkg/src/manual_25.py b/autonav_ws/src/xb_manual_pkg/src/manual_25.py index 197d891..38d2448 100644 --- a/autonav_ws/src/xb_manual_pkg/src/manual_25.py +++ b/autonav_ws/src/xb_manual_pkg/src/manual_25.py @@ -2,7 +2,7 @@ import rclpy import json -from autonav_msgs.msg import ControllerInput, MotorInput +from autonav_msgs.msg import ControllerInput, MotorInput, MotorFeedback from types import SimpleNamespace import numpy as np from autonav_shared.node import Node @@ -21,30 +21,34 @@ 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__('manual_25') + super().__init__('manual25_node') + self.write_config(Manual25Config()) def init(self): - self.config = self.get_default_config() 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.request_all_configs() + self.get_logger().info("HERE") self.controllerSubscriber = self.create_subscription( @@ -53,6 +57,13 @@ def init(self): self.input_callback, 10 ) + + self.motorSubscription = self.create_subscription( + MotorFeedback, + '/autonav/MotorFeedback', + self.on_motor_feedback, + 10 + ) self.motorPublisher = self.create_publisher( MotorInput, @@ -63,16 +74,7 @@ def init(self): self.controllerSubscriber # prevent unused variable warning - def config_updated(self, jsonObject): - self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) - - - def get_default_config(self): - return Manual25Config() - - def input_callback(self, msg): - self.new_time = time.time() self.set_device_state(DeviceState.OPERATING) self.deserialize_controller_state(msg) # self.get_logger().info(f"I heard: {str(self.controller_state)}") @@ -80,16 +82,15 @@ def input_callback(self, msg): self.change_controller_mode() self.change_system_state() + self.get_logger().info(f"orientation: {self.orientation}") # local vs. global toggle - - self.delta_t = self.new_time - self.last_time if self.mode == ControllerMode.LOCAL: self.compose_motorinput_message_local() elif self.mode == ControllerMode.GLOBAL: self.compose_motorinput_message_global() - self.last_time = self.new_time + def deserialize_controller_state(self, msg): attributes = [n for n in dir(msg) if not (n.startswith('__') or n.startswith('_'))] @@ -105,9 +106,11 @@ def normalize(self, input, output_start, output_end, input_start, input_end): def change_controller_mode(self): if self.controller_state["btn_north"] == 1.0: + self.orientation = 0 self.mode = ControllerMode.GLOBAL self.get_logger().info(f'changed controller mode to {self.mode}') elif self.controller_state["btn_south"] == 1.0: + self.orientation = 0 self.mode = ControllerMode.LOCAL self.get_logger().info(f'changed controller mode to {self.mode}') @@ -116,15 +119,18 @@ 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.get_logger().info(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.get_logger().info(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.get_logger().info(f'Setting system state to {new_system_state}') - self.set_system_state(new_system_state) + self.get_logger().info(f'Setting system state to {new_system_state}') + self.set_system_state(new_system_state) def compose_motorinput_message_local(self): @@ -132,9 +138,9 @@ def compose_motorinput_message_local(self): 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) + 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) + 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 @@ -142,7 +148,6 @@ def compose_motorinput_message_local(self): motor_msg.angular_velocity = angular_velocity self.motorPublisher.publish(motor_msg) - self.orientation += angular_velocity * self.delta_t # https://math.stackexchange.com/questions/2895880/inversion-of-rotation-matrix def compose_motorinput_message_global(self): @@ -150,9 +155,9 @@ def compose_motorinput_message_global(self): 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) + 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) + 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) @@ -160,9 +165,12 @@ def compose_motorinput_message_global(self): motor_msg.angular_velocity = angular_velocity self.motorPublisher.publish(motor_msg) - self.orientation += angular_velocity * self.delta_t + 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() From 9d2e75085f7827d9858e4e97ce5e8429261c168a Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Tue, 5 Nov 2024 18:42:51 -0600 Subject: [PATCH 5/6] polish --- .../src/autonav_launch/launch/manual24.xml | 12 +++------- .../src/autonav_launch/launch/manual24sim.xml | 11 ++++------ .../src/autonav_launch/launch/manual25.xml | 12 +++------- .../src/autonav_launch/launch/manual25sim.xml | 9 +++----- .../CMakeLists.txt | 2 +- .../{xb_manual_pkg => autonav_manual}/LICENSE | 0 .../package.xml | 6 ++--- .../src/controller_input.py | 4 ---- .../src/controller_listener.py | 0 .../src/manual_24.py | 22 +++++++++---------- .../src/manual_25.py | 16 ++++++-------- .../src/motormessage_listener.py | 0 12 files changed, 35 insertions(+), 59 deletions(-) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/CMakeLists.txt (98%) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/LICENSE (100%) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/package.xml (86%) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/src/controller_input.py (99%) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/src/controller_listener.py (100%) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/src/manual_24.py (86%) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/src/manual_25.py (91%) rename autonav_ws/src/{xb_manual_pkg => autonav_manual}/src/motormessage_listener.py (100%) diff --git a/autonav_ws/src/autonav_launch/launch/manual24.xml b/autonav_ws/src/autonav_launch/launch/manual24.xml index 3be9967..a8b5c45 100644 --- a/autonav_ws/src/autonav_launch/launch/manual24.xml +++ b/autonav_ws/src/autonav_launch/launch/manual24.xml @@ -1,15 +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 index cb81892..73f17ac 100644 --- a/autonav_ws/src/autonav_launch/launch/manual24sim.xml +++ b/autonav_ws/src/autonav_launch/launch/manual24sim.xml @@ -1,13 +1,10 @@ - - - - + - - - + + + diff --git a/autonav_ws/src/autonav_launch/launch/manual25.xml b/autonav_ws/src/autonav_launch/launch/manual25.xml index 0001b2e..8cfab62 100644 --- a/autonav_ws/src/autonav_launch/launch/manual25.xml +++ b/autonav_ws/src/autonav_launch/launch/manual25.xml @@ -1,15 +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 index 997cf9f..416cedf 100644 --- a/autonav_ws/src/autonav_launch/launch/manual25sim.xml +++ b/autonav_ws/src/autonav_launch/launch/manual25sim.xml @@ -1,13 +1,10 @@ - - - - - - + + + diff --git a/autonav_ws/src/xb_manual_pkg/CMakeLists.txt b/autonav_ws/src/autonav_manual/CMakeLists.txt similarity index 98% rename from autonav_ws/src/xb_manual_pkg/CMakeLists.txt rename to autonav_ws/src/autonav_manual/CMakeLists.txt index fe414f9..857a221 100644 --- a/autonav_ws/src/xb_manual_pkg/CMakeLists.txt +++ b/autonav_ws/src/autonav_manual/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.8) -project(xb_manual_pkg) +project(autonav_manual) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/autonav_ws/src/xb_manual_pkg/LICENSE b/autonav_ws/src/autonav_manual/LICENSE similarity index 100% rename from autonav_ws/src/xb_manual_pkg/LICENSE rename to autonav_ws/src/autonav_manual/LICENSE diff --git a/autonav_ws/src/xb_manual_pkg/package.xml b/autonav_ws/src/autonav_manual/package.xml similarity index 86% rename from autonav_ws/src/xb_manual_pkg/package.xml rename to autonav_ws/src/autonav_manual/package.xml index f7307d3..891f7df 100644 --- a/autonav_ws/src/xb_manual_pkg/package.xml +++ b/autonav_ws/src/autonav_manual/package.xml @@ -1,9 +1,9 @@ - xb_manual_pkg - 0.0.0 - TODO: Package description + autonav_manual + 2025.0.1 + Contains nodes for reading, using, and debugging controller input tony MIT diff --git a/autonav_ws/src/xb_manual_pkg/src/controller_input.py b/autonav_ws/src/autonav_manual/src/controller_input.py similarity index 99% rename from autonav_ws/src/xb_manual_pkg/src/controller_input.py rename to autonav_ws/src/autonav_manual/src/controller_input.py index 3da0644..f9e3d06 100644 --- a/autonav_ws/src/xb_manual_pkg/src/controller_input.py +++ b/autonav_ws/src/autonav_manual/src/controller_input.py @@ -26,12 +26,8 @@ def __init__(self): def init(self): - self.get_logger().info("HERE") - self.set_device_state(DeviceState.WARMING) - self.get_logger().info("HERE") - self.timer_period_s = 0.1 self.publisher = self.create_publisher(ControllerInput, '/autonav/controller_input', 10) diff --git a/autonav_ws/src/xb_manual_pkg/src/controller_listener.py b/autonav_ws/src/autonav_manual/src/controller_listener.py similarity index 100% rename from autonav_ws/src/xb_manual_pkg/src/controller_listener.py rename to autonav_ws/src/autonav_manual/src/controller_listener.py diff --git a/autonav_ws/src/xb_manual_pkg/src/manual_24.py b/autonav_ws/src/autonav_manual/src/manual_24.py similarity index 86% rename from autonav_ws/src/xb_manual_pkg/src/manual_24.py rename to autonav_ws/src/autonav_manual/src/manual_24.py index 9d74087..f9f9374 100644 --- a/autonav_ws/src/xb_manual_pkg/src/manual_24.py +++ b/autonav_ws/src/autonav_manual/src/manual_24.py @@ -14,12 +14,13 @@ 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.config = self.get_default_config() # self.max_forward_speed = 1 self.max_forward_speed = self.config.max_forward_speed # self.max_angular_speed = np.pi/4 @@ -28,8 +29,8 @@ def init(self): self.controller_state = {} self.set_device_state(DeviceState.WARMING) - - self.get_logger().info("HERE") + + self.request_all_configs() self.controllerSubscriber = self.create_subscription( ControllerInput, @@ -46,10 +47,6 @@ def init(self): self.controllerSubscriber # prevent unused variable warning - def config_updated(self, jsonObject): - self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) - - def get_default_config(self): return Manual24Config() @@ -57,7 +54,7 @@ def get_default_config(self): def input_callback(self, msg): self.set_device_state(DeviceState.OPERATING) self.deserialize_controller_state(msg) - self.get_logger().info(f"I heard: {str(self.controller_state)}") + # self.log(f"I heard: {str(self.controller_state)}") self.change_system_state() self.compose_motorinput_message() @@ -79,15 +76,18 @@ 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.get_logger().info(f'Setting system state to {new_system_state}') - self.set_system_state(new_system_state) + self.log(f'Setting system state to {new_system_state}') + self.set_system_state(new_system_state) def compose_motorinput_message(self): diff --git a/autonav_ws/src/xb_manual_pkg/src/manual_25.py b/autonav_ws/src/autonav_manual/src/manual_25.py similarity index 91% rename from autonav_ws/src/xb_manual_pkg/src/manual_25.py rename to autonav_ws/src/autonav_manual/src/manual_25.py index 38d2448..5b8ad0c 100644 --- a/autonav_ws/src/xb_manual_pkg/src/manual_25.py +++ b/autonav_ws/src/autonav_manual/src/manual_25.py @@ -49,8 +49,6 @@ def init(self): self.request_all_configs() - self.get_logger().info("HERE") - self.controllerSubscriber = self.create_subscription( ControllerInput, '/autonav/controller_input', @@ -77,12 +75,12 @@ def init(self): def input_callback(self, msg): self.set_device_state(DeviceState.OPERATING) self.deserialize_controller_state(msg) - # self.get_logger().info(f"I heard: {str(self.controller_state)}") + # self.log(f"I heard: {str(self.controller_state)}") self.change_controller_mode() self.change_system_state() - self.get_logger().info(f"orientation: {self.orientation}") + self.log(f"orientation: {self.orientation}") # local vs. global toggle if self.mode == ControllerMode.LOCAL: @@ -108,28 +106,28 @@ def change_controller_mode(self): if self.controller_state["btn_north"] == 1.0: self.orientation = 0 self.mode = ControllerMode.GLOBAL - self.get_logger().info(f'changed controller mode to {self.mode}') + 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.get_logger().info(f'changed controller mode to {self.mode}') + 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.get_logger().info(f'Setting system state to {new_system_state}') + 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.get_logger().info(f'Setting system state to {new_system_state}') + 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.get_logger().info(f'Setting system state to {new_system_state}') + self.log(f'Setting system state to {new_system_state}') self.set_system_state(new_system_state) diff --git a/autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py b/autonav_ws/src/autonav_manual/src/motormessage_listener.py similarity index 100% rename from autonav_ws/src/xb_manual_pkg/src/motormessage_listener.py rename to autonav_ws/src/autonav_manual/src/motormessage_listener.py From ad3e0186702f512e26a51ff5803819f4f625d8b0 Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Tue, 5 Nov 2024 18:45:47 -0600 Subject: [PATCH 6/6] no longer requesting all configs --- autonav_ws/src/autonav_manual/src/manual_24.py | 2 -- autonav_ws/src/autonav_manual/src/manual_25.py | 2 -- 2 files changed, 4 deletions(-) diff --git a/autonav_ws/src/autonav_manual/src/manual_24.py b/autonav_ws/src/autonav_manual/src/manual_24.py index f9f9374..c920a02 100644 --- a/autonav_ws/src/autonav_manual/src/manual_24.py +++ b/autonav_ws/src/autonav_manual/src/manual_24.py @@ -30,8 +30,6 @@ def init(self): self.set_device_state(DeviceState.WARMING) - self.request_all_configs() - self.controllerSubscriber = self.create_subscription( ControllerInput, '/autonav/controller_input', diff --git a/autonav_ws/src/autonav_manual/src/manual_25.py b/autonav_ws/src/autonav_manual/src/manual_25.py index 5b8ad0c..f719b91 100644 --- a/autonav_ws/src/autonav_manual/src/manual_25.py +++ b/autonav_ws/src/autonav_manual/src/manual_25.py @@ -47,8 +47,6 @@ def init(self): self.set_device_state(DeviceState.WARMING) - self.request_all_configs() - self.controllerSubscriber = self.create_subscription( ControllerInput, '/autonav/controller_input',