quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
timer = tc.Wait(duration=45.0, foreground=False)
tc.addCondition(ConditionIsCompleted("Timer condition", tc, timer))
try:
while not finished_home:
en_pub.publish(Bool(True))
goto_pub.publish(pose)
tc.Wait(duration=3)
finally:
en_pub.publish(Bool(False))
gtb = tc.GoToBase(foreground=True)
# w4roi = tc.WaitForFace(foreground=False)
# tc.addCondition(ConditionIsCompleted("Face fund condition", tc, w4roi))
# try:
# tc.Wander(max_linear_speed=0.5)
# except TaskConditionException, e:
# rospy.loginfo("Path following interrupted on condition: %s"
# "or ".join([str(c) for c in e.conditions]))
# tc.TaskStareAtFace(angle_threshold=0.01)
# tc.Wait(duration=3.0)
# timer = tc.Wait(duration=5.0, foreground=False)
# tc.addCondition(ConditionIsCompleted("Timer condition", tc, timer))
# try:
# tc.Wander(max_linear_speed=0.5)
# except TaskConditionException, e:
# pass
