door_perception_old: door_description.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004   Gets prior from model_learner_prior, serves DoorSrv
00005 """
00006 
00007 import roslib; roslib.load_manifest('door_perception')
00008 import rospy
00009 from articulation_models import track_utils
00010 from articulation_models.track_utils import *
00011 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
00012 from door_perception.srv import *
00013 from door_perception.msg import Door 
00014 import math
00015 
00016 #def normalized_quaternion(x,y,z,theta)
00017 #    norm = math.sqrt(x*x + y*y + z*z)
00018 #    q = Quaternion();
00019 #    q.x = (x/norm) * math.sin(theta/2)
00020 #    q.y = (y/norm) * math.sin(theta/2)
00021 #    q.z = (z/norm) * math.sin(theta/2)
00022 #    q.w = math.cos(theta/2)
00023 #    return q
00024 
00025 def door_from_articulation_models(model):
00026     new_door = Door()
00027     isdoor = False
00028     for p in model.params:
00029       if p.name == "rot_center.x":
00030         new_door.axis.position.x = p.value
00031         isdoor = True
00032       if p.name == "rot_center.y":
00033         new_door.axis.position.y = p.value
00034         isdoor = True
00035       if p.name == "rot_center.z":
00036         new_door.axis.position.z = p.value
00037         isdoor = True
00038       if p.name == "rot_axis.x":
00039           new_door.axis.orientation.x = p.value
00040         isdoor = True
00041       if p.name == "rot_axis.y":
00042           new_door.axis.orientation.y = p.value
00043         isdoor = True
00044       if p.name == "rot_axis.z":
00045           new_door.axis.orientation.z = p.value
00046         isdoor = True
00047       if p.name == "rot_axis.w":
00048           new_door.axis.orientation.w = p.value
00049         isdoor = True
00050       if p.name == "rot_radius":
00051         new_door.radius = p.value
00052         isdoor = True
00053       if p.name == "rot_orientation.x":
00054         pass
00055       if p.name == "rot_orientation.y":
00056         pass
00057       if p.name == "rot_orientation.z":
00058         pass
00059       if p.name == "rot_orientation.w":
00060         pass
00061       if p.name == "rot_mode":
00062         pass
00063     if isdoor:
00064       new_door.header = model.header #to copy frame_id
00065   return doors_resp
00066 
00067 def door_service_callback(request):
00068   doors_response = DoorSrvResponse()
00069   doors_resp.doors.append(global_door)
00070   return doors_response
00071 
00072 
00073 def get_door_descriptions():
00074   try:
00075     model_prior_get = rospy.ServiceProxy('model_prior_get', GetModelPriorSrv)
00076     rospy.wait_for_service('model_prior_get', 10)
00077     request = GetModelPriorSrvRequest()
00078     response = model_prior_get(request)
00079     rospy.loginfo("Got %d prior models"%len(response.model))
00080   except rospy.ServiceException, e:
00081     print "Service did not process request: %s"%str(e)
00082   except rospy.ROSException, e:
00083     print "Service did not come up: %s"%str(e)
00084   return response.model
00085      
00086 
00087 if __name__ == '__main__':
00088   rospy.init_node('door_description_server')
00089   rospy.loginfo("Requesting Door Model");
00090   models = get_door_descriptions()
00091   global global_door
00092   global_door = door_from_articulation_models(models[0])
00093   s = rospy.Service('doors', DoorSrv, door_service_callback)
00094   pub = rospy.Publisher('door_poses', PoseStamped)
00095   rospy.loginfo("Periodically Sending Door Model(s) to <namespace>/door_poses");
00096   while not rospy.is_shutdown():
00097       door_axis_stamped = PoseStamped()
00098       door_axis_stamped.pose = global_door.axis
00099       door_axis_stamped.header = global_door.header
00100       door_axis_stamped.header.stamp = rospy.Time.now()
00101 
00102       pub.publish(door_axis_stamped)
00103       rospy.sleep(1.0)
00104   
00105 
00106