point_head_test.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('pr2_handy_tools')
import rospy
import actionlib
from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal
from object_manipulator.convert_functions import *
from geometry_msgs.msg import *
    
# TODO Would like to take command line arguments or have some kind of input interface...
# For now, this just let's you hard-code a point to look at for debugging purposes.

def point_head():
    client = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)

    # Waits until the action server has started up and started
    # listening for goals.
    client.wait_for_server()

    # Creates a goal to send to the action server.
    point = [0., 0, 0]
    point_frame = '/l_wrist_roll_link'
    axis = [1, 0, 0]
    axis_frame = '/high_def_frame'
    point_head_goal = PointHeadGoal()
    point_head_goal.target = create_point_stamped(point, point_frame)
    point_head_goal.pointing_frame = axis_frame
    point_head_goal.pointing_axis = Vector3(*axis)
    point_head_goal.max_velocity = 1.0

    print"Pointing",axis_frame, pplist(axis), "at", point_frame, pplist(point)
    # Sends the goal to the action server.
    client.send_goal(point_head_goal)

    # Waits for the server to finish performing the action.
    client.wait_for_result()


if __name__ == '__main__':
    rospy.init_node('point_head_test')
    point_head()
    

---

pr2_handy_tools
Author(s): Adam Leeper
autogenerated on Fri Jan 11 09:40:09 2013