Skip to content
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

Fix false trajectory success when blocked by obstacle #156

Merged
merged 4 commits into from
Feb 14, 2025
Merged

Fix false trajectory success when blocked by obstacle #156

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8b4976a
improved precision of trajectory command feedback and results by addi…
gaddison-bdai Feb 12, 2025
448bc9b
lint
gaddison-bdai Feb 12, 2025
ea2b00c
fixed trajectory handling of last_trajectory_command flag
gaddison-bdai Feb 13, 2025
b12e6bb
trajectory flag last_trajectory_cmd is now properly not reset if robo…
gaddison-bdai Feb 13, 2025
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
57 changes: 39 additions & 18 deletions spot_wrapper/wrapper.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -265,21 +265,40 @@ def _start_query(self) -> None:
status = (
response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.status
)
# STATUS_AT_GOAL always means that the robot reached the goal. If the trajectory command did not
# request precise positioning, then STATUS_NEAR_GOAL also counts as reaching the goal
if status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL or (
status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL
and not self._spot_wrapper.last_trajectory_command_precise
):
self._spot_wrapper.at_goal = True
# Clear the command once at the goal
self._spot_wrapper.last_trajectory_command = None
final_goal_status = (
response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.final_goal_status
)

if status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_STOPPED:
# Robot is stopped
self.stopped = True
self._spot_wrapper._trajectory_status_unknown = False
elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL:
if (
final_goal_status
== basic_command_pb2.SE2TrajectoryCommand.Feedback.FINAL_GOAL_STATUS_ACHIEVABLE
):
self._spot_wrapper.trajectory_complete = True
self._spot_wrapper.at_goal = True
self._spot_wrapper.last_trajectory_command = None
# Clear the command once at the goal
elif final_goal_status == basic_command_pb2.SE2TrajectoryCommand.Feedback.FINAL_GOAL_STATUS_BLOCKED:
self._spot_wrapper.trajectory_complete = True
self._spot_wrapper.last_trajectory_command = None
elif (
final_goal_status
== basic_command_pb2.SE2TrajectoryCommand.Feedback.FINAL_GOAL_STATUS_IN_PROGRESS
):
self._logger.info(
"Robot stopped but trajectory still in progress. Perhaps something is in the way"
)
else:
self._logger.error("Robot stopped but final goal status is unknown.")
self._spot_wrapper.last_trajectory_command = None
elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_STOPPING:
is_moving = True
elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL:
self._spot_wrapper.is_stopping = True
elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_IN_PROGRESS:
is_moving = True
self._spot_wrapper.near_goal = True
elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN:
self._spot_wrapper.trajectory_status_unknown = True
self._spot_wrapper.last_trajectory_command = None
Expand Down Expand Up @@ -862,12 +881,12 @@ def is_moving(self, state: bool) -> None:
self._state.is_moving = state

@property
def near_goal(self) -> bool:
return self._state.near_goal
def is_stopping(self) -> bool:
return self._state.is_stopping

@near_goal.setter
def near_goal(self, state: bool) -> None:
self._state.near_goal = state
@is_stopping.setter
def is_stopping(self, state: bool) -> None:
self._state.is_stopping = state

@property
def at_goal(self) -> bool:
Expand Down Expand Up @@ -1386,8 +1405,10 @@ def trajectory_cmd(
if mobility_params is None:
mobility_params = self._mobility_params
self._trajectory_status_unknown = False
self.trajectory_complete = False
self.stopped = False
self.at_goal = False
self.near_goal = False
self.is_stopping = False
self.last_trajectory_command_precise = precise_position
self._logger.info("got command duration of {}".format(cmd_duration))
end_time = time.time() + cmd_duration
Expand Down