approach_pose_finder.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #***********************************************************
00003 #* Software License Agreement (BSD License)
00004 #*
00005 #*  Copyright (c) 2009, Willow Garage, Inc.
00006 #*  All rights reserved.
00007 #*
00008 #*  Redistribution and use in source and binary forms, with or without
00009 #*  modification, are permitted provided that the following conditions
00010 #*  are met:
00011 #*
00012 #*   * Redistributions of source code must retain the above copyright
00013 #*     notice, this list of conditions and the following disclaimer.
00014 #*   * Redistributions in binary form must reproduce the above
00015 #*     copyright notice, this list of conditions and the following
00016 #*     disclaimer in the documentation and/or other materials provided
00017 #*     with the distribution.
00018 #*   * Neither the name of Willow Garage, Inc. nor the names of its
00019 #*     contributors may be used to endorse or promote products derived
00020 #*     from this software without specific prior written permission.
00021 #*
00022 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #*  POSSIBILITY OF SUCH DAMAGE.
00034 #* 
00035 #* Author: Eitan Marder-Eppstein
00036 #***********************************************************
00037 PKG = 'approach_table_tools' # this package name
00038 NAME = 'find_approach_pose'
00039 
00040 import roslib; roslib.load_manifest(PKG) 
00041 import rospy 
00042 import tf
00043 import numpy
00044 import math
00045 
00046 from geometry_msgs.msg import Quaternion, Pose
00047 from approach_table_tools.srv import GetApproachPose, GetApproachPoseResponse
00048 
00049 def msg_to_quaternion(msg):
00050   return [msg.x, msg.y, msg.z, msg.w]
00051 
00052 def quaternion_to_msg(q):
00053   msg = Quaternion()
00054   msg.x = q[0]
00055   msg.y = q[1]
00056   msg.z = q[2]
00057   msg.w = q[3]
00058   return msg
00059 
00060 def msg_to_pose(msg):
00061   trans = tf.transformations.quaternion_matrix(msg_to_quaternion(msg.orientation))
00062   trans[0, 3] = msg.position.x
00063   trans[1, 3] = msg.position.y
00064   trans[2, 3] = msg.position.z
00065   trans[3, 3] = 1.0
00066   return trans
00067 
00068 def pose_to_msg(pose):
00069   msg = Pose()
00070   msg.position.x = pose[0, 3]
00071   msg.position.y = pose[1, 3]
00072   msg.position.z = pose[2, 3]
00073   q = tf.transformations.quaternion_from_matrix(pose)
00074   msg.orientation.x = q[0]
00075   msg.orientation.y = q[1]
00076   msg.orientation.z = q[2]
00077   msg.orientation.w = q[3]
00078   return msg
00079 
00080 class ApproachPoseFinder:
00081   def __init__(self):
00082     self.listener = tf.TransformListener()
00083     self.server = rospy.Service('get_approach_pose', GetApproachPose, self.get_approach_pose)
00084     self.fixed_frame = rospy.get_param('~fixed_frame', 'odom_combined')
00085 
00086   def get_approach_pose(self, req):
00087     board_pose = req.board_pose
00088     #We need to create a goal that's actually reachable for the navigation stack... let's say 0.5 meters away from and centered on the board
00089     trans = msg_to_pose(board_pose.pose)
00090     origin = tf.transformations.translation_matrix([2 * 0.08, 0, -1.0])
00091     pose_mat =  numpy.dot(trans, origin)
00092     board_pose.pose = pose_to_msg(pose_mat)
00093   
00094     #Now that we have our pose, we want to transform it into a frame the navigation stack can move in
00095     #wait for the transform between the camera and odometric frame to be available
00096     self.listener.waitForTransform(self.fixed_frame, board_pose.header.frame_id, board_pose.header.stamp, rospy.Duration(2.0))
00097   
00098     #transform the pose from the camera to odometric frame
00099     odom_pose = self.listener.transformPose(self.fixed_frame, board_pose)
00100   
00101     #project the goal down to the ground
00102     odom_pose.pose.position.z = 0.0
00103   
00104     #we want our goal to point at the board, so we need to rotate it by 90 degrees
00105     rot_q = tf.transformations.quaternion_from_euler(0.0, 0.0, math.pi / 2)
00106     new_q = tf.transformations.quaternion_multiply(rot_q, msg_to_quaternion(odom_pose.pose.orientation))
00107   
00108     #we want to remove any pitch and roll from the goal... so we'll just use the yaw element
00109     roll, pitch, yaw = tf.transformations.euler_from_quaternion(new_q)
00110   
00111     #now, we're ready to construct the quaternion for what we'll send to navigation
00112     odom_pose.pose.orientation = quaternion_to_msg(tf.transformations.quaternion_from_euler(0.0, 0.0, yaw))
00113   
00114     return GetApproachPoseResponse(odom_pose)
00115 
00116 def approach_pose_finder():
00117   rospy.init_node(NAME)
00118   af = ApproachPoseFinder()
00119   rospy.spin()
00120 
00121 if __name__ == "__main__":
00122   approach_pose_finder()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


approach_table_tools
Author(s):
autogenerated on Thu May 23 2013 14:05:20