jsk_pcl_ros: calculate_polygon_from

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 import numpy as np
00004 PKG='jsk_pcl_ros'
00005 
00006 import imp
00007 ## import message_filters
00008 try:
00009     imp.find_module(PKG)
00010 except:
00011     import roslib;roslib.load_manifest(PKG)
00012 
00013 import math
00014 from sensor_msgs.msg import Imu, CameraInfo, PointCloud2
00015 from jsk_recognition_msgs.msg import PolygonArray
00016 from jsk_recognition_msgs.msg import ModelCoefficientsArray
00017 from geometry_msgs.msg import *
00018 from pcl_msgs.msg import ModelCoefficients
00019 from std_msgs.msg import Header
00020 
00021 def imu_cb(imu):
00022     ax, ay, az = imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z 
00023     # rospy.loginfo("%f %f %f" % (ax, ay, az))
00024     if az > 0.1:
00025         dx, dy = [-az, 0, ax], [0, -az, ay]
00026     elif ay > 0.1:
00027         dx, dy = [-ay, ax, 0], [0, az, -ay]
00028     else:
00029         dx, dy = [ay, -ax, 0], [az, 0, -az]
00030     PStamped = PolygonStamped()
00031     for x, y in [[-10, -10], [-10, 10], [10, 10], [10, -10]]:
00032         PStamped.polygon.points.append(Point32(x*dx[0]+y*dy[0],x*dx[1]+y*dy[1],x*dx[2]+y*dy[2]))
00033 
00034     PStamped.header = imu.header
00035     PArrayPub.publish(PolygonArray(imu.header, [PStamped]))
00036     normal_array = np.array([ax, ay, az, 0])
00037     normal_array = normal_array / np.linalg.norm(x)
00038     MStamped = ModelCoefficients(imu.header, normal_array)
00039     MArrayPub.publish(ModelCoefficientsArray(imu.header, [MStamped]))
00040     
00041 if __name__ == "__main__":
00042     rospy.init_node('calc_imu_plane', anonymous=True)
00043     PArrayPub = rospy.Publisher('polygon_array', PolygonArray)
00044     MArrayPub = rospy.Publisher('model_coefficients_array', ModelCoefficientsArray)
00045     rospy.Subscriber("imu_data", Imu, imu_cb)
00046     rospy.spin()