Reset the walk cycle when the manual penalty is activated. (#724)

Co-authored-by: Jan Gutsche <[email protected]>
Co-authored-by: Jan Gutsche <[email protected]>

Author: Flova

bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp

@@ -115,8 +115,10 @@ void WalkNode::run() {
   // necessary as timer in simulation does not work correctly https://github.com/ros2/rclcpp/issues/465
   if (dt != 0.0) {
     if (robot_state_ == bitbots_msgs::msg::RobotControlState::FALLING ||
-        robot_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP) {
-      // the robot fell, we have to reset everything and do nothing else
+        robot_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP ||
+        robot_state_ == bitbots_msgs::msg::RobotControlState::PENALTY) {
+      // The robot fell or the penalty button was pressed.
+      // We have to reset everything and do nothing else to ensure a stable restart afterwards.
       walk_engine_.reset();
       stabilizer_.reset();
     } else {