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

Jump to bottom

added methods in fleet adapter and RobotCommandHandle to read and use parking waypoint #229

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

added methods in fleet adapter and RobotCommandHandle to read and use parking waypoint #229

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
6 changes: 6 additions & 0 deletions rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ def __init__(self,
start,
position,
charger_waypoint,
parking_waypoint,
update_frequency,
lane_merge_distance,
adapter,
Expand All @@ -87,6 +88,11 @@ def __init__(self,
assert waypoint, f"Charger waypoint {charger_waypoint} \
does not exist in the navigation graph"
self.charger_waypoint_index = waypoint.index
# Get the index of the parking spot waypoint
waypoint = self.graph.find_waypoint(parking_waypoint)
assert waypoint, f"Parking waypoint {parking_waypoint} \
does not exist in the navigation graph"
self.parking_waypoint_index = waypoint.index
self.update_frequency = update_frequency
self.lane_merge_distance = lane_merge_distance
self.update_handle = None # RobotUpdateHandle
Expand Down
9 changes: 9 additions & 0 deletions rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -195,6 +195,14 @@ def _action_executor(category: str,
cmd_handle.node.get_logger().warn(
"Invalid waypoint supplied for charger. "
"Using default nearest charger in the map")
if (cmd_handle.parking_waypoint_index <
cmd_handle.graph.num_waypoints):
cmd_handle.update_handle.set_parking_waypoint(
cmd_handle.parking_waypoint_index)
else:
cmd_handle.node.get_logger().warn(
"Invalid waypoint supplied for parking spot. "
"Using default nearest parking spot in the map")

# Initialize robot API for this fleet
prefix = 'http://' + fleet_config['fleet_manager']['ip'] + \
Expand Down Expand Up @@ -277,6 +285,7 @@ def _add_fleet_robots():
start=starts[0],
position=position,
charger_waypoint=rmf_config['charger']['waypoint'],
parking_waypoint=rmf_config['start']['waypoint'],
update_frequency=rmf_config.get(
'robot_state_update_frequency', 1),
lane_merge_distance=lane_merge_distance,
Expand Down
Loading