diff --git a/stretch_deep_perception/nodes/detection_node_python3.py b/stretch_deep_perception/nodes/detection_node_python3.py index de7531f..291180a 100644 --- a/stretch_deep_perception/nodes/detection_node_python3.py +++ b/stretch_deep_perception/nodes/detection_node_python3.py @@ -102,8 +102,9 @@ class DetectionNode: include_axes = True include_z_axes = False axes_array = None + axes_scale = 4.0 if include_axes or include_z_axes: - axes_array = self.marker_collection.get_ros_axes_array(include_z_axes, include_axes) + axes_array = self.marker_collection.get_ros_axes_array(include_z_axes, include_axes, axes_scale=axes_scale) if self.publish_marker_point_clouds: for marker in self.marker_collection: diff --git a/stretch_deep_perception/nodes/detection_ros_markers_python3.py b/stretch_deep_perception/nodes/detection_ros_markers_python3.py index 9355f23..6391231 100644 --- a/stretch_deep_perception/nodes/detection_ros_markers_python3.py +++ b/stretch_deep_perception/nodes/detection_ros_markers_python3.py @@ -141,7 +141,7 @@ class DetectionBoxMarker: return self.marker - def create_axis_marker(self, axis, id_num, rgba=None, name=None): + def create_axis_marker(self, axis, id_num, rgba=None, name=None, axes_scale=1.0): marker = Marker() marker.header.frame_id = self.frame_id marker.header.stamp = self.timestamp @@ -154,11 +154,17 @@ class DetectionBoxMarker: # against the documentation "NOTE: only used for text markers # string text" marker.text = name - axis_arrow = {'head_diameter': 0.02, - 'shaft_diameter': 0.012, - 'head_length': 0.012, - 'length': 0.08} + # axis_arrow = {'head_diameter': 0.02, + # 'shaft_diameter': 0.012, + # 'head_length': 0.012, + # 'length': 0.08} + axis_arrow = {'head_diameter': 0.02 * axes_scale, + 'shaft_diameter': 0.012 * axes_scale, + 'head_length': 0.012 * axes_scale, + 'length': 0.08 * axes_scale} + + # "scale.x is the shaft diameter, and scale.y is the # head diameter. If scale.z is not zero, it specifies # the head length." - @@ -202,7 +208,7 @@ class DetectionBoxMarker: name = base_name = '_z_axis' return self.create_axis_marker(self.z_axis, id_num, rgba, name) - def get_ros_axes_markers(self): + def get_ros_axes_markers(self, axes_scale=1.0): markers = [] if not self.ready: @@ -219,17 +225,17 @@ class DetectionBoxMarker: id_num = 4 * self.detection_box_id rgba = [0.0, 0.0, 1.0, 0.5] name = base_name = '_z_axis' - markers.append(self.create_axis_marker(self.z_axis, id_num, rgba, name)) + markers.append(self.create_axis_marker(self.z_axis, id_num, rgba, name, axes_scale=axes_scale)) if self.x_axis is not None: id_num = (4 * self.detection_box_id) + 1 rgba = [1.0, 0.0, 0.0, 0.5] name = base_name = '_x_axis' - markers.append(self.create_axis_marker(self.x_axis, id_num, rgba, name)) + markers.append(self.create_axis_marker(self.x_axis, id_num, rgba, name, axes_scale=axes_scale)) if self.y_axis is not None: id_num = (4 * self.detection_box_id) + 2 rgba = [0.0, 1.0, 0.0, 0.5] name = base_name = '_y_axis' - markers.append(self.create_axis_marker(self.y_axis, id_num, rgba, name)) + markers.append(self.create_axis_marker(self.y_axis, id_num, rgba, name, axes_scale=axes_scale)) return markers @@ -286,7 +292,7 @@ class DetectionBoxMarkerCollection: marker_array.markers.append(landmarks_marker) return marker_array - def get_ros_axes_array(self, include_z_axes=True, include_axes=True): + def get_ros_axes_array(self, include_z_axes=True, include_axes=True, axes_scale=1.0): marker_array = MarkerArray() for key in self.collection: marker = self.collection[key] @@ -296,6 +302,6 @@ class DetectionBoxMarkerCollection: if ros_z_axis_marker is not None: marker_array.markers.append(ros_z_axis_marker) if include_axes: - ros_axes_markers= marker.get_ros_axes_markers() + ros_axes_markers= marker.get_ros_axes_markers(axes_scale=axes_scale) marker_array.markers.extend(ros_axes_markers) return marker_array diff --git a/stretch_demos/launch/handover_object.launch b/stretch_demos/launch/handover_object.launch index c1e5c03..fb2273f 100644 --- a/stretch_demos/launch/handover_object.launch +++ b/stretch_demos/launch/handover_object.launch @@ -3,7 +3,7 @@ - + diff --git a/stretch_demos/nodes/handover_object b/stretch_demos/nodes/handover_object index 358dd49..cd1c745 100755 --- a/stretch_demos/nodes/handover_object +++ b/stretch_demos/nodes/handover_object @@ -198,8 +198,10 @@ class HandoverObjectNode(hm.HelloNode): # This rate determines how quickly the head pans back and forth. rate = rospy.Rate(0.5) + look_around = False while not rospy.is_shutdown(): - self.look_around_callback() + if look_around: + self.look_around_callback() rate.sleep() diff --git a/stretch_demos/rviz/handover_object.rviz b/stretch_demos/rviz/handover_object.rviz index 8cc15df..05404b0 100644 --- a/stretch_demos/rviz/handover_object.rviz +++ b/stretch_demos/rviz/handover_object.rviz @@ -50,7 +50,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.75 + - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false Enabled: true @@ -220,6 +220,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + link_puller: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true link_right_wheel: Alpha: 1 Show Axes: false @@ -241,7 +246,7 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Alpha: 0.20000000298023224 + - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 3.660540819168091 @@ -276,14 +281,14 @@ Visualization Manager: Marker Topic: /nearest_mouth/axes Name: MarkerArray Namespaces: - "": true + {} Queue Size: 100 Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: base_link + Fixed Frame: map Frame Rate: 30 Name: root Tools: