## todo: Ian - should this wait for some period (e.g., timeout=60s), otherwise we will wait forever
move_base_started = move_base.wait_for_server()
if not move_base_started:
fail_hard("fatal error: navigation stack has failed to start")
print("Starting up Gazebo interface")
try:
gazebo = GazeboInterface(
0,
0) ## todo: unsure what these args mean but they appear in cli.py
## todo: CP3.convert_to_class(cp) ## appears a lot in cli.py but i don't know what it means
## todo: this could be totally busted
cp.gazebo = gazebo
cp.init()
start_coords = cp.map_server.waypoint_to_coords(ready_resp.start_loc)
gazebo.set_turtlebot_position(start_coords['x'], start_coords['y'], 0)
except Exception as e:
fail_hard("failed to connect to gazebo: %s" % e)
## todo: for RR3, do things with utility function
if ready_resp.use_adaptation:
try:
rainbow_log = open(os.path.expanduser("~/rainbow.log"), 'w')
rainbow = RainbowInterface()
##todo: Ian: be careful copying this for cp1
rainbow.launchRainbow("cp3", rainbow_log)
ok = rainbow.startRainbow()
if not ok:
fail_hard("did not connect to rainbow in a timely fashion")
except Exception as e:
raise Exception("Failed to start")
# this should block until the navigation stack is ready to recieve goals
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
move_base_started = move_base.wait_for_server()
if not move_base_started:
log_das(LogError.STARTUP_ERROR,
"Fatal: Navigation stack failed to start")
th_das_error(Error.DAS_OTHER_ERROR,
"Fatal: could not connect to move_base")
raise Exception("Failed to start")
# arrange the bot in the location specified by the config
try:
start_coords = waypoint_to_coords(config.start_loc)
gazebo.set_turtlebot_position(start_coords['x'], start_coords['y'],
config.start_yaw)
except Exception as e:
log_das(LogError.STARTUP_ERROR,
"Fatal: config file inconsistent with map: %s" % e)
th_das_error(Error.DAS_OTHER_ERROR,
"Fatal: config file inconsistent with map: %s" % e)
raise
# start Rainbow
try:
rainbow_log = open("/test/rainbow.log", 'w')
rainbow = RainbowInterface()
rainbow.launchRainbow(config.enable_adaptation, rainbow_log)
ok = rainbow.startRainbow()
if not ok:
elif args.command == 'place_obstacle':
pargs = po_parser.parse_args(extras)
id = gazebo.place_new_obstacle(pargs.x, pargs.y, pargs.height)
if id is None:
print('Could not place an obstacle')
else:
print('Obstacle "%s" placed in the world.' % id)
elif args.command == 'remove_obstacle':
rargs = do_parser.parse_args(extras)
result = gazebo.delete_obstacle(rargs.obstacle_id, False)
print('Obstacle was removed %s' %
('successfully' if result else 'unsuccessfully'))
elif args.command == 'set_pose':
pargs = sp_parser.parse_args(extras)
pargs.w = math.radians(pargs.w)
result = gazebo.set_turtlebot_position(pargs.x, pargs.y, pargs.w)
print('Turtlebot was placed %s' %
('successfully' if result else 'unsuccessfully'))
elif args.command == 'set_location':
largs = sl_parser.parse_args(extras)
location = cp.map_server.waypoint_to_coords(largs.waypoint)
result = gazebo.set_turtlebot_position(location["x"], location["y"], 0)
print('Turtlebot was placed %s' %
('successfully' if result else 'unsuccessfully'))
elif args.command == 'kinect':
kargs = k_parser.parse_args(extras)
gazebo.set_kinect_mode(kargs.enablement)
elif args.command == 'lidar':
largs = l_parser.parse_args(extras)
gazebo.set_lidar_mode(largs.enablement)
elif args.command == 'where':