def monitored_navigation(self, pose, command):
succeeded = True
goal = MonitoredNavigationGoal()
goal.action_server = command
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.get_rostime()
goal.target_pose.pose = pose
self.goal_reached = False
self.goal_failed = False
self.monNavClient.send_goal(goal)
status = self.monNavClient.get_state()
while (
status == GoalStatus.ACTIVE or status == GoalStatus.PENDING
) and not self.cancelled and not self.goal_reached and not self.goal_failed:
status = self.monNavClient.get_state()
rospy.sleep(rospy.Duration.from_sec(0.05))
if status == GoalStatus.SUCCEEDED or self.goal_reached:
succeeded = True
else:
succeeded = False #preempted nav in order to send a new goal, i.e., from the topo nav pov the navigation succeeded
return (succeeded, status)
def monitored_navigation(self, pose, command):
result = True
goal = MonitoredNavigationGoal()
goal.action_server = command
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.get_rostime()
goal.target_pose.pose = pose
self.goal_reached = False
self.goal_failed = False
self.monNavClient.send_goal(goal)
status = self.monNavClient.get_state()
while (
status == GoalStatus.ACTIVE or status == GoalStatus.PENDING
) and not self.cancelled and not self.goal_reached and not self.goal_failed:
status = self.monNavClient.get_state()
rospy.sleep(rospy.Duration.from_sec(0.05))
if status != GoalStatus.SUCCEEDED:
if not self.goal_reached:
result = False
else:
result = True
else:
result = True
return result
def monitored_navigation(self, pose, command):
result = True
goal=MonitoredNavigationGoal()
goal.action_server=command
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.get_rostime()
goal.target_pose.pose = pose
self.goal_reached=False
self.goal_failed=False
self.monNavClient.send_goal(goal)
status=self.monNavClient.get_state()
while (status == GoalStatus.ACTIVE or status == GoalStatus.PENDING) and not self.cancelled and not self.goal_reached and not self.goal_failed :
status=self.monNavClient.get_state()
rospy.sleep(rospy.Duration.from_sec(0.1))
if status != GoalStatus.SUCCEEDED :
if not self.goal_reached:
result = False
else:
result = True
else :
result = True
return result
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
rospy.loginfo('Reseting preempt...')
rospy.loginfo('Enter wandering mode... ' +
str(self.preempt_requested()))
while not self.preempt_requested():
nav_goals = self.nav_goals(1, 0.7, self.polygon)
client = actionlib.SimpleActionClient(
'monitored_navigation', MonitoredNavigationAction)
client.wait_for_server(rospy.Duration(60))
monav_goal = MonitoredNavigationGoal()
monav_goal.target_pose.header.frame_id = 'map'
monav_goal.target_pose.header.stamp = rospy.Time.now()
monav_goal.action_server = 'move_base'
if self.mode == 'normal':
monav_goal.target_pose.pose = nav_goals.goals.poses[0]
client.send_goal(monav_goal)
elif point_inside_poly(self.wait_point.position, self.points):
monav_goal.target_pose.pose = self.wait_point
client.send_goal(monav_goal)
client.wait_for_result(rospy.Duration(10))
while not self.preempt_requested() and \
client.get_state() == GoalStatus.ACTIVE:
rospy.sleep(rospy.Duration(0.5))
client.cancel_goal()
rospy.loginfo('Waiting for move base...')
return 'succeeded'
def execute_cb(self, goal):
# helper variables
print goal.go_to_person
# goal.timeout is how long to run this behaviour for
# self._timeout is how long to extend behaviour for on
self._timeout_after = rospy.get_rostime() + rospy.Duration(
goal.timeout)
if goal.go_to_person:
print 'going to person'
# prevent interruption
self._is_in_active_time = True
self.send_feedback('going to person')
mon_nav_goal = MonitoredNavigationGoal(action_server='move_base',
target_pose=goal.pose)
self._mon_nav_client.send_goal(mon_nav_goal)
print "CREATING GAZE"
gaze_dir_goal = GazeAtPoseGoal(
topic_name='/info_terminal/gaze_pose', runtime_sec=0)
print "SENDING GOAL"
self.gaze_act_client.send_goal(gaze_dir_goal)
print "DONE GAZING"
self.gaze_topic_pub.publish(goal.pose)
finished_moving = self._mon_nav_client.wait_for_result(
rospy.Duration(1))
while not finished_moving:
self.gaze_topic_pub.publish(goal.pose)
finished_moving = self._mon_nav_client.wait_for_result(
rospy.Duration(1))
self.send_feedback('Reached the right position')
self.gaze_act_client.cancel_all_goals()
# assume some default activity
self._start_activity_timer()
strands_webserver.client_utils.display_url(0, 'http://localhost:8080')
self.currentPan = -180
self.currentTilt = 0
self.head_command = JointState()
self.head_command.name = ["HeadPan", "HeadTilt"]
self.head_command.position = [self.currentPan, self.currentTilt]
self.pub.publish(self.head_command)
self.send_feedback('Turning the camera to the person...')
#turn head cam to person
self.ptugoal.pan = -180
self.ptugoal.tilt = 20
self.ptuclient.send_goal(self.ptugoal)
self.ptuclient.wait_for_result()
self.send_feedback('camera turned successfully!')
rate = rospy.Rate(1)
# preempt will not be requested while activity is happening
while not rospy.is_shutdown() and not self._as.is_preempt_requested(
) and rospy.get_rostime() < self._timeout_after:
# loop for duration
rate.sleep()
self.exit_as()
def monitored_navigation(self, gpose, command):
inc = 0
result = True
goal=MonitoredNavigationGoal()
goal.action_server=command
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.get_rostime()
goal.target_pose.pose = gpose
self.goal_reached=False
self.monNavClient.send_goal(goal)
status=self.monNavClient.get_state()
while (status == GoalStatus.ACTIVE or status == GoalStatus.PENDING) and not self.cancelled and not self.goal_reached :
status=self.monNavClient.get_state()
rospy.sleep(rospy.Duration.from_sec(0.01))
#rospy.loginfo(str(status))
#print status
res=self.monNavClient.get_result()
# print "--------------RESULT------------"
# print res
# print "--------------RESULT------------"
if status != GoalStatus.SUCCEEDED :
if not self.goal_reached:
result = False
if status is GoalStatus.PREEMPTED:
self.preempted = True
else:
result = True
if not res:
if not result:
inc = 1
else :
inc = 0
else:
if res.recovered is True and res.human_interaction is False :
inc = 1
else :
inc = 0
rospy.sleep(rospy.Duration.from_sec(0.3))
return result, inc
def monitored_navigation(self, gpose, command):
inc = 0
result = True
goal = MonitoredNavigationGoal()
goal.action_server = command
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.get_rostime()
goal.target_pose.pose = gpose
self.goal_reached = False
self.monNavClient.send_goal(goal)
status = self.monNavClient.get_state()
while (status == GoalStatus.ACTIVE or status == GoalStatus.PENDING
) and not self.cancelled and not self.goal_reached:
status = self.monNavClient.get_state()
rospy.sleep(rospy.Duration.from_sec(0.01))
#rospy.loginfo(str(status))
#print status
res = self.monNavClient.get_result()
# print "--------------RESULT------------"
# print res
# print "--------------RESULT------------"
if status != GoalStatus.SUCCEEDED:
if not self.goal_reached:
result = False
if status is GoalStatus.PREEMPTED:
self.preempted = True
else:
result = True
if not res:
if not result:
inc = 1
else:
inc = 0
else:
if res.recovered is True and res.human_interaction is False:
inc = 1
else:
inc = 0
rospy.sleep(rospy.Duration.from_sec(0.3))
return result, inc
def execute(self, userdata):
status = 'aborted'
rospy.loginfo('Enter wandering mode... ')
monav_goal = MonitoredNavigationGoal()
monav_goal.target_pose.header.frame_id = 'map'
monav_goal.target_pose.header.stamp = rospy.Time.now()
monav_goal.action_server = 'move_base'
if self.mode == 'normal':
status, userdata = self.wander_mode(monav_goal, userdata)
elif point_inside_poly(self.wait_point.position, self.points):
status, userdata = self.wait_mode(monav_goal, userdata)
if self.preempt_requested():
self.service_preempt()
status = 'preempted'
self.current_uuid = -1
return status
def goto(self, duration):
"""
Given a viewpoint - send the robot there, and fix the PTU angle
returns: Nav_Failure stats.
"""
inds = range(len(self.possible_poses))
random.shuffle(inds)
# add one more possible pose - for the waypoint pose
inds.append(len(self.possible_poses))
self.possible_poses.append(self.robot_pose)
start = rospy.Time.now()
end = rospy.Time.now()
self.view_info = []
for cnt, ind in enumerate(inds):
"""For all possible viewpoints, try to go to one - if fails, loop."""
if (end - start).secs > duration.secs:
rem_duration = rospy.Duration(0)
return False, rem_duration
if self._as.is_preempt_requested():
rem_duration = rospy.Duration(0)
return False, rem_duration
# Publish ViewPose for visualisation
s = PoseStamped()
s.header.frame_id = "/map"
s.pose = self.possible_poses[ind]
self.pub_viewpose.publish(s)
rospy.loginfo("NAV GoTo: x: %s y: %s.",
self.possible_poses[ind].position.x,
self.possible_poses[ind].position.y)
goal = MonitoredNavigationGoal()
goal.action_server = 'move_base'
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.get_rostime(
) #rospy.Time.now()
goal.target_pose.pose = self.possible_poses[ind]
self.selected_robot_pose = goal.target_pose.pose
self.nav_client.send_goal(goal)
self.nav_client.wait_for_result()
res = self.nav_client.get_result()
try:
result = res.outcome
except AttributeError:
rospy.loginfo("No res from nav client: %s" % res)
result = ""
if result != 'succeeded':
rospy.loginfo("nav goal fail: %s" % result)
end = rospy.Time.now()
continue
else:
rospy.loginfo("Reached nav goal: %s" % result)
obj = self.selected_object_pose
dist_z = abs(
self.ptu_height - obj.position.z -
1.0) # equiv to raising the object 1m off the floor
p = self.possible_poses[ind]
dist = abs(
math.hypot((p.position.x - obj.position.x),
(p.position.y - obj.position.y)))
ptu_tilt = math.degrees(math.atan2(dist_z, dist))
rospy.loginfo("ptu: 175, ptu tilt: %s" % ptu_tilt)
self.set_ptu_state(pan=175, tilt=ptu_tilt)
end = rospy.Time.now()
rem_duration = rospy.Duration(duration.secs -
(end - start).secs)
return True, rem_duration
"""IF NO VIEWS work (even the waypoint?)- try looking on mongo for one that has previously worked?"""
rem_duration = rospy.Duration(duration.secs - (end - start).secs)
return False, rem_duration
def execute(self, userdata):
rospy.loginfo("Entering Follow mode...")
while not hasattr(userdata, 'current_uuid'):
rospy.sleep(rospy.Duration(0.1))
if userdata.current_uuid == -1:
self.old_angle = 0
rospy.sleep(rospy.Duration(0.3))
move_goal = MonitoredNavigationGoal()
move_goal.target_pose.header.frame_id = 'map'
move_goal.target_pose.header.stamp = rospy.Time.now()
move_goal.action_server = 'move_base'
delta_x = userdata.current_pose_tf.position.x - \
userdata.current_robot.position.x
delta_y = userdata.current_pose_tf.position.y - \
userdata.current_robot.position.y
distance = hypot(delta_x, delta_y)
quaternion = [
userdata.current_robot.orientation.x,
userdata.current_robot.orientation.y,
userdata.current_robot.orientation.z,
userdata.current_robot.orientation.w]
robot_angle = euler_from_quaternion(quaternion, 'rxyz')
human_angle = atan(delta_y / delta_x)
if delta_x < 0:
if delta_y > 0:
human_angle = human_angle + 3.14
else:
human_angle = human_angle - 3.14
delta_angle = human_angle - robot_angle[2]
if delta_angle < -3.14:
delta_angle = delta_angle + 6.28
if delta_angle > 3.14:
delta_angle = delta_angle - 6.28
delta_angle_degree = delta_angle / 6.28 * 360
p = Point(userdata.current_pose_tf.position.x,
userdata.current_pose_tf.position.y,
userdata.current_pose_tf.position.z)
if distance <= self.default_distance or \
not point_inside_poly(p, self.points):
move_goal.target_pose.pose = userdata.current_robot
else:
move_goal.target_pose.pose = userdata.current_pose_tf
self.monav_client.send_goal(move_goal)
self.monav_client.wait_for_result(rospy.Duration(1))
if delta_angle_degree >= 20 or delta_angle_degree <= -20:
pan_goal = PtuGotoGoal()
temp = self.old_angle * self.alpha
temp1 = delta_angle_degree * (1 - self.alpha)
pan_goal.pan = temp + temp1
pan_goal.tilt = 0
pan_goal.pan_vel = 50
pan_goal.tilt_vel = 10
self.pan_client.send_goal(pan_goal)
self.pan_client.wait_for_result(rospy.Duration(1))
self.old_angle = pan_goal.pan
userdata.degree_to_go = delta_angle_degree
rospy.loginfo("Robot is in new position...")
if self.preempt_requested():
self.service_preempt()
return 'preempted'
return 'succeeded'
