From 0371896b0496ee21f95f99b1d05529a5955dcbc5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Du=C5=A1an=20Jovanovi=C4=87?= Date: Wed, 25 May 2022 18:16:27 +0200 Subject: [PATCH] Osnovna strategija florian big (#221) * new stuck implemented, but NOT IN STRAT * main simple strategy * fix resistance meter driver * Set new dynamixel angle position, set position for big on yellow side * add homologation, mirror servos --- .../assets/strategies/big/florian.xml | 85 ++++++++++--------- .../strategies/big/florian_homologation.xml | 51 +++++++++++ .../assets/strategies/big/test_stuck.xml | 42 +++++++++ mep3_bringup/launch/robot_launch.py | 9 +- .../mep3_driver/resistance_meter_driver.py | 5 +- .../distance_angle_regulator.cpp | 21 ++++- 6 files changed, 161 insertions(+), 52 deletions(-) create mode 100644 mep3_behavior_tree/assets/strategies/big/florian_homologation.xml create mode 100644 mep3_behavior_tree/assets/strategies/big/test_stuck.xml diff --git a/mep3_behavior_tree/assets/strategies/big/florian.xml b/mep3_behavior_tree/assets/strategies/big/florian.xml index 10a94e0a5..125bc92ba 100644 --- a/mep3_behavior_tree/assets/strategies/big/florian.xml +++ b/mep3_behavior_tree/assets/strategies/big/florian.xml @@ -5,15 +5,15 @@ - + - + @@ -24,17 +24,17 @@ - + - + - + - + @@ -46,22 +46,24 @@ - + + + - + - + - + - + @@ -82,13 +84,13 @@ - - + + - + @@ -97,8 +99,8 @@ - - + + @@ -106,14 +108,12 @@ - - - - - - - - + + + + + + @@ -124,7 +124,8 @@ - + + @@ -135,38 +136,38 @@ - + - + - + - + - - + + - + @@ -174,20 +175,20 @@ - - + + - + - + - + @@ -200,7 +201,7 @@ - + @@ -208,7 +209,7 @@ - + @@ -221,7 +222,7 @@ - + @@ -235,9 +236,9 @@ - + - + diff --git a/mep3_behavior_tree/assets/strategies/big/florian_homologation.xml b/mep3_behavior_tree/assets/strategies/big/florian_homologation.xml new file mode 100644 index 000000000..90bfd7d3a --- /dev/null +++ b/mep3_behavior_tree/assets/strategies/big/florian_homologation.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior_tree/assets/strategies/big/test_stuck.xml b/mep3_behavior_tree/assets/strategies/big/test_stuck.xml new file mode 100644 index 000000000..68d68f001 --- /dev/null +++ b/mep3_behavior_tree/assets/strategies/big/test_stuck.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py index fdcc49983..0b8a76c18 100644 --- a/mep3_bringup/launch/robot_launch.py +++ b/mep3_bringup/launch/robot_launch.py @@ -17,8 +17,8 @@ INITIAL_POSE_MATRIX = [ ('small', 'purple', [1.2755, 0.443, pi]), - ('big', 'yellow', [-1.236, 0.162, -pi / 2]), - ('big', 'purple', [1.249, 0.491, pi/2]), + ('big', 'yellow', [-1.249, 0.102, pi/2]), + ('big', 'purple', [1.249, 0.102, pi/2]), ('small', 'yellow', [-1.242, 0.452, pi/2]), ] @@ -51,6 +51,7 @@ 'fork_right' ] + def verify_color(context, *args, **kwargs): if LaunchConfiguration('color').perform(context) \ not in ['purple', 'yellow']: @@ -211,8 +212,8 @@ def generate_launch_description(): laser_inflator = Node(package='mep3_navigation', executable='laser_inflator', parameters=[{ - 'inflation_radius': 0.15, - 'inflation_angular_step': 0.12 + 'inflation_radius': 0.05, + 'inflation_angular_step': 0.09 }], remappings=[('/tf_static', 'tf_static'), ('/tf', 'tf'), diff --git a/mep3_driver/mep3_driver/resistance_meter_driver.py b/mep3_driver/mep3_driver/resistance_meter_driver.py index 9afeec1e9..11145275b 100755 --- a/mep3_driver/mep3_driver/resistance_meter_driver.py +++ b/mep3_driver/mep3_driver/resistance_meter_driver.py @@ -20,10 +20,7 @@ def readout_to_resistance(value): - i = 0.7e-3 # ADC current - c = 4096 # ADC resolution - v = 3.3 # ADC maximum voltage - return int(value / ((c / v) * i)) + return int(1.2*value - 250) class ResistanceMeterDriver(Node): diff --git a/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp b/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp index dd873cc36..959f02750 100644 --- a/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp +++ b/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp @@ -513,7 +513,7 @@ void DistanceAngleRegulator::motion_command() } lock.unlock(); - enum class MotionState { START, RUNNING_COMMAND, FINISHED }; + enum class MotionState { START, RUNNING_COMMAND, RUNNING_COMMAND_STUCK, FINISHED }; MotionState state = MotionState::START; @@ -547,14 +547,19 @@ void DistanceAngleRegulator::motion_command() case MotionState::START: if (goal->command == "forward") { forward(goal->value); + state = MotionState::RUNNING_COMMAND; + } else if (goal->command == "forward_stuck") { + forward(goal->value); + state = MotionState::RUNNING_COMMAND_STUCK; } else if (goal->command == "rotate_absolute") { rotate_absolute(goal->value); + state = MotionState::RUNNING_COMMAND; } else if (goal->command == "rotate_relative") { rotate_relative(goal->value); + state = MotionState::RUNNING_COMMAND; } timeout_counter = timeout; - state = MotionState::RUNNING_COMMAND; break; case MotionState::RUNNING_COMMAND: @@ -569,6 +574,18 @@ void DistanceAngleRegulator::motion_command() } break; + case MotionState::RUNNING_COMMAND_STUCK: + if (motion_profile_finished()) { + result->set__result("success"); + action_running_ = false; + reset_regulation(); + reset_stuck(); + + motion_command_server_->succeeded_current(result); + return; + } + break; + case MotionState::FINISHED: if (distance_regulator_finished() && angle_regulator_finished()) { result->set__result("success");