From 9f26709ca4eeb84952372bbab2a4059e124a0c48 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Wed, 8 Dec 2021 12:39:00 +0100
Subject: [PATCH] Add collision object for the support

---
 scripts/hw_node.py | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/scripts/hw_node.py b/scripts/hw_node.py
index 7f6c6ef..d4c7d2c 100755
--- a/scripts/hw_node.py
+++ b/scripts/hw_node.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 
 from controller_manager_msgs.srv import *
+import geometry_msgs.msg
 import numpy as np
 import rospy
 
@@ -8,6 +9,7 @@ from active_grasp.bbox import AABBox, to_bbox_msg
 from active_grasp.rviz import Visualizer
 from active_grasp.srv import *
 from robot_helpers.io import load_yaml
+from robot_helpers.ros.conversions import to_pose_msg
 from robot_helpers.ros.moveit import MoveItClient
 from robot_helpers.ros.panda import PandaGripperClient
 from robot_helpers.spatial import Transform
@@ -31,6 +33,7 @@ class HwNode:
             "controller_manager/switch_controller", SwitchController
         )
         self.moveit = MoveItClient("panda_arm")
+        rospy.Timer(rospy.Duration(1), self.publish_table_co)
 
     def init_visualizer(self):
         self.vis = Visualizer()
@@ -75,6 +78,12 @@ class HwNode:
         _, bbox = self.load_config()
         self.vis.bbox("panda_link0", bbox)
 
+    def publish_table_co(self, event):
+        msg = geometry_msgs.msg.PoseStamped()
+        msg.header.frame_id = "panda_link0"
+        msg.pose = to_pose_msg(self.T_base_roi * Transform.t([0.15, 0.15, 0.005]))
+        self.moveit.scene.add_box("table", msg, size=(0.8, 0.8, 0.01))
+
 
 def main():
     rospy.init_node("hw")