52 lines
1.2 KiB
Python
Executable File
52 lines
1.2 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
from controller_manager_msgs.srv import *
|
|
import rospy
|
|
|
|
from active_grasp.bbox import AABBox, to_bbox_msg
|
|
from active_grasp.srv import *
|
|
|
|
|
|
from robot_helpers.ros.moveit import MoveItClient
|
|
|
|
|
|
class HwNode:
|
|
def __init__(self):
|
|
self.advertise_services()
|
|
self.switch_controller = rospy.ServiceProxy(
|
|
"controller_manager/switch_controller", SwitchController
|
|
)
|
|
|
|
self.moveit = MoveItClient("panda_arm")
|
|
rospy.spin()
|
|
|
|
def advertise_services(self):
|
|
rospy.Service("seed", Seed, self.seed)
|
|
rospy.Service("reset", Reset, self.reset)
|
|
|
|
def seed(self, req):
|
|
# Nothing to do
|
|
return SeedResponse()
|
|
|
|
def reset(self, req):
|
|
req = SwitchControllerRequest()
|
|
req.start_controllers = ["position_joint_trajectory_controller"]
|
|
req.stop_controllers = ["cartesian_velocity_controller"]
|
|
self.switch_controller(req)
|
|
|
|
# Move to the initial configuration
|
|
self.moveit.goto([0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79])
|
|
|
|
# Detect target
|
|
bbox = AABBox([0.4, -0.1, 0.0], [0.5, 0.1, 0.1])
|
|
return ResetResponse(to_bbox_msg(bbox))
|
|
|
|
|
|
def main():
|
|
rospy.init_node("hw")
|
|
HwNode()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|