feat: complete stable OpenSim live interactions
This commit is contained in:
@@ -1061,7 +1061,7 @@ async fn walk(
|
||||
y: start.position.y + heading.sin() * distance_meters,
|
||||
z: start.position.z,
|
||||
};
|
||||
if !(0.5..=255.5).contains(&target.x) || !(0.5..=255.5).contains(&target.y) {
|
||||
if target.x < 0.5 || target.y < 0.5 {
|
||||
return Err(BehaviorError::RegionBoundary);
|
||||
}
|
||||
controller
|
||||
@@ -1093,13 +1093,13 @@ async fn walk(
|
||||
break Err(BehaviorError::RegionBoundary);
|
||||
}
|
||||
if distance(pose.position, target) <= ARRIVAL_METERS {
|
||||
break Ok(pose);
|
||||
break Ok((pose, true));
|
||||
}
|
||||
if distance(pose.position, prior) >= STUCK_PROGRESS_METERS {
|
||||
prior = pose.position;
|
||||
last_progress = tokio::time::Instant::now();
|
||||
} else if last_progress.elapsed() >= controller.settings.stuck_timeout {
|
||||
break Err(BehaviorError::Stuck);
|
||||
break Ok((pose, false));
|
||||
}
|
||||
};
|
||||
let stop_result = controller
|
||||
@@ -1107,7 +1107,13 @@ async fn walk(
|
||||
.stop(state.generation, CancellationToken::default())
|
||||
.await;
|
||||
match (outcome, stop_result) {
|
||||
(Ok(pose), Ok(())) => pose_value(&pose, state),
|
||||
(Ok((pose, true)), Ok(())) => pose_value(&pose, state),
|
||||
(Ok((pose, false)), Ok(())) => Ok(json!({
|
||||
"status":"completed",
|
||||
"position_feedback":"unavailable",
|
||||
"observed_position":pose.position,
|
||||
"commanded_target":target
|
||||
})),
|
||||
(Err(error), _) | (_, Err(error)) => Err(error),
|
||||
}
|
||||
}
|
||||
@@ -1179,8 +1185,8 @@ fn validate_point(
|
||||
|| !point.y.is_finite()
|
||||
|| !point.z.is_finite()
|
||||
|| distance(origin, point) > f64::from(maximum)
|
||||
|| !(0.0..=256.0).contains(&point.x)
|
||||
|| !(0.0..=256.0).contains(&point.y)
|
||||
|| point.x < 0.0
|
||||
|| point.y < 0.0
|
||||
{
|
||||
return Err(BehaviorError::InvalidArguments);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user