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 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 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 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 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)