11 import roslib;roslib.load_manifest(PKG)
14 from sensor_msgs.msg
import Imu, CameraInfo, PointCloud2
18 from pcl_msgs.msg
import ModelCoefficients
19 from std_msgs.msg
import Header
22 ax, ay, az = imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z
25 dx, dy = [-az, 0, ax], [0, -az, ay]
26 elif np.abs(ay) > 0.1:
27 dx, dy = [-ay, ax, 0], [0, az, -ay]
29 dx, dy = [ay, -ax, 0], [az, 0, -az]
30 PStamped = PolygonStamped()
31 for x, y
in [[-10, -10], [-10, 10], [10, 10], [10, -10]]:
32 PStamped.polygon.points.append(Point32(x*dx[0]+y*dy[0],x*dx[1]+y*dy[1],x*dx[2]+y*dy[2]))
34 PStamped.header = imu.header
35 PArrayPub.publish(PolygonArray(header=imu.header, polygons=[PStamped]))
36 normal_array = np.array([ax, ay, az, 0])
37 normal_array = normal_array / np.linalg.norm(x)
38 MStamped = ModelCoefficients(imu.header, normal_array)
39 MArrayPub.publish(ModelCoefficientsArray(imu.header, [MStamped]))
41 if __name__ ==
"__main__":
42 rospy.init_node(
'calc_imu_plane', anonymous=
True)
43 PArrayPub = rospy.Publisher(
'polygon_array', PolygonArray, queue_size=1)
44 MArrayPub = rospy.Publisher(
45 'model_coefficients_array', ModelCoefficientsArray, queue_size=1)
46 rospy.Subscriber(
"imu_data", Imu, imu_cb)