calculate_polygon_from_imu.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import numpy as np
4 PKG='jsk_pcl_ros'
5 
6 import imp
7 ## import message_filters
8 try:
9  imp.find_module(PKG)
10 except:
11  import roslib;roslib.load_manifest(PKG)
12 
13 import math
14 from sensor_msgs.msg import Imu, CameraInfo, PointCloud2
15 from jsk_recognition_msgs.msg import PolygonArray
16 from jsk_recognition_msgs.msg import ModelCoefficientsArray
17 from geometry_msgs.msg import *
18 from pcl_msgs.msg import ModelCoefficients
19 from std_msgs.msg import Header
20 
21 def imu_cb(imu):
22  ax, ay, az = imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z
23  # rospy.loginfo("%f %f %f" % (ax, ay, az))
24  if np.abs(az) > 0.1:
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]
28  else:
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]))
33 
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]))
40 
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)
47  rospy.spin()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46