Cleanup motion

bit-bots · Aug 11, 2023 · 33c8f82 · 33c8f82
1 parent d72f578
commit 33c8f82
Show file tree

Hide file tree

Showing 26 changed files with 1,369 additions and 421 deletions.
diff --git a/bitbots_animation_server/launch/test.launch b/bitbots_animation_server/launch/test.launch
@@ -7,9 +7,9 @@
     <include file="$(find-pkg-share bitbots_ros_control)/launch/ros_control.launch" />
   </group>
 
-  <group>
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch" />
-  </group>
-    <node pkg="bitbots_animation_server" exec="animation_node" output="screen" launch-prefix="$(var taskset)"/>
-    <node name="animation_hcm_bridge" pkg="bitbots_animation_server" exec="animation_hcm_bridge.py"  output="screen" launch-prefix="$(var taskset)"/>
+  <include file="$(find-pkg-share bitbots_bringup)/launch/base.launch" />
+  <include file="$(find-pkg-share bitbots_utils)/launch/motion_base.launch" />
+
+  <node pkg="bitbots_animation_server" exec="animation_node" output="screen" launch-prefix="$(var taskset)"/>
+  <node name="animation_hcm_bridge" pkg="bitbots_animation_server" exec="animation_hcm_bridge.py"  output="screen" launch-prefix="$(var taskset)"/>
 </launch>
diff --git a/bitbots_dynamic_kick/launch/test.launch b/bitbots_dynamic_kick/launch/test.launch
@@ -6,29 +6,45 @@
     <let if="$(env IS_ROBOT false)" name="taskset" value="taskset -c 3,4"/>
     <let unless="$(env IS_ROBOT false)" name="taskset" value=""/>
 
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch" >
+    <include file="$(find-pkg-share bitbots_bringup)/launch/base.launch" >
         <arg name="sim" value="$(var sim)"/>
     </include>
 
+    <include file="$(find-pkg-share bitbots_utils)/launch/motion_base.launch" >
+        <arg name="sim" value="$(var sim)"/>
+    </include>
+
+    <!-- launch the base footprint -->
+    <node pkg="humanoid_base_footprint" exec="base_footprint" output="screen">
+      <param name="support_state_topics" value="[walk_support_state, dynamic_kick_support_state]"/>
+      <param name="use_sim_time" value="$(var sim)"/>
+    </node>
+
+    <!-- launch the odometry -->
+    <include file="$(find-pkg-share bitbots_odometry)/launch/odometry.launch">
+      <arg name="sim" value="$(var sim)"/>
+    </include>
+
+
     <group unless="$(var sim)">
         <group unless="$(var viz)">
             <include file="$(find-pkg-share bitbots_ros_control)/launch/ros_control.launch" />
-            <node pkg="bitbots_dynamic_kick" exec="KickNode" output="screen" launch-prefix="$(var taskset)"> -->
+            <node pkg="bitbots_dynamic_kick" exec="KickNode" output="screen" launch-prefix="$(var taskset)">
                 <remap from="/kick_motor_goals" to="/DynamixelController/command"/>
                 <param from="$(find-pkg-share bitbots_dynamic_kick)/config/kick_config.yaml" />
             </node>
         </group>
     </group>
 
     <group if="$(var sim)">
-        <node pkg="bitbots_dynamic_kick" exec="KickNode" output="screen" launch-prefix="$(var taskset)"> -->
+        <node pkg="bitbots_dynamic_kick" exec="KickNode" output="screen" launch-prefix="$(var taskset)">
             <remap from="/kick_motor_goals" to="/DynamixelController/command"/>
             <param from="$(find-pkg-share bitbots_dynamic_kick)/config/kick_sim_config.yaml" />
         </node>
     </group>
 
     <group if="$(var viz)">
-        <node pkg="bitbots_dynamic_kick" exec="KickNode" output="screen" launch-prefix="$(var taskset)"> -->
+        <node pkg="bitbots_dynamic_kick" exec="KickNode" output="screen" launch-prefix="$(var taskset)">
             <param from="$(find-pkg-share bitbots_dynamic_kick)/config/kick_sim_config.yaml" />
         </node>
         <node pkg="bitbots_utils" exec="motor_goals_viz_helper.py" output="screen" args="--kick" />

diff --git a/bitbots_dynamic_kick/launch/viz.launch b/bitbots_dynamic_kick/launch/viz.launch
@@ -1,8 +1,21 @@
 <launch>
-  <include file="$(find-pkg-share bitbots_utils)/launch/base.launch" />
+  <include file="$(find-pkg-share bitbots_bringup)/launch/base.launch" />
+
+  <include file="$(find-pkg-share bitbots_utils)/launch/motion_base.launch" />
+
+  <!-- launch the base footprint -->
+  <node pkg="humanoid_base_footprint" exec="base_footprint" output="screen">
+    <param name="support_state_topics" value="[walk_support_state, dynamic_kick_support_state]"/>
+  </node>
+
+  <!-- launch the odometry -->
+  <include file="$(find-pkg-share bitbots_odometry)/launch/odometry.launch" />
+
   <node name="rviz2" pkg="rviz2" exec="rviz2" output="screen"
     args="-d $(find-pkg-share wolfgang_description)/config/wolfgang.rviz" />
+
   <node name="motor_goals_viz_helper" pkg="bitbots_utils" exec="motor_goals_viz_helper.py" args="--kick" />
+
   <node pkg="bitbots_dynamic_kick" exec="KickNode" output="screen">
     <param from="$(find-pkg-share bitbots_dynamic_kick)/config/kick_config.yaml" />
   </node>

diff --git a/bitbots_dynamic_kick/test/rostests/test_kick_runs.launch b/bitbots_dynamic_kick/test/rostests/test_kick_runs.launch
diff --git a/bitbots_dynamic_kick/test/rostests/test_kick_runs.py b/bitbots_dynamic_kick/test/rostests/test_kick_runs.py
diff --git a/bitbots_dynup/launch/test.launch b/bitbots_dynup/launch/test.launch
@@ -8,12 +8,14 @@
   <let if="$(env IS_ROBOT false)" name="taskset" value="taskset -c 3,4"/>
   <let unless="$(env IS_ROBOT false)" name="taskset" value=""/>
 
-  <group>
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch" >
+  <include file="$(find-pkg-share bitbots_bringup)/launch/base.launch" >
+    <arg name="sim" value="$(var sim)"/>
+    <arg name="robot_type" value="$(var robot_type)"/>
+  </include>
+
+  <include file="$(find-pkg-share bitbots_utils)/launch/motion_base.launch" >
       <arg name="sim" value="$(var sim)"/>
-      <arg name="robot_type" value="$(var robot_type)"/>
-    </include>
-  </group>
+  </include>
 
   <group unless="$(var sim)">
     <group unless="$(var viz)">

diff --git a/bitbots_dynup/test/rostests/test_dynup_runs.launch b/bitbots_dynup/test/rostests/test_dynup_runs.launch
diff --git a/bitbots_dynup/test/rostests/test_dynup_runs.py b/bitbots_dynup/test/rostests/test_dynup_runs.py
diff --git a/bitbots_hcm/bitbots_hcm/fall_classifier.py b/bitbots_hcm/bitbots_hcm/fall_classifier.py
@@ -7,6 +7,7 @@
 from geometry_msgs.msg import Point
 import math
 import numpy as np
+from ros2_numpy import numpify
 
 class FallClassifier:
 
@@ -52,7 +53,7 @@ def smooth_classify(self, imu, joint_state, cop_l, cop_r):
         return result
 
 
-def get_data_from_msgs(imu_msg, joint_state_msg, cop_l_msg, cop_r_msg, imu_raw=True, imu_orient=True, joint_states=True,
+def get_data_from_msgs(imu_msg: Imu, joint_state_msg: JointState, cop_l_msg, cop_r_msg, imu_raw=True, imu_orient=True, joint_states=True,
                        imu_fused=True, cop=True):
     data = []
     if imu_raw:
@@ -63,7 +64,7 @@ def get_data_from_msgs(imu_msg, joint_state_msg, cop_l_msg, cop_r_msg, imu_raw=T
         data.append(imu_msg.angular_velocity.y)
         data.append(imu_msg.angular_velocity.z)
     if imu_orient:
-        euler = transforms3d.euler.quat2euler([imu_msg.orientation.w, imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z])
+        euler = transforms3d.euler.quat2euler(numpify(imu_msg.orientation)[[3, 0, 1, 2]])
         data.append(euler[0])
         data.append(euler[1])
         data.append(euler[2])

diff --git a/bitbots_hcm/bitbots_hcm/humanoid_control_module.py b/bitbots_hcm/bitbots_hcm/humanoid_control_module.py
@@ -1,37 +1,34 @@
 #!/usr/bin/env python3
-import numpy
+
+import os
+import threading
 import rclpy
-from rclpy.duration import Duration
-from rclpy.node import Node
+from ament_index_python import get_package_share_directory
+from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard
+from bitbots_msgs.action import Dynup
+from bitbots_msgs.msg import FootPressure
+from bitbots_utils.utils import get_parameters_from_ros_yaml
+from builtin_interfaces.msg import Time as TimeMsg
+from diagnostic_msgs.msg import DiagnosticArray
+from dynamic_stack_decider.dsd import DSD
+from geometry_msgs.msg import PointStamped
+from humanoid_league_msgs.action import PlayAnimation
+from humanoid_league_msgs.msg import RobotControlState, Audio
+from humanoid_league_speaker.speaker import speak
+from rcl_interfaces.msg import Parameter as ParameterMsg
 from rclpy.action import ActionClient
+from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
 from rclpy.parameter import Parameter
+from rclpy.serialization import deserialize_message
 from rclpy.time import Time
-
-from geometry_msgs.msg import PointStamped
-
-from std_msgs.msg import Bool
+from ros2_numpy import numpify
 from sensor_msgs.msg import Imu, JointState
-from diagnostic_msgs.msg import DiagnosticArray
+from std_msgs.msg import Bool
 
-from humanoid_league_msgs.msg import RobotControlState, Audio
-from humanoid_league_msgs.action import PlayAnimation
-from humanoid_league_speaker.speaker import speak
-from bitbots_msgs.msg import FootPressure
-from bitbots_msgs.action import Dynup
-
-from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard
-from dynamic_stack_decider.dsd import DSD
-import os
-import threading
-from bitbots_utils.utils import get_parameters_from_ros_yaml
-from ament_index_python import get_package_share_directory
-from rcl_interfaces.msg import Parameter as ParameterMsg
-from rclpy.serialization import deserialize_message
-from builtin_interfaces.msg import Time as TimeMsg
 
 class HardwareControlManager:
-
     def __init__(self, use_sim_time, simulation_active, visualization_active):
         rclpy.init(args=None)
         node_name = "hcm_py"
@@ -56,11 +53,9 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
         self.spin_thread = threading.Thread(target=multi_executor.spin, args=(), daemon=True)
         self.spin_thread.start()
 
-        self.blackboard = None
-
         # --- Initialize Node ---
         # Otherwise messages will get lost, bc the init is not finished
-        self.node.get_clock().sleep_for(Duration(seconds=0.1))
+        self.node.get_clock().sleep_for(Duration(seconds=0.5))
         self.node.get_logger().debug("Starting hcm")
         self.simulation_active = self.node.get_parameter("simulation_active")
         # dsd
@@ -171,33 +166,28 @@ def set_cop(self, cop_msg_serialized, left):
             self.blackboard.cop_r_msg = deserialize_message(cop_msg_serialized, PointStamped)
 
     def set_pressure_left(self, pressure_msg_serialized):
-        msg = deserialize_message(pressure_msg_serialized, FootPressure)
+        msg: FootPressure = deserialize_message(pressure_msg_serialized, FootPressure)
         self.blackboard.pressures[0] = msg.left_front
         self.blackboard.pressures[1] = msg.left_back
         self.blackboard.pressures[2] = msg.right_front
         self.blackboard.pressures[3] = msg.right_back
 
     def set_pressure_right(self, pressure_msg_serialized):
-        msg = deserialize_message(pressure_msg_serialized, FootPressure)
+        msg: FootPressure = deserialize_message(pressure_msg_serialized, FootPressure)
         self.blackboard.pressures[4] = msg.left_front
         self.blackboard.pressures[5] = msg.left_back
         self.blackboard.pressures[6] = msg.right_front
         self.blackboard.pressures[7] = msg.right_back
 
     def set_imu(self, imu_msg_serialized):
-        msg = deserialize_message(imu_msg_serialized, Imu)
-
-        self.blackboard.accel = numpy.array(
-            [msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z])
-        self.blackboard.gyro = numpy.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])
-        self.blackboard.quaternion = numpy.array(
-            ([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
-
-        self.blackboard.smooth_gyro = numpy.multiply(self.blackboard.smooth_gyro, 0.95) + numpy.multiply(
-            self.blackboard.gyro, 0.05)
-        self.blackboard.smooth_accel = numpy.multiply(self.blackboard.smooth_accel, 0.99) + numpy.multiply(
-            self.blackboard.accel, 0.01)
-        self.blackboard.not_much_smoothed_gyro = numpy.multiply(self.blackboard.not_much_smoothed_gyro,
-                                                                0.5) + numpy.multiply(self.blackboard.gyro, 0.5)
+        msg: Imu = deserialize_message(imu_msg_serialized, Imu)
+
+        self.blackboard.accel = numpify(msg.linear_acceleration)
+        self.blackboard.gyro = numpify(msg.angular_velocity)
+        self.blackboard.quaternion = numpify(msg.orientation)
+
+        self.blackboard.smooth_gyro = 0.95 * self.blackboard.smooth_gyro + 0.05 * self.blackboard.gyro
+        self.blackboard.smooth_accel = 0.99 * self.blackboard.smooth_accel + 0.01 * self.blackboard.accel
+        self.blackboard.not_much_smoothed_gyro = 0.5 * self.blackboard.not_much_smoothed_gyro + 0.5 * self.blackboard.gyro
 
         self.blackboard.imu_msg = msg
diff --git a/bitbots_hcm/launch/test.launch b/bitbots_hcm/launch/test.launch
@@ -1,7 +1,11 @@
 <launch>
     <arg name="sim" default="false" description="Disables checks for hardware, since we are in simulation."/>
 
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch">
+    <include file="$(find-pkg-share bitbots_bringup)/launch/base.launch">
+        <arg name="sim" value="$(var sim)"/>
+    </include>
+
+    <include file="$(find-pkg-share bitbots_utils)/launch/motion_base.launch" >
         <arg name="sim" value="$(var sim)"/>
     </include>