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");