You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
123 lines
4.4 KiB
123 lines
4.4 KiB
#!/usr/bin/env python3
|
|
|
|
import copy
|
|
import pickle
|
|
import pathlib
|
|
import subprocess
|
|
import numpy as np
|
|
|
|
import rospy
|
|
from std_msgs.msg import Empty, Float32
|
|
from geometry_msgs.msg import PointStamped
|
|
from sensor_msgs.msg import PointCloud2
|
|
import hello_helpers.hello_misc as hm
|
|
|
|
import stretch_pyfunmap.manipulation_planning as mp
|
|
import stretch_pyfunmap.segment_max_height_image as sm
|
|
|
|
|
|
class PlacePointNode(hm.HelloNode):
|
|
|
|
def __init__(self):
|
|
hm.HelloNode.__init__(self)
|
|
self.active = False
|
|
self.update_perception = False
|
|
|
|
self.location_above_surface_m = None
|
|
self.placement_target = None
|
|
|
|
pc_pkl_fpath = str(pathlib.Path(subprocess.Popen("rospack find stretch_demos", shell=True, stdout=subprocess.PIPE).stdout.read().decode()[:-1]) / 'empty_pc.pkl')
|
|
with open(pc_pkl_fpath, 'rb') as inp:
|
|
self.empty_pc = pickle.load(inp)
|
|
|
|
def enable_callback(self, msg):
|
|
self.location_above_surface_m = msg.data
|
|
print(self.location_above_surface_m)
|
|
self.placement_target = None
|
|
self.update_perception = True
|
|
self.active = True
|
|
print(' - place_point enabled')
|
|
|
|
def disable_callback(self, msg):
|
|
self.active = False
|
|
self.update_perception = False
|
|
rospy.sleep(1)
|
|
self.location_above_surface_m = None
|
|
self.placement_target = None
|
|
print(' - place_point disabled')
|
|
|
|
def clicked_point_callback(self, clicked_point_msg):
|
|
self.place_object()
|
|
|
|
def place_point_callback(self, point_msg):
|
|
self.place_object()
|
|
|
|
def place_object(self):
|
|
if self.placement_target is None:
|
|
print('cannot place object on surface because no placement targets are available')
|
|
return
|
|
self.update_perception = False
|
|
|
|
target = copy.copy(self.placement_target)
|
|
self.manip.perform_cartesian_grasp(target, self, tooltip_frame='link_grasp_center', placing=True)
|
|
msg = Empty()
|
|
self.result_pub.publish(msg)
|
|
|
|
self.location_above_surface_m = None
|
|
self.update_perception = False
|
|
self.active = False
|
|
|
|
def perceive_objects(self):
|
|
if not self.update_perception:
|
|
return
|
|
|
|
self.manip.update(self.point_cloud, self.tf2_buffer)
|
|
surface_mask, plane_parameters = sm.find_closest_flat_surface(self.manip.max_height_im, None)
|
|
if surface_mask is None:
|
|
return
|
|
|
|
if self.location_above_surface_m is None:
|
|
print('WARN: cannot find placement target')
|
|
return
|
|
placement_target = sm.find_placement_target_on_surface(surface_mask, self.location_above_surface_m, self.manip.max_height_im)
|
|
if placement_target is None:
|
|
return
|
|
self.placement_target = placement_target
|
|
|
|
self.ready_pub.publish(Empty())
|
|
|
|
def publish_markers(self):
|
|
if self.placement_target is not None:
|
|
image_copy = self.manip.max_height_im.image.copy()
|
|
image_copy[self.placement_target['surface_convex_hull_mask'] <= 0] = 0
|
|
surface_placement_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy)
|
|
self.surface_target_marker_pub.publish(surface_placement_pc)
|
|
else:
|
|
self.surface_target_marker_pub.publish(self.empty_pc)
|
|
|
|
def main(self):
|
|
hm.HelloNode.main(self, 'place_point', 'place_point', wait_for_first_pointcloud=True)
|
|
self.surface_target_marker_pub = rospy.Publisher('/place_point/surface_target_marker', PointCloud2, queue_size=1)
|
|
|
|
self.clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_callback)
|
|
self.trigger_sub = rospy.Subscriber('/place_point/trigger_place_point', PointStamped, self.place_point_callback)
|
|
self.enable_sub = rospy.Subscriber('/place_point/enable', Float32, self.enable_callback)
|
|
self.ready_pub = rospy.Publisher('/place_point/ready', Empty, queue_size=1)
|
|
self.disable_sub = rospy.Subscriber('/place_point/disable', Empty, self.disable_callback)
|
|
self.result_pub = rospy.Publisher('/place_point/result', Empty, queue_size=1)
|
|
|
|
self.manip = mp.ManipulationView(self.tf2_buffer, None, self.get_tool())
|
|
|
|
rate = rospy.Rate(10.0)
|
|
while not rospy.is_shutdown():
|
|
self.perceive_objects()
|
|
self.publish_markers()
|
|
rate.sleep()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
node = PlacePointNode()
|
|
node.main()
|
|
except KeyboardInterrupt:
|
|
pass
|