def path_following_closure():
# This function defines a worker thread that will wakeup at whatever times are needed
while (not self._path_quit_event.is_set()) and self.remaining_path:
next_ros_time = self.remaining_path[0].t
next_mission_time = next_ros_time.sec + next_ros_time.nanosec / 1e9
next_mission_wait = next_mission_time - time.time()
# print(f'next_mission_time: {next_mission_time}, \
# next_mission_wait: {next_mission_wait}')
if next_mission_wait <= 0 and self.mode == MirState.READY and self.remaining_path:
self.remaining_path.pop(0)
if not self.remaining_path:
return
next_mission_location = self.remaining_path[0]
mir_p = self.parent.rmf2mir_transform.transform(
[next_mission_location.x, next_mission_location.y])
mir_location = Location()
mir_location.x = mir_p[0]
mir_location.y = mir_p[1]
yaw = math.degrees(
next_mission_location.yaw +
self.parent.rmf2mir_transform.get_rotation())
print(
f'RMF location x:{next_mission_location.x} y:{next_mission_location.y}'
)
if yaw > 180.0:
yaw = yaw - 360.0
elif yaw <= -180.0:
yaw = yaw + 360.0
mir_location.yaw = yaw
print(f'location: {mir_location}')
# Check whether mission is in mission list
mission_name = f'move_coordinate_to_{mir_location.x:.2f}_{mir_location.y:.2f}_{mir_location.yaw:.2f}'
if mission_name not in self.missions:
print(f'Creating a new mission named {mission_name}')
mission_id = self.parent.create_move_coordinate_mission(
self, mir_location)
else:
mission_id = self.missions[mission_name]
try:
mission = PostMissionQueues(mission_id=mission_id)
self.api.mission_queue_post(mission)
except KeyError:
self.parent.get_logger().error(
f'no mission to move coordinates to [{mir_location.x:3f}_{mir_location.y:.3f}]!'
)
continue
self._path_quit_cv.acquire()
self._path_quit_cv.wait(next_mission_wait)
self._path_quit_cv.release()
def __init__(self, vehicle_traits):
self._svy_transformer = Transformer.from_crs('EPSG:4326', 'EPSG:3414')
self.initialized = False
self.latest_gps_json = None
self.battery_percentage = 0.0
# To be initialized over gps update
self.current_loc = Location()
# To be updated by adapter tasks
self.target_loc = Location()
self.offset_x = 0.0
self.offset_y = 0.0
self.vehicle_traits = vehicle_traits
def navigate(self, pose, map_name: str):
'''
Request the robot to navigate to pose:[x,y,theta] where x, y and theta
are in the robot's coordinate convention. This functihould return True
if the robot has accepted the request, else False
'''
# Compute displacement required
# BH(TODO): Error handling
self.state.target_loc = Location()
self.state.target_loc.x = pose[0]
self.state.target_loc.y = pose[1]
self.state.target_loc.yaw = pose[2]
t = self.get_clock().now().to_msg()
self.state.target_loc.t = self.get_clock().now().to_msg()
duration_to_target_loc = self.state.duration_to_target()
if duration_to_target_loc is None:
return False
t.sec = t.sec + duration_to_target_loc
path_request = PathRequest()
path_request.fleet_name = self.fleet_name
path_request.robot_name = self.robot_name
path_request.path.append(self.state.get_offset_current_loc())
path_request.path.append(self.state.get_offset_target_loc())
self.task_id += random.randint(0, 1000)
path_request.task_id = str(self.task_id)
self.path_pub.publish(path_request)
return True
def make_location(p, level_name):
location = Location()
location.x = p[0]
location.y = p[1]
location.yaw = p[2]
location.level_name = level_name
return location
def send_path_request(args):
print('fleet_name: {}'.format(args.fleet_name))
print('robot_name: {}'.format(args.robot_name))
print('x: {}'.format(args.x))
print('y: {}'.format(args.y))
print('yaw: {}'.format(args.yaw))
print('task_id: {}'.format(args.task_id))
print('topic_name: {}'.format(args.topic_name))
rclpy.init()
node = rclpy.create_node('send_path_request_node')
pub = node.create_publisher(PathRequest, args.topic_name, 10)
msg = PathRequest()
msg.fleet_name = args.fleet_name
msg.robot_name = args.robot_name
msg.task_id = args.task_id
# ignore time for now
location = Location()
location.x = float(args.x)
location.y = float(args.y)
location.yaw = float(args.yaw)
location.level_name = "" # todo: param?
# todo: set realistic time?
location.t.sec = 0
location.t.nanosec = 0
msg.path.append(location)
rclpy.spin_once(node, timeout_sec=2.0)
pub.publish(msg)
rclpy.spin_once(node, timeout_sec=0.5)
print('all done!')
def get_path_message(json_path_string):
path_dict = json.loads(json_path_string)
path = []
for waypoint in path_dict:
new_wp = Location()
# ignore time for now
new_wp.x = float(waypoint['x'])
new_wp.y = float(waypoint['y'])
new_wp.yaw = float(waypoint['yaw'])
new_wp.level_name = waypoint['level_name']
path.append(new_wp)
return path
def make_robot_state(name: str = "test_robot") -> RobotState:
return RobotState(
name=name,
model="test_model",
task_id="",
seq=0,
mode=RobotMode(mode=RobotMode.MODE_IDLE, mode_request_id=0),
battery_percent=0.0,
location=Location(
t=Time(sec=0, nanosec=0),
x=0.0,
y=0.0,
yaw=0.0,
level_name="test_level",
index=0,
),
path=[],
)
def next_loc_cb(self, msg):
next_loc = msg.data
Utils.logger.info(f'Received request to go to location {next_loc}')
loc_details = self.get_nav_location_loc_details(next_loc)
if loc_details:
Utils.logger.info(f'Found details for location: {loc_details}')
location = Location()
location.x = loc_details['x']
location.y = loc_details['y']
location.yaw = 0.0
location.level_name = loc_details['level']
self.robot_state.path.append(location)
self.robot_state.task_id = next_loc
Utils.logger.info(f'New Path: {self.robot_state.path}')
else:
Utils.logger.error(f'Did not find location for {next_loc}.')
return
def __init__(self, argv=sys.argv):
super().__init__('teleop_publisher')
parser = argparse.ArgumentParser()
parser.add_argument('-F',
'--fleet',
required=True,
type=str,
help='Fleet name')
parser.add_argument('-R',
'--robot',
required=True,
help='Robot name',
type=str)
parser.add_argument('-p',
'--points',
required=True,
nargs='+',
help='Coordinate points [x,y,yaw] for teleop',
type=str)
parser.add_argument('-m',
'--map',
required=True,
help='Map name',
type=str)
self.args = parser.parse_args(argv[1:])
self.pub = self.create_publisher(PathRequest, 'robot_path_requests', 1)
msg = PathRequest()
msg.fleet_name = self.args.fleet
msg.robot_name = self.args.robot
msg.task_id = str(uuid.uuid1())
for p in self.args.points:
pts = p.split(',')
assert len(pts) == 3, "each point should have 3 values {x,y,yaw}"
loc = Location()
loc.x = float(pts[0])
loc.y = float(pts[1])
loc.yaw = float(pts[2])
loc.level_name = self.args.map
msg.path.append(loc)
self.pub.publish(msg)
def set_robot_state(robot_state: RobotState,
robot_name: str,
x: float,
y: float,
yaw: float,
level: str):
robot_state.name = robot_name
robot_mode = RobotMode()
robot_mode.mode = robot_mode.MODE_IDLE
robot_state.mode = robot_mode
robot_state.battery_percent = 100.0
robot_location = Location()
robot_location.x = x
robot_location.y = y
robot_location.yaw = yaw
robot_location.level_name = level
robot_state.location = robot_location
return robot_state
def update_internal_robot_state(self, api_response=None):
"""Update internal robot state message. Does not publish!"""
# NOTE(CH3): You might need to use robot.mir_name depending
# on whether you want to use the config yaml name or MiR server
# name whereever the FleetState message is intended to be used
robot_state = RobotState() # Temporary msg to avoid race conditions
robot_state.name = self.name
# NOTE(CH3): Presuming model here means robot model, not sim model
robot_state.model = "MiR100"
if self.dry_run:
self.robot_state = robot_state
return
try:
if api_response is None:
api_response = self.mir_api.status_get()
now_sec, now_ns = math.modf(
self.node.get_clock().now().seconds_nanoseconds())
# Populate Location message
rmf_location = Location()
rmf_location.x, rmf_location.y, rmf_location.yaw = (
self.get_position(rmf=True, api_response=api_response))
rmf_location.level_name = self.rmf_map_name
# Populate RobotState message
robot_state.task_id = str(self.current_task_id)
robot_state.battery_percent = api_response.battery_percentage
robot_state.location = rmf_location
robot_state.path = self.rmf_robot_state_path_locations
robot_state.location.t.sec = now_sec
robot_state.location.t.nanosec = now_ns
if api_response.mission_text.startswith('Charging'): # Charging
robot_state.mode.mode = RobotMode.MODE_CHARGING
self.mir_state = MiRState.READY
elif api_response.state_id == MiRState.PAUSE: # Paused/Pre-empted
self.pause()
robot_state.mode.mode = RobotMode.MODE_PAUSED
self.mir_state = MiRState.PAUSE
elif (api_response.state_id == MiRState.EXECUTING # Moving
and not api_response.mission_text.startswith('Charging')):
self.resume()
robot_state.mode.mode = RobotMode.MODE_MOVING
self.mir_state = MiRState.EXECUTING
elif api_response.state_id == MiRState.READY: # Idle/Moved
self.resume()
robot_state.mode.mode = RobotMode.MODE_IDLE
self.mir_state = MiRState.READY
if self.rmf_docking_requested:
if self.rmf_docking_executed: # Docked
if api_response.state_id == MiRState.READY:
robot_state.mode.mode = RobotMode.MODE_IDLE
else: # Docking
robot_state.mode.mode = RobotMode.MODE_DOCKING
# Update internal RobotState
self.robot_state = robot_state
self.last_robot_state_update = (
self.node.get_clock().now().nanoseconds / 1e9)
except ApiException as e:
self.node.get_logger().warn('Exception when calling '
'DefaultApi->status_get: %s\n' % e)
def follow_new_path(
self,
waypoints,
next_arrival_estimator, # function!
path_finished_callback):
self.stop()
self.current_task_id += 1
self.rmf_path_requested = True
# Obtain plan waypoints ===============================================
self.rmf_remaining_path_waypoints = copy.copy(waypoints)
# We reverse this list so that we can pop it instead of traversing
# it using an index (which is more Pythonic)
self.rmf_remaining_path_waypoints.reverse()
# Construct robot state path list =====================================
self.rmf_robot_state_path_locations = []
for waypoint in self.rmf_remaining_path_waypoints:
# Split timestamp into decimal and whole second portions
_sub_seconds, _seconds = math.modf(waypoint.time.timestamp())
_msg = Location()
_msg.x, _msg.y, _msg.yaw = waypoint.position
_msg.t.sec, _msg.t.nanosec = int(_seconds), int(_sub_seconds * 1e9)
self.rmf_robot_state_path_locations.append(_msg)
if not self.dry_run:
status = PutStatus(state_id=MiRState.READY)
self.mir_api.status_put(status)
def path_following_closure():
_current_waypoint = None
_next_waypoint = None
# LOOP ============================================================
# Kept alive if paused
while ((self.rmf_remaining_path_waypoints or self.paused)
or _current_waypoint):
if not self.paused: # Skipped if paused
if _current_waypoint is None:
_current_waypoint = (
self.rmf_remaining_path_waypoints.pop())
self.rmf_path_requested = True
waypoint_leave_msg = _current_waypoint.t
ros_waypoint_leave_time = (
waypoint_leave_msg.sec +
waypoint_leave_msg.nanosec / 1e9)
ros_now = self.node.get_clock().now().nanoseconds / 1e9
next_mission_wait = (ros_waypoint_leave_time - ros_now)
else:
# Prevent spinning out of control when paused
self._path_quit_cv.acquire()
self._path_quit_cv.wait(1)
self._path_quit_cv.release()
# CHECK FOR PRE-EMPT ==========================================
if self._path_quit_event.is_set():
self.clear()
self.node.get_logger().info(
'[ABORT] {self.name}: Pre-empted path following!')
return
# EXECUTE NEXT COMMAND ========================================
# Wait for time to leave and robot to finish moving
if (next_mission_wait <= 0 and self.mir_state == MiRState.READY
and not self.paused): # Skipped if paused
# END =====================================================
if not self.rmf_remaining_path_waypoints: # We're done!
self.rmf_path_requested = False
path_finished_callback()
self.execute_updates()
return
# ASSIGN NEXT TARGET ======================================
else:
_next_waypoint = self.rmf_remaining_path_waypoints[-1]
# Grab graph indices
if _next_waypoint.graph_index.has_value:
_next_index = _next_waypoint.graph_index.value
else:
_next_index = None
if _current_waypoint.graph_index.has_value:
_current_index = (
_current_waypoint.graph_index.value)
else:
_current_index = None
_current_waypoint = None
# Update Internal Location Trackers ===================
# [IdleAtWaypoint -> RMF_Move]
# [IdleAtLane -> RMF_Move]
# [IdleAtUnknown -> RMF_Move]
# Set target index
self.rmf_target_waypoint_index = _next_index
# Infer and set lane index
if not self.rmf_current_lane_index:
if _current_index is not None:
self.rmf_current_lane_index = (
self.lane_dict.get(
(_current_index, _next_index)))
# Unset current index
self.rmf_current_waypoint_index = None
# SEND NEXT TARGET ========================================
_mir_pos = self.transforms['rmf_to_mir'].transform([
_next_waypoint.position[0], _next_waypoint.position[1]
])
_mir_ori_rad = (
math.radians(_next_waypoint.position[2] % 360) +
self.transforms['rmf_to_mir'].get_rotation())
# NOTE(CH3): MiR Location is sent in Degrees
_mir_ori = math.degrees(_mir_ori_rad % (2 * math.pi))
if _mir_ori > 180.0:
_mir_ori = _mir_ori - 360.0
elif _mir_ori <= -180.0:
_mir_ori = _mir_ori + 360.0
mir_location = MiRLocation(x=_mir_pos[0],
y=_mir_pos[1],
yaw=_mir_ori)
print(f"RMF location x:{_next_waypoint.position[0]}"
f"y:{_next_waypoint.position[1]}")
print(f'MiR location: {mir_location}')
self.queue_move_coordinate_mission(mir_location)
self.execute_updates()
continue
if not self.paused: # Skipped if paused
# Prevent spinning out of control
if next_mission_wait <= 0:
next_mission_wait = 0.1
self._path_quit_cv.acquire()
self._path_quit_cv.wait(next_mission_wait)
self._path_quit_cv.release()
self._path_quit_event.clear()
if self._path_following_thread is not None:
self._path_following_thread.join()
self._path_following_thread = threading.Thread(
target=path_following_closure)
self._path_following_thread.start()
def pub_fleet(self):
fleet_state = FleetState()
fleet_state.name = self.FLEET_NAME
now = time.time()
now_sec = int(now)
now_ns = int((now - now_sec) * 1e9)
try:
for robot in self.robots.values():
api_response = robot.api.status_get()
robot_state = RobotState()
robot_state.name = robot.name
robot_state.task_id = robot.current_task_id
robot_state.battery_percent = api_response.battery_percentage
location = Location()
location.x = api_response.position.x
location.y = api_response.position.y
location.yaw = api_response.position.orientation
# TODO Transform yaw from MiR frame to RMF frame
mir_pos = [location.x, location.y]
rmf_pos = self.mir2rmf_transform.transform(mir_pos)
rmf_location = Location()
rmf_location.x = rmf_pos[0]
rmf_location.y = rmf_pos[1]
rmf_location.yaw = math.radians(
location.yaw) + self.mir2rmf_transform.get_rotation()
robot_state.location = rmf_location
robot_state.path = robot.remaining_path
robot_state.location.t.sec = now_sec
robot_state.location.t.nanosec = now_ns
if api_response.mission_text.startswith('Charging'):
robot_state.mode.mode = RobotMode.MODE_CHARGING
robot.mode = MirState.READY
elif api_response.state_id == MirState.PAUSE:
robot_state.mode.mode = RobotMode.MODE_PAUSED
robot.mode = MirState.PAUSE
elif api_response.state_id == MirState.EXECUTING and \
not api_response.mission_text.startswith('Charging'):
robot_state.mode.mode = RobotMode.MODE_MOVING
robot.mode = MirState.EXECUTING
elif api_response.state_id == MirState.READY:
robot_state.mode.mode = RobotMode.MODE_IDLE
robot.mode = MirState.READY
# print(f'[{api_response.state_id}] [{api_response.state_text}] [{api_response.mission_text}]')
fleet_state.robots.append(robot_state)
if robot.docking_requested:
if not robot.docking_executed:
robot.docking_executed = (
'docking' in api_response.mission_text.lower())
if robot.docking_executed and api_response.state_id == MirState.READY:
robot_state.mode.mode = RobotMode.MODE_IDLE
else:
robot_state.mode.mode = RobotMode.MODE_DOCKING
self.status_pub.publish(fleet_state)
except ApiException as e:
self.get_logger().warn('Exception when calling \
DefaultApi->status_get: %s\n' % e)