feat(head_movement): reset to using JointStates as JointCommands

Author: texhnolyze

ddlitlab2024/dataset/imports/strategies/bitbots.py

@@ -6,7 +6,6 @@
 from mcap.reader import make_reader
 from mcap.summary import Summary
 from mcap_ros2.decoder import DecoderFactory
-from scipy.interpolate import CubicSpline
 
 from ddlitlab2024.dataset import logger
 from ddlitlab2024.dataset.converters.converter import Converter
@@ -105,17 +104,6 @@ def convert_to_model_data(self, file_path: Path) -> ModelData:
                         if converter:
                             self._create_models(converter, last_messages_by_topic, relative_msg_timestamp)
 
-            # timestamps = [command.stamp for command in self.model_data.joint_commands]
-            # head_pans = [command.head_pan for command in self.model_data.joint_commands]
-            # head_tilts = [command.head_tilt for command in self.model_data.joint_commands]
-
-            # head_pan_splines = CubicSpline(timestamps, head_pans, bc_type='clamped')
-            # head_tilt_splines = CubicSpline(timestamps, head_tilts, bc_type='clamped')
-
-            # for idx, t in enumerate(timestamps):
-            #     self.model_data.joint_commands[idx].head_pan = head_pan_splines(t)
-            #     self.model_data.joint_commands[idx].head_tilt = head_tilt_splines(t)
-
             return self.model_data
 
     def _initial_conversion(self, data: InputData):
@@ -135,11 +123,9 @@ def _create_models(self, converter: Converter, data: InputData, relative_timesta
         model_data = converter.convert_to_model(data, relative_timestamp, self.model_data.recording)
 
         # @TODO: find a better way to handle interpolation of head movements
-        # for idx, command in enumerate(model_data.joint_commands):
-        #     command.head_pan = model_data.joint_commands[idx].head_pan
-        #     command.head_tilt = model_data.joint_commands[idx].head_tilt
-
-
+        for idx, command in enumerate(model_data.joint_commands):
+            command.head_pan = model_data.joint_commands[idx].head_pan
+            command.head_tilt = model_data.joint_commands[idx].head_tilt
 
         self.model_data = self.model_data.merge(model_data)