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

Visualize hw bbox

Browse Source
This commit is contained in:
Michel Breyer 2021-12-03 16:51:13 +01:00
parent 7dcb6fb027
commit 542a01de4d
1 changed files with 23 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
scripts/hw_node.py
View File

@ -5,6 +5,7 @@ import numpy as np
import rospy import rospy
from active_grasp.bbox import AABBox, to_bbox_msg from active_grasp.bbox import AABBox, to_bbox_msg
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.moveit import MoveItClient from robot_helpers.ros.moveit import MoveItClient
@ -16,13 +17,13 @@ class HwNode:
def __init__(self): def __init__(self):
self.load_parameters() self.load_parameters()
self.init_robot_connection() self.init_robot_connection()
self.init_visualizer()
self.advertise_services() self.advertise_services()
rospy.spin() rospy.spin()
def load_parameters(self): def load_parameters(self):
cfg = rospy.get_param("hw") self.cfg = rospy.get_param("hw")
self.T_base_roi = Transform.from_matrix(np.loadtxt(cfg["roi_calib_file"])) self.T_base_roi = Transform.from_matrix(np.loadtxt(self.cfg["roi_calib_file"]))
self.scene_config = load_yaml(cfg["scene_file"])
def init_robot_connection(self): def init_robot_connection(self):
self.gripper = PandaGripperClient() self.gripper = PandaGripperClient()
@ -31,6 +32,10 @@ class HwNode:
) )
self.moveit = MoveItClient("panda_arm") self.moveit = MoveItClient("panda_arm")
def init_visualizer(self):
self.vis = Visualizer()
rospy.Timer(rospy.Duration(1), self.draw_bbox)
def advertise_services(self): def advertise_services(self):
rospy.Service("seed", Seed, self.seed) rospy.Service("seed", Seed, self.seed)
rospy.Service("reset", Reset, self.reset) rospy.Service("reset", Reset, self.reset)
@ -40,18 +45,23 @@ class HwNode:
return SeedResponse() return SeedResponse()
def reset(self, req): def reset(self, req):
q0, bbox = self.load_config()
# Move to the initial configuration # Move to the initial configuration
self.switch_to_joint_trajectory_controller() self.switch_to_joint_trajectory_controller()
self.moveit.goto(self.scene_config["q0"]) self.moveit.goto(q0)
self.gripper.move(0.04) self.gripper.move(0.04)
# Construct bounding box
bbox_min = self.T_base_roi.apply(self.scene_config["target"]["min"])
bbox_max = self.T_base_roi.apply(self.scene_config["target"]["max"])
bbox = AABBox(bbox_min, bbox_max)
return ResetResponse(to_bbox_msg(bbox)) return ResetResponse(to_bbox_msg(bbox))
def load_config(self):
scene_config = load_yaml(self.cfg["scene_file"])
q0 = scene_config["q0"]
bbox_min = self.T_base_roi.apply(scene_config["target"]["min"])
bbox_max = self.T_base_roi.apply(scene_config["target"]["max"])
bbox = AABBox(bbox_min, bbox_max)
return q0, bbox
def switch_to_joint_trajectory_controller(self): def switch_to_joint_trajectory_controller(self):
req = SwitchControllerRequest() req = SwitchControllerRequest()
req.start_controllers = ["position_joint_trajectory_controller"] req.start_controllers = ["position_joint_trajectory_controller"]
@ -59,6 +69,10 @@ class HwNode:
req.strictness = 1 req.strictness = 1
self.switch_controller(req) self.switch_controller(req)
def draw_bbox(self, event):
_, bbox = self.load_config()
self.vis.bbox("panda_link0", bbox)
def main(): def main():
rospy.init_node("hw") rospy.init_node("hw")