head_pointer.py

Go to the documentation of this file.

#!/usr/bin/env python
# 
# head_pointer.py
# Mac Mason <mmason@willowgarage.com>
# 
# Make the head point at the currently-published "best plane", or generally
# forward if there isn't one. To keep ourselves from continually looking
# backward, we set a maximum_angle flag, which decides just how far backward
# we're willing to look. (Specifically, it sets how far off of "frontward"
# we're willing to look). 

import sys
import math
import roslib; roslib.load_manifest('semanticmodel')
import rospy
import actionlib
from actionlib_msgs.msg import * 
from pr2_controllers_msgs.msg import * 
from geometry_msgs.msg import * 
import tf
import dynamic_reconfigure.client

# Get all started up.
rospy.init_node('head_pointer')

# An extra command-line argument turns us into a very simple algorithm.
ALWAYS_LOOK_FORWARD = False

client = actionlib.SimpleActionClient(
  '/head_traj_controller/point_head_action', PointHeadAction)
rospy.loginfo("Waiting for actionlib server...")
client.wait_for_server()
rospy.loginfo("...got actionlib server")

listener = tf.TransformListener()

# How far we're willing to turn the head.
LIMIT = math.radians(60.0)

while not rospy.is_shutdown():
  plane = False
  try:
    time = listener.getLatestCommonTime('/map', '/best_plane')
    rospy.logdebug(
        "Delta is {0}.".format((rospy.get_rostime() - time).to_sec()))
    plane = True
  except tf.Exception as e:
    rospy.loginfo("Failed first lookup of tf transform." )
    plane = False

  if plane:
    delta = rospy.get_rostime() - time
    if delta.to_sec() > 5.0:
      rospy.logdebug("Delta of {0} too large.".format(delta.to_sec()))
      plane = False

  # Next: make sure we aren't over-turning the head. We do this by getting the
  # transformation from /base_link (where +x necessarily means "forward") to
  # /best_plane, and then looking at the x and y coordinates of that frame.
  # In theory, I could use dynamic_reconfigure to set this parameter, but it's
  # utterly undocumented, and I'm in a hurry.
  if plane:
    try:
      form = listener.lookupTransform('/base_link', 
                                      '/best_plane', 
                                      rospy.Time())
      ang = math.atan2(form[0][1], form[0][0])
      if ang > LIMIT or ang < -LIMIT:
        rospy.loginfo(
            "Got a good delta ({0}) but {1} is outside the range.".
            format(delta.to_sec(), ang))
        plane = False
    except tf.Exception as e:
      rospy.loginfo("Second lookup failure.")
      plane = False

  if ALWAYS_LOOK_FORWARD:
    rospy.loginfo("Always looking forward.")
    plane = False

  if plane:
    rospy.logdebug("Pointing head {0} degrees.".format(math.degrees(ang)))

  g = PointHeadGoal()
  if plane:
    g.target.header.frame_id = '/best_plane'
    g.target.point.x = 0.0
    g.target.point.y = 0.0
    g.target.point.z = 0.0
    rospy.logdebug("Pointed the head!")
  else:
    g.target.header.frame_id = '/base_link'
    g.target.point.x = 3.0
    g.target.point.y = 0.0
    g.target.point.z = 0.0
    rospy.logdebug("Forwarded the head!")
  g.min_duration = rospy.Duration(1.0)

  client.send_goal(g)
  client.wait_for_result()

  if client.get_state() != GoalStatus.SUCCEEDED:
    rospy.loginfo("Whoa! Head pointing failed!")