diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index bdd1b40..0723abd 100644 --- a/launch/active_grasp.rviz +++ b/launch/active_grasp.rviz @@ -3,9 +3,7 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: - - /Markers1/Namespaces1 - - /TF1/Frames1 + Expanded: ~ Splitter Ratio: 0.49705880880355835 Tree Height: 814 - Class: rviz/Selection @@ -27,7 +25,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: SceneCloud + SyncSource: GraspQuality Preferences: PromptSaveOnExit: true Toolbars: @@ -53,74 +51,149 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /camera/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: ColorImage - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: rviz/Image - Enabled: false - Image Topic: /camera/depth/image_rect_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: DepthImage - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Alpha: 0.05000000074505806 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: distance - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: MapCloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.007499999832361937 - Style: Boxes - Topic: /map_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/MarkerArray + - Class: rviz/TF Enabled: true - Marker Topic: visualization_marker_array - Name: Markers - Namespaces: - bbox: true - best_grasp: true - grasps: true - path: true - rays: true - views: true - workspace: true - Queue Size: 100 + Frame Timeout: 15 + Frames: + All Enabled: false + camera_depth_optical_frame: + Value: true + panda_hand: + Value: true + panda_leftfinger: + Value: false + panda_link0: + Value: true + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false + task: + Value: true + world: + Value: false + Marker Alpha: 1 + Marker Scale: 0.30000001192092896 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + camera_depth_optical_frame: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} + task: + {} + Update Interval: 0 Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true - Acceleration_Scaling_Factor: 0.1 Class: moveit_rviz_plugin/MotionPlanning Enabled: false @@ -357,6 +430,30 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: false Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: ColorImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /camera/depth/image_rect_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: DepthImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -385,116 +482,19 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 138; 226; 52 - Max Intensity: 1 - Min Color: 239; 41; 41 - Min Intensity: 0.800000011920929 - Name: GraspQuality - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.007499999832361937 - Style: Boxes - Topic: /quality - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - Value: false - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false + - Class: rviz/MarkerArray Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 + Marker Topic: visualization_marker_array + Name: Markers + Namespaces: + bbox: true + grasp: true + grasps: true + path: true + views: true + workspace: true + Queue Size: 100 Value: true - Visual Enabled: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -523,68 +523,65 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz/TF + - Alpha: 0.05000000074505806 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: distance + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.007499999832361937 + Style: Boxes + Topic: /map_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - camera_depth_optical_frame: - Value: true - panda_hand: - Value: true - panda_leftfinger: - Value: false - panda_link0: - Value: true - panda_link1: - Value: false - panda_link2: - Value: false - panda_link3: - Value: false - panda_link4: - Value: false - panda_link5: - Value: false - panda_link6: - Value: false - panda_link7: - Value: false - panda_link8: - Value: false - panda_rightfinger: - Value: false - task: - Value: true - world: - Value: false - Marker Alpha: 1 - Marker Scale: 0.30000001192092896 - Name: TF - Show Arrows: false - Show Axes: true - Show Names: false - Tree: - world: - panda_link0: - panda_link1: - panda_link2: - panda_link3: - panda_link4: - panda_link5: - panda_link6: - panda_link7: - panda_link8: - panda_hand: - camera_depth_optical_frame: - {} - panda_leftfinger: - {} - panda_rightfinger: - {} - task: - {} - Update Interval: 0 + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 1 + Min Color: 255; 0; 0 + Min Intensity: 0.5 + Name: GraspQuality + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.007499999832361937 + Style: Boxes + Topic: /quality + Unreliable: false + Use Fixed Frame: true + Use rainbow: false Value: true Enabled: true Global Options: @@ -614,7 +611,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.710027813911438 + Distance: 1.7602970600128174 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -622,17 +619,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.17848753929138184 - Y: 0.3464738428592682 - Z: 0.36626341938972473 + X: 0.23816421627998352 + Y: 0.3857060968875885 + Z: 0.35086870193481445 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.25500035285949707 + Pitch: 0.22000055015087128 Target Frame: - Yaw: 5.125002384185791 + Yaw: 5.094988822937012 Saved: ~ Window Geometry: ColorImage: @@ -643,12 +640,12 @@ Window Geometry: collapsed: false Height: 965 Hide Left Dock: false - Hide Right Dock: false + Hide Right Dock: true MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -658,5 +655,5 @@ Window Geometry: Views: collapsed: false Width: 1379 - X: 824 - Y: 205 + X: 1070 + Y: 288 diff --git a/requirements.txt b/requirements.txt index a5f76bc..c3db445 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,2 +1 @@ numba -scikit-image diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index bdb2e8c..3543caa 100755 --- a/scripts/bt_sim_node.py +++ b/scripts/bt_sim_node.py @@ -9,7 +9,6 @@ from geometry_msgs.msg import Twist import numpy as np import rospy from sensor_msgs.msg import JointState, Image, CameraInfo -import skimage.transform from scipy import interpolate from threading import Thread @@ -17,6 +16,7 @@ from active_grasp.bbox import to_bbox_msg from active_grasp.srv import * from active_grasp.simulation import Simulation from robot_helpers.ros.conversions import * +from vgn.simulation import apply_noise class BtSimNode: @@ -318,20 +318,9 @@ class CameraPlugin(Plugin): self.depth_pub.publish(msg) -def apply_noise(img, k=1000, theta=0.001, sigma=0.005, l=4.0): - # Multiplicative and additive noise - img *= np.random.gamma(k, theta) - h, w = img.shape - noise = np.random.randn(int(h / l), int(w / l)) * sigma - img += skimage.transform.resize(noise, img.shape, order=1, mode="constant") - return img - - def main(): rospy.init_node("bt_sim") server = BtSimNode() - # server.seed(SeedRequest(1)) - # server.reset(ResetRequest()) server.run() diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py index 4e76908..7f7a966 100644 --- a/src/active_grasp/policy.py +++ b/src/active_grasp/policy.py @@ -4,7 +4,7 @@ from pathlib import Path import rospy from .timer import Timer -from .visualization import Visualizer +from .rviz import Visualizer from robot_helpers.model import KDLModel from robot_helpers.ros import tf from robot_helpers.ros.conversions import * @@ -62,36 +62,33 @@ class Policy: self.T_task_base = self.T_base_task.inv() tf.broadcast(self.T_base_task, self.base_frame, self.task_frame) rospy.sleep(1.0) # Wait for tf tree to be updated - self.vis.workspace(self.task_frame, 0.3) def update(self, img, x, q): raise NotImplementedError - def sort_grasps(self, in_grasps, q): - # Transforms grasps into base frame, checks whether they lie on the target, and sorts by their score - grasps, scores = [], [] - - for grasp in in_grasps: + def sort_grasps(self, grasps, qualities, q): + """ + 1. Transform grasp configurations into base_frame + 2. Check whether the finger tips lie within the bounding box + 3. Remove grasps for which no IK solution was found + 4. Sort grasps according to score_fn + """ + filtered_grasps, scores = [], [] + for grasp, quality in zip(grasps, qualities): pose = self.T_base_task * grasp.pose R, t = pose.rotation, pose.translation tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation - - # Filter out artifacts close to the support - if t[2] < self.bbox.min[2] + 0.05 or tip[2] < self.bbox.min[2] + 0.02: - continue - if self.bbox.is_inside(tip): grasp.pose = pose q_grasp = self.ee_model.ik(q, pose * self.T_grasp_ee) if q_grasp is not None: - grasps.append(grasp) - scores.append(self.score_fn(grasp, q, q_grasp)) + filtered_grasps.append(grasp) + scores.append(self.score_fn(grasp, quality, q, q_grasp)) + filtered_grasps, scores = np.asarray(filtered_grasps), np.asarray(scores) + i = np.argsort(-scores) + return filtered_grasps[i], qualities[i], scores[i] - grasps, scores = np.asarray(grasps), np.asarray(scores) - indices = np.argsort(-scores) - return grasps[indices], scores[indices] - - def score_fn(self, grasp, q, q_grasp): + def score_fn(self, grasp, quality, q, q_grasp): return -np.linalg.norm(q - q_grasp) @@ -108,8 +105,8 @@ class SingleViewPolicy(Policy): out = self.vgn.predict(tsdf_grid) self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5) - grasps = select_grid(voxel_size, out, threshold=self.qual_threshold) - grasps, _ = self.sort_grasps(grasps, q) + grasps, qualities = select_grid(voxel_size, out, self.qual_threshold) + grasps, _ = self.sort_grasps(grasps, qualities, q) if len(grasps) > 0: self.best_grasp = grasps[0] @@ -143,17 +140,16 @@ class MultiViewPolicy(Policy): self.qual_hist[t, ...] = out.qual with Timer("grasp_selection"): - grasps = select_grid(voxel_size, out, threshold=self.qual_threshold) - grasps, _ = self.sort_grasps(grasps, q) - - self.vis.clear_grasps() + grasps, qualities = select_grid(voxel_size, out, self.qual_threshold) + grasps, qualities, _ = self.sort_grasps(grasps, qualities, q) if len(grasps) > 0: self.best_grasp = grasps[0] - self.vis.grasps(self.base_frame, grasps) - self.vis.best_grasp(self.base_frame, self.best_grasp) + # self.vis.grasps(self.base_frame, grasps, qualities) + self.vis.grasp(self.base_frame, self.best_grasp, qualities[0]) else: self.best_grasp = None + self.vis.clear_grasp() def compute_error(x_d, x): diff --git a/src/active_grasp/rviz.py b/src/active_grasp/rviz.py new file mode 100644 index 0000000..7cbae1e --- /dev/null +++ b/src/active_grasp/rviz.py @@ -0,0 +1,143 @@ +import numpy as np + +from robot_helpers.ros.rviz import * +from robot_helpers.spatial import Transform +import vgn.rviz + +cm = lambda s: tuple([float(1 - s), float(s), float(0)]) +red = np.r_[1.0, 0.0, 0.0] +blue = np.r_[0, 0.6, 1.0] + + +class Visualizer(vgn.rviz.Visualizer): + def bbox(self, frame, bbox): + pose = Transform.identity() + scale = [0.004, 0.0, 0.0] + color = red + corners = bbox.corners + edges = [ + (0, 1), + (1, 3), + (3, 2), + (2, 0), + (4, 5), + (5, 7), + (7, 6), + (6, 4), + (0, 4), + (1, 5), + (3, 7), + (2, 6), + ] + lines = [(corners[s], corners[e]) for s, e in edges] + marker = create_line_list_marker(frame, pose, scale, color, lines, "bbox") + self.draw([marker]) + + def rays(self, frame, origin, directions, t_max=1.0): + lines = [[origin, origin + t_max * direction] for direction in directions] + marker = create_line_list_marker( + frame, + Transform.identity(), + [0.001, 0.0, 0.0], + [0.9, 0.9, 0.9], + lines, + "rays", + ) + self.draw([marker]) + + def path(self, frame, poses): + color = blue + points = [p.translation for p in poses] + spheres = create_sphere_list_marker( + frame, + Transform.identity(), + np.full(3, 0.01), + color, + points, + "path", + 0, + ) + markers = [spheres] + if len(poses) > 1: + lines = create_line_strip_marker( + frame, + Transform.identity(), + [0.005, 0.0, 0.0], + color, + points, + "path", + 1, + ) + markers.append(lines) + self.draw(markers) + + def point(self, frame, point): + marker = create_sphere_marker( + frame, + Transform.translation(point), + np.full(3, 0.01), + [0, 0, 1], + "point", + ) + self.draw([marker]) + + def views(self, frame, intrinsic, views, values): + vmin, vmax = min(values), max(values) + scale = [0.002, 0.0, 0.0] + near, far = 0.0, 0.02 + markers = [] + for i, (view, value) in enumerate(zip(views, values)): + color = cm((value - vmin) / (vmax - vmin)) + marker = create_cam_view_marker( + frame, + view, + scale, + color, + intrinsic, + near, + far, + ns="views", + id=i, + ) + markers.append(marker) + self.draw(markers) + + +def create_cam_view_marker( + frame, pose, scale, color, intrinsic, near, far, ns="", id=0 +): + marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id) + x_n = near * intrinsic.width / (2.0 * intrinsic.fx) + y_n = near * intrinsic.height / (2.0 * intrinsic.fy) + z_n = near + x_f = far * intrinsic.width / (2.0 * intrinsic.fx) + y_f = far * intrinsic.height / (2.0 * intrinsic.fy) + z_f = far + points = [ + [x_n, y_n, z_n], + [-x_n, y_n, z_n], + [-x_n, y_n, z_n], + [-x_n, -y_n, z_n], + [-x_n, -y_n, z_n], + [x_n, -y_n, z_n], + [x_n, -y_n, z_n], + [x_n, y_n, z_n], + [x_f, y_f, z_f], + [-x_f, y_f, z_f], + [-x_f, y_f, z_f], + [-x_f, -y_f, z_f], + [-x_f, -y_f, z_f], + [x_f, -y_f, z_f], + [x_f, -y_f, z_f], + [x_f, y_f, z_f], + [x_n, y_n, z_n], + [x_f, y_f, z_f], + [-x_n, y_n, z_n], + [-x_f, y_f, z_f], + [-x_n, -y_n, z_n], + [-x_f, -y_f, z_f], + [x_n, -y_n, z_n], + [x_f, -y_f, z_f], + ] + marker.points = [to_point_msg(p) for p in points] + return marker diff --git a/src/active_grasp/visualization.py b/src/active_grasp/visualization.py deleted file mode 100644 index a987162..0000000 --- a/src/active_grasp/visualization.py +++ /dev/null @@ -1,244 +0,0 @@ -from geometry_msgs.msg import PoseStamped -import matplotlib.colors -import numpy as np -import rospy - -from robot_helpers.ros.rviz import * -from robot_helpers.spatial import Transform -from vgn.utils import * - -cmap = matplotlib.colors.LinearSegmentedColormap.from_list("RedGreen", ["r", "g"]) -red = np.r_[1.0, 0.0, 0.0] -blue = np.r_[0, 0.6, 1.0] - - -class Visualizer: - def __init__(self, topic="visualization_marker_array"): - self.marker_pub = rospy.Publisher(topic, MarkerArray, queue_size=1) - self.scene_cloud_pub = rospy.Publisher( - "scene_cloud", - PointCloud2, - latch=True, - queue_size=1, - ) - self.map_cloud_pub = rospy.Publisher( - "map_cloud", - PointCloud2, - latch=True, - queue_size=1, - ) - self.pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=1) - self.quality_pub = rospy.Publisher("quality", PointCloud2, queue_size=1) - - self.grasp_marker_count = 0 - - def clear(self): - self.clear_markers() - msg = to_cloud_msg("panda_link0", np.array([])) - self.scene_cloud_pub.publish(msg) - self.map_cloud_pub.publish(msg) - self.quality_pub.publish(msg) - rospy.sleep(0.1) - - def clear_markers(self): - self.draw([Marker(action=Marker.DELETEALL)]) - - def draw(self, markers): - self.marker_pub.publish(MarkerArray(markers=markers)) - - def bbox(self, frame, bbox): - pose = Transform.identity() - scale = [0.004, 0.0, 0.0] - color = red - corners = bbox.corners - edges = [ - (0, 1), - (1, 3), - (3, 2), - (2, 0), - (4, 5), - (5, 7), - (7, 6), - (6, 4), - (0, 4), - (1, 5), - (3, 7), - (2, 6), - ] - lines = [(corners[s], corners[e]) for s, e in edges] - marker = create_line_list_marker(frame, pose, scale, color, lines, ns="bbox") - self.draw([marker]) - - def best_grasp(self, frame, grasp, qmin=0.5, qmax=1.0): - color = cmap((grasp.quality - qmin) / (qmax - qmin)) - self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.006)) - - def grasps(self, frame, grasps, qmin=0.5, qmax=1.0): - markers = [] - for i, grasp in enumerate(grasps): - color = cmap((grasp.quality - qmin) / (qmax - qmin)) - markers += create_grasp_markers(frame, grasp, color, "grasps", 4 * i) - self.grasp_marker_count = len(markers) - self.draw(markers) - - def clear_grasps(self): - if self.grasp_marker_count > 0: - markers = [ - Marker(action=Marker.DELETE, ns="grasps", id=i) - for i in range(self.grasp_marker_count) - ] - markers += [ - Marker(action=Marker.DELETE, ns="best_grasp", id=i) for i in range(4) - ] - self.draw(markers) - self.grasp_marker_count = 0 - - def rays(self, frame, origin, directions, t_max=1.0): - lines = [[origin, origin + t_max * direction] for direction in directions] - marker = create_line_list_marker( - frame, - Transform.identity(), - [0.001, 0.0, 0.0], - [0.9, 0.9, 0.9], - lines, - "rays", - ) - self.draw([marker]) - - def map_cloud(self, frame, cloud): - points = np.asarray(cloud.points) - distances = np.expand_dims(np.asarray(cloud.colors)[:, 0], 1) - msg = to_cloud_msg(frame, points, distances=distances) - self.map_cloud_pub.publish(msg) - - def path(self, frame, poses): - color = blue - points = [p.translation for p in poses] - spheres = create_sphere_list_marker( - frame, - Transform.identity(), - np.full(3, 0.01), - color, - points, - "path", - 0, - ) - markers = [spheres] - if len(poses) > 1: - lines = create_line_strip_marker( - frame, - Transform.identity(), - [0.005, 0.0, 0.0], - color, - points, - "path", - 1, - ) - markers.append(lines) - self.draw(markers) - - def point(self, frame, point): - marker = create_sphere_marker( - frame, - Transform.translation(point), - np.full(3, 0.01), - [0, 0, 1], - "point", - ) - self.draw([marker]) - - def pose(self, frame, pose): - msg = to_pose_stamped_msg(pose, frame) - self.pose_pub.publish(msg) - - def quality(self, frame, voxel_size, quality, threshold=0.9): - points, values = grid_to_map_cloud(voxel_size, quality, threshold) - msg = to_cloud_msg(frame, points, intensities=values) - self.quality_pub.publish(msg) - - def scene_cloud(self, frame, cloud): - msg = to_cloud_msg(frame, np.asarray(cloud.points)) - self.scene_cloud_pub.publish(msg) - - def views(self, frame, intrinsic, views, values): - vmin, vmax = min(values), max(values) - scale = [0.002, 0.0, 0.0] - near, far = 0.0, 0.02 - markers = [] - for i, (view, value) in enumerate(zip(views, values)): - color = cmap((value - vmin) / (vmax - vmin)) - marker = create_cam_view_marker( - frame, - view, - scale, - color, - intrinsic, - near, - far, - ns="views", - id=i, - ) - markers.append(marker) - self.draw(markers) - - def workspace(self, frame, size): - scale = size * 0.005 - pose = Transform.identity() - scale = [scale, 0.0, 0.0] - color = [0.5, 0.5, 0.5] - lines = [ - ([0.0, 0.0, 0.0], [size, 0.0, 0.0]), - ([size, 0.0, 0.0], [size, size, 0.0]), - ([size, size, 0.0], [0.0, size, 0.0]), - ([0.0, size, 0.0], [0.0, 0.0, 0.0]), - ([0.0, 0.0, size], [size, 0.0, size]), - ([size, 0.0, size], [size, size, size]), - ([size, size, size], [0.0, size, size]), - ([0.0, size, size], [0.0, 0.0, size]), - ([0.0, 0.0, 0.0], [0.0, 0.0, size]), - ([size, 0.0, 0.0], [size, 0.0, size]), - ([size, size, 0.0], [size, size, size]), - ([0.0, size, 0.0], [0.0, size, size]), - ] - msg = create_line_list_marker(frame, pose, scale, color, lines, ns="workspace") - self.draw([msg]) - - -def create_cam_view_marker( - frame, pose, scale, color, intrinsic, near, far, ns="", id=0 -): - marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id) - x_n = near * intrinsic.width / (2.0 * intrinsic.fx) - y_n = near * intrinsic.height / (2.0 * intrinsic.fy) - z_n = near - x_f = far * intrinsic.width / (2.0 * intrinsic.fx) - y_f = far * intrinsic.height / (2.0 * intrinsic.fy) - z_f = far - points = [ - [x_n, y_n, z_n], - [-x_n, y_n, z_n], - [-x_n, y_n, z_n], - [-x_n, -y_n, z_n], - [-x_n, -y_n, z_n], - [x_n, -y_n, z_n], - [x_n, -y_n, z_n], - [x_n, y_n, z_n], - [x_f, y_f, z_f], - [-x_f, y_f, z_f], - [-x_f, y_f, z_f], - [-x_f, -y_f, z_f], - [-x_f, -y_f, z_f], - [x_f, -y_f, z_f], - [x_f, -y_f, z_f], - [x_f, y_f, z_f], - [x_n, y_n, z_n], - [x_f, y_f, z_f], - [-x_n, y_n, z_n], - [-x_f, y_f, z_f], - [-x_n, -y_n, z_n], - [-x_f, -y_f, z_f], - [x_n, -y_n, z_n], - [x_f, -y_f, z_f], - ] - marker.points = [to_point_msg(p) for p in points] - return marker