rafal-sugestions

rosbot_gazebo/test/simulation_test_node.py

@@ -27,34 +27,29 @@
 
 class SimulationTestNode(Node):
     __test__ = False
-    DISTANCE_TO_LINEAR_VELOCITY_SCALE = 1.0
-    DISTANCE_TO_ANGULAR_VELOCITY_SCALE = 6.0
+    XY_TOLERANCE = 0.05
+    YAW_TOLERANCE = 0.1
 
     def __init__(self, name="test_node"):
         super().__init__(name)
-        self.goal_x_distance = 0.0
-        self.goal_y_distance = 0.0
-        self.goal_theta_angle = 0.0
-
-        self.velocity_x = 0.0
-        self.velocity_y = 0.0
-        self.velocity_theta = 0.0
+        self.v_x = 0.0
+        self.v_y = 0.0
+        self.v_yaw = 0.0
 
         self.goal_x_event = Event()
         self.goal_y_event = Event()
-        self.goal_theta_event = Event()
+        self.goal_yaw_event = Event()
 
-    def set_and_publish_destination_goal(
-        self, goal_x_distance, goal_y_distance, goal_yaw_angle
-    ):
-        self.goal_x_distance = goal_x_distance
-        self.goal_y_distance = goal_y_distance
-        self.goal_theta_angle = goal_yaw_angle
+    def clear_events(self):
+        self.goal_x_event.clear()
+        self.goal_y_event.clear()
+        self.goal_yaw_event.clear()
 
-        self.velocity_x = self.DISTANCE_TO_LINEAR_VELOCITY_SCALE * goal_x_distance
-        self.velocity_y = self.DISTANCE_TO_LINEAR_VELOCITY_SCALE * goal_y_distance
-        self.velocity_theta = self.DISTANCE_TO_ANGULAR_VELOCITY_SCALE * goal_yaw_angle
-        self.publish_cmd_vel_messages()
+    def set_destination_speed(self, v_x, v_y, v_yaw):
+        self.clear_events()
+        self.v_x = v_x
+        self.v_y = v_y
+        self.v_yaw = v_yaw
 
     def create_test_subscribers_and_publishers(self):
         self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10)
@@ -72,31 +67,22 @@ def start_node_thread(self):
         self.timer = self.create_timer(1.0 / 10.0, self.publish_cmd_vel_messages)
 
     def odometry_callback(self, data: Odometry):
-        pose = data.pose.pose
-        q = (
-            pose.orientation.x,
-            pose.orientation.y,
-            pose.orientation.z,
-            pose.orientation.w,
-        )
-        roll, pitch, yaw = tf_transformations.euler_from_quaternion(q)
+        twist = data.twist.twist
 
-        print(f"x: {pose.position.x}, y: {pose.position.y}, yaw: {yaw}")
-        print(f"roll: {roll}, pitch: {pitch}, yaw: {yaw}")
-
-        if pose.position.x > self.goal_x_distance and self.goal_x_distance != 0.0:
+        if abs(twist.linear.x - self.v_x) < self.XY_TOLERANCE:
             self.goal_x_event.set()
 
-        if pose.position.y > self.goal_y_distance and self.goal_y_distance != 0.0:
+        if abs(twist.linear.y - self.v_y) < self.XY_TOLERANCE:
             self.goal_y_event.set()
 
-        if yaw > self.goal_theta_angle and self.goal_theta_angle != 0.0:
-            self.goal_theta_event.set()
+        if abs(twist.angular.z - self.v_yaw) < self.YAW_TOLERANCE:
+            self.goal_yaw_event.set()
 
     def publish_cmd_vel_messages(self):
         twist_msg = Twist()
-        twist_msg.linear.x = self.velocity_x
-        twist_msg.linear.y = self.velocity_y
-        twist_msg.angular.z = self.velocity_theta
+
+        twist_msg.linear.x = self.v_x
+        twist_msg.linear.y = self.v_y
+        twist_msg.angular.z = self.v_yaw
 
         self.cmd_vel_publisher.publish(twist_msg)

rosbot_gazebo/test/test_diff_drive_simulation.py

@@ -42,7 +42,7 @@ def generate_test_description():
             )
         ),
         launch_arguments={
-            "headless": "True",
+            "headless": "False",
         }.items(),
     )
 
@@ -57,13 +57,13 @@ def test_diff_drive_simulation():
         node.create_test_subscribers_and_publishers()
         node.start_node_thread()
 
-        node.set_and_publish_destination_goal(0.6, 0.0, 0.0)
+        node.set_destination_speed(0.2, 0.0, 0.0)
         msgs_received_flag = node.goal_x_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot can't move by x axis!"
+        assert msgs_received_flag, "ROSbot does not move properly in x direction!"
 
-        node.set_and_publish_destination_goal(0.0, 0.0, 1.57)
-        msgs_received_flag = node.goal_theta_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot can't rotate!"
+        node.set_destination_speed(0.0, 0.0, 1.57)
+        msgs_received_flag = node.goal_yaw_event.wait(timeout=60.0)
+        assert msgs_received_flag, "ROSbot does not rotate properly!"
 
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching

rosbot_gazebo/test/test_mecanum_simulation.py

@@ -43,7 +43,7 @@ def generate_test_description():
         ),
         launch_arguments={
             "mecanum": "True",
-            "headless": "True",
+            "headless": "False",
         }.items(),
     )
 
@@ -58,17 +58,17 @@ def test_mecanum_simulation():
         node.create_test_subscribers_and_publishers()
         node.start_node_thread()
 
-        node.set_and_publish_destination_goal(0.6, 0.0, 0.0)
+        node.set_destination_speed(0.2, 0.0, 0.0)
         msgs_received_flag = node.goal_x_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot can't move by x axis!"
+        assert msgs_received_flag, "ROSbot does not move properly in x direction!"
 
-        node.set_and_publish_destination_goal(0.0, 0.6, 0.0)
+        node.set_destination_speed(0.0, 0.2, 0.0)
         msgs_received_flag = node.goal_y_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot can't move by y axis!"
+        assert msgs_received_flag, "ROSbot does not move properly in y direction!"
 
-        node.set_and_publish_destination_goal(0.0, 0.0, 1.57)
-        msgs_received_flag = node.goal_theta_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot can't rotate!"
+        node.set_destination_speed(0.0, 0.0, 1.00)
+        msgs_received_flag = node.goal_yaw_event.wait(timeout=60.0)
+        assert msgs_received_flag, "ROSbot does not rotate properly!"
 
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching