From 6221b7c4bea43f1b844ad1269fafeccc3af1c62a Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 24 Oct 2023 16:29:51 +0200 Subject: [PATCH 01/77] Add catkin_lint and compile github action --- .github/workflows/catkin-lint.yml | 48 +++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 .github/workflows/catkin-lint.yml diff --git a/.github/workflows/catkin-lint.yml b/.github/workflows/catkin-lint.yml new file mode 100644 index 00000000..323556d7 --- /dev/null +++ b/.github/workflows/catkin-lint.yml @@ -0,0 +1,48 @@ +name: Catkin_lint ros style check +on: [push, pull_request] +jobs: + check_ros_style: + runs-on: ubuntu-latest + container: + image: ros:noetic + steps: + - uses: actions/checkout@v3 + - run: echo "catkin_lint" > ./requirements.txt + # - uses: actions/setup-python@v4 + # with: + # python-version: '3.9' + # cache: 'pip' # caching pip dependencies + # - run: apt install python-pip + - run: apt update && apt install catkin-lint -y + - run: | + mkdir -p catkin_ws/src + mv mirte_bringup catkin_ws/src/ + mv mirte_control catkin_ws/src/ + mv mirte_msgs catkin_ws/src/ + mv mirte_telemetrix catkin_ws/src/ + mv mirte_teleop catkin_ws/src/ + cd catkin_ws/src + . /opt/ros/noetic/setup.sh + catkin_init_workspace + cd .. + rosdep init || true + rosdep update || true + rosdep install -y --from-paths src/ --ignore-src --rosdistro noetic + catkin_make + . devel/setup.sh + - run: | + . catkin_ws/devel/setup.sh + failure=0 + catkin_lint --pkg mirte_bringup || failure=1 + catkin_lint --pkg mirte_control || failure=1 + catkin_lint --pkg mirte_msgs || failure=1 + catkin_lint --pkg mirte_telemetrix || failure=1 + catkin_lint --pkg mirte_teleop || failure=1 + if failure; then + echo "Automated tests failed" >&2 + exit 1 + else + echo "Automated tests succeeded" >&2 + exit 0 + fi + From 563fa032b4a859758953c1949cef318fcb564a07 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 24 Oct 2023 16:30:09 +0200 Subject: [PATCH 02/77] fix mirte-bringup package --- mirte_bringup/CMakeLists.txt | 3 +++ mirte_bringup/package.xml | 1 + 2 files changed, 4 insertions(+) diff --git a/mirte_bringup/CMakeLists.txt b/mirte_bringup/CMakeLists.txt index dc45c205..7a1ac986 100644 --- a/mirte_bringup/CMakeLists.txt +++ b/mirte_bringup/CMakeLists.txt @@ -103,6 +103,9 @@ find_package(catkin REQUIRED COMPONENTS # CATKIN_DEPENDS roscpp rospy std_msgs mirte_msgs message_runtime controller_manager hardware_interface # DEPENDS system_lib #) +catkin_package( + CATKIN_DEPENDS +) ########### diff --git a/mirte_bringup/package.xml b/mirte_bringup/package.xml index a102b190..d7b43959 100644 --- a/mirte_bringup/package.xml +++ b/mirte_bringup/package.xml @@ -15,6 +15,7 @@ Apache2 + controller_manager From 02b7fe963e69f178c0d1e0eacf09abc13886c7fc Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 24 Oct 2023 16:50:59 +0200 Subject: [PATCH 03/77] Fix mirte_control --- .github/workflows/catkin-lint.yml | 10 +++++----- mirte_control/CMakeLists.txt | 5 +++-- mirte_control/package.xml | 29 ++++++++++++++++------------- 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/.github/workflows/catkin-lint.yml b/.github/workflows/catkin-lint.yml index 323556d7..c5c911ed 100644 --- a/.github/workflows/catkin-lint.yml +++ b/.github/workflows/catkin-lint.yml @@ -33,11 +33,11 @@ jobs: - run: | . catkin_ws/devel/setup.sh failure=0 - catkin_lint --pkg mirte_bringup || failure=1 - catkin_lint --pkg mirte_control || failure=1 - catkin_lint --pkg mirte_msgs || failure=1 - catkin_lint --pkg mirte_telemetrix || failure=1 - catkin_lint --pkg mirte_teleop || failure=1 + catkin_lint --explain --ignore uninstalled_target --pkg mirte_bringup || failure=1 + catkin_lint --explain --ignore uninstalled_target --pkg mirte_control || failure=1 + catkin_lint --explain --ignore uninstalled_target --pkg mirte_msgs || failure=1 + catkin_lint --explain --ignore uninstalled_target --pkg mirte_telemetrix || failure=1 + catkin_lint --explain --ignore uninstalled_target --pkg mirte_teleop || failure=1 if failure; then echo "Automated tests failed" >&2 exit 1 diff --git a/mirte_control/CMakeLists.txt b/mirte_control/CMakeLists.txt index b215f1c9..6d38d01e 100644 --- a/mirte_control/CMakeLists.txt +++ b/mirte_control/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS mirte_msgs controller_manager hardware_interface + trajectory_msgs ) ## System dependencies are found with CMake's conventions @@ -103,9 +104,9 @@ find_package(catkin REQUIRED COMPONENTS ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( - INCLUDE_DIRS include + # INCLUDE_DIRS include/ # LIBRARIES mirte_ros_package - CATKIN_DEPENDS roscpp std_msgs mirte_msgs message_runtime controller_manager hardware_interface + CATKIN_DEPENDS roscpp std_msgs mirte_msgs message_runtime controller_manager hardware_interface trajectory_msgs # DEPENDS system_lib ) diff --git a/mirte_control/package.xml b/mirte_control/package.xml index 81be6fad..9eadf111 100644 --- a/mirte_control/package.xml +++ b/mirte_control/package.xml @@ -50,24 +50,27 @@ catkin - roscpp - std_msgs - mirte_msgs - controller_manager - hardware_interface - trajectory_msgs - roscpp - std_msgs + roscpp + std_msgs + mirte_msgs + controller_manager + hardware_interface + trajectory_msgs + diff_drive_controller + effort_controllers + message_runtime + + + - roscpp + From 2e08a481ff073218efe0929cc69722125e6f9a06 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 24 Oct 2023 16:54:45 +0200 Subject: [PATCH 04/77] Fix workflow to make it fail when not good --- .github/workflows/catkin-lint.yml | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/.github/workflows/catkin-lint.yml b/.github/workflows/catkin-lint.yml index c5c911ed..e50bfbaf 100644 --- a/.github/workflows/catkin-lint.yml +++ b/.github/workflows/catkin-lint.yml @@ -7,12 +7,6 @@ jobs: image: ros:noetic steps: - uses: actions/checkout@v3 - - run: echo "catkin_lint" > ./requirements.txt - # - uses: actions/setup-python@v4 - # with: - # python-version: '3.9' - # cache: 'pip' # caching pip dependencies - # - run: apt install python-pip - run: apt update && apt install catkin-lint -y - run: | mkdir -p catkin_ws/src @@ -32,13 +26,13 @@ jobs: . devel/setup.sh - run: | . catkin_ws/devel/setup.sh - failure=0 - catkin_lint --explain --ignore uninstalled_target --pkg mirte_bringup || failure=1 - catkin_lint --explain --ignore uninstalled_target --pkg mirte_control || failure=1 - catkin_lint --explain --ignore uninstalled_target --pkg mirte_msgs || failure=1 - catkin_lint --explain --ignore uninstalled_target --pkg mirte_telemetrix || failure=1 - catkin_lint --explain --ignore uninstalled_target --pkg mirte_teleop || failure=1 - if failure; then + failure=false + catkin_lint --explain --ignore uninstalled_target --pkg mirte_bringup || failure=true + catkin_lint --explain --ignore uninstalled_target --pkg mirte_control || failure=true + catkin_lint --explain --ignore uninstalled_target --pkg mirte_msgs || failure=true + catkin_lint --explain --ignore uninstalled_target --pkg mirte_telemetrix || failure=true + catkin_lint --explain --ignore uninstalled_target --pkg mirte_teleop || failure=true + if $failure; then echo "Automated tests failed" >&2 exit 1 else From 41e512cd48db54485686528951353716542f0f42 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 24 Oct 2023 16:57:20 +0200 Subject: [PATCH 05/77] Fix mirte_msgs --- mirte_msgs/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mirte_msgs/CMakeLists.txt b/mirte_msgs/CMakeLists.txt index 1d342a55..d468f4d0 100644 --- a/mirte_msgs/CMakeLists.txt +++ b/mirte_msgs/CMakeLists.txt @@ -49,14 +49,14 @@ find_package(catkin REQUIRED COMPONENTS ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder - add_message_files( +add_message_files( FILES Encoder.msg color.msg Intensity.msg IntensityDigital.msg Keypad.msg - ) +) ## Generate services in the 'srv' folder add_service_files( From 08159f9665d4edf5ed9a80778cb310577a3bd7dc Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 24 Oct 2023 17:00:57 +0200 Subject: [PATCH 06/77] Fix mirte_telemetrix --- .github/workflows/catkin-lint.yml | 10 +++++----- mirte_telemetrix/CMakeLists.txt | 1 - mirte_telemetrix/package.xml | 1 + 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/catkin-lint.yml b/.github/workflows/catkin-lint.yml index e50bfbaf..ee8411ff 100644 --- a/.github/workflows/catkin-lint.yml +++ b/.github/workflows/catkin-lint.yml @@ -27,11 +27,11 @@ jobs: - run: | . catkin_ws/devel/setup.sh failure=false - catkin_lint --explain --ignore uninstalled_target --pkg mirte_bringup || failure=true - catkin_lint --explain --ignore uninstalled_target --pkg mirte_control || failure=true - catkin_lint --explain --ignore uninstalled_target --pkg mirte_msgs || failure=true - catkin_lint --explain --ignore uninstalled_target --pkg mirte_telemetrix || failure=true - catkin_lint --explain --ignore uninstalled_target --pkg mirte_teleop || failure=true + catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_bringup || failure=true + catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_control || failure=true + catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_msgs || failure=true + catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_telemetrix || failure=true + catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_teleop || failure=true if $failure; then echo "Automated tests failed" >&2 exit 1 diff --git a/mirte_telemetrix/CMakeLists.txt b/mirte_telemetrix/CMakeLists.txt index 456c6b1a..78bd4f0e 100644 --- a/mirte_telemetrix/CMakeLists.txt +++ b/mirte_telemetrix/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs mirte_msgs - roslaunch ) ## System dependencies are found with CMake's conventions diff --git a/mirte_telemetrix/package.xml b/mirte_telemetrix/package.xml index e04b23bf..7cd969f5 100644 --- a/mirte_telemetrix/package.xml +++ b/mirte_telemetrix/package.xml @@ -55,6 +55,7 @@ mirte_msgs rospy std_msgs + mirte_msgs rospy std_msgs From 75e5e516f02a61aa7bd3012d60e9e400ca49e347 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 24 Oct 2023 17:08:13 +0200 Subject: [PATCH 07/77] Fix mirte_teleop --- .github/workflows/catkin-lint.yml | 15 ++++++++------- mirte_teleop/CMakeLists.txt | 3 +++ 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/.github/workflows/catkin-lint.yml b/.github/workflows/catkin-lint.yml index ee8411ff..d97014a7 100644 --- a/.github/workflows/catkin-lint.yml +++ b/.github/workflows/catkin-lint.yml @@ -1,4 +1,4 @@ -name: Catkin_lint ros style check +name: Compile and catkin_lint ros style check on: [push, pull_request] jobs: check_ros_style: @@ -6,7 +6,7 @@ jobs: container: image: ros:noetic steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - run: apt update && apt install catkin-lint -y - run: | mkdir -p catkin_ws/src @@ -27,11 +27,12 @@ jobs: - run: | . catkin_ws/devel/setup.sh failure=false - catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_bringup || failure=true - catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_control || failure=true - catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_msgs || failure=true - catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_telemetrix || failure=true - catkin_lint --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_teleop || failure=true + # Run all tests before failing to test all packages + catkin_lint --strict --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_bringup || failure=true + catkin_lint --strict --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_control || failure=true + catkin_lint --strict --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_msgs || failure=true + catkin_lint --strict --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_telemetrix || failure=true + catkin_lint --strict --explain --ignore uninstalled_target --ignore uninstalled_script --pkg mirte_teleop || failure=true if $failure; then echo "Automated tests failed" >&2 exit 1 diff --git a/mirte_teleop/CMakeLists.txt b/mirte_teleop/CMakeLists.txt index a5bf3989..ebb866c6 100644 --- a/mirte_teleop/CMakeLists.txt +++ b/mirte_teleop/CMakeLists.txt @@ -9,6 +9,9 @@ add_compile_options(-std=c++11) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS ) +catkin_package( + CATKIN_DEPENDS +) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) From db2f0bfa08f6f3a33b79632f7c71307df6a0eca8 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 14 Nov 2023 11:58:54 +0100 Subject: [PATCH 08/77] start mirte master config --- .../config/mirte_master_config.yaml | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 mirte_telemetrix/config/mirte_master_config.yaml diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml new file mode 100644 index 00000000..ffe02ca7 --- /dev/null +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -0,0 +1,26 @@ +device: + mirte: + type: pcb + version: 0.6 + max_frequency: 50 +module: + motorservocontroller: + type: PCA9685 + id: 0x40 + # frequency: 1600 + motors: + left_front: + pin_A: 8 + pin_B: 9 + left_rear: + pin_A: 10 + pin_B: 11 + right_front: + pin_A: 12 + pin_B: 13 + right_rear: + pin_A: 14 + pin_B: 15 + servos: + servo1: + pin: 1 \ No newline at end of file From 91096f3797b3954cd4fa10d5af8cfa5adbdbffde Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 14 Nov 2023 13:08:16 +0100 Subject: [PATCH 09/77] start modules --- mirte_telemetrix/config/mirte_master_config.yaml | 1 + mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 12 ++++++++++++ 2 files changed, 13 insertions(+) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index ffe02ca7..a1ede025 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -5,6 +5,7 @@ device: max_frequency: 50 module: motorservocontroller: + device: mirte type: PCA9685 id: 0x40 # frequency: 1600 diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index ec99a643..71694374 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -932,6 +932,8 @@ def actuators(loop, board, device): servo = Servo(board, servos[servo]) servers.append(loop.create_task(servo.start())) + if rospy.has_param("/mirte/modules"): + servers += add_modules(rospy.get_param("/mirte/modules")) # Set a raw pin value server = rospy.Service("/mirte/set_pin_value", SetPinValue, handle_set_pin_value) @@ -1036,6 +1038,16 @@ def sensors(loop, board, device): return tasks +def add_modules(modules, device) -> []: + # pca9685 module: + modules = {k: v for k, v in modules.items() if v["device"] == device} + for module in modules: + print(module) + + return [] + + + # Shutdown procedure closing = False From a3d01ae4b4270d71ef210921fcb8a04f284af8f3 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 14 Nov 2023 13:25:58 +0000 Subject: [PATCH 10/77] Add pca motor --- .../scripts/ROS_telemetrix_aio_api.py | 83 +++++++++++++++++-- mirte_telemetrix/scripts/mappings/pcb.py | 1 - 2 files changed, 78 insertions(+), 6 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 71694374..f38d624e 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -933,7 +933,7 @@ def actuators(loop, board, device): servers.append(loop.create_task(servo.start())) if rospy.has_param("/mirte/modules"): - servers += add_modules(rospy.get_param("/mirte/modules")) + servers += add_modules(rospy.get_param("/mirte/modules"), device) # Set a raw pin value server = rospy.Service("/mirte/set_pin_value", SetPinValue, handle_set_pin_value) @@ -1039,14 +1039,87 @@ def sensors(loop, board, device): return tasks def add_modules(modules, device) -> []: + print("add modules") + tasks = [] # pca9685 module: - modules = {k: v for k, v in modules.items() if v["device"] == device} - for module in modules: - print(module) + module_names = {k for k, v in modules.items() if v["device"] == device} + for module_name in module_names: + print(module_name, modules[module_name]) + module = modules[module_name] + if(module["type"].lower() == "pca9685"): + pca_module = PCA9685(board, module_name, module) + tasks.append(loop.create_task(pca_module.start())) + + return tasks + + +def sign(i): + if i == 0: + return 0 + return i/abs(i) + +class PCA_Motor(Motor): + def __init__(self, motor_name, motor_obj, pca_update_func): + self.pca_update_func = pca_update_func + self.pin_A = motor_obj["pin_A"] + self.pin_B = motor_obj["pin_B"] + self.name = motor_name + self.prev_motor_speed = 0 + self.inverted = motor_obj["inverted"] if "inverted" in motor_obj else False - return [] + async def init_motors(self, speed): + pass + + async def set_speed(self, speed): + if self.inverted: + speed = -speed + if self.prev_motor_speed != speed: + change_dir = sign(self.prev_motor_speed) != sign(speed) + if(change_dir): # stop the motor before sending out new values if + await self.pca_update_func(self.pin_A, 0) + await self.pca_update_func(self.pin_B, 0) + if speed == 0: + await self.pca_update_func(self.pin_A, 0) + await self.pca_update_func(self.pin_B, 0) + elif speed > 0: + await self.pca_update_func(self.pin_A, int(min(speed, 100) / 100.0 * 4095)) + elif speed < 0: + await self.pca_update_func(self.pin_B, int(min(-speed, 100) / 100.0 * 4095)) + self.prev_motor_speed = speed +class PCA9685: + def __init__(self, board, module_name, module): + self.name = module_name + self.module = module + self.board = board + self.motors = {} + async def start(self): + # setup i2c, check with oled to not init twice + if board_mapping.get_mcu() == "pico": + if "connector" in self.module: + pins = board_mapping.connector_to_pins(self.module["connector"]) + else: + pins = self.module["pins"] + pin_numbers = {} + for item in pins: + pin_numbers[item] = board_mapping.pin_name_to_pin_number(pins[item]) + self.i2c_port = board_mapping.get_I2C_port(pin_numbers["sda"]) + await self.board.set_pin_mode_i2c( + i2c_port=self.i2c_port, + sda_gpio=pin_numbers["sda"], + scl_gpio=pin_numbers["scl"], + ) + # setup pca + self.write_pca = await self.board.modules.add_pca9685(self.i2c_port) + # create motors + for motor_name in self.module["motors"]: + print("motor", motor_name) + self.motors[motor_name] = PCA_Motor(motor_name, self.module["motors"][motor_name], self.write_pca) + await self.motors[motor_name].start() + # create servos + + pass # Shutdown procedure diff --git a/mirte_telemetrix/scripts/mappings/pcb.py b/mirte_telemetrix/scripts/mappings/pcb.py index 1ec34564..0e7392c1 100644 --- a/mirte_telemetrix/scripts/mappings/pcb.py +++ b/mirte_telemetrix/scripts/mappings/pcb.py @@ -1,6 +1,5 @@ import mappings.pico import mappings.nanoatmega328 -import mappings.stm32 import mappings.blackpill_f103c8 mirte_pico_pcb_map06 = { From dd02ea45cfe8f4951884c1a26e910718e9328e39 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 14 Nov 2023 13:36:50 +0000 Subject: [PATCH 11/77] start servo --- .../scripts/ROS_telemetrix_aio_api.py | 43 +++++++++++++++++-- 1 file changed, 39 insertions(+), 4 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index f38d624e..8e64c072 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1058,6 +1058,31 @@ def sign(i): return 0 return i/abs(i) +class PCA_Servo(): + def __init__(self, servo_name, servo_obj, pca_update_func): + + self.pin = servo_obj["pin"] + self.name = servo_obj["name"] + self.min_pulse = 544 + if "min_pulse" in servo_obj: + self.min_pulse = servo_obj["min_pulse"] + self.max_pulse = 2400 + if "max_pulse" in servo_obj: + self.max_pulse = servo_obj["max_pulse"] + + async def start(self): + await self.pca_update_func(self.pin, 0, 0) + server = rospy.Service( + "/mirte/set_" + self.name + "_servo_angle", + SetServoAngle, + self.set_servo_angle_service, + ) + async def servo_write(self, angle): + pass + def set_servo_angle_service(self, req): + asyncio.run(board.servo_write(self.pins["pin"], req.angle)) + return SetServoAngleResponse(True) + class PCA_Motor(Motor): def __init__(self, motor_name, motor_obj, pca_update_func): self.pca_update_func = pca_update_func @@ -1067,8 +1092,14 @@ def __init__(self, motor_name, motor_obj, pca_update_func): self.prev_motor_speed = 0 self.inverted = motor_obj["inverted"] if "inverted" in motor_obj else False - async def init_motors(self, speed): - pass + async def start(): + await Motor.start(self) + await self.init_motors(0) + + async def init_motors(self): + print("init motor") + await self.pca_update_func(self.pin_A, 0) + await self.pca_update_func(self.pin_B, 0) async def set_speed(self, speed): @@ -1076,7 +1107,7 @@ async def set_speed(self, speed): speed = -speed if self.prev_motor_speed != speed: change_dir = sign(self.prev_motor_speed) != sign(speed) - if(change_dir): # stop the motor before sending out new values if + if(change_dir): # stop the motor before sending out new values if changing direction await self.pca_update_func(self.pin_A, 0) await self.pca_update_func(self.pin_B, 0) if speed == 0: @@ -1093,6 +1124,7 @@ def __init__(self, board, module_name, module): self.module = module self.board = board self.motors = {} + self.servos= {} async def start(self): # setup i2c, check with oled to not init twice @@ -1118,8 +1150,11 @@ async def start(self): self.motors[motor_name] = PCA_Motor(motor_name, self.module["motors"][motor_name], self.write_pca) await self.motors[motor_name].start() # create servos + for servo_name in self.module["servos"]: + servo_obj = self.module["servos"][servo_name] + self.servos[servo_name] = PCA_Servo(servo_name, servo_obj, self.write_pca) + await self.servos[servo_name].start() - pass # Shutdown procedure From 502a6f12a2574ca93ddcb2317a9ee6fe1a2aa3bf Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 14 Nov 2023 14:23:40 +0000 Subject: [PATCH 12/77] fix servo --- .../scripts/ROS_telemetrix_aio_api.py | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 8e64c072..8dbc1359 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1058,18 +1058,27 @@ def sign(i): return 0 return i/abs(i) +def scale(val, src, dst): + """ + Scale the given value from the scale of src to the scale of dst. + """ + return ((val - src[0]) / (src[1]-src[0])) * (dst[1]-dst[0]) + dst[0] + class PCA_Servo(): def __init__(self, servo_name, servo_obj, pca_update_func): self.pin = servo_obj["pin"] - self.name = servo_obj["name"] + self.name = servo_name + self.pca_update_func = pca_update_func self.min_pulse = 544 if "min_pulse" in servo_obj: self.min_pulse = servo_obj["min_pulse"] + self.min_pulse = int(scale(self.min_pulse, [0,1_000_000/50], [0, 4095])) self.max_pulse = 2400 if "max_pulse" in servo_obj: self.max_pulse = servo_obj["max_pulse"] - + self.max_pulse = int(scale(self.max_pulse, [0,1_000_000/50], [0, 4095])) + print(self.min_pulse, self.max_pulse) async def start(self): await self.pca_update_func(self.pin, 0, 0) server = rospy.Service( @@ -1078,9 +1087,12 @@ async def start(self): self.set_servo_angle_service, ) async def servo_write(self, angle): - pass + pwm = int(scale(angle, [0,180], [self.min_pulse, self.max_pulse])) + print(self.name, pwm) + await self.pca_update_func(self.pin, pwm, 0) + def set_servo_angle_service(self, req): - asyncio.run(board.servo_write(self.pins["pin"], req.angle)) + asyncio.run(self.servo_write(req.angle)) return SetServoAngleResponse(True) class PCA_Motor(Motor): @@ -1092,9 +1104,9 @@ def __init__(self, motor_name, motor_obj, pca_update_func): self.prev_motor_speed = 0 self.inverted = motor_obj["inverted"] if "inverted" in motor_obj else False - async def start(): + async def start(self): await Motor.start(self) - await self.init_motors(0) + await self.init_motors() async def init_motors(self): print("init motor") From 7be4695a9be0d8f312035986cc1672975fcf6701 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 14 Nov 2023 15:11:33 +0000 Subject: [PATCH 13/77] Fixes pca --- .../scripts/ROS_telemetrix_aio_api.py | 37 ++++++++++++------- 1 file changed, 23 insertions(+), 14 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 8dbc1359..94a2d1de 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1065,8 +1065,8 @@ def scale(val, src, dst): return ((val - src[0]) / (src[1]-src[0])) * (dst[1]-dst[0]) + dst[0] class PCA_Servo(): - def __init__(self, servo_name, servo_obj, pca_update_func): - + + def __init__(self, servo_name, servo_obj, pca_update_func): self.pin = servo_obj["pin"] self.name = servo_name self.pca_update_func = pca_update_func @@ -1078,7 +1078,7 @@ def __init__(self, servo_name, servo_obj, pca_update_func): if "max_pulse" in servo_obj: self.max_pulse = servo_obj["max_pulse"] self.max_pulse = int(scale(self.max_pulse, [0,1_000_000/50], [0, 4095])) - print(self.min_pulse, self.max_pulse) + async def start(self): await self.pca_update_func(self.pin, 0, 0) server = rospy.Service( @@ -1086,9 +1086,9 @@ async def start(self): SetServoAngle, self.set_servo_angle_service, ) + async def servo_write(self, angle): pwm = int(scale(angle, [0,180], [self.min_pulse, self.max_pulse])) - print(self.name, pwm) await self.pca_update_func(self.pin, pwm, 0) def set_servo_angle_service(self, req): @@ -1109,7 +1109,6 @@ async def start(self): await self.init_motors() async def init_motors(self): - print("init motor") await self.pca_update_func(self.pin_A, 0) await self.pca_update_func(self.pin_B, 0) @@ -1130,6 +1129,7 @@ async def set_speed(self, speed): elif speed < 0: await self.pca_update_func(self.pin_B, int(min(-speed, 100) / 100.0 * 4095)) self.prev_motor_speed = speed + class PCA9685: def __init__(self, board, module_name, module): self.name = module_name @@ -1154,18 +1154,27 @@ async def start(self): sda_gpio=pin_numbers["sda"], scl_gpio=pin_numbers["scl"], ) + frequency = 200 + if "frequency" in self.module: + frequency = self.module["frequency"] + if "servos" in self.module: # servos need to run at 50Hz + frequency = 50 + id = 0x40 # default pca id + if "id" in self.module: + id = self.module["id"] # setup pca - self.write_pca = await self.board.modules.add_pca9685(self.i2c_port) + self.write_pca = await self.board.modules.add_pca9685(self.i2c_port, id, frequency) # create motors - for motor_name in self.module["motors"]: - print("motor", motor_name) - self.motors[motor_name] = PCA_Motor(motor_name, self.module["motors"][motor_name], self.write_pca) - await self.motors[motor_name].start() + if "motors" in self.module: + for motor_name in self.module["motors"]: + self.motors[motor_name] = PCA_Motor(motor_name, self.module["motors"][motor_name], self.write_pca) + await self.motors[motor_name].start() # create servos - for servo_name in self.module["servos"]: - servo_obj = self.module["servos"][servo_name] - self.servos[servo_name] = PCA_Servo(servo_name, servo_obj, self.write_pca) - await self.servos[servo_name].start() + if "servos" in self.module: + for servo_name in self.module["servos"]: + servo_obj = self.module["servos"][servo_name] + self.servos[servo_name] = PCA_Servo(servo_name, servo_obj, self.write_pca) + await self.servos[servo_name].start() From 9808f33587d083015fb5db28e5b49b09e4ee6d7e Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 14 Nov 2023 15:30:46 +0000 Subject: [PATCH 14/77] start ina --- .../scripts/ROS_telemetrix_aio_api.py | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 94a2d1de..da196edd 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1049,6 +1049,9 @@ def add_modules(modules, device) -> []: if(module["type"].lower() == "pca9685"): pca_module = PCA9685(board, module_name, module) tasks.append(loop.create_task(pca_module.start())) + if(module["type"].lower() == "ina226"): + ina_module = INA226(board, module_name, module) + tasks.append(loop.create_task(ina_module.start())) return tasks @@ -1176,7 +1179,55 @@ async def start(self): self.servos[servo_name] = PCA_Servo(servo_name, servo_obj, self.write_pca) await self.servos[servo_name].start() +class INA226(): + def __init__(self, board, module_name, module): + self.name = module_name + self.module = module + self.board = board + self.min_voltage = module["min_voltage"] if "min_voltage" in module else -1 + self.max_voltage = module["max_voltage"] if "max_voltage" in module else -1 + self.max_current = module["max_current"] if "max_current" in module else -1 + + async def start(self): + print("start ina!") + # setup i2c, check with oled to not init twice + if board_mapping.get_mcu() == "pico": + if "connector" in self.module: + pins = board_mapping.connector_to_pins(self.module["connector"]) + else: + pins = self.module["pins"] + pin_numbers = {} + for item in pins: + pin_numbers[item] = board_mapping.pin_name_to_pin_number(pins[item]) + self.i2c_port = board_mapping.get_I2C_port(pin_numbers["sda"]) + try: + await self.board.set_pin_mode_i2c( + i2c_port=self.i2c_port, + sda_gpio=pin_numbers["sda"], + scl_gpio=pin_numbers["scl"], + ) + except Exception as e: + pass + id = 0x40 # default ina id + if "id" in self.module: + id = self.module["id"] + + await self.board.sensors.add_ina226(self.i2c_port, self.callback, id) + async def callback(self,data): + # TODO: move this to the library + ints = list( map(lambda i:i.to_bytes(1, 'big'), data)) + bytes_obj = b''.join(ints) + vals = list(struct.unpack('<2f', bytes_obj)) + self.voltage = vals[0] + self.current = vals[1] + if self.min_voltage != -1 and self.voltage < self.min_voltage: + print("TOO low voltage!") + if self.max_voltage != -1 and self.voltage > self.max_voltage: + print("TOO high voltage!") + if self.max_current != -1 and self.current > self.max_current: + print("TOO high current!") + print(self.voltage, self.current) # Shutdown procedure closing = False From 2849825cfc43f20ec870362e37b55be3aa865ab5 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 14 Nov 2023 15:58:51 +0000 Subject: [PATCH 15/77] Add ina226 battery message + warnings --- .../config/mirte_master_config.yaml | 15 ++++-- .../scripts/ROS_telemetrix_aio_api.py | 50 ++++++++++++++----- 2 files changed, 49 insertions(+), 16 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index a1ede025..a9a6b037 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -3,12 +3,13 @@ device: type: pcb version: 0.6 max_frequency: 50 -module: +modules: motorservocontroller: device: mirte type: PCA9685 id: 0x40 - # frequency: 1600 + connector: I2C1 + frequency: 1500 motors: left_front: pin_A: 8 @@ -24,4 +25,12 @@ module: pin_B: 15 servos: servo1: - pin: 1 \ No newline at end of file + pin: 1 + power_watcher: + device: mirte + type: INA226 + connector: I2C1 + id: 0x41 + min_voltage: 11 + max_current: 1 + max_voltage: 15 \ No newline at end of file diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index da196edd..1fbc6a15 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -35,7 +35,7 @@ async def analog_write(board, pin, value): # Import ROS message types from std_msgs.msg import Header, Int32 -from sensor_msgs.msg import Range +from sensor_msgs.msg import Range, BatteryState from mirte_msgs.msg import * # Import ROS services @@ -642,7 +642,10 @@ async def start(self): ) for ev in self.init_awaits: - await ev + try: # catch set_pin_mode_i2c already for this port + await ev + except Exception as e: + pass for cmd in self.write_commands: out = await self.board.i2c_write(60, cmd, i2c_port=self.i2c_port) if ( @@ -1152,11 +1155,14 @@ async def start(self): for item in pins: pin_numbers[item] = board_mapping.pin_name_to_pin_number(pins[item]) self.i2c_port = board_mapping.get_I2C_port(pin_numbers["sda"]) - await self.board.set_pin_mode_i2c( - i2c_port=self.i2c_port, - sda_gpio=pin_numbers["sda"], - scl_gpio=pin_numbers["scl"], - ) + try: + await self.board.set_pin_mode_i2c( + i2c_port=self.i2c_port, + sda_gpio=pin_numbers["sda"], + scl_gpio=pin_numbers["scl"], + ) + except Exception as e: + pass frequency = 200 if "frequency" in self.module: frequency = self.module["frequency"] @@ -1187,7 +1193,9 @@ def __init__(self, board, module_name, module): self.min_voltage = module["min_voltage"] if "min_voltage" in module else -1 self.max_voltage = module["max_voltage"] if "max_voltage" in module else -1 self.max_current = module["max_current"] if "max_current" in module else -1 - + self.ina_publisher = rospy.Publisher( + "/mirte/power/" + module_name, BatteryState, queue_size=1 + ) async def start(self): print("start ina!") @@ -1215,19 +1223,35 @@ async def start(self): await self.board.sensors.add_ina226(self.i2c_port, self.callback, id) async def callback(self,data): - # TODO: move this to the library + # TODO: move this decoding to the library ints = list( map(lambda i:i.to_bytes(1, 'big'), data)) bytes_obj = b''.join(ints) vals = list(struct.unpack('<2f', bytes_obj)) self.voltage = vals[0] self.current = vals[1] if self.min_voltage != -1 and self.voltage < self.min_voltage: - print("TOO low voltage!") + rospy.logwarn("Low voltage: %f", self.voltage) if self.max_voltage != -1 and self.voltage > self.max_voltage: - print("TOO high voltage!") + rospy.logwarn("High voltage: %f", self.voltage) if self.max_current != -1 and self.current > self.max_current: - print("TOO high current!") - print(self.voltage, self.current) + rospy.logwarn("High current: %f", self.current) + bs = BatteryState() + bs.header = Header() + bs.header.stamp = rospy.Time.now() + bs.voltage = self.voltage + bs.current = self.current + bs.temperature = math.nan + bs.charge = math.nan + bs.capacity= math.nan + bs.design_capacity= math.nan + bs.percentage = math.nan + bs.power_supply_status = 0 # uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0 + bs.power_supply_health = 0 # uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 + bs.power_supply_technology = 0 # uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 + self.ina_publisher.publish(bs) + +# uint8 power_supply_health # The battery health metric. Values defined above +# uint8 power_supply_technology # The battery chemistry. Values defined above # Shutdown procedure closing = False From b510509b2027d1a5ea36607cfed686d6846e9884 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 14 Nov 2023 16:59:55 +0100 Subject: [PATCH 16/77] styling ina --- .../scripts/ROS_telemetrix_aio_api.py | 102 ++++++++++-------- 1 file changed, 60 insertions(+), 42 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 1fbc6a15..7ba14f77 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -642,7 +642,7 @@ async def start(self): ) for ev in self.init_awaits: - try: # catch set_pin_mode_i2c already for this port + try: # catch set_pin_mode_i2c already for this port await ev except Exception as e: pass @@ -1041,6 +1041,7 @@ def sensors(loop, board, device): return tasks + def add_modules(modules, device) -> []: print("add modules") tasks = [] @@ -1049,10 +1050,10 @@ def add_modules(modules, device) -> []: for module_name in module_names: print(module_name, modules[module_name]) module = modules[module_name] - if(module["type"].lower() == "pca9685"): + if module["type"].lower() == "pca9685": pca_module = PCA9685(board, module_name, module) tasks.append(loop.create_task(pca_module.start())) - if(module["type"].lower() == "ina226"): + if module["type"].lower() == "ina226": ina_module = INA226(board, module_name, module) tasks.append(loop.create_task(ina_module.start())) @@ -1062,28 +1063,29 @@ def add_modules(modules, device) -> []: def sign(i): if i == 0: return 0 - return i/abs(i) + return i / abs(i) + def scale(val, src, dst): """ Scale the given value from the scale of src to the scale of dst. """ - return ((val - src[0]) / (src[1]-src[0])) * (dst[1]-dst[0]) + dst[0] + return ((val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0] -class PCA_Servo(): - def __init__(self, servo_name, servo_obj, pca_update_func): +class PCA_Servo: + def __init__(self, servo_name, servo_obj, pca_update_func): self.pin = servo_obj["pin"] self.name = servo_name self.pca_update_func = pca_update_func self.min_pulse = 544 if "min_pulse" in servo_obj: self.min_pulse = servo_obj["min_pulse"] - self.min_pulse = int(scale(self.min_pulse, [0,1_000_000/50], [0, 4095])) + self.min_pulse = int(scale(self.min_pulse, [0, 1_000_000 / 50], [0, 4095])) self.max_pulse = 2400 if "max_pulse" in servo_obj: self.max_pulse = servo_obj["max_pulse"] - self.max_pulse = int(scale(self.max_pulse, [0,1_000_000/50], [0, 4095])) + self.max_pulse = int(scale(self.max_pulse, [0, 1_000_000 / 50], [0, 4095])) async def start(self): await self.pca_update_func(self.pin, 0, 0) @@ -1094,13 +1096,14 @@ async def start(self): ) async def servo_write(self, angle): - pwm = int(scale(angle, [0,180], [self.min_pulse, self.max_pulse])) + pwm = int(scale(angle, [0, 180], [self.min_pulse, self.max_pulse])) await self.pca_update_func(self.pin, pwm, 0) def set_servo_angle_service(self, req): asyncio.run(self.servo_write(req.angle)) return SetServoAngleResponse(True) + class PCA_Motor(Motor): def __init__(self, motor_name, motor_obj, pca_update_func): self.pca_update_func = pca_update_func @@ -1109,7 +1112,7 @@ def __init__(self, motor_name, motor_obj, pca_update_func): self.name = motor_name self.prev_motor_speed = 0 self.inverted = motor_obj["inverted"] if "inverted" in motor_obj else False - + async def start(self): await Motor.start(self) await self.init_motors() @@ -1118,31 +1121,37 @@ async def init_motors(self): await self.pca_update_func(self.pin_A, 0) await self.pca_update_func(self.pin_B, 0) - async def set_speed(self, speed): if self.inverted: speed = -speed if self.prev_motor_speed != speed: change_dir = sign(self.prev_motor_speed) != sign(speed) - if(change_dir): # stop the motor before sending out new values if changing direction + if ( + change_dir + ): # stop the motor before sending out new values if changing direction await self.pca_update_func(self.pin_A, 0) await self.pca_update_func(self.pin_B, 0) if speed == 0: await self.pca_update_func(self.pin_A, 0) await self.pca_update_func(self.pin_B, 0) elif speed > 0: - await self.pca_update_func(self.pin_A, int(min(speed, 100) / 100.0 * 4095)) + await self.pca_update_func( + self.pin_A, int(min(speed, 100) / 100.0 * 4095) + ) elif speed < 0: - await self.pca_update_func(self.pin_B, int(min(-speed, 100) / 100.0 * 4095)) + await self.pca_update_func( + self.pin_B, int(min(-speed, 100) / 100.0 * 4095) + ) self.prev_motor_speed = speed + class PCA9685: def __init__(self, board, module_name, module): self.name = module_name self.module = module self.board = board self.motors = {} - self.servos= {} + self.servos = {} async def start(self): # setup i2c, check with oled to not init twice @@ -1157,35 +1166,42 @@ async def start(self): self.i2c_port = board_mapping.get_I2C_port(pin_numbers["sda"]) try: await self.board.set_pin_mode_i2c( - i2c_port=self.i2c_port, - sda_gpio=pin_numbers["sda"], - scl_gpio=pin_numbers["scl"], - ) + i2c_port=self.i2c_port, + sda_gpio=pin_numbers["sda"], + scl_gpio=pin_numbers["scl"], + ) except Exception as e: - pass + pass frequency = 200 if "frequency" in self.module: frequency = self.module["frequency"] - if "servos" in self.module: # servos need to run at 50Hz + if "servos" in self.module: # servos need to run at 50Hz frequency = 50 - id = 0x40 # default pca id + id = 0x40 # default pca id if "id" in self.module: id = self.module["id"] # setup pca - self.write_pca = await self.board.modules.add_pca9685(self.i2c_port, id, frequency) + self.write_pca = await self.board.modules.add_pca9685( + self.i2c_port, id, frequency + ) # create motors if "motors" in self.module: for motor_name in self.module["motors"]: - self.motors[motor_name] = PCA_Motor(motor_name, self.module["motors"][motor_name], self.write_pca) + self.motors[motor_name] = PCA_Motor( + motor_name, self.module["motors"][motor_name], self.write_pca + ) await self.motors[motor_name].start() # create servos if "servos" in self.module: for servo_name in self.module["servos"]: servo_obj = self.module["servos"][servo_name] - self.servos[servo_name] = PCA_Servo(servo_name, servo_obj, self.write_pca) + self.servos[servo_name] = PCA_Servo( + servo_name, servo_obj, self.write_pca + ) await self.servos[servo_name].start() -class INA226(): + +class INA226: def __init__(self, board, module_name, module): self.name = module_name self.module = module @@ -1211,22 +1227,23 @@ async def start(self): self.i2c_port = board_mapping.get_I2C_port(pin_numbers["sda"]) try: await self.board.set_pin_mode_i2c( - i2c_port=self.i2c_port, - sda_gpio=pin_numbers["sda"], - scl_gpio=pin_numbers["scl"], - ) + i2c_port=self.i2c_port, + sda_gpio=pin_numbers["sda"], + scl_gpio=pin_numbers["scl"], + ) except Exception as e: pass - id = 0x40 # default ina id + id = 0x40 # default ina id if "id" in self.module: id = self.module["id"] - + await self.board.sensors.add_ina226(self.i2c_port, self.callback, id) - async def callback(self,data): + + async def callback(self, data): # TODO: move this decoding to the library - ints = list( map(lambda i:i.to_bytes(1, 'big'), data)) - bytes_obj = b''.join(ints) - vals = list(struct.unpack('<2f', bytes_obj)) + ints = list(map(lambda i: i.to_bytes(1, "big"), data)) + bytes_obj = b"".join(ints) + vals = list(struct.unpack("<2f", bytes_obj)) self.voltage = vals[0] self.current = vals[1] if self.min_voltage != -1 and self.voltage < self.min_voltage: @@ -1242,14 +1259,15 @@ async def callback(self,data): bs.current = self.current bs.temperature = math.nan bs.charge = math.nan - bs.capacity= math.nan - bs.design_capacity= math.nan + bs.capacity = math.nan + bs.design_capacity = math.nan bs.percentage = math.nan - bs.power_supply_status = 0 # uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0 - bs.power_supply_health = 0 # uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 - bs.power_supply_technology = 0 # uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 + bs.power_supply_status = 0 # uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0 + bs.power_supply_health = 0 # uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 + bs.power_supply_technology = 0 # uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 self.ina_publisher.publish(bs) + # uint8 power_supply_health # The battery health metric. Values defined above # uint8 power_supply_technology # The battery chemistry. Values defined above From c1537611529ab9dfd18d9b69655bca7fa84318fd Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Wed, 15 Nov 2023 14:21:45 +0100 Subject: [PATCH 17/77] start mypy --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 7ba14f77..83232c63 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -11,7 +11,7 @@ from inspect import signature from tmx_pico_aio import tmx_pico_aio from telemetrix_aio import telemetrix_aio - +from typing import Literal, Tuple # Import the right Telemetrix AIO devices = rospy.get_param("/mirte/device") @@ -1042,8 +1042,7 @@ def sensors(loop, board, device): return tasks -def add_modules(modules, device) -> []: - print("add modules") +def add_modules(modules: dict, device: dict) -> []: tasks = [] # pca9685 module: module_names = {k for k, v in modules.items() if v["device"] == device} @@ -1060,13 +1059,13 @@ def add_modules(modules, device) -> []: return tasks -def sign(i): +def sign(i: float) -> Literal[-1, 0, 1]: if i == 0: return 0 - return i / abs(i) + return 1 if i / abs(i) > 0 else -1 -def scale(val, src, dst): +def scale(val: float, src: Tuple[int, int], dst: Tuple[int, int]) -> float: """ Scale the given value from the scale of src to the scale of dst. """ From 6b7a75f6a0945d9b91339d40f55cba59ca23a3ca Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 6 Dec 2023 17:10:06 +0100 Subject: [PATCH 18/77] Add hiwonder servo --- .../config/mirte_master_config.yaml | 15 ++++ .../scripts/ROS_telemetrix_aio_api.py | 69 ++++++++++++++++++- 2 files changed, 82 insertions(+), 2 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index a9a6b037..ef9c2275 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -26,6 +26,21 @@ modules: servos: servo1: pin: 1 + servo_controller: + device: mirte + type: Hiwonder_Servo + uart: 1 + rx_pin: 4 + tx_pin: 5 + servos: + servoH1: + id: 3 + min_angle: 9000 + max_angle: 14000 + servoH2: + id: 4 + min_angle: 800 + max_angle: 4000 power_watcher: device: mirte type: INA226 diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 83232c63..2821372c 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1055,6 +1055,9 @@ def add_modules(modules: dict, device: dict) -> []: if module["type"].lower() == "ina226": ina_module = INA226(board, module_name, module) tasks.append(loop.create_task(ina_module.start())) + if module["type"].lower() == "hiwonder_servo": + servo_module = Hiwonder_Servo(board, module_name, module) + tasks.append(loop.create_task(servo_module.start())) return tasks @@ -1261,14 +1264,76 @@ async def callback(self, data): bs.capacity = math.nan bs.design_capacity = math.nan bs.percentage = math.nan + # uint8 power_supply_health # The battery health metric. Values defined above + # uint8 power_supply_technology # The battery chemistry. Values defined above bs.power_supply_status = 0 # uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0 bs.power_supply_health = 0 # uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 bs.power_supply_technology = 0 # uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 self.ina_publisher.publish(bs) -# uint8 power_supply_health # The battery health metric. Values defined above -# uint8 power_supply_technology # The battery chemistry. Values defined above +class Hiwonder_Servo: + def __init__(self, servo_name, servo_obj, write_single_servo): + self.id = servo_obj["id"] + self.name = servo_name + self.write_single_servo = write_single_servo + self.min_angle = 0 + if "min_angle" in servo_obj: + self.min_pulse = servo_obj["min_angle"] + self.max_angle = 24000 # centidegrees + if "max_angle" in servo_obj: + self.max_angle = servo_obj["max_angle"] + + async def start(self): + server = rospy.Service( + "/mirte/set_" + self.name + "_servo_angle", + SetServoAngle, + self.set_servo_angle_service, + ) + + async def servo_write(self, angle): + angle = angle *100 # centidegrees + angle = max(self.min_angle, min(angle, self.max_angle)) + await self.write_single_servo(self.id, angle, 100) + + def set_servo_angle_service(self, req): + asyncio.run(self.servo_write(req.angle)) + return SetServoAngleResponse(True) + + +class Hiwonder_Bus: + def __init__(self, board, module_name, module): + self.name = module_name + self.module = module + self.board = board + self.servos = {} + # TODO: create publisher for current state + # self.servo_publisher = rospy.Publisher( + # "/mirte/servos/" + module_name, BatteryState, queue_size=1 + # ) + + async def start(self): + print("start hiwonder!") + uart = self.module["uart"] + if(uart !=0 and uart !=1): + return + rx = self.module["rx_pin"] + tx = self.module["tx_pin"] + ids = [] + if "servos" in self.module: + for servo_name in self.module["servos"]: + servo_obj = self.module["servos"][servo_name] + self.servos[servo_name] = Hiwonder_Servo( + servo_name, servo_obj, self.write_single_servo + ) + await self.servos[servo_name].start() + + await self.board.modules.add_hiwonder_servo(uart, rx, tx, ids, self.callback) + + async def callback(self, data): + # todo: on each update, publish all, including old data of the other servos. + pass + # Shutdown procedure closing = False From 599062fb48a6e3fb05f87810e91da787ed8b655c Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 6 Dec 2023 17:11:56 +0100 Subject: [PATCH 19/77] Fix styling hiwonder servo --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 2821372c..044f8c04 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1280,7 +1280,7 @@ def __init__(self, servo_name, servo_obj, write_single_servo): self.min_angle = 0 if "min_angle" in servo_obj: self.min_pulse = servo_obj["min_angle"] - self.max_angle = 24000 # centidegrees + self.max_angle = 24000 # centidegrees if "max_angle" in servo_obj: self.max_angle = servo_obj["max_angle"] @@ -1292,7 +1292,7 @@ async def start(self): ) async def servo_write(self, angle): - angle = angle *100 # centidegrees + angle = angle * 100 # centidegrees angle = max(self.min_angle, min(angle, self.max_angle)) await self.write_single_servo(self.id, angle, 100) @@ -1315,7 +1315,7 @@ def __init__(self, board, module_name, module): async def start(self): print("start hiwonder!") uart = self.module["uart"] - if(uart !=0 and uart !=1): + if uart != 0 and uart != 1: return rx = self.module["rx_pin"] tx = self.module["tx_pin"] From b004dc02458c461718d7134b2562dc31103dfcf6 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Thu, 7 Dec 2023 11:21:18 +0000 Subject: [PATCH 20/77] fix hiwonder bus tmx --- .../scripts/ROS_telemetrix_aio_api.py | 41 +++++++++++-------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 044f8c04..e1a638bd 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1056,7 +1056,7 @@ def add_modules(modules: dict, device: dict) -> []: ina_module = INA226(board, module_name, module) tasks.append(loop.create_task(ina_module.start())) if module["type"].lower() == "hiwonder_servo": - servo_module = Hiwonder_Servo(board, module_name, module) + servo_module = Hiwonder_Bus(board, module_name, module) tasks.append(loop.create_task(servo_module.start())) return tasks @@ -1273,13 +1273,13 @@ async def callback(self, data): class Hiwonder_Servo: - def __init__(self, servo_name, servo_obj, write_single_servo): + def __init__(self, servo_name, servo_obj, bus): self.id = servo_obj["id"] self.name = servo_name - self.write_single_servo = write_single_servo + self.bus = bus self.min_angle = 0 if "min_angle" in servo_obj: - self.min_pulse = servo_obj["min_angle"] + self.min_angle = servo_obj["min_angle"] self.max_angle = 24000 # centidegrees if "max_angle" in servo_obj: self.max_angle = servo_obj["max_angle"] @@ -1290,16 +1290,21 @@ async def start(self): SetServoAngle, self.set_servo_angle_service, ) + self.publisher = rospy.Publisher(f"/mirte/servos/{self.name}/position", Int32, queue_size=1) async def servo_write(self, angle): angle = angle * 100 # centidegrees angle = max(self.min_angle, min(angle, self.max_angle)) - await self.write_single_servo(self.id, angle, 100) + await self.bus.set_single_servo(self.id, angle, 100) def set_servo_angle_service(self, req): asyncio.run(self.servo_write(req.angle)) return SetServoAngleResponse(True) + def callback(self, data): + message = Int32 + message.data = data["value"] + self.publisher.publish(message) class Hiwonder_Bus: def __init__(self, board, module_name, module): @@ -1307,13 +1312,8 @@ def __init__(self, board, module_name, module): self.module = module self.board = board self.servos = {} - # TODO: create publisher for current state - # self.servo_publisher = rospy.Publisher( - # "/mirte/servos/" + module_name, BatteryState, queue_size=1 - # ) async def start(self): - print("start hiwonder!") uart = self.module["uart"] if uart != 0 and uart != 1: return @@ -1323,16 +1323,25 @@ async def start(self): if "servos" in self.module: for servo_name in self.module["servos"]: servo_obj = self.module["servos"][servo_name] - self.servos[servo_name] = Hiwonder_Servo( - servo_name, servo_obj, self.write_single_servo + servo = Hiwonder_Servo( + servo_name, servo_obj, self ) - await self.servos[servo_name].start() + ids.append(servo.id) + await servo.start() + self.servos[servo_name] = servo + + updaters = await self.board.modules.add_hiwonder_servo(uart, rx, tx, ids, self.callback) + self.set_single_servo = updaters["set_single_servo"] + self.set_multiple_servos = updaters["set_multiple_servos"] + + # TODO: add service to update multiple servos - await self.board.modules.add_hiwonder_servo(uart, rx, tx, ids, self.callback) async def callback(self, data): - # todo: on each update, publish all, including old data of the other servos. - pass + for servo_update in data: + servos = list(filter(lambda servo:servo_update["id"]==servo.id, self.servos)) + if(len(servos) == 1): + servos[0].callback(servo_update) # Shutdown procedure From b0db1bb53bee4c34bd70ac967ac98ee2e0a4423c Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Fri, 8 Dec 2023 09:25:12 +0100 Subject: [PATCH 21/77] Add pylint --- .github/workflows/pylint.yml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 .github/workflows/pylint.yml diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml new file mode 100644 index 00000000..9ebfa29a --- /dev/null +++ b/.github/workflows/pylint.yml @@ -0,0 +1,19 @@ +name: Pylint + +on: [push] + +jobs: + linter: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v3 + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pylint + - name: Analysing the code with pylint + run: | + pylint --fail-under=4 $(git ls-files '*.py') From 35190c51f681ad9dfd83ba67b6406dd0ae1d3cec Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Fri, 19 Jan 2024 13:18:10 +0100 Subject: [PATCH 22/77] add mirte-control_master --- .gitignore | 4 + mirte_control_master/CHANGELOG.rst | 99 ++++++ mirte_control_master/CMakeLists.txt | 205 ++++++++++++ mirte_control_master/config/control.yaml | 68 ++++ .../config/my_robot_common.yaml | 59 ++++ .../config/my_robot_control.yaml | 23 ++ .../include/my_robot_hw_interface.h | 304 ++++++++++++++++++ mirte_control_master/launch/control.launch | 12 + mirte_control_master/package.xml | 50 +++ mirte_control_master/src/my_robot_base.cpp | 43 +++ 10 files changed, 867 insertions(+) create mode 100644 mirte_control_master/CHANGELOG.rst create mode 100644 mirte_control_master/CMakeLists.txt create mode 100644 mirte_control_master/config/control.yaml create mode 100644 mirte_control_master/config/my_robot_common.yaml create mode 100644 mirte_control_master/config/my_robot_control.yaml create mode 100644 mirte_control_master/include/my_robot_hw_interface.h create mode 100644 mirte_control_master/launch/control.launch create mode 100644 mirte_control_master/package.xml create mode 100644 mirte_control_master/src/my_robot_base.cpp diff --git a/.gitignore b/.gitignore index 407f5f3f..a6699d10 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,6 @@ __pycache__ .vscode/ +rb/ +old/ +build/ +ridgeback_control/ \ No newline at end of file diff --git a/mirte_control_master/CHANGELOG.rst b/mirte_control_master/CHANGELOG.rst new file mode 100644 index 00000000..e8a38a34 --- /dev/null +++ b/mirte_control_master/CHANGELOG.rst @@ -0,0 +1,99 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ridgeback_base +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2022-05-17) +------------------ + +0.3.1 (2021-06-15) +------------------ + +0.3.0 (2020-11-12) +------------------ +* [ridgeback_base] Added dependency. +* [ridgeback_base] Updated to use new functions from puma_motor_drivers. +* [ridgeback_base] Used ros::ok() for while condition on thread loops. +* Updates for C++11. +* Contributors: Tony Baltovski + +0.2.6 (2020-11-12) +------------------ +* Bump CMake version to avoid CMP0048 warning. +* [ridgeback_base] Fixed missing forward slash. +* Contributors: Tony Baltovski + +0.2.5 (2020-10-19) +------------------ +* Use eval + find to properly load the default mag config file +* Added RIDGEBACK_MAG_CONFIG to madgwick filter and set a default optenv +* Removed env-hooks +* Contributors: Chris Iverach-Brereton, Dave Niewinski + +0.2.4 (2019-11-22) +------------------ +* Merge pull request `#26 `_ from ridgeback/RPSW-119 + Added the ability to disable using the MCU +* [ridgeback_base] Added param type for use_mcu. +* [ridgeback_base] Added lighting and cooling under use_mcu flag. +* [ridgeback_base] Added envar for using MCU. +* Contributors: Tony Baltovski + +0.2.3 (2019-03-23) +------------------ +* [ridgeback_base] Updated compute_calibration to use a MagneticField message. +* Contributors: Tony Baltovski + +0.2.1 (2018-08-02) +------------------ + +0.2.0 (2018-05-23) +------------------ +* [ridgeback_base] Added absolute value checking for fans. +* Updated to package format 2. +* [ridgeback_base] Switched to rosserial_server_udp. +* Updated bringup for kinetic +* [ridgeback_base] Fixed a typo. +* Updated maintainer. +* Fixed temperature warning for PCB and MCU temperature. +* Contributors: Dave Niewinski, Tony Baltovski + +0.1.7 (2016-10-03) +------------------ +* Removed GPS dependencies. +* Contributors: Johannes Meyer, Tony Baltovski + +0.1.6 (2016-04-22) +------------------ +* Combined hostname and version for hardware ID. +* Fixed initialization of constant doubles. +* Added hardware ID based hostname. +* Contributors: Tony Baltovski + +0.1.5 (2016-03-02) +------------------ +* Fixed state order for lighting. +* Separated passive joint into header. +* Contributors: Tony Baltovski + +0.1.4 (2015-12-01) +------------------ + +0.1.3 (2015-11-20) +------------------ +* Install the ridgeback_node target. +* Contributors: Mike Purvis + +0.1.2 (2015-11-20) +------------------ +* Fixed dependency. +* Contributors: Tony Baltovski + +0.1.1 (2015-11-20) +------------------ +* Added lighting. +* Contributors: Tony Baltovski + +0.1.0 (2015-11-19) +------------------ +* Initial Ridgeback release. +* Contributors: Mike Purvis, Tony Baltovski diff --git a/mirte_control_master/CMakeLists.txt b/mirte_control_master/CMakeLists.txt new file mode 100644 index 00000000..166fac42 --- /dev/null +++ b/mirte_control_master/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(mirte_control_master) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + mirte_msgs + controller_manager + hardware_interface + +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +#add_message_files( +# FILES +# MotorCommand.msg +#) + +## Generate services in the 'srv' folder +#add_service_files( + # FILES + # get_distance.srv +#) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +#generate_messages( +# DEPENDENCIES +# std_msgs +#) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES mirte_ros_package + CATKIN_DEPENDS roscpp std_msgs mirte_msgs message_runtime controller_manager hardware_interface +# DEPENDS system_lib +) + + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include/ + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/mobile_controller.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node + src/my_robot_base.cpp +) + +add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variablese +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mirte_ros_package.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mirte_control_master/config/control.yaml b/mirte_control_master/config/control.yaml new file mode 100644 index 00000000..7538f13a --- /dev/null +++ b/mirte_control_master/config/control.yaml @@ -0,0 +1,68 @@ +ridgeback_joint_publisher: + type: "joint_state_controller/JointStateController" + publish_rate: 50 + +ridgeback_velocity_controller: + type: "mecanum_drive_controller/MecanumDriveController" + front_left_wheel_joint: "front_left_wheel" + back_left_wheel_joint: "rear_left_wheel" + front_right_wheel_joint: "front_right_wheel" + back_right_wheel_joint: "rear_right_wheel" + publish_rate: 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03] + cmd_vel_timeout: 0.25 + + # Override URDF look-up for wheel separation since the parent link is not the base_link. + wheel_separation_x: 0.638 + wheel_separation_y: 0.551 + + # Odometry fused with IMU is published by robot_localization, so + # no need to publish a TF based on encoders alone. + enable_odom_tf: false + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 1.1 # m/s + has_acceleration_limits: true + max_acceleration : 2.5 # m/s^2 + y: + has_velocity_limits : true + max_velocity : 1.1 # m/s + has_acceleration_limits: true + max_acceleration : 2.5 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 2.0 # rad/s + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + +# ekf_localization: +# frequency: 50 +# two_d_mode: true +# odom0: /ridgeback_velocity_controller/odom +# odom0_config: [false, false, false, +# false, false, false, +# true, true, false, +# false, false, true, +# false, false, false] +# odom0_differential: false +# imu0: /imu/data +# imu0_config: [false, false, false, +# false, false, false, +# false, false, false, +# false, false, true, +# true, true, false] +# imu0_differential: false +# odom_frame: odom +# base_link_frame: base_link +# world_frame: odom +# predict_to_current_time: true diff --git a/mirte_control_master/config/my_robot_common.yaml b/mirte_control_master/config/my_robot_common.yaml new file mode 100644 index 00000000..ace0de05 --- /dev/null +++ b/mirte_control_master/config/my_robot_common.yaml @@ -0,0 +1,59 @@ +mobile_base_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'wheel_0_joint' + right_wheel : 'wheel_1_joint' + publish_rate: 50.0 # default: 50 + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + #pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] + #twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.125 # meters + wheel_radius : 0.003 # meters + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + # This setting also works together with the repeat_rate and key_timeout parameters + # from teleop_twist_keyboard. + cmd_vel_timeout: 0.5 + + # Base frame_id + base_frame_id: base_footprint #default: base_link + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + #max_velocity : 0.42 # m/s at 5V + max_velocity : 0.55 # m/s at 6V +# min_velocity : -0.42 #-0.5 # m/s + has_acceleration_limits: false + #max_acceleration : 0.42 #0.8 # m/s^2 + #min_acceleration : -0.42 #-0.4 # m/s^2 + has_jerk_limits : false + # max_jerk : 5.0 # m/s^3 + angular: + z: + has_velocity_limits : true + max_velocity : 2 #1.7 # rad/s + has_acceleration_limits: false + #max_acceleration : 8 # rad/s^2 + has_jerk_limits : false + #max_jerk : 2.5 # rad/s^3 + +#Publish to TF directly or not +enable_odom_tf: true + +#Name of frame to publish odometry in +odom_frame_id: odom + +# Publish the velocity command to be executed. +# It is to monitor the effect of limiters on the controller input. +publish_cmd: true diff --git a/mirte_control_master/config/my_robot_control.yaml b/mirte_control_master/config/my_robot_control.yaml new file mode 100644 index 00000000..b8386d0f --- /dev/null +++ b/mirte_control_master/config/my_robot_control.yaml @@ -0,0 +1,23 @@ +# .yaml config file +# +# The PID gains and controller settings must be saved in a yaml file that gets loaded +# to the param server via the roslaunch file (my_robot_control.launch). + +my_robot: + # Publish all joint states ----------------------------------- + # Creates the /joint_states topic necessary in ROS + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Effort Controllers --------------------------------------- + leftWheel_effort_controller: + type: effort_controllers/JointEffortController + joint: left_wheel_hinge + pid: {p: 100.0, i: 0.1, d: 10.0} + #pid: {p: 50.0, i: 0.1, d: 0.0} + rightWheel_effort_controller: + type: effort_controllers/JointEffortController + joint: right_wheel_hinge + pid: {p: 100.0, i: 0.1, d: 10.0} + #pid: {p: 1.0, i: 1.0, d: 0.0} diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h new file mode 100644 index 00000000..be32868d --- /dev/null +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -0,0 +1,304 @@ +// https://slaterobots.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot +// Roughly based on: +// https://github.com/eborghi10/my_ROS_mobile_robot/blob/master/my_robot_base/include/my_robot_hw_interface.h +// https://github.com/PickNikRobotics/ros_control_boilerplate +// https://github.com/DeborggraeveR/ampru + +// https://github.com/resibots/dynamixel_control_hw/blob/master/include/dynamixel_control_hw/hardware_interface.hpp +// https://github.com/FRC900/2018RobotCode/blob/master/zebROS_ws/src/ros_control_boilerplate/include/ros_control_boilerplate/frcrobot_hw_interface.h +// INTERESTING CLEAN ONE: +// https://github.com/ros-controls/ros_controllers/blob/indigo-devel/diff_drive_controller/test/diffbot.h + +#pragma once +#define _USE_MATH_DEFINES + +// ROS +#include +#include +#include +#include + +// ros_control +#include +#include +#include +#include + +// ostringstream +#include +#include +#include + +#include +#include +#include +#include + +const unsigned int NUM_JOINTS = 4; + +/// \brief Hardware interface for a robot +class MyRobotHWInterface : public hardware_interface::RobotHW +{ +public: + MyRobotHWInterface(); + + bool write_single(int joint, int speed) + { + int speed_mapped = + std::max(std::min(int(cmd[joint] / (6 * M_PI) * 100), 100), -100); + if (speed_mapped != _last_cmd[joint]) + { + service_requests[joint].request.speed = speed_mapped; + _last_cmd[joint] = speed_mapped; + if (!service_clients[joint].call(service_requests[joint])) + { + this->start_reconnect(); + return false; + } + } + return true; + } + /* + * + */ + void write() + { + if (running_) + { + // make sure the clients don't get overwritten while calling them + const std::lock_guard lock(this->service_clients_mutex); + + // cmd[0] = ros_control calculated speed of left motor in rad/s + // cmd[1] = ros_control calculated speed of right motor in rad/s + + // This function converts cmd[0] to pwm and calls that service + + // NOTE: this *highly* depends on the voltage of the motors!!!! + // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) + // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s + // (6*pi) + for (int i = 0; i < NUM_JOINTS; i++) + { + if (!write_single(i, cmd[i])) + { + return; + } + } + // Set the direction in so the read() can use it + // TODO: this does not work properly, because at the end of a series + // cmd_vel is negative, while the rotation is not + for (int i = 0; i < NUM_JOINTS; i++) + { + _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; + } + } + } + + /** + * Reading encoder values and setting position and velocity of encoders + */ + void read(const ros::Duration &period) + { + //_wheel_encoder[0] = number of ticks of left encoder since last call of + // this function _wheel_encoder[1] = number of ticks of right encoder since + // last call of this function + + double meterPerEncoderTick = (_wheel_diameter / 2) * 2 * M_PI / 40.0; + int diff_left = _wheel_encoder[0] - _last_value[0]; + int diff_right = _wheel_encoder[1] - _last_value[1]; + _last_value[0] = _wheel_encoder[0]; + _last_value[1] = _wheel_encoder[1]; + + double magic_number = 1.0; + + double distance_left = diff_left * meterPerEncoderTick * + _last_wheel_cmd_direction[0] * magic_number; + double distance_right = diff_right * meterPerEncoderTick * + _last_wheel_cmd_direction[1] * magic_number; + + pos[0] += distance_left; + // vel[0] = distance_left / period.toSec(); + pos[1] += distance_right; + // vel[1] = distance_right / period.toSec(); + } + + /* + ros::Time get_time() { + prev_update_time = curr_update_time; + curr_update_time = ros::Time::now(); + return curr_update_time; + } + + ros::Duration get_period() { + return curr_update_time - prev_update_time; + } + */ + ros::NodeHandle nh; + ros::NodeHandle private_nh; + +private: + hardware_interface::JointStateInterface jnt_state_interface; + hardware_interface::VelocityJointInterface jnt_vel_interface; + double cmd[NUM_JOINTS]; + double pos[NUM_JOINTS]; + double vel[NUM_JOINTS]; + double eff[NUM_JOINTS]; + + bool running_; + double _wheel_diameter; + double _max_speed; + double _wheel_angle[NUM_JOINTS]; + int _wheel_encoder[NUM_JOINTS]; + int _last_cmd[NUM_JOINTS]; + int _last_value[NUM_JOINTS]; + int _last_wheel_cmd_direction[NUM_JOINTS]; + + ros::Time curr_update_time, prev_update_time; + + // ros::Subscriber left_wheel_encoder_sub_; + // ros::Subscriber right_wheel_encoder_sub_; + std::array wheel_encoder_subs_; + ros::ServiceServer start_srv_; + ros::ServiceServer stop_srv_; + + // ros::ServiceClient left_client; + // ros::ServiceClient right_client; + std::array service_clients; + std::array service_requests; + + bool start_callback(std_srvs::Empty::Request & /*req*/, + std_srvs::Empty::Response & /*res*/) + { + running_ = true; + return true; + } + + bool stop_callback(std_srvs::Empty::Request & /*req*/, + std_srvs::Empty::Response & /*res*/) + { + running_ = false; + return true; + } + + void WheelEncoderCallback( const mirte_msgs::Encoder::ConstPtr& msg, int joint ) + { + _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; + } + + // void rightWheelEncoderCallback(const mirte_msgs::Encoder &msg) + // { + // _wheel_encoder[1] = _wheel_encoder[1] + msg.value; + // } + + // Thread and function to restart service clients when the service server has + // restarted + std::future reconnect_thread; + void init_service_clients(); + void start_reconnect(); + std::mutex service_clients_mutex; +}; // class + +void MyRobotHWInterface::init_service_clients() +{ + std::vector services = {"/mirte/set_left_speed", "/mirte/set_right_speed"}; + if (NUM_JOINTS == 4) + { + services = {"/mirte/set_left_front_speed", "/mirte/set_left_back_speed", // TODO: check ordering + "/mirte/set_right_front_speed", "/mirte/set_right_back_speed"}; + } + for (auto service : services) + { + ROS_INFO_STREAM("Waiting for service " << service); + ros::service::waitForService(service); + } + { + const std::lock_guard lock(this->service_clients_mutex); + for (int i = 0; i < NUM_JOINTS; i++) + { + service_clients[i] = nh.serviceClient( + services[i], true); + } + } +} + +MyRobotHWInterface::MyRobotHWInterface() + : running_(true), private_nh("~"), + start_srv_(nh.advertiseService( + "start", &MyRobotHWInterface::start_callback, this)), + stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, + this)) +{ + private_nh.param("wheel_diameter", _wheel_diameter, 0.06); + private_nh.param("max_speed", _max_speed, 2.0); + + // Initialize raw data + std::fill_n(pos, NUM_JOINTS, 0.0); + std::fill_n(vel, NUM_JOINTS, 0.0); + std::fill_n(eff, NUM_JOINTS, 0.0); + std::fill_n(cmd, NUM_JOINTS, 0.0); + + // connect and register the joint state and velocity interfaces + for (unsigned int i = 0; i < NUM_JOINTS; ++i) + { + std::ostringstream os; + os << "wheel_" << i << "_joint"; + + hardware_interface::JointStateHandle state_handle(os.str(), &pos[i], + &vel[i], &eff[i]); + jnt_state_interface.registerHandle(state_handle); + + hardware_interface::JointHandle vel_handle( + jnt_state_interface.getHandle(os.str()), &cmd[i]); + jnt_vel_interface.registerHandle(vel_handle); + + _wheel_encoder[i] = 0; + _last_value[i] = 0; + _last_wheel_cmd_direction[i] = 0; + _last_cmd[i] = 0; + } + registerInterface(&jnt_state_interface); + registerInterface(&jnt_vel_interface); + + // Initialize publishers and subscribers + for (int i = 0; i < NUM_JOINTS; i++) + { + std::ostringstream os; + os << "/mirte/encoder/" << i; + wheel_encoder_subs_[i] = nh.subscribe( + os.str(), 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i )); + } + // left_wheel_encoder_sub_ = + // nh.subscribe("/mirte/encoder/left", 1, + // &MyRobotHWInterface::leftWheelEncoderCallback, this); + // right_wheel_encoder_sub_ = + // nh.subscribe("/mirte/encoder/right", 1, + // &MyRobotHWInterface::rightWheelEncoderCallback, this); + + this->init_service_clients(); +} + +void MyRobotHWInterface::start_reconnect() +{ + using namespace std::chrono_literals; + + if (this->reconnect_thread.valid()) + { // does it already exist or not? + + // Use wait_for() with zero milliseconds to check thread status. + auto status = this->reconnect_thread.wait_for(0ms); + + if (status != + std::future_status::ready) + { // Still running -> already reconnecting + return; + } + } + + /* Run the reconnection on a different thread to not pause the ros-control + loop. The launch policy std::launch::async makes sure that the task is run + asynchronously on a new thread. */ + + this->reconnect_thread = + std::async(std::launch::async, [this] + { this->init_service_clients(); }); +} \ No newline at end of file diff --git a/mirte_control_master/launch/control.launch b/mirte_control_master/launch/control.launch new file mode 100644 index 00000000..5f34d69d --- /dev/null +++ b/mirte_control_master/launch/control.launch @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/mirte_control_master/package.xml b/mirte_control_master/package.xml new file mode 100644 index 00000000..f2ccbdf0 --- /dev/null +++ b/mirte_control_master/package.xml @@ -0,0 +1,50 @@ + + + mirte_control_master + 0.3.2 + Ridgeback's mobility and sensor base. + + Mike Purvis + Tony Baltovski + + Tony Baltovski + + BSD + + http://wiki.ros.org/Robots/Ridgeback + + catkin + + + + roscpp + controller_manager + diagnostic_updater + geometry_msgs + ridgeback_msgs + puma_motor_driver + realtime_tools + rosserial_server + sensor_msgs + std_msgs + mirte_msgs + message_runtime + hardware_interface + diagnostic_aggregator + diff_drive_controller + imu_filter_madgwick + ridgeback_control + ridgeback_description + python-scipy + rosserial_python + teleop_twist_joy + tf + topic_tools + socat + + roslaunch + roslint + + + + diff --git a/mirte_control_master/src/my_robot_base.cpp b/mirte_control_master/src/my_robot_base.cpp new file mode 100644 index 00000000..801bf82e --- /dev/null +++ b/mirte_control_master/src/my_robot_base.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include + +void controlLoop(MyRobotHWInterface &hw, + controller_manager::ControllerManager &cm, + std::chrono::system_clock::time_point &last_time) { + std::chrono::system_clock::time_point current_time = + std::chrono::system_clock::now(); + std::chrono::duration elapsed_time = current_time - last_time; + ros::Duration elapsed(elapsed_time.count()); + last_time = current_time; + + hw.read(elapsed); + cm.update(ros::Time::now(), elapsed); + hw.write(); +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "my_robot_base_node"); + + MyRobotHWInterface hw; + controller_manager::ControllerManager cm(&hw, hw.nh); + + double control_frequency; + hw.private_nh.param("control_frequency", control_frequency, 10.0); + + ros::CallbackQueue my_robot_queue; + ros::AsyncSpinner my_robot_spinner(1, &my_robot_queue); + + std::chrono::system_clock::time_point last_time = + std::chrono::system_clock::now(); + ros::TimerOptions control_timer( + ros::Duration(1 / control_frequency), + std::bind(controlLoop, std::ref(hw), std::ref(cm), std::ref(last_time)), + &my_robot_queue); + ros::Timer control_loop = hw.nh.createTimer(control_timer); + my_robot_spinner.start(); + ros::spin(); + + return 0; +} From 7ecaadc4fbb3b44f9b1bfe52508495752a9a5cf5 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Fri, 19 Jan 2024 14:49:48 +0100 Subject: [PATCH 23/77] Fix mirte master control --- mirte_control/include/my_robot_hw_interface.h | 57 ++++++++++++------ mirte_control_master/config/control.yaml | 12 ++-- .../config/my_robot_common.yaml | 59 ------------------- .../include/my_robot_hw_interface.h | 9 ++- mirte_control_master/launch/control.launch | 7 ++- mirte_control_master/scripts/mock.py | 23 ++++++++ 6 files changed, 77 insertions(+), 90 deletions(-) delete mode 100644 mirte_control_master/config/my_robot_common.yaml create mode 100755 mirte_control_master/scripts/mock.py diff --git a/mirte_control/include/my_robot_hw_interface.h b/mirte_control/include/my_robot_hw_interface.h index 0c03faa5..102d850b 100644 --- a/mirte_control/include/my_robot_hw_interface.h +++ b/mirte_control/include/my_robot_hw_interface.h @@ -37,15 +37,18 @@ const unsigned int NUM_JOINTS = 2; /// \brief Hardware interface for a robot -class MyRobotHWInterface : public hardware_interface::RobotHW { +class MyRobotHWInterface : public hardware_interface::RobotHW +{ public: MyRobotHWInterface(); /* * */ - void write() { - if (running_) { + void write() + { + if (running_) + { // make sure the clients don't get overwritten while calling them const std::lock_guard lock(this->service_clients_mutex); @@ -61,10 +64,12 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { int left_speed = std::max(std::min(int(cmd[0] / (6 * M_PI) * 100), 100), -100); - if (left_speed != _last_cmd[0]) { + if (left_speed != _last_cmd[0]) + { left_motor_service.request.speed = left_speed; _last_cmd[0] = left_speed; - if (!left_client.call(left_motor_service)) { + if (!left_client.call(left_motor_service)) + { this->start_reconnect(); return; } @@ -72,10 +77,12 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { int right_speed = std::max(std::min(int(cmd[1] / (6 * M_PI) * 100), 100), -100); - if (right_speed != _last_cmd[1]) { + if (right_speed != _last_cmd[1]) + { right_motor_service.request.speed = right_speed; _last_cmd[1] = right_speed; - if (!right_client.call(right_motor_service)) { + if (!right_client.call(right_motor_service)) + { this->start_reconnect(); } } @@ -90,7 +97,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { /** * Reading encoder values and setting position and velocity of encoders */ - void read(const ros::Duration &period) { + void read(const ros::Duration &period) + { //_wheel_encoder[0] = number of ticks of left encoder since last call of // this function _wheel_encoder[1] = number of ticks of right encoder since // last call of this function @@ -160,22 +168,26 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { mirte_msgs::SetMotorSpeed right_motor_service; bool start_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { + std_srvs::Empty::Response & /*res*/) + { running_ = true; return true; } bool stop_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { + std_srvs::Empty::Response & /*res*/) + { running_ = false; return true; } - void leftWheelEncoderCallback(const mirte_msgs::Encoder &msg) { + void leftWheelEncoderCallback(const mirte_msgs::Encoder &msg) + { _wheel_encoder[0] = _wheel_encoder[0] + msg.value; } - void rightWheelEncoderCallback(const mirte_msgs::Encoder &msg) { + void rightWheelEncoderCallback(const mirte_msgs::Encoder &msg) + { _wheel_encoder[1] = _wheel_encoder[1] + msg.value; } @@ -187,7 +199,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { std::mutex service_clients_mutex; }; // class -void MyRobotHWInterface::init_service_clients() { +void MyRobotHWInterface::init_service_clients() +{ ros::service::waitForService("/mirte/set_left_speed"); ros::service::waitForService("/mirte/set_right_speed"); { @@ -204,7 +217,8 @@ MyRobotHWInterface::MyRobotHWInterface() start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, - this)) { + this)) +{ private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); @@ -215,7 +229,8 @@ MyRobotHWInterface::MyRobotHWInterface() std::fill_n(cmd, NUM_JOINTS, 0.0); // connect and register the joint state and velocity interfaces - for (unsigned int i = 0; i < NUM_JOINTS; ++i) { + for (unsigned int i = 0; i < NUM_JOINTS; ++i) + { std::ostringstream os; os << "wheel_" << i << "_joint"; @@ -246,16 +261,19 @@ MyRobotHWInterface::MyRobotHWInterface() this->init_service_clients(); } -void MyRobotHWInterface::start_reconnect() { +void MyRobotHWInterface::start_reconnect() +{ using namespace std::chrono_literals; - if (this->reconnect_thread.valid()) { // does it already exist or not? + if (this->reconnect_thread.valid()) + { // does it already exist or not? // Use wait_for() with zero milliseconds to check thread status. auto status = this->reconnect_thread.wait_for(0ms); if (status != - std::future_status::ready) { // Still running -> already reconnecting + std::future_status::ready) + { // Still running -> already reconnecting return; } } @@ -265,5 +283,6 @@ void MyRobotHWInterface::start_reconnect() { asynchronously on a new thread. */ this->reconnect_thread = - std::async(std::launch::async, [this] { this->init_service_clients(); }); + std::async(std::launch::async, [this] + { this->init_service_clients(); }); } \ No newline at end of file diff --git a/mirte_control_master/config/control.yaml b/mirte_control_master/config/control.yaml index 7538f13a..422670cb 100644 --- a/mirte_control_master/config/control.yaml +++ b/mirte_control_master/config/control.yaml @@ -2,12 +2,12 @@ ridgeback_joint_publisher: type: "joint_state_controller/JointStateController" publish_rate: 50 -ridgeback_velocity_controller: +mirte_master_velocity_controller: type: "mecanum_drive_controller/MecanumDriveController" - front_left_wheel_joint: "front_left_wheel" - back_left_wheel_joint: "rear_left_wheel" - front_right_wheel_joint: "front_right_wheel" - back_right_wheel_joint: "rear_right_wheel" + front_left_wheel_joint: "wheel_0_joint" + back_left_wheel_joint: "wheel_1_joint" + front_right_wheel_joint: "wheel_2_joint" + back_right_wheel_joint: "wheel_3_joint" publish_rate: 50 pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03] @@ -16,7 +16,7 @@ ridgeback_velocity_controller: # Override URDF look-up for wheel separation since the parent link is not the base_link. wheel_separation_x: 0.638 wheel_separation_y: 0.551 - + wheel_radius: 0.165 # Odometry fused with IMU is published by robot_localization, so # no need to publish a TF based on encoders alone. enable_odom_tf: false diff --git a/mirte_control_master/config/my_robot_common.yaml b/mirte_control_master/config/my_robot_common.yaml deleted file mode 100644 index ace0de05..00000000 --- a/mirte_control_master/config/my_robot_common.yaml +++ /dev/null @@ -1,59 +0,0 @@ -mobile_base_controller: - type : "diff_drive_controller/DiffDriveController" - left_wheel : 'wheel_0_joint' - right_wheel : 'wheel_1_joint' - publish_rate: 50.0 # default: 50 - pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - #pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] - #twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] - - # Wheel separation and diameter. These are both optional. - # diff_drive_controller will attempt to read either one or both from the - # URDF if not specified as a parameter - wheel_separation : 0.125 # meters - wheel_radius : 0.003 # meters - - # Wheel separation and radius multipliers - wheel_separation_multiplier: 1.0 # default: 1.0 - wheel_radius_multiplier : 1.0 # default: 1.0 - - # Velocity commands timeout [s], default 0.5 - # This setting also works together with the repeat_rate and key_timeout parameters - # from teleop_twist_keyboard. - cmd_vel_timeout: 0.5 - - # Base frame_id - base_frame_id: base_footprint #default: base_link - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - #max_velocity : 0.42 # m/s at 5V - max_velocity : 0.55 # m/s at 6V -# min_velocity : -0.42 #-0.5 # m/s - has_acceleration_limits: false - #max_acceleration : 0.42 #0.8 # m/s^2 - #min_acceleration : -0.42 #-0.4 # m/s^2 - has_jerk_limits : false - # max_jerk : 5.0 # m/s^3 - angular: - z: - has_velocity_limits : true - max_velocity : 2 #1.7 # rad/s - has_acceleration_limits: false - #max_acceleration : 8 # rad/s^2 - has_jerk_limits : false - #max_jerk : 2.5 # rad/s^3 - -#Publish to TF directly or not -enable_odom_tf: true - -#Name of frame to publish odometry in -odom_frame_id: odom - -# Publish the velocity command to be executed. -# It is to monitor the effect of limiters on the controller input. -publish_cmd: true diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index be32868d..0245c74e 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -44,10 +44,13 @@ class MyRobotHWInterface : public hardware_interface::RobotHW bool write_single(int joint, int speed) { + int speed_mapped = std::max(std::min(int(cmd[joint] / (6 * M_PI) * 100), 100), -100); if (speed_mapped != _last_cmd[joint]) { + std::cout << "write " << joint << " " << speed << std::endl; + service_requests[joint].request.speed = speed_mapped; _last_cmd[joint] = speed_mapped; if (!service_clients[joint].call(service_requests[joint])) @@ -102,7 +105,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW //_wheel_encoder[0] = number of ticks of left encoder since last call of // this function _wheel_encoder[1] = number of ticks of right encoder since // last call of this function - + // TODO: fix reading double meterPerEncoderTick = (_wheel_diameter / 2) * 2 * M_PI / 40.0; int diff_left = _wheel_encoder[0] - _last_value[0]; int diff_right = _wheel_encoder[1] - _last_value[1]; @@ -180,7 +183,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW return true; } - void WheelEncoderCallback( const mirte_msgs::Encoder::ConstPtr& msg, int joint ) + void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, int joint) { _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } @@ -265,7 +268,7 @@ MyRobotHWInterface::MyRobotHWInterface() std::ostringstream os; os << "/mirte/encoder/" << i; wheel_encoder_subs_[i] = nh.subscribe( - os.str(), 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i )); + os.str(), 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i)); } // left_wheel_encoder_sub_ = // nh.subscribe("/mirte/encoder/left", 1, diff --git a/mirte_control_master/launch/control.launch b/mirte_control_master/launch/control.launch index 5f34d69d..9cec6e2a 100644 --- a/mirte_control_master/launch/control.launch +++ b/mirte_control_master/launch/control.launch @@ -1,12 +1,13 @@ - + + args="mirte_master_velocity_controller" /> - + \ No newline at end of file diff --git a/mirte_control_master/scripts/mock.py b/mirte_control_master/scripts/mock.py new file mode 100755 index 00000000..6089dc59 --- /dev/null +++ b/mirte_control_master/scripts/mock.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python + +import rospy +from std_srvs.srv import Empty, EmptyResponse +from mirte_msgs.srv import SetMotorSpeed, SetMotorSpeedResponse, SetMotorSpeedRequest +def my_service_callback(request, i): + # Add your service logic here + print(request, i) + # rospy.loginfo("Service called", i, request.speed) + # Return an appropriate response + return SetMotorSpeedResponse() + +if __name__ == '__main__': + # Initialize the ROS node + rospy.init_node('my_node') + # Create the service + my_service1 = rospy.Service('/mirte/set_left_front_speed', SetMotorSpeed, lambda request: my_service_callback(request, 1)) + my_service2 = rospy.Service('/mirte/set_left_back_speed', SetMotorSpeed, lambda request: my_service_callback(request, 2)) + my_service3 = rospy.Service('/mirte/set_right_front_speed', SetMotorSpeed, lambda request: my_service_callback(request, 3)) + my_service4 = rospy.Service('/mirte/set_right_back_speed', SetMotorSpeed, lambda request: my_service_callback(request, 4)) + + # Spin the node to process callbacks + rospy.spin() From ae4aca4a433ff3f9ba9e663a4282c55dee1a4f25 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Fri, 19 Jan 2024 14:57:09 +0100 Subject: [PATCH 24/77] Fix styling --- mirte_control/include/my_robot_hw_interface.h | 57 ++++-------- .../include/my_robot_hw_interface.h | 86 +++++++------------ mirte_control_master/scripts/mock.py | 31 +++++-- .../scripts/ROS_telemetrix_aio_api.py | 20 +++-- 4 files changed, 89 insertions(+), 105 deletions(-) diff --git a/mirte_control/include/my_robot_hw_interface.h b/mirte_control/include/my_robot_hw_interface.h index 102d850b..0c03faa5 100644 --- a/mirte_control/include/my_robot_hw_interface.h +++ b/mirte_control/include/my_robot_hw_interface.h @@ -37,18 +37,15 @@ const unsigned int NUM_JOINTS = 2; /// \brief Hardware interface for a robot -class MyRobotHWInterface : public hardware_interface::RobotHW -{ +class MyRobotHWInterface : public hardware_interface::RobotHW { public: MyRobotHWInterface(); /* * */ - void write() - { - if (running_) - { + void write() { + if (running_) { // make sure the clients don't get overwritten while calling them const std::lock_guard lock(this->service_clients_mutex); @@ -64,12 +61,10 @@ class MyRobotHWInterface : public hardware_interface::RobotHW int left_speed = std::max(std::min(int(cmd[0] / (6 * M_PI) * 100), 100), -100); - if (left_speed != _last_cmd[0]) - { + if (left_speed != _last_cmd[0]) { left_motor_service.request.speed = left_speed; _last_cmd[0] = left_speed; - if (!left_client.call(left_motor_service)) - { + if (!left_client.call(left_motor_service)) { this->start_reconnect(); return; } @@ -77,12 +72,10 @@ class MyRobotHWInterface : public hardware_interface::RobotHW int right_speed = std::max(std::min(int(cmd[1] / (6 * M_PI) * 100), 100), -100); - if (right_speed != _last_cmd[1]) - { + if (right_speed != _last_cmd[1]) { right_motor_service.request.speed = right_speed; _last_cmd[1] = right_speed; - if (!right_client.call(right_motor_service)) - { + if (!right_client.call(right_motor_service)) { this->start_reconnect(); } } @@ -97,8 +90,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW /** * Reading encoder values and setting position and velocity of encoders */ - void read(const ros::Duration &period) - { + void read(const ros::Duration &period) { //_wheel_encoder[0] = number of ticks of left encoder since last call of // this function _wheel_encoder[1] = number of ticks of right encoder since // last call of this function @@ -168,26 +160,22 @@ class MyRobotHWInterface : public hardware_interface::RobotHW mirte_msgs::SetMotorSpeed right_motor_service; bool start_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) - { + std_srvs::Empty::Response & /*res*/) { running_ = true; return true; } bool stop_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) - { + std_srvs::Empty::Response & /*res*/) { running_ = false; return true; } - void leftWheelEncoderCallback(const mirte_msgs::Encoder &msg) - { + void leftWheelEncoderCallback(const mirte_msgs::Encoder &msg) { _wheel_encoder[0] = _wheel_encoder[0] + msg.value; } - void rightWheelEncoderCallback(const mirte_msgs::Encoder &msg) - { + void rightWheelEncoderCallback(const mirte_msgs::Encoder &msg) { _wheel_encoder[1] = _wheel_encoder[1] + msg.value; } @@ -199,8 +187,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW std::mutex service_clients_mutex; }; // class -void MyRobotHWInterface::init_service_clients() -{ +void MyRobotHWInterface::init_service_clients() { ros::service::waitForService("/mirte/set_left_speed"); ros::service::waitForService("/mirte/set_right_speed"); { @@ -217,8 +204,7 @@ MyRobotHWInterface::MyRobotHWInterface() start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, - this)) -{ + this)) { private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); @@ -229,8 +215,7 @@ MyRobotHWInterface::MyRobotHWInterface() std::fill_n(cmd, NUM_JOINTS, 0.0); // connect and register the joint state and velocity interfaces - for (unsigned int i = 0; i < NUM_JOINTS; ++i) - { + for (unsigned int i = 0; i < NUM_JOINTS; ++i) { std::ostringstream os; os << "wheel_" << i << "_joint"; @@ -261,19 +246,16 @@ MyRobotHWInterface::MyRobotHWInterface() this->init_service_clients(); } -void MyRobotHWInterface::start_reconnect() -{ +void MyRobotHWInterface::start_reconnect() { using namespace std::chrono_literals; - if (this->reconnect_thread.valid()) - { // does it already exist or not? + if (this->reconnect_thread.valid()) { // does it already exist or not? // Use wait_for() with zero milliseconds to check thread status. auto status = this->reconnect_thread.wait_for(0ms); if (status != - std::future_status::ready) - { // Still running -> already reconnecting + std::future_status::ready) { // Still running -> already reconnecting return; } } @@ -283,6 +265,5 @@ void MyRobotHWInterface::start_reconnect() asynchronously on a new thread. */ this->reconnect_thread = - std::async(std::launch::async, [this] - { this->init_service_clients(); }); + std::async(std::launch::async, [this] { this->init_service_clients(); }); } \ No newline at end of file diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index 0245c74e..f770c505 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -37,24 +37,20 @@ const unsigned int NUM_JOINTS = 4; /// \brief Hardware interface for a robot -class MyRobotHWInterface : public hardware_interface::RobotHW -{ +class MyRobotHWInterface : public hardware_interface::RobotHW { public: MyRobotHWInterface(); - bool write_single(int joint, int speed) - { + bool write_single(int joint, int speed) { int speed_mapped = std::max(std::min(int(cmd[joint] / (6 * M_PI) * 100), 100), -100); - if (speed_mapped != _last_cmd[joint]) - { + if (speed_mapped != _last_cmd[joint]) { std::cout << "write " << joint << " " << speed << std::endl; service_requests[joint].request.speed = speed_mapped; _last_cmd[joint] = speed_mapped; - if (!service_clients[joint].call(service_requests[joint])) - { + if (!service_clients[joint].call(service_requests[joint])) { this->start_reconnect(); return false; } @@ -64,10 +60,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW /* * */ - void write() - { - if (running_) - { + void write() { + if (running_) { // make sure the clients don't get overwritten while calling them const std::lock_guard lock(this->service_clients_mutex); @@ -80,18 +74,15 @@ class MyRobotHWInterface : public hardware_interface::RobotHW // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s // (6*pi) - for (int i = 0; i < NUM_JOINTS; i++) - { - if (!write_single(i, cmd[i])) - { + for (int i = 0; i < NUM_JOINTS; i++) { + if (!write_single(i, cmd[i])) { return; } } // Set the direction in so the read() can use it // TODO: this does not work properly, because at the end of a series // cmd_vel is negative, while the rotation is not - for (int i = 0; i < NUM_JOINTS; i++) - { + for (int i = 0; i < NUM_JOINTS; i++) { _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; } } @@ -100,8 +91,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW /** * Reading encoder values and setting position and velocity of encoders */ - void read(const ros::Duration &period) - { + void read(const ros::Duration &period) { //_wheel_encoder[0] = number of ticks of left encoder since last call of // this function _wheel_encoder[1] = number of ticks of right encoder since // last call of this function @@ -170,21 +160,19 @@ class MyRobotHWInterface : public hardware_interface::RobotHW std::array service_requests; bool start_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) - { + std_srvs::Empty::Response & /*res*/) { running_ = true; return true; } bool stop_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) - { + std_srvs::Empty::Response & /*res*/) { running_ = false; return true; } - void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, int joint) - { + void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, + int joint) { _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } @@ -201,25 +189,23 @@ class MyRobotHWInterface : public hardware_interface::RobotHW std::mutex service_clients_mutex; }; // class -void MyRobotHWInterface::init_service_clients() -{ - std::vector services = {"/mirte/set_left_speed", "/mirte/set_right_speed"}; - if (NUM_JOINTS == 4) - { - services = {"/mirte/set_left_front_speed", "/mirte/set_left_back_speed", // TODO: check ordering +void MyRobotHWInterface::init_service_clients() { + std::vector services = {"/mirte/set_left_speed", + "/mirte/set_right_speed"}; + if (NUM_JOINTS == 4) { + services = {"/mirte/set_left_front_speed", + "/mirte/set_left_back_speed", // TODO: check ordering "/mirte/set_right_front_speed", "/mirte/set_right_back_speed"}; } - for (auto service : services) - { + for (auto service : services) { ROS_INFO_STREAM("Waiting for service " << service); ros::service::waitForService(service); } { const std::lock_guard lock(this->service_clients_mutex); - for (int i = 0; i < NUM_JOINTS; i++) - { - service_clients[i] = nh.serviceClient( - services[i], true); + for (int i = 0; i < NUM_JOINTS; i++) { + service_clients[i] = + nh.serviceClient(services[i], true); } } } @@ -229,8 +215,7 @@ MyRobotHWInterface::MyRobotHWInterface() start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, - this)) -{ + this)) { private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); @@ -241,8 +226,7 @@ MyRobotHWInterface::MyRobotHWInterface() std::fill_n(cmd, NUM_JOINTS, 0.0); // connect and register the joint state and velocity interfaces - for (unsigned int i = 0; i < NUM_JOINTS; ++i) - { + for (unsigned int i = 0; i < NUM_JOINTS; ++i) { std::ostringstream os; os << "wheel_" << i << "_joint"; @@ -263,12 +247,12 @@ MyRobotHWInterface::MyRobotHWInterface() registerInterface(&jnt_vel_interface); // Initialize publishers and subscribers - for (int i = 0; i < NUM_JOINTS; i++) - { + for (int i = 0; i < NUM_JOINTS; i++) { std::ostringstream os; os << "/mirte/encoder/" << i; wheel_encoder_subs_[i] = nh.subscribe( - os.str(), 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i)); + os.str(), 1, + boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i)); } // left_wheel_encoder_sub_ = // nh.subscribe("/mirte/encoder/left", 1, @@ -280,19 +264,16 @@ MyRobotHWInterface::MyRobotHWInterface() this->init_service_clients(); } -void MyRobotHWInterface::start_reconnect() -{ +void MyRobotHWInterface::start_reconnect() { using namespace std::chrono_literals; - if (this->reconnect_thread.valid()) - { // does it already exist or not? + if (this->reconnect_thread.valid()) { // does it already exist or not? // Use wait_for() with zero milliseconds to check thread status. auto status = this->reconnect_thread.wait_for(0ms); if (status != - std::future_status::ready) - { // Still running -> already reconnecting + std::future_status::ready) { // Still running -> already reconnecting return; } } @@ -302,6 +283,5 @@ void MyRobotHWInterface::start_reconnect() asynchronously on a new thread. */ this->reconnect_thread = - std::async(std::launch::async, [this] - { this->init_service_clients(); }); + std::async(std::launch::async, [this] { this->init_service_clients(); }); } \ No newline at end of file diff --git a/mirte_control_master/scripts/mock.py b/mirte_control_master/scripts/mock.py index 6089dc59..da6326bd 100755 --- a/mirte_control_master/scripts/mock.py +++ b/mirte_control_master/scripts/mock.py @@ -3,6 +3,8 @@ import rospy from std_srvs.srv import Empty, EmptyResponse from mirte_msgs.srv import SetMotorSpeed, SetMotorSpeedResponse, SetMotorSpeedRequest + + def my_service_callback(request, i): # Add your service logic here print(request, i) @@ -10,14 +12,31 @@ def my_service_callback(request, i): # Return an appropriate response return SetMotorSpeedResponse() -if __name__ == '__main__': + +if __name__ == "__main__": # Initialize the ROS node - rospy.init_node('my_node') + rospy.init_node("my_node") # Create the service - my_service1 = rospy.Service('/mirte/set_left_front_speed', SetMotorSpeed, lambda request: my_service_callback(request, 1)) - my_service2 = rospy.Service('/mirte/set_left_back_speed', SetMotorSpeed, lambda request: my_service_callback(request, 2)) - my_service3 = rospy.Service('/mirte/set_right_front_speed', SetMotorSpeed, lambda request: my_service_callback(request, 3)) - my_service4 = rospy.Service('/mirte/set_right_back_speed', SetMotorSpeed, lambda request: my_service_callback(request, 4)) + my_service1 = rospy.Service( + "/mirte/set_left_front_speed", + SetMotorSpeed, + lambda request: my_service_callback(request, 1), + ) + my_service2 = rospy.Service( + "/mirte/set_left_back_speed", + SetMotorSpeed, + lambda request: my_service_callback(request, 2), + ) + my_service3 = rospy.Service( + "/mirte/set_right_front_speed", + SetMotorSpeed, + lambda request: my_service_callback(request, 3), + ) + my_service4 = rospy.Service( + "/mirte/set_right_back_speed", + SetMotorSpeed, + lambda request: my_service_callback(request, 4), + ) # Spin the node to process callbacks rospy.spin() diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index b97ec6cf..c1f6347d 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1293,7 +1293,9 @@ async def start(self): SetServoAngle, self.set_servo_angle_service, ) - self.publisher = rospy.Publisher(f"/mirte/servos/{self.name}/position", Int32, queue_size=1) + self.publisher = rospy.Publisher( + f"/mirte/servos/{self.name}/position", Int32, queue_size=1 + ) async def servo_write(self, angle): angle = angle * 100 # centidegrees @@ -1309,6 +1311,7 @@ def callback(self, data): message.data = data["value"] self.publisher.publish(message) + class Hiwonder_Bus: def __init__(self, board, module_name, module): self.name = module_name @@ -1326,24 +1329,25 @@ async def start(self): if "servos" in self.module: for servo_name in self.module["servos"]: servo_obj = self.module["servos"][servo_name] - servo = Hiwonder_Servo( - servo_name, servo_obj, self - ) + servo = Hiwonder_Servo(servo_name, servo_obj, self) ids.append(servo.id) await servo.start() self.servos[servo_name] = servo - updaters = await self.board.modules.add_hiwonder_servo(uart, rx, tx, ids, self.callback) + updaters = await self.board.modules.add_hiwonder_servo( + uart, rx, tx, ids, self.callback + ) self.set_single_servo = updaters["set_single_servo"] self.set_multiple_servos = updaters["set_multiple_servos"] # TODO: add service to update multiple servos - async def callback(self, data): for servo_update in data: - servos = list(filter(lambda servo:servo_update["id"]==servo.id, self.servos)) - if(len(servos) == 1): + servos = list( + filter(lambda servo: servo_update["id"] == servo.id, self.servos) + ) + if len(servos) == 1: servos[0].callback(servo_update) From 5a95498923dfdef6d0ba07ee20f8555b430a1e31 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 22 Jan 2024 14:01:08 +0100 Subject: [PATCH 25/77] Change joints name and make it more flexible --- mirte_control_master/config/control.yaml | 8 +- .../config/my_robot_control.yaml | 1 + .../include/my_robot_hw_interface.h | 158 +++++++++++------- 3 files changed, 99 insertions(+), 68 deletions(-) diff --git a/mirte_control_master/config/control.yaml b/mirte_control_master/config/control.yaml index 422670cb..d025c577 100644 --- a/mirte_control_master/config/control.yaml +++ b/mirte_control_master/config/control.yaml @@ -4,10 +4,10 @@ ridgeback_joint_publisher: mirte_master_velocity_controller: type: "mecanum_drive_controller/MecanumDriveController" - front_left_wheel_joint: "wheel_0_joint" - back_left_wheel_joint: "wheel_1_joint" - front_right_wheel_joint: "wheel_2_joint" - back_right_wheel_joint: "wheel_3_joint" + front_left_wheel_joint: "wheel_left_front_joint" + back_left_wheel_joint: "wheel_left_rear_joint" + front_right_wheel_joint: "wheel_right_front_joint" + back_right_wheel_joint: "wheel_right_back_joint" publish_rate: 50 pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03] diff --git a/mirte_control_master/config/my_robot_control.yaml b/mirte_control_master/config/my_robot_control.yaml index b8386d0f..cb792f92 100644 --- a/mirte_control_master/config/my_robot_control.yaml +++ b/mirte_control_master/config/my_robot_control.yaml @@ -21,3 +21,4 @@ my_robot: joint: right_wheel_hinge pid: {p: 100.0, i: 0.1, d: 10.0} #pid: {p: 1.0, i: 1.0, d: 0.0} +# TODO: extend this for the mirte-master \ No newline at end of file diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index f770c505..64e0b232 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -35,22 +35,28 @@ #include const unsigned int NUM_JOINTS = 4; +const auto service_format = "/mirte/set_{}_speed"; +const bool bidirectional = true; /// \brief Hardware interface for a robot -class MyRobotHWInterface : public hardware_interface::RobotHW { +class MyRobotHWInterface : public hardware_interface::RobotHW +{ public: MyRobotHWInterface(); - bool write_single(int joint, int speed) { + bool write_single(int joint, int speed) + { int speed_mapped = std::max(std::min(int(cmd[joint] / (6 * M_PI) * 100), 100), -100); - if (speed_mapped != _last_cmd[joint]) { + if (speed_mapped != _last_cmd[joint]) + { std::cout << "write " << joint << " " << speed << std::endl; service_requests[joint].request.speed = speed_mapped; _last_cmd[joint] = speed_mapped; - if (!service_clients[joint].call(service_requests[joint])) { + if (!service_clients[joint].call(service_requests[joint])) + { this->start_reconnect(); return false; } @@ -60,8 +66,10 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { /* * */ - void write() { - if (running_) { + void write() + { + if (running_) + { // make sure the clients don't get overwritten while calling them const std::lock_guard lock(this->service_clients_mutex); @@ -74,45 +82,59 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s // (6*pi) - for (int i = 0; i < NUM_JOINTS; i++) { - if (!write_single(i, cmd[i])) { + for (int i = 0; i < NUM_JOINTS; i++) + { + if (!write_single(i, cmd[i])) + { return; } } // Set the direction in so the read() can use it // TODO: this does not work properly, because at the end of a series // cmd_vel is negative, while the rotation is not - for (int i = 0; i < NUM_JOINTS; i++) { + for (int i = 0; i < NUM_JOINTS; i++) + { _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; } } } + double meter_per_enc_tick() { + return (_wheel_diameter / 2) * 2 * M_PI / 40.0; // TODO: get ticks from parameter server + } + /** * Reading encoder values and setting position and velocity of encoders */ - void read(const ros::Duration &period) { - //_wheel_encoder[0] = number of ticks of left encoder since last call of - // this function _wheel_encoder[1] = number of ticks of right encoder since - // last call of this function - // TODO: fix reading - double meterPerEncoderTick = (_wheel_diameter / 2) * 2 * M_PI / 40.0; - int diff_left = _wheel_encoder[0] - _last_value[0]; - int diff_right = _wheel_encoder[1] - _last_value[1]; - _last_value[0] = _wheel_encoder[0]; - _last_value[1] = _wheel_encoder[1]; - - double magic_number = 1.0; - - double distance_left = diff_left * meterPerEncoderTick * - _last_wheel_cmd_direction[0] * magic_number; - double distance_right = diff_right * meterPerEncoderTick * - _last_wheel_cmd_direction[1] * magic_number; - - pos[0] += distance_left; - // vel[0] = distance_left / period.toSec(); - pos[1] += distance_right; - // vel[1] = distance_right / period.toSec(); + void read_single(int joint, const ros::Duration &period) + { + auto diff = _wheel_encoder[joint] - _last_value[joint]; + _last_value[joint] = _wheel_encoder[joint]; + double meterPerEncoderTick = meter_per_enc_tick(); + double distance; + if (bidirectional) + { // if encoder is counting bidirectional, then it decreases by itself, dont want to use last_wheel_cmd_direction + distance = diff * meterPerEncoderTick * 1.0; + } + else + { + distance = diff * meterPerEncoderTick * + _last_wheel_cmd_direction[joint] * 1.0; + } + pos[joint] += distance; + vel[joint] = distance / period.toSec(); // WHY: was this turned off? + } + + /** + * Reading encoder values and setting position and velocity of encoders + */ + void read(const ros::Duration &period) + { + + for (int i = 0; i < NUM_JOINTS; i++) + { + read_single(i); + } } /* @@ -158,21 +180,24 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { // ros::ServiceClient right_client; std::array service_clients; std::array service_requests; - + std::vector joints; bool start_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { + std_srvs::Empty::Response & /*res*/) + { running_ = true; return true; } bool stop_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { + std_srvs::Empty::Response & /*res*/) + { running_ = false; return true; } void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, - int joint) { + int joint) + { _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } @@ -189,23 +214,29 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { std::mutex service_clients_mutex; }; // class -void MyRobotHWInterface::init_service_clients() { - std::vector services = {"/mirte/set_left_speed", - "/mirte/set_right_speed"}; - if (NUM_JOINTS == 4) { - services = {"/mirte/set_left_front_speed", - "/mirte/set_left_back_speed", // TODO: check ordering - "/mirte/set_right_front_speed", "/mirte/set_right_back_speed"}; +void MyRobotHWInterface::init_service_clients() +{ + this->joints = {"left", // Edit the control.yaml when using this for the normal mirte as well + "right"}; + if (NUM_JOINTS == 4) + { + this->joints = {"left_front", + "left_back", // TODO: check ordering + "right_front", "right_back"}; } - for (auto service : services) { + for (auto joint : this.joint) + { + auto service = std::format(service_format, joint); ROS_INFO_STREAM("Waiting for service " << service); - ros::service::waitForService(service); + ros::service::waitForService(service, + -1); } { const std::lock_guard lock(this->service_clients_mutex); - for (int i = 0; i < NUM_JOINTS; i++) { + for (int i = 0; i < NUM_JOINTS; i++) + { service_clients[i] = - nh.serviceClient(services[i], true); + nh.serviceClient(std::format(service_format, joints[i]), true); } } } @@ -215,7 +246,8 @@ MyRobotHWInterface::MyRobotHWInterface() start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, - this)) { + this)) +{ private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); @@ -226,9 +258,10 @@ MyRobotHWInterface::MyRobotHWInterface() std::fill_n(cmd, NUM_JOINTS, 0.0); // connect and register the joint state and velocity interfaces - for (unsigned int i = 0; i < NUM_JOINTS; ++i) { + for (unsigned int i = 0; i < NUM_JOINTS; ++i) + { std::ostringstream os; - os << "wheel_" << i << "_joint"; + os << "wheel_" << this.joints[i] << "_joint"; hardware_interface::JointStateHandle state_handle(os.str(), &pos[i], &vel[i], &eff[i]); @@ -247,33 +280,29 @@ MyRobotHWInterface::MyRobotHWInterface() registerInterface(&jnt_vel_interface); // Initialize publishers and subscribers - for (int i = 0; i < NUM_JOINTS; i++) { - std::ostringstream os; - os << "/mirte/encoder/" << i; + for (int i = 0; i < NUM_JOINTS; i++) + { + auto encoder_topic = std::format("/mirte/encoder/{}", this->joints[i]); wheel_encoder_subs_[i] = nh.subscribe( - os.str(), 1, + encoder_topic, 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i)); } - // left_wheel_encoder_sub_ = - // nh.subscribe("/mirte/encoder/left", 1, - // &MyRobotHWInterface::leftWheelEncoderCallback, this); - // right_wheel_encoder_sub_ = - // nh.subscribe("/mirte/encoder/right", 1, - // &MyRobotHWInterface::rightWheelEncoderCallback, this); - this->init_service_clients(); } -void MyRobotHWInterface::start_reconnect() { +void MyRobotHWInterface::start_reconnect() +{ using namespace std::chrono_literals; - if (this->reconnect_thread.valid()) { // does it already exist or not? + if (this->reconnect_thread.valid()) + { // does it already exist or not? // Use wait_for() with zero milliseconds to check thread status. auto status = this->reconnect_thread.wait_for(0ms); if (status != - std::future_status::ready) { // Still running -> already reconnecting + std::future_status::ready) + { // Still running -> already reconnecting return; } } @@ -283,5 +312,6 @@ void MyRobotHWInterface::start_reconnect() { asynchronously on a new thread. */ this->reconnect_thread = - std::async(std::launch::async, [this] { this->init_service_clients(); }); + std::async(std::launch::async, [this] + { this->init_service_clients(); }); } \ No newline at end of file From 8c98d7ddbf88f4e92f32a331ce20b0a691418cfa Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 22 Jan 2024 14:12:27 +0100 Subject: [PATCH 26/77] Fixes --- .../include/my_robot_hw_interface.h | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index 64e0b232..24256b7d 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -33,10 +33,11 @@ #include #include #include +#include const unsigned int NUM_JOINTS = 4; -const auto service_format = "/mirte/set_{}_speed"; -const bool bidirectional = true; +auto service_format = "/mirte/set_{}_speed"; +bool bidirectional = true; /// \brief Hardware interface for a robot class MyRobotHWInterface : public hardware_interface::RobotHW @@ -99,7 +100,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW } } - double meter_per_enc_tick() { + double meter_per_enc_tick() + { return (_wheel_diameter / 2) * 2 * M_PI / 40.0; // TODO: get ticks from parameter server } @@ -133,7 +135,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW for (int i = 0; i < NUM_JOINTS; i++) { - read_single(i); + this->read_single(i, period); } } @@ -224,9 +226,9 @@ void MyRobotHWInterface::init_service_clients() "left_back", // TODO: check ordering "right_front", "right_back"}; } - for (auto joint : this.joint) + for (auto joint : this->joints) { - auto service = std::format(service_format, joint); + auto service = (boost::format(service_format) % joint).str(); ROS_INFO_STREAM("Waiting for service " << service); ros::service::waitForService(service, -1); @@ -236,7 +238,7 @@ void MyRobotHWInterface::init_service_clients() for (int i = 0; i < NUM_JOINTS; i++) { service_clients[i] = - nh.serviceClient(std::format(service_format, joints[i]), true); + nh.serviceClient((boost::format(service_format) % this->joints[i]).str(), true); } } } @@ -250,6 +252,7 @@ MyRobotHWInterface::MyRobotHWInterface() { private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); + private_nh.param("bidirectional", bidirectional, true); // Initialize raw data std::fill_n(pos, NUM_JOINTS, 0.0); @@ -261,7 +264,7 @@ MyRobotHWInterface::MyRobotHWInterface() for (unsigned int i = 0; i < NUM_JOINTS; ++i) { std::ostringstream os; - os << "wheel_" << this.joints[i] << "_joint"; + os << "wheel_" << this->joints[i] << "_joint"; hardware_interface::JointStateHandle state_handle(os.str(), &pos[i], &vel[i], &eff[i]); @@ -282,7 +285,7 @@ MyRobotHWInterface::MyRobotHWInterface() // Initialize publishers and subscribers for (int i = 0; i < NUM_JOINTS; i++) { - auto encoder_topic = std::format("/mirte/encoder/{}", this->joints[i]); + auto encoder_topic = (boost::format(service_format) % this->joints[i]).str(); wheel_encoder_subs_[i] = nh.subscribe( encoder_topic, 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i)); From b30d960e52634759097ec8168cbd39ebad914d05 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 22 Jan 2024 14:54:58 +0100 Subject: [PATCH 27/77] Fixes control-master --- mirte_bringup/launch/minimal_master.launch | 58 ++++++++ mirte_control_master/CMakeLists.txt | 2 +- mirte_control_master/config/control.yaml | 8 +- .../include/my_robot_hw_interface.h | 140 +++++++----------- mirte_control_master/launch/control.launch | 2 +- mirte_control_master/scripts/mock.py | 4 +- 6 files changed, 121 insertions(+), 93 deletions(-) create mode 100644 mirte_bringup/launch/minimal_master.launch diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch new file mode 100644 index 00000000..ed7a56fc --- /dev/null +++ b/mirte_bringup/launch/minimal_master.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mirte_control_master/CMakeLists.txt b/mirte_control_master/CMakeLists.txt index 166fac42..50aa2f1f 100644 --- a/mirte_control_master/CMakeLists.txt +++ b/mirte_control_master/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(mirte_control_master) ## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) +add_compile_options(-std=c++17) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/mirte_control_master/config/control.yaml b/mirte_control_master/config/control.yaml index d025c577..7e45e98c 100644 --- a/mirte_control_master/config/control.yaml +++ b/mirte_control_master/config/control.yaml @@ -1,13 +1,13 @@ -ridgeback_joint_publisher: +my_robot: type: "joint_state_controller/JointStateController" publish_rate: 50 -mirte_master_velocity_controller: +mobile_base_controller: type: "mecanum_drive_controller/MecanumDriveController" front_left_wheel_joint: "wheel_left_front_joint" back_left_wheel_joint: "wheel_left_rear_joint" front_right_wheel_joint: "wheel_right_front_joint" - back_right_wheel_joint: "wheel_right_back_joint" + back_right_wheel_joint: "wheel_right_rear_joint" publish_rate: 50 pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03] @@ -19,7 +19,7 @@ mirte_master_velocity_controller: wheel_radius: 0.165 # Odometry fused with IMU is published by robot_localization, so # no need to publish a TF based on encoders alone. - enable_odom_tf: false + enable_odom_tf: true # Wheel separation and radius multipliers wheel_separation_multiplier: 1.0 # default: 1.0 diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index 24256b7d..515443d5 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -29,35 +29,29 @@ #include #include +#include #include #include #include #include -#include const unsigned int NUM_JOINTS = 4; -auto service_format = "/mirte/set_{}_speed"; +auto service_format = "/mirte/set_%s_speed"; bool bidirectional = true; /// \brief Hardware interface for a robot -class MyRobotHWInterface : public hardware_interface::RobotHW -{ +class MyRobotHWInterface : public hardware_interface::RobotHW { public: MyRobotHWInterface(); - bool write_single(int joint, int speed) - { + bool write_single(int joint, int speed) { int speed_mapped = std::max(std::min(int(cmd[joint] / (6 * M_PI) * 100), 100), -100); - if (speed_mapped != _last_cmd[joint]) - { - std::cout << "write " << joint << " " << speed << std::endl; - + if (speed_mapped != _last_cmd[joint]) { service_requests[joint].request.speed = speed_mapped; _last_cmd[joint] = speed_mapped; - if (!service_clients[joint].call(service_requests[joint])) - { + if (!service_clients[joint].call(service_requests[joint])) { this->start_reconnect(); return false; } @@ -67,10 +61,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW /* * */ - void write() - { - if (running_) - { + void write() { + if (running_) { // make sure the clients don't get overwritten while calling them const std::lock_guard lock(this->service_clients_mutex); @@ -83,45 +75,40 @@ class MyRobotHWInterface : public hardware_interface::RobotHW // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s // (6*pi) - for (int i = 0; i < NUM_JOINTS; i++) - { - if (!write_single(i, cmd[i])) - { + for (int i = 0; i < NUM_JOINTS; i++) { + if (!write_single(i, cmd[i])) { return; } } // Set the direction in so the read() can use it // TODO: this does not work properly, because at the end of a series // cmd_vel is negative, while the rotation is not - for (int i = 0; i < NUM_JOINTS; i++) - { + for (int i = 0; i < NUM_JOINTS; i++) { _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; } } } - double meter_per_enc_tick() - { - return (_wheel_diameter / 2) * 2 * M_PI / 40.0; // TODO: get ticks from parameter server + double meter_per_enc_tick() { + return (_wheel_diameter / 2) * 2 * M_PI / + 40.0; // TODO: get ticks from parameter server } /** * Reading encoder values and setting position and velocity of encoders */ - void read_single(int joint, const ros::Duration &period) - { + void read_single(int joint, const ros::Duration &period) { auto diff = _wheel_encoder[joint] - _last_value[joint]; _last_value[joint] = _wheel_encoder[joint]; double meterPerEncoderTick = meter_per_enc_tick(); double distance; - if (bidirectional) - { // if encoder is counting bidirectional, then it decreases by itself, dont want to use last_wheel_cmd_direction + if (bidirectional) { // if encoder is counting bidirectional, then it + // decreases by itself, dont want to use + // last_wheel_cmd_direction distance = diff * meterPerEncoderTick * 1.0; - } - else - { - distance = diff * meterPerEncoderTick * - _last_wheel_cmd_direction[joint] * 1.0; + } else { + distance = + diff * meterPerEncoderTick * _last_wheel_cmd_direction[joint] * 1.0; } pos[joint] += distance; vel[joint] = distance / period.toSec(); // WHY: was this turned off? @@ -130,11 +117,9 @@ class MyRobotHWInterface : public hardware_interface::RobotHW /** * Reading encoder values and setting position and velocity of encoders */ - void read(const ros::Duration &period) - { + void read(const ros::Duration &period) { - for (int i = 0; i < NUM_JOINTS; i++) - { + for (int i = 0; i < NUM_JOINTS; i++) { this->read_single(i, period); } } @@ -184,22 +169,19 @@ class MyRobotHWInterface : public hardware_interface::RobotHW std::array service_requests; std::vector joints; bool start_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) - { + std_srvs::Empty::Response & /*res*/) { running_ = true; return true; } bool stop_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) - { + std_srvs::Empty::Response & /*res*/) { running_ = false; return true; } void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, - int joint) - { + int joint) { _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } @@ -216,29 +198,17 @@ class MyRobotHWInterface : public hardware_interface::RobotHW std::mutex service_clients_mutex; }; // class -void MyRobotHWInterface::init_service_clients() -{ - this->joints = {"left", // Edit the control.yaml when using this for the normal mirte as well - "right"}; - if (NUM_JOINTS == 4) - { - this->joints = {"left_front", - "left_back", // TODO: check ordering - "right_front", "right_back"}; - } - for (auto joint : this->joints) - { +void MyRobotHWInterface::init_service_clients() { + for (auto joint : this->joints) { auto service = (boost::format(service_format) % joint).str(); ROS_INFO_STREAM("Waiting for service " << service); - ros::service::waitForService(service, - -1); + ros::service::waitForService(service, -1); } { const std::lock_guard lock(this->service_clients_mutex); - for (int i = 0; i < NUM_JOINTS; i++) - { - service_clients[i] = - nh.serviceClient((boost::format(service_format) % this->joints[i]).str(), true); + for (int i = 0; i < NUM_JOINTS; i++) { + service_clients[i] = nh.serviceClient( + (boost::format(service_format) % this->joints[i]).str(), true); } } } @@ -248,30 +218,34 @@ MyRobotHWInterface::MyRobotHWInterface() start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, - this)) -{ + this)) { private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); - private_nh.param("bidirectional", bidirectional, true); + // private_nh.param("bidirectional", bidirectional, true); // Initialize raw data std::fill_n(pos, NUM_JOINTS, 0.0); std::fill_n(vel, NUM_JOINTS, 0.0); std::fill_n(eff, NUM_JOINTS, 0.0); std::fill_n(cmd, NUM_JOINTS, 0.0); - + this->joints = {"left", // Edit the control.yaml when using this for the + // normal mirte as well + "right"}; + if (NUM_JOINTS == 4) { + this->joints = {"left_front", + "left_rear", // TODO: check ordering + "right_front", "right_rear"}; + } // connect and register the joint state and velocity interfaces - for (unsigned int i = 0; i < NUM_JOINTS; ++i) - { - std::ostringstream os; - os << "wheel_" << this->joints[i] << "_joint"; - - hardware_interface::JointStateHandle state_handle(os.str(), &pos[i], - &vel[i], &eff[i]); + for (unsigned int i = 0; i < NUM_JOINTS; ++i) { + std::string joint = + (boost::format("wheel_%s_joint") % this->joints[i]).str(); + hardware_interface::JointStateHandle state_handle(joint, &pos[i], &vel[i], + &eff[i]); jnt_state_interface.registerHandle(state_handle); hardware_interface::JointHandle vel_handle( - jnt_state_interface.getHandle(os.str()), &cmd[i]); + jnt_state_interface.getHandle(joint), &cmd[i]); jnt_vel_interface.registerHandle(vel_handle); _wheel_encoder[i] = 0; @@ -283,9 +257,9 @@ MyRobotHWInterface::MyRobotHWInterface() registerInterface(&jnt_vel_interface); // Initialize publishers and subscribers - for (int i = 0; i < NUM_JOINTS; i++) - { - auto encoder_topic = (boost::format(service_format) % this->joints[i]).str(); + for (int i = 0; i < NUM_JOINTS; i++) { + auto encoder_topic = + (boost::format(service_format) % this->joints[i]).str(); wheel_encoder_subs_[i] = nh.subscribe( encoder_topic, 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i)); @@ -293,19 +267,16 @@ MyRobotHWInterface::MyRobotHWInterface() this->init_service_clients(); } -void MyRobotHWInterface::start_reconnect() -{ +void MyRobotHWInterface::start_reconnect() { using namespace std::chrono_literals; - if (this->reconnect_thread.valid()) - { // does it already exist or not? + if (this->reconnect_thread.valid()) { // does it already exist or not? // Use wait_for() with zero milliseconds to check thread status. auto status = this->reconnect_thread.wait_for(0ms); if (status != - std::future_status::ready) - { // Still running -> already reconnecting + std::future_status::ready) { // Still running -> already reconnecting return; } } @@ -315,6 +286,5 @@ void MyRobotHWInterface::start_reconnect() asynchronously on a new thread. */ this->reconnect_thread = - std::async(std::launch::async, [this] - { this->init_service_clients(); }); + std::async(std::launch::async, [this] { this->init_service_clients(); }); } \ No newline at end of file diff --git a/mirte_control_master/launch/control.launch b/mirte_control_master/launch/control.launch index 9cec6e2a..a9395974 100644 --- a/mirte_control_master/launch/control.launch +++ b/mirte_control_master/launch/control.launch @@ -2,7 +2,7 @@ + args="mobile_base_controller" /> diff --git a/mirte_control_master/scripts/mock.py b/mirte_control_master/scripts/mock.py index da6326bd..96d79862 100755 --- a/mirte_control_master/scripts/mock.py +++ b/mirte_control_master/scripts/mock.py @@ -23,7 +23,7 @@ def my_service_callback(request, i): lambda request: my_service_callback(request, 1), ) my_service2 = rospy.Service( - "/mirte/set_left_back_speed", + "/mirte/set_left_rear_speed", SetMotorSpeed, lambda request: my_service_callback(request, 2), ) @@ -33,7 +33,7 @@ def my_service_callback(request, i): lambda request: my_service_callback(request, 3), ) my_service4 = rospy.Service( - "/mirte/set_right_back_speed", + "/mirte/set_right_rear_speed", SetMotorSpeed, lambda request: my_service_callback(request, 4), ) From 44dcc7db8c9ecab697029cd6e78873e18f78d8b4 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Wed, 24 Jan 2024 10:56:40 +0100 Subject: [PATCH 28/77] remove unnecesary packages --- mirte_control_master/package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mirte_control_master/package.xml b/mirte_control_master/package.xml index f2ccbdf0..18c73bf2 100644 --- a/mirte_control_master/package.xml +++ b/mirte_control_master/package.xml @@ -22,7 +22,6 @@ diagnostic_updater geometry_msgs ridgeback_msgs - puma_motor_driver realtime_tools rosserial_server sensor_msgs @@ -35,7 +34,7 @@ imu_filter_madgwick ridgeback_control ridgeback_description - python-scipy + rosserial_python teleop_twist_joy tf From 4672b5586a7c4db38f786f378243dbd6b9fc776b Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Wed, 24 Jan 2024 11:46:42 +0100 Subject: [PATCH 29/77] Fix pca --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index c1f6347d..7ffb95d8 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1082,7 +1082,7 @@ class PCA_Servo: def __init__(self, servo_name, servo_obj, pca_update_func): self.pin = servo_obj["pin"] self.name = servo_name - self.pca_update_func = pca_update_func + self.pca_update_func = pca_update_func["set_pwm"] self.min_pulse = 544 if "min_pulse" in servo_obj: self.min_pulse = servo_obj["min_pulse"] @@ -1111,7 +1111,7 @@ def set_servo_angle_service(self, req): class PCA_Motor(Motor): def __init__(self, motor_name, motor_obj, pca_update_func): - self.pca_update_func = pca_update_func + self.pca_update_func = pca_update_func["set_pwm"] self.pin_A = motor_obj["pin_A"] self.pin_B = motor_obj["pin_B"] self.name = motor_name From 54a05a7ff39ef8300bdd65b98348fe8502d30b64 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 29 Jan 2024 11:29:49 +0100 Subject: [PATCH 30/77] template class option control --- .../include/my_robot_hw_interface.h | 18 +++++++++--------- mirte_control_master/src/my_robot_base.cpp | 7 ++++--- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index 515443d5..2353a4e0 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -35,11 +35,12 @@ #include #include -const unsigned int NUM_JOINTS = 4; +// const unsigned int NUM_JOINTS = 4; auto service_format = "/mirte/set_%s_speed"; -bool bidirectional = true; +bool bidirectional = true; // TODO: make this a parameter /// \brief Hardware interface for a robot +template class MyRobotHWInterface : public hardware_interface::RobotHW { public: MyRobotHWInterface(); @@ -185,10 +186,6 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } - // void rightWheelEncoderCallback(const mirte_msgs::Encoder &msg) - // { - // _wheel_encoder[1] = _wheel_encoder[1] + msg.value; - // } // Thread and function to restart service clients when the service server has // restarted @@ -198,7 +195,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { std::mutex service_clients_mutex; }; // class -void MyRobotHWInterface::init_service_clients() { +template +void MyRobotHWInterface::init_service_clients() { for (auto joint : this->joints) { auto service = (boost::format(service_format) % joint).str(); ROS_INFO_STREAM("Waiting for service " << service); @@ -213,7 +211,8 @@ void MyRobotHWInterface::init_service_clients() { } } -MyRobotHWInterface::MyRobotHWInterface() +template +MyRobotHWInterface::MyRobotHWInterface() : running_(true), private_nh("~"), start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), @@ -267,7 +266,8 @@ MyRobotHWInterface::MyRobotHWInterface() this->init_service_clients(); } -void MyRobotHWInterface::start_reconnect() { +template +void MyRobotHWInterface::start_reconnect() { using namespace std::chrono_literals; if (this->reconnect_thread.valid()) { // does it already exist or not? diff --git a/mirte_control_master/src/my_robot_base.cpp b/mirte_control_master/src/my_robot_base.cpp index 801bf82e..894a8186 100644 --- a/mirte_control_master/src/my_robot_base.cpp +++ b/mirte_control_master/src/my_robot_base.cpp @@ -3,7 +3,8 @@ #include #include -void controlLoop(MyRobotHWInterface &hw, +template +void controlLoop(MyRobotHWInterface &hw, controller_manager::ControllerManager &cm, std::chrono::system_clock::time_point &last_time) { std::chrono::system_clock::time_point current_time = @@ -20,7 +21,7 @@ void controlLoop(MyRobotHWInterface &hw, int main(int argc, char **argv) { ros::init(argc, argv, "my_robot_base_node"); - MyRobotHWInterface hw; + MyRobotHWInterface<2> hw; controller_manager::ControllerManager cm(&hw, hw.nh); double control_frequency; @@ -33,7 +34,7 @@ int main(int argc, char **argv) { std::chrono::system_clock::now(); ros::TimerOptions control_timer( ros::Duration(1 / control_frequency), - std::bind(controlLoop, std::ref(hw), std::ref(cm), std::ref(last_time)), + std::bind(controlLoop<2>, std::ref(hw), std::ref(cm), std::ref(last_time)), &my_robot_queue); ros::Timer control_loop = hw.nh.createTimer(control_timer); my_robot_spinner.start(); From acdea7e2fcd3fdee4200ad720d44803de43f9df5 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 29 Jan 2024 11:47:26 +0100 Subject: [PATCH 31/77] update to vectors --- .../include/my_robot_hw_interface.h | 87 +++++++++++-------- mirte_control_master/src/my_robot_base.cpp | 7 +- 2 files changed, 52 insertions(+), 42 deletions(-) diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index 2353a4e0..4593a985 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -40,7 +40,6 @@ auto service_format = "/mirte/set_%s_speed"; bool bidirectional = true; // TODO: make this a parameter /// \brief Hardware interface for a robot -template class MyRobotHWInterface : public hardware_interface::RobotHW { public: MyRobotHWInterface(); @@ -142,32 +141,27 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { private: hardware_interface::JointStateInterface jnt_state_interface; hardware_interface::VelocityJointInterface jnt_vel_interface; - double cmd[NUM_JOINTS]; - double pos[NUM_JOINTS]; - double vel[NUM_JOINTS]; - double eff[NUM_JOINTS]; + std::vector cmd; + std::vector pos; + std::vector vel; + std::vector eff; bool running_; double _wheel_diameter; double _max_speed; - double _wheel_angle[NUM_JOINTS]; - int _wheel_encoder[NUM_JOINTS]; - int _last_cmd[NUM_JOINTS]; - int _last_value[NUM_JOINTS]; - int _last_wheel_cmd_direction[NUM_JOINTS]; + std::vector _wheel_encoder; + std::vector _last_cmd; + std::vector _last_value; + std::vector _last_wheel_cmd_direction; ros::Time curr_update_time, prev_update_time; - // ros::Subscriber left_wheel_encoder_sub_; - // ros::Subscriber right_wheel_encoder_sub_; - std::array wheel_encoder_subs_; + std::vector wheel_encoder_subs_; ros::ServiceServer start_srv_; ros::ServiceServer stop_srv_; - // ros::ServiceClient left_client; - // ros::ServiceClient right_client; - std::array service_clients; - std::array service_requests; + std::vector service_clients; + std::vector service_requests; std::vector joints; bool start_callback(std_srvs::Empty::Request & /*req*/, std_srvs::Empty::Response & /*res*/) { @@ -186,17 +180,17 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } - // Thread and function to restart service clients when the service server has // restarted std::future reconnect_thread; void init_service_clients(); void start_reconnect(); std::mutex service_clients_mutex; + + int NUM_JOINTS = 2; }; // class -template -void MyRobotHWInterface::init_service_clients() { +void MyRobotHWInterface::init_service_clients() { for (auto joint : this->joints) { auto service = (boost::format(service_format) % joint).str(); ROS_INFO_STREAM("Waiting for service " << service); @@ -205,14 +199,14 @@ void MyRobotHWInterface::init_service_clients() { { const std::lock_guard lock(this->service_clients_mutex); for (int i = 0; i < NUM_JOINTS; i++) { - service_clients[i] = nh.serviceClient( - (boost::format(service_format) % this->joints[i]).str(), true); + service_clients.push_back(nh.serviceClient( + (boost::format(service_format) % this->joints[i]).str(), true)); + service_requests.push_back(mirte_msgs::SetMotorSpeed()); } } } -template -MyRobotHWInterface::MyRobotHWInterface() +MyRobotHWInterface::MyRobotHWInterface() : running_(true), private_nh("~"), start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), @@ -221,12 +215,28 @@ MyRobotHWInterface::MyRobotHWInterface() private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); // private_nh.param("bidirectional", bidirectional, true); - + this->NUM_JOINTS = 2; // Initialize raw data - std::fill_n(pos, NUM_JOINTS, 0.0); - std::fill_n(vel, NUM_JOINTS, 0.0); - std::fill_n(eff, NUM_JOINTS, 0.0); - std::fill_n(cmd, NUM_JOINTS, 0.0); + + for (int i = 0; i < NUM_JOINTS; i++) { + _wheel_encoder.push_back(0); + _last_value.push_back(0); + _last_wheel_cmd_direction.push_back(0); + _last_cmd.push_back(0); + pos.push_back(0); + vel.push_back(0); + eff.push_back(0); + cmd.push_back(0); + } + assert(_wheel_encoder.size() == NUM_JOINTS); + assert(_last_value.size() == NUM_JOINTS); + assert(_last_wheel_cmd_direction.size() == NUM_JOINTS); + assert(_last_cmd.size() == NUM_JOINTS); + assert(pos.size() == NUM_JOINTS); + assert(vel.size() == NUM_JOINTS); + assert(eff.size() == NUM_JOINTS); + assert(cmd.size() == NUM_JOINTS); + this->joints = {"left", // Edit the control.yaml when using this for the // normal mirte as well "right"}; @@ -235,6 +245,9 @@ MyRobotHWInterface::MyRobotHWInterface() "left_rear", // TODO: check ordering "right_front", "right_rear"}; } + std::cout << "Initializing MyRobotHWInterface with " << NUM_JOINTS + << " joints" << std::endl; + // connect and register the joint state and velocity interfaces for (unsigned int i = 0; i < NUM_JOINTS; ++i) { std::string joint = @@ -246,11 +259,6 @@ MyRobotHWInterface::MyRobotHWInterface() hardware_interface::JointHandle vel_handle( jnt_state_interface.getHandle(joint), &cmd[i]); jnt_vel_interface.registerHandle(vel_handle); - - _wheel_encoder[i] = 0; - _last_value[i] = 0; - _last_wheel_cmd_direction[i] = 0; - _last_cmd[i] = 0; } registerInterface(&jnt_state_interface); registerInterface(&jnt_vel_interface); @@ -259,15 +267,18 @@ MyRobotHWInterface::MyRobotHWInterface() for (int i = 0; i < NUM_JOINTS; i++) { auto encoder_topic = (boost::format(service_format) % this->joints[i]).str(); - wheel_encoder_subs_[i] = nh.subscribe( + wheel_encoder_subs_.push_back(nh.subscribe( encoder_topic, 1, - boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i)); + boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i))); } + assert(joints.size() == NUM_JOINTS); this->init_service_clients(); + assert(service_requests.size() == NUM_JOINTS); + + assert(service_clients.size() == NUM_JOINTS); } -template -void MyRobotHWInterface::start_reconnect() { +void MyRobotHWInterface ::start_reconnect() { using namespace std::chrono_literals; if (this->reconnect_thread.valid()) { // does it already exist or not? diff --git a/mirte_control_master/src/my_robot_base.cpp b/mirte_control_master/src/my_robot_base.cpp index 894a8186..801bf82e 100644 --- a/mirte_control_master/src/my_robot_base.cpp +++ b/mirte_control_master/src/my_robot_base.cpp @@ -3,8 +3,7 @@ #include #include -template -void controlLoop(MyRobotHWInterface &hw, +void controlLoop(MyRobotHWInterface &hw, controller_manager::ControllerManager &cm, std::chrono::system_clock::time_point &last_time) { std::chrono::system_clock::time_point current_time = @@ -21,7 +20,7 @@ void controlLoop(MyRobotHWInterface &hw, int main(int argc, char **argv) { ros::init(argc, argv, "my_robot_base_node"); - MyRobotHWInterface<2> hw; + MyRobotHWInterface hw; controller_manager::ControllerManager cm(&hw, hw.nh); double control_frequency; @@ -34,7 +33,7 @@ int main(int argc, char **argv) { std::chrono::system_clock::now(); ros::TimerOptions control_timer( ros::Duration(1 / control_frequency), - std::bind(controlLoop<2>, std::ref(hw), std::ref(cm), std::ref(last_time)), + std::bind(controlLoop, std::ref(hw), std::ref(cm), std::ref(last_time)), &my_robot_queue); ros::Timer control_loop = hw.nh.createTimer(control_timer); my_robot_spinner.start(); From 0559e92a16492e6d27d1b1f5264184e345eb0ec7 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 29 Jan 2024 12:15:59 +0100 Subject: [PATCH 32/77] Bidirectional fix, cleanup and fix cpp checks --- mirte_control_master/CMakeLists.txt | 2 +- .../include/my_robot_hw_interface.h | 55 ++++++++++--------- 2 files changed, 31 insertions(+), 26 deletions(-) diff --git a/mirte_control_master/CMakeLists.txt b/mirte_control_master/CMakeLists.txt index 50aa2f1f..939a32f1 100644 --- a/mirte_control_master/CMakeLists.txt +++ b/mirte_control_master/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(mirte_control_master) ## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++17) +add_compile_options(-std=c++17) # -Wall -Wextra -Wpedantic -Werror) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h index 4593a985..5d80635f 100644 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ b/mirte_control_master/include/my_robot_hw_interface.h @@ -36,8 +36,7 @@ #include // const unsigned int NUM_JOINTS = 4; -auto service_format = "/mirte/set_%s_speed"; -bool bidirectional = true; // TODO: make this a parameter +const auto service_format = "/mirte/set_%s_speed"; /// \brief Hardware interface for a robot class MyRobotHWInterface : public hardware_interface::RobotHW { @@ -47,7 +46,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { bool write_single(int joint, int speed) { int speed_mapped = - std::max(std::min(int(cmd[joint] / (6 * M_PI) * 100), 100), -100); + std::max(std::min(int(speed / (6 * M_PI) * 100), 100), -100); if (speed_mapped != _last_cmd[joint]) { service_requests[joint].request.speed = speed_mapped; _last_cmd[joint] = speed_mapped; @@ -75,7 +74,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s // (6*pi) - for (int i = 0; i < NUM_JOINTS; i++) { + for (size_t i = 0; i < NUM_JOINTS; i++) { if (!write_single(i, cmd[i])) { return; } @@ -83,7 +82,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { // Set the direction in so the read() can use it // TODO: this does not work properly, because at the end of a series // cmd_vel is negative, while the rotation is not - for (int i = 0; i < NUM_JOINTS; i++) { + for (size_t i = 0; i < NUM_JOINTS; i++) { _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; } } @@ -119,22 +118,11 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { */ void read(const ros::Duration &period) { - for (int i = 0; i < NUM_JOINTS; i++) { + for (size_t i = 0; i < NUM_JOINTS; i++) { this->read_single(i, period); } } - /* - ros::Time get_time() { - prev_update_time = curr_update_time; - curr_update_time = ros::Time::now(); - return curr_update_time; - } - - ros::Duration get_period() { - return curr_update_time - prev_update_time; - } - */ ros::NodeHandle nh; ros::NodeHandle private_nh; @@ -146,7 +134,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { std::vector vel; std::vector eff; - bool running_; + bool running_ = true; double _wheel_diameter; double _max_speed; std::vector _wheel_encoder; @@ -177,6 +165,9 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, int joint) { + if (msg->value < 0) { + bidirectional = true; + } _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } @@ -187,7 +178,9 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { void start_reconnect(); std::mutex service_clients_mutex; - int NUM_JOINTS = 2; + bool bidirectional = false; // assume it is one direction, when receiving any + // negative value, it will be set to true + unsigned int NUM_JOINTS = 2; }; // class void MyRobotHWInterface::init_service_clients() { @@ -198,7 +191,7 @@ void MyRobotHWInterface::init_service_clients() { } { const std::lock_guard lock(this->service_clients_mutex); - for (int i = 0; i < NUM_JOINTS; i++) { + for (size_t i = 0; i < NUM_JOINTS; i++) { service_clients.push_back(nh.serviceClient( (boost::format(service_format) % this->joints[i]).str(), true)); service_requests.push_back(mirte_msgs::SetMotorSpeed()); @@ -206,8 +199,21 @@ void MyRobotHWInterface::init_service_clients() { } } +unsigned int detect_joints(ros::NodeHandle &nh) { + std::string type; + nh.param("/mobile_base_controller/type", type, ""); + if (type.rfind("mecanum", 0) == 0) { // starts with mecanum + return 4; + } else if (type.rfind("diff", 0) == 0) { // starts with diff + return 2; + } else { + ROS_ERROR_STREAM("Unknown type: " << type); + return 0; + } +} + MyRobotHWInterface::MyRobotHWInterface() - : running_(true), private_nh("~"), + : private_nh("~"), running_(true), start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, @@ -215,10 +221,9 @@ MyRobotHWInterface::MyRobotHWInterface() private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); // private_nh.param("bidirectional", bidirectional, true); - this->NUM_JOINTS = 2; + this->NUM_JOINTS = detect_joints(private_nh); // Initialize raw data - - for (int i = 0; i < NUM_JOINTS; i++) { + for (size_t i = 0; i < NUM_JOINTS; i++) { _wheel_encoder.push_back(0); _last_value.push_back(0); _last_wheel_cmd_direction.push_back(0); @@ -264,7 +269,7 @@ MyRobotHWInterface::MyRobotHWInterface() registerInterface(&jnt_vel_interface); // Initialize publishers and subscribers - for (int i = 0; i < NUM_JOINTS; i++) { + for (size_t i = 0; i < NUM_JOINTS; i++) { auto encoder_topic = (boost::format(service_format) % this->joints[i]).str(); wheel_encoder_subs_.push_back(nh.subscribe( From e4a637a8847ddf274da70174d87a12845130b87b Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 30 Jan 2024 09:44:08 +0100 Subject: [PATCH 33/77] move control_master into control --- mirte_bringup/launch/minimal_master.launch | 6 +- mirte_control/CMakeLists.txt | 3 +- mirte_control/config/control_master.yaml | 68 +++++ mirte_control/include/my_robot_hw_interface.h | 253 ++++++++++-------- 4 files changed, 217 insertions(+), 113 deletions(-) create mode 100644 mirte_control/config/control_master.yaml diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index ed7a56fc..e539b031 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -17,8 +17,8 @@ + pkg="mirte_control" + type="mirte_control_node"/> - + #include +#include #include #include #include #include -const unsigned int NUM_JOINTS = 2; +// const unsigned int NUM_JOINTS = 4; +const auto service_format = "/mirte/set_%s_speed"; /// \brief Hardware interface for a robot class MyRobotHWInterface : public hardware_interface::RobotHW { public: MyRobotHWInterface(); + bool write_single(int joint, int speed) { + + int speed_mapped = + std::max(std::min(int(speed / (6 * M_PI) * 100), 100), -100); + if (speed_mapped != _last_cmd[joint]) { + service_requests[joint].request.speed = speed_mapped; + _last_cmd[joint] = speed_mapped; + if (!service_clients[joint].call(service_requests[joint])) { + this->start_reconnect(); + return false; + } + } + return true; + } /* * */ @@ -58,107 +74,83 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s // (6*pi) - - int left_speed = - std::max(std::min(int(cmd[0] / (6 * M_PI) * 100), 100), -100); - if (left_speed != _last_cmd[0]) { - left_motor_service.request.speed = left_speed; - _last_cmd[0] = left_speed; - if (!left_client.call(left_motor_service)) { - this->start_reconnect(); + for (size_t i = 0; i < NUM_JOINTS; i++) { + if (!write_single(i, cmd[i])) { return; } } - - int right_speed = - std::max(std::min(int(cmd[1] / (6 * M_PI) * 100), 100), -100); - if (right_speed != _last_cmd[1]) { - right_motor_service.request.speed = right_speed; - _last_cmd[1] = right_speed; - if (!right_client.call(right_motor_service)) { - this->start_reconnect(); - } - } // Set the direction in so the read() can use it // TODO: this does not work properly, because at the end of a series // cmd_vel is negative, while the rotation is not - _last_wheel_cmd_direction[0] = cmd[0] > 0.0 ? 1 : -1; - _last_wheel_cmd_direction[1] = cmd[1] > 0.0 ? 1 : -1; + for (size_t i = 0; i < NUM_JOINTS; i++) { + _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; + } } } + double meter_per_enc_tick() { + return (_wheel_diameter / 2) * 2 * M_PI / + 40.0; // TODO: get ticks from parameter server + } + /** * Reading encoder values and setting position and velocity of encoders */ - void read(const ros::Duration &period) { - //_wheel_encoder[0] = number of ticks of left encoder since last call of - // this function _wheel_encoder[1] = number of ticks of right encoder since - // last call of this function - - double meterPerEncoderTick = (_wheel_diameter / 2) * 2 * M_PI / 40.0; - int diff_left = _wheel_encoder[0] - _last_value[0]; - int diff_right = _wheel_encoder[1] - _last_value[1]; - _last_value[0] = _wheel_encoder[0]; - _last_value[1] = _wheel_encoder[1]; - - double magic_number = 1.0; - - double distance_left = diff_left * meterPerEncoderTick * - _last_wheel_cmd_direction[0] * magic_number; - double distance_right = diff_right * meterPerEncoderTick * - _last_wheel_cmd_direction[1] * magic_number; - - pos[0] += distance_left; - // vel[0] = distance_left / period.toSec(); - pos[1] += distance_right; - // vel[1] = distance_right / period.toSec(); + void read_single(int joint, const ros::Duration &period) { + auto diff = _wheel_encoder[joint] - _last_value[joint]; + _last_value[joint] = _wheel_encoder[joint]; + double meterPerEncoderTick = meter_per_enc_tick(); + double distance; + if (bidirectional) { // if encoder is counting bidirectional, then it + // decreases by itself, dont want to use + // last_wheel_cmd_direction + distance = diff * meterPerEncoderTick * 1.0; + } else { + distance = + diff * meterPerEncoderTick * _last_wheel_cmd_direction[joint] * 1.0; + } + pos[joint] += distance; + vel[joint] = distance / period.toSec(); // WHY: was this turned off? } - /* - ros::Time get_time() { - prev_update_time = curr_update_time; - curr_update_time = ros::Time::now(); - return curr_update_time; - } + /** + * Reading encoder values and setting position and velocity of encoders + */ + void read(const ros::Duration &period) { - ros::Duration get_period() { - return curr_update_time - prev_update_time; + for (size_t i = 0; i < NUM_JOINTS; i++) { + this->read_single(i, period); } - */ + } + ros::NodeHandle nh; ros::NodeHandle private_nh; private: hardware_interface::JointStateInterface jnt_state_interface; hardware_interface::VelocityJointInterface jnt_vel_interface; - double cmd[NUM_JOINTS]; - double pos[NUM_JOINTS]; - double vel[NUM_JOINTS]; - double eff[NUM_JOINTS]; + std::vector cmd; + std::vector pos; + std::vector vel; + std::vector eff; - bool running_; + bool running_ = true; double _wheel_diameter; double _max_speed; - double _wheel_angle[NUM_JOINTS]; - int _wheel_encoder[NUM_JOINTS]; - int _last_cmd[NUM_JOINTS]; - int _last_value[NUM_JOINTS]; - int _last_wheel_cmd_direction[NUM_JOINTS]; + std::vector _wheel_encoder; + std::vector _last_cmd; + std::vector _last_value; + std::vector _last_wheel_cmd_direction; ros::Time curr_update_time, prev_update_time; - ros::Subscriber left_wheel_encoder_sub_; - ros::Subscriber right_wheel_encoder_sub_; - + std::vector wheel_encoder_subs_; ros::ServiceServer start_srv_; ros::ServiceServer stop_srv_; - ros::ServiceClient left_client; - ros::ServiceClient right_client; - - mirte_msgs::SetMotorSpeed left_motor_service; - mirte_msgs::SetMotorSpeed right_motor_service; - + std::vector service_clients; + std::vector service_requests; + std::vector joints; bool start_callback(std_srvs::Empty::Request & /*req*/, std_srvs::Empty::Response & /*res*/) { running_ = true; @@ -171,12 +163,12 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { return true; } - void leftWheelEncoderCallback(const mirte_msgs::Encoder &msg) { - _wheel_encoder[0] = _wheel_encoder[0] + msg.value; - } - - void rightWheelEncoderCallback(const mirte_msgs::Encoder &msg) { - _wheel_encoder[1] = _wheel_encoder[1] + msg.value; + void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, + int joint) { + if (msg->value < 0) { + bidirectional = true; + } + _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; } // Thread and function to restart service clients when the service server has @@ -185,68 +177,113 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { void init_service_clients(); void start_reconnect(); std::mutex service_clients_mutex; + + bool bidirectional = false; // assume it is one direction, when receiving any + // negative value, it will be set to true + unsigned int NUM_JOINTS = 2; }; // class void MyRobotHWInterface::init_service_clients() { - ros::service::waitForService("/mirte/set_left_speed"); - ros::service::waitForService("/mirte/set_right_speed"); + for (auto joint : this->joints) { + auto service = (boost::format(service_format) % joint).str(); + ROS_INFO_STREAM("Waiting for service " << service); + ros::service::waitForService(service, -1); + } { const std::lock_guard lock(this->service_clients_mutex); - this->left_client = nh.serviceClient( - "/mirte/set_left_speed", true); - this->right_client = nh.serviceClient( - "/mirte/set_right_speed", true); + for (size_t i = 0; i < NUM_JOINTS; i++) { + service_clients.push_back(nh.serviceClient( + (boost::format(service_format) % this->joints[i]).str(), true)); + service_requests.push_back(mirte_msgs::SetMotorSpeed()); + } + } +} + +unsigned int detect_joints(ros::NodeHandle &nh) { + std::string type; + nh.param("/mobile_base_controller/type", type, ""); + if (type.rfind("mecanum", 0) == 0) { // starts with mecanum + return 4; + } else if (type.rfind("diff", 0) == 0) { // starts with diff + return 2; + } else { + ROS_ERROR_STREAM("Unknown type: " << type); + return 0; } } MyRobotHWInterface::MyRobotHWInterface() - : running_(true), private_nh("~"), + : private_nh("~"), running_(true), start_srv_(nh.advertiseService( "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, this)) { private_nh.param("wheel_diameter", _wheel_diameter, 0.06); private_nh.param("max_speed", _max_speed, 2.0); - + // private_nh.param("bidirectional", bidirectional, true); + this->NUM_JOINTS = detect_joints(private_nh); // Initialize raw data - std::fill_n(pos, NUM_JOINTS, 0.0); - std::fill_n(vel, NUM_JOINTS, 0.0); - std::fill_n(eff, NUM_JOINTS, 0.0); - std::fill_n(cmd, NUM_JOINTS, 0.0); + for (size_t i = 0; i < NUM_JOINTS; i++) { + _wheel_encoder.push_back(0); + _last_value.push_back(0); + _last_wheel_cmd_direction.push_back(0); + _last_cmd.push_back(0); + pos.push_back(0); + vel.push_back(0); + eff.push_back(0); + cmd.push_back(0); + } + assert(_wheel_encoder.size() == NUM_JOINTS); + assert(_last_value.size() == NUM_JOINTS); + assert(_last_wheel_cmd_direction.size() == NUM_JOINTS); + assert(_last_cmd.size() == NUM_JOINTS); + assert(pos.size() == NUM_JOINTS); + assert(vel.size() == NUM_JOINTS); + assert(eff.size() == NUM_JOINTS); + assert(cmd.size() == NUM_JOINTS); + + this->joints = {"left", // Edit the control.yaml when using this for the + // normal mirte as well + "right"}; + if (NUM_JOINTS == 4) { + this->joints = {"left_front", + "left_rear", // TODO: check ordering + "right_front", "right_rear"}; + } + std::cout << "Initializing MyRobotHWInterface with " << NUM_JOINTS + << " joints" << std::endl; // connect and register the joint state and velocity interfaces for (unsigned int i = 0; i < NUM_JOINTS; ++i) { - std::ostringstream os; - os << "wheel_" << i << "_joint"; - - hardware_interface::JointStateHandle state_handle(os.str(), &pos[i], - &vel[i], &eff[i]); + std::string joint = + (boost::format("wheel_%s_joint") % this->joints[i]).str(); + hardware_interface::JointStateHandle state_handle(joint, &pos[i], &vel[i], + &eff[i]); jnt_state_interface.registerHandle(state_handle); hardware_interface::JointHandle vel_handle( - jnt_state_interface.getHandle(os.str()), &cmd[i]); + jnt_state_interface.getHandle(joint), &cmd[i]); jnt_vel_interface.registerHandle(vel_handle); - - _wheel_encoder[i] = 0; - _last_value[i] = 0; - _last_wheel_cmd_direction[i] = 0; - _last_cmd[i] = 0; } registerInterface(&jnt_state_interface); registerInterface(&jnt_vel_interface); // Initialize publishers and subscribers - left_wheel_encoder_sub_ = - nh.subscribe("/mirte/encoder/left", 1, - &MyRobotHWInterface::leftWheelEncoderCallback, this); - right_wheel_encoder_sub_ = - nh.subscribe("/mirte/encoder/right", 1, - &MyRobotHWInterface::rightWheelEncoderCallback, this); - + for (size_t i = 0; i < NUM_JOINTS; i++) { + auto encoder_topic = + (boost::format(service_format) % this->joints[i]).str(); + wheel_encoder_subs_.push_back(nh.subscribe( + encoder_topic, 1, + boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i))); + } + assert(joints.size() == NUM_JOINTS); this->init_service_clients(); + assert(service_requests.size() == NUM_JOINTS); + + assert(service_clients.size() == NUM_JOINTS); } -void MyRobotHWInterface::start_reconnect() { +void MyRobotHWInterface ::start_reconnect() { using namespace std::chrono_literals; if (this->reconnect_thread.valid()) { // does it already exist or not? From 6e62939d48bcc13db9a5c9e4781f1e723c244330 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Thu, 1 Feb 2024 17:17:56 +0100 Subject: [PATCH 34/77] Add angle mapping + add enable hiwonder servos --- .../scripts/ROS_telemetrix_aio_api.py | 43 +++++++++++++++---- 1 file changed, 35 insertions(+), 8 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 7ffb95d8..fc758ecd 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -35,6 +35,7 @@ async def analog_write(board, pin, value): # Import ROS message types from std_msgs.msg import Header, Int32 +from std_srvs.srv import SetBool, SetBoolResponse from sensor_msgs.msg import Range, BatteryState from mirte_msgs.msg import * @@ -1280,12 +1281,14 @@ def __init__(self, servo_name, servo_obj, bus): self.id = servo_obj["id"] self.name = servo_name self.bus = bus - self.min_angle = 0 - if "min_angle" in servo_obj: - self.min_angle = servo_obj["min_angle"] - self.max_angle = 24000 # centidegrees - if "max_angle" in servo_obj: - self.max_angle = servo_obj["max_angle"] + self.min_angle_in = 0 + self.max_angle_in = 180 # degrees or whatever you want to use + + self.min_angle_out = 0 + self.max_angle_out = 24000 # centidegrees + for name in ["min_angle_in", "min_angle_out", "max_angle_in", "max_angle_out"]: + if name in servo_obj: + setattr(self, name, servo_obj[name]) async def start(self): server = rospy.Service( @@ -1293,13 +1296,26 @@ async def start(self): SetServoAngle, self.set_servo_angle_service, ) + rospy.Service( + "/mirte/set_" + self.name + "_servo_enable", + SetBool, + self.set_servo_enabled_service, + ) self.publisher = rospy.Publisher( f"/mirte/servos/{self.name}/position", Int32, queue_size=1 ) + def set_servo_enabled_service(self, req): + asyncio.run(self.bus.set_enabled(self.id, req.value)) + return SetBoolResponse(True) + async def servo_write(self, angle): - angle = angle * 100 # centidegrees - angle = max(self.min_angle, min(angle, self.max_angle)) + angle = scale( + angle, + [self.min_angle_in, self.max_angle_in], + [self.min_angle_out, self.max_angle_out], + ) + angle = max(self.min_angle_out, min(angle, self.max_angle_out)) # clamp await self.bus.set_single_servo(self.id, angle, 100) def set_servo_angle_service(self, req): @@ -1339,9 +1355,20 @@ async def start(self): ) self.set_single_servo = updaters["set_single_servo"] self.set_multiple_servos = updaters["set_multiple_servos"] + self.set_enabled = updaters["set_enabled"] + self.set_enabled_all = updaters["set_enabled_all"] + rospy.Service( + "/mirte/set_" + self.name + "_all_servos_enable", + SetBool, + self.set_servo_enabled_service, + ) # TODO: add service to update multiple servos + def set_all_servos_enabled(self, req): + asyncio.run(self.bus.set_enabled_all(req.value)) + return SetBoolResponse(True) + async def callback(self, data): for servo_update in data: servos = list( From 1e0892c1aa0e031fc1da7258f9ec85827ec10052 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Thu, 8 Feb 2024 14:17:39 +0100 Subject: [PATCH 35/77] Fix hiwonder no message publish --- .../scripts/ROS_telemetrix_aio_api.py | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index fc758ecd..e3cdd088 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1323,10 +1323,7 @@ def set_servo_angle_service(self, req): return SetServoAngleResponse(True) def callback(self, data): - message = Int32 - message.data = data["value"] - self.publisher.publish(message) - + self.publisher.publish(data["angle"]) class Hiwonder_Bus: def __init__(self, board, module_name, module): @@ -1349,6 +1346,8 @@ async def start(self): ids.append(servo.id) await servo.start() self.servos[servo_name] = servo + self.servos[servo.id] = servo + updaters = await self.board.modules.add_hiwonder_servo( uart, rx, tx, ids, self.callback @@ -1360,7 +1359,7 @@ async def start(self): rospy.Service( "/mirte/set_" + self.name + "_all_servos_enable", SetBool, - self.set_servo_enabled_service, + self.set_all_servos_enabled, ) # TODO: add service to update multiple servos @@ -1370,12 +1369,14 @@ def set_all_servos_enabled(self, req): return SetBoolResponse(True) async def callback(self, data): - for servo_update in data: - servos = list( - filter(lambda servo: servo_update["id"] == servo.id, self.servos) - ) - if len(servos) == 1: - servos[0].callback(servo_update) + try: + for servo_update in data: + sid = servo_update["id"] + if sid in self.servos: + self.servos[sid].callback(servo_update) + except Exception as e: + print("hiwonder servo callback err:") + print(e) # Shutdown procedure From 7ea4affc529980afb6d43b6c160419a12772feff Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Thu, 8 Feb 2024 14:27:02 +0100 Subject: [PATCH 36/77] Fix servo write --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index e3cdd088..4955a46d 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1315,7 +1315,7 @@ async def servo_write(self, angle): [self.min_angle_in, self.max_angle_in], [self.min_angle_out, self.max_angle_out], ) - angle = max(self.min_angle_out, min(angle, self.max_angle_out)) # clamp + angle = int(max(self.min_angle_out, min(angle, self.max_angle_out))) # clamp await self.bus.set_single_servo(self.id, angle, 100) def set_servo_angle_service(self, req): @@ -1323,6 +1323,7 @@ def set_servo_angle_service(self, req): return SetServoAngleResponse(True) def callback(self, data): + # TODO: map back to 'angle' self.publisher.publish(data["angle"]) class Hiwonder_Bus: From 111d5bc77f98fb85b004f88e795fad7608d77824 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Thu, 8 Feb 2024 14:40:02 +0100 Subject: [PATCH 37/77] Fix bool response --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 4955a46d..ab6b52ad 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1306,8 +1306,8 @@ async def start(self): ) def set_servo_enabled_service(self, req): - asyncio.run(self.bus.set_enabled(self.id, req.value)) - return SetBoolResponse(True) + asyncio.run(self.bus.set_enabled(self.id, req.data)) + return SetBoolResponse(True, "enabled" if req.data else "disabled") async def servo_write(self, angle): angle = scale( @@ -1366,8 +1366,8 @@ async def start(self): # TODO: add service to update multiple servos def set_all_servos_enabled(self, req): - asyncio.run(self.bus.set_enabled_all(req.value)) - return SetBoolResponse(True) + asyncio.run(self.set_enabled_all(req.data)) + return SetBoolResponse(True, "enabled" if req.data else "disabled") async def callback(self, data): try: From c1ce24fbac74dffaecd3cdd5a3ef29ab6bd07e21 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Thu, 8 Feb 2024 14:46:05 +0100 Subject: [PATCH 38/77] Fix styling and add pylintrc --- .pylintrc | 638 ++++++++++++++++++ .../scripts/ROS_telemetrix_aio_api.py | 4 +- 2 files changed, 640 insertions(+), 2 deletions(-) create mode 100644 .pylintrc diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 00000000..924c77eb --- /dev/null +++ b/.pylintrc @@ -0,0 +1,638 @@ +[MAIN] + +# Analyse import fallback blocks. This can be used to support both Python 2 and +# 3 compatible code, which means that the block might have code that exists +# only in one or another interpreter, leading to false positives when analysed. +analyse-fallback-blocks=no + +# Clear in-memory caches upon conclusion of linting. Useful if running pylint +# in a server-like mode. +clear-cache-post-run=no + +# Load and enable all available extensions. Use --list-extensions to see a list +# all available extensions. +#enable-all-extensions= + +# In error mode, messages with a category besides ERROR or FATAL are +# suppressed, and no reports are done by default. Error mode is compatible with +# disabling specific errors. +#errors-only= + +# Always return a 0 (non-error) status code, even if lint errors are found. +# This is primarily useful in continuous integration scripts. +#exit-zero= + +# A comma-separated list of package or module names from where C extensions may +# be loaded. Extensions are loading into the active Python interpreter and may +# run arbitrary code. +extension-pkg-allow-list= + +# A comma-separated list of package or module names from where C extensions may +# be loaded. Extensions are loading into the active Python interpreter and may +# run arbitrary code. (This is an alternative name to extension-pkg-allow-list +# for backward compatibility.) +extension-pkg-whitelist= + +# Return non-zero exit code if any of these messages/categories are detected, +# even if score is above --fail-under value. Syntax same as enable. Messages +# specified are enabled, while categories only check already-enabled messages. +fail-on= + +# Specify a score threshold under which the program will exit with error. +fail-under=10 + +# Interpret the stdin as a python script, whose filename needs to be passed as +# the module_or_package argument. +#from-stdin= + +# Files or directories to be skipped. They should be base names, not paths. +ignore=CVS + +# Add files or directories matching the regular expressions patterns to the +# ignore-list. The regex matches against paths and can be in Posix or Windows +# format. Because '\\' represents the directory delimiter on Windows systems, +# it can't be used as an escape character. +ignore-paths= + +# Files or directories matching the regular expression patterns are skipped. +# The regex matches against base names, not paths. The default value ignores +# Emacs file locks +ignore-patterns=^\.# + +# List of module names for which member attributes should not be checked +# (useful for modules/projects where namespaces are manipulated during runtime +# and thus existing member attributes cannot be deduced by static analysis). It +# supports qualified module names, as well as Unix pattern matching. +ignored-modules= + +# Python code to execute, usually for sys.path manipulation such as +# pygtk.require(). +#init-hook= + +# Use multiple processes to speed up Pylint. Specifying 0 will auto-detect the +# number of processors available to use, and will cap the count on Windows to +# avoid hangs. +jobs=1 + +# Control the amount of potential inferred values when inferring a single +# object. This can help the performance when dealing with large functions or +# complex, nested conditions. +limit-inference-results=100 + +# List of plugins (as comma separated values of python module names) to load, +# usually to register additional checkers. +load-plugins= + +# Pickle collected data for later comparisons. +persistent=yes + +# Minimum Python version to use for version dependent checks. Will default to +# the version used to run pylint. +py-version=3.8 + +# Discover python modules and packages in the file system subtree. +recursive=no + +# Add paths to the list of the source roots. Supports globbing patterns. The +# source root is an absolute path or a path relative to the current working +# directory used to determine a package namespace for modules located under the +# source root. +source-roots= + +# When enabled, pylint would attempt to guess common misconfiguration and emit +# user-friendly hints instead of false-positive error messages. +suggestion-mode=yes + +# Allow loading of arbitrary C extensions. Extensions are imported into the +# active Python interpreter and may run arbitrary code. +unsafe-load-any-extension=no + +# In verbose mode, extra non-checker-related info will be displayed. +#verbose= + + +[BASIC] + +# Naming style matching correct argument names. +argument-naming-style=snake_case + +# Regular expression matching correct argument names. Overrides argument- +# naming-style. If left empty, argument names will be checked with the set +# naming style. +#argument-rgx= + +# Naming style matching correct attribute names. +attr-naming-style=snake_case + +# Regular expression matching correct attribute names. Overrides attr-naming- +# style. If left empty, attribute names will be checked with the set naming +# style. +#attr-rgx= + +# Bad variable names which should always be refused, separated by a comma. +bad-names=foo, + bar, + baz, + toto, + tutu, + tata + +# Bad variable names regexes, separated by a comma. If names match any regex, +# they will always be refused +bad-names-rgxs= + +# Naming style matching correct class attribute names. +class-attribute-naming-style=any + +# Regular expression matching correct class attribute names. Overrides class- +# attribute-naming-style. If left empty, class attribute names will be checked +# with the set naming style. +#class-attribute-rgx= + +# Naming style matching correct class constant names. +class-const-naming-style=UPPER_CASE + +# Regular expression matching correct class constant names. Overrides class- +# const-naming-style. If left empty, class constant names will be checked with +# the set naming style. +#class-const-rgx= + +# Naming style matching correct class names. +class-naming-style=PascalCase + +# Regular expression matching correct class names. Overrides class-naming- +# style. If left empty, class names will be checked with the set naming style. +#class-rgx= + +# Naming style matching correct constant names. +const-naming-style=UPPER_CASE + +# Regular expression matching correct constant names. Overrides const-naming- +# style. If left empty, constant names will be checked with the set naming +# style. +#const-rgx= + +# Minimum line length for functions/classes that require docstrings, shorter +# ones are exempt. +docstring-min-length=-1 + +# Naming style matching correct function names. +function-naming-style=snake_case + +# Regular expression matching correct function names. Overrides function- +# naming-style. If left empty, function names will be checked with the set +# naming style. +#function-rgx= + +# Good variable names which should always be accepted, separated by a comma. +good-names=i, + j, + k, + ex, + Run, + _ + +# Good variable names regexes, separated by a comma. If names match any regex, +# they will always be accepted +good-names-rgxs= + +# Include a hint for the correct naming format with invalid-name. +include-naming-hint=no + +# Naming style matching correct inline iteration names. +inlinevar-naming-style=any + +# Regular expression matching correct inline iteration names. Overrides +# inlinevar-naming-style. If left empty, inline iteration names will be checked +# with the set naming style. +#inlinevar-rgx= + +# Naming style matching correct method names. +method-naming-style=snake_case + +# Regular expression matching correct method names. Overrides method-naming- +# style. If left empty, method names will be checked with the set naming style. +#method-rgx= + +# Naming style matching correct module names. +module-naming-style=snake_case + +# Regular expression matching correct module names. Overrides module-naming- +# style. If left empty, module names will be checked with the set naming style. +#module-rgx= + +# Colon-delimited sets of names that determine each other's naming style when +# the name regexes allow several styles. +name-group= + +# Regular expression which should only match function or class names that do +# not require a docstring. +no-docstring-rgx=^_ + +# List of decorators that produce properties, such as abc.abstractproperty. Add +# to this list to register other decorators that produce valid properties. +# These decorators are taken in consideration only for invalid-name. +property-classes=abc.abstractproperty + +# Regular expression matching correct type alias names. If left empty, type +# alias names will be checked with the set naming style. +#typealias-rgx= + +# Regular expression matching correct type variable names. If left empty, type +# variable names will be checked with the set naming style. +#typevar-rgx= + +# Naming style matching correct variable names. +variable-naming-style=snake_case + +# Regular expression matching correct variable names. Overrides variable- +# naming-style. If left empty, variable names will be checked with the set +# naming style. +#variable-rgx= + + +[CLASSES] + +# Warn about protected attribute access inside special methods +check-protected-access-in-special-methods=no + +# List of method names used to declare (i.e. assign) instance attributes. +defining-attr-methods=__init__, + __new__, + setUp, + asyncSetUp, + __post_init__ + +# List of member names, which should be excluded from the protected access +# warning. +exclude-protected=_asdict,_fields,_replace,_source,_make,os._exit + +# List of valid names for the first argument in a class method. +valid-classmethod-first-arg=cls + +# List of valid names for the first argument in a metaclass class method. +valid-metaclass-classmethod-first-arg=mcs + + +[DESIGN] + +# List of regular expressions of class ancestor names to ignore when counting +# public methods (see R0903) +exclude-too-few-public-methods= + +# List of qualified class names to ignore when counting class parents (see +# R0901) +ignored-parents= + +# Maximum number of arguments for function / method. +max-args=5 + +# Maximum number of attributes for a class (see R0902). +max-attributes=7 + +# Maximum number of boolean expressions in an if statement (see R0916). +max-bool-expr=5 + +# Maximum number of branch for function / method body. +max-branches=12 + +# Maximum number of locals for function / method body. +max-locals=15 + +# Maximum number of parents for a class (see R0901). +max-parents=7 + +# Maximum number of public methods for a class (see R0904). +max-public-methods=20 + +# Maximum number of return / yield for function / method body. +max-returns=6 + +# Maximum number of statements in function / method body. +max-statements=50 + +# Minimum number of public methods for a class (see R0903). +min-public-methods=2 + + +[EXCEPTIONS] + +# Exceptions that will emit a warning when caught. +overgeneral-exceptions=builtins.BaseException,builtins.Exception + + +[FORMAT] + +# Expected format of line ending, e.g. empty (any line ending), LF or CRLF. +expected-line-ending-format= + +# Regexp for a line that is allowed to be longer than the limit. +ignore-long-lines=^\s*(# )??$ + +# Number of spaces of indent required inside a hanging or continued line. +indent-after-paren=4 + +# String used as indentation unit. This is usually " " (4 spaces) or "\t" (1 +# tab). +indent-string=' ' + +# Maximum number of characters on a single line. +max-line-length=100 + +# Maximum number of lines in a module. +max-module-lines=1000 + +# Allow the body of a class to be on the same line as the declaration if body +# contains single statement. +single-line-class-stmt=no + +# Allow the body of an if to be on the same line as the test if there is no +# else. +single-line-if-stmt=no + + +[IMPORTS] + +# List of modules that can be imported at any level, not just the top level +# one. +allow-any-import-level= + +# Allow explicit reexports by alias from a package __init__. +allow-reexport-from-package=no + +# Allow wildcard imports from modules that define __all__. +allow-wildcard-with-all=no + +# Deprecated modules which should not be used, separated by a comma. +deprecated-modules= + +# Output a graph (.gv or any supported image format) of external dependencies +# to the given file (report RP0402 must not be disabled). +ext-import-graph= + +# Output a graph (.gv or any supported image format) of all (i.e. internal and +# external) dependencies to the given file (report RP0402 must not be +# disabled). +import-graph= + +# Output a graph (.gv or any supported image format) of internal dependencies +# to the given file (report RP0402 must not be disabled). +int-import-graph= + +# Force import order to recognize a module as part of the standard +# compatibility libraries. +known-standard-library= + +# Force import order to recognize a module as part of a third party library. +known-third-party=enchant + +# Couples of modules and preferred modules, separated by a comma. +preferred-modules= + + +[LOGGING] + +# The type of string formatting that logging methods do. `old` means using % +# formatting, `new` is for `{}` formatting. +logging-format-style=old + +# Logging modules to check that the string format arguments are in logging +# function parameter format. +logging-modules=logging + + +[MESSAGES CONTROL] + +# Only show warnings with the listed confidence levels. Leave empty to show +# all. Valid levels: HIGH, CONTROL_FLOW, INFERENCE, INFERENCE_FAILURE, +# UNDEFINED. +confidence=HIGH, + CONTROL_FLOW, + INFERENCE, + INFERENCE_FAILURE, + UNDEFINED + +# Disable the message, report, category or checker with the given id(s). You +# can either give multiple identifiers separated by comma (,) or put this +# option multiple times (only on the command line, not in the configuration +# file where it should appear only once). You can also use "--disable=all" to +# disable everything first and then re-enable specific checks. For example, if +# you want to run only the similarities checker, you can use "--disable=all +# --enable=similarities". If you want to run only the classes checker, but have +# no Warning level messages displayed, use "--disable=all --enable=classes +# --disable=W". +disable=raw-checker-failed, + bad-inline-option, + locally-disabled, + file-ignored, + suppressed-message, + useless-suppression, + deprecated-pragma, + use-implicit-booleaness-not-comparison-to-string, + use-implicit-booleaness-not-comparison-to-zero, + use-symbolic-message-instead, + missing-function-docstring, # TODO: eventually we should enable this + missing-module-docstring, + invalid-name, + import_error + +# Enable the message, report, category or checker with the given id(s). You can +# either give multiple identifier separated by comma (,) or put this option +# multiple time (only on the command line, not in the configuration file where +# it should appear only once). See also the "--disable" option for examples. +enable= + + +[METHOD_ARGS] + +# List of qualified names (i.e., library.method) which require a timeout +# parameter e.g. 'requests.api.get,requests.api.post' +timeout-methods=requests.api.delete,requests.api.get,requests.api.head,requests.api.options,requests.api.patch,requests.api.post,requests.api.put,requests.api.request + + +[MISCELLANEOUS] + +# List of note tags to take in consideration, separated by a comma. +notes=FIXME, + XXX, + TODO + +# Regular expression of note tags to take in consideration. +notes-rgx= + + +[REFACTORING] + +# Maximum number of nested blocks for function / method body +max-nested-blocks=5 + +# Complete name of functions that never returns. When checking for +# inconsistent-return-statements if a never returning function is called then +# it will be considered as an explicit return statement and no message will be +# printed. +never-returning-functions=sys.exit,argparse.parse_error + + +[REPORTS] + +# Python expression which should return a score less than or equal to 10. You +# have access to the variables 'fatal', 'error', 'warning', 'refactor', +# 'convention', and 'info' which contain the number of messages in each +# category, as well as 'statement' which is the total number of statements +# analyzed. This score is used by the global evaluation report (RP0004). +evaluation=max(0, 0 if fatal else 10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10)) + +# Template used to display messages. This is a python new-style format string +# used to format the message information. See doc for all details. +msg-template= + +# Set the output format. Available formats are: text, parseable, colorized, +# json2 (improved json format), json (old json format) and msvs (visual +# studio). You can also give a reporter class, e.g. +# mypackage.mymodule.MyReporterClass. +#output-format= + +# Tells whether to display a full report or only the messages. +reports=no + +# Activate the evaluation score. +score=yes + + +[SIMILARITIES] + +# Comments are removed from the similarity computation +ignore-comments=yes + +# Docstrings are removed from the similarity computation +ignore-docstrings=yes + +# Imports are removed from the similarity computation +ignore-imports=yes + +# Signatures are removed from the similarity computation +ignore-signatures=yes + +# Minimum lines number of a similarity. +min-similarity-lines=4 + + +[SPELLING] + +# Limits count of emitted suggestions for spelling mistakes. +max-spelling-suggestions=4 + +# Spelling dictionary name. Available dictionaries: en_GB (aspell), en_US +# (hunspell), en_AU (aspell), en (aspell), en_CA (aspell). +spelling-dict= + +# List of comma separated words that should be considered directives if they +# appear at the beginning of a comment and should not be checked. +spelling-ignore-comment-directives=fmt: on,fmt: off,noqa:,noqa,nosec,isort:skip,mypy: + +# List of comma separated words that should not be checked. +spelling-ignore-words= + +# A path to a file that contains the private dictionary; one word per line. +spelling-private-dict-file= + +# Tells whether to store unknown words to the private dictionary (see the +# --spelling-private-dict-file option) instead of raising a message. +spelling-store-unknown-words=no + + +[STRING] + +# This flag controls whether inconsistent-quotes generates a warning when the +# character used as a quote delimiter is used inconsistently within a module. +check-quote-consistency=no + +# This flag controls whether the implicit-str-concat should generate a warning +# on implicit string concatenation in sequences defined over several lines. +check-str-concat-over-line-jumps=no + + +[TYPECHECK] + +# List of decorators that produce context managers, such as +# contextlib.contextmanager. Add to this list to register other decorators that +# produce valid context managers. +contextmanager-decorators=contextlib.contextmanager + +# List of members which are set dynamically and missed by pylint inference +# system, and so shouldn't trigger E1101 when accessed. Python regular +# expressions are accepted. +generated-members= + +# Tells whether to warn about missing members when the owner of the attribute +# is inferred to be None. +ignore-none=yes + +# This flag controls whether pylint should warn about no-member and similar +# checks whenever an opaque object is returned when inferring. The inference +# can return multiple potential results while evaluating a Python object, but +# some branches might not be evaluated, which results in partial inference. In +# that case, it might be useful to still emit no-member and other checks for +# the rest of the inferred objects. +ignore-on-opaque-inference=yes + +# List of symbolic message names to ignore for Mixin members. +ignored-checks-for-mixins=no-member, + not-async-context-manager, + not-context-manager, + attribute-defined-outside-init + +# List of class names for which member attributes should not be checked (useful +# for classes with dynamically set attributes). This supports the use of +# qualified names. +ignored-classes=optparse.Values,thread._local,_thread._local,argparse.Namespace + +# Show a hint with possible names when a member name was not found. The aspect +# of finding the hint is based on edit distance. +missing-member-hint=yes + +# The minimum edit distance a name should have in order to be considered a +# similar match for a missing member name. +missing-member-hint-distance=1 + +# The total number of similar names that should be taken in consideration when +# showing a hint for a missing member. +missing-member-max-choices=1 + +# Regex pattern to define which classes are considered mixins. +mixin-class-rgx=.*[Mm]ixin + +# List of decorators that change the signature of a decorated function. +signature-mutators= + + +[VARIABLES] + +# List of additional names supposed to be defined in builtins. Remember that +# you should avoid defining new builtins when possible. +additional-builtins= + +# Tells whether unused global variables should be treated as a violation. +allow-global-unused-variables=yes + +# List of names allowed to shadow builtins +allowed-redefined-builtins= + +# List of strings which can identify a callback function by name. A callback +# name must start or end with one of those strings. +callbacks=cb_, + _cb + +# A regular expression matching the name of dummy variables (i.e. expected to +# not be used). +dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_ + +# Argument names that match this expression will be ignored. +ignored-argument-names=_.*|^ignored_|^unused_ + +# Tells whether we should check for unused import in __init__ files. +init-import=no + +# List of qualified module names which can have objects that can redefine +# builtins. +redefining-builtins-modules=six.moves,past.builtins,future.builtins,builtins,io diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index ab6b52ad..e3fcc73a 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1326,6 +1326,7 @@ def callback(self, data): # TODO: map back to 'angle' self.publisher.publish(data["angle"]) + class Hiwonder_Bus: def __init__(self, board, module_name, module): self.name = module_name @@ -1348,7 +1349,6 @@ async def start(self): await servo.start() self.servos[servo_name] = servo self.servos[servo.id] = servo - updaters = await self.board.modules.add_hiwonder_servo( uart, rx, tx, ids, self.callback @@ -1371,7 +1371,7 @@ def set_all_servos_enabled(self, req): async def callback(self, data): try: - for servo_update in data: + for servo_update in data: sid = servo_update["id"] if sid in self.servos: self.servos[sid].callback(servo_update) From bbe88c56acbdaf8809e6ff1bf9ef27c88ee611d9 Mon Sep 17 00:00:00 2001 From: Arendjan Date: Fri, 9 Feb 2024 09:25:04 +0000 Subject: [PATCH 39/77] Add ps3 holonomic control + config master01 --- mirte_bringup/launch/minimal_master.launch | 2 +- mirte_control/config/control_master.yaml | 14 ++-- .../config/mirte_master_config.yaml | 68 +++++++++---------- mirte_teleop/config/ps3-holonomic.config.yaml | 12 ++++ mirte_teleop/launch/teleop_holo_joy.launch | 17 +++++ mirte_teleop/launch/teleopkey.launch | 4 +- 6 files changed, 73 insertions(+), 44 deletions(-) create mode 100644 mirte_teleop/config/ps3-holonomic.config.yaml create mode 100644 mirte_teleop/launch/teleop_holo_joy.launch diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index e539b031..4775f865 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -3,7 +3,7 @@ - + + + + diff --git a/mirte_teleop/launch/teleopkey.launch b/mirte_teleop/launch/teleopkey.launch index 22fbc4d4..d7b1e6e9 100644 --- a/mirte_teleop/launch/teleopkey.launch +++ b/mirte_teleop/launch/teleopkey.launch @@ -4,8 +4,8 @@ - - + + From bfb21551798a9f5fa21f1bea998840dcf217b181 Mon Sep 17 00:00:00 2001 From: Arendjan Date: Fri, 9 Feb 2024 10:18:09 +0000 Subject: [PATCH 40/77] Add encoder pins config --- .../config/mirte_master_config.yaml | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 30865b6f..5161fc62 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -23,6 +23,32 @@ modules: right_rear: pin_A: 7 pin_B: 6 +encoder: + left_front: + name: left_front + device: mirte + pins: + A: 21 + B: 20 + left_rear: + name: left_rear + device: mirte + pins: + A: 17 + B: 16 + right_front: + name: right_front + device: mirte + pins: + A: 18 + B: 19 + right_rear: + name: right_rear + device: mirte + pins: + A: 15 + B: 14 + # servos: # servo1: # pin: 1 From 0920c4c37b88bfe6244a01f7597ca87e8f077a33 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Fri, 9 Feb 2024 13:37:04 +0000 Subject: [PATCH 41/77] fixes martin arm --- mirte_bringup/launch/minimal_master.launch | 30 +++---------------- .../config/mirte_master_config.yaml | 14 ++------- .../scripts/ROS_telemetrix_aio_api.py | 2 +- ros_control_boilerplate | 1 + 4 files changed, 9 insertions(+), 38 deletions(-) create mode 160000 ros_control_boilerplate diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index e539b031..74bbef73 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -3,7 +3,7 @@ - + - - - + + + - - - - - - - - - - - - - - - - - diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index ef9c2275..18e16200 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -29,9 +29,9 @@ modules: servo_controller: device: mirte type: Hiwonder_Servo - uart: 1 - rx_pin: 4 - tx_pin: 5 + uart: 0 + rx_pin: 0 + tx_pin: 1 servos: servoH1: id: 3 @@ -41,11 +41,3 @@ modules: id: 4 min_angle: 800 max_angle: 4000 - power_watcher: - device: mirte - type: INA226 - connector: I2C1 - id: 0x41 - min_voltage: 11 - max_current: 1 - max_voltage: 15 \ No newline at end of file diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index e3fcc73a..99597ec0 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1302,7 +1302,7 @@ async def start(self): self.set_servo_enabled_service, ) self.publisher = rospy.Publisher( - f"/mirte/servos/{self.name}/position", Int32, queue_size=1 + f"/mirte/servos/{self.name}/position", Int32, queue_size=1, latch=True ) def set_servo_enabled_service(self, req): diff --git a/ros_control_boilerplate b/ros_control_boilerplate new file mode 160000 index 00000000..0f9cd01b --- /dev/null +++ b/ros_control_boilerplate @@ -0,0 +1 @@ +Subproject commit 0f9cd01bc1430c718554a3801683ada9cd2aa0a6 From b43139c78613fd4e12e072c7e7e8598a2dba1b91 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 13 Feb 2024 14:34:38 +0000 Subject: [PATCH 42/77] Add shutdown relay --- .../config/mirte_master_config.yaml | 7 +++- .../scripts/ROS_telemetrix_aio_api.py | 42 ++++++++++++++++++- 2 files changed, 46 insertions(+), 3 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index ef9c2275..6756a1aa 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -45,7 +45,10 @@ modules: device: mirte type: INA226 connector: I2C1 - id: 0x41 + id: 0b1000101 min_voltage: 11 max_current: 1 - max_voltage: 15 \ No newline at end of file + max_voltage: 15 + turn_off_pin: 27 + turn_off_value: True + turn_off_time: 10 \ No newline at end of file diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index e3fcc73a..aa0daf52 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -12,7 +12,7 @@ from tmx_pico_aio import tmx_pico_aio from telemetrix_aio import telemetrix_aio from typing import Literal, Tuple - +import subprocess # Import the right Telemetrix AIO devices = rospy.get_param("/mirte/device") @@ -1215,6 +1215,12 @@ def __init__(self, board, module_name, module): self.min_voltage = module["min_voltage"] if "min_voltage" in module else -1 self.max_voltage = module["max_voltage"] if "max_voltage" in module else -1 self.max_current = module["max_current"] if "max_current" in module else -1 + self.turn_off_pin = board_mapping.pin_name_to_pin_number(module["turn_off_pin"]) if "turn_off_pin" in module else -1 + self.turn_off_pin_value = module["turn_off_pin_value"] if "turn_off_pin_value" in module else True # what should be the output when the relay should be opened. + self.turn_off_time = module["turn_off_time"] if "turn_off_time" in module else 30 # time to wait for computer to shut down + self.enable_turn_off = False # require at least a single message with a real value before arming the turn off system + self.shutdown_triggered = False + self.turn_off_trigger_start_time = -1 self.ina_publisher = rospy.Publisher( "/mirte/power/" + module_name, BatteryState, queue_size=1 ) @@ -1244,6 +1250,8 @@ async def start(self): id = self.module["id"] await self.board.sensors.add_ina226(self.i2c_port, self.callback, id) + if(self.turn_off_pin != -1): + self.trigger_shutdown_relay = await self.board.modules.add_shutdown_relay(self.turn_off_pin, not not self.turn_off_pin_value, self.turn_off_time+10) async def callback(self, data): # TODO: move this decoding to the library @@ -1274,7 +1282,39 @@ async def callback(self, data): bs.power_supply_health = 0 # uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 bs.power_supply_technology = 0 # uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 self.ina_publisher.publish(bs) + await self.turn_off_cb() + + async def turn_off_cb(self): + if(self.turn_off_pin == -1): + # Dont turn off when this feature is disabled + return + # make sure that there are real values coming from the ina226 module + if(not self.enable_turn_off): + if(self.voltage > 6 and self.current > 0.3): + self.enable_turn_off = True + rospy.logwarn("Shutdown relay armed") + return + + # at first dip of too low voltage, start timer, when longer than 5s below trigger voltage, then shut down + # this makes sure that a short dip (motor start) does not trigger it + if self.min_voltage != -1 and self.voltage < self.min_voltage: + if(self.turn_off_trigger_start_time == -1): + self.turn_off_trigger_start_time = time.time() + rospy.logwarn("Low voltage, %ss till shutdown.", 5- (time.time()-self.turn_off_trigger_start_time)) + else: + self.turn_off_trigger_start_time = -1 + + if(self.turn_off_trigger_start_time != -1 and time.time()-self.turn_off_trigger_start_time > 5): + await self.shutdown_robot() + + async def shutdown_robot(self): + if(not self.shutdown_triggered): + rospy.logerr("Triggering shutdown, shutting down in 10s") + await self.trigger_shutdown_relay() + self.shutdown_triggered = True + # subprocess.run("sudo shutdown 10s") + class Hiwonder_Servo: def __init__(self, servo_name, servo_obj, bus): From 4f81c7636e9a0df17b7d63476b424783e0a6610f Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 13 Feb 2024 14:49:23 +0000 Subject: [PATCH 43/77] small fixes config --- mirte_bringup/launch/minimal_master.launch | 2 +- mirte_control_master/CHANGELOG.rst | 99 ------ mirte_control_master/CMakeLists.txt | 205 ------------ mirte_control_master/config/control.yaml | 68 ---- .../config/my_robot_control.yaml | 24 -- .../include/my_robot_hw_interface.h | 306 ------------------ mirte_control_master/launch/control.launch | 13 - mirte_control_master/package.xml | 49 --- mirte_control_master/scripts/mock.py | 42 --- mirte_control_master/src/my_robot_base.cpp | 43 --- .../config/mirte_master_config.yaml | 9 +- ros_control_boilerplate | 1 - 12 files changed, 7 insertions(+), 854 deletions(-) delete mode 100644 mirte_control_master/CHANGELOG.rst delete mode 100644 mirte_control_master/CMakeLists.txt delete mode 100644 mirte_control_master/config/control.yaml delete mode 100644 mirte_control_master/config/my_robot_control.yaml delete mode 100644 mirte_control_master/include/my_robot_hw_interface.h delete mode 100644 mirte_control_master/launch/control.launch delete mode 100644 mirte_control_master/package.xml delete mode 100755 mirte_control_master/scripts/mock.py delete mode 100644 mirte_control_master/src/my_robot_base.cpp delete mode 160000 ros_control_boilerplate diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index 74bbef73..c1bdfc63 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -24,7 +24,7 @@ - + `_ from ridgeback/RPSW-119 - Added the ability to disable using the MCU -* [ridgeback_base] Added param type for use_mcu. -* [ridgeback_base] Added lighting and cooling under use_mcu flag. -* [ridgeback_base] Added envar for using MCU. -* Contributors: Tony Baltovski - -0.2.3 (2019-03-23) ------------------- -* [ridgeback_base] Updated compute_calibration to use a MagneticField message. -* Contributors: Tony Baltovski - -0.2.1 (2018-08-02) ------------------- - -0.2.0 (2018-05-23) ------------------- -* [ridgeback_base] Added absolute value checking for fans. -* Updated to package format 2. -* [ridgeback_base] Switched to rosserial_server_udp. -* Updated bringup for kinetic -* [ridgeback_base] Fixed a typo. -* Updated maintainer. -* Fixed temperature warning for PCB and MCU temperature. -* Contributors: Dave Niewinski, Tony Baltovski - -0.1.7 (2016-10-03) ------------------- -* Removed GPS dependencies. -* Contributors: Johannes Meyer, Tony Baltovski - -0.1.6 (2016-04-22) ------------------- -* Combined hostname and version for hardware ID. -* Fixed initialization of constant doubles. -* Added hardware ID based hostname. -* Contributors: Tony Baltovski - -0.1.5 (2016-03-02) ------------------- -* Fixed state order for lighting. -* Separated passive joint into header. -* Contributors: Tony Baltovski - -0.1.4 (2015-12-01) ------------------- - -0.1.3 (2015-11-20) ------------------- -* Install the ridgeback_node target. -* Contributors: Mike Purvis - -0.1.2 (2015-11-20) ------------------- -* Fixed dependency. -* Contributors: Tony Baltovski - -0.1.1 (2015-11-20) ------------------- -* Added lighting. -* Contributors: Tony Baltovski - -0.1.0 (2015-11-19) ------------------- -* Initial Ridgeback release. -* Contributors: Mike Purvis, Tony Baltovski diff --git a/mirte_control_master/CMakeLists.txt b/mirte_control_master/CMakeLists.txt deleted file mode 100644 index 939a32f1..00000000 --- a/mirte_control_master/CMakeLists.txt +++ /dev/null @@ -1,205 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(mirte_control_master) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++17) # -Wall -Wextra -Wpedantic -Werror) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - mirte_msgs - controller_manager - hardware_interface - -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -#add_message_files( -# FILES -# MotorCommand.msg -#) - -## Generate services in the 'srv' folder -#add_service_files( - # FILES - # get_distance.srv -#) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -# std_msgs -#) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES mirte_ros_package - CATKIN_DEPENDS roscpp std_msgs mirte_msgs message_runtime controller_manager hardware_interface -# DEPENDS system_lib -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include/ - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/mobile_controller.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node - src/my_robot_base.cpp -) - -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node - ${catkin_LIBRARIES} - ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variablese -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mirte_ros_package.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/mirte_control_master/config/control.yaml b/mirte_control_master/config/control.yaml deleted file mode 100644 index 7e45e98c..00000000 --- a/mirte_control_master/config/control.yaml +++ /dev/null @@ -1,68 +0,0 @@ -my_robot: - type: "joint_state_controller/JointStateController" - publish_rate: 50 - -mobile_base_controller: - type: "mecanum_drive_controller/MecanumDriveController" - front_left_wheel_joint: "wheel_left_front_joint" - back_left_wheel_joint: "wheel_left_rear_joint" - front_right_wheel_joint: "wheel_right_front_joint" - back_right_wheel_joint: "wheel_right_rear_joint" - publish_rate: 50 - pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03] - cmd_vel_timeout: 0.25 - - # Override URDF look-up for wheel separation since the parent link is not the base_link. - wheel_separation_x: 0.638 - wheel_separation_y: 0.551 - wheel_radius: 0.165 - # Odometry fused with IMU is published by robot_localization, so - # no need to publish a TF based on encoders alone. - enable_odom_tf: true - - # Wheel separation and radius multipliers - wheel_separation_multiplier: 1.0 # default: 1.0 - wheel_radius_multiplier : 1.0 # default: 1.0 - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - max_velocity : 1.1 # m/s - has_acceleration_limits: true - max_acceleration : 2.5 # m/s^2 - y: - has_velocity_limits : true - max_velocity : 1.1 # m/s - has_acceleration_limits: true - max_acceleration : 2.5 # m/s^2 - angular: - z: - has_velocity_limits : true - max_velocity : 2.0 # rad/s - has_acceleration_limits: true - max_acceleration : 1.0 # rad/s^2 - -# ekf_localization: -# frequency: 50 -# two_d_mode: true -# odom0: /ridgeback_velocity_controller/odom -# odom0_config: [false, false, false, -# false, false, false, -# true, true, false, -# false, false, true, -# false, false, false] -# odom0_differential: false -# imu0: /imu/data -# imu0_config: [false, false, false, -# false, false, false, -# false, false, false, -# false, false, true, -# true, true, false] -# imu0_differential: false -# odom_frame: odom -# base_link_frame: base_link -# world_frame: odom -# predict_to_current_time: true diff --git a/mirte_control_master/config/my_robot_control.yaml b/mirte_control_master/config/my_robot_control.yaml deleted file mode 100644 index cb792f92..00000000 --- a/mirte_control_master/config/my_robot_control.yaml +++ /dev/null @@ -1,24 +0,0 @@ -# .yaml config file -# -# The PID gains and controller settings must be saved in a yaml file that gets loaded -# to the param server via the roslaunch file (my_robot_control.launch). - -my_robot: - # Publish all joint states ----------------------------------- - # Creates the /joint_states topic necessary in ROS - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - - # Effort Controllers --------------------------------------- - leftWheel_effort_controller: - type: effort_controllers/JointEffortController - joint: left_wheel_hinge - pid: {p: 100.0, i: 0.1, d: 10.0} - #pid: {p: 50.0, i: 0.1, d: 0.0} - rightWheel_effort_controller: - type: effort_controllers/JointEffortController - joint: right_wheel_hinge - pid: {p: 100.0, i: 0.1, d: 10.0} - #pid: {p: 1.0, i: 1.0, d: 0.0} -# TODO: extend this for the mirte-master \ No newline at end of file diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h deleted file mode 100644 index 5d80635f..00000000 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ /dev/null @@ -1,306 +0,0 @@ -// https://slaterobots.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot -// Roughly based on: -// https://github.com/eborghi10/my_ROS_mobile_robot/blob/master/my_robot_base/include/my_robot_hw_interface.h -// https://github.com/PickNikRobotics/ros_control_boilerplate -// https://github.com/DeborggraeveR/ampru - -// https://github.com/resibots/dynamixel_control_hw/blob/master/include/dynamixel_control_hw/hardware_interface.hpp -// https://github.com/FRC900/2018RobotCode/blob/master/zebROS_ws/src/ros_control_boilerplate/include/ros_control_boilerplate/frcrobot_hw_interface.h -// INTERESTING CLEAN ONE: -// https://github.com/ros-controls/ros_controllers/blob/indigo-devel/diff_drive_controller/test/diffbot.h - -#pragma once -#define _USE_MATH_DEFINES - -// ROS -#include -#include -#include -#include - -// ros_control -#include -#include -#include -#include - -// ostringstream -#include -#include -#include - -#include -#include -#include -#include -#include - -// const unsigned int NUM_JOINTS = 4; -const auto service_format = "/mirte/set_%s_speed"; - -/// \brief Hardware interface for a robot -class MyRobotHWInterface : public hardware_interface::RobotHW { -public: - MyRobotHWInterface(); - - bool write_single(int joint, int speed) { - - int speed_mapped = - std::max(std::min(int(speed / (6 * M_PI) * 100), 100), -100); - if (speed_mapped != _last_cmd[joint]) { - service_requests[joint].request.speed = speed_mapped; - _last_cmd[joint] = speed_mapped; - if (!service_clients[joint].call(service_requests[joint])) { - this->start_reconnect(); - return false; - } - } - return true; - } - /* - * - */ - void write() { - if (running_) { - // make sure the clients don't get overwritten while calling them - const std::lock_guard lock(this->service_clients_mutex); - - // cmd[0] = ros_control calculated speed of left motor in rad/s - // cmd[1] = ros_control calculated speed of right motor in rad/s - - // This function converts cmd[0] to pwm and calls that service - - // NOTE: this *highly* depends on the voltage of the motors!!!! - // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) - // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s - // (6*pi) - for (size_t i = 0; i < NUM_JOINTS; i++) { - if (!write_single(i, cmd[i])) { - return; - } - } - // Set the direction in so the read() can use it - // TODO: this does not work properly, because at the end of a series - // cmd_vel is negative, while the rotation is not - for (size_t i = 0; i < NUM_JOINTS; i++) { - _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; - } - } - } - - double meter_per_enc_tick() { - return (_wheel_diameter / 2) * 2 * M_PI / - 40.0; // TODO: get ticks from parameter server - } - - /** - * Reading encoder values and setting position and velocity of encoders - */ - void read_single(int joint, const ros::Duration &period) { - auto diff = _wheel_encoder[joint] - _last_value[joint]; - _last_value[joint] = _wheel_encoder[joint]; - double meterPerEncoderTick = meter_per_enc_tick(); - double distance; - if (bidirectional) { // if encoder is counting bidirectional, then it - // decreases by itself, dont want to use - // last_wheel_cmd_direction - distance = diff * meterPerEncoderTick * 1.0; - } else { - distance = - diff * meterPerEncoderTick * _last_wheel_cmd_direction[joint] * 1.0; - } - pos[joint] += distance; - vel[joint] = distance / period.toSec(); // WHY: was this turned off? - } - - /** - * Reading encoder values and setting position and velocity of encoders - */ - void read(const ros::Duration &period) { - - for (size_t i = 0; i < NUM_JOINTS; i++) { - this->read_single(i, period); - } - } - - ros::NodeHandle nh; - ros::NodeHandle private_nh; - -private: - hardware_interface::JointStateInterface jnt_state_interface; - hardware_interface::VelocityJointInterface jnt_vel_interface; - std::vector cmd; - std::vector pos; - std::vector vel; - std::vector eff; - - bool running_ = true; - double _wheel_diameter; - double _max_speed; - std::vector _wheel_encoder; - std::vector _last_cmd; - std::vector _last_value; - std::vector _last_wheel_cmd_direction; - - ros::Time curr_update_time, prev_update_time; - - std::vector wheel_encoder_subs_; - ros::ServiceServer start_srv_; - ros::ServiceServer stop_srv_; - - std::vector service_clients; - std::vector service_requests; - std::vector joints; - bool start_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { - running_ = true; - return true; - } - - bool stop_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { - running_ = false; - return true; - } - - void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, - int joint) { - if (msg->value < 0) { - bidirectional = true; - } - _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; - } - - // Thread and function to restart service clients when the service server has - // restarted - std::future reconnect_thread; - void init_service_clients(); - void start_reconnect(); - std::mutex service_clients_mutex; - - bool bidirectional = false; // assume it is one direction, when receiving any - // negative value, it will be set to true - unsigned int NUM_JOINTS = 2; -}; // class - -void MyRobotHWInterface::init_service_clients() { - for (auto joint : this->joints) { - auto service = (boost::format(service_format) % joint).str(); - ROS_INFO_STREAM("Waiting for service " << service); - ros::service::waitForService(service, -1); - } - { - const std::lock_guard lock(this->service_clients_mutex); - for (size_t i = 0; i < NUM_JOINTS; i++) { - service_clients.push_back(nh.serviceClient( - (boost::format(service_format) % this->joints[i]).str(), true)); - service_requests.push_back(mirte_msgs::SetMotorSpeed()); - } - } -} - -unsigned int detect_joints(ros::NodeHandle &nh) { - std::string type; - nh.param("/mobile_base_controller/type", type, ""); - if (type.rfind("mecanum", 0) == 0) { // starts with mecanum - return 4; - } else if (type.rfind("diff", 0) == 0) { // starts with diff - return 2; - } else { - ROS_ERROR_STREAM("Unknown type: " << type); - return 0; - } -} - -MyRobotHWInterface::MyRobotHWInterface() - : private_nh("~"), running_(true), - start_srv_(nh.advertiseService( - "start", &MyRobotHWInterface::start_callback, this)), - stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, - this)) { - private_nh.param("wheel_diameter", _wheel_diameter, 0.06); - private_nh.param("max_speed", _max_speed, 2.0); - // private_nh.param("bidirectional", bidirectional, true); - this->NUM_JOINTS = detect_joints(private_nh); - // Initialize raw data - for (size_t i = 0; i < NUM_JOINTS; i++) { - _wheel_encoder.push_back(0); - _last_value.push_back(0); - _last_wheel_cmd_direction.push_back(0); - _last_cmd.push_back(0); - pos.push_back(0); - vel.push_back(0); - eff.push_back(0); - cmd.push_back(0); - } - assert(_wheel_encoder.size() == NUM_JOINTS); - assert(_last_value.size() == NUM_JOINTS); - assert(_last_wheel_cmd_direction.size() == NUM_JOINTS); - assert(_last_cmd.size() == NUM_JOINTS); - assert(pos.size() == NUM_JOINTS); - assert(vel.size() == NUM_JOINTS); - assert(eff.size() == NUM_JOINTS); - assert(cmd.size() == NUM_JOINTS); - - this->joints = {"left", // Edit the control.yaml when using this for the - // normal mirte as well - "right"}; - if (NUM_JOINTS == 4) { - this->joints = {"left_front", - "left_rear", // TODO: check ordering - "right_front", "right_rear"}; - } - std::cout << "Initializing MyRobotHWInterface with " << NUM_JOINTS - << " joints" << std::endl; - - // connect and register the joint state and velocity interfaces - for (unsigned int i = 0; i < NUM_JOINTS; ++i) { - std::string joint = - (boost::format("wheel_%s_joint") % this->joints[i]).str(); - hardware_interface::JointStateHandle state_handle(joint, &pos[i], &vel[i], - &eff[i]); - jnt_state_interface.registerHandle(state_handle); - - hardware_interface::JointHandle vel_handle( - jnt_state_interface.getHandle(joint), &cmd[i]); - jnt_vel_interface.registerHandle(vel_handle); - } - registerInterface(&jnt_state_interface); - registerInterface(&jnt_vel_interface); - - // Initialize publishers and subscribers - for (size_t i = 0; i < NUM_JOINTS; i++) { - auto encoder_topic = - (boost::format(service_format) % this->joints[i]).str(); - wheel_encoder_subs_.push_back(nh.subscribe( - encoder_topic, 1, - boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i))); - } - assert(joints.size() == NUM_JOINTS); - this->init_service_clients(); - assert(service_requests.size() == NUM_JOINTS); - - assert(service_clients.size() == NUM_JOINTS); -} - -void MyRobotHWInterface ::start_reconnect() { - using namespace std::chrono_literals; - - if (this->reconnect_thread.valid()) { // does it already exist or not? - - // Use wait_for() with zero milliseconds to check thread status. - auto status = this->reconnect_thread.wait_for(0ms); - - if (status != - std::future_status::ready) { // Still running -> already reconnecting - return; - } - } - - /* Run the reconnection on a different thread to not pause the ros-control - loop. The launch policy std::launch::async makes sure that the task is run - asynchronously on a new thread. */ - - this->reconnect_thread = - std::async(std::launch::async, [this] { this->init_service_clients(); }); -} \ No newline at end of file diff --git a/mirte_control_master/launch/control.launch b/mirte_control_master/launch/control.launch deleted file mode 100644 index a9395974..00000000 --- a/mirte_control_master/launch/control.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/mirte_control_master/package.xml b/mirte_control_master/package.xml deleted file mode 100644 index 18c73bf2..00000000 --- a/mirte_control_master/package.xml +++ /dev/null @@ -1,49 +0,0 @@ - - - mirte_control_master - 0.3.2 - Ridgeback's mobility and sensor base. - - Mike Purvis - Tony Baltovski - - Tony Baltovski - - BSD - - http://wiki.ros.org/Robots/Ridgeback - - catkin - - - - roscpp - controller_manager - diagnostic_updater - geometry_msgs - ridgeback_msgs - realtime_tools - rosserial_server - sensor_msgs - std_msgs - mirte_msgs - message_runtime - hardware_interface - diagnostic_aggregator - diff_drive_controller - imu_filter_madgwick - ridgeback_control - ridgeback_description - - rosserial_python - teleop_twist_joy - tf - topic_tools - socat - - roslaunch - roslint - - - - diff --git a/mirte_control_master/scripts/mock.py b/mirte_control_master/scripts/mock.py deleted file mode 100755 index 96d79862..00000000 --- a/mirte_control_master/scripts/mock.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -import rospy -from std_srvs.srv import Empty, EmptyResponse -from mirte_msgs.srv import SetMotorSpeed, SetMotorSpeedResponse, SetMotorSpeedRequest - - -def my_service_callback(request, i): - # Add your service logic here - print(request, i) - # rospy.loginfo("Service called", i, request.speed) - # Return an appropriate response - return SetMotorSpeedResponse() - - -if __name__ == "__main__": - # Initialize the ROS node - rospy.init_node("my_node") - # Create the service - my_service1 = rospy.Service( - "/mirte/set_left_front_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 1), - ) - my_service2 = rospy.Service( - "/mirte/set_left_rear_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 2), - ) - my_service3 = rospy.Service( - "/mirte/set_right_front_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 3), - ) - my_service4 = rospy.Service( - "/mirte/set_right_rear_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 4), - ) - - # Spin the node to process callbacks - rospy.spin() diff --git a/mirte_control_master/src/my_robot_base.cpp b/mirte_control_master/src/my_robot_base.cpp deleted file mode 100644 index 801bf82e..00000000 --- a/mirte_control_master/src/my_robot_base.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include -#include -#include - -void controlLoop(MyRobotHWInterface &hw, - controller_manager::ControllerManager &cm, - std::chrono::system_clock::time_point &last_time) { - std::chrono::system_clock::time_point current_time = - std::chrono::system_clock::now(); - std::chrono::duration elapsed_time = current_time - last_time; - ros::Duration elapsed(elapsed_time.count()); - last_time = current_time; - - hw.read(elapsed); - cm.update(ros::Time::now(), elapsed); - hw.write(); -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "my_robot_base_node"); - - MyRobotHWInterface hw; - controller_manager::ControllerManager cm(&hw, hw.nh); - - double control_frequency; - hw.private_nh.param("control_frequency", control_frequency, 10.0); - - ros::CallbackQueue my_robot_queue; - ros::AsyncSpinner my_robot_spinner(1, &my_robot_queue); - - std::chrono::system_clock::time_point last_time = - std::chrono::system_clock::now(); - ros::TimerOptions control_timer( - ros::Duration(1 / control_frequency), - std::bind(controlLoop, std::ref(hw), std::ref(cm), std::ref(last_time)), - &my_robot_queue); - ros::Timer control_loop = hw.nh.createTimer(control_timer); - my_robot_spinner.start(); - ros::spin(); - - return 0; -} diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 337d7e0d..df42d227 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -23,8 +23,9 @@ modules: right_rear: pin_A: 7 pin_B: 6 - - type: Hiwonder_Servo + servo_controller: + device: mirte + type: Hiwonder_Servo uart: 0 rx_pin: 0 tx_pin: 1 @@ -37,7 +38,9 @@ modules: id: 4 min_angle: 800 max_angle: 4000 - type: INA226 + power_watcher: + device: mirte + type: INA226 connector: I2C1 id: 0b1000101 min_voltage: 11 diff --git a/ros_control_boilerplate b/ros_control_boilerplate deleted file mode 160000 index 0f9cd01b..00000000 --- a/ros_control_boilerplate +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0f9cd01bc1430c718554a3801683ada9cd2aa0a6 From ab1820b5b3d7b068f0d75a7316c81576be55be23 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 13 Feb 2024 15:56:45 +0100 Subject: [PATCH 44/77] Fix styling, add comment --- .../scripts/ROS_telemetrix_aio_api.py | 51 +++++++++++++------ 1 file changed, 35 insertions(+), 16 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 045636a6..8dabff04 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -13,6 +13,7 @@ from telemetrix_aio import telemetrix_aio from typing import Literal, Tuple import subprocess + # Import the right Telemetrix AIO devices = rospy.get_param("/mirte/device") @@ -1215,10 +1216,18 @@ def __init__(self, board, module_name, module): self.min_voltage = module["min_voltage"] if "min_voltage" in module else -1 self.max_voltage = module["max_voltage"] if "max_voltage" in module else -1 self.max_current = module["max_current"] if "max_current" in module else -1 - self.turn_off_pin = board_mapping.pin_name_to_pin_number(module["turn_off_pin"]) if "turn_off_pin" in module else -1 - self.turn_off_pin_value = module["turn_off_pin_value"] if "turn_off_pin_value" in module else True # what should be the output when the relay should be opened. - self.turn_off_time = module["turn_off_time"] if "turn_off_time" in module else 30 # time to wait for computer to shut down - self.enable_turn_off = False # require at least a single message with a real value before arming the turn off system + self.turn_off_pin = ( + board_mapping.pin_name_to_pin_number(module["turn_off_pin"]) + if "turn_off_pin" in module + else -1 + ) + self.turn_off_pin_value = ( + module["turn_off_pin_value"] if "turn_off_pin_value" in module else True + ) # what should be the output when the relay should be opened. + self.turn_off_time = ( + module["turn_off_time"] if "turn_off_time" in module else 30 + ) # time to wait for computer to shut down + self.enable_turn_off = False # require at least a single message with a real value before arming the turn off system self.shutdown_triggered = False self.turn_off_trigger_start_time = -1 self.ina_publisher = rospy.Publisher( @@ -1250,8 +1259,12 @@ async def start(self): id = self.module["id"] await self.board.sensors.add_ina226(self.i2c_port, self.callback, id) - if(self.turn_off_pin != -1): - self.trigger_shutdown_relay = await self.board.modules.add_shutdown_relay(self.turn_off_pin, not not self.turn_off_pin_value, self.turn_off_time+10) + if self.turn_off_pin != -1: + self.trigger_shutdown_relay = await self.board.modules.add_shutdown_relay( + self.turn_off_pin, + not not self.turn_off_pin_value, + self.turn_off_time + 10, # add 10s to the shutdown time for the 10s shutdown command wait time + ) async def callback(self, data): # TODO: move this decoding to the library @@ -1285,36 +1298,42 @@ async def callback(self, data): await self.turn_off_cb() async def turn_off_cb(self): - if(self.turn_off_pin == -1): + if self.turn_off_pin == -1: # Dont turn off when this feature is disabled return # make sure that there are real values coming from the ina226 module - if(not self.enable_turn_off): - if(self.voltage > 6 and self.current > 0.3): + if not self.enable_turn_off: + if self.voltage > 6 and self.current > 0.3: self.enable_turn_off = True rospy.logwarn("Shutdown relay armed") return - + # at first dip of too low voltage, start timer, when longer than 5s below trigger voltage, then shut down # this makes sure that a short dip (motor start) does not trigger it if self.min_voltage != -1 and self.voltage < self.min_voltage: - if(self.turn_off_trigger_start_time == -1): + if self.turn_off_trigger_start_time == -1: self.turn_off_trigger_start_time = time.time() - rospy.logwarn("Low voltage, %ss till shutdown.", 5- (time.time()-self.turn_off_trigger_start_time)) + rospy.logwarn( + "Low voltage, %ss till shutdown.", + 5 - (time.time() - self.turn_off_trigger_start_time), + ) else: self.turn_off_trigger_start_time = -1 - if(self.turn_off_trigger_start_time != -1 and time.time()-self.turn_off_trigger_start_time > 5): + if ( + self.turn_off_trigger_start_time != -1 + and time.time() - self.turn_off_trigger_start_time > 5 + ): await self.shutdown_robot() - + async def shutdown_robot(self): - if(not self.shutdown_triggered): + if not self.shutdown_triggered: rospy.logerr("Triggering shutdown, shutting down in 10s") await self.trigger_shutdown_relay() self.shutdown_triggered = True # subprocess.run("sudo shutdown 10s") - + class Hiwonder_Servo: def __init__(self, servo_name, servo_obj, bus): From 739c0443e5b57398f56a58b961873dd8b5364a0e Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 13 Feb 2024 15:58:33 +0100 Subject: [PATCH 45/77] stylefix2 --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 8dabff04..c0cc699d 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1263,7 +1263,8 @@ async def start(self): self.trigger_shutdown_relay = await self.board.modules.add_shutdown_relay( self.turn_off_pin, not not self.turn_off_pin_value, - self.turn_off_time + 10, # add 10s to the shutdown time for the 10s shutdown command wait time + self.turn_off_time + + 10, # add 10s to the shutdown time for the 10s shutdown command wait time ) async def callback(self, data): From 56ec51b1aa28211e031a122b0e6a41b366f1cc2e Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 13 Feb 2024 19:59:14 +0000 Subject: [PATCH 46/77] Add neopixel --- .../config/mirte_master_config.yaml | 29 ++++++++----- .../scripts/ROS_telemetrix_aio_api.py | 43 +++++++++++++++++-- 2 files changed, 58 insertions(+), 14 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index df42d227..cf0dfd5a 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -38,17 +38,17 @@ modules: id: 4 min_angle: 800 max_angle: 4000 - power_watcher: - device: mirte - type: INA226 - connector: I2C1 - id: 0b1000101 - min_voltage: 11 - max_current: 1 - max_voltage: 15 - turn_off_pin: 27 - turn_off_value: True - turn_off_time: 10 + # power_watcher: + # device: mirte + # type: INA226 + # connector: I2C1 + # id: 0b1000101 + # min_voltage: 11 + # max_current: 1 + # max_voltage: 15 + # turn_off_pin: 27 + # turn_off_value: True + # turn_off_time: 10 encoder: left_front: name: left_front @@ -74,6 +74,13 @@ encoder: pins: A: 15 B: 14 +neopixel: + name: leds + pins: + pin: 27 + default_color: 0x123456 + max_intensity: 50 + pixels: 18 # servos: # servo1: diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index c0cc699d..1aa64db1 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -33,6 +33,9 @@ async def analog_write(board, pin, value): else: await board.analog_write(pin, value) +def get_obj_value(s, obj, key, def_value=None): + # shorthand of self.key = obj["key"] if "key" in obj else def_value + setattr(s, key, obj[key] if key in obj else def_value ) # Import ROS message types from std_msgs.msg import Header, Int32 @@ -94,8 +97,8 @@ async def analog_write(board, pin, value): def get_pin_numbers(component): - devices = rospy.get_param("/mirte/device") - device = devices[component["device"]] + # devices = rospy.get_param("/mirte/device") + # device = devices[component["device"]] pins = {} if "connector" in component: pins = board_mapping.connector_to_pins(component["connector"]) @@ -389,6 +392,36 @@ async def publish_data(self, data): await self.publish(encoder) +class Neopixel: + def __init__(self, board, neo_obj): + self.board = board + self.settings = neo_obj + self.pins = get_pin_numbers(neo_obj) + self.name = neo_obj["name"] + self.pixels = neo_obj["pixels"] # num of leds + get_obj_value(self, neo_obj, "max_intensity", 50) + get_obj_value(self, neo_obj, "default_color", 0x000000) + server = rospy.Service(f"/mirte/set_{self.name}_color_all", SetLEDValue, self.set_color_all_service) + + async def start(self): + colors = self.unpack_color_and_scale(self.default_color) + print(colors[0], colors[1], colors[2]) + await board.set_pin_mode_neopixel( pin_number=self.pins["pin"], num_pixels=self.pixels, fill_r=colors[0], fill_g=colors[1], fill_b=colors[2] ) + + async def set_color_all(self, color): + colors = self.unpack_color_and_scale(color) + await board.neopixel_fill( r=colors[0], g=colors[1], b=colors[2] ) + + async def set_color_single(self, pixel, color): + colors = self.unpack_color_and_scale(color) + await board.neopixel_fill( r=colors[0], g=colors[1], b=colors[2] ) + + def set_color_all_service(self, req): + asyncio.run(self.set_color_all(req.value)) + return SetLEDValueResponse(True) + + def unpack_color_and_scale(self, color_num): # hex color number (0x123456) or string ("0x123456") + return [int(x*0.5) for x in struct.unpack('BBB', bytes.fromhex(hex(int(str(color_num), 0))[2:].zfill(6)))] class Servo: def __init__(self, board, servo_obj): @@ -940,6 +973,10 @@ def actuators(loop, board, device): servo = Servo(board, servos[servo]) servers.append(loop.create_task(servo.start())) + if rospy.has_param("/mirte/neopixel"): + neopixel = rospy.get_param("/mirte/neopixel") + servers.append(loop.create_task(Neopixel(board, neopixel).start())) + if rospy.has_param("/mirte/modules"): servers += add_modules(rospy.get_param("/mirte/modules"), device) # Set a raw pin value @@ -1036,7 +1073,7 @@ def sensors(loop, board, device): tasks.append(loop.create_task(monitor.start())) # encoder sensors do not need a max_frequency. They are interrupts on # on the mcu side. - + # Get a raw pin value # TODO: this still needs to be tested. We are waiting on an implementation of ananlog_read() # on the telemetrix side From 7a6a27e9295dddd11334d6e5df95c500a4ea73ff Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 13 Feb 2024 20:30:14 +0000 Subject: [PATCH 47/77] Add set single led neopixel & service --- mirte_msgs/CMakeLists.txt | 1 + mirte_msgs/srv/SetSingleLEDValue.srv | 5 +++++ mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 12 ++++++++++-- 3 files changed, 16 insertions(+), 2 deletions(-) create mode 100644 mirte_msgs/srv/SetSingleLEDValue.srv diff --git a/mirte_msgs/CMakeLists.txt b/mirte_msgs/CMakeLists.txt index d468f4d0..d0cecd55 100644 --- a/mirte_msgs/CMakeLists.txt +++ b/mirte_msgs/CMakeLists.txt @@ -70,6 +70,7 @@ add_service_files( GetPinValue.srv SetPinValue.srv SetLEDValue.srv + SetSingleLEDValue.srv SetOLEDImage.srv SetServoAngle.srv get_virtual_color.srv diff --git a/mirte_msgs/srv/SetSingleLEDValue.srv b/mirte_msgs/srv/SetSingleLEDValue.srv new file mode 100644 index 00000000..60d68a47 --- /dev/null +++ b/mirte_msgs/srv/SetSingleLEDValue.srv @@ -0,0 +1,5 @@ +int32 value +int32 pixel +--- +bool status + diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 1aa64db1..8aa250ae 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -402,10 +402,11 @@ def __init__(self, board, neo_obj): get_obj_value(self, neo_obj, "max_intensity", 50) get_obj_value(self, neo_obj, "default_color", 0x000000) server = rospy.Service(f"/mirte/set_{self.name}_color_all", SetLEDValue, self.set_color_all_service) + server = rospy.Service(f"/mirte/set_{self.name}_color_single", SetSingleLEDValue, self.set_color_single_service) + async def start(self): colors = self.unpack_color_and_scale(self.default_color) - print(colors[0], colors[1], colors[2]) await board.set_pin_mode_neopixel( pin_number=self.pins["pin"], num_pixels=self.pixels, fill_r=colors[0], fill_g=colors[1], fill_b=colors[2] ) async def set_color_all(self, color): @@ -414,12 +415,19 @@ async def set_color_all(self, color): async def set_color_single(self, pixel, color): colors = self.unpack_color_and_scale(color) - await board.neopixel_fill( r=colors[0], g=colors[1], b=colors[2] ) + if(pixel >=self.pixels): + return False + await board.neo_pixel_set_value(pixel, r=colors[0], g=colors[1], b=colors[2], auto_show=True ) + return True def set_color_all_service(self, req): asyncio.run(self.set_color_all(req.value)) return SetLEDValueResponse(True) + def set_color_single_service(self, req): + ok = asyncio.run(self.set_color_single(req.pixel, req.value)) + return SetSingleLEDValueResponse(ok) + def unpack_color_and_scale(self, color_num): # hex color number (0x123456) or string ("0x123456") return [int(x*0.5) for x in struct.unpack('BBB', bytes.fromhex(hex(int(str(color_num), 0))[2:].zfill(6)))] From ef6ad146ae5594b6c91fdcce99cb85d841a7815d Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 14 Feb 2024 08:17:02 +0000 Subject: [PATCH 48/77] fixes martin arm --- mirte_bringup/launch/minimal_master.launch | 9 + mirte_control/include/my_robot_hw_interface.h | 3 +- mirte_control_master/CHANGELOG.rst | 99 ------ mirte_control_master/CMakeLists.txt | 205 ------------ mirte_control_master/config/control.yaml | 68 ---- .../config/my_robot_control.yaml | 24 -- .../include/my_robot_hw_interface.h | 306 ------------------ mirte_control_master/launch/control.launch | 13 - mirte_control_master/package.xml | 49 --- mirte_control_master/scripts/mock.py | 42 --- mirte_control_master/src/my_robot_base.cpp | 43 --- .../config/mirte_master_config.yaml | 107 +++--- mirte_teleop/launch/teleop_holo_joy.launch | 2 + 13 files changed, 67 insertions(+), 903 deletions(-) delete mode 100644 mirte_control_master/CHANGELOG.rst delete mode 100644 mirte_control_master/CMakeLists.txt delete mode 100644 mirte_control_master/config/control.yaml delete mode 100644 mirte_control_master/config/my_robot_control.yaml delete mode 100644 mirte_control_master/include/my_robot_hw_interface.h delete mode 100644 mirte_control_master/launch/control.launch delete mode 100644 mirte_control_master/package.xml delete mode 100755 mirte_control_master/scripts/mock.py delete mode 100644 mirte_control_master/src/my_robot_base.cpp diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index 74bbef73..fb130ad2 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -32,5 +32,14 @@ output="screen" args="mobile_base_controller"/> + + + + + + + + + diff --git a/mirte_control/include/my_robot_hw_interface.h b/mirte_control/include/my_robot_hw_interface.h index 5d80635f..71cd49bd 100644 --- a/mirte_control/include/my_robot_hw_interface.h +++ b/mirte_control/include/my_robot_hw_interface.h @@ -37,7 +37,7 @@ // const unsigned int NUM_JOINTS = 4; const auto service_format = "/mirte/set_%s_speed"; - +const auto max_speed = 80; // Quick fix hopefully for power dip. /// \brief Hardware interface for a robot class MyRobotHWInterface : public hardware_interface::RobotHW { public: @@ -47,6 +47,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { int speed_mapped = std::max(std::min(int(speed / (6 * M_PI) * 100), 100), -100); + speed_mapped = std::clamp(speed_mapped, -max_speed, max_speed); if (speed_mapped != _last_cmd[joint]) { service_requests[joint].request.speed = speed_mapped; _last_cmd[joint] = speed_mapped; diff --git a/mirte_control_master/CHANGELOG.rst b/mirte_control_master/CHANGELOG.rst deleted file mode 100644 index e8a38a34..00000000 --- a/mirte_control_master/CHANGELOG.rst +++ /dev/null @@ -1,99 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package ridgeback_base -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.2 (2022-05-17) ------------------- - -0.3.1 (2021-06-15) ------------------- - -0.3.0 (2020-11-12) ------------------- -* [ridgeback_base] Added dependency. -* [ridgeback_base] Updated to use new functions from puma_motor_drivers. -* [ridgeback_base] Used ros::ok() for while condition on thread loops. -* Updates for C++11. -* Contributors: Tony Baltovski - -0.2.6 (2020-11-12) ------------------- -* Bump CMake version to avoid CMP0048 warning. -* [ridgeback_base] Fixed missing forward slash. -* Contributors: Tony Baltovski - -0.2.5 (2020-10-19) ------------------- -* Use eval + find to properly load the default mag config file -* Added RIDGEBACK_MAG_CONFIG to madgwick filter and set a default optenv -* Removed env-hooks -* Contributors: Chris Iverach-Brereton, Dave Niewinski - -0.2.4 (2019-11-22) ------------------- -* Merge pull request `#26 `_ from ridgeback/RPSW-119 - Added the ability to disable using the MCU -* [ridgeback_base] Added param type for use_mcu. -* [ridgeback_base] Added lighting and cooling under use_mcu flag. -* [ridgeback_base] Added envar for using MCU. -* Contributors: Tony Baltovski - -0.2.3 (2019-03-23) ------------------- -* [ridgeback_base] Updated compute_calibration to use a MagneticField message. -* Contributors: Tony Baltovski - -0.2.1 (2018-08-02) ------------------- - -0.2.0 (2018-05-23) ------------------- -* [ridgeback_base] Added absolute value checking for fans. -* Updated to package format 2. -* [ridgeback_base] Switched to rosserial_server_udp. -* Updated bringup for kinetic -* [ridgeback_base] Fixed a typo. -* Updated maintainer. -* Fixed temperature warning for PCB and MCU temperature. -* Contributors: Dave Niewinski, Tony Baltovski - -0.1.7 (2016-10-03) ------------------- -* Removed GPS dependencies. -* Contributors: Johannes Meyer, Tony Baltovski - -0.1.6 (2016-04-22) ------------------- -* Combined hostname and version for hardware ID. -* Fixed initialization of constant doubles. -* Added hardware ID based hostname. -* Contributors: Tony Baltovski - -0.1.5 (2016-03-02) ------------------- -* Fixed state order for lighting. -* Separated passive joint into header. -* Contributors: Tony Baltovski - -0.1.4 (2015-12-01) ------------------- - -0.1.3 (2015-11-20) ------------------- -* Install the ridgeback_node target. -* Contributors: Mike Purvis - -0.1.2 (2015-11-20) ------------------- -* Fixed dependency. -* Contributors: Tony Baltovski - -0.1.1 (2015-11-20) ------------------- -* Added lighting. -* Contributors: Tony Baltovski - -0.1.0 (2015-11-19) ------------------- -* Initial Ridgeback release. -* Contributors: Mike Purvis, Tony Baltovski diff --git a/mirte_control_master/CMakeLists.txt b/mirte_control_master/CMakeLists.txt deleted file mode 100644 index 939a32f1..00000000 --- a/mirte_control_master/CMakeLists.txt +++ /dev/null @@ -1,205 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(mirte_control_master) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++17) # -Wall -Wextra -Wpedantic -Werror) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - mirte_msgs - controller_manager - hardware_interface - -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -#add_message_files( -# FILES -# MotorCommand.msg -#) - -## Generate services in the 'srv' folder -#add_service_files( - # FILES - # get_distance.srv -#) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -# std_msgs -#) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES mirte_ros_package - CATKIN_DEPENDS roscpp std_msgs mirte_msgs message_runtime controller_manager hardware_interface -# DEPENDS system_lib -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include/ - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/mobile_controller.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node - src/my_robot_base.cpp -) - -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node - ${catkin_LIBRARIES} - ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variablese -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mirte_ros_package.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/mirte_control_master/config/control.yaml b/mirte_control_master/config/control.yaml deleted file mode 100644 index 7e45e98c..00000000 --- a/mirte_control_master/config/control.yaml +++ /dev/null @@ -1,68 +0,0 @@ -my_robot: - type: "joint_state_controller/JointStateController" - publish_rate: 50 - -mobile_base_controller: - type: "mecanum_drive_controller/MecanumDriveController" - front_left_wheel_joint: "wheel_left_front_joint" - back_left_wheel_joint: "wheel_left_rear_joint" - front_right_wheel_joint: "wheel_right_front_joint" - back_right_wheel_joint: "wheel_right_rear_joint" - publish_rate: 50 - pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03] - cmd_vel_timeout: 0.25 - - # Override URDF look-up for wheel separation since the parent link is not the base_link. - wheel_separation_x: 0.638 - wheel_separation_y: 0.551 - wheel_radius: 0.165 - # Odometry fused with IMU is published by robot_localization, so - # no need to publish a TF based on encoders alone. - enable_odom_tf: true - - # Wheel separation and radius multipliers - wheel_separation_multiplier: 1.0 # default: 1.0 - wheel_radius_multiplier : 1.0 # default: 1.0 - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - max_velocity : 1.1 # m/s - has_acceleration_limits: true - max_acceleration : 2.5 # m/s^2 - y: - has_velocity_limits : true - max_velocity : 1.1 # m/s - has_acceleration_limits: true - max_acceleration : 2.5 # m/s^2 - angular: - z: - has_velocity_limits : true - max_velocity : 2.0 # rad/s - has_acceleration_limits: true - max_acceleration : 1.0 # rad/s^2 - -# ekf_localization: -# frequency: 50 -# two_d_mode: true -# odom0: /ridgeback_velocity_controller/odom -# odom0_config: [false, false, false, -# false, false, false, -# true, true, false, -# false, false, true, -# false, false, false] -# odom0_differential: false -# imu0: /imu/data -# imu0_config: [false, false, false, -# false, false, false, -# false, false, false, -# false, false, true, -# true, true, false] -# imu0_differential: false -# odom_frame: odom -# base_link_frame: base_link -# world_frame: odom -# predict_to_current_time: true diff --git a/mirte_control_master/config/my_robot_control.yaml b/mirte_control_master/config/my_robot_control.yaml deleted file mode 100644 index cb792f92..00000000 --- a/mirte_control_master/config/my_robot_control.yaml +++ /dev/null @@ -1,24 +0,0 @@ -# .yaml config file -# -# The PID gains and controller settings must be saved in a yaml file that gets loaded -# to the param server via the roslaunch file (my_robot_control.launch). - -my_robot: - # Publish all joint states ----------------------------------- - # Creates the /joint_states topic necessary in ROS - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - - # Effort Controllers --------------------------------------- - leftWheel_effort_controller: - type: effort_controllers/JointEffortController - joint: left_wheel_hinge - pid: {p: 100.0, i: 0.1, d: 10.0} - #pid: {p: 50.0, i: 0.1, d: 0.0} - rightWheel_effort_controller: - type: effort_controllers/JointEffortController - joint: right_wheel_hinge - pid: {p: 100.0, i: 0.1, d: 10.0} - #pid: {p: 1.0, i: 1.0, d: 0.0} -# TODO: extend this for the mirte-master \ No newline at end of file diff --git a/mirte_control_master/include/my_robot_hw_interface.h b/mirte_control_master/include/my_robot_hw_interface.h deleted file mode 100644 index 5d80635f..00000000 --- a/mirte_control_master/include/my_robot_hw_interface.h +++ /dev/null @@ -1,306 +0,0 @@ -// https://slaterobots.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot -// Roughly based on: -// https://github.com/eborghi10/my_ROS_mobile_robot/blob/master/my_robot_base/include/my_robot_hw_interface.h -// https://github.com/PickNikRobotics/ros_control_boilerplate -// https://github.com/DeborggraeveR/ampru - -// https://github.com/resibots/dynamixel_control_hw/blob/master/include/dynamixel_control_hw/hardware_interface.hpp -// https://github.com/FRC900/2018RobotCode/blob/master/zebROS_ws/src/ros_control_boilerplate/include/ros_control_boilerplate/frcrobot_hw_interface.h -// INTERESTING CLEAN ONE: -// https://github.com/ros-controls/ros_controllers/blob/indigo-devel/diff_drive_controller/test/diffbot.h - -#pragma once -#define _USE_MATH_DEFINES - -// ROS -#include -#include -#include -#include - -// ros_control -#include -#include -#include -#include - -// ostringstream -#include -#include -#include - -#include -#include -#include -#include -#include - -// const unsigned int NUM_JOINTS = 4; -const auto service_format = "/mirte/set_%s_speed"; - -/// \brief Hardware interface for a robot -class MyRobotHWInterface : public hardware_interface::RobotHW { -public: - MyRobotHWInterface(); - - bool write_single(int joint, int speed) { - - int speed_mapped = - std::max(std::min(int(speed / (6 * M_PI) * 100), 100), -100); - if (speed_mapped != _last_cmd[joint]) { - service_requests[joint].request.speed = speed_mapped; - _last_cmd[joint] = speed_mapped; - if (!service_clients[joint].call(service_requests[joint])) { - this->start_reconnect(); - return false; - } - } - return true; - } - /* - * - */ - void write() { - if (running_) { - // make sure the clients don't get overwritten while calling them - const std::lock_guard lock(this->service_clients_mutex); - - // cmd[0] = ros_control calculated speed of left motor in rad/s - // cmd[1] = ros_control calculated speed of right motor in rad/s - - // This function converts cmd[0] to pwm and calls that service - - // NOTE: this *highly* depends on the voltage of the motors!!!! - // For 5V power bank: 255 pwm = 90 ticks/sec -> ca 2 rot/s (4*pi) - // For 6V power supply: 255 pwm = 120 ticks/sec -> ca 3 rot/s - // (6*pi) - for (size_t i = 0; i < NUM_JOINTS; i++) { - if (!write_single(i, cmd[i])) { - return; - } - } - // Set the direction in so the read() can use it - // TODO: this does not work properly, because at the end of a series - // cmd_vel is negative, while the rotation is not - for (size_t i = 0; i < NUM_JOINTS; i++) { - _last_wheel_cmd_direction[i] = cmd[i] > 0.0 ? 1 : -1; - } - } - } - - double meter_per_enc_tick() { - return (_wheel_diameter / 2) * 2 * M_PI / - 40.0; // TODO: get ticks from parameter server - } - - /** - * Reading encoder values and setting position and velocity of encoders - */ - void read_single(int joint, const ros::Duration &period) { - auto diff = _wheel_encoder[joint] - _last_value[joint]; - _last_value[joint] = _wheel_encoder[joint]; - double meterPerEncoderTick = meter_per_enc_tick(); - double distance; - if (bidirectional) { // if encoder is counting bidirectional, then it - // decreases by itself, dont want to use - // last_wheel_cmd_direction - distance = diff * meterPerEncoderTick * 1.0; - } else { - distance = - diff * meterPerEncoderTick * _last_wheel_cmd_direction[joint] * 1.0; - } - pos[joint] += distance; - vel[joint] = distance / period.toSec(); // WHY: was this turned off? - } - - /** - * Reading encoder values and setting position and velocity of encoders - */ - void read(const ros::Duration &period) { - - for (size_t i = 0; i < NUM_JOINTS; i++) { - this->read_single(i, period); - } - } - - ros::NodeHandle nh; - ros::NodeHandle private_nh; - -private: - hardware_interface::JointStateInterface jnt_state_interface; - hardware_interface::VelocityJointInterface jnt_vel_interface; - std::vector cmd; - std::vector pos; - std::vector vel; - std::vector eff; - - bool running_ = true; - double _wheel_diameter; - double _max_speed; - std::vector _wheel_encoder; - std::vector _last_cmd; - std::vector _last_value; - std::vector _last_wheel_cmd_direction; - - ros::Time curr_update_time, prev_update_time; - - std::vector wheel_encoder_subs_; - ros::ServiceServer start_srv_; - ros::ServiceServer stop_srv_; - - std::vector service_clients; - std::vector service_requests; - std::vector joints; - bool start_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { - running_ = true; - return true; - } - - bool stop_callback(std_srvs::Empty::Request & /*req*/, - std_srvs::Empty::Response & /*res*/) { - running_ = false; - return true; - } - - void WheelEncoderCallback(const mirte_msgs::Encoder::ConstPtr &msg, - int joint) { - if (msg->value < 0) { - bidirectional = true; - } - _wheel_encoder[joint] = _wheel_encoder[joint] + msg->value; - } - - // Thread and function to restart service clients when the service server has - // restarted - std::future reconnect_thread; - void init_service_clients(); - void start_reconnect(); - std::mutex service_clients_mutex; - - bool bidirectional = false; // assume it is one direction, when receiving any - // negative value, it will be set to true - unsigned int NUM_JOINTS = 2; -}; // class - -void MyRobotHWInterface::init_service_clients() { - for (auto joint : this->joints) { - auto service = (boost::format(service_format) % joint).str(); - ROS_INFO_STREAM("Waiting for service " << service); - ros::service::waitForService(service, -1); - } - { - const std::lock_guard lock(this->service_clients_mutex); - for (size_t i = 0; i < NUM_JOINTS; i++) { - service_clients.push_back(nh.serviceClient( - (boost::format(service_format) % this->joints[i]).str(), true)); - service_requests.push_back(mirte_msgs::SetMotorSpeed()); - } - } -} - -unsigned int detect_joints(ros::NodeHandle &nh) { - std::string type; - nh.param("/mobile_base_controller/type", type, ""); - if (type.rfind("mecanum", 0) == 0) { // starts with mecanum - return 4; - } else if (type.rfind("diff", 0) == 0) { // starts with diff - return 2; - } else { - ROS_ERROR_STREAM("Unknown type: " << type); - return 0; - } -} - -MyRobotHWInterface::MyRobotHWInterface() - : private_nh("~"), running_(true), - start_srv_(nh.advertiseService( - "start", &MyRobotHWInterface::start_callback, this)), - stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, - this)) { - private_nh.param("wheel_diameter", _wheel_diameter, 0.06); - private_nh.param("max_speed", _max_speed, 2.0); - // private_nh.param("bidirectional", bidirectional, true); - this->NUM_JOINTS = detect_joints(private_nh); - // Initialize raw data - for (size_t i = 0; i < NUM_JOINTS; i++) { - _wheel_encoder.push_back(0); - _last_value.push_back(0); - _last_wheel_cmd_direction.push_back(0); - _last_cmd.push_back(0); - pos.push_back(0); - vel.push_back(0); - eff.push_back(0); - cmd.push_back(0); - } - assert(_wheel_encoder.size() == NUM_JOINTS); - assert(_last_value.size() == NUM_JOINTS); - assert(_last_wheel_cmd_direction.size() == NUM_JOINTS); - assert(_last_cmd.size() == NUM_JOINTS); - assert(pos.size() == NUM_JOINTS); - assert(vel.size() == NUM_JOINTS); - assert(eff.size() == NUM_JOINTS); - assert(cmd.size() == NUM_JOINTS); - - this->joints = {"left", // Edit the control.yaml when using this for the - // normal mirte as well - "right"}; - if (NUM_JOINTS == 4) { - this->joints = {"left_front", - "left_rear", // TODO: check ordering - "right_front", "right_rear"}; - } - std::cout << "Initializing MyRobotHWInterface with " << NUM_JOINTS - << " joints" << std::endl; - - // connect and register the joint state and velocity interfaces - for (unsigned int i = 0; i < NUM_JOINTS; ++i) { - std::string joint = - (boost::format("wheel_%s_joint") % this->joints[i]).str(); - hardware_interface::JointStateHandle state_handle(joint, &pos[i], &vel[i], - &eff[i]); - jnt_state_interface.registerHandle(state_handle); - - hardware_interface::JointHandle vel_handle( - jnt_state_interface.getHandle(joint), &cmd[i]); - jnt_vel_interface.registerHandle(vel_handle); - } - registerInterface(&jnt_state_interface); - registerInterface(&jnt_vel_interface); - - // Initialize publishers and subscribers - for (size_t i = 0; i < NUM_JOINTS; i++) { - auto encoder_topic = - (boost::format(service_format) % this->joints[i]).str(); - wheel_encoder_subs_.push_back(nh.subscribe( - encoder_topic, 1, - boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i))); - } - assert(joints.size() == NUM_JOINTS); - this->init_service_clients(); - assert(service_requests.size() == NUM_JOINTS); - - assert(service_clients.size() == NUM_JOINTS); -} - -void MyRobotHWInterface ::start_reconnect() { - using namespace std::chrono_literals; - - if (this->reconnect_thread.valid()) { // does it already exist or not? - - // Use wait_for() with zero milliseconds to check thread status. - auto status = this->reconnect_thread.wait_for(0ms); - - if (status != - std::future_status::ready) { // Still running -> already reconnecting - return; - } - } - - /* Run the reconnection on a different thread to not pause the ros-control - loop. The launch policy std::launch::async makes sure that the task is run - asynchronously on a new thread. */ - - this->reconnect_thread = - std::async(std::launch::async, [this] { this->init_service_clients(); }); -} \ No newline at end of file diff --git a/mirte_control_master/launch/control.launch b/mirte_control_master/launch/control.launch deleted file mode 100644 index a9395974..00000000 --- a/mirte_control_master/launch/control.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/mirte_control_master/package.xml b/mirte_control_master/package.xml deleted file mode 100644 index 18c73bf2..00000000 --- a/mirte_control_master/package.xml +++ /dev/null @@ -1,49 +0,0 @@ - - - mirte_control_master - 0.3.2 - Ridgeback's mobility and sensor base. - - Mike Purvis - Tony Baltovski - - Tony Baltovski - - BSD - - http://wiki.ros.org/Robots/Ridgeback - - catkin - - - - roscpp - controller_manager - diagnostic_updater - geometry_msgs - ridgeback_msgs - realtime_tools - rosserial_server - sensor_msgs - std_msgs - mirte_msgs - message_runtime - hardware_interface - diagnostic_aggregator - diff_drive_controller - imu_filter_madgwick - ridgeback_control - ridgeback_description - - rosserial_python - teleop_twist_joy - tf - topic_tools - socat - - roslaunch - roslint - - - - diff --git a/mirte_control_master/scripts/mock.py b/mirte_control_master/scripts/mock.py deleted file mode 100755 index 96d79862..00000000 --- a/mirte_control_master/scripts/mock.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -import rospy -from std_srvs.srv import Empty, EmptyResponse -from mirte_msgs.srv import SetMotorSpeed, SetMotorSpeedResponse, SetMotorSpeedRequest - - -def my_service_callback(request, i): - # Add your service logic here - print(request, i) - # rospy.loginfo("Service called", i, request.speed) - # Return an appropriate response - return SetMotorSpeedResponse() - - -if __name__ == "__main__": - # Initialize the ROS node - rospy.init_node("my_node") - # Create the service - my_service1 = rospy.Service( - "/mirte/set_left_front_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 1), - ) - my_service2 = rospy.Service( - "/mirte/set_left_rear_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 2), - ) - my_service3 = rospy.Service( - "/mirte/set_right_front_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 3), - ) - my_service4 = rospy.Service( - "/mirte/set_right_rear_speed", - SetMotorSpeed, - lambda request: my_service_callback(request, 4), - ) - - # Spin the node to process callbacks - rospy.spin() diff --git a/mirte_control_master/src/my_robot_base.cpp b/mirte_control_master/src/my_robot_base.cpp deleted file mode 100644 index 801bf82e..00000000 --- a/mirte_control_master/src/my_robot_base.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include -#include -#include - -void controlLoop(MyRobotHWInterface &hw, - controller_manager::ControllerManager &cm, - std::chrono::system_clock::time_point &last_time) { - std::chrono::system_clock::time_point current_time = - std::chrono::system_clock::now(); - std::chrono::duration elapsed_time = current_time - last_time; - ros::Duration elapsed(elapsed_time.count()); - last_time = current_time; - - hw.read(elapsed); - cm.update(ros::Time::now(), elapsed); - hw.write(); -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "my_robot_base_node"); - - MyRobotHWInterface hw; - controller_manager::ControllerManager cm(&hw, hw.nh); - - double control_frequency; - hw.private_nh.param("control_frequency", control_frequency, 10.0); - - ros::CallbackQueue my_robot_queue; - ros::AsyncSpinner my_robot_spinner(1, &my_robot_queue); - - std::chrono::system_clock::time_point last_time = - std::chrono::system_clock::now(); - ros::TimerOptions control_timer( - ros::Duration(1 / control_frequency), - std::bind(controlLoop, std::ref(hw), std::ref(cm), std::ref(last_time)), - &my_robot_queue); - ros::Timer control_loop = hw.nh.createTimer(control_timer); - my_robot_spinner.start(); - ros::spin(); - - return 0; -} diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 5b79b640..2550af94 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -12,19 +12,64 @@ modules: frequency: 1500 motors: left_front: - pin_A: 0 - pin_B: 1 + pin_A: 1 + pin_B: 0 left_rear: pin_A: 3 pin_B: 2 - right_front: - pin_A: 5 - pin_B: 4 right_rear: - pin_A: 7 - pin_B: 6 - - type: Hiwonder_Servo + pin_A: 4 + pin_B: 5 + right_front: + pin_A: 6 + pin_B: 7 +# servo_controller: +# device: mirte +# type: Hiwonder_Servo +# uart: 0 +# rx_pin: 0 +# tx_pin: 1 +# servos: +# servoH1: +# id: 3 +# min_angle: 9000 +# max_angle: 14000 +# servoH2: +# id: 4 +# min_angle: 800 +# max_angle: 4000 +# encoder: +# left_front: +# name: left_front +# device: mirte +# pins: +# A: 21 +# B: 20 +# left_rear: +# name: left_rear +# device: mirte +# pins: +# A: 17 +# B: 16 +# right_front: +# name: right_front +# device: mirte +# pins: +# A: 18 +# B: 19 +# right_rear: +# name: right_rear +# device: mirte +# pins: +# A: 15 +# B: 14 + + # servos: + # servo1: + # pin: 1 + servo_controller: + device: mirte + type: Hiwonder_Servo uart: 0 rx_pin: 0 tx_pin: 1 @@ -37,50 +82,6 @@ modules: id: 4 min_angle: 800 max_angle: 4000 -encoder: - left_front: - name: left_front - device: mirte - pins: - A: 21 - B: 20 - left_rear: - name: left_rear - device: mirte - pins: - A: 17 - B: 16 - right_front: - name: right_front - device: mirte - pins: - A: 18 - B: 19 - right_rear: - name: right_rear - device: mirte - pins: - A: 15 - B: 14 - - # servos: - # servo1: - # pin: 1 - # servo_controller: - # device: mirte - # type: Hiwonder_Servo - # uart: 0 - # rx_pin: 0 - # tx_pin: 1 - # servos: - # servoH1: - # id: 3 - # min_angle: 9000 - # max_angle: 14000 - # servoH2: - # id: 4 - # min_angle: 800 - # max_angle: 4000 # power_watcher: # device: mirte # type: INA226 diff --git a/mirte_teleop/launch/teleop_holo_joy.launch b/mirte_teleop/launch/teleop_holo_joy.launch index c16e23ff..d6fe098e 100644 --- a/mirte_teleop/launch/teleop_holo_joy.launch +++ b/mirte_teleop/launch/teleop_holo_joy.launch @@ -3,6 +3,8 @@ pkg="joy" type="joy_node" output="screen"> + + From 7f24e87513183df96f16e47b1712c2818da797d7 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 14 Feb 2024 09:59:38 +0000 Subject: [PATCH 49/77] Add hiwonder servo angle mapping --- .../config/mirte_master_config_goede_arm.yaml | 118 ++++++++++++++++++ .../scripts/ROS_telemetrix_aio_api.py | 8 +- 2 files changed, 123 insertions(+), 3 deletions(-) create mode 100644 mirte_telemetrix/config/mirte_master_config_goede_arm.yaml diff --git a/mirte_telemetrix/config/mirte_master_config_goede_arm.yaml b/mirte_telemetrix/config/mirte_master_config_goede_arm.yaml new file mode 100644 index 00000000..12ae23a0 --- /dev/null +++ b/mirte_telemetrix/config/mirte_master_config_goede_arm.yaml @@ -0,0 +1,118 @@ +device: + mirte: + type: pcb + version: 0.6 + max_frequency: 50 +modules: + motorservocontroller: + device: mirte + type: PCA9685 + id: 0x40 + connector: I2C1 + frequency: 1500 + motors: + left_front: + pin_A: 1 + pin_B: 0 + left_rear: + pin_A: 3 + pin_B: 2 + right_rear: + pin_A: 4 + pin_B: 5 + right_front: + pin_A: 6 + pin_B: 7 + servo_controller: + device: mirte + type: Hiwonder_Servo + uart: 0 + rx_pin: 0 + tx_pin: 1 + servos: + servoRot: + id: 2 + min_angle_out: 2952 + max_angle_out: 20832 + servoShoulder: + id: 3 + min_angle_out: 10944 + max_angle_out: 20328 + servoElbow: + id: 4 + min_angle_out: 2976 + max_angle_out: 13896 + servoWrist: + id: 5 + min_angle_out: 10032 + max_angle_out: 14208 + # power_watcher: + # device: mirte + # type: INA226 + # connector: I2C1 + # id: 0b1000101 + # min_voltage: 11 + # max_current: 1 + # max_voltage: 15 + # turn_off_pin: 27 + # turn_off_value: True + # turn_off_time: 10 +encoder: + left_front: + name: left_front + device: mirte + pins: + A: 21 + B: 20 + left_rear: + name: left_rear + device: mirte + pins: + A: 17 + B: 16 + right_front: + name: right_front + device: mirte + pins: + A: 18 + B: 19 + right_rear: + name: right_rear + device: mirte + pins: + A: 15 + B: 14 +# neopixel: +# name: leds +# pins: +# pin: 27 +# default_color: 0x123456 +# max_intensity: 50 +# pixels: 18 + + # servos: + # servo1: + # pin: 1 + # servo_controller: + # device: mirte + # type: Hiwonder_Servo + # uart: 0 + # rx_pin: 0 + # tx_pin: 1 + # servos: + # servoH1: + # id: 3 + # min_angle: 9000 + # max_angle: 14000 + # servoH2: + # id: 4 + # min_angle: 800 + # max_angle: 4000 + # power_watcher: + # device: mirte + # type: INA226 + # connector: I2C1 + # id: 0x41 + # min_voltage: 11 + # max_current: 1 + # max_voltage: 15 diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 8aa250ae..be5c0733 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1387,8 +1387,9 @@ def __init__(self, servo_name, servo_obj, bus): self.name = servo_name self.bus = bus self.min_angle_in = 0 - self.max_angle_in = 180 # degrees or whatever you want to use + self.max_angle_in = 18000 # centidegrees or whatever you want to use + # angles for the servo, differs probably per servo self.min_angle_out = 0 self.max_angle_out = 24000 # centidegrees for name in ["min_angle_in", "min_angle_out", "max_angle_in", "max_angle_out"]: @@ -1428,8 +1429,9 @@ def set_servo_angle_service(self, req): return SetServoAngleResponse(True) def callback(self, data): - # TODO: map back to 'angle' - self.publisher.publish(data["angle"]) + angle = int(scale(data["angle"], + [self.min_angle_out, self.max_angle_out],[self.min_angle_in, self.max_angle_in])) + self.publisher.publish(angle) class Hiwonder_Bus: From 1cf0665e6e4fa6d2582659bd63c066d1575202fc Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 14 Feb 2024 11:34:34 +0000 Subject: [PATCH 50/77] Add shutdown service --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index be5c0733..03234787 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1278,6 +1278,9 @@ def __init__(self, board, module_name, module): self.ina_publisher = rospy.Publisher( "/mirte/power/" + module_name, BatteryState, queue_size=1 ) + server = rospy.Service("/mirte/shutdown", SetBool, self.shutdown_service) + + self.last_low_voltage = -1 async def start(self): print("start ina!") @@ -1319,8 +1322,9 @@ async def callback(self, data): vals = list(struct.unpack("<2f", bytes_obj)) self.voltage = vals[0] self.current = vals[1] - if self.min_voltage != -1 and self.voltage < self.min_voltage: + if self.min_voltage != -1 and self.voltage < self.min_voltage and self.last_low_voltage != self.voltage: rospy.logwarn("Low voltage: %f", self.voltage) + self.last_low_voltage = self.voltage if self.max_voltage != -1 and self.voltage > self.max_voltage: rospy.logwarn("High voltage: %f", self.voltage) if self.max_current != -1 and self.current > self.max_current: @@ -1376,10 +1380,15 @@ async def turn_off_cb(self): async def shutdown_robot(self): if not self.shutdown_triggered: rospy.logerr("Triggering shutdown, shutting down in 10s") + # This will make the pico unresponsive after the delay, so ping errors are expected. Need to restart telemetrix to continue. await self.trigger_shutdown_relay() self.shutdown_triggered = True # subprocess.run("sudo shutdown 10s") + def shutdown_service(self, req): + # also called from systemd shutdown service + asyncio.run(self.shutdown_robot()) + return SetBoolResponse(True, "") class Hiwonder_Servo: def __init__(self, servo_name, servo_obj, bus): From 6f5318941135e5ec8f0695f28b21aefadf331ff1 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 14 Feb 2024 12:41:33 +0000 Subject: [PATCH 51/77] Calculate used power --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 03234787..fcebec00 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1278,6 +1278,9 @@ def __init__(self, board, module_name, module): self.ina_publisher = rospy.Publisher( "/mirte/power/" + module_name, BatteryState, queue_size=1 ) + self.ina_publisher_used = rospy.Publisher(f"/mirte/power/{module_name}/used", Int32, queue_size=1) + self.used_energy = 0 # mah + self.last_used_calc = time.time() server = rospy.Service("/mirte/shutdown", SetBool, self.shutdown_service) self.last_low_voltage = -1 @@ -1345,8 +1348,19 @@ async def callback(self, data): bs.power_supply_health = 0 # uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0 bs.power_supply_technology = 0 # uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 self.ina_publisher.publish(bs) + self.integrate_usage() await self.turn_off_cb() + def integrate_usage(self): + time_diff = time.time() - self.last_used_calc # seconds + self.last_used_calc = time.time() + used_mA_sec = time_diff * self.current * 1000 + used_mAh = used_mA_sec/3600 + print(time_diff, self.current, used_mA_sec, used_mAh, self.used_energy) + self.used_energy += used_mAh + + self.ina_publisher_used.publish(int(self.used_energy)) + async def turn_off_cb(self): if self.turn_off_pin == -1: # Dont turn off when this feature is disabled From 4100fb85261acf9d4bf01e48f2fb47dc2ec3a34d Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 26 Feb 2024 15:52:16 +0100 Subject: [PATCH 52/77] yaml fixes --- .../config/mirte_master_config.yaml | 2 +- .../scripts/ROS_telemetrix_aio_api.py | 78 ++++++++++++++----- 2 files changed, 58 insertions(+), 22 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 1a6782d9..5fe31447 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -18,7 +18,7 @@ modules: pin_A: 3 pin_B: 2 right_rear: - pin_A: 4 + pin_A: 4 pin_B: 5 right_front: pin_A: 6 diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index fcebec00..c7dc9f1f 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -33,9 +33,11 @@ async def analog_write(board, pin, value): else: await board.analog_write(pin, value) + def get_obj_value(s, obj, key, def_value=None): # shorthand of self.key = obj["key"] if "key" in obj else def_value - setattr(s, key, obj[key] if key in obj else def_value ) + setattr(s, key, obj[key] if key in obj else def_value) + # Import ROS message types from std_msgs.msg import Header, Int32 @@ -392,32 +394,46 @@ async def publish_data(self, data): await self.publish(encoder) + class Neopixel: def __init__(self, board, neo_obj): self.board = board self.settings = neo_obj self.pins = get_pin_numbers(neo_obj) self.name = neo_obj["name"] - self.pixels = neo_obj["pixels"] # num of leds + self.pixels = neo_obj["pixels"] # num of leds get_obj_value(self, neo_obj, "max_intensity", 50) get_obj_value(self, neo_obj, "default_color", 0x000000) - server = rospy.Service(f"/mirte/set_{self.name}_color_all", SetLEDValue, self.set_color_all_service) - server = rospy.Service(f"/mirte/set_{self.name}_color_single", SetSingleLEDValue, self.set_color_single_service) + server = rospy.Service( + f"/mirte/set_{self.name}_color_all", SetLEDValue, self.set_color_all_service + ) + server = rospy.Service( + f"/mirte/set_{self.name}_color_single", + SetSingleLEDValue, + self.set_color_single_service, + ) - async def start(self): colors = self.unpack_color_and_scale(self.default_color) - await board.set_pin_mode_neopixel( pin_number=self.pins["pin"], num_pixels=self.pixels, fill_r=colors[0], fill_g=colors[1], fill_b=colors[2] ) + await board.set_pin_mode_neopixel( + pin_number=self.pins["pin"], + num_pixels=self.pixels, + fill_r=colors[0], + fill_g=colors[1], + fill_b=colors[2], + ) async def set_color_all(self, color): colors = self.unpack_color_and_scale(color) - await board.neopixel_fill( r=colors[0], g=colors[1], b=colors[2] ) + await board.neopixel_fill(r=colors[0], g=colors[1], b=colors[2]) async def set_color_single(self, pixel, color): colors = self.unpack_color_and_scale(color) - if(pixel >=self.pixels): + if pixel >= self.pixels: return False - await board.neo_pixel_set_value(pixel, r=colors[0], g=colors[1], b=colors[2], auto_show=True ) + await board.neo_pixel_set_value( + pixel, r=colors[0], g=colors[1], b=colors[2], auto_show=True + ) return True def set_color_all_service(self, req): @@ -428,8 +444,16 @@ def set_color_single_service(self, req): ok = asyncio.run(self.set_color_single(req.pixel, req.value)) return SetSingleLEDValueResponse(ok) - def unpack_color_and_scale(self, color_num): # hex color number (0x123456) or string ("0x123456") - return [int(x*0.5) for x in struct.unpack('BBB', bytes.fromhex(hex(int(str(color_num), 0))[2:].zfill(6)))] + def unpack_color_and_scale( + self, color_num + ): # hex color number (0x123456) or string ("0x123456") + return [ + int(x * 0.5) + for x in struct.unpack( + "BBB", bytes.fromhex(hex(int(str(color_num), 0))[2:].zfill(6)) + ) + ] + class Servo: def __init__(self, board, servo_obj): @@ -1081,7 +1105,7 @@ def sensors(loop, board, device): tasks.append(loop.create_task(monitor.start())) # encoder sensors do not need a max_frequency. They are interrupts on # on the mcu side. - + # Get a raw pin value # TODO: this still needs to be tested. We are waiting on an implementation of ananlog_read() # on the telemetrix side @@ -1278,11 +1302,13 @@ def __init__(self, board, module_name, module): self.ina_publisher = rospy.Publisher( "/mirte/power/" + module_name, BatteryState, queue_size=1 ) - self.ina_publisher_used = rospy.Publisher(f"/mirte/power/{module_name}/used", Int32, queue_size=1) - self.used_energy = 0 # mah + self.ina_publisher_used = rospy.Publisher( + f"/mirte/power/{module_name}/used", Int32, queue_size=1 + ) + self.used_energy = 0 # mah self.last_used_calc = time.time() server = rospy.Service("/mirte/shutdown", SetBool, self.shutdown_service) - + self.last_low_voltage = -1 async def start(self): @@ -1325,7 +1351,11 @@ async def callback(self, data): vals = list(struct.unpack("<2f", bytes_obj)) self.voltage = vals[0] self.current = vals[1] - if self.min_voltage != -1 and self.voltage < self.min_voltage and self.last_low_voltage != self.voltage: + if ( + self.min_voltage != -1 + and self.voltage < self.min_voltage + and self.last_low_voltage != self.voltage + ): rospy.logwarn("Low voltage: %f", self.voltage) self.last_low_voltage = self.voltage if self.max_voltage != -1 and self.voltage > self.max_voltage: @@ -1352,10 +1382,10 @@ async def callback(self, data): await self.turn_off_cb() def integrate_usage(self): - time_diff = time.time() - self.last_used_calc # seconds + time_diff = time.time() - self.last_used_calc # seconds self.last_used_calc = time.time() used_mA_sec = time_diff * self.current * 1000 - used_mAh = used_mA_sec/3600 + used_mAh = used_mA_sec / 3600 print(time_diff, self.current, used_mA_sec, used_mAh, self.used_energy) self.used_energy += used_mAh @@ -1404,6 +1434,7 @@ def shutdown_service(self, req): asyncio.run(self.shutdown_robot()) return SetBoolResponse(True, "") + class Hiwonder_Servo: def __init__(self, servo_name, servo_obj, bus): self.id = servo_obj["id"] @@ -1452,9 +1483,14 @@ def set_servo_angle_service(self, req): return SetServoAngleResponse(True) def callback(self, data): - angle = int(scale(data["angle"], - [self.min_angle_out, self.max_angle_out],[self.min_angle_in, self.max_angle_in])) - self.publisher.publish(angle) + angle = int( + scale( + data["angle"], + [self.min_angle_out, self.max_angle_out], + [self.min_angle_in, self.max_angle_in], + ) + ) + self.publisher.publish(angle) class Hiwonder_Bus: From c98de9487a58adbd316dbf2ec777b241fe8624b0 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Mon, 26 Feb 2024 15:24:49 +0000 Subject: [PATCH 53/77] Fix odom --- mirte_bringup/launch/minimal_master.launch | 6 +++--- mirte_control/config/control_master.yaml | 3 ++- mirte_control/include/my_robot_hw_interface.h | 13 ++++++++----- mirte_telemetrix/config/mirte_master_config.yaml | 2 +- 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index b2570b48..46996bcf 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -16,7 +16,7 @@ - @@ -33,13 +33,13 @@ args="mobile_base_controller"/> - + diff --git a/mirte_control/config/control_master.yaml b/mirte_control/config/control_master.yaml index 45dc6a8e..4cd0484e 100644 --- a/mirte_control/config/control_master.yaml +++ b/mirte_control/config/control_master.yaml @@ -3,6 +3,7 @@ my_robot: publish_rate: 50 mobile_base_controller: + ticks: 330 # TODO: check 11pps*30reduction type: "mecanum_drive_controller/MecanumDriveController" front_left_wheel_joint: "wheel_left_front_joint" back_left_wheel_joint: "wheel_left_rear_joint" @@ -16,7 +17,7 @@ mobile_base_controller: # Override URDF look-up for wheel separation since the parent link is not the base_link. wheel_separation_x: 0.638 wheel_separation_y: 0.551 - wheel_radius: 0.08 + wheel_radius: 0.08 # 8cm # Odometry fused with IMU is published by robot_localization, so # no need to publish a TF based on encoders alone. enable_odom_tf: true diff --git a/mirte_control/include/my_robot_hw_interface.h b/mirte_control/include/my_robot_hw_interface.h index 71cd49bd..f8d89b66 100644 --- a/mirte_control/include/my_robot_hw_interface.h +++ b/mirte_control/include/my_robot_hw_interface.h @@ -37,6 +37,7 @@ // const unsigned int NUM_JOINTS = 4; const auto service_format = "/mirte/set_%s_speed"; +const auto encoder_format = "/mirte/encoder/%s"; const auto max_speed = 80; // Quick fix hopefully for power dip. /// \brief Hardware interface for a robot class MyRobotHWInterface : public hardware_interface::RobotHW { @@ -90,8 +91,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { } double meter_per_enc_tick() { - return (_wheel_diameter / 2) * 2 * M_PI / - 40.0; // TODO: get ticks from parameter server + return (this->_wheel_diameter / 2) * 2 * M_PI / + this->ticks; } /** @@ -138,6 +139,8 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { bool running_ = true; double _wheel_diameter; double _max_speed; + double ticks = 40.0; + std::vector _wheel_encoder; std::vector _last_cmd; std::vector _last_value; @@ -220,8 +223,8 @@ MyRobotHWInterface::MyRobotHWInterface() stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, this)) { private_nh.param("wheel_diameter", _wheel_diameter, 0.06); - private_nh.param("max_speed", _max_speed, 2.0); - // private_nh.param("bidirectional", bidirectional, true); + private_nh.param("max_speed", _max_speed, 2.0); // TODO: unused + private_nh.param("ticks", ticks, 40.0); this->NUM_JOINTS = detect_joints(private_nh); // Initialize raw data for (size_t i = 0; i < NUM_JOINTS; i++) { @@ -272,7 +275,7 @@ MyRobotHWInterface::MyRobotHWInterface() // Initialize publishers and subscribers for (size_t i = 0; i < NUM_JOINTS; i++) { auto encoder_topic = - (boost::format(service_format) % this->joints[i]).str(); + (boost::format(encoder_format) % this->joints[i]).str(); wheel_encoder_subs_.push_back(nh.subscribe( encoder_topic, 1, boost::bind(&MyRobotHWInterface::WheelEncoderCallback, this, _1, i))); diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 1a6782d9..5fe31447 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -18,7 +18,7 @@ modules: pin_A: 3 pin_B: 2 right_rear: - pin_A: 4 + pin_A: 4 pin_B: 5 right_front: pin_A: 6 From ccc4db10c651cdde2f108d1f09f51a8e40a711af Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 26 Feb 2024 16:54:00 +0100 Subject: [PATCH 54/77] fix catkin_lint check --- .github/workflows/catkin-lint.yml | 6 +++++- mirte_bringup/package.xml | 4 +++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/.github/workflows/catkin-lint.yml b/.github/workflows/catkin-lint.yml index d97014a7..e51e8233 100644 --- a/.github/workflows/catkin-lint.yml +++ b/.github/workflows/catkin-lint.yml @@ -7,7 +7,7 @@ jobs: image: ros:noetic steps: - uses: actions/checkout@v4 - - run: apt update && apt install catkin-lint -y + - run: apt update && apt install catkin-lint git libuvc-dev -y - run: | mkdir -p catkin_ws/src mv mirte_bringup catkin_ws/src/ @@ -16,6 +16,10 @@ jobs: mv mirte_telemetrix catkin_ws/src/ mv mirte_teleop catkin_ws/src/ cd catkin_ws/src + git clone https://github.com/arendjan/ridgeback.git + git clone https://github.com/Slamtec/rplidar_ros.git + git clone https://github.com/orbbec/ros_astra_camera.git + . /opt/ros/noetic/setup.sh catkin_init_workspace cd .. diff --git a/mirte_bringup/package.xml b/mirte_bringup/package.xml index d7b43959..48319789 100644 --- a/mirte_bringup/package.xml +++ b/mirte_bringup/package.xml @@ -55,7 +55,9 @@ mirte_teleop mirte_control mirte_msgs - + tf + rplidar_ros + astra_camera usb_cam image_common image_transport_plugins From 96758c0ace8f1f0aa844a470c65528160e9ede9a Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 4 Mar 2024 10:08:07 +0100 Subject: [PATCH 55/77] add wall shutdown system --- mirte_control/include/my_robot_hw_interface.h | 3 +-- .../scripts/ROS_telemetrix_aio_api.py | 26 +++++++++++++++---- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/mirte_control/include/my_robot_hw_interface.h b/mirte_control/include/my_robot_hw_interface.h index f8d89b66..a238c14a 100644 --- a/mirte_control/include/my_robot_hw_interface.h +++ b/mirte_control/include/my_robot_hw_interface.h @@ -91,8 +91,7 @@ class MyRobotHWInterface : public hardware_interface::RobotHW { } double meter_per_enc_tick() { - return (this->_wheel_diameter / 2) * 2 * M_PI / - this->ticks; + return (this->_wheel_diameter / 2) * 2 * M_PI / this->ticks; } /** diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index c7dc9f1f..1df41ba9 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -35,8 +35,20 @@ async def analog_write(board, pin, value): def get_obj_value(s, obj, key, def_value=None): - # shorthand of self.key = obj["key"] if "key" in obj else def_value - setattr(s, key, obj[key] if key in obj else def_value) + # shorthand of self.key = obj["key"] if "key" in obj else def_value with type checking + def_type = type(def_value) + out = obj[key] if key in obj else def_value + out_type = type(out) + if def_type != out_type: + if def_type == int: + out = int(out) + if def_type == float: + out = float(out) + if def_type == str: + out = str(out) + if def_type == bool: + out = bool(out) + setattr(s, key, out) # Import ROS message types @@ -1297,6 +1309,7 @@ def __init__(self, board, module_name, module): module["turn_off_time"] if "turn_off_time" in module else 30 ) # time to wait for computer to shut down self.enable_turn_off = False # require at least a single message with a real value before arming the turn off system + get_obj_value(self, module, "power_low_time", 5) # how long(s) for the voltage to be below the trigger voltage before triggering shutting down self.shutdown_triggered = False self.turn_off_trigger_start_time = -1 self.ina_publisher = rospy.Publisher( @@ -1312,12 +1325,12 @@ def __init__(self, board, module_name, module): self.last_low_voltage = -1 async def start(self): - print("start ina!") # setup i2c, check with oled to not init twice if board_mapping.get_mcu() == "pico": if "connector" in self.module: pins = board_mapping.connector_to_pins(self.module["connector"]) else: + # TODO: no other boards have support for this yet pins = self.module["pins"] pin_numbers = {} for item in pins: @@ -1403,14 +1416,16 @@ async def turn_off_cb(self): rospy.logwarn("Shutdown relay armed") return - # at first dip of too low voltage, start timer, when longer than 5s below trigger voltage, then shut down + # at start dip of too low voltage, start timer, when longer than 5s below trigger voltage, then shut down # this makes sure that a short dip (motor start) does not trigger it if self.min_voltage != -1 and self.voltage < self.min_voltage: if self.turn_off_trigger_start_time == -1: self.turn_off_trigger_start_time = time.time() + # Send a message to all users that the voltage is low and possibly shutting down + subprocess.run(f"wall 'Low voltage, shutting down in {self.power_low_time}s if not restored.'") rospy.logwarn( "Low voltage, %ss till shutdown.", - 5 - (time.time() - self.turn_off_trigger_start_time), + self.power_low_time - (time.time() - self.turn_off_trigger_start_time), ) else: self.turn_off_trigger_start_time = -1 @@ -1423,6 +1438,7 @@ async def turn_off_cb(self): async def shutdown_robot(self): if not self.shutdown_triggered: + subprocess.run(f"wall 'Low voltage, shutting down now.'") rospy.logerr("Triggering shutdown, shutting down in 10s") # This will make the pico unresponsive after the delay, so ping errors are expected. Need to restart telemetrix to continue. await self.trigger_shutdown_relay() From 62b311969ece4e48074325c729e96ba79e7fda6e Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 4 Mar 2024 10:12:56 +0100 Subject: [PATCH 56/77] set config to safe value --- mirte_telemetrix/config/mirte_master_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 5fe31447..fe69df4d 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -105,6 +105,6 @@ encoder: # type: INA226 # connector: I2C1 # id: 0x41 - # min_voltage: 11 + # min_voltage: 11.18 # max_current: 1 - # max_voltage: 15 + # max_voltage: 13.0 From beb6109269e9beec75bf8882702caa6da23104da Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 4 Mar 2024 11:10:48 +0100 Subject: [PATCH 57/77] add show ip, hostname and wifi --- .../config/mirte_master_config.yaml | 8 ++++++ .../scripts/ROS_telemetrix_aio_api.py | 25 +++++++++++++++++-- 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index fe69df4d..97e37d2c 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -74,6 +74,14 @@ encoder: pins: A: 15 B: 14 +oled: + middle: + device: mirte + connector: I2C1 + show_hostname: True + show_ip: True + show_wifi: True + # neopixel: # name: leds # pins: diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 1df41ba9..ff3671c5 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -735,6 +735,23 @@ async def start(self): print("write failed start", self.oled_obj["name"]) self.failed = True return + await self.show_default() + + async def show_default(self): + text = "" + if "show_ip" in self.oled_obj and self.oled_obj["show_ip"]: + ips = subprocess.getoutput("hostname -I").split(" ") + text += "IPs: " + ', '.join(ips) + if "show_hostname" in self.oled_obj and self.oled_obj["show_hostname"]: + text += "\nHostname:" + subprocess.getoutput("hostname") + if "show_wifi" in self.oled_obj and self.oled_obj["show_wifi"]: + wifi = subprocess.getoutput("iwgetid -r").strip() + if len(wifi) > 0: + text += "\nWi-Fi:" + wifi + if len(text) > 0: + await self.set_oled_image_service_async( + SetOLEDImageRequest(type="text", value=text) + ) async def set_oled_image_service_async(self, req): if req.type == "text": @@ -1309,7 +1326,9 @@ def __init__(self, board, module_name, module): module["turn_off_time"] if "turn_off_time" in module else 30 ) # time to wait for computer to shut down self.enable_turn_off = False # require at least a single message with a real value before arming the turn off system - get_obj_value(self, module, "power_low_time", 5) # how long(s) for the voltage to be below the trigger voltage before triggering shutting down + get_obj_value( + self, module, "power_low_time", 5 + ) # how long(s) for the voltage to be below the trigger voltage before triggering shutting down self.shutdown_triggered = False self.turn_off_trigger_start_time = -1 self.ina_publisher = rospy.Publisher( @@ -1422,7 +1441,9 @@ async def turn_off_cb(self): if self.turn_off_trigger_start_time == -1: self.turn_off_trigger_start_time = time.time() # Send a message to all users that the voltage is low and possibly shutting down - subprocess.run(f"wall 'Low voltage, shutting down in {self.power_low_time}s if not restored.'") + subprocess.run( + f"wall 'Low voltage, shutting down in {self.power_low_time}s if not restored.'" + ) rospy.logwarn( "Low voltage, %ss till shutdown.", self.power_low_time - (time.time() - self.turn_off_trigger_start_time), From 129eb6e606747b1ae280fc36e1c2827f83590c02 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 4 Mar 2024 11:15:11 +0100 Subject: [PATCH 58/77] Style fix --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index ff3671c5..db667a89 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -741,7 +741,7 @@ async def show_default(self): text = "" if "show_ip" in self.oled_obj and self.oled_obj["show_ip"]: ips = subprocess.getoutput("hostname -I").split(" ") - text += "IPs: " + ', '.join(ips) + text += "IPs: " + ", ".join(ips) if "show_hostname" in self.oled_obj and self.oled_obj["show_hostname"]: text += "\nHostname:" + subprocess.getoutput("hostname") if "show_wifi" in self.oled_obj and self.oled_obj["show_wifi"]: From f77235729ca363f6066f5420756fb01054268aa9 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Mon, 4 Mar 2024 20:33:17 +0000 Subject: [PATCH 59/77] Add turnoff switch --- .../config/mirte_master_config.yaml | 85 ++++++++++++------- .../scripts/ROS_telemetrix_aio_api.py | 77 ++++++++++++++++- 2 files changed, 125 insertions(+), 37 deletions(-) diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 97e37d2c..8cade744 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -38,42 +38,60 @@ modules: id: 4 min_angle: 800 max_angle: 4000 - # power_watcher: - # device: mirte - # type: INA226 - # connector: I2C1 - # id: 0b1000101 - # min_voltage: 11 - # max_current: 1 - # max_voltage: 15 - # turn_off_pin: 27 - # turn_off_value: True - # turn_off_time: 10 -encoder: - left_front: - name: left_front - device: mirte - pins: - A: 21 - B: 20 - left_rear: - name: left_rear - device: mirte - pins: - A: 17 - B: 16 - right_front: - name: right_front + power_watcher: device: mirte - pins: - A: 18 - B: 19 - right_rear: - name: right_rear + type: INA226 + connector: I2C1 + id: 0b1000101 + min_voltage: 11 + max_current: 1 + max_voltage: 15 + switch_pin: GP28 + switch_off_value: True + switch_pull: 1 # -1 for pulldown, 1 for pullup, 0 for none + switch_time: 5 + # turn_off_pin: 27 + # turn_off_value: True + # turn_off_time: 10 +# encoder: +# left_front: +# name: left_front +# device: mirte +# pins: +# A: 21 +# B: 20 +# left_rear: +# name: left_rear +# device: mirte +# pins: +# A: 17 +# B: 16 +# right_front: +# name: right_front +# device: mirte +# pins: +# A: 18 +# B: 19 +# right_rear: +# name: right_rear +# device: mirte +# pins: +# A: 15 +# B: 14 +neopixel: + name: leds + pins: + pin: 27 + default_color: 0xF + max_intensity: 50 + pixels: 100 +intensity: + left: + name: left device: mirte pins: - A: 15 - B: 14 + analog: GP26 + digital: GP10 oled: middle: device: mirte @@ -90,6 +108,7 @@ oled: # max_intensity: 50 # pixels: 18 + # servos: # servo1: # pin: 1 diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index db667a89..f3736b8c 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1343,6 +1343,16 @@ def __init__(self, board, module_name, module): self.last_low_voltage = -1 + self.switch_pin = ( + board_mapping.pin_name_to_pin_number(module["switch_pin"]) + if "switch_pin" in module + else -1 + ) + get_obj_value(self, module, "switch_off_value", True) + get_obj_value(self, module, "switch_pull", 0) + get_obj_value(self, module, "switch_time", 5) + + async def start(self): # setup i2c, check with oled to not init twice if board_mapping.get_mcu() == "pico": @@ -1375,6 +1385,60 @@ async def start(self): self.turn_off_time + 10, # add 10s to the shutdown time for the 10s shutdown command wait time ) + if(self.switch_pin > 0): + if(self.switch_pull == 1): + await self.board.set_pin_mode_digital_input_pullup( + self.switch_pin, callback=self.switch_data, + ) + elif(self.switch_pull == -1): + await self.board.set_pin_mode_digital_input_pull_down( + self.switch_pin, callback=self.switch_data, + ) + else: + await self.board.set_pin_mode_digital_input( + self.switch_pin, callback=self.switch_data, + ) + self.switch_armed = False + self.switch_trigger_start_time = -1 + self.switch_val = -1 + rospy.Timer(rospy.Duration(0.5), self.check_switch_sync) + + + + async def switch_data(self, data): + self.switch_val= bool(data[2]) + await self.check_switch() + + def check_switch_sync(self, event=None): + asyncio.run(self.check_switch()) + async def check_switch(self): + if(self.switch_val == -1): + return + if(not self.switch_armed): + if(self.switch_val != self.switch_off_value): + rospy.logwarn("Shutdown switch armed") + self.switch_armed = True + return + + if(self.switch_val == self.switch_off_value): + if self.switch_trigger_start_time == -1: + self.switch_trigger_start_time = time.time() + rospy.logwarn(f"Switch turned off, shutting down in {self.switch_time}s if not restored.") + + # Send a message to all users that the switch is off and possibly shutting down + subprocess.run( + f"wall 'Switch turned off, shutting down in {self.switch_time}s if not restored.'", shell=True + ) + else: + self.switch_trigger_start_time = -1 + + if ( + self.switch_trigger_start_time != -1 + and time.time() - self.switch_trigger_start_time > self.switch_time + ): + await self.shutdown_robot() + + async def callback(self, data): # TODO: move this decoding to the library @@ -1418,7 +1482,6 @@ def integrate_usage(self): self.last_used_calc = time.time() used_mA_sec = time_diff * self.current * 1000 used_mAh = used_mA_sec / 3600 - print(time_diff, self.current, used_mA_sec, used_mAh, self.used_energy) self.used_energy += used_mAh self.ina_publisher_used.publish(int(self.used_energy)) @@ -1453,16 +1516,22 @@ async def turn_off_cb(self): if ( self.turn_off_trigger_start_time != -1 - and time.time() - self.turn_off_trigger_start_time > 5 + and time.time() - self.turn_off_trigger_start_time > self.power_low_time ): + subprocess.run("wall 'Low voltage, shutting down now.'", shell=True) await self.shutdown_robot() async def shutdown_robot(self): if not self.shutdown_triggered: - subprocess.run(f"wall 'Low voltage, shutting down now.'") + subprocess.run( + f"bash -c \"wall 'Shutting down.'\"", shell=True + ) + # ("echo 'Shutting down now.'") + rospy.logerr("Triggering shutdown, shutting down in 10s") # This will make the pico unresponsive after the delay, so ping errors are expected. Need to restart telemetrix to continue. - await self.trigger_shutdown_relay() + if(hasattr(self,"trigger_shutdown_relay" )): + await self.trigger_shutdown_relay() self.shutdown_triggered = True # subprocess.run("sudo shutdown 10s") From 365afeb25a1c8e402607e3bc6d236a974e878eab Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Mon, 4 Mar 2024 20:43:27 +0000 Subject: [PATCH 60/77] add wall comment --- mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index f3736b8c..53f7993e 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1524,9 +1524,9 @@ async def turn_off_cb(self): async def shutdown_robot(self): if not self.shutdown_triggered: subprocess.run( + # wall does not show up on vscode terminal f"bash -c \"wall 'Shutting down.'\"", shell=True ) - # ("echo 'Shutting down now.'") rospy.logerr("Triggering shutdown, shutting down in 10s") # This will make the pico unresponsive after the delay, so ping errors are expected. Need to restart telemetrix to continue. From b8dd9ada881574f510ddeb1231057574ae1fe6f3 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 4 Mar 2024 21:45:31 +0100 Subject: [PATCH 61/77] style fixes --- .../scripts/ROS_telemetrix_aio_api.py | 51 ++++++++++--------- 1 file changed, 27 insertions(+), 24 deletions(-) diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index 53f7993e..a6fef0e8 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1352,7 +1352,6 @@ def __init__(self, board, module_name, module): get_obj_value(self, module, "switch_pull", 0) get_obj_value(self, module, "switch_time", 5) - async def start(self): # setup i2c, check with oled to not init twice if board_mapping.get_mcu() == "pico": @@ -1385,49 +1384,54 @@ async def start(self): self.turn_off_time + 10, # add 10s to the shutdown time for the 10s shutdown command wait time ) - if(self.switch_pin > 0): - if(self.switch_pull == 1): + if self.switch_pin > 0: + if self.switch_pull == 1: await self.board.set_pin_mode_digital_input_pullup( - self.switch_pin, callback=self.switch_data, - ) - elif(self.switch_pull == -1): + self.switch_pin, + callback=self.switch_data, + ) + elif self.switch_pull == -1: await self.board.set_pin_mode_digital_input_pull_down( - self.switch_pin, callback=self.switch_data, - ) + self.switch_pin, + callback=self.switch_data, + ) else: await self.board.set_pin_mode_digital_input( - self.switch_pin, callback=self.switch_data, - ) + self.switch_pin, + callback=self.switch_data, + ) self.switch_armed = False self.switch_trigger_start_time = -1 self.switch_val = -1 rospy.Timer(rospy.Duration(0.5), self.check_switch_sync) - - async def switch_data(self, data): - self.switch_val= bool(data[2]) + self.switch_val = bool(data[2]) await self.check_switch() def check_switch_sync(self, event=None): asyncio.run(self.check_switch()) + async def check_switch(self): - if(self.switch_val == -1): + if self.switch_val == -1: return - if(not self.switch_armed): - if(self.switch_val != self.switch_off_value): + if not self.switch_armed: + if self.switch_val != self.switch_off_value: rospy.logwarn("Shutdown switch armed") self.switch_armed = True return - - if(self.switch_val == self.switch_off_value): + + if self.switch_val == self.switch_off_value: if self.switch_trigger_start_time == -1: self.switch_trigger_start_time = time.time() - rospy.logwarn(f"Switch turned off, shutting down in {self.switch_time}s if not restored.") + rospy.logwarn( + f"Switch turned off, shutting down in {self.switch_time}s if not restored." + ) # Send a message to all users that the switch is off and possibly shutting down subprocess.run( - f"wall 'Switch turned off, shutting down in {self.switch_time}s if not restored.'", shell=True + f"wall 'Switch turned off, shutting down in {self.switch_time}s if not restored.'", + shell=True, ) else: self.switch_trigger_start_time = -1 @@ -1438,8 +1442,6 @@ async def check_switch(self): ): await self.shutdown_robot() - - async def callback(self, data): # TODO: move this decoding to the library ints = list(map(lambda i: i.to_bytes(1, "big"), data)) @@ -1525,12 +1527,13 @@ async def shutdown_robot(self): if not self.shutdown_triggered: subprocess.run( # wall does not show up on vscode terminal - f"bash -c \"wall 'Shutting down.'\"", shell=True + f"bash -c \"wall 'Shutting down.'\"", + shell=True, ) rospy.logerr("Triggering shutdown, shutting down in 10s") # This will make the pico unresponsive after the delay, so ping errors are expected. Need to restart telemetrix to continue. - if(hasattr(self,"trigger_shutdown_relay" )): + if hasattr(self, "trigger_shutdown_relay"): await self.trigger_shutdown_relay() self.shutdown_triggered = True # subprocess.run("sudo shutdown 10s") From be03c1c83422d30f46044e9f7cb564b16c3ca069 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 5 Mar 2024 15:36:02 +0100 Subject: [PATCH 62/77] start usb_switch package --- mirte_master/CMakeLists.txt | 204 +++++++++++++++++++++++++++++ mirte_master/package.xml | 62 +++++++++ mirte_master/scripts/usb_switch.py | 5 + 3 files changed, 271 insertions(+) create mode 100644 mirte_master/CMakeLists.txt create mode 100644 mirte_master/package.xml create mode 100644 mirte_master/scripts/usb_switch.py diff --git a/mirte_master/CMakeLists.txt b/mirte_master/CMakeLists.txt new file mode 100644 index 00000000..290b90f3 --- /dev/null +++ b/mirte_master/CMakeLists.txt @@ -0,0 +1,204 @@ +cmake_minimum_required(VERSION 3.0.2) +project(mirte_master) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES mirte_master +# CATKIN_DEPENDS rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/mirte_master.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/mirte_master_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mirte_master.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mirte_master/package.xml b/mirte_master/package.xml new file mode 100644 index 00000000..0ff45409 --- /dev/null +++ b/mirte_master/package.xml @@ -0,0 +1,62 @@ + + + mirte_master + 0.0.1 + The mirte_master package + + + + + Arend-Jan van Hilten + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + rospy + rospy + gpiod + + + + + + + diff --git a/mirte_master/scripts/usb_switch.py b/mirte_master/scripts/usb_switch.py new file mode 100644 index 00000000..898afcba --- /dev/null +++ b/mirte_master/scripts/usb_switch.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3.8 + + +import gpiod + From 286fe73e0f9c561fa81e3c27b290167aa01bce4d Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 6 Mar 2024 09:10:25 +0000 Subject: [PATCH 63/77] rm mirte_master usb switch code --- mirte_master/CMakeLists.txt | 204 ----------------------------- mirte_master/package.xml | 62 --------- mirte_master/scripts/usb_switch.py | 5 - 3 files changed, 271 deletions(-) delete mode 100644 mirte_master/CMakeLists.txt delete mode 100644 mirte_master/package.xml delete mode 100644 mirte_master/scripts/usb_switch.py diff --git a/mirte_master/CMakeLists.txt b/mirte_master/CMakeLists.txt deleted file mode 100644 index 290b90f3..00000000 --- a/mirte_master/CMakeLists.txt +++ /dev/null @@ -1,204 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(mirte_master) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES mirte_master -# CATKIN_DEPENDS rospy -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/mirte_master.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/mirte_master_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mirte_master.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/mirte_master/package.xml b/mirte_master/package.xml deleted file mode 100644 index 0ff45409..00000000 --- a/mirte_master/package.xml +++ /dev/null @@ -1,62 +0,0 @@ - - - mirte_master - 0.0.1 - The mirte_master package - - - - - Arend-Jan van Hilten - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - rospy - rospy - gpiod - - - - - - - diff --git a/mirte_master/scripts/usb_switch.py b/mirte_master/scripts/usb_switch.py deleted file mode 100644 index 898afcba..00000000 --- a/mirte_master/scripts/usb_switch.py +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env python3.8 - - -import gpiod - From 812021085046e2cf8777bf498ce4b27f974837bd Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 6 Mar 2024 15:57:39 +0000 Subject: [PATCH 64/77] goede arm upd --- mirte_bringup/launch/base.launch | 45 +++++++++++++++++++ mirte_bringup/launch/minimal_master.launch | 27 ++++++++--- .../config/mirte_master_config.yaml | 10 ++--- .../scripts/ROS_telemetrix_aio_api.py | 3 +- mirte_teleop/config/ps3-holonomic.config.yaml | 2 +- ros_control_boilerplate | 1 + 6 files changed, 74 insertions(+), 14 deletions(-) create mode 100644 mirte_bringup/launch/base.launch create mode 160000 ros_control_boilerplate diff --git a/mirte_bringup/launch/base.launch b/mirte_bringup/launch/base.launch new file mode 100644 index 00000000..e40ce14d --- /dev/null +++ b/mirte_bringup/launch/base.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index b2570b48..36460f78 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -3,7 +3,7 @@ - + - +--> + + + + + - + - +--> + - + - + diff --git a/mirte_telemetrix/config/mirte_master_config.yaml b/mirte_telemetrix/config/mirte_master_config.yaml index 1a6782d9..7addd8d0 100644 --- a/mirte_telemetrix/config/mirte_master_config.yaml +++ b/mirte_telemetrix/config/mirte_master_config.yaml @@ -18,7 +18,7 @@ modules: pin_A: 3 pin_B: 2 right_rear: - pin_A: 4 + pin_A: 4 pin_B: 5 right_front: pin_A: 6 @@ -32,12 +32,12 @@ modules: servos: servoH1: id: 3 - min_angle: 9000 - max_angle: 14000 + min_angle_out: 9000 + max_angle_out: 14000 servoH2: id: 4 - min_angle: 800 - max_angle: 4000 + min_angle_out: 800 + max_angle_out: 4000 # power_watcher: # device: mirte # type: INA226 diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index fcebec00..d9ff60a8 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -1174,6 +1174,7 @@ async def init_motors(self): await self.pca_update_func(self.pin_B, 0) async def set_speed(self, speed): + # print(self.name, speed) if self.inverted: speed = -speed if self.prev_motor_speed != speed: @@ -1445,7 +1446,7 @@ async def servo_write(self, angle): [self.min_angle_out, self.max_angle_out], ) angle = int(max(self.min_angle_out, min(angle, self.max_angle_out))) # clamp - await self.bus.set_single_servo(self.id, angle, 100) + await self.bus.set_single_servo(self.id, angle, 0) def set_servo_angle_service(self, req): asyncio.run(self.servo_write(req.angle)) diff --git a/mirte_teleop/config/ps3-holonomic.config.yaml b/mirte_teleop/config/ps3-holonomic.config.yaml index 7a0b97dd..905acb84 100644 --- a/mirte_teleop/config/ps3-holonomic.config.yaml +++ b/mirte_teleop/config/ps3-holonomic.config.yaml @@ -8,5 +8,5 @@ axis_angular: yaw: 3 scale_angular: yaw: 2 -enable_button: 0 +enable_button: 5 enable_turbo_button: 8 diff --git a/ros_control_boilerplate b/ros_control_boilerplate new file mode 160000 index 00000000..0f9cd01b --- /dev/null +++ b/ros_control_boilerplate @@ -0,0 +1 @@ +Subproject commit 0f9cd01bc1430c718554a3801683ada9cd2aa0a6 From bee57a16da44d74f863cb41ed7d4cec0bc28c3f3 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 6 Mar 2024 17:41:59 +0000 Subject: [PATCH 65/77] Fix hiwonder to float, with home and radians --- mirte_bringup/launch/minimal_master.launch | 6 +-- mirte_msgs/srv/SetServoAngle.srv | 2 +- .../config/mirte_master_config.yaml | 4 ++ .../config/mirte_master_config_goede_arm.yaml | 8 ++++ .../scripts/ROS_telemetrix_aio_api.py | 37 +++++++++++++++---- 5 files changed, 45 insertions(+), 12 deletions(-) diff --git a/mirte_bringup/launch/minimal_master.launch b/mirte_bringup/launch/minimal_master.launch index 648db417..07aa1738 100644 --- a/mirte_bringup/launch/minimal_master.launch +++ b/mirte_bringup/launch/minimal_master.launch @@ -23,15 +23,15 @@ - + - + - - + - + - + args="mobile_base_controller joint_state_controller position_trajectory_controller"/>