From 91a521d26ffe1a9313583b8d841c089fe01011df Mon Sep 17 00:00:00 2001
From: Mohamed Fazil <mfazil@hello-robot.com>
Date: Mon, 27 Feb 2023 16:17:47 -0800
Subject: [PATCH] Arucohead scan action tests

---
 stretch_core/nodes/aruco_head_scan_client.py  |  2 +-
 .../nodes/aruco_head_scan_action.py           | 21 ++++++++++++++++---
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/stretch_core/nodes/aruco_head_scan_client.py b/stretch_core/nodes/aruco_head_scan_client.py
index bd6adc2..f9abbad 100755
--- a/stretch_core/nodes/aruco_head_scan_client.py
+++ b/stretch_core/nodes/aruco_head_scan_client.py
@@ -18,7 +18,7 @@ def main():
     goal.aruco_id = 245
     goal.tilt_angle = -0.68
     goal.publish_to_map = True
-    goal.fill_in_blindspot_with_second_scan = False
+    goal.fill_in_blindspot_with_second_scan = True
     goal.fast_scan = False
 
     aruco_head_scan_client.send_goal(goal)
diff --git a/stretch_funmap/nodes/aruco_head_scan_action.py b/stretch_funmap/nodes/aruco_head_scan_action.py
index 405bf76..334c801 100755
--- a/stretch_funmap/nodes/aruco_head_scan_action.py
+++ b/stretch_funmap/nodes/aruco_head_scan_action.py
@@ -6,7 +6,7 @@ from geometry_msgs.msg import Transform, TransformStamped, Pose
 import ros_numpy
 import numpy as np
 import tf2_ros
-
+import stretch_funmap.navigate as nv
 import actionlib
 from stretch_core.msg import ArucoHeadScanAction, ArucoHeadScanGoal, ArucoHeadScanFeedback, ArucoHeadScanResult
 from sensor_msgs.msg import JointState
@@ -42,6 +42,7 @@ class ArucoHeadScan(hm.HelloNode):
         self.markers = MarkerArray().markers
         self.joint_state = JointState()
         self.merged_map = None
+        self.move_base = nv.MoveBase(self, None)
 
     def execute_cb(self, goal):
         self.goal = goal
@@ -51,6 +52,21 @@ class ArucoHeadScan(hm.HelloNode):
         self.fast_scan = self.goal.fast_scan
         self.publish_to_map = self.goal.publish_to_map
         self.scan_and_detect()
+        if self.fill_in_blindspot_with_second_scan:
+            # blindspot due to its mast.
+            turn_ang = (70.0 / 180.0) * np.pi
+
+            # Command the robot to turn to point to the next
+            # waypoint.
+            rospy.loginfo('robot turn angle in degrees =' +
+                          str(turn_ang * (180.0 / np.pi)))
+            at_goal = self.move_base.turn(
+                turn_ang, publish_visualizations=True)
+            if not at_goal:
+                message_text = 'Failed to reach turn goal.'
+                rospy.loginfo(message_text)
+            self.scan_and_detect()
+        self.result_cb(self.aruco_found, "after headscan")
 
     def scan_and_detect(self):
         node = self
@@ -123,7 +139,7 @@ class ArucoHeadScan(hm.HelloNode):
             if not self.aruco_found:
                 self.feedback.pan_angle = pan_ang
                 self.server.publish_feedback(self.feedback)
-            
+
         look_at_self = True
         if look_at_self:
             head_tilt = -1.2
@@ -138,7 +154,6 @@ class ArucoHeadScan(hm.HelloNode):
         head_scan.make_robot_footprint_unobserved()
 
         self.merged_map = head_scan
-        self.result_cb(self.aruco_found, "after headscan")
 
     def result_cb(self, aruco_found, str=None):
         self.result.aruco_found = aruco_found