point_head_test.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('busbots_unloader')
import rospy
import actionlib
from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal
from object_manipulator.convert_functions import *
from geometry_msgs.msg import *

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()