@@ -39,7 +39,7 @@ def _is_ros2_time(obj: Any) -> bool:
3939 msg_name = obj .__msg_name__
4040 return (
4141 msg_name in ('builtin_interfaces/msg/Time' , 'builtin_interfaces/Time' ) and
42- hasattr (obj , 'sec' ) and
42+ hasattr (obj , 'sec' ) and
4343 hasattr (obj , 'nanosec' )
4444 )
4545
@@ -52,17 +52,17 @@ def _is_ros2_duration(obj: Any) -> bool:
5252 msg_name = obj .__msg_name__
5353 return (
5454 msg_name in ('builtin_interfaces/msg/Duration' , 'builtin_interfaces/Duration' ) and
55- hasattr (obj , 'sec' ) and
55+ hasattr (obj , 'sec' ) and
5656 hasattr (obj , 'nanosec' )
5757 )
5858
5959
6060def ros1_time_to_ros2 (time : Ros1Time ) -> Ros2Time :
6161 """Convert ROS1 Time to ROS2 Time.
62-
62+
6363 Args:
6464 time: ROS1 Time with secs and nsecs attributes.
65-
65+
6666 Returns:
6767 ROS2 Time with sec and nanosec attributes.
6868 """
@@ -71,10 +71,10 @@ def ros1_time_to_ros2(time: Ros1Time) -> Ros2Time:
7171
7272def ros2_time_to_ros1 (time : Any ) -> Ros1Time :
7373 """Convert ROS2 Time to ROS1 Time.
74-
74+
7575 Args:
7676 time: ROS2 Time with sec and nanosec attributes.
77-
77+
7878 Returns:
7979 ROS1 Time with secs and nsecs attributes.
8080 """
@@ -83,10 +83,10 @@ def ros2_time_to_ros1(time: Any) -> Ros1Time:
8383
8484def ros1_duration_to_ros2 (duration : Ros1Duration ) -> Ros2Duration :
8585 """Convert ROS1 Duration to ROS2 Duration.
86-
86+
8787 Args:
8888 duration: ROS1 Duration with secs and nsecs attributes.
89-
89+
9090 Returns:
9191 ROS2 Duration with sec and nanosec attributes.
9292 """
@@ -95,10 +95,10 @@ def ros1_duration_to_ros2(duration: Ros1Duration) -> Ros2Duration:
9595
9696def ros2_duration_to_ros1 (duration : Any ) -> Ros1Duration :
9797 """Convert ROS2 Duration to ROS1 Duration.
98-
98+
9999 Args:
100100 duration: ROS2 Duration with sec and nanosec attributes.
101-
101+
102102 Returns:
103103 ROS1 Duration with secs and nsecs attributes.
104104 """
@@ -107,14 +107,14 @@ def ros2_duration_to_ros1(duration: Any) -> Ros1Duration:
107107
108108def translate_ros1_to_ros2 (message : T ) -> T :
109109 """Recursively translate a ROS1 message to ROS2 format.
110-
110+
111111 This function walks through the message object and converts:
112112 - ROS1 Time(secs, nsecs) -> ROS2 Time(sec, nanosec)
113113 - ROS1 Duration(secs, nsecs) -> ROS2 Duration(sec, nanosec)
114-
114+
115115 Args:
116116 message: A ROS1 message object (dataclass).
117-
117+
118118 Returns:
119119 A new message object with time/duration fields converted to ROS2 format.
120120 """
@@ -126,40 +126,40 @@ def _translate_value_ros1_to_ros2(value: Any) -> Any:
126126 # Handle ROS1 Time
127127 if _is_ros1_time (value ):
128128 return ros1_time_to_ros2 (value )
129-
129+
130130 # Handle ROS1 Duration
131131 if _is_ros1_duration (value ):
132132 return ros1_duration_to_ros2 (value )
133-
133+
134134 # Handle lists/arrays
135135 if isinstance (value , list ):
136136 return [_translate_value_ros1_to_ros2 (item ) for item in value ]
137-
137+
138138 # Handle dataclass (nested message)
139139 if is_dataclass (value ) and not isinstance (value , type ):
140140 # Get all field values and translate them
141141 new_values = {}
142142 for field in fields (value ):
143143 field_value = getattr (value , field .name )
144144 new_values [field .name ] = _translate_value_ros1_to_ros2 (field_value )
145-
145+
146146 # Create new instance with translated values
147147 return replace (value , ** new_values )
148-
148+
149149 # Return other values unchanged
150150 return value
151151
152152
153153def translate_ros2_to_ros1 (message : T ) -> T :
154154 """Recursively translate a ROS2 message to ROS1 format.
155-
155+
156156 This function walks through the message object and converts:
157157 - ROS2 Time(sec, nanosec) -> ROS1 Time(secs, nsecs)
158158 - ROS2 Duration(sec, nanosec) -> ROS1 Duration(secs, nsecs)
159-
159+
160160 Args:
161161 message: A ROS2 message object (dataclass).
162-
162+
163163 Returns:
164164 A new message object with time/duration fields converted to ROS1 format.
165165 """
@@ -171,43 +171,43 @@ def _translate_value_ros2_to_ros1(value: Any) -> Any:
171171 # Handle ROS2 Time
172172 if _is_ros2_time (value ):
173173 return ros2_time_to_ros1 (value )
174-
174+
175175 # Handle ROS2 Duration
176176 if _is_ros2_duration (value ):
177177 return ros2_duration_to_ros1 (value )
178-
178+
179179 # Handle lists/arrays
180180 if isinstance (value , list ):
181181 return [_translate_value_ros2_to_ros1 (item ) for item in value ]
182-
182+
183183 # Handle dataclass (nested message)
184184 if is_dataclass (value ) and not isinstance (value , type ):
185185 # Get all field values and translate them
186186 new_values = {}
187187 for field in fields (value ):
188188 field_value = getattr (value , field .name )
189189 new_values [field .name ] = _translate_value_ros2_to_ros1 (field_value )
190-
190+
191191 # Create new instance with translated values
192192 return replace (value , ** new_values )
193-
193+
194194 # Return other values unchanged
195195 return value
196196
197197
198198def translate_schema_ros1_to_ros2 (msg_name : str , schema_text : str ) -> SchemaText :
199199 """Translate a ROS1 message schema to ROS2 format.
200-
200+
201201 This function transforms ROS1 message definition text to ROS2 format:
202202 - time -> builtin_interfaces/Time (with sec/nanosec fields)
203203 - duration -> builtin_interfaces/Duration (with sec/nanosec fields)
204204 - char remains as-is (handled by the serializer as uint8 vs string)
205205 - Message name: package/Message -> package/msg/Message
206-
206+
207207 Args:
208208 msg_name: ROS1 message type name (e.g., "std_msgs/Header").
209209 schema_text: ROS1 message definition text.
210-
210+
211211 Returns:
212212 SchemaText with ROS2-compatible name and definition text.
213213 """
@@ -221,48 +221,48 @@ def translate_schema_ros1_to_ros2(msg_name: str, schema_text: str) -> SchemaText
221221 result_lines = []
222222 has_time = False
223223 has_duration = False
224-
224+
225225 for line in lines :
226226 stripped = line .strip ()
227-
227+
228228 # Skip empty lines and comments
229229 if not stripped or stripped .startswith ('#' ):
230230 result_lines .append (line )
231231 continue
232-
232+
233233 # Check for MSG: delimiter (sub-message definition)
234234 if stripped .startswith ('MSG:' ):
235235 result_lines .append (line )
236236 continue
237-
237+
238238 # Check for separator
239239 if stripped == '=' * 80 :
240240 result_lines .append (line )
241241 continue
242-
242+
243243 # Parse field: TYPE NAME [=VALUE]
244244 # Handle constants: TYPE NAME=VALUE
245245 if '=' in stripped and not stripped .startswith ('=' ):
246246 # This is a constant definition, keep as-is
247247 result_lines .append (line )
248248 continue
249-
249+
250250 # Match field definition
251251 match = re .match (r'^(\S+)\s+(\S+)(.*)$' , stripped )
252252 if not match :
253253 result_lines .append (line )
254254 continue
255-
255+
256256 field_type , field_name , rest = match .groups ()
257-
257+
258258 # Handle array notation
259259 array_suffix = ''
260260 base_type = field_type
261261 if '[' in field_type :
262262 bracket_pos = field_type .index ('[' )
263263 base_type = field_type [:bracket_pos ]
264264 array_suffix = field_type [bracket_pos :]
265-
265+
266266 # Translate time and duration
267267 if base_type == 'time' :
268268 has_time = True
@@ -274,45 +274,45 @@ def translate_schema_ros1_to_ros2(msg_name: str, schema_text: str) -> SchemaText
274274 result_lines .append (f'{ new_type } { field_name } { rest } ' )
275275 else :
276276 result_lines .append (line )
277-
277+
278278 result = '\n ' .join (result_lines )
279-
279+
280280 # Add builtin_interfaces sub-schemas if needed
281281 time_schema = """================================================================================
282282MSG: builtin_interfaces/Time
283283int32 sec
284284uint32 nanosec"""
285-
285+
286286 duration_schema = """================================================================================
287287MSG: builtin_interfaces/Duration
288288int32 sec
289289uint32 nanosec"""
290-
290+
291291 # Check if the schema already contains these definitions
292292 if has_time and 'MSG: builtin_interfaces/Time' not in result :
293293 result += '\n ' + time_schema
294-
294+
295295 if has_duration and 'MSG: builtin_interfaces/Duration' not in result :
296296 result += '\n ' + duration_schema
297-
297+
298298 return SchemaText (name = ros2_msg_name , text = result )
299299
300300
301301def translate_schema_ros2_to_ros1 (msg_name : str , schema_text : str ) -> SchemaText :
302302 """Translate a ROS2 message schema to ROS1 format.
303-
303+
304304 This function transforms ROS2 message definition text to ROS1 format:
305305 - builtin_interfaces/Time -> time (primitive)
306306 - builtin_interfaces/Duration -> duration (primitive)
307307 - builtin_interfaces/msg/Time -> time (primitive)
308308 - builtin_interfaces/msg/Duration -> duration (primitive)
309309 - Message name: package/msg/Message -> package/Message
310310 - char remains as-is (handled by the serializer)
311-
311+
312312 Args:
313313 msg_name: ROS2 message type name (e.g., "std_msgs/msg/Header").
314314 schema_text: ROS2 message definition text.
315-
315+
316316 Returns:
317317 SchemaText with ROS1-compatible name and definition text.
318318 """
@@ -324,7 +324,7 @@ def translate_schema_ros2_to_ros1(msg_name: str, schema_text: str) -> SchemaText
324324
325325 for line in lines :
326326 stripped = line .strip ()
327-
327+
328328 # Check for MSG: delimiter (sub-message definition)
329329 if stripped .startswith ('MSG:' ):
330330 sub_msg_name = stripped [4 :].strip ()
@@ -340,45 +340,45 @@ def translate_schema_ros2_to_ros1(msg_name: str, schema_text: str) -> SchemaText
340340 skip_sub_schema = False
341341 result_lines .append (line )
342342 continue
343-
343+
344344 # Check for separator - might start a new sub-schema
345345 if stripped == '=' * 80 :
346346 if skip_sub_schema :
347347 # This separator ends the skipped sub-schema
348348 skip_sub_schema = False
349349 result_lines .append (line )
350350 continue
351-
351+
352352 # Skip lines that are part of a builtin_interfaces sub-schema
353353 if skip_sub_schema :
354354 continue
355-
355+
356356 # Skip empty lines and comments
357357 if not stripped or stripped .startswith ('#' ):
358358 result_lines .append (line )
359359 continue
360-
360+
361361 # Handle constants: TYPE NAME=VALUE
362362 if '=' in stripped and not stripped .startswith ('=' ):
363363 result_lines .append (line )
364364 continue
365-
365+
366366 # Match field definition
367367 match = re .match (r'^(\S+)\s+(\S+)(.*)$' , stripped )
368368 if not match :
369369 result_lines .append (line )
370370 continue
371-
371+
372372 field_type , field_name , rest = match .groups ()
373-
373+
374374 # Handle array notation
375375 array_suffix = ''
376376 base_type = field_type
377377 if '[' in field_type :
378378 bracket_pos = field_type .index ('[' )
379379 base_type = field_type [:bracket_pos ]
380380 array_suffix = field_type [bracket_pos :]
381-
381+
382382 # Translate builtin_interfaces/Time and Duration to primitives
383383 if base_type in ('builtin_interfaces/Time' , 'builtin_interfaces/msg/Time' ):
384384 new_type = f'time{ array_suffix } '
@@ -394,11 +394,11 @@ def translate_schema_ros2_to_ros1(msg_name: str, schema_text: str) -> SchemaText
394394 result_lines .append (f'{ new_type } { field_name } { rest } ' )
395395 else :
396396 result_lines .append (line )
397-
397+
398398 # Clean up any trailing empty separators
399399 while result_lines and result_lines [- 1 ].strip () == '=' * 80 :
400400 result_lines .pop ()
401-
401+
402402 return SchemaText (name = ros1_msg_name , text = '\n ' .join (result_lines ))
403403
404404
0 commit comments