diff --git a/rmf_mock_robot_sim/rmf_mock_robot_sim/mock_robot_sim_node.py b/rmf_mock_robot_sim/rmf_mock_robot_sim/mock_robot_sim_node.py index db9653e5..59620c9c 100644 --- a/rmf_mock_robot_sim/rmf_mock_robot_sim/mock_robot_sim_node.py +++ b/rmf_mock_robot_sim/rmf_mock_robot_sim/mock_robot_sim_node.py @@ -72,9 +72,11 @@ def __init__(self): self.current_path = [] self.current_waypoint_idx = 0 self.current_plan_id = None + self.plan_start_pos = (self.x, self.y) self.last_update_time = self.get_clock().now() self.wait_time_remaining = 0.0 self.released_waypoint_idx = 0 + self.latest_release = None self.was_blocked = False # 3. Configure QoS for Discovery & Reliable Data @@ -172,6 +174,15 @@ def publish_discovery(self): self.dest_discovery_pub.publish(msg) self.plan_discovery_pub.publish(msg) + def _plan_id_matches(self, plan_id_a, plan_id_b): + if plan_id_a is None or plan_id_b is None: + return False + return ( + list(plan_id_a.destination_session.uuid) + == list(plan_id_b.destination_session.uuid) + and plan_id_a.plan_version == plan_id_b.plan_version + ) + def handle_plan(self, msg: Plan): self.get_logger().info( f'Received new plan with {len(msg.waypoints)} waypoints. ' @@ -180,8 +191,14 @@ def handle_plan(self, msg: Plan): self.current_path = msg.waypoints self.current_plan_id = msg.plan_id self.current_waypoint_idx = 0 + self.plan_start_pos = (self.x, self.y) self.wait_time_remaining = 0.0 - self.released_waypoint_idx = 0 + if self.latest_release and self._plan_id_matches( + self.latest_release.plan_id, self.current_plan_id + ): + self.released_waypoint_idx = self.latest_release.waypoint_id + else: + self.released_waypoint_idx = 0 self.was_blocked = False # If the path is not empty and we are far from the start, @@ -194,6 +211,7 @@ def handle_release(self, msg: PlanRelease): f'Received PlanRelease: waypoint_id={msg.waypoint_id}, ' f'plan_version={msg.plan_id.plan_version}' ) + self.latest_release = msg if not self.current_plan_id: self.get_logger().info('Cannot update release: current_plan_id is None') return @@ -276,29 +294,55 @@ def sim_step(self): self.current_waypoint_idx += 1 self.wait_time_remaining = 1.0 - # 2. Publish plan progress + # 2. Publish plan progress + if self.current_path: self.publish_progress() # 3. Publish Odometry self.publish_odometry() def publish_progress(self): - if not self.current_plan_id: + if not self.current_plan_id or not self.current_path: return progress_msg = Progress() progress_msg.plan_id = self.current_plan_id - # Determine reached and target waypoints - # reached_waypoint is the last waypoint reached - reached_idx = max(0, self.current_waypoint_idx - 1) - target_idx = min(len(self.current_path) - 1, self.current_waypoint_idx) - - progress_msg.reached_waypoint = int(reached_idx) - progress_msg.target_waypoint = int(target_idx) + num_waypoints = len(self.current_path) + + if self.current_waypoint_idx >= num_waypoints: + last_idx = num_waypoints - 1 + progress_msg.reached_waypoint = int(last_idx) + progress_msg.target_waypoint = int(last_idx) + progress_msg.progress = float(self.current_path[last_idx].progress) + elif self.current_waypoint_idx == 0: + first_wp = self.current_path[0] + progress_msg.reached_waypoint = 0 + progress_msg.target_waypoint = 0 + seg_dist = math.hypot( + first_wp.position[0] - self.plan_start_pos[0], + first_wp.position[1] - self.plan_start_pos[1], + ) + if seg_dist > 1e-3: + dist_from_start = math.hypot( + self.x - self.plan_start_pos[0], + self.y - self.plan_start_pos[1], + ) + fraction = min(1.0, max(0.0, dist_from_start / seg_dist)) + progress_msg.progress = float(fraction * first_wp.progress) + else: + progress_msg.progress = 0.0 + elif self.is_blocked(): + reached_idx = self.current_waypoint_idx - 1 + progress_msg.reached_waypoint = int(reached_idx) + progress_msg.target_waypoint = int(reached_idx) + progress_msg.progress = float(self.current_path[reached_idx].progress) + else: + reached_idx = self.current_waypoint_idx - 1 + target_idx = self.current_waypoint_idx + progress_msg.reached_waypoint = int(reached_idx) + progress_msg.target_waypoint = int(target_idx) - # Interpolate progress value - if len(self.current_path) > 1: prev_wp = self.current_path[reached_idx] next_wp = self.current_path[target_idx] @@ -306,13 +350,13 @@ def publish_progress(self): next_pos = next_wp.position seg_dist = math.hypot( next_pos[0] - prev_pos[0], - next_pos[1] - prev_pos[1] + next_pos[1] - prev_pos[1], ) if seg_dist > 1e-3: dist_from_prev = math.hypot( self.x - prev_pos[0], - self.y - prev_pos[1] + self.y - prev_pos[1], ) fraction = min(1.0, max(0.0, dist_from_prev / seg_dist)) diff = next_wp.progress - prev_wp.progress @@ -320,9 +364,11 @@ def publish_progress(self): prev_wp.progress + fraction * diff ) else: - progress_msg.progress = float(next_wp.progress) - else: - progress_msg.progress = float(self.current_path[reached_idx].progress) + fraction = min(1.0, max(0.0, 1.0 - self.wait_time_remaining)) + diff = next_wp.progress - prev_wp.progress + progress_msg.progress = float( + prev_wp.progress + fraction * diff + ) progress_msg.reached_keys = [] self.progress_pub.publish(progress_msg) diff --git a/rmf_mock_robot_sim/test/test_mock_robot_sim.py b/rmf_mock_robot_sim/test/test_mock_robot_sim.py index 6b6b12f1..75d036a2 100644 --- a/rmf_mock_robot_sim/test/test_mock_robot_sim.py +++ b/rmf_mock_robot_sim/test/test_mock_robot_sim.py @@ -123,13 +123,6 @@ def test_robot_sim_behavior(self): 10 ) - self.node.create_subscription( - Progress, - f'{robot_name}/plan/progress', - lambda msg: received_progress.append(msg), - 10 - ) - reliable_transient_qos = QoSProfile( depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL, @@ -137,6 +130,13 @@ def test_robot_sim_behavior(self): reliability=ReliabilityPolicy.RELIABLE, ) + self.node.create_subscription( + Progress, + f'{robot_name}/plan/progress', + lambda msg: received_progress.append(msg), + qos_profile=reliable_transient_qos + ) + plan_pub = self.node.create_publisher( Plan, f'{robot_name}/plan', @@ -149,7 +149,7 @@ def test_robot_sim_behavior(self): qos_profile=reliable_transient_qos ) - # 3. Construct and publish Plan + # 3. Construct and publish Plan (initially released only up to waypoint 0) plan_msg = Plan() # Setup PlanId @@ -158,66 +158,124 @@ def test_robot_sim_behavior(self): plan_id.plan_version = 42 plan_msg.plan_id = plan_id - # Waypoint 1: (2.0, 0.0) + # Waypoint 0: (2.0, 0.0), progress 1.0 + wp0 = Waypoint() + wp0.position = [2.0, 0.0] + wp0.progress = 1.0 + + # Waypoint 1: (2.0, 0.0), progress 2.0 (stationary wait waypoint) wp1 = Waypoint() wp1.position = [2.0, 0.0] - wp1.progress = 1.0 + wp1.progress = 2.0 - # Waypoint 2: (4.0, 0.0) + # Waypoint 2: (4.0, 0.0), progress 3.0 wp2 = Waypoint() wp2.position = [4.0, 0.0] - wp2.progress = 2.0 - - plan_msg.waypoints = [wp1, wp2] + wp2.progress = 3.0 - # Create and publish PlanRelease message releasing up to waypoint index 1 - release_msg = PlanRelease() - release_msg.waypoint_id = 1 - release_msg.plan_id = plan_id + plan_msg.waypoints = [wp0, wp1, wp2] + plan_pub.publish(plan_msg) - # Let's publish the plan repeatedly for a short time to ensure delivery + # 4. Wait until robot reaches wp0 and reports blocked progress at wp0 start_time = time.time() - while time.time() - start_time < 0.5: - plan_pub.publish(plan_msg) - release_pub.publish(release_msg) + timeout = 4.0 + blocked_at_wp0 = False + + while time.time() - start_time < timeout: rclpy.spin_once(self.node, timeout_sec=0.05) + if received_progress and received_odoms: + latest_prog = received_progress[-1] + latest_x = received_odoms[-1].pose.pose.position.x + if ( + abs(latest_x - 2.0) < 0.05 + and latest_prog.reached_waypoint == 0 + and latest_prog.target_waypoint == 0 + and abs(latest_prog.progress - 1.0) < 1e-3 + ): + blocked_at_wp0 = True + break + + self.assertTrue( + blocked_at_wp0, + 'Robot did not hold at waypoint 0 with reached_waypoint=0, ' + 'target_waypoint=0, progress=1.0 while unreleased' + ) + + # Verify that before reaching wp0, progress interpolated between 0.0 and 1.0 + pre_wp0_progress = [p.progress for p in received_progress if p.progress < 0.99] + self.assertGreater( + len(pre_wp0_progress), + 0, + 'Expected intermediate progress values (< 1.0) before reaching waypoint 0' + ) + + # 5. Release up to final waypoint (index 2) and wait for completion + release_msg = PlanRelease() + release_msg.waypoint_id = 2 + release_msg.plan_id = plan_id + release_pub.publish(release_msg) - # 4. Run simulation and verify movement and progress updates start_time = time.time() - timeout = 6.0 - target_reached = False + timeout = 8.0 + plan_completed = False while time.time() - start_time < timeout: - rclpy.spin_once(self.node, timeout_sec=0.1) + rclpy.spin_once(self.node, timeout_sec=0.05) - if received_odoms: + if received_odoms and received_progress: latest_odom = received_odoms[-1] + latest_prog = received_progress[-1] current_x = latest_odom.pose.pose.position.x - # If close enough to final waypoint (4.0, 0.0) - if abs(current_x - 4.0) < 0.15: - target_reached = True + if ( + abs(current_x - 4.0) < 0.05 + and latest_prog.reached_waypoint == 2 + and latest_prog.target_waypoint == 2 + and abs(latest_prog.progress - 3.0) < 1e-3 + ): + plan_completed = True break - time.sleep(0.1) - self.assertTrue(target_reached, 'Robot did not reach the final waypoint in the plan') + self.assertTrue( + plan_completed, + 'Robot did not complete the plan with reached_waypoint=2, ' + 'target_waypoint=2, progress=3.0' + ) - # Verify odometry positions and twist in between + # Verify odometry positions moved forward in x-direction self.assertTrue(len(received_odoms) > 0) first_x = received_odoms[0].pose.pose.position.x last_x = received_odoms[-1].pose.pose.position.x self.assertLess(first_x, last_x, 'Robot did not move forward in x-direction') - # Verify progress messages were received during travel - self.assertTrue(len(received_progress) > 0, 'No progress messages received') + # Verify PlanId, bounds, and monotonic non-decreasing progress across all messages + expected_uuid = list(plan_id.destination_session.uuid) + prev_progress_val = 0.0 + saw_stationary_segment = False + saw_final_segment = False - # Verify PlanId matches in progress messages for prog in received_progress: - expected_uuid = list(plan_id.destination_session.uuid) self.assertEqual(list(prog.plan_id.destination_session.uuid), expected_uuid) self.assertEqual(prog.plan_id.plan_version, plan_id.plan_version) - self.assertTrue(0 <= prog.reached_waypoint <= 1) - self.assertTrue(0 <= prog.target_waypoint <= 1) - self.assertTrue(0.0 <= prog.progress <= 2.0) + self.assertTrue(0 <= prog.reached_waypoint <= 2) + self.assertTrue(0 <= prog.target_waypoint <= 2) + self.assertLessEqual(prog.reached_waypoint, prog.target_waypoint) + self.assertTrue(0.0 <= prog.progress <= 3.0) + self.assertGreaterEqual( + prog.progress + 1e-4, + prev_progress_val, + f'Progress decreased from {prev_progress_val} to {prog.progress}' + ) + prev_progress_val = prog.progress + + if prog.reached_waypoint == 0 and prog.target_waypoint == 1: + saw_stationary_segment = True + self.assertTrue(1.0 - 1e-4 <= prog.progress <= 2.0 + 1e-4) + if prog.reached_waypoint == 1 and prog.target_waypoint == 2: + saw_final_segment = True + self.assertTrue(2.0 - 1e-4 <= prog.progress <= 3.0 + 1e-4) + + self.assertTrue(saw_stationary_segment, 'Did not observe progress on segment 0 -> 1') + self.assertTrue(saw_final_segment, 'Did not observe progress on segment 1 -> 2') print( f'Test finished. Received {len(received_odoms)} odom and '