Apply PR feedback

src/bitbots_misc/bitbots_robot_description/launch/load_robot_description.launch

@@ -2,7 +2,6 @@
 <launch>
     <arg name="sim" default="false"/>
 
-    <!--- TODO load the robot state publisher here -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
         <param name="publish_frequency" value="100.0"/>
         <param name="robot_description" value="$(command '$(find-exec xacro) $(find-pkg-share piplus_description)/urdf/pi_plus_22dof.urdf.xacro')"/>

src/bitbots_misc/bitbots_teleop/bitbots_teleop/joy_node.py

@@ -33,10 +33,10 @@ def __init__(self):
             self.declare_parameter(f"{controller_type}.walking.duo_turn", False)
             self.declare_parameter(f"{controller_type}.walking.gain_turn", 0.0)
             self.declare_parameter(f"{controller_type}.walking.btn_full_stop", 0)
-            self.declare_parameter(f"{controller_type}.head.gain_tilt", 0.0)
-            self.declare_parameter(f"{controller_type}.head.stick_tilt", 0)
-            self.declare_parameter(f"{controller_type}.head.gain_pan", 0.0)
-            self.declare_parameter(f"{controller_type}.head.stick_pan", 0)
+            self.declare_parameter(f"{controller_type}.head.gain_pitch", 0.0)
+            self.declare_parameter(f"{controller_type}.head.stick_pitch", 0)
+            self.declare_parameter(f"{controller_type}.head.gain_yaw", 0.0)
+            self.declare_parameter(f"{controller_type}.head.stick_yaw", 0)
             self.declare_parameter(f"{controller_type}.kick.btn_left", 0)
             self.declare_parameter(f"{controller_type}.kick.btn_right", 0)
             self.declare_parameter(f"{controller_type}.misc.btn_cheering", 0)
@@ -76,16 +76,16 @@ def __init__(self):
         }
 
         self.config["head"] = {
-            "gain_tilt": self.get_parameter(f"{selected_controller_type}.head.gain_tilt")
+            "gain_pitch": self.get_parameter(f"{selected_controller_type}.head.gain_pitch")
             .get_parameter_value()
             .double_value,
-            "stick_tilt": self.get_parameter(f"{selected_controller_type}.head.stick_tilt")
+            "stick_pitch": self.get_parameter(f"{selected_controller_type}.head.stick_pitch")
             .get_parameter_value()
             .integer_value,
-            "gain_pan": self.get_parameter(f"{selected_controller_type}.head.gain_pan")
+            "gain_yaw": self.get_parameter(f"{selected_controller_type}.head.gain_yaw")
             .get_parameter_value()
             .double_value,
-            "stick_pan": self.get_parameter(f"{selected_controller_type}.head.stick_pan")
+            "stick_yaw": self.get_parameter(f"{selected_controller_type}.head.stick_yaw")
             .get_parameter_value()
             .integer_value,
         }
@@ -217,15 +217,15 @@ def joy_cb(self, msg: Joy):
 
         # head movement with right joystick
         if self.get_parameter("head").get_parameter_value().bool_value:
-            pan_goal = float(
-                self.denormalize_joy(self.config["head"]["gain_pan"], self.config["head"]["stick_pan"], msg, -1)
+            yaw_goal = float(
+                self.denormalize_joy(self.config["head"]["gain_yaw"], self.config["head"]["stick_yaw"], msg, -1)
             )
-            tilt_goal = float(
-                self.denormalize_joy(self.config["head"]["gain_tilt"], self.config["head"]["stick_tilt"], msg, -1)
+            pitch_goal = float(
+                self.denormalize_joy(self.config["head"]["gain_pitch"], self.config["head"]["stick_pitch"], msg, -1)
             )
 
             self.head_msg.joint_names = ["head_yaw_joint", "head_pitch_joint"]
-            self.head_msg.positions = [pan_goal, tilt_goal]
+            self.head_msg.positions = [yaw_goal, pitch_goal]
             self.head_pub.publish(self.head_msg)
 
         if msg.buttons[self.config["kick"]["btn_left"]]:

src/bitbots_misc/bitbots_teleop/config/controller.yaml

@@ -12,10 +12,10 @@
         gain_turn: -0.5
         btn_full_stop: 1
       head:
-        gain_tilt: 1.5
-        stick_tilt: 4
-        gain_pan: 2.0
-        stick_pan: 3
+        gain_pitch: 1.5
+        stick_pitch: 4
+        gain_yaw: 2.0
+        stick_yaw: 3
       kick:
         btn_left: 4
         btn_right: 5
@@ -34,10 +34,10 @@
         gain_turn: 0.8
         btn_full_stop: 1
       head:
-        gain_tilt: 1.5
-        stick_tilt: 4
-        gain_pan: 2.0
-        stick_pan: 3
+        gain_pitch: 1.5
+        stick_pitch: 4
+        gain_yaw: 2.0
+        stick_yaw: 3
       kick:
         btn_left: 4
         btn_right: 5

src/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py

@@ -151,8 +151,8 @@ def __init__(self):
 
         self.head_mode_msg = HeadMode(head_mode=HeadMode.DONT_MOVE)
 
-        self.head_pan_step = 0.05
-        self.head_tilt_step = 0.05
+        self.head_yaw_step = 0.05
+        self.head_pitch_step = 0.05
 
         self.walk_kick_pub = self.create_publisher(Bool, "kick", 1)
         self.power_switch_pub = self.create_publisher(PowerSwitch, "/power_switch_state", 1)
@@ -215,8 +215,8 @@ def loop(self):
                     self.th = round(self.th, 2)
                     self.a_x = 0
                 elif key in head_bindings.keys():
-                    self.head_msg.positions[0] = self.head_yaw_pos + head_bindings[key][1] * self.head_pan_step
-                    self.head_msg.positions[1] = self.head_pitch_pos + head_bindings[key][0] * self.head_tilt_step
+                    self.head_msg.positions[0] = self.head_yaw_pos + head_bindings[key][1] * self.head_yaw_step
+                    self.head_msg.positions[1] = self.head_pitch_pos + head_bindings[key][0] * self.head_pitch_step
                     self.head_pub.publish(self.head_msg)
                 elif key == "k" or key == "K":
                     # put head back in init

src/bitbots_misc/bitbots_utils/bitbots_utils/transforms.py

@@ -85,7 +85,7 @@ def fused2quat(fused_roll: float, fused_pitch: float, fused_yaw: float, hemi: bo
     # Calculate the sine sum criterion
     crit = sth * sth + sphi * sphi
 
-    # Calculate the tilt angle alpha
+    # Calculate the pitch angle alpha
     if crit >= 1.0:
         alpha = math.pi / 2
     else:
@@ -94,7 +94,7 @@ def fused2quat(fused_roll: float, fused_pitch: float, fused_yaw: float, hemi: bo
         else:
             alpha = math.acos(-math.sqrt(1.0 - crit))
 
-    # Calculate the tilt axis angle gamma
+    # Calculate the pitch axis angle gamma
     gamma = math.atan2(sth, sphi)
 
     # Evaluate the required intermediate angles

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

@@ -50,7 +50,7 @@ def perform(self, reevaluate=False):
 
         if self.blackboard.servo_overload:
             return "OVERLOAD"
-        # We comment this out for now, but maybe add this later again
+        # TODO: We comment this out for now, but maybe add this later again
         # elif self.blackboard.servo_overheat:
         #     return "OVERHEAT"
 
@@ -173,7 +173,8 @@ def __init__(self, blackboard, dsd, parameters):
 
     def perform(self, reevaluate=False):
         if (
-            self.blackboard.battery_voltage is None
+            self.blackboard.battery_voltage
+            is None  # TODO: This becomes a problem if we never get a battery message. Create a separate state for that?
             or self.blackboard.battery_voltage >= self.blackboard.battery_voltage_threshold
         ):
             self.last_battery_high_time = self.blackboard.node.get_clock().now()

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py

@@ -41,7 +41,6 @@ def __init__(self, node: Node):
         # Create action clients and corresponding goal handles
         self.animation_action_client: ActionClient = ActionClient(self.node, PlayAnimation, "animation")
         self.animation_action_current_goal: Optional[Future] = None
-        self.dynup_action_current_goal: Optional[Future] = None
 
         # Create publishers
         self.walk_pub = self.node.create_publisher(Twist, "cmd_vel", 1)
@@ -86,7 +85,7 @@ def __init__(self, node: Node):
         self.current_joint_state: Optional[JointState] = None
         self.previous_joint_state: Optional[JointState] = None
         self.last_different_joint_state_time: Optional[Time] = None
-        self.is_power_on: bool = True
+        self.is_power_on: bool = True  # TODO: This never gets updated, but read in check_hardware.py.
 
         # Motor Parameters
         self.motor_timeout_duration: float = self.node.get_parameter("motor_timeout_duration").value
@@ -97,7 +96,7 @@ def __init__(self, node: Node):
         self.last_walking_goal_time: Optional[Time] = None
 
         # Falling
-        # Paramerters
+        # Parameters
         self.is_stand_up_active = self.node.get_parameter("stand_up_active").value
         self.falling_detection_active = self.node.get_parameter("falling_active").value
         self.in_squat: bool = False  # Needed for sequencing of the stand up motion

src/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py

@@ -36,7 +36,7 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
 
         # Load parameters from yaml file because this is a hacky cpp python hybrid node for performance reasons
         parameter_msgs: list[ParameterMsg] = get_parameters_from_ros_yaml(
-            node_name, f"{get_package_share_directory('bitbots_hcm')}/config/hcm_wolfgang.yaml", use_wildcard=True
+            node_name, f"{get_package_share_directory('bitbots_hcm')}/config/hcm_piplus.yaml", use_wildcard=True
         )
         parameters = [
             Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=use_sim_time),

src/bitbots_motion/bitbots_hcm/config/hcm_wolfgang.yaml → src/bitbots_motion/bitbots_hcm/config/hcm_piplus.yaml

@@ -23,7 +23,7 @@
       startup: "startup"
       turning_front_left: "turning_front_left"
       turning_front_right: "turning_front_right"
-      walk_ready: "walkready" # does not exist yet
+      walk_ready: "walkready"
       rise: "rise" # does not exist yet
 
     # Falling

src/bitbots_motion/bitbots_head_mover/config/head_config.yml

@@ -1,45 +1,45 @@
 move_head:
   # Max values for the head position
-  max_acceleration_pan:
+  max_acceleration_yaw:
     type: double
     default_value: 14.0
-    description: "Max acceleration for pan"
+    description: "Max acceleration for yaw"
     validation:
       bounds<>: [-1, 20]
 
-  max_acceleration_tilt:
+  max_acceleration_pitch:
     type: double
     default_value: 14.0
-    description: "Max acceleration for tilt"
+    description: "Max acceleration for pitch"
     validation:
       bounds<>: [-1, 20]
 
-  max_pan:
+  max_yaw:
     type: double_array
     default_value: [-1.23, 1.23]
     description: "Max values for the head position (in radians)"
     validation:
       fixed_size<>: 2
       element_bounds<>: [-3.14, 3.14]
 
-  max_tilt:
+  max_pitch:
     type: double_array
     default_value: [-0.6, 0.79]
     description: "Max values for the head position (in radians)"
     validation:
       fixed_size<>: 2
 
   look_at:
-    tilt_speed:
+    pitch_speed:
       type: double
       default_value: 6.0
-      description: "Tilt speed for the look at action"
+      description: "pitch speed for the look at action"
       validation:
         bounds<>: [0.0, 8.0]
-    pan_speed:
+    yaw_speed:
       type: double
       default_value: 6.0
-      description: "Pan speed for the look at action"
+      description: "yaw speed for the look at action"
       validation:
         bounds<>: [0.0, 8.0]
 
@@ -53,17 +53,17 @@ move_head:
         validation:
           gt<>: [0.0]
 
-      pan_max:
+      yaw_max:
         type: double_array
         default_value: [-30.0, 30.0]
-        description: "Maximum pan values for the search pattern (in degrees)"
+        description: "Maximum yaw values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2
 
-      tilt_max:
+      pitch_max:
         type: double_array
         default_value: [-7.0, -30.0]
-        description: "Maximum tilt values for the search pattern (in degrees)"
+        description: "Maximum pitch values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2
 
@@ -89,17 +89,17 @@ move_head:
         validation:
           gt<>: [0.0]
 
-      pan_max:
+      yaw_max:
         type: double_array
         default_value: [-70.0, 70.0]
-        description: "Maximum pan values for the search pattern (in degrees)"
+        description: "Maximum yaw values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2
 
-      tilt_max:
+      pitch_max:
         type: double_array
         default_value: [0.0, 45.0]
-        description: "Maximum tilt values for the search pattern (in degrees)"
+        description: "Maximum pitch values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2
 
@@ -125,17 +125,17 @@ move_head:
         validation:
           gt<>: [0.0]
 
-      pan_max:
+      yaw_max:
         type: double_array
         default_value: [ 0.0, 0.0 ]
-        description: "Maximum pan values for the search pattern (in degrees)"
+        description: "Maximum yaw values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2
 
-      tilt_max:
+      pitch_max:
         type: double_array
         default_value: [ -10.0, 45.0 ]
-        description: "Maximum tilt values for the search pattern (in degrees)"
+        description: "Maximum pitch values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2
 
@@ -161,17 +161,17 @@ move_head:
         validation:
           gt<>: [0.0]
 
-      pan_max:
+      yaw_max:
         type: double_array
         default_value: [ 0.0, 0.0 ]
-        description: "Maximum pan values for the search pattern (in degrees)"
+        description: "Maximum yaw values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2
 
-      tilt_max:
+      pitch_max:
         type: double_array
         default_value: [ 7.0, 7.0 ]
-        description: "Maximum tilt values for the search pattern (in degrees)"
+        description: "Maximum pitch values for the search pattern (in degrees)"
         validation:
           fixed_size<>: 2