diff --git a/stretch_core/nodes/aruco_head_scan_client.py b/stretch_core/nodes/aruco_head_scan_client.py index 77fdb55..f9023ec 100755 --- a/stretch_core/nodes/aruco_head_scan_client.py +++ b/stretch_core/nodes/aruco_head_scan_client.py @@ -5,6 +5,7 @@ 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: @@ -23,6 +24,5 @@ def main(): if __name__ == '__main__': - head_scan = ArucoHeadScan() rospy.loginfo('Ensure stretch_driver is launched before executing') main() 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 0000000..e87348e --- /dev/null +++ b/stretch_funmap/nodes/aruco_head_scan_action.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 + +import rospy +from visualization_msgs.msg import MarkerArray +from geometry_msgs.msg import Transform, TransformStamped +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.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 + self.aruco_tf = None + self.predock_tf = 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 + + 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) + for pan_ang in pan_left: + pose = {'joint_head_pan': pan_ang} + head_scan.capture_point_clouds(node, pose, capture_params) + + 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: + 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': + rospy.loginfo("Publishing predock pose") + # 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 + t = Transform() + t.translation.x = 0.0 + t.translation.y = -0.25 + t.translation.z = 0.5 + t.rotation.x = 0.5 + t.rotation.y = 0.5 + t.rotation.z = 0.5 + t.rotation.w = -0.5 + self.predock_tf = self.broadcast_tf(t, 'predock_pose', self.aruco_name) + time.sleep(1.0) + trans = self.tfBuffer.lookup_transform('map', 'predock_pose', rospy.Time()) + trans.transform.translation.z = 0 + self.predock_tf = self.broadcast_tf(trans, 'predock_pose', 'map') + 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) + 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) + self.tf2_broadcaster.sendTransform(self.predock_tf) + 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