def init_position(self):
"Init to position [0, 0]"
# Initialize last time reloc
self.last_time_reloc = time.time()
# Get the robot position in world frame and initialize home frame
# At this position
xw0, yw0, thetaw0 = self.services.ALMotion.getRobotPosition(True)
self.home_frame = pu.Pose(x_coord=xw0, y_coord=yw0, theta=thetaw0)
self.robot_in_home = pu.Pose(x_coord=0, y_coord=0, theta=0)
self.relative_cumulated_displacement = almath.Pose2D(
self.services.ALMotion._getCumulatedDisplacement())
# Save position in file
self.__save_position_in_file()
def __init__(self, qiapp):
self.qiapp = qiapp
self.session = qiapp.session
self.events = stk.events.EventHelper(self.session)
self.services = stk.services.ServiceCache(self.session)
self.logger = stk.logging.get_logger(self.session, self.APP_ID)
# Wait some services
self.session.waitForService("ALMotion")
self.session.waitForService("ALMemory")
self.session.waitForService("ALPreferenceManager")
# Position of the robot initial
xw0, yw0, thetaw0 = self.services.ALMotion.getRobotPosition(True)
self.home_frame = pu.Pose(x_coord=xw0, y_coord=yw0, theta=thetaw0)
self.robot_in_home = self.home_frame.get_relative(self.home_frame)
# Load saved position
if os.path.exists(PATH_POSITION_FILE):
self.__load_position_in_file()
else:
folder = os.path.dirname(PATH_POSITION_FILE)
if not os.path.exists(folder):
os.makedirs(folder)
self.__save_position_in_file()
# Save the time for checking if we need to relocalize or not
# We need to relocalize each X minutes for safety and being
# sur to know where we are.
self.last_time_reloc = time.time()
# periodic task
self.relative_cumulated_displacement = almath.Pose2D(
self.services.ALMotion._getCumulatedDisplacement())
def __load_position_in_file(self):
"Load the position from the file on the robot"
json_data = None
with open(PATH_POSITION_FILE, 'r') as file:
json_data_str = file.read()
if json_data_str:
json_data = json.loads(json_data_str)
if json_data and (time.time() - json_data["timestamp"]) < 60 * 10:
self.home_frame = pu.Pose(
pos=complex(json_data["home_frame"]['pos']),
orientation=complex(json_data["home_frame"]['orientation']))
self.robot_in_home = pu.Pose(
pos=complex(json_data["robot_in_home"]['pos']),
orientation=complex(json_data["robot_in_home"]['orientation']))
self.__save_position_in_file()
else:
self.init_position()
def get_polar_coord(self, coord_home):
x , y , theta = coord_home
object_pos = pu.coords_to_pos(0, 0)
robot_pose = pu.Pose(x_coord=x, y_coord=y, theta=theta)
object_pos_in_r = robot_pose.get_relative_pos(object_pos)
r_theta = float(pu.get_pos_theta(object_pos_in_r))
r = float(math.sqrt(math.pow(x,2) + math.pow(y,2)))
return r, r_theta
def __recalculate_position(self):
"Recalculate actual position of the robot"
x_coord, y_coord, theta = (
self.services.ALMotion.getRobotPosition(True))
# Robot position in the world frame
robot_in_world = pu.Pose(x_coord=x_coord, y_coord=y_coord, theta=theta)
# Robot in the home frame
self.robot_in_home = self.home_frame.get_relative(robot_in_world)
# Save in a file on the robot
self.__save_position_in_file()
def init_position_with_coord(self, coord):
"Init Position of the robot at the coordinate give in argument"
# Initialize last time reloc
self.last_time_reloc = time.time()
# Get the robot position in world frame and initialize home frame
# At this position
xw0, yw0, thetaw0 = coord
self.home_frame = pu.Pose(x_coord=xw0, y_coord=yw0, theta=thetaw0)
self.robot_in_home = self.home_frame.get_relative(self.home_frame)
self.relative_cumulated_displacement = almath.Pose2D(
self.services.ALMotion._getCumulatedDisplacement())
# Save position in file
self.__save_position_in_file()