Headscan for aruco detection as a ROS action

stretch_core/CMakeLists.txt

@@ -8,6 +8,7 @@ project(stretch_core)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  genmsg
   actionlib
   actionlib_msgs
   geometry_msgs
@@ -69,17 +70,16 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
+add_action_files(DIRECTORY action
+  FILES
+  ArucoHeadScan.action
+)
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   actionlib_msgs#   geometry_msgs#   nav_msgs#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

stretch_core/action/ArucoHeadScan.action

@@ -0,0 +1,28 @@
+# Define the goal
+
+# Specify aruco ID to look for
+uint32 aruco_id
+
+# Specify the camera tilt_angle at which to scan
+float32 tilt_angle
+
+# Publish tf relative to the map frame
+bool publish_to_map
+
+# If robot should rotate to cover the blind spot generated by the mast
+bool fill_in_blindspot_with_second_scan 
+
+# If scan should stop as soon as a matching aruco ID is found
+bool fast_scan
+
+---
+# Define the result
+
+# Whether goal aruco ID was found
+bool aruco_found
+
+---
+# Define a feedback message
+
+# Pan angle of the camera
+float32 pan_angle

stretch_core/config/stretch_marker_dict.yaml

@@ -37,6 +37,11 @@
     'use_rgb_only': False
     'name': 'shoulder_top'
     'link': 'link_aruco_shoulder'
+  '245':
+    'length_mm': 88.0
+    'use_rgb_only': False
+    'name': 'docking_station'
+    'link': None
   '246':
     'length_mm': 179.0
     'use_rgb_only': False

stretch_core/launch/stretch_aruco.launch

@@ -2,7 +2,7 @@
 
   <!-- ARUCO MARKER DETECTOR -->
   <rosparam command="load" file="$(find stretch_core)/config/stretch_marker_dict.yaml" />
-  <node name="detect_aruco_markers" pkg="stretch_core" type="detect_aruco_markers" output="screen"/>
+  <node name="detect_aruco_markers" pkg="stretch_core" type="detect_aruco_markers.py" output="screen"/>
   <!-- -->
 
 </launch>

stretch_core/nodes/aruco_head_scan_client.py

@@ -0,0 +1,28 @@
+from stretch_core.msg import ArucoHeadScanGoal, ArucoHeadScanAction
+import actionlib
+import rospy
+import sys
+from detect_aruco_markers import ArucoHeadScan
+
+def main():
+    rospy.init_node('aruco_head_scan_client')
+    aruco_head_scan_client = actionlib.SimpleActionClient('ArucoHeadScan', ArucoHeadScanAction)
+    server_reached = aruco_head_scan_client.wait_for_server(timeout=rospy.Duration(10.0))
+    if not server_reached:
+            rospy.signal_shutdown('Unable to connect to aruco head scan action server. Timeout exceeded.')
+            sys.exit()
+
+    goal = ArucoHeadScanGoal()
+    goal.aruco_id = 245
+    goal.tilt_angle = -0.68
+    goal.publish_to_map = True
+    goal.fill_in_blindspot_with_second_scan = False
+    goal.fast_scan = False
+
+    aruco_head_scan_client.send_goal(goal)
+    aruco_head_scan_client.wait_for_result()
+
+
+if __name__ == '__main__':
+    rospy.loginfo('Ensure stretch_driver is launched before executing')
+    main()

stretch_core/nodes/detect_aruco_markers → stretch_core/nodes/detect_aruco_markers.py

@@ -26,6 +26,14 @@
 import hello_helpers.fit_plane as fp
 import threading
 from collections import deque
+import actionlib
+from stretch_core.msg import ArucoHeadScanAction, ArucoHeadScanGoal, ArucoHeadScanFeedback, ArucoHeadScanResult
+from control_msgs.msg import FollowJointTrajectoryGoal
+from sensor_msgs.msg import JointState
+from trajectory_msgs.msg import JointTrajectoryPoint
+import hello_helpers.hello_misc as hm
+import time
+
 
 class ArucoMarker:
     def __init__(self, aruco_id, marker_info, show_debug_images=False):
@@ -505,7 +513,111 @@ def get_ros_axes_markers(self):
         return markers
 
 
-
+class ArucoHeadScan(hm.HelloNode):
+    def __init__(self):
+        hm.HelloNode.__init__(self)
+        hm.HelloNode.main(self, 'aruco_head_scan', 'aruco_head_scan', wait_for_first_pointcloud=False)
+        self.server = actionlib.SimpleActionServer('ArucoHeadScan', ArucoHeadScanAction, self.execute_cb, False)
+        self.goal = ArucoHeadScanGoal()
+        self.feedback = ArucoHeadScanFeedback()
+        self.result = ArucoHeadScanResult()
+        self.aruco_marker_array = rospy.Subscriber('aruco/marker_array', MarkerArray, self.aruco_callback)
+        self.joint_states_sub = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_state_callback)
+        self.server.start()
+        self.aruco_id = 1000 # Placeholder value, can never be true
+        self.aruco_found = False
+        self.markers = MarkerArray().markers
+        self.joint_state = JointState()
+        self.listener = tf.TransformListener()
+        self.tf_broadcaster = tf.TransformBroadcaster()
+
+    def execute_cb(self, goal):
+        self.goal = goal
+        self.aruco_id = self.goal.aruco_id
+        self.tilt_angle = self.goal.tilt_angle
+        self.fill_in_blindspot_with_second_scan = self.goal.fill_in_blindspot_with_second_scan
+        self.fast_scan = self.goal.fast_scan
+        self.publish_to_map = self.goal.publish_to_map
+        pan_angle = 0.0
+
+        scan_point = JointTrajectoryPoint()
+        scan_point.time_from_start = rospy.Duration(3.0)
+        scan_point.positions = [self.tilt_angle, -3.69]
+
+        trajectory_goal = FollowJointTrajectoryGoal()
+        trajectory_goal.trajectory.joint_names = ['joint_head_tilt', 'joint_head_pan']
+        trajectory_goal.trajectory.points = [scan_point]
+        trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+        trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+        self.trajectory_client.send_goal(trajectory_goal)
+        self.trajectory_client.wait_for_result()
+
+        self.feedback.pan_angle = pan_angle
+        self.server.publish_feedback(self.feedback)
+
+        # TODO: check distance of aruco to base_link < 2m
+        for pan_angle in np.arange(-3.69, 1.66, 0.35):
+            time.sleep(2.0)
+            markers = self.markers
+            rospy.loginfo("Markers found: {}".format(markers))
+
+            if markers != []:
+                for marker in markers:
+                    if marker.id == self.aruco_id:
+                        self.aruco_found = True
+                        self.aruco_name = marker.text
+                        if self.publish_to_map:
+                            (trans,rot) = self.listener.lookupTransform('map', self.aruco_name, rospy.Time(0))
+                            rospy.loginfo("trans: {}".format(trans))
+                            rospy.loginfo("rot: {}".format(rot))
+                            self.broadcast_tf(trans, rot, self.aruco_name, 'map')
+                            if self.aruco_name == 'docking_station':
+                                rospy.loginfo("Publishing predock pose")
+                                self.broadcast_tf([0.0, -0.249, 1.0], [0.0, 0.0, 0.0, 1.0], 'predock_pose', 'docking_station')
+                                rospy.loginfo("Publishing dock pose")
+                                self.broadcast_tf([0.0, -0.249, 0.25], [0.0, 0.0, 0.0, 1.0], 'dock_pose', 'docking_station')
+
+            if not self.aruco_found:
+                self.feedback.pan_angle = pan_angle
+                self.server.publish_feedback(self.feedback)
+
+                scan_point.time_from_start = rospy.Duration(3.0)
+                scan_point.positions = [self.tilt_angle, pan_angle]
+                trajectory_goal.trajectory.points = [scan_point]
+                trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+                trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+                self.trajectory_client.send_goal(trajectory_goal)
+                self.trajectory_client.wait_for_result()
+            else:
+                break
+
+        self.result_cb(self.aruco_found, "after headscan")
+
+    def result_cb(self, aruco_found, str=None):
+        self.result.aruco_found = aruco_found
+        if aruco_found:    
+            rospy.loginfo("Aruco marker found")
+            self.server.set_succeeded(self.result)
+        else:
+            rospy.loginfo("Could not find aruco marker {}".format(str))
+            self.server.set_aborted(self.result)
+
+    def broadcast_tf(self, pos, rot, name, ref):
+        self.tf_broadcaster.sendTransform(pos,
+                                     rot,
+                                     rospy.Time.now(),
+                                     name,
+                                     ref)
+
+    def aruco_callback(self, msg):
+        self.markers = msg.markers
+
+    def joint_state_callback(self, msg):
+        pass
+
+
 class ArucoMarkerCollection:
     def __init__(self, marker_info, show_debug_images=False):
         self.show_debug_images = show_debug_images