diff --git a/stretch_core/CMakeLists.txt b/stretch_core/CMakeLists.txt
index c3f2704e..f1ba75d5 100644
--- a/stretch_core/CMakeLists.txt
+++ b/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 ##
diff --git a/stretch_core/action/ArucoHeadScan.action b/stretch_core/action/ArucoHeadScan.action
new file mode 100644
index 00000000..8ae0ef4a
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/stretch_core/config/stretch_marker_dict.yaml b/stretch_core/config/stretch_marker_dict.yaml
index 981014f7..339cc3a6 100644
--- a/stretch_core/config/stretch_marker_dict.yaml
+++ b/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
diff --git a/stretch_core/launch/stretch_aruco.launch b/stretch_core/launch/stretch_aruco.launch
index b86f5f1b..67cb5ff6 100644
--- a/stretch_core/launch/stretch_aruco.launch
+++ b/stretch_core/launch/stretch_aruco.launch
@@ -2,7 +2,7 @@
-
+
diff --git a/stretch_core/nodes/aruco_head_scan_client.py b/stretch_core/nodes/aruco_head_scan_client.py
new file mode 100755
index 00000000..f9023ecc
--- /dev/null
+++ b/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()
diff --git a/stretch_core/nodes/detect_aruco_markers b/stretch_core/nodes/detect_aruco_markers.py
similarity index 85%
rename from stretch_core/nodes/detect_aruco_markers
rename to stretch_core/nodes/detect_aruco_markers.py
index d7cc399d..26b0bb40 100755
--- a/stretch_core/nodes/detect_aruco_markers
+++ b/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
diff --git a/stretch_funmap/nodes/aruco_head_scan_action.py b/stretch_funmap/nodes/aruco_head_scan_action.py
new file mode 100755
index 00000000..405bf76f
--- /dev/null
+++ b/stretch_funmap/nodes/aruco_head_scan_action.py
@@ -0,0 +1,231 @@
+#!/usr/bin/env python3
+
+import rospy
+from visualization_msgs.msg import MarkerArray
+from geometry_msgs.msg import Transform, TransformStamped, Pose
+import ros_numpy
+import numpy as np
+import tf2_ros
+
+import actionlib
+from stretch_core.msg import ArucoHeadScanAction, ArucoHeadScanGoal, ArucoHeadScanFeedback, ArucoHeadScanResult
+from sensor_msgs.msg import JointState
+from sensor_msgs.msg import PointCloud2
+import hello_helpers.hello_misc as hm
+
+import stretch_funmap.mapping as ma
+import time
+
+def create_map_to_odom_transform(t_mat):
+ t = TransformStamped()
+ t.header.stamp = rospy.Time.now()
+ t.header.frame_id = 'map'
+ t.child_frame_id = 'odom'
+ t.transform = ros_numpy.msgify(Transform, t_mat)
+ return t
+
+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.predock_pose_pub = rospy.Publisher('/predock_pose', Pose, queue_size=10)
+ self.dock_pose_pub = rospy.Publisher('/dock_pose', Pose, queue_size=10)
+ 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.merged_map = None
+
+ 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
+ self.scan_and_detect()
+
+ def scan_and_detect(self):
+ node = self
+ self.aruco_tf = None
+ self.predock_tf = None
+
+ far_right_pan = -3.6
+ far_left_pan = 1.45
+ head_tilt = -0.8
+ num_pan_steps = 7
+ fast_scan = False
+ capture_params = {
+ 'fast_scan': fast_scan,
+ 'head_settle_time': 0.5,
+ 'num_point_clouds_per_pan_ang': 10, # low numbers may not be effective for some surfaces and environments
+ 'time_between_point_clouds': 1.0/15.0 # point clouds at 15 Hz, so this should help obtain distinct clouds
+ }
+
+ head_scan = ma.HeadScan(voi_side_m=16.0)
+
+ pose = {'joint_head_pan': far_right_pan, 'joint_head_tilt': head_tilt}
+ node.move_to_pose(pose)
+
+ pan_left = np.linspace(far_right_pan, far_left_pan, num_pan_steps)
+ markers = []
+ for pan_ang in pan_left:
+ pose = {'joint_head_pan': pan_ang}
+ head_scan.capture_point_clouds(node, pose, capture_params)
+
+ for i in range(20):
+ if self.markers:
+ markers = self.markers
+ break
+
+ 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:
+ try:
+ trans = self.tfBuffer.lookup_transform('map', self.aruco_name, rospy.Time())
+ self.aruco_tf = self.broadcast_tf(trans.transform, self.aruco_name, 'map')
+ rospy.loginfo("Pose published to tf")
+ if self.aruco_name == 'docking_station':
+ # Transform the docking station frame such that x-axis points out of the aruco plane and 0.5 m in the front of the dock
+ # This facilitates passing the goal pose as this predock frame so that the robot can back up into the dock
+ saved_pose = Transform()
+ saved_pose.translation.x = 0.0
+ saved_pose.translation.y = -0.45
+ saved_pose.translation.z = 0.47
+ saved_pose.rotation.x = -0.382
+ saved_pose.rotation.y = -0.352
+ saved_pose.rotation.z = -0.604
+ saved_pose.rotation.w = 0.604
+ tran = self.broadcast_tf(saved_pose, 'predock_pose', 'docking_station')
+ self.tf2_broadcaster.sendTransform(tran)
+ trans = self.tfBuffer.lookup_transform('map', 'predock_pose', rospy.Time())
+ # Bring predock_frame at base_link level
+ trans.transform.translation.z = 0
+ trans.header.stamp = rospy.Time.now()
+ self.predock_tf = trans
+ rospy.loginfo("Published predock pose")
+ except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
+ rospy.loginfo("Could not publish pose to tf")
+ pass
+
+ 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
+ head_pan = 0.1
+ pose = {'joint_head_pan': head_pan, 'joint_head_tilt': head_tilt}
+ head_scan.capture_point_clouds(node, pose, capture_params)
+
+ # record robot pose information and potentially useful transformations
+ head_scan.robot_xy_pix, head_scan.robot_ang_rad, head_scan.timestamp = head_scan.max_height_im.get_robot_pose_in_image(node.tf2_buffer)
+
+ head_scan.make_robot_mast_blind_spot_unobserved()
+ 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
+ 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, trans, name, ref):
+ t = TransformStamped()
+ t.header.stamp = rospy.Time.now()
+ t.header.frame_id = ref
+ t.child_frame_id = name
+ t.transform = trans
+ return t
+
+ def aruco_callback(self, msg):
+ self.markers = msg.markers
+
+ def joint_state_callback(self, msg):
+ pass
+
+ def publish_map_point_cloud(self):
+ if self.merged_map is not None:
+ max_height_point_cloud = self.merged_map.max_height_im.to_point_cloud()
+ self.point_cloud_pub.publish(max_height_point_cloud)
+
+ # pub_voi = True
+ # if pub_voi:
+ # marker = self.merged_map.max_height_im.voi.get_ros_marker(
+ # duration=1000.0)
+ # self.voi_marker_pub.publish(marker)
+
+ def main(self):
+ self.rate = 5.0
+ rate = rospy.Rate(self.rate)
+
+ self.point_cloud_pub = rospy.Publisher(
+ '/funmap/point_cloud2', PointCloud2, queue_size=1)
+ # self.voi_marker_pub = rospy.Publisher(
+ # '/funmap/voi_marker', Marker, queue_size=1)
+
+ self.tfBuffer = tf2_ros.Buffer()
+ self.listener = tf2_ros.TransformListener(self.tfBuffer)
+ self.tf2_broadcaster = tf2_ros.TransformBroadcaster()
+
+ self.map_to_odom_transform_mat = np.identity(4)
+ dock_pose = Pose()
+ predock_pose = Pose()
+ while not rospy.is_shutdown():
+ self.tf2_broadcaster.sendTransform(
+ create_map_to_odom_transform(self.map_to_odom_transform_mat))
+ self.publish_map_point_cloud()
+
+ try:
+ self.aruco_tf.header.stamp = rospy.Time.now()
+ self.predock_tf.header.stamp = rospy.Time.now()
+ self.tf2_broadcaster.sendTransform(self.aruco_tf)
+ if self.aruco_name == 'docking_station':
+ dock_pose.position.x = self.aruco_tf.transform.translation.x
+ dock_pose.position.y = self.aruco_tf.transform.translation.y
+ dock_pose.position.z = self.aruco_tf.transform.translation.z
+ dock_pose.orientation.x = self.aruco_tf.transform.rotation.x
+ dock_pose.orientation.y = self.aruco_tf.transform.rotation.y
+ dock_pose.orientation.z = self.aruco_tf.transform.rotation.z
+ dock_pose.orientation.w = self.aruco_tf.transform.rotation.w
+ self.dock_pose_pub.publish(dock_pose)
+ self.tf2_broadcaster.sendTransform(self.predock_tf)
+ predock_pose.position.x = self.predock_tf.transform.translation.x
+ predock_pose.position.y = self.predock_tf.transform.translation.y
+ predock_pose.position.z = self.predock_tf.transform.translation.z
+ predock_pose.orientation.x = self.predock_tf.transform.rotation.x
+ predock_pose.orientation.y = self.predock_tf.transform.rotation.y
+ predock_pose.orientation.z = self.predock_tf.transform.rotation.z
+ predock_pose.orientation.w = self.predock_tf.transform.rotation.w
+ self.predock_pose_pub.publish(predock_pose)
+ except AttributeError:
+ pass
+ rate.sleep()
+
+
+if __name__ == '__main__':
+ try:
+ node = ArucoHeadScan()
+ node.main()
+ rospy.spin()
+ except KeyboardInterrupt:
+ print('interrupt received, so shutting down')
\ No newline at end of file