estirabot_common.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('estirabot_apps')
import rospy
import smach
import smach_ros
import threading
import time
import tf
import geometry_msgs
import numpy

from sensor_msgs.msg import PointCloud2
from iri_wam_common_msgs.srv import pose_move,pose_moveRequest,wamInverseKinematics,joints_move
from object_manipulation_msgs.msg import Grasp, PickupGoal
from geometry_msgs.msg import Pose, PoseStamped, Transform, TransformStamped

from pprint import pprint

# return a basic Pose() type
def build_home_pose():
    return build_pose_stamped_msg('/wam_fk/wam0',0.5, 0, 0.13, 0.0, 1.0, 0.0, 0.0)

# Build a PickupGoal message from a given pose
def build_simple_pickup_goal(grasp_joint_state, pre_grasp_joint_state, fingers_config):
    # Adjust finger config un param server
    rospy.set_param('/estirabot/skills/grasp/fingers_configuration', fingers_config)

    g = Grasp()
    g.grasp_posture     = grasp_joint_state
    g.pre_grasp_posture = pre_grasp_joint_state

    r = PickupGoal()
    r.desired_grasps.append(g)

    return r

# TODO: remove this after testing
def build_test_pickup_goal():
    g = Grasp()
    g.pre_grasp_posture.name.append('my_pre_grasp')

    g.pre_grasp_posture.position.append(0.0);
    g.pre_grasp_posture.position.append(0.0);
    g.pre_grasp_posture.position.append(0.0);
    g.pre_grasp_posture.position.append(2.0);
    g.pre_grasp_posture.position.append(0.0);
    g.pre_grasp_posture.position.append(0.0);
    g.pre_grasp_posture.position.append(0.0);

    g.grasp_posture.position.append(0.1);
    g.grasp_posture.position.append(0.0);
    g.grasp_posture.position.append(0.0);
    g.grasp_posture.position.append(2.0);
    g.grasp_posture.position.append(0.0);
    g.grasp_posture.position.append(0.0);
    g.grasp_posture.position.append(0.0);

    r = PickupGoal()
    r.desired_grasps.append(g)

    return r

# Common SMACH States to be used in other app scripts
class EstirabotGoHome(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['success','fail'])

    def execute(self, userdata):
        try:
            return wam_cartesian_move(build_home_pose())
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

        return 'fail'

class EstirabotGetPCL(iri_common_smach.GetPCL):
    pass