place_action_test.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('iri_place')
import rospy

# Brings in the SimpleActionClient
import actionlib

#from estirabot_common import EstirabotGoHome, EstirabotGetKinectImage

# Brings in the messages used by the action, including the
# goal message and the result message.
from object_manipulation_msgs.msg import PlaceAction,PlaceGoal
from geometry_msgs.msg import  PoseStamped
from math import sqrt

def place_client():
    # Creates the SimpleActionClient, passing the type of the action
    # (FibonacciAction) to the constructor.
    client = actionlib.SimpleActionClient('/iri_place/place', PlaceAction)

    print "waiting action server"
    # 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.
    goal = PlaceGoal()

    place_location = PoseStamped() 
    place_location.header.frame_id = "/wam_link0"
    place_location.pose.position.x = 0.45;
    place_location.pose.position.y = 0;
    place_location.pose.position.z = 0;
    place_location.pose.orientation.x = sqrt(2)/2;
    place_location.pose.orientation.y = sqrt(2)/2;
    place_location.pose.orientation.z = 0;
    place_location.pose.orientation.w = 0;

    goal.place_locations.append(place_location);

    print "send goal"
    # Sends the goal to the action server.
    client.send_goal(goal)

    print "wait result"
    # Waits for the server to finish performing the action.
    client.wait_for_result()

    print "returning result"
    # Prints out the result of executing the action
    return client.get_result() 

if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('place_check')
        result = place_client()
        print "Result: ",result
    except rospy.ROSInterruptException:
        print "program interrupted before completion"