Skip to content

Commit fa91efe

Browse files
committed
feat(head_movement): reset to using JointStates as JointCommands
1 parent 123545f commit fa91efe

File tree

1 file changed

+3
-17
lines changed

1 file changed

+3
-17
lines changed

ddlitlab2024/dataset/imports/strategies/bitbots.py

+3-17
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66
from mcap.reader import make_reader
77
from mcap.summary import Summary
88
from mcap_ros2.decoder import DecoderFactory
9-
from scipy.interpolate import CubicSpline
109

1110
from ddlitlab2024.dataset import logger
1211
from ddlitlab2024.dataset.converters.converter import Converter
@@ -105,17 +104,6 @@ def convert_to_model_data(self, file_path: Path) -> ModelData:
105104
if converter:
106105
self._create_models(converter, last_messages_by_topic, relative_msg_timestamp)
107106

108-
# timestamps = [command.stamp for command in self.model_data.joint_commands]
109-
# head_pans = [command.head_pan for command in self.model_data.joint_commands]
110-
# head_tilts = [command.head_tilt for command in self.model_data.joint_commands]
111-
112-
# head_pan_splines = CubicSpline(timestamps, head_pans, bc_type='clamped')
113-
# head_tilt_splines = CubicSpline(timestamps, head_tilts, bc_type='clamped')
114-
115-
# for idx, t in enumerate(timestamps):
116-
# self.model_data.joint_commands[idx].head_pan = head_pan_splines(t)
117-
# self.model_data.joint_commands[idx].head_tilt = head_tilt_splines(t)
118-
119107
return self.model_data
120108

121109
def _initial_conversion(self, data: InputData):
@@ -135,11 +123,9 @@ def _create_models(self, converter: Converter, data: InputData, relative_timesta
135123
model_data = converter.convert_to_model(data, relative_timestamp, self.model_data.recording)
136124

137125
# @TODO: find a better way to handle interpolation of head movements
138-
# for idx, command in enumerate(model_data.joint_commands):
139-
# command.head_pan = model_data.joint_commands[idx].head_pan
140-
# command.head_tilt = model_data.joint_commands[idx].head_tilt
141-
142-
126+
for idx, command in enumerate(model_data.joint_commands):
127+
command.head_pan = model_data.joint_commands[idx].head_pan
128+
command.head_tilt = model_data.joint_commands[idx].head_tilt
143129

144130
self.model_data = self.model_data.merge(model_data)
145131

0 commit comments

Comments
 (0)