From db3300ee45335149eba61100bb171660c1a8bfff Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Mon, 17 Jul 2023 16:43:17 +0300
Subject: [PATCH] simple grasp marker publisher

---
 .../scripts/grasp_marker_publish.py           | 107 ++++++++++++++++++
 1 file changed, 107 insertions(+)
 create mode 100644 rbs_perception/scripts/grasp_marker_publish.py

diff --git a/rbs_perception/scripts/grasp_marker_publish.py b/rbs_perception/scripts/grasp_marker_publish.py
new file mode 100644
index 0000000..95961d7
--- /dev/null
+++ b/rbs_perception/scripts/grasp_marker_publish.py
@@ -0,0 +1,107 @@
+#!/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()
\ No newline at end of file