Merge branch 'main' into feature/imu_yaw_in_odometry

Author: val-ba

Makefile

@@ -90,7 +90,7 @@ rosdep:
 	# Initialize rosdep if not already done
 	[ -f /etc/ros/rosdep/sources.list.d/20-default.list ] || sudo rosdep init
 	# Update rosdep and install dependencies from meta directory
-	rosdep update
+	rosdep update --include-eol-distros
 	rosdep install --from-paths . --ignore-src --rosdistro iron -y
 
 status:

bitbots_misc/bitbots_utils/bitbots_utils/utils.py

@@ -130,6 +130,28 @@ def get_parameters_from_other_node(
     return results
 
 
+def get_parameters_from_other_node_sync(
+    own_node: Node, other_node_name: str, parameter_names: List[str], service_timeout_sec: float = 20.0
+) -> Dict:
+    """
+    Used to receive parameters from other running nodes. It does not use async internally.
+    It should not be used in callback functions, but it is a bit more reliable than the async version.
+    Returns a dict with requested parameter name as dict key and parameter value as dict value.
+    """
+    client = own_node.create_client(GetParameters, f"{other_node_name}/get_parameters")
+    ready = client.wait_for_service(timeout_sec=service_timeout_sec)
+    if not ready:
+        raise RuntimeError(f"Wait for {other_node_name} parameter service timed out")
+    request = GetParameters.Request()
+    request.names = parameter_names
+    response = client.call(request)
+
+    results = {}  # Received parameter
+    for i, param in enumerate(parameter_names):
+        results[param] = parameter_value_to_python(response.values[i])
+    return results
+
+
 def set_parameters_of_other_node(
     own_node: Node,
     other_node_name: str,

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py

@@ -21,15 +21,6 @@ def perform(self, reevaluate=False):
         raise NotImplementedError
 
 
-class TurnMotorsOn(AbstractChangeMotorPower):
-    def perform(self, reevaluate=False):
-        if not self.blackboard.visualization_active and not self.blackboard.simulation_active:
-            req = SetBool.Request()
-            req.data = True
-            self.blackboard.motor_switch_service.call(req)
-        return self.pop()
-
-
 class TurnMotorsOff(AbstractChangeMotorPower):
     def perform(self, reevaluate=False):
         if not self.blackboard.visualization_active and not self.blackboard.simulation_active:

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait.py

@@ -6,6 +6,7 @@
 
 Just waits for something (i.e. that preconditions will be fullfilled)
 """
+
 from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement
 
 
@@ -34,3 +35,16 @@ def perform(self, reevaluate=False):
         # Pop if the time has elapsed
         if self.blackboard.node.get_clock().now().nanoseconds / 1e9 > self.start_time + self.duration:
             self.pop()
+
+
+class WaitRosControlStartDelay(Wait):
+    """
+    The motors need some time to start up,
+    the time at which we start sending commands is defined in the config of the ros_control node.
+    This action waits for that time.
+    """
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.duration = self.blackboard.motor_start_delay
+        self.blackboard.node.get_logger().info("Waiting for the motors to start up...")

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py

@@ -12,6 +12,7 @@ class CheckMotors(AbstractHCMDecisionElement):
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
         self.had_problem = False
+        self.power_was_off = False
 
     def perform(self, reevaluate=False):
         self.clear_debug_data()
@@ -71,8 +72,16 @@ def perform(self, reevaluate=False):
                     # wait for motors to connect
                     return "PROBLEM"
             else:
-                # we have to turn the motors on
-                return "TURN_ON"
+                # The motors are off, so we will not complain
+                self.power_was_off = True
+                return "MOTORS_NOT_STARTED"
+
+        elif self.power_was_off:
+            # motors are now on and we can continue
+            self.blackboard.node.get_logger().info("Motors are now connected. Will resume.")
+            self.power_was_off = False
+            # But we want to perform a clean start, so we don't jump directly into the last goal position
+            return "TURN_ON"
 
         if self.had_problem:
             # had problem before, just tell that this is solved now

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd

@@ -1,17 +1,20 @@
 #EMERGENCY_FALL
 @RobotStateMotorOff, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait + time:1 + r:false, @TurnMotorsOff, @Wait
 
+#INIT_PATTERN
+@RobotStateStartup, @SetTorque + stiff:false + r:false, @WaitRosControlStartDelay + r:false, @SetTorque + stiff:true + r:false, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
+
 -->HCM
 $StartHCM
     START_UP --> $Simulation
         YES --> @RobotStateStartup, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
-        NO --> @RobotStateStartup, @Wait + time:1 + r:false, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
+        NO --> #INIT_PATTERN
     RUNNING --> $CheckMotors
         MOTORS_NOT_STARTED --> @RobotStateStartup, @Wait
         OVERLOAD --> #EMERGENCY_FALL
         OVERHEAT --> #EMERGENCY_FALL
         PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors
-        TURN_ON --> @TurnMotorsOn, @PlayAnimationDynup + direction:walkready + r:false, @Wait
+        TURN_ON --> #INIT_PATTERN
         OKAY --> $RecordAnimation
             RECORD_ACTIVE --> @RobotStateRecord, @Wait
             FREE --> $TeachingMode

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py

@@ -1,6 +1,7 @@
 from typing import List, Optional
 
 import numpy
+from bitbots_utils.utils import get_parameters_from_other_node_sync
 from geometry_msgs.msg import Twist
 from rclpy.action import ActionClient
 from rclpy.node import Node
@@ -32,6 +33,11 @@ def __init__(self, node: Node):
         self.visualization_active: bool = self.node.get_parameter("visualization_active").value
         self.pickup_accel_threshold: float = self.node.get_parameter("pick_up_accel_threshold").value
         self.pressure_sensors_installed: bool = self.node.get_parameter("pressure_sensors_installed").value
+        self.motor_start_delay: int = 0
+        if not self.simulation_active:  # The hardware interface is obviously not available in simulation
+            self.motor_start_delay = get_parameters_from_other_node_sync(
+                self.node, "/wolfgang_hardware_interface", ["start_delay"]
+            )["start_delay"]
 
         # Create service clients
         self.foot_zero_service = self.node.create_client(EmptySrv, "set_foot_zero")

bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py

@@ -56,7 +56,7 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
         )
 
         # Create own executor for Python part
-        multi_executor = MultiThreadedExecutor()
+        multi_executor = MultiThreadedExecutor(num_threads=10)
         multi_executor.add_node(self.node)
         self.spin_thread = threading.Thread(target=multi_executor.spin, args=(), daemon=True)
         self.spin_thread.start()

bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp

@@ -858,12 +858,23 @@ void WalkEngine::stepFromOrders(const std::vector<double>& linear_orders, double
   // No change in forward step and upward step
   tmp_diff.getOrigin()[0] = linear_orders[0];
   tmp_diff.getOrigin()[2] = linear_orders[2];
+
   // Add lateral foot offset
-  if (is_left_support_foot_) {
-    tmp_diff.getOrigin()[1] = config_.foot_distance;
-  } else {
-    tmp_diff.getOrigin()[1] = -1 * config_.foot_distance;
+  // This is normally just the foot distance, but if we turn we need to also move the foot forward/backward
+  geometry_msgs::msg::Vector3 foot_offset;
+  foot_offset.x = -sin(angular_z / 2) * config_.foot_distance;
+  foot_offset.y = cos(angular_z / 2) * config_.foot_distance;
+
+  // Invert lateral offset for right foot
+  if (!is_left_support_foot_) {
+    foot_offset.x *= -1;
+    foot_offset.y *= -1;
   }
+
+  // Add foot offset to step diff
+  tmp_diff.getOrigin()[0] += foot_offset.x;
+  tmp_diff.getOrigin()[1] += foot_offset.y;
+
   // Allow lateral step only on external foot
   //(internal foot will return to zero pose)
   if ((is_left_support_foot_ && linear_orders[1] > 0.0) || (!is_left_support_foot_ && linear_orders[1] < 0.0)) {