Add proper recording file controls, correct teleportation behavior

Author: Cornelius Krupp

src/bitbots_misc/bitbots_auto_test/CMakeLists.txt

@@ -4,6 +4,7 @@ project(bitbots_auto_test)
 find_package(ament_cmake REQUIRED)
 find_package(bitbots_docs REQUIRED)
 find_package(bitbots_msgs REQUIRED)
+find_package(game_controller_hsl REQUIRED)
 find_package(rclpy REQUIRED)
 
 enable_bitbots_docs()

src/bitbots_misc/bitbots_auto_test/bitbots_auto_test/auto_test.py

@@ -15,11 +15,10 @@
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.qos import DurabilityPolicy, QoSProfile
-from std_srvs.srv import Empty
-
+from tf_transformations import euler_from_quaternion, quaternion_from_euler
 
 from bitbots_auto_test.monitoring_node import Monitoring
-from bitbots_msgs.srv import SetObjectPose, SetObjectPosition
+from bitbots_msgs.srv import SetObjectPose, SetObjectPosition, SimulatorRecord
 
 
 class TestResult:
@@ -58,7 +57,7 @@ def start_recording(self):
         self.start_time = self.handle.get_clock().now()
 
     def judge(self) -> float:
-        pass
+        raise NotImplementedError(f"{self.__class__.__name__}.judge")
 
     def is_over(self) -> TestResult | None:
         if not self.is_pose_in(self.handle.robot_pose, self.FIELD_BOUNDS):
@@ -82,23 +81,33 @@ def score(self):
         else:
             return self.judge()
 
-    def set_robot(self, x: float, y: float):
+    def set_robot(self, x: float, y: float, angle: float = 0.0):
         request = SetObjectPose.Request()
         request.object_name = "amy"
         request.pose.position.x = x
         request.pose.position.y = y
         request.pose.position.z = 0.42
-        request.pose.orientation.x = 0.0
-        request.pose.orientation.y = 0.0
-        request.pose.orientation.z = 0.0
-        request.pose.orientation.w = 0.0
+
+        old_orientation = self.handle.robot_pose.orientation
+        old_quat = [old_orientation.x, old_orientation.y, old_orientation.z, old_orientation.w]
+        try:
+            old_roll, old_pitch, _old_yaw = euler_from_quaternion(old_quat)
+        except ValueError:
+            old_roll, old_pitch, _old_yaw = 0.0, 0.0, 0.0
+
+        x_q, y_q, z_q, w_q = quaternion_from_euler(old_roll, old_pitch, angle)
+        request.pose.orientation.x = x_q
+        request.pose.orientation.y = y_q
+        request.pose.orientation.z = z_q
+        request.pose.orientation.w = w_q
+
         self.handle.set_robot_pose_service.call(request)
         if not self.handle.get_parameter("fake_localization").value:
             rf = ResetFilter.Request()
             rf.init_mode = ResetFilter.Request.POSE
             rf.x = x
             rf.y = y
-            rf.angle = 0.0
+            rf.angle = angle
             self.handle.reset_localization.call(rf)
 
     def set_ball(self, x: float, y: float):
@@ -222,7 +231,7 @@ def __init__(self, monitoring_node: Monitoring):
 
         self.set_robot_pose_service = self.create_client(SetObjectPose, "set_robot_pose")
         self.set_ball_pos_service = self.create_client(SetObjectPosition, "set_ball_position")
-        self.recording_service = self.create_client(Empty, "simulator_record")
+        self.recording_service = self.create_client(SimulatorRecord, "simulator_record")
         self.create_subscription(ModelStates, "/model_states", qos_profile=10, callback=self.model_states_callback)
         self.ball_pose = Pose()
         self.ball_twist = Twist()
@@ -274,15 +283,17 @@ def setup_sequence(self):
         gs_msg.header.stamp = self.get_clock().now().to_msg()
         gs_msg.kicking_team = TEAM_ID
         gs_msg.main_state = 2  # 2=SET, force robot to stand still
+        gs_msg.stopped = True
         self.gamestate_publisher.publish(gs_msg)
 
-        self.get_clock().sleep_for(Duration(seconds=2))  # Let robot react & simulation settle
+        self.get_clock().sleep_for(Duration(seconds=3))  # Let robot react & simulation settle
         self.current_test.setup()  # Teleport robot
 
-        self.get_clock().sleep_for(Duration(seconds=2))  # Let simulation settle
+        self.get_clock().sleep_for(Duration(seconds=3))  # Let simulation settle
 
         gs_msg.header.stamp = self.get_clock().now().to_msg()
         gs_msg.main_state = 3  # 3=PLAYING, make robot play ball
+        gs_msg.stopped = False
         self.gamestate_publisher.publish(gs_msg)
         self.current_test.start_recording()
         self.monitoring_node.write_event(
@@ -313,12 +324,13 @@ def finished(self):
         )
 
     def loop(self):
-        start = self.get_clock().now()
-        while self.get_clock().now() - start < Duration(seconds=1):
-            pass
+        for service in [self.set_robot_pose_service, self.set_ball_pos_service, self.recording_service]:
+            service.wait_for_service()
+        self.get_clock().sleep_for(Duration(seconds=1))
         self.get_logger().info("Starting main loop")
-        request = Empty.Request()
-        self.recording_service.call(request) # start
+        request = SimulatorRecord.Request()
+        request.start = True
+        self.recording_service.call(request)  # start
         self.next_test()
         self.setup_sequence()
         while True:
@@ -331,7 +343,9 @@ def loop(self):
                     self.get_logger().info("Done with all tests")
                     break
                 self.setup_sequence()
-        self.recording_service.call(request) # stop
+        stop_request = SimulatorRecord.Request()
+        stop_request.stop = True
+        self.recording_service.call(stop_request)  # stop
 
 
 def main():

src/bitbots_misc/bitbots_auto_test/package.xml

@@ -19,7 +19,7 @@
   <depend>rclpy</depend>
   <depend>std_msgs</depend>
   <depend>tf_transformations</depend>
-  <depend>game_controller_hl</depend>
+  <depend>game_controller_hsl</depend>
 
   <test_depend>python3-pytest</test_depend>
 

src/bitbots_msgs/CMakeLists.txt

@@ -56,6 +56,7 @@ rosidl_generate_interfaces(
   "srv/SetObjectPosition.srv"
   "srv/SetTeachingMode.srv"
   "srv/SimulatorPush.srv"
+  "srv/SimulatorRecord.srv"
   DEPENDENCIES
   action_msgs
   geometry_msgs

src/bitbots_msgs/srv/SimulatorRecord.srv

@@ -0,0 +1,6 @@
+bool start False
+bool stop False
+string file_path ""
+---
+bool running
+string file_path

src/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py

@@ -11,7 +11,7 @@
 from rosgraph_msgs.msg import Clock
 from std_srvs.srv import Empty
 
-from bitbots_msgs.srv import SetObjectPose, SetObjectPosition, SimulatorPush
+from bitbots_msgs.srv import SetObjectPose, SetObjectPosition, SimulatorPush, SimulatorRecord
 
 G = 9.81
 
@@ -64,6 +64,7 @@ def __init__(
         self.joint_nodes = {}
         self.link_nodes = {}
         self.currently_recording = False
+        self.current_recording_file_path = ""
 
         # set reset height based on the robot, so that we can reset different robots easily
         self.robot_type = robot
@@ -125,7 +126,7 @@ def __init__(
                 SimulatorPush, base_ns + "simulator_push", self.simulator_push
             )
             self.simulator_record_service = self.ros_node.create_service(
-                Empty, base_ns + "simulator_record", self.simulator_record
+                SimulatorRecord, base_ns + "simulator_record", self.simulator_record
             )
 
         self.world_info = self.supervisor.getFromDef("world_info")
@@ -255,14 +256,31 @@ def simulator_push(self, request=None, response=None):
         return response or Empty.Response()
 
     def simulator_record(self, request=None, response=None):
-        if not self.currently_recording:
-            file_path = str((Path.home() / "latest_run" /"latest_run.html").absolute())
+        if request is None:
+            request = SimulatorRecord.Request()
+
+        if request.start:
+            file_path = (
+                request.file_path
+                if request.file_path
+                else str((Path.home() / "latest_run" / "latest_run.html").absolute())
+            )
             self.ros_node.get_logger().info(f"Starting recording to file {file_path}")
             self.currently_recording = self.supervisor.animationStartRecording(file_path)
+            self.current_recording_file_path = file_path
+        elif request.stop:
+            if self.currently_recording:
+                self.ros_node.get_logger().info("Stopping recording")
+                self.currently_recording = not self.supervisor.animationStopRecording()
+            else:
+                self.ros_node.get_logger().info("Recording is not active")
         else:
-            self.ros_node.get_logger().info("Stopping recording")
-            self.currently_recording = not self.supervisor.animationStopRecording()
-        return response or Empty.Response()
+            self.ros_node.get_logger().info("Querying recording state")
+
+        response = response or SimulatorRecord.Response()
+        response.running = self.currently_recording
+        response.file_path = self.current_recording_file_path
+        return response
 
     def on_shutdown(self):
         if self.currently_recording: