From ec5503a119d55801ed5021652dc135d0e189cf6e Mon Sep 17 00:00:00 2001
From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com>
Date: Sat, 4 Jul 2020 00:05:50 -0400
Subject: [PATCH] various changes for object handover autonomy video

---
 .../nodes/detection_node_python3.py           |  3 +-
 .../nodes/detection_ros_markers_python3.py    | 28 +++++++++++--------
 stretch_demos/launch/handover_object.launch   |  2 +-
 stretch_demos/nodes/handover_object           |  4 ++-
 stretch_demos/rviz/handover_object.rviz       | 13 ++++++---
 5 files changed, 32 insertions(+), 18 deletions(-)

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 @@
   <arg name="debug_directory" value="$(env HELLO_FLEET_PATH)/debug/"/>
 
   <!-- REALSENSE D435i -->
-  <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
+  <include file="$(find stretch_core)/launch/d435i_low_resolution.launch"></include>
   <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
     <param name="initial_mode" type="string" value="High Accuracy"/>
   </node>
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: <Fixed 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: