107 lines
3.2 KiB
Python
107 lines
3.2 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
|
||
|
|
from math import pi, sin, cos
|
||
|
|
from numpy import array
|
||
|
|
from numpy.linalg import norm
|
||
|
|
|
||
|
|
import rclpy
|
||
|
|
from rclpy.node import Node
|
||
|
|
from std_msgs.msg import Header
|
||
|
|
from vision_msgs.msg import BoundingBox3D, BoundingBox3DArray
|
||
|
|
|
||
|
|
|
||
|
|
def quaternion_about_axis(angle, axis):
|
||
|
|
axis = array(axis)
|
||
|
|
axis = axis / norm(axis)
|
||
|
|
half_angle = angle / 2
|
||
|
|
sine = sin(half_angle)
|
||
|
|
w = cos(half_angle)
|
||
|
|
x, y, z = axis * sine
|
||
|
|
return x, y, z, w
|
||
|
|
|
||
|
|
|
||
|
|
class pub_detection3_d_array(Node):
|
||
|
|
def __init__(self):
|
||
|
|
super().__init__('pub_bounding_box_3_d_array_sample')
|
||
|
|
self.__pub = self.create_publisher(
|
||
|
|
BoundingBox3DArray, "bounding_box_3d_array", 10)
|
||
|
|
self.__timer = self.create_timer(0.1, self.pub_sample)
|
||
|
|
self.__counter = 0
|
||
|
|
self.__header = Header()
|
||
|
|
|
||
|
|
def pub_sample(self):
|
||
|
|
while self.__pub.get_subscription_count() == 0:
|
||
|
|
return
|
||
|
|
self.__header.stamp = self.get_clock().now().to_msg()
|
||
|
|
self.__header.frame_id = 'world'
|
||
|
|
msg = BoundingBox3DArray()
|
||
|
|
msg.header = self.__header
|
||
|
|
bbox = BoundingBox3D()
|
||
|
|
quat = quaternion_about_axis(0, [0, 0, 1])
|
||
|
|
bbox.center.orientation.x = quat[0]
|
||
|
|
bbox.center.orientation.y = quat[1]
|
||
|
|
bbox.center.orientation.z = quat[2]
|
||
|
|
bbox.center.orientation.w = quat[3]
|
||
|
|
bbox.center.position.x = 0.0
|
||
|
|
bbox.center.position.y = 0.55
|
||
|
|
bbox.center.position.z = 0.66
|
||
|
|
bbox.size.x = 0.01
|
||
|
|
bbox.size.y = 0.1
|
||
|
|
bbox.size.z = 0.01
|
||
|
|
msg.boxes.append(bbox)
|
||
|
|
|
||
|
|
bbox = BoundingBox3D()
|
||
|
|
quat = quaternion_about_axis(0, [0, 0, 1])
|
||
|
|
bbox.center.orientation.x = quat[0]
|
||
|
|
bbox.center.orientation.y = quat[1]
|
||
|
|
bbox.center.orientation.z = quat[2]
|
||
|
|
bbox.center.orientation.w = quat[3]
|
||
|
|
bbox.center.position.x = 0.0
|
||
|
|
bbox.center.position.y = 0.6
|
||
|
|
bbox.center.position.z = 0.66
|
||
|
|
bbox.size.x = 0.1
|
||
|
|
bbox.size.y = 0.01
|
||
|
|
bbox.size.z = 0.01
|
||
|
|
msg.boxes.append(bbox)
|
||
|
|
|
||
|
|
bbox = BoundingBox3D()
|
||
|
|
quat = quaternion_about_axis(0, [0, 0, 1])
|
||
|
|
bbox.center.orientation.x = quat[0]
|
||
|
|
bbox.center.orientation.y = quat[1]
|
||
|
|
bbox.center.orientation.z = quat[2]
|
||
|
|
bbox.center.orientation.w = quat[3]
|
||
|
|
bbox.center.position.x = -0.05
|
||
|
|
bbox.center.position.y = 0.62
|
||
|
|
bbox.center.position.z = 0.66
|
||
|
|
bbox.size.x = 0.01
|
||
|
|
bbox.size.y = 0.05
|
||
|
|
bbox.size.z = 0.01
|
||
|
|
msg.boxes.append(bbox)
|
||
|
|
|
||
|
|
bbox = BoundingBox3D()
|
||
|
|
quat = quaternion_about_axis(0, [0, 0, 1])
|
||
|
|
bbox.center.orientation.x = quat[0]
|
||
|
|
bbox.center.orientation.y = quat[1]
|
||
|
|
bbox.center.orientation.z = quat[2]
|
||
|
|
bbox.center.orientation.w = quat[3]
|
||
|
|
bbox.center.position.x = 0.05
|
||
|
|
bbox.center.position.y = 0.62
|
||
|
|
bbox.center.position.z = 0.66
|
||
|
|
bbox.size.x = 0.01
|
||
|
|
bbox.size.y = 0.05
|
||
|
|
bbox.size.z = 0.01
|
||
|
|
msg.boxes.append(bbox)
|
||
|
|
|
||
|
|
self.__pub.publish(msg)
|
||
|
|
|
||
|
|
|
||
|
|
def main(args=None):
|
||
|
|
rclpy.init(args=args)
|
||
|
|
node = pub_detection3_d_array()
|
||
|
|
rclpy.spin(node)
|
||
|
|
node.destroy_node()
|
||
|
|
rclpy.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main()
|