Skip to content

fix(rmf_mock_robot_sim): accurately compute and publish Progress across all plan states #67

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
arjo129 wants to merge 1 commit into from
Open

fix(rmf_mock_robot_sim): accurately compute and publish Progress across all plan states #67

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
80 changes: 63 additions & 17 deletions rmf_mock_robot_sim/rmf_mock_robot_sim/mock_robot_sim_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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. '
Expand All @@ -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,
Expand All @@ -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
Expand Down Expand Up @@ -276,53 +294,81 @@ 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]

prev_pos = prev_wp.position
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
progress_msg.progress = float(
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)
Expand Down
138 changes: 98 additions & 40 deletions rmf_mock_robot_sim/test/test_mock_robot_sim.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,20 +123,20 @@ 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,
history=HistoryPolicy.KEEP_LAST,
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',
Expand All @@ -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
Expand All @@ -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 '
Expand Down
Loading