door.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('door_perception')
import rospy
import numpy
import tf
import tf.transformations as tfm
import sys

from articulation_msgs.msg import *
from articulation_msgs.srv import *
from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
from sensor_msgs.msg import ChannelFloat32
from std_srvs.srv import Empty, EmptyResponse
from door_perception.srv import DoorSrvResponse, DoorSrv
from door_perception.msg import Door 

def door_from_articulation_model(model):
  new_door = Door()
  isdoor = False
  for p in model.params:
    if p.name == "rot_center.x":
      new_door.axis.position.x = p.value
      isdoor = True
    if p.name == "rot_center.y":
      new_door.axis.position.y = p.value
      isdoor = True
    if p.name == "rot_center.z":
      new_door.axis.position.z = p.value
      isdoor = True
    if p.name == "rot_axis.x":
      new_door.axis.orientation.x = p.value
      isdoor = True
    if p.name == "rot_axis.y":
      new_door.axis.orientation.y = p.value
      isdoor = True
    if p.name == "rot_axis.z":
      new_door.axis.orientation.z = p.value
      isdoor = True
    if p.name == "rot_axis.w":
      new_door.axis.orientation.w = p.value
      isdoor = True
    if p.name == "rot_radius":
      new_door.radius = p.value
      isdoor = True
    if p.name == "rot_orientation.x":
      pass
    if p.name == "rot_orientation.y":
      pass
    if p.name == "rot_orientation.z":
      pass
    if p.name == "rot_orientation.w":
      pass
    if p.name == "rot_mode":
      pass
  if isdoor:
    new_door.header = model.header #to copy frame_id
  return new_door

def load_model(filename):
  result = ModelMsg()
  with open(filename, "r") as file:
    mystring = file.read()
    result.deserialize(mystring)
    rospy.loginfo("Deserialized Previous Model from " + filename)
  return result

def save_model(model_msg, filename):
    with open(filename, "w") as file:
      model_msg.serialize(file)

def set_model_prior(model):
  try:
    rospy.logdebug(str(model.params))
    model_prior_set = rospy.ServiceProxy('model_prior_set', SetModelPriorSrv)
    rospy.wait_for_service('model_prior_set', 10)
    request = SetModelPriorSrvRequest()
    request.model = [model]
    response = model_prior_set(request)
  except rospy.ServiceException, e:
    print "Service did not process request: %s"%str(e)
  except rospy.ROSException, e:
    print "Service did not come up: %s"%str(e)


##############################################################
class DoorNode:
  def __init__(self):
    self.model = ModelMsg()
    self.door = Door()
    self.setup_ros_communication()
    #LOADING
    self.model_file = rospy.get_param('~model_file', "")
    self.fixed_frame = rospy.get_param('~fixed_frame', "/odom_combined")
    self.load_door_model(self.model_file)
  

  def load_door_model_callback(self, request):
    filename = "model.msg"
    self.load_door_model(filename)
    return EmptyResponse()

  def load_door_model(self, filename):
    try:
      loaded_model = load_model(filename)
      if len(loaded_model.track.pose) > 0:
        self.model = loaded_model
        self.model.header.stamp = rospy.get_rostime()
        self.model.id = 0
        rospy.loginfo("Setting Model")
        set_model_prior(self.model)
        self.pose_callback(None)
    except IOError as (errno, strerror):
      rospy.logwarn("I/O error({0}): {1} '".format(errno, strerror) + self.model_file +"'") 

    #END LOADING

  def save_door_model_callback(self, request):
    filename = "model.msg"
    self.save_door_model(filename)
    return EmptyResponse()

  def save_door_model(self, filename):
    if(filename == ''):
      return
    rospy.loginfo("Saving model to " + filename)
    save_model(self.model, filename)

  def pose_callback(self, pose_stamped):
    rospy.loginfo("Pose arrived")

    try:
      request = TrackModelSrvRequest()
      if pose_stamped != None:
        pose_stamped.header.frame_id = pose_stamped.header.frame_id
        ff_pose_stamped = self.tfros.transformPose(self.fixed_frame, pose_stamped)

        #Add pose to Track
        self.model.track.header = ff_pose_stamped.header
        self.model.track.pose.append( ff_pose_stamped.pose )
        self.model.track.pose_headers.append( ff_pose_stamped.header )

      try:      
          #Request a model from articulatin stack
          request.model = self.model
          response = self.model_select(request)

          #Extract stats
          loglikelihood = [entry.value for entry in response.model.params if entry.name=='loglikelihood'][0]
          q_min = [entry.value for entry in response.model.params if entry.name=='q_min[0]'][0]
          q_max = [entry.value for entry in response.model.params if entry.name=='q_max[0]'][0]
          rospy.loginfo("selected model: '%s' (n = %d, log LH = %f)" % (
              response.model.name, len(response.model.track.pose), loglikelihood))
          rospy.loginfo("Angular range: %f - %f" % (q_min, q_max))

          #If what we expect, process it further
          if response.model.name == 'rotational' and loglikelihood < 0.0: 
            self.model_pub.publish(response.model)
  
            #extract door specific information
            #Rotate the coordinate frame, s.t. the X axis points horizontally along the door
            door_raw = door_from_articulation_model(response.model)
            axis = PoseStamped()
            axis.pose = door_raw.axis
            axis.header = door_raw.header
            self.door_axis_pub2.publish(axis)

            q = door_raw.axis.orientation
            #if(q.z > 0.0):
            rot_to_min_q = tfm.quaternion_about_axis(-q_min, (0,0,1))
            zx_aligned = tfm.quaternion_multiply((q.x, q.y, q.z, q.w), rot_to_min_q)
            door_orientation = zx_aligned
            #align_y = tfm.quaternion_about_axis(-numpy.pi/2.0, (0,0,1))
            #door_orientation = tfm.quaternion_multiply(align_y, zx_aligned)
            #else:
            #  rot_to_min_q = tfm.quaternion_about_axis(q_min, (0,0,1))
            #  x_aligned = tfm.quaternion_multiply((q.x, q.y, q.z, q.w), rot_to_min_q)
            #  #flip_z_axis_up = tfm.quaternion_about_axis(numpy.pi, (1,0,0))
            #  #zx_aligned = tfm.quaternion_multiply(flip_z_axis_up, x_aligned)
            #  door_orientation = x_aligned
            #  #align_y = tfm.quaternion_about_axis(-numpy.pi/2.0, (0,0,1))
            #  #door_orientation = tfm.quaternion_multiply(align_y, zx_aligned)
            door_raw.axis.orientation.x = door_orientation[0]
            door_raw.axis.orientation.y = door_orientation[1]
            door_raw.axis.orientation.z = door_orientation[2]
            door_raw.axis.orientation.w = door_orientation[3]
            self.door = door_raw
            self.publish_door()

            t = door_raw.axis.position
            handle2axis = tfm.translation_matrix((0, door_raw.radius, 0))
            axis2fixed  = tfm.quaternion_matrix(door_orientation)
            axis2fixed[3,:] =(t.x, t.y, t.z, 1.0) 
            handle2fixed  = numpy.dot(axis2fixed , handle2axis)


            #rmat_q = tfm.rotation_matrix(q_min, (0,0,1))
            #handle
          #if response.model.name == 'rotational' and loglikelihood < -100.0: 
            #self.model.track.pose.pop()# =  self.model.track.pose[0:21]
            #self.model.track.pose_headers.pop()# =  self.model.track.pose_headers[0:20]
            #self.pose_subscriber.unregister()
          #self.model = response.model

      except rospy.ServiceException: 
          print "model selection failed"
          pass

    except tf.ExtrapolationException:
      rospy.loginfo("Skipped Pose, Transform from %s to %s not available"%(pose_stamped.header.frame_id, self.fixed_frame))

  def publish_door(self):
      #publish
      axis = PoseStamped()
      axis.pose = self.door.axis
      axis.header = self.door.header
      self.door_axis_pub.publish(axis)
    
  def door_service_callback(self, request):
    doors_response = DoorSrvResponse()
    doors_response.doors.append(self.door)
    return self.doors_response

  def setup_ros_communication(self):
      self.tfros = tf.TransformListener()
      self.pose_subscriber = rospy.Subscriber("pose0", PoseStamped, self.pose_callback)

      rospy.wait_for_service('model_select')
      self.model_select = rospy.ServiceProxy('model_select', TrackModelSrv)
      self.model_pub = rospy.Publisher('model', ModelMsg)
      self.door_axis_pub = rospy.Publisher('door_axis',PoseStamped)
      self.door_axis_pub2 = rospy.Publisher('door_axis2',PoseStamped)
      self.service = rospy.Service('door_description', DoorSrv, self.door_service_callback)
      self.service = rospy.Service('save_door_model', Empty, self.save_door_model_callback)
      self.service = rospy.Service('load_door_model', Empty, self.load_door_model_callback)
 

if __name__ == '__main__':
  rospy.init_node('door_fitting')
  dn = DoorNode();

  #Process until the user kills the node
  r = rospy.Rate(10)
  while not rospy.is_shutdown():
    dn.publish_door()

  #dn.save_door_model_callback(Empty())