hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add collision object for the support

Browse Source
This commit is contained in:
Michel Breyer 2021-12-08 12:39:00 +01:00
parent 47d5840aa6
commit 9f26709ca4
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
scripts/hw_node.py
View File

@ -1,6 +1,7 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from controller_manager_msgs.srv import * from controller_manager_msgs.srv import *
import geometry_msgs.msg
import numpy as np import numpy as np
import rospy import rospy
@ -8,6 +9,7 @@ from active_grasp.bbox import AABBox, to_bbox_msg
from active_grasp.rviz import Visualizer from active_grasp.rviz import Visualizer
from active_grasp.srv import * from active_grasp.srv import *
from robot_helpers.io import load_yaml 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.moveit import MoveItClient
from robot_helpers.ros.panda import PandaGripperClient from robot_helpers.ros.panda import PandaGripperClient
from robot_helpers.spatial import Transform from robot_helpers.spatial import Transform
@ -31,6 +33,7 @@ class HwNode:
"controller_manager/switch_controller", SwitchController "controller_manager/switch_controller", SwitchController
) )
self.moveit = MoveItClient("panda_arm") self.moveit = MoveItClient("panda_arm")
rospy.Timer(rospy.Duration(1), self.publish_table_co)
def init_visualizer(self): def init_visualizer(self):
self.vis = Visualizer() self.vis = Visualizer()
@ -75,6 +78,12 @@ class HwNode:
_, bbox = self.load_config() _, bbox = self.load_config()
self.vis.bbox("panda_link0", bbox) 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(): def main():
rospy.init_node("hw") rospy.init_node("hw")