def TakeClosePic(self):
# x – normalized, unitless, velocity along X-axis.
# +1 and -1 correspond to the maximum velocity in the forward and backward directions, respectively.
x = 1.0
y = 0.0
theta = 0.0
# index of image
image_i = 0
frequency = 0.1
# get current time
t0 = time.time()
initRobotPosition = almath.Pose2D(
self.motion_service.getRobotPosition(False))
# since now the robot is 1.5 away from the human
# let the robot move 0.7 meters forward while taking photos(now the velocity is 0.1 meter per second)
# in order to keep 0.8 meter distance away from human
while ((time.time() - t0) <= 7.0):
# once 7s has been passed, break the loop
if (time.time() - t0) > 7.0:
break
# print "time difference :" + str((time.time() - t0))
self.motion_service.moveToward(x, y, theta,
[["MaxVelXY", frequency]])
image_i += 1
self.GetImage(image_i)
# stop the robot
self.motion_service.stopMove()
endRobotPosition = almath.Pose2D(
self.motion_service.getRobotPosition(False))
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Distance change and angle change after approaching the human:", robotMove
def moverPernas(xMetros, yMetros, eixoGraus, posturaStandInit = True):
try:
if posturaStandInit:
Motor.posturaStandInit()
motion = Motor.movimento()
initPosition = almath.Pose2D(motion.getRobotPosition(True))
targetDistance = almath.Pose2D(xMetros, yMetros, eixoGraus * almath.PI / 180)
expectedEndPosition = initPosition * targetDistance
motion.setMoveArmsEnabled(True, True)
varmovimento = motion.moveTo(xMetros, yMetros, eixoGraus * almath.PI / 180)
positionErrorThresholdPos = 0.01
positionErrorThresholdAng = 0.03
# The move is finished so output
realEndPosition = almath.Pose2D(motion.getRobotPosition(False))
positionError = realEndPosition.diff(expectedEndPosition)
positionError.theta = almath.modulo2PI(positionError.theta)
if (abs(positionError.x) < positionErrorThresholdPos
and abs(positionError.y) < positionErrorThresholdPos
and abs(positionError.theta) < positionErrorThresholdAng):
print("Movimento OK")
else:
print("Movimento ERROR", positionError.toVector())
Motor.posturaCrouch()
ModoAutonomo.on()
time.sleep(0.25)
ModoAutonomo.off()
time.sleep(0.25)
Motor.posturaStandInit()
return varmovimento
except Exception as e:
print("Exception -> Motor.moverPernas():", e)
return False
def return_home_with_alnavigation(self):
cpt = 0
if self.slam_pos_in_world:
for i in range(0, 100):
robotPose = almath.Pose2D(
self.services.ALNavigation.getRobotPositionInMap()[0])
center = almath.Pose2D(0.0, 0.0, 0.0)
if robotPose.distance(center) > 0.3:
self.navigation_future = self.services.ALNavigation.navigateToInMap(
[0.0, 0.0],
_async=True)
yield self.navigation_future
if robotPose.distance(center) < 0.3:
poseDiff = robotPose.diff(center)
if math.fabs(poseDiff.theta) > 0.2:
self.navigation_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(
poseDiff.theta),
_async=True)
yield self.navigation_future
else:
cpt += 1
if cpt > 3:
self.slam_pos_in_world = [0, 0]
yield stk.coroutines.Return(True)
return
self.logger.info("End back home.")
yield stk.coroutines.Return(False)
def callback(self, poseStamped):
pose = Pose(poseStamped.pose.position, poseStamped.pose.orientation)
quat = almath.Quaternion(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z)
rotation = almath.rotation3DFromQuaternion(quat)
position3d = almath.Position3D(pose.position.x, pose.position.y, pose.position.z)
worldToTarget = almath.Pose2D(position3d.x, position3d.y, rotation.wz)
worldToRobot = almath.Pose2D(self.motionProxy.getRobotPosition(True))
robotToTarget = almath.pinv(worldToRobot) * worldToTarget
robotToTarget.theta = almath.modulo2PI(robotToTarget.theta)
self.motionProxy.moveTo(robotToTarget.x, robotToTarget.y, robotToTarget.theta)
def main(session):
"""
Move To: Small example to make Nao Move To an Objective.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motion_service.setMoveArmsEnabled(True, True)
#~ motion_service.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi/2.0
motion_service.moveTo(X, Y, Theta, _async=True)
# wait is useful because with _async moveTo is not blocking function
motion_service.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motion_service.rest()
def main(session):
"""
Move To: Small example to make Nao Move To an Objective.
"""
# Get the services ALMotion & ALRobotPosture.
motion_service = session.service("ALMotion")
posture_service = session.service("ALRobotPosture")
# Wake up robot
motion_service.wakeUp()
# Send robot to Stand Init
posture_service.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motion_service.setMoveArmsEnabled(True, True)
#~ motion_service.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi/2.0
motion_service.moveTo(X, Y, Theta, _async=True)
# wait is useful because with _async moveTo is not blocking function
motion_service.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motion_service.rest()
def callback(self, poseStamped):
pose = Pose(poseStamped.pose.position, poseStamped.pose.orientation)
quat = almath.Quaternion(pose.orientation.w, pose.orientation.x,
pose.orientation.y, pose.orientation.z)
rotation = almath.rotation3DFromQuaternion(quat)
position3d = almath.Position3D(pose.position.x, pose.position.y,
pose.position.z)
worldToTarget = almath.Pose2D(position3d.x, position3d.y, rotation.wz)
worldToRobot = almath.Pose2D(self.motionProxy.getRobotPosition(True))
robotToTarget = almath.pinv(worldToRobot) * worldToTarget
robotToTarget.theta = almath.modulo2PI(robotToTarget.theta)
self.motionProxy.moveTo(robotToTarget.x, robotToTarget.y,
robotToTarget.theta)
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand Init
postureProxy.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motionProxy.setMoveArmsEnabled(True, True)
#~ motionProxy.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
X = 0.0
Y = 0.0
Theta = math.pi/2.0
motionProxy.post.moveTo(X, Y, Theta)
motionProxy.setAngles("HeadYaw", 0.6, 0.6)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = m.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motionProxy.rest()
def main(robotIP, PORT=9559):
motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
# Wake up robot
motionProxy.wakeUp()
# Send robot to Stand Init
postureProxy.goToPosture("StandInit", 0.5)
#####################
## Enable arms control by move algorithm
#####################
motionProxy.setMoveArmsEnabled(True, True)
#~ motionProxy.setMoveArmsEnabled(False, False)
#####################
## FOOT CONTACT PROTECTION
#####################
#~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
#####################
## get robot position before move
#####################
initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
X = 0.3
Y = 0.1
Theta = math.pi / 2.0
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
#####################
## get robot position after move
#####################
endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))
#####################
## compute and print the robot motion
#####################
robotMove = m.pose2DInverse(initRobotPosition) * endRobotPosition
# return an angle between ]-PI, PI]
robotMove.theta = m.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
# Go to rest position
motionProxy.rest()
def last_move(self, x, y, theta):
value = yield self.services.ALMotion.moveTo(x,
y,
0,
MOVE_CONFIG_LOW_SPEED,
_async=True)
if value:
value = yield self.services.ALMotion.moveTo(
0, 0, almath.modulo2PI(theta), _async=True)
if value:
self.pod_pos_in_world = [0.0, 0.0]
self.pod_position_from_robot = None
yield stk.coroutines.Return(True)
yield stk.coroutines.Return(False)
def moveFunction(self, target):
diff = self.getPositionDiff(target)
print diff
while (abs(diff.y) > 0.1):
diff = self.getPositionDiff(target)
print "turn:", diff
resTurn = self.motion.moveTo(0, 0,
almath.modulo2PI(diff.getAngle()))
time.sleep(0.01)
diff = self.getPositionDiff(target)
diff = self.getPositionDiff(target)
while (diff.x > 0.1):
diff = self.getPositionDiff(target)
print "move: ", diff
fut = self.motion.moveTo(diff.x,
diff.y,
almath.modulo2PI(diff.getAngle()),
_async=True)
time.sleep(0.2)
diff = self.getPositionDiff(target)
try:
fut.wait()
except Exception, e:
pass
def walking(motionProxy, X, Y, Theta):
# Enable arms control by move algorithm
motionProxy.setMoveArmsEnabled(True, True)
# FOOT CONTACT PROTECTION
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
# get robot position before move
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
# get robot position after move
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
# return an angle between [-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
def walking(motionProxy, X, Y, Theta):
# Enable arms control by move algorithm
motionProxy.setMoveArmsEnabled(True, True)
# FOOT CONTACT PROTECTION
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
# get robot position before move
initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
motionProxy.post.moveTo(X, Y, Theta)
# wait is useful because with post moveTo is not blocking function
motionProxy.waitUntilMoveIsFinished()
# get robot position after move
endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
# compute and print the robot motion
robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
# return an angle between [-PI, PI]
robotMove.theta = almath.modulo2PI(robotMove.theta)
print "Robot Move:", robotMove
def EjecuteMovimiento(x, y, Theta):
x = 0.5
y = 0.0
initPosition = almath.Pose2D(motionProxy.getRobotPosition(True))
print "EJECUTE en x: " + str(x) + "\nen y: " + str(
y) + "\nen Theta: " + str(Theta)
targetDistance = almath.Pose2D(x, y, Theta * almath.PI / 180)
expectedEndPosition = initPosition * targetDistance
# enableArms = self.getParameter("Arms movement enabled")
# self.motion.setMoveArmsEnabled(enableArms, enableArms)
motionProxy.moveTo(x, y, Theta * almath.PI / 180)
# The move is finished so output
realEndPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
positionError = realEndPosition.diff(expectedEndPosition)
positionError.theta = almath.modulo2PI(positionError.theta)
if (abs(positionError.x) < positionErrorThresholdPos
and abs(positionError.y) < positionErrorThresholdPos
and abs(positionError.theta) < positionErrorThresholdAng):
#onArrivedAtDestination()
print "on arrived destination"
else:
#onStoppedBeforeArriving(positionError.toVector())
print "onStoppedBeforeArriving" + str(positionError.toVector())
def return_home(self, research_360_activated=False):
on_home = False
id_aruco = None
if self.need_approach_home() and not research_360_activated:
self.aruco_future = self.approach_home()
yield self.aruco_future
if not self.aruco_future.value():
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"obstacle")
yield stk.coroutines.Return(False)
self.aruco_future = self.find_home(research_360_activated)
coord_home = yield self.aruco_future
if coord_home:
id_aruco = coord_home[1]
coord_home = coord_home[0]
r, r_theta = self.get_polar_coord(coord_home)
if id_aruco == 128 or (id_aruco == 448 and r > 0.5):
id_aruco = 128
self.aruco_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(r_theta),
MOVE_CONFIG_LOW_SPEED,
_async=True)
yield self.aruco_future
if self.aruco_future.value():
self.aruco_future = self.services.ALMotion.moveTo(
r, 0, 0, MOVE_CONFIG_LOW_SPEED, _async=True)
yield self.aruco_future
if self.aruco_future.value():
on_home = True
self.aruco_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(coord_home[-1] - r_theta),
MOVE_CONFIG_LOW_SPEED,
_async=True)
yield self.aruco_future
else:
on_home = True
else:
# can't find aruco
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"no_target")
if on_home:
if id_aruco == 128:
self.aruco_future = self.find_home()
coord_home = yield self.aruco_future
if coord_home:
id_aruco = coord_home[1]
coord_home = coord_home[0]
if coord_home and id_aruco == 448:
x, y, theta = coord_home
self.aruco_future = self.last_move(x, y, theta)
yield self.aruco_future
if not self.aruco_future.value():
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"obstacle")
yield stk.coroutines.Return(self.aruco_future.value())
else:
# can't find aruco
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"no_target")
if self.services.DXHomeFinder:
if self.services.DXHomeFinder.reason_cant_go_home.value(
) == "unknown":
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"obstacle")
yield stk.coroutines.Return(False)
def is_detected(self, value):
if 1 <= value[6] <= 6:
print "detection"
print "Landmark " + str(value[6]) + " is detected!"
self.memory_service.insertData("priority", 5)
# Calculate the relative pose to the robot frame
current_global_goal = self.memory_service.getData("goal")
#print "current_global_goal: ",current_global_goal
landmark_global_pose = self.landmark_pose.get_pose(value[6])
#print "landmark_global_pose: ",landmark_global_pose
currVel = self.motion_service.getRobotVelocity()
#print "currVel",currVel
#print "current_global_goal",current_global_goal
current_global_goal_2 = [
a - b * 1.2 for a, b in zip(current_global_goal, currVel)
]
current_global_goal = current_global_goal_2
name = 'CameraBottom'
frame = 2
use_sensor_values = True
camera_pose = self.motion_service.getPosition(
name, frame, use_sensor_values)
temp = math.sqrt(
math.pow(value[1] * 0.001, 2) + math.pow(value[2] * 0.001, 2))
x_offset = math.sqrt(
math.pow(temp, 2) -
math.pow(camera_pose[2], 2)) + camera_pose[0]
y_offset = -value[0] * 0.001
theta_offset = -value[5]
rotation_matrix = self.operation.rotation_matrix(
(landmark_global_pose[2] - theta_offset))
transform = np.dot(
rotation_matrix,
np.array([[x_offset], [y_offset], [theta_offset]]))
current_robot_pose = [
landmark_global_pose[0] - transform[0][0],
landmark_global_pose[1] - transform[1][0],
self.operation.normalize_angle(landmark_global_pose[2] -
transform[2][0])
]
results = self.operation.global_to_local(current_global_goal[0],
current_global_goal[1],
current_global_goal[2],
current_robot_pose[0],
current_robot_pose[1],
current_robot_pose[2])
# Adjust movement
adjust_config = velocity_adjustment(results)
init_pose = almath.Pose2D(
self.motion_service.getRobotPosition(True))
target_distance = almath.Pose2D(results[0], results[1], results[2])
expected_end_pose = init_pose * target_distance
self.motion_service.moveTo(float(results[0]), float(results[1]),
float(results[2]), adjust_config)
# Check if reach the goal
real_end_pose = almath.Pose2D(
self.motion_service.getRobotPosition(True))
position_error = real_end_pose.diff(expected_end_pose)
position_error.theta = almath.modulo2PI(position_error.theta)
if abs(position_error.x) < linear_error \
and abs(position_error.y) < linear_error \
and abs(position_error.theta) < angular_error:
print "Arrived"
self.memory_service.insertData("state", "finished")
else:
print "Not arrived"
def return_home(self, *args, **kwargs):
on_home = False
self.navigation_future = self.find_home()
coord_home = yield self.navigation_future
if coord_home:
r, r_theta = self.get_polar_coord(coord_home)
if r > 0.3:
self.services.ALMotion.angleInterpolation(["HeadYaw", "HeadPitch"],
[0, -0.26],
2,
1,
_async=True)
self.navigation_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(
r_theta),
_async=True)
yield self.navigation_future
if self.navigation_future.value():
move_object = move.Move(self.session, self.logger, r, 1.5)
self.navigation_future = move_object.run()
yield self.navigation_future
if self.navigation_future.value():
on_home = True
robot_pose = almath.Pose2D(coord_home)
center = almath.Pose2D(0.0, 0.0, 0.0)
poseDiff = robot_pose.diff(center)
self.aruco_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(
poseDiff.theta - r_theta),
_async=True)
yield self.aruco_future
else:
on_home = True
else:
# can't find slam position
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue("no_target")
if on_home:
self.navigation_future = self.find_home()
coord_home = yield self.navigation_future
if coord_home:
robot_pose = almath.Pose2D(coord_home)
center = almath.Pose2D(0.0, 0.0, 0.0)
poseDiff = robot_pose.diff(center)
self.navigation_future = self.last_move(
poseDiff.x,
poseDiff.y,
poseDiff.theta)
yield self.navigation_future
if not self.navigation_future.value():
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue("obstacle")
yield stk.coroutines.Return(self.navigation_future.value())
else:
# can't find slam position
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue("no_target")
if self.services.DXHomeFinder:
if self.services.DXHomeFinder.reason_cant_go_home.value() == "unknown":
self.services.DXHomeFinder.reason_cant_go_home.setValue("obstacle")
yield stk.coroutines.Return(False)
def return_home(self, research_360_activated=False):
on_home = False
if self.need_approach_home() and not research_360_activated:
self.pod_future = self.approach_home()
yield self.pod_future
if not self.pod_future.value():
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"obstacle")
yield stk.coroutines.Return(False)
self.pod_future = self.find_home(research_360_activated)
coord_home = yield self.pod_future
if coord_home:
r, r_theta = self.get_polar_coord(coord_home)
if r > 0.5:
x, y, theta = coord_home
x += 0.3 * math.cos(theta)
y += 0.3 * math.sin(theta)
r, r_theta = self.get_polar_coord([x, y, theta])
self.pod_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(r_theta),
MOVE_CONFIG_LOW_SPEED,
_async=True)
yield self.pod_future
if self.pod_future.value():
self.pod_future = self.services.ALMotion.moveTo(
r, 0, 0, MOVE_CONFIG_LOW_SPEED, _async=True)
yield self.pod_future
if self.pod_future.value():
on_home = True
self.pod_future = self.services.ALMotion.moveTo(
0,
0,
almath.modulo2PI(coord_home[-1] - r_theta +
math.pi),
MOVE_CONFIG_LOW_SPEED,
_async=True)
yield self.pod_future
coord_home = None
else:
on_home = True
else:
# can't find pod
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"no_target")
if on_home:
if not coord_home:
self.pod_future = self.find_home()
coord_home = yield self.pod_future
if coord_home:
x, y, theta = coord_home
self.pod_future = self.last_move(x, y, theta)
yield self.pod_future
if not self.pod_future.value():
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"obstacle")
yield stk.coroutines.Return(self.pod_future.value())
else:
# can't find pod
if self.services.DXHomeFinder:
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"no_target")
if self.services.DXHomeFinder:
if self.services.DXHomeFinder.reason_cant_go_home.value(
) == "unknown":
self.services.DXHomeFinder.reason_cant_go_home.setValue(
"obstacle")
yield stk.coroutines.Return(False)
def run(self):
print "qaaaaaaaaaaa"
while True:
print "bbbbbbbbbbbbbbbbb"
time.sleep(0.2)
try:
robotPose = self.navigation.getRobotPositionInMap()
print self.currentTarget
currentPose = almath.Pose2D(robotPose[0][0], robotPose[0][1],
robotPose[0][2])
if (self.currentTarget != self.init):
target = almath.Pose2D(currentPose.x, currentPose.y,
self.angle)
currentPoseToTarget = currentPose.inverse() * target
thetaModulo2PI = almath.modulo2PI(
currentPoseToTarget.theta)
future = self.motion.moveTo(0,
0,
thetaModulo2PI,
_async=True)
self.motion.angleInterpolationWithSpeed(["HeadPitch"], [0],
0.2,
_async=True)
future.wait()
else:
target = almath.Pose2D(currentPose.x, currentPose.y,
self.initAngle)
currentPoseToTarget = currentPose.inverse() * target
thetaModulo2PI = almath.modulo2PI(
currentPoseToTarget.theta)
future = self.motion.moveTo(0,
0,
thetaModulo2PI,
_async=True)
self.motion.angleInterpolationWithSpeed(["HeadPitch"], [0],
0.2,
_async=True)
future.wait()
if self.currentTarget == self.firstTarget:
#self.anime.say("This is the Item numero one, I can tell you a lot of informations about it!")
self.anime.say("one ")
time.sleep(self.tsec)
self.currentTarget = self.secondTarget
elif self.currentTarget == self.init:
# self.anime.say("Do you want to know have more informations about our items?")
self.anime.say("init")
time.sleep(self.tsec)
self.currentTarget = self.firstTarget
elif self.currentTarget == self.secondTarget:
#self.anime.say("This is the item numero two, it is really interesting for a lot of reasons!")
self.anime.say("two")
time.sleep(self.tsec)
self.currentTarget = self.thirdTarget
elif self.currentTarget == self.thirdTarget:
#self.anime.say("This is the item numero three, what an interesting item!")
self.anime.say("three")
time.sleep(self.tsec)
self.currentTarget = self.init
self.moveFunction(self.currentTarget)
except Exception, e:
print "Error :"
print str(e)
raise
except (KeyboardInterrupt, SystemExit):
print '\nkeyboardinterrupt found'
break