From 9fa7bd518e5569a091c316c8ffd4c665b94c8566 Mon Sep 17 00:00:00 2001
From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com>
Date: Sun, 5 Jul 2020 11:46:47 -0400
Subject: [PATCH] command for a fixed number of mapping iterations

---
 stretch_core/nodes/keyboard_teleop | 26 +++++++++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop
index b10ee1a..e7f2bc8 100755
--- a/stretch_core/nodes/keyboard_teleop
+++ b/stretch_core/nodes/keyboard_teleop
@@ -94,6 +94,20 @@ class GetKeyboardCommands:
         ## MOSTLY MAPPING RELATED CAPABILITIES
         ## (There are non-mapping outliers.)
         ####################################################
+        
+        # Sequential performs a fixed number of autonomus mapping iterations
+        if (c == '!') and self.mapping_on:
+            number_iterations = 4
+            for n in range(number_iterations):
+                # Trigger a 3D scan with the D435i
+                trigger_request = TriggerRequest() 
+                trigger_result = node.trigger_head_scan_service(trigger_request)
+                rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
+                # Trigger driving the robot to the estimated next best place to scan
+                trigger_request = TriggerRequest() 
+                trigger_result = node.trigger_drive_to_scan_service(trigger_request)
+                rospy.loginfo('trigger_result = {0}'.format(trigger_result))
                 
         # Trigger localizing the robot to a new pose anywhere on the current map
         if ((c == '+') or (c == '=')) and self.mapping_on:
@@ -136,8 +150,11 @@ class GetKeyboardCommands:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_lower_until_contact_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
-        ####################################################
+            
         
+        ####################################################
+        ## OTHER CAPABILITIES
+        ####################################################
 
         # Trigger Hello World whiteboard writing demo
         if ((c == '`') or (c == '~')) and self.hello_world_on:
@@ -175,6 +192,11 @@ class GetKeyboardCommands:
             trigger_result = node.trigger_deliver_object_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
+            
+        ####################################################
+        ## BASIC KEYBOARD TELEOPERATION COMMANDS
+        ####################################################
+        
         # 8 or up arrow
         if c == '8' or c == '\x1b[A':
             command = {'joint': 'joint_lift', 'delta': self.get_deltas()['translate']}
@@ -239,6 +261,8 @@ class GetKeyboardCommands:
             rospy.loginfo('keyboard_teleop exiting...')
             rospy.signal_shutdown('Received quit character (q), so exiting')
 
+        ####################################################
+
         return command