head_pose_filter.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('head_pose_estimation')
00003 import rospy
00004 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion 
00005 from collections import deque
00006 import numpy as np
00007 
00008 def pose_quat_to_euler(pose_msg):
00009         return np.concatenate([
00010                 [
00011                         pose_msg.pose.position.x,
00012                         pose_msg.pose.position.y,
00013                         pose_msg.pose.position.z
00014                 ],
00015                 euler_from_quaternion([
00016                         pose_msg.pose.orientation.x,
00017                         pose_msg.pose.orientation.y,
00018                         pose_msg.pose.orientation.z,
00019                         pose_msg.pose.orientation.w
00020                 ])])
00021         
00022 def pose_euler_to_quat(pose):
00023         return Pose(
00024                 Point(*pose[:3]),
00025                 Quaternion(*(quaternion_from_euler(*pose[3:])))
00026         )
00027 
00028 
00029 
00030 poses = None
00031 pose_pub = None
00032 
00033 def pose_sub(pose_msg):
00034         poses.append(pose_quat_to_euler(pose_msg))
00035                                  
00036         if len(poses) == poses.maxlen:
00037                 filtered = np.median(np.array(poses), 0)
00038                 pose_pub.publish(
00039                         PoseStamped(
00040                                 header = pose_msg.header,
00041                                 pose   = pose_euler_to_quat(filtered)
00042                         )
00043                 )
00044 
00045 
00046 if __name__ == '__main__':
00047         rospy.init_node('head_pose_filter')
00048         
00049         window_size = rospy.get_param("~window_size", 3)
00050         rospy.loginfo("window size is: %s" % window_size)
00051         poses = deque([], window_size)
00052         rospy.Subscriber('head_pose', PoseStamped, pose_sub)
00053         pose_pub = rospy.Publisher('head_pose_filtered', PoseStamped)
00054         rospy.spin()


head_pose_estimation
Author(s): Dan Lazewatsky
autogenerated on Thu Feb 11 2016 23:06:59