Revert fix for wm 2023 and remove unused code

bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py

@@ -4,6 +4,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
 from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
 
 TOPICS_TO_RECORD: list[str] = [
     "/animation",
@@ -71,9 +72,23 @@ def generate_launch_arguments():
         DeclareLaunchArgument(
             "max_image_frequency", default_value="1.0", description="Max frequency [hz] for recording images"
         ),
-        DeclareLaunchArgument(
-            "max_pointcloud_frequency", default_value="1.0", description="Max frequency [hz] for recording pointclouds"
-        ),
+    ]
+
+
+def generate_nodes():
+    return [
+        Node(
+            package="topic_tools",
+            executable="throttle",
+            output="screen",
+            name="record_rosbag_drop_images",
+            arguments=[
+                "messages",
+                "/camera/image_proc",
+                LaunchConfiguration("max_image_frequency"),
+                "/camera/image_to_record",
+            ],
+        )
     ]
 
 
@@ -123,8 +138,9 @@ def generate_action(context):
 
 def generate_launch_description():
     launch_arguments = generate_launch_arguments()
+    nodes = generate_nodes()
 
     action = OpaqueFunction(function=generate_action)
 
     # Construct LaunchDescription from parts
-    return LaunchDescription(launch_arguments + [action])
+    return LaunchDescription(launch_arguments + nodes + [action])

bitbots_misc/bitbots_bringup/launch/teamplayer.launch

@@ -10,9 +10,10 @@
     <arg name="path_planning" default="true" description="Whether the path planning should be started"/>
     <arg name="sim" default="false" description="Whether the robot is running in simulation or on real hardware" />
     <arg name="teamcom" default="false" description="Whether the team communication system should be started" />
-    <arg name="tools" default="false" description="Whether the tools should be started"/>
     <arg name="vision" default="true" description="Whether the vision system should be started" />
     <arg name="world_model" default="true" description="Whether the world model should be started"/>
+    <arg name="monitoring" default="true" description="Whether the system monitor and udp bridge should be started" />
+    <arg name="record" default="false" description="Whether the ros bag recording should be started" />
 
     <!-- load the global parameters -->
     <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch">
@@ -46,9 +47,16 @@
         <arg name="world_model" value="$(var world_model)" />
     </include>
 
-    <!-- load tools -->
-    <group if="$(var tools)">
-        <include file="$(find-pkg-share bitbots_utils)/launch/tools.launch">
+    <!-- load monitoring -->
+    <group if="$(var monitoring)">
+        <group unless="$(var sim)">
+            <include file="$(find-pkg-share bitbots_bringup)/launch/monitoring.launch" />
+        </group>
+    </group>
+
+    <!-- record rosbag -->
+    <group if="$(var record)">
+        <include file="$(find-pkg-share bitbots_bringup)/launch/rosbag_record.launch.py">
             <arg name="sim" value="$(var sim)" />
         </include>
     </group>

bitbots_misc/bitbots_bringup/package.xml

@@ -35,6 +35,7 @@
     <exec_depend>humanoid_base_footprint</exec_depend>
     <exec_depend>soccer_ipm</exec_depend>
     <exec_depend>system_monitor</exec_depend>
+    <exec_depend>topic_tools</exec_depend>
     <exec_depend>udp_bridge</exec_depend>
     <exec_depend>wolfgang_description</exec_depend>
     <exec_depend>wolfgang_moveit_config</exec_depend>

bitbots_vision/bitbots_vision/vision.py

@@ -50,16 +50,6 @@ def __init__(self) -> None:
 
         self._vision_components: List[yoeo.IVisionComponent] = []
 
-        # TODO: Fix for wm 2023
-        # This is necessary for the image recording via ros2 bag record
-        # We will republish every Xth image
-        # Previously, we used the throttle node from topic_tools to reduce the frequency of logged images to 1 Hz
-        # Unfortunately, adding this additional subscriber reduces the /camera/image_proc frequency by 30%.
-        # To avoid this additional subscription of the throttle node, we republish it here with lower frequency
-        self._images_to_log_republisher = self.create_publisher(Image, "/camera/image_to_record", 1)
-        self._images_to_log_republish_every_Xth_image: int = 10
-        self._images_to_log_current_callback_count: int = 0
-
         # Setup reconfiguration
         gen.declare_params(self)
         self.add_on_set_parameters_callback(self._dynamic_reconfigure_callback)
@@ -131,36 +121,9 @@ def _register_subscribers(self, config: Dict) -> None:
             self._sub_image,
             "ROS_img_msg_topic",
             Image,
-            callback=self._image_callback,
+            callback=self._run_vision_pipeline,
         )
 
-    def _image_callback(self, image_msg: Image) -> None:
-        """
-        This method is called by the Image-message subscriber.
-        Too old Image-Messages are dropped.
-
-        Sometimes the queue gets too large, even if the size is limited to 1.
-        That is why we drop old images manually.
-        """
-        if not self._image_is_too_old(image_msg):
-            self._run_vision_pipeline(image_msg)
-
-        # TODO: Fix for the wm 2023 see comments in init for more information
-        if self._images_to_log_current_callback_count % self._images_to_log_republish_every_Xth_image == 0:
-            self._images_to_log_republisher.publish(image_msg)
-        self._images_to_log_current_callback_count += 1
-
-    def _image_is_too_old(self, image_msg: Image) -> bool:
-        return False  # TODO: Fix for the wm 2022
-        image_age = self.get_clock().now() - rclpy.time.Time.from_msg(image_msg.header.stamp)
-        if 1.0 < image_age.nanoseconds / 1000000000 < 1000.0:
-            logger.warning(
-                f"Vision: Dropping incoming Image-Message because it is too old! ({image_age.to_msg().sec} sec)"
-            )
-            return True
-        else:
-            return False
-
     @profile
     def _run_vision_pipeline(self, image_msg: Image) -> None:
         image = self._extract_image_from_message(image_msg)

bitbots_wolfgang/wolfgang_robocup_api/launch/robocup_teamplayer.launch

@@ -21,7 +21,6 @@
         <arg name="sim" value="true" />
         <arg name="simple" value="false" />
         <arg name="teamcom" value="true" />
-        <arg name="tools" value="false" />
         <arg name="vision" value="true" />
         <arg name="world_model" value="true" />
     </include>

scripts/deploy/tasks/launch.py

@@ -150,7 +150,7 @@ def _check_tmux_session_already_running(self, connections: Group) -> GroupResult
     def _launch_teamplayer(self, connections: Group) -> GroupResult:
         print_debug("Launching teamplayer")
         # Create tmux session
-        cmd = f"tmux new-session -d -s {self._tmux_session_name} && tmux send-keys -t {self._tmux_session_name} 'ros2 launch bitbots_bringup teamplayer.launch' Enter"
+        cmd = f"tmux new-session -d -s {self._tmux_session_name} && tmux send-keys -t {self._tmux_session_name} 'ros2 launch bitbots_bringup teamplayer.launch record:=true' Enter"
 
         print_debug(f"Calling {cmd}")
         try: