diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 2e8c3df..93746a3 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -121,9 +121,12 @@ class HelloNode: # Query TF2 to obtain the current estimated transformation # from the robot's base_link frame to the frame. robot_to_odom_mat, timestamp = get_p1_to_p2_matrix('base_link', floor_frame, self.tf2_buffer) + print('robot_to_odom_mat =', robot_to_odom_mat) + print('timestamp =', timestamp) # Find the robot's current location in the frame. r0 = np.array([0.0, 0.0, 0.0, 1.0]) + print('r0 =', r0) r0 = np.matmul(robot_to_odom_mat, r0)[:2] # Find the current angle of the robot in the frame. diff --git a/stretch_demos/nodes/handover_object b/stretch_demos/nodes/handover_object index cd1c745..dfaf91d 100755 --- a/stretch_demos/nodes/handover_object +++ b/stretch_demos/nodes/handover_object @@ -1,6 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -from __future__ import print_function +#from __future__ import print_function from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist @@ -107,7 +107,8 @@ class HandoverObjectNode(hm.HelloNode): lookup_time = rospy.Time(0) # return most recent transform timeout_ros = rospy.Duration(0.1) - old_frame_id = self.mouth_point.header.frame_id[1:] + #old_frame_id = self.mouth_point.header.frame_id[1:] + old_frame_id = self.mouth_point.header.frame_id[:] new_frame_id = 'base_link' stamped_transform = self.tf2_buffer.lookup_transform(new_frame_id, old_frame_id, lookup_time, timeout_ros) points_in_old_frame_to_new_frame_mat = rn.numpify(stamped_transform.transform) diff --git a/stretch_demos/rviz/handover_object.rviz b/stretch_demos/rviz/handover_object.rviz index 05404b0..b6b578b 100644 --- a/stretch_demos/rviz/handover_object.rviz +++ b/stretch_demos/rviz/handover_object.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 728 + Tree Height: 719 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -220,11 +220,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - link_puller: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true link_right_wheel: Alpha: 1 Show Axes: false @@ -249,8 +244,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 3.660540819168091 - Min Value: -0.17842411994934082 + Max Value: 10 + Min Value: -10 Value: true Axis: Z Channel Name: intensity @@ -261,16 +256,14 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 - Style: Points + Style: Flat Squares Topic: /camera/depth/color/points Unreliable: false Use Fixed Frame: true @@ -281,7 +274,7 @@ Visualization Manager: Marker Topic: /nearest_mouth/axes Name: MarkerArray Namespaces: - {} + "": true Queue Size: 100 Value: true Enabled: true @@ -318,6 +311,7 @@ Visualization Manager: Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: -0.28927484154701233 Y: -0.1668422967195511 @@ -329,16 +323,15 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.3497966527938843 Target Frame: - Value: Orbit (rviz) Yaw: 0.9340142011642456 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1025 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001c900000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002ed000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000056e0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c90000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002ed000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005690000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -347,6 +340,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 + Width: 1848 + X: 72 Y: 27 diff --git a/stretch_funmap/nodes/funmap b/stretch_funmap/nodes/funmap index 86317f6..eab4ab7 100755 --- a/stretch_funmap/nodes/funmap +++ b/stretch_funmap/nodes/funmap @@ -1,6 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -from __future__ import print_function +#from __future__ import print_function import rospy import actionlib @@ -43,6 +43,7 @@ import stretch_funmap.segment_max_height_image as sm import stretch_funmap.navigation_planning as na import stretch_funmap.manipulation_planning as mp + def create_map_to_odom_transform(t_mat): t = TransformStamped() t.header.stamp = rospy.Time.now() @@ -62,12 +63,12 @@ class ContactDetector(): self.contact_state_lock = threading.Lock() self.contact_mode = None self.contact_mode_lock = threading.Lock() - + self.position = None self.av_effort = None self.av_effort_window_size = 3 - self.get_joint_state = get_joint_state + self.get_joint_state = get_joint_state self.direction_sign = None self.stopping_position = None @@ -77,7 +78,7 @@ class ContactDetector(): def set_regulate_contact(self): with self.contact_mode_lock: return self.contact_mode == 'regulate_contact' - + def set_stopping_position(self, stopping_position, direction_sign): assert((direction_sign == -1) or (direction_sign == 1)) self.stopping_position = stopping_position @@ -90,10 +91,10 @@ class ContactDetector(): def contact_position(self): with self.contact_state_lock: return self.in_contact_position - + def get_position(self): return self.position - + def passed_stopping_position(self): if (self.position is None) or (self.stopping_position is None): return False @@ -105,15 +106,15 @@ class ContactDetector(): def not_stopped(self): with self.contact_mode_lock: return self.contact_mode == 'stop_on_contact' - - def reset(self): + + def reset(self): with self.contact_state_lock: self.in_contact = False self.in_contact_position = None self.turn_off() self.stopping_position = None self.direction_sign = None - + def turn_off(self): with self.contact_mode_lock: self.contact_mode = None @@ -131,18 +132,20 @@ class ContactDetector(): self.position = position # First, check that the stopping position, if defined, has not been passed - if self.passed_stopping_position(): - trigger_request = TriggerRequest() - trigger_result = stop_the_robot_service(trigger_request) + if self.passed_stopping_position(): + trigger_request = TriggerRequest() + trigger_result = stop_the_robot_service(trigger_request) with self.contact_mode_lock: self.contact_mode = 'passed_stopping_point' - rospy.loginfo('stop_on_contact: stopping the robot due to passing the stopping position, position = {0}, stopping_position = {1}, direction_sign = {2}'.format(self.position, self.stopping_position, self.direction_sign)) + rospy.loginfo('stop_on_contact: stopping the robot due to passing the stopping position, position = {0}, stopping_position = {1}, direction_sign = {2}'.format( + self.position, self.stopping_position, self.direction_sign)) # Second, check that the effort thresholds have not been exceeded if self.av_effort is None: self.av_effort = effort else: - self.av_effort = (((self.av_effort_window_size - 1.0) * self.av_effort) + effort) / self.av_effort_window_size + self.av_effort = (((self.av_effort_window_size - 1.0) * + self.av_effort) + effort) / self.av_effort_window_size if self.in_contact_func(effort, self.av_effort): # Contact detected! @@ -151,9 +154,10 @@ class ContactDetector(): self.in_contact_position = self.position with self.contact_mode_lock: if self.contact_mode == 'stop_on_contact': - trigger_request = TriggerRequest() - trigger_result = stop_the_robot_service(trigger_request) - rospy.loginfo('stop_on_contact: stopping the robot due to detected contact, effort = {0}, av_effort = {1}'.format(effort, self.av_effort)) + trigger_request = TriggerRequest() + trigger_result = stop_the_robot_service(trigger_request) + rospy.loginfo('stop_on_contact: stopping the robot due to detected contact, effort = {0}, av_effort = {1}'.format( + effort, self.av_effort)) self.contact_mode = 'regulate_contact' elif self.contact_mode == 'regulate_contact': pass @@ -164,14 +168,13 @@ class ContactDetector(): else: pass - def move_until_contact(self, joint_name, stopping_position, direction_sign, move_to_pose): self.reset() self.set_stopping_position(stopping_position, direction_sign) - + success = False message = 'Unknown result.' - + if not self.passed_stopping_position(): # The target has not been passed self.turn_on() @@ -181,26 +184,27 @@ class ContactDetector(): finished = False while self.not_stopped(): position = self.get_position() - if position is not None: + if position is not None: new_target = self.get_position() + move_increment - pose = {joint_name : new_target} + pose = {joint_name: new_target} move_to_pose(pose, return_before_done=True) move_rate.sleep() if self.is_in_contact(): # back off from the detected contact location - + contact_position = self.contact_position() - if contact_position is not None: - new_target = contact_position - 0.001 #- 0.002 + if contact_position is not None: + new_target = contact_position - 0.001 # - 0.002 else: - new_target = self.position() - 0.001 #- 0.002 - pose = {joint_name : new_target} + new_target = self.position() - 0.001 # - 0.002 + pose = {joint_name: new_target} move_to_pose(pose, return_before_done=False) - rospy.loginfo('backing off after contact: moving away from surface to decrease force') + rospy.loginfo( + 'backing off after contact: moving away from surface to decrease force') success = True message = 'Successfully reached until contact.' - else: + else: success = False message = 'Terminated without detecting contact.' @@ -208,8 +212,7 @@ class ContactDetector(): return success, message - - + class FunmapNode(hm.HelloNode): def __init__(self, map_filename): @@ -217,7 +220,7 @@ class FunmapNode(hm.HelloNode): self.map_filename = map_filename self.debug_directory = None - + # This holds all the poses the robot's mobile base was in # while making scans merged into the map. They are defined # with respect to the map's image. One use of this list is to @@ -227,25 +230,28 @@ class FunmapNode(hm.HelloNode): # floor. self.robot_poses = [] self.prev_nav_markers = None - + self.wrist_position = None - - self.use_hook = False #True #False - if self.use_hook: + self.use_hook = False # True #False + + if self.use_hook: def extension_contact_func(effort, av_effort): single_effort_threshold = 38.0 av_effort_threshold = 34.0 if (effort >= single_effort_threshold): - rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(effort, single_effort_threshold)) + rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format( + effort, single_effort_threshold)) if (av_effort >= av_effort_threshold): - rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(av_effort, av_effort_threshold)) + rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format( + av_effort, av_effort_threshold)) return ((effort >= single_effort_threshold) or (av_effort > av_effort_threshold)) - self.extension_contact_detector = ContactDetector(hm.get_wrist_state, extension_contact_func, move_increment=0.008) + self.extension_contact_detector = ContactDetector( + hm.get_wrist_state, extension_contact_func, move_increment=0.008) else: def extension_contact_func(effort, av_effort): @@ -253,44 +259,50 @@ class FunmapNode(hm.HelloNode): av_effort_threshold = 40.0 if (effort >= single_effort_threshold): - rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(effort, single_effort_threshold)) + rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format( + effort, single_effort_threshold)) if (av_effort >= av_effort_threshold): - rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(av_effort, av_effort_threshold)) - + rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format( + av_effort, av_effort_threshold)) + return ((effort >= single_effort_threshold) or (av_effort > av_effort_threshold)) - - self.extension_contact_detector = ContactDetector(hm.get_wrist_state, extension_contact_func) - + + self.extension_contact_detector = ContactDetector( + hm.get_wrist_state, extension_contact_func) + def lift_contact_func(effort, av_effort): single_effort_threshold = 20.0 av_effort_threshold = 20.0 - + if (effort <= single_effort_threshold): - rospy.loginfo('Lift single effort less than single_effort_threshold: {0} <= {1}'.format(effort, single_effort_threshold)) + rospy.loginfo('Lift single effort less than single_effort_threshold: {0} <= {1}'.format( + effort, single_effort_threshold)) if (av_effort <= av_effort_threshold): - rospy.loginfo('Lift average effort less than av_effort_threshold: {0} <= {1}'.format(av_effort, av_effort_threshold)) - + rospy.loginfo('Lift average effort less than av_effort_threshold: {0} <= {1}'.format( + av_effort, av_effort_threshold)) + return ((effort <= single_effort_threshold) or (av_effort < av_effort_threshold)) - - self.lift_down_contact_detector = ContactDetector(hm.get_lift_state, lift_contact_func) - - + + self.lift_down_contact_detector = ContactDetector( + hm.get_lift_state, lift_contact_func) + 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) + marker = self.merged_map.max_height_im.voi.get_ros_marker( + duration=1000.0) self.voi_marker_pub.publish(marker) - def publish_nav_plan_markers(self, line_segment_path, image_to_points_mat, clicked_frame_id): path_height_m = 0.2 - points = [np.matmul(image_to_points_mat, np.array([p[0], p[1], path_height_m, 1.0]))[:3] for p in line_segment_path] + points = [np.matmul(image_to_points_mat, np.array( + [p[0], p[1], path_height_m, 1.0]))[:3] for p in line_segment_path] points = [[p[0], p[1], path_height_m] for p in points] self.publish_path_markers(points, clicked_frame_id) @@ -303,18 +315,19 @@ class FunmapNode(hm.HelloNode): m.action = m.DELETE self.navigation_plan_markers_pub.publish(self.prev_nav_markers) nav_markers = MarkerArray() - duration_s = 1 * 60 + duration_s = 1 * 60 timestamp = rospy.Time.now() - m = hr.create_line_strip(points, 0, points_frame_id, timestamp, rgba=[0.0, 1.0, 0.0, 1.0], line_width_m=0.05, duration_s=duration_s) + m = hr.create_line_strip(points, 0, points_frame_id, timestamp, rgba=[ + 0.0, 1.0, 0.0, 1.0], line_width_m=0.05, duration_s=duration_s) nav_markers.markers.append(m) for i, p in enumerate(points): - m = hr.create_sphere_marker(p, i+1, points_frame_id, timestamp, rgba=[1.0, 1.0, 1.0, 1.0], diameter_m=0.15, duration_s=duration_s) + m = hr.create_sphere_marker(p, i+1, points_frame_id, timestamp, rgba=[ + 1.0, 1.0, 1.0, 1.0], diameter_m=0.15, duration_s=duration_s) nav_markers.markers.append(m) self.navigation_plan_markers_pub.publish(nav_markers) self.prev_nav_markers = nav_markers - - def trigger_align_with_nearest_cliff_service_callback(self, request): + def trigger_align_with_nearest_cliff_service_callback(self, request): manip = mp.ManipulationView(self.tf2_buffer, self.debug_directory) manip.move_head(self.move_to_pose) manip.update(self.point_cloud, self.tf2_buffer) @@ -326,9 +339,10 @@ class FunmapNode(hm.HelloNode): filename = 'nearest_cliff_scan_' + hm.create_time_string() manip.save_scan(dirname + filename) else: - rospy.loginfo('FunmapNode trigger_align_with_nearest_cliff_service_callback: No debug directory provided, so debugging data will not be saved.') + rospy.loginfo( + 'FunmapNode trigger_align_with_nearest_cliff_service_callback: No debug directory provided, so debugging data will not be saved.') p0, p1, normal = manip.get_nearest_cliff('odom', self.tf2_buffer) - if normal is not None: + if normal is not None: cliff_ang = np.arctan2(normal[1], normal[0]) # Find the robot's current pose in the odom frame. @@ -342,32 +356,34 @@ class FunmapNode(hm.HelloNode): # Command the robot to turn to point to the next # waypoint. - at_goal = self.move_base.turn(turn_ang, publish_visualizations=True) + 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) - success=False - message=message_text + success = False + message = message_text else: success = True message = 'Aligned with the nearest edge.' else: success = False message = 'Failed to detect cliff.' - + return TriggerResponse( success=success, message=message - ) - + ) + def joint_states_callback(self, joint_states): - self.extension_contact_detector.update(joint_states, self.stop_the_robot_service) + self.extension_contact_detector.update( + joint_states, self.stop_the_robot_service) self.wrist_position = self.extension_contact_detector.get_position() - self.lift_down_contact_detector.update(joint_states, self.stop_the_robot_service) + self.lift_down_contact_detector.update( + joint_states, self.stop_the_robot_service) self.lift_position = self.lift_down_contact_detector.get_position() - def trigger_reach_until_contact_service_callback(self, request): manip = mp.ManipulationView(self.tf2_buffer, self.debug_directory) manip.move_head(self.move_to_pose) @@ -380,13 +396,15 @@ class FunmapNode(hm.HelloNode): filename = 'reach_until_contact_' + hm.create_time_string() manip.save_scan(dirname + filename) else: - rospy.loginfo('FunmapNode trigger_reach_until_contact_service_callback: No debug directory provided, so debugging data will not be saved.') + rospy.loginfo( + 'FunmapNode trigger_reach_until_contact_service_callback: No debug directory provided, so debugging data will not be saved.') if self.use_hook: tooltip_frame = 'link_hook' else: tooltip_frame = 'link_grasp_center' - reach_m = manip.estimate_reach_to_contact_distance(tooltip_frame, self.tf2_buffer) + reach_m = manip.estimate_reach_to_contact_distance( + tooltip_frame, self.tf2_buffer) rospy.loginfo('----------------') rospy.loginfo('reach_m = {0}'.format(reach_m)) rospy.loginfo('----------------') @@ -412,63 +430,65 @@ class FunmapNode(hm.HelloNode): # Or, a freespace reach was not observed, so reach # cautiously over the full reach. direction_sign = 1 - success, message = self.extension_contact_detector.move_until_contact('wrist_extension', max_reach_target_m, direction_sign, self.move_to_pose) - else: + success, message = self.extension_contact_detector.move_until_contact( + 'wrist_extension', max_reach_target_m, direction_sign, self.move_to_pose) + else: # A freespace region was observed. Agressively move to # within a safe distance of the expected obstacle. - + safety_margin_m = 0.02 safe_target_m = reach_target_m - safety_margin_m if self.use_hook: safe_target_m = safe_target_m + 0.03 - if safe_target_m > self.wrist_position: - pose = {'wrist_extension' : safe_target_m} + if safe_target_m > self.wrist_position: + pose = {'wrist_extension': safe_target_m} self.move_to_pose(pose, return_before_done=False) # target depth within the surface - target_depth_m = 0.08 + target_depth_m = 0.08 in_contact_target_m = reach_target_m + target_depth_m direction_sign = 1 - success, message = self.extension_contact_detector.move_until_contact('wrist_extension', in_contact_target_m, direction_sign, self.move_to_pose) - + success, message = self.extension_contact_detector.move_until_contact( + 'wrist_extension', in_contact_target_m, direction_sign, self.move_to_pose) + return TriggerResponse( success=success, message=message - ) - + ) def trigger_lower_until_contact_service_callback(self, request): direction_sign = -1 lowest_allowed_m = 0.3 - success, message = self.lift_down_contact_detector.move_until_contact('joint_lift', lowest_allowed_m, direction_sign, self.move_to_pose) + success, message = self.lift_down_contact_detector.move_until_contact( + 'joint_lift', lowest_allowed_m, direction_sign, self.move_to_pose) return TriggerResponse( success=success, message=message - ) + ) - def trigger_global_localization_service_callback(self, request): self.perform_head_scan(localize_only=True, global_localization=True) return TriggerResponse( success=True, message='Completed localization with scan.' - ) - + ) + def trigger_local_localization_service_callback(self, request): - self.perform_head_scan(localize_only=True, global_localization=False, fast_scan=True) + self.perform_head_scan( + localize_only=True, global_localization=False, fast_scan=True) return TriggerResponse( success=True, message='Completed localization with scan.' - ) - + ) + def trigger_head_scan_service_callback(self, request): self.perform_head_scan() return TriggerResponse( success=True, message='Completed head scan.' - ) - + ) + def trigger_drive_to_scan_service_callback(self, request): if self.merged_map is None: @@ -476,25 +496,26 @@ class FunmapNode(hm.HelloNode): success=False, message='No map exists yet, so unable to drive to a good scan spot.' ) - + max_height_im = self.merged_map.max_height_im - - robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer) + + robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image( + self.tf2_buffer) robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad] robot_x_pix = int(round(robot_xy_pix[0])) robot_y_pix = int(round(robot_xy_pix[1])) - + # Define the target maximum observation distance for any # observed point in the map. This serves as a goal for mapping. max_scan_distance_m = 1.5 - - # The best case minimum width of the robot in meters when moving forward and backward. + + # The best case minimum width of the robot in meters when moving forward and backward. min_robot_width_m = 0.34 - + camera_height_m = 1.12 floor_mask = sm.compute_floor_mask(max_height_im) - + # Select the next location on the map from which to # attempt to make a head scan. best_xy = na.select_next_scan_location(floor_mask, max_height_im, min_robot_width_m, @@ -506,35 +527,37 @@ class FunmapNode(hm.HelloNode): success=False, message='No good scan location was detected.' ) - + # Plan an optimistic path on the floor to the next # location for scanning. end_xy = np.array(best_xy) - success, message = self.navigate_to_map_pixel(end_xy, robot_xya_pix=robot_xya_pix, floor_mask=floor_mask) + success, message = self.navigate_to_map_pixel( + end_xy, robot_xya_pix=robot_xya_pix, floor_mask=floor_mask) return TriggerResponse( - success=success, + success=success, message=message ) - + def pose_to_map_pixel(self, pose_stamped): clicked_frame_id = pose_stamped.header.frame_id clicked_timestamp = pose_stamped.header.stamp clicked_point = pose_stamped.pose.position - + # Check if a map exists if self.merged_map is None: success = False message = 'No map exists yet, so unable to drive to a good scan spot.' rospy.logerr(message) return None - + max_height_im = self.merged_map.max_height_im map_frame_id = self.merged_map.max_height_im.voi.frame_id - - points_to_image_mat, pi_timestamp = max_height_im.get_points_to_image_mat(clicked_frame_id, self.tf2_buffer) - #lookup_time=clicked_timestamp) + + points_to_image_mat, pi_timestamp = max_height_im.get_points_to_image_mat( + clicked_frame_id, self.tf2_buffer) + # lookup_time=clicked_timestamp) if (points_to_image_mat is not None): c_x = clicked_point.x @@ -549,8 +572,7 @@ class FunmapNode(hm.HelloNode): return end_xy return None - - + def plan_a_path(self, end_xy_pix, robot_xya_pix=None, floor_mask=None): # Transform the robot's current estimated pose as represented # by TF2 to the map image. Currently, the estimated pose is @@ -558,15 +580,16 @@ class FunmapNode(hm.HelloNode): # base_link frame, which is updated by odometry and # corrections based on matching head scans to the map. path = None - + # Check if a map exists if self.merged_map is None: message = 'No map exists yet, so unable to drive to a good scan spot.' return path, message max_height_im = self.merged_map.max_height_im - if robot_xya_pix is None: - robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer) + if robot_xya_pix is None: + robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image( + self.tf2_buffer) robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad] max_height_im = self.merged_map.max_height_im @@ -574,8 +597,6 @@ class FunmapNode(hm.HelloNode): end_xy_pix, floor_mask=floor_mask) return line_segment_path, message - - def plan_to_reach(self, reach_xyz_pix, robot_xya_pix=None, floor_mask=None): # This is intended to perform coarse positioning of the # gripper near a target 3D point. @@ -583,18 +604,19 @@ class FunmapNode(hm.HelloNode): wrist_extension_m = None i_x, i_y, i_z = reach_xyz_pix - + max_height_im = self.merged_map.max_height_im # Check if a map exists if self.merged_map is None: message = 'No map exists yet, so unable to plan a reach.' rospy.logerr(message) return None, None - - if robot_xya_pix is None: - robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer) + + if robot_xya_pix is None: + robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image( + self.tf2_buffer) robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad] - + end_xy_pix = np.int64(np.round(np.array([i_x, i_y]))) m_per_height_unit = max_height_im.m_per_height_unit # move the gripper to be above the target point @@ -610,19 +632,20 @@ class FunmapNode(hm.HelloNode): target_xyz_pix, robot_xya_pix, image_display_on=image_display_on) - if image_display_on: + if image_display_on: c = cv2.waitKey(0) - + if base_x_pix is None: rospy.logerr('No valid base pose found for reaching the target.') return None, None robot_reach_xya_pix = [base_x_pix, base_y_pix, base_ang_rad] - base_link_point = max_height_im.get_pix_in_frame(np.array(reach_xyz_pix), 'base_link', self.tf2_buffer) - + base_link_point = max_height_im.get_pix_in_frame( + np.array(reach_xyz_pix), 'base_link', self.tf2_buffer) + simple_reach_plan = [] - + # close the gripper simple_reach_plan.append({'joint_gripper_finger_left': 0.0}) @@ -634,25 +657,25 @@ class FunmapNode(hm.HelloNode): height_m = base_link_point[2] safety_z_m = 0.0 simple_reach_plan.append({'joint_lift': height_m + safety_z_m}) - + # rotate the gripper to be in the center # of the swept volume of the wrist (a # little right of center when looking out # from the robot to the gripper) #simple_reach_plan.append({'joint_gripper': -0.25}) simple_reach_plan.append({'joint_wrist_yaw': -0.25}) - + # reach out to the target # Reach to a point that is not fully at the target. - safety_reach_m = 0.1 # 10cm away from the target - simple_reach_plan.append({'wrist_extension': wrist_extension_m - safety_reach_m}) - + safety_reach_m = 0.1 # 10cm away from the target + simple_reach_plan.append( + {'wrist_extension': wrist_extension_m - safety_reach_m}) + return robot_reach_xya_pix, simple_reach_plan - def reach_to_click_callback(self, clicked_msg): rospy.loginfo('clicked_msg =' + str(clicked_msg)) - + clicked_frame_id = clicked_msg.header.frame_id clicked_timestamp = clicked_msg.header.stamp clicked_point = clicked_msg.point @@ -664,7 +687,8 @@ class FunmapNode(hm.HelloNode): rospy.logerr(message) return - points_to_image_mat, pi_timestamp = max_height_im.get_points_to_image_mat(clicked_frame_id, self.tf2_buffer) + points_to_image_mat, pi_timestamp = max_height_im.get_points_to_image_mat( + clicked_frame_id, self.tf2_buffer) if points_to_image_mat is None: rospy.logerr('points_to_image_mat not found') @@ -680,20 +704,23 @@ class FunmapNode(hm.HelloNode): h, w = max_height_im.image.shape if not ((i_x >= 0) and (i_y >= 0) and (i_x < w) and (i_y < h)): - rospy.logerr('clicked point does not fall within the bounds of the max_height_image') - return + rospy.logerr( + 'clicked point does not fall within the bounds of the max_height_image') + return - robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer) + robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image( + self.tf2_buffer) robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad] - + reach_xyz_pix = clicked_image_pixel - robot_reach_xya_pix, simple_reach_plan = self.plan_to_reach(reach_xyz_pix, robot_xya_pix=robot_xya_pix) + robot_reach_xya_pix, simple_reach_plan = self.plan_to_reach( + reach_xyz_pix, robot_xya_pix=robot_xya_pix) success, message = self.navigate_to_map_pixel(robot_reach_xya_pix[:2], end_angle=robot_reach_xya_pix[2], robot_xya_pix=robot_xya_pix) - if success: + if success: for pose in simple_reach_plan: self.move_to_pose(pose) else: @@ -701,12 +728,12 @@ class FunmapNode(hm.HelloNode): rospy.logerr('Aborting reach attempt due to failed navigation') return - - + def navigate_to_map_pixel(self, end_xy, end_angle=None, robot_xya_pix=None, floor_mask=None): # Set the D435i to Default mode for obstacle detection - trigger_request = TriggerRequest() - trigger_result = self.trigger_d435i_default_mode_service(trigger_request) + trigger_request = TriggerRequest() + trigger_result = self.trigger_d435i_default_mode_service( + trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Move the head to a pose from which the D435i can detect @@ -714,7 +741,8 @@ class FunmapNode(hm.HelloNode): # forward. self.move_base.head_to_forward_motion_pose() - line_segment_path, message = self.plan_a_path(end_xy, robot_xya_pix=robot_xya_pix, floor_mask=floor_mask) + line_segment_path, message = self.plan_a_path( + end_xy, robot_xya_pix=robot_xya_pix, floor_mask=floor_mask) if line_segment_path is None: success = False return success, message @@ -726,15 +754,17 @@ class FunmapNode(hm.HelloNode): return success, 'No map available for planning and navigation.' max_height_im = self.merged_map.max_height_im map_frame_id = self.merged_map.max_height_im.voi.frame_id - + # Query TF2 to obtain the current estimated transformation # from the map image to the map frame. - image_to_points_mat, ip_timestamp = max_height_im.get_image_to_points_mat(map_frame_id, self.tf2_buffer) - - if image_to_points_mat is not None: + image_to_points_mat, ip_timestamp = max_height_im.get_image_to_points_mat( + map_frame_id, self.tf2_buffer) + + if image_to_points_mat is not None: # Publish a marker array to visualize the line segment path. - self.publish_nav_plan_markers(line_segment_path, image_to_points_mat, map_frame_id) + self.publish_nav_plan_markers( + line_segment_path, image_to_points_mat, map_frame_id) # Iterate through the vertices of the line segment path, # commanding the robot to drive to them in sequence using @@ -743,11 +773,13 @@ class FunmapNode(hm.HelloNode): for p0, p1 in zip(line_segment_path, line_segment_path[1:]): # Query TF2 to obtain the current estimated transformation # from the image to the odometry frame. - image_to_odom_mat, io_timestamp = max_height_im.get_image_to_points_mat('odom', self.tf2_buffer) + image_to_odom_mat, io_timestamp = max_height_im.get_image_to_points_mat( + 'odom', self.tf2_buffer) # Query TF2 to obtain the current estimated transformation # from the robot's base_link frame to the odometry frame. - robot_to_odom_mat, ro_timestamp = hm.get_p1_to_p2_matrix('base_link', 'odom', self.tf2_buffer) + robot_to_odom_mat, ro_timestamp = hm.get_p1_to_p2_matrix( + 'base_link', 'odom', self.tf2_buffer) # Navigation planning is performed with respect to a # odom frame height of 0.0, so the heights of @@ -756,10 +788,10 @@ class FunmapNode(hm.HelloNode): # frame is aligned with the floor, so that ignoring # the z coordinate is approximately equivalent to # projecting a point onto the floor. - + # Convert the current and next waypoints from map # image pixel coordinates to the odom - # frame. + # frame. p0 = np.array([p0[0], p0[1], 0.0, 1.0]) p0 = np.matmul(image_to_odom_mat, p0)[:2] p1 = np.array([p1[0], p1[1], 0.0, 1.0]) @@ -770,7 +802,7 @@ class FunmapNode(hm.HelloNode): xya, timestamp = self.get_robot_floor_pose_xya() r0 = xya[:2] r_ang = xya[2] - + # Check how far the robot's current location is from # its current waypoint. The current waypoint is where # the robot would ideally be located. @@ -780,8 +812,8 @@ class FunmapNode(hm.HelloNode): if waypoint_error > waypoint_tolerance_m: message_text = 'Failed due to waypoint_error being above the maximum allowed error.' rospy.loginfo(message_text) - success=False - message=message_text + success = False + message = message_text return success, message # Find the angle in the odometry frame that would @@ -798,15 +830,17 @@ class FunmapNode(hm.HelloNode): # 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) + 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) - success=False - message=message_text + success = False + message = message_text return success, message - + # The head seems to drift sometimes over time, such # that the obstacle detection region is no longer # observed resulting in false positives. Hopefully, @@ -815,65 +849,75 @@ class FunmapNode(hm.HelloNode): # FOR FUTURE DEVELOPMENT OF LOCAL NAVIGATION testing_future_code = False - if testing_future_code: - check_result = self.move_base.check_line_path(next_point_xyz, 'odom') - rospy.loginfo('Result of check line path = {0}'.format(check_result)) - local_path, local_path_frame_id = self.move_base.local_plan(next_point_xyz, 'odom') + if testing_future_code: + check_result = self.move_base.check_line_path( + next_point_xyz, 'odom') + rospy.loginfo( + 'Result of check line path = {0}'.format(check_result)) + local_path, local_path_frame_id = self.move_base.local_plan( + next_point_xyz, 'odom') if local_path is not None: - rospy.loginfo('Found local path! Publishing markers for it!') - self.publish_path_markers(local_path, local_path_frame_id) + rospy.loginfo( + 'Found local path! Publishing markers for it!') + self.publish_path_markers( + local_path, local_path_frame_id) else: rospy.loginfo('Did not find a local path...') - - # Command the robot to move forward to the next waypoing. - at_goal = self.move_base.forward(travel_dist, publish_visualizations=True) + + # Command the robot to move forward to the next waypoing. + at_goal = self.move_base.forward( + travel_dist, publish_visualizations=True) if not at_goal: message_text = 'Failed to reach forward motion goal.' rospy.loginfo(message_text) - success=False - message=message_text + success = False + message = message_text return success, message - + rospy.loginfo('Turn and forward motion succeeded.') if end_angle is not None: # If a final target angle has been provided, rotate # the robot to match the target angle. - rospy.loginfo('Attempting to achieve the final target orientation.') - + rospy.loginfo( + 'Attempting to achieve the final target orientation.') + # Find the robot's current pose in the map frame. This # assumes that the target angle has been specified # with respect to the map frame. - xya, timestamp = self.get_robot_floor_pose_xya(floor_frame='map') + xya, timestamp = self.get_robot_floor_pose_xya( + floor_frame='map') r_ang = xya[2] - + # Find the angle that the robot should turn in order # to point toward the next waypoint. turn_ang = hm.angle_diff_rad(end_angle, r_ang) # 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) + 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) - success=False - message=message_text + success = False + message = message_text return success, message - - success=True - message='Completed drive to new scan location.' + + success = True + message = 'Completed drive to new scan location.' return success, message - def perform_head_scan(self, fill_in_blindspot_with_second_scan=True, localize_only=False, global_localization=False, fast_scan=False): node = self - - trigger_request = TriggerRequest() - trigger_result = self.trigger_d435i_high_accuracy_mode_service(trigger_request) + + trigger_request = TriggerRequest() + trigger_result = self.trigger_d435i_high_accuracy_mode_service( + trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - + # Reduce the occlusion due to the arm and grabber. This is # intended to be run when the standard grabber is not holding # an object. @@ -896,36 +940,41 @@ class FunmapNode(hm.HelloNode): filename = 'head_scan_' + hm.create_time_string() head_scan.save(dirname + filename) else: - rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') + rospy.loginfo( + 'FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') head_scan.make_robot_footprint_unobserved() save_merged_map = False - + if self.merged_map is None: # The robot does not currently have a map, so initialize # the map with the new head scan. - rospy.loginfo('perform_head_scan: No map available, so setting the map to be the scan that was just taken.') + rospy.loginfo( + 'perform_head_scan: No map available, so setting the map to be the scan that was just taken.') self.merged_map = head_scan - robot_pose = [head_scan.robot_xy_pix[0], head_scan.robot_xy_pix[1], head_scan.robot_ang_rad] + robot_pose = [head_scan.robot_xy_pix[0], + head_scan.robot_xy_pix[1], head_scan.robot_ang_rad] self.robot_poses.append(robot_pose) self.localized = True save_merged_map = True else: if localize_only and (not global_localization): # The scan was performed to localize the robot locally. - rospy.loginfo('perform_head_scan: Performing local localization.') + rospy.loginfo( + 'perform_head_scan: Performing local localization.') use_full_size_scans = False - if use_full_size_scans: + if use_full_size_scans: affine_matrix, original_robot_map_pose, corrected_robot_map_pose = mm.estimate_scan_1_to_scan_2_transform(head_scan, - self.merged_map, + self.merged_map, display_on=False, show_unaligned=False, full_localization=False, init_target=None, grid_search=False, small_search=False) - else: - original_robot_map_frame_pose, corrected_robot_map_frame_pose, original_robot_map_image_pose, corrected_robot_map_image_pose, scaled_scan, scaled_merged_map = ma.localize_with_reduced_images(head_scan, self.merged_map, global_localization=False, divisor=2, small_search=True) + else: + original_robot_map_frame_pose, corrected_robot_map_frame_pose, original_robot_map_image_pose, corrected_robot_map_image_pose, scaled_scan, scaled_merged_map = ma.localize_with_reduced_images( + head_scan, self.merged_map, global_localization=False, divisor=2, small_search=True) corrected_robot_map_pose = corrected_robot_map_frame_pose original_robot_map_pose = original_robot_map_frame_pose @@ -941,7 +990,8 @@ class FunmapNode(hm.HelloNode): filename = 'localization_scaled_merged_map_' + time_string scaled_merged_map.save(dirname + filename) else: - rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') + rospy.loginfo( + 'FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') self.localized = True elif (not self.localized) or (localize_only and global_localization): # The robot has not been localized with respect to the @@ -952,10 +1002,12 @@ class FunmapNode(hm.HelloNode): # search for a match globally. # This does not merge the new scan into the current map. - rospy.loginfo('perform_head_scan: Performing global localization.') + rospy.loginfo( + 'perform_head_scan: Performing global localization.') save_merged_map = False - - original_robot_map_frame_pose, corrected_robot_map_frame_pose, original_robot_map_image_pose, corrected_robot_map_image_pose, scaled_scan, scaled_merged_map = ma.localize_with_reduced_images(head_scan, self.merged_map, global_localization=True, divisor=6) #4) + + original_robot_map_frame_pose, corrected_robot_map_frame_pose, original_robot_map_image_pose, corrected_robot_map_image_pose, scaled_scan, scaled_merged_map = ma.localize_with_reduced_images( + head_scan, self.merged_map, global_localization=True, divisor=6) # 4) corrected_robot_map_pose = corrected_robot_map_frame_pose original_robot_map_pose = original_robot_map_frame_pose self.localized = True @@ -972,8 +1024,9 @@ class FunmapNode(hm.HelloNode): filename = 'localization_scaled_merged_map_' + time_string scaled_merged_map.save(dirname + filename) else: - rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') - else: + rospy.loginfo( + 'FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') + else: # The robot has been localized with respect to the # current map, so proceed to merge the new head scan # into the map. This assumes that the robot's @@ -981,18 +1034,21 @@ class FunmapNode(hm.HelloNode): # map. It constrains the matching optimization to a # limited range of positions and orientations. rospy.loginfo('perform_head_scan: Performing local map merge.') - original_robot_map_pose, corrected_robot_map_pose = mm.merge_scan_1_into_scan_2(head_scan, self.merged_map) + original_robot_map_pose, corrected_robot_map_pose = mm.merge_scan_1_into_scan_2( + head_scan, self.merged_map) save_merged_map = True - + # Store the corrected robot pose relative to the map frame. self.robot_poses.append(corrected_robot_map_pose) - self.correct_robot_pose(original_robot_map_pose, corrected_robot_map_pose) + self.correct_robot_pose( + original_robot_map_pose, corrected_robot_map_pose) pub_robot_markers = True - if pub_robot_markers: - self.publish_corrected_robot_pose_markers(original_robot_map_pose, corrected_robot_map_pose) - - if save_merged_map: + if pub_robot_markers: + self.publish_corrected_robot_pose_markers( + original_robot_map_pose, corrected_robot_map_pose) + + if save_merged_map: # If the merged map has been updated, save it to disk. if self.debug_directory is not None: head_scans_dirname = self.debug_directory + 'head_scans/' @@ -1004,33 +1060,34 @@ class FunmapNode(hm.HelloNode): if not os.path.exists(merged_maps_dirname): os.makedirs(merged_maps_dirname) time_string = hm.create_time_string() - if scaled_scan is not None: + if scaled_scan is not None: filename = 'localization_scaled_head_scan_' + time_string scaled_scan.save(head_scans_dirname + filename) - if scaled_merged_map is not None: + if scaled_merged_map is not None: filename = 'localization_scaled_merged_map_' + time_string scaled_merged_map.save(merged_maps_dirname + filename) filename = 'merged_map_' + hm.create_time_string() self.merged_map.save(merged_maps_dirname + filename) else: - rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') - + rospy.loginfo( + 'FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.') if fill_in_blindspot_with_second_scan and (not localize_only): # Turn the robot to the left in attempt to fill in its # 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) + 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.perform_head_scan(fill_in_blindspot_with_second_scan=False) - def get_plan_service_callback(self, request): # request.start, request.goal, request.tolerance goal_pose = request.goal @@ -1056,11 +1113,12 @@ class FunmapNode(hm.HelloNode): return success, 'No map available for planning and navigation.' max_height_im = self.merged_map.max_height_im map_frame_id = self.merged_map.max_height_im.voi.frame_id - + # Query TF2 to obtain the current estimated transformation # from the map image to the map frame. - image_to_points_mat, ip_timestamp = max_height_im.get_image_to_points_mat(map_frame_id, self.tf2_buffer) - + image_to_points_mat, ip_timestamp = max_height_im.get_image_to_points_mat( + map_frame_id, self.tf2_buffer) + if image_to_points_mat is None: rospy.logerr('image_to_points_mat unavailable via TF2') return plan @@ -1083,22 +1141,27 @@ class FunmapNode(hm.HelloNode): # the map frame to the odom frame. print('original_robot_map_pose_xya =', original_robot_map_pose_xya) print('corrected_robot_map_pose_xya =', corrected_robot_map_pose_xya) - x_delta = corrected_robot_map_pose_xya[0] - original_robot_map_pose_xya[0] - y_delta = corrected_robot_map_pose_xya[1] - original_robot_map_pose_xya[1] - ang_rad_correction = hm.angle_diff_rad(corrected_robot_map_pose_xya[2], original_robot_map_pose_xya[2]) + x_delta = corrected_robot_map_pose_xya[0] - \ + original_robot_map_pose_xya[0] + y_delta = corrected_robot_map_pose_xya[1] - \ + original_robot_map_pose_xya[1] + ang_rad_correction = hm.angle_diff_rad( + corrected_robot_map_pose_xya[2], original_robot_map_pose_xya[2]) c = np.cos(ang_rad_correction) s = np.sin(ang_rad_correction) rot_mat = np.array([[c, -s], [s, c]]) x_old, y_old, a_old = original_robot_map_pose_xya xy_old = np.array([x_old, y_old]) - tx, ty = np.matmul(rot_mat, -xy_old) + np.array([x_delta, y_delta]) + xy_old + tx, ty = np.matmul(rot_mat, -xy_old) + \ + np.array([x_delta, y_delta]) + xy_old t = np.identity(4) - t[0,3] = tx - t[1,3] = ty - t[:2,:2] = rot_mat - self.map_to_odom_transform_mat = np.matmul(t, self.map_to_odom_transform_mat) - self.tf2_broadcaster.sendTransform(create_map_to_odom_transform(self.map_to_odom_transform_mat)) - + t[0, 3] = tx + t[1, 3] = ty + t[:2, :2] = rot_mat + self.map_to_odom_transform_mat = np.matmul( + t, self.map_to_odom_transform_mat) + self.tf2_broadcaster.sendTransform( + create_map_to_odom_transform(self.map_to_odom_transform_mat)) def publish_corrected_robot_pose_markers(self, original_robot_map_pose_xya, corrected_robot_map_pose_xya): # Publish markers to visualize the corrected and @@ -1111,43 +1174,49 @@ class FunmapNode(hm.HelloNode): point = [x, y, 0.1] rgba = [0.0, 1.0, 0.0, 0.5] m_id = 0 - m = hr.create_sphere_marker(point, m_id, 'map', timestamp, rgba=rgba, diameter_m=0.1, duration_s=0.0) + m = hr.create_sphere_marker( + point, m_id, 'map', timestamp, rgba=rgba, diameter_m=0.1, duration_s=0.0) markers.markers.append(m) m_id += 1 - m = hr.create_axis_marker(point, x_axis, m_id, 'map', timestamp, rgba, length=0.2, arrow_scale=3.0) + m = hr.create_axis_marker( + point, x_axis, m_id, 'map', timestamp, rgba, length=0.2, arrow_scale=3.0) markers.markers.append(m) m_id += 1 x, y, a = original_robot_map_pose_xya point = [x, y, 0.1] rgba = [1.0, 0.0, 0.0, 0.5] - m = hr.create_sphere_marker(point, m_id, 'map', timestamp, rgba=rgba, diameter_m=0.1, duration_s=0.0) + m = hr.create_sphere_marker( + point, m_id, 'map', timestamp, rgba=rgba, diameter_m=0.1, duration_s=0.0) markers.markers.append(m) m_id += 1 - m = hr.create_axis_marker(point, x_axis, m_id, 'map', timestamp, rgba, length=0.2, arrow_scale=3.0) + m = hr.create_axis_marker( + point, x_axis, m_id, 'map', timestamp, rgba, length=0.2, arrow_scale=3.0) markers.markers.append(m) m_id += 1 self.marker_array_pub.publish(markers) - def set_robot_pose_callback(self, pose_with_cov_stamped): - rospy.loginfo('Set robot pose called. This will set the pose of the robot on the map.') + rospy.loginfo( + 'Set robot pose called. This will set the pose of the robot on the map.') rospy.loginfo(pose_with_cov_stamped) - original_robot_map_pose_xya, timestamp = self.get_robot_floor_pose_xya(floor_frame='map') + original_robot_map_pose_xya, timestamp = self.get_robot_floor_pose_xya( + floor_frame='map') pwcs = pose_with_cov_stamped frame_id = pwcs.header.frame_id timestamp = pwcs.header.stamp pose = pwcs.pose.pose - + if frame_id != 'map': - lookup_time = rospy.Time(0) # return most recent transform + lookup_time = rospy.Time(0) # return most recent transform timeout_ros = rospy.Duration(0.1) - stamped_transform = tf2_buffer.lookup_transform('map', frame_id, lookup_time, timeout_ros) + stamped_transform = tf2_buffer.lookup_transform( + 'map', frame_id, lookup_time, timeout_ros) map_pose = do_transform_pose(pose, stamped_transform) else: map_pose = pose - + p = map_pose.position q = map_pose.orientation q_list = [q.x, q.y, q.z, q.w] @@ -1156,13 +1225,15 @@ class FunmapNode(hm.HelloNode): z = p.z roll, pitch, yaw = euler_from_quaternion(q_list) corrected_robot_map_pose_xya = [x, y, yaw] - - self.correct_robot_pose(original_robot_map_pose_xya, corrected_robot_map_pose_xya) - self.publish_corrected_robot_pose_markers(original_robot_map_pose_xya, corrected_robot_map_pose_xya) + self.correct_robot_pose( + original_robot_map_pose_xya, corrected_robot_map_pose_xya) + self.publish_corrected_robot_pose_markers( + original_robot_map_pose_xya, corrected_robot_map_pose_xya) def navigate_to_goal_topic_callback(self, goal_pose): - rospy.loginfo('Navigate to goal simple navigate to goal topic received a command!') + rospy.loginfo( + 'Navigate to goal simple navigate to goal topic received a command!') rospy.loginfo(goal_pose) end_xy = self.pose_to_map_pixel(goal_pose) @@ -1171,18 +1242,18 @@ class FunmapNode(hm.HelloNode): rospy.logerr(message) return success, message = self.navigate_to_map_pixel(end_xy) - + if success: rospy.loginfo(message) else: rospy.logerr(message) return - def navigate_to_goal_action_callback(self, goal): # geometry_msgs/PoseStamped target_pose goal_pose = goal.target_pose - rospy.loginfo('Navigate to goal simple action server received a command!') + rospy.loginfo( + 'Navigate to goal simple action server received a command!') rospy.loginfo(goal_pose) end_xy = self.pose_to_map_pixel(goal_pose) @@ -1192,8 +1263,8 @@ class FunmapNode(hm.HelloNode): self.navigate_to_goal_action_server.set_aborted() return success, message = self.navigate_to_map_pixel(end_xy) - - if success: + + if success: result = MoveBaseResult() self.navigate_to_goal_action_server.set_succeeded(result) else: @@ -1201,38 +1272,38 @@ class FunmapNode(hm.HelloNode): self.navigate_to_goal_action_server.set_aborted() return - def main(self): hm.HelloNode.main(self, 'funmap', 'funmap') self.debug_directory = rospy.get_param('~debug_directory') - + self.merged_map = None self.localized = False if self.map_filename is not None: self.merged_map = ma.HeadScan.from_file(self.map_filename) self.localized = False - + ########################### - # Related to move_base API + # Related to move_base API self.navigate_to_goal_action_server = actionlib.SimpleActionServer('/move_base', MoveBaseAction, - execute_cb = self.navigate_to_goal_action_callback, - auto_start = False) + execute_cb=self.navigate_to_goal_action_callback, + auto_start=False) self.navigate_to_goal_action_server.start() self.navigation_goal_subscriber = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.navigate_to_goal_topic_callback) - self.set_robot_pose_subscriber = rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, self.set_robot_pose_callback) - + self.set_robot_pose_subscriber = rospy.Subscriber( + '/initialpose', PoseWithCovarianceStamped, self.set_robot_pose_callback) + self.get_plan_service = rospy.Service('/make_plan', GetPlan, self.get_plan_service_callback) ########################### - + self.trigger_head_scan_service = rospy.Service('/funmap/trigger_head_scan', Trigger, self.trigger_head_scan_service_callback) @@ -1240,15 +1311,15 @@ class FunmapNode(hm.HelloNode): Trigger, self.trigger_drive_to_scan_service_callback) self.trigger_global_localization_service = rospy.Service('/funmap/trigger_global_localization', - Trigger, - self.trigger_global_localization_service_callback) + Trigger, + self.trigger_global_localization_service_callback) self.trigger_local_localization_service = rospy.Service('/funmap/trigger_local_localization', Trigger, self.trigger_local_localization_service_callback) self.trigger_align_with_nearest_cliff_service = rospy.Service('/funmap/trigger_align_with_nearest_cliff', - Trigger, - self.trigger_align_with_nearest_cliff_service_callback) + Trigger, + self.trigger_align_with_nearest_cliff_service_callback) self.trigger_reach_until_contact_service = rospy.Service('/funmap/trigger_reach_until_contact', Trigger, @@ -1258,46 +1329,59 @@ class FunmapNode(hm.HelloNode): Trigger, self.trigger_lower_until_contact_service_callback) - - self.reach_to_click_subscriber = rospy.Subscriber('/clicked_point', PointStamped, self.reach_to_click_callback) + self.reach_to_click_subscriber = rospy.Subscriber( + '/clicked_point', PointStamped, self.reach_to_click_callback) - default_service = '/camera/switch_to_default_mode' high_accuracy_service = '/camera/switch_to_high_accuracy_mode' - rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service) + rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + + default_service + ' and ' + high_accuracy_service) rospy.wait_for_service(default_service) - rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service) - self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger) + rospy.loginfo('Node ' + self.node_name + + ' connected to ' + default_service) + self.trigger_d435i_default_mode_service = rospy.ServiceProxy( + default_service, Trigger) rospy.wait_for_service(high_accuracy_service) - rospy.loginfo('Node ' + self.node_name + ' connected to' + high_accuracy_service) - self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger) + rospy.loginfo('Node ' + self.node_name + + ' connected to' + high_accuracy_service) + self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy( + high_accuracy_service, Trigger) self.tf2_broadcaster = tf2_ros.TransformBroadcaster() - 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.marker_array_pub = rospy.Publisher('/funmap/marker_array', MarkerArray, queue_size=1) - self.navigation_plan_markers_pub = rospy.Publisher('/funmap/navigation_plan_markers', MarkerArray, queue_size=1) - self.obstacle_point_cloud_pub = rospy.Publisher('/funmap/obstacle_point_cloud2', PointCloud2, queue_size=1) + 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.marker_array_pub = rospy.Publisher( + '/funmap/marker_array', MarkerArray, queue_size=1) + self.navigation_plan_markers_pub = rospy.Publisher( + '/funmap/navigation_plan_markers', MarkerArray, queue_size=1) + self.obstacle_point_cloud_pub = rospy.Publisher( + '/funmap/obstacle_point_cloud2', PointCloud2, queue_size=1) + + self.joint_states_subscriber = rospy.Subscriber( + '/stretch/joint_states', JointState, self.joint_states_callback) - self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) - self.rate = 5.0 rate = rospy.Rate(self.rate) self.move_base = nv.MoveBase(self, self.debug_directory) - + 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.tf2_broadcaster.sendTransform( + create_map_to_odom_transform(self.map_to_odom_transform_mat)) self.publish_map_point_cloud() rate.sleep() - - + + if __name__ == '__main__': try: - parser = ap.ArgumentParser(description='Keyboard teleoperation for stretch.') - parser.add_argument('--load_map', default=None, help='Provide directory from which to load a map.') + parser = ap.ArgumentParser( + description='Keyboard teleoperation for stretch.') + parser.add_argument('--load_map', default=None, + help='Provide directory from which to load a map.') args, unknown = parser.parse_known_args() map_filename = args.load_map node = FunmapNode(map_filename) diff --git a/stretch_funmap/src/stretch_funmap/compile_cython_code.sh b/stretch_funmap/src/stretch_funmap/compile_cython_code.sh index af33f98..303e771 100755 --- a/stretch_funmap/src/stretch_funmap/compile_cython_code.sh +++ b/stretch_funmap/src/stretch_funmap/compile_cython_code.sh @@ -1,11 +1,17 @@ #!/bin/bash echo "Building cython_min_cost_path.pyx..." -cython ./cython_min_cost_path.pyx -gcc -shared -pthread -fPIC -fwrapv -ffast-math -O3 -Wall -fno-strict-aliasing -I/usr/include/python2.7 -o cython_min_cost_path.so cython_min_cost_path.c +cython3 ./cython_min_cost_path.pyx +gcc -shared -pthread -fPIC -fwrapv -ffast-math -O3 -Wall -fno-strict-aliasing -I/usr/include/python3.8 -o cython_min_cost_path.so cython_min_cost_path.c echo "Done." echo "" +# In the future, consider the following recommendation from the Cython +# documentation found at +# https://cython.readthedocs.io/en/latest/src/quickstart/build.html +# +# "Write a setuptools setup.py. This is the normal and recommended way." +# # In the future try adding -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION to address the following error (adding this argument currently results in compilation failure). # diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index be618bf..cfa79e1 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/stretch_funmap/src/stretch_funmap/manipulation_planning.py @@ -1,6 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -from __future__ import print_function +#from __future__ import print_function import numpy as np import scipy.ndimage as nd @@ -8,16 +8,16 @@ import scipy.signal as si import cv2 import skimage as sk import math -import max_height_image as mh -import segment_max_height_image as sm -import ros_max_height_image as rm +import stretch_funmap.max_height_image as mh +import stretch_funmap.segment_max_height_image as sm +import stretch_funmap.ros_max_height_image as rm import hello_helpers.hello_misc as hm import ros_numpy as rn import rospy import os -from numba_manipulation_planning import numba_find_base_poses_that_reach_target, numba_check_that_tool_can_deploy -from numba_check_line_path import numba_find_contact_along_line_path, numba_find_line_path_on_surface +from stretch_funmap.numba_manipulation_planning import numba_find_base_poses_that_reach_target, numba_check_that_tool_can_deploy +from stretch_funmap.numba_check_line_path import numba_find_contact_along_line_path, numba_find_line_path_on_surface def plan_surface_coverage(tool_start_xy_pix, tool_end_xy_pix, tool_extension_direction_xy_pix, step_size_pix, max_extension_pix, surface_mask_image, obstacle_mask_image): # This was designed to be used when planning to clean a flat diff --git a/stretch_funmap/src/stretch_funmap/mapping.py b/stretch_funmap/src/stretch_funmap/mapping.py index 164aa41..e057d40 100644 --- a/stretch_funmap/src/stretch_funmap/mapping.py +++ b/stretch_funmap/src/stretch_funmap/mapping.py @@ -1,23 +1,24 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + +#from __future__ import print_function -from __future__ import print_function import numpy as np import ros_numpy as rn -import ros_max_height_image as rm +import stretch_funmap.ros_max_height_image as rm from actionlib_msgs.msg import GoalStatus import rospy import hello_helpers.hello_misc as hm -import ros_max_height_image as rm +import stretch_funmap.ros_max_height_image as rm import ros_numpy import yaml -import navigation_planning as na +import stretch_funmap.navigation_planning as na import time import cv2 import copy import scipy.ndimage as nd -import merge_maps as mm +import stretch_funmap.merge_maps as mm import tf_conversions -import segment_max_height_image as sm +import stretch_funmap.segment_max_height_image as sm def stow_and_lower_arm(node): diff --git a/stretch_funmap/src/stretch_funmap/max_height_image.py b/stretch_funmap/src/stretch_funmap/max_height_image.py index 95e9b25..7043ba0 100644 --- a/stretch_funmap/src/stretch_funmap/max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/max_height_image.py @@ -1,6 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -from __future__ import print_function +#from __future__ import print_function import sys import cv2 @@ -14,8 +14,8 @@ import struct import threading from collections import deque -import numba_height_image as nh -from numba_create_plane_image import numba_create_plane_image, numba_correct_height_image, transform_original_to_corrected, transform_corrected_to_original +import stretch_funmap.numba_height_image as nh +from stretch_funmap.numba_create_plane_image import numba_create_plane_image, numba_correct_height_image, transform_original_to_corrected, transform_corrected_to_original from scipy.spatial.transform import Rotation diff --git a/stretch_funmap/src/stretch_funmap/merge_maps.py b/stretch_funmap/src/stretch_funmap/merge_maps.py index 4a09cb7..e6d51de 100755 --- a/stretch_funmap/src/stretch_funmap/merge_maps.py +++ b/stretch_funmap/src/stretch_funmap/merge_maps.py @@ -1,8 +1,8 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -from __future__ import print_function +#from __future__ import print_function -import max_height_image as mh +import stretch_funmap.max_height_image as mh import numpy as np import scipy.ndimage as nd import scipy.signal as si @@ -10,7 +10,7 @@ import cv2 import skimage as sk import math import hello_helpers.hello_misc as hm -import navigation_planning as na +import stretch_funmap.navigation_planning as na import copy import time @@ -18,8 +18,8 @@ import time from scipy.optimize import minimize, minimize_scalar -from numba_compare_images import numba_compare_images_2 -import mapping as ma +from stretch_funmap.numba_compare_images import numba_compare_images_2 +import stretch_funmap.mapping as ma import tf_conversions diff --git a/stretch_funmap/src/stretch_funmap/navigate.py b/stretch_funmap/src/stretch_funmap/navigate.py index 2011bd7..8b396eb 100644 --- a/stretch_funmap/src/stretch_funmap/navigate.py +++ b/stretch_funmap/src/stretch_funmap/navigate.py @@ -1,15 +1,16 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + +#from __future__ import print_function -from __future__ import print_function import numpy as np import ros_numpy as rn -import ros_max_height_image as rm +import stretch_funmap.ros_max_height_image as rm from control_msgs.msg import FollowJointTrajectoryResult from actionlib_msgs.msg import GoalStatus import rospy from std_srvs.srv import Trigger, TriggerRequest import hello_helpers.hello_misc as hm -import navigation_planning as na +import stretch_funmap.navigation_planning as na import cv2 class ForwardMotionObstacleDetector(): diff --git a/stretch_funmap/src/stretch_funmap/navigation_planning.py b/stretch_funmap/src/stretch_funmap/navigation_planning.py index d7f49fa..8d0ede4 100644 --- a/stretch_funmap/src/stretch_funmap/navigation_planning.py +++ b/stretch_funmap/src/stretch_funmap/navigation_planning.py @@ -2,10 +2,10 @@ import copy import numpy as np import scipy.ndimage as nd import cv2 -import cython_min_cost_path as cm -from numba_check_line_path import numba_check_line_path -from numba_sample_ridge import numba_sample_ridge, numba_sample_ridge_list -import segment_max_height_image as sm +import stretch_funmap.cython_min_cost_path as cm +from stretch_funmap.numba_check_line_path import numba_check_line_path +from stretch_funmap.numba_sample_ridge import numba_sample_ridge, numba_sample_ridge_list +import stretch_funmap.segment_max_height_image as sm import hello_helpers.hello_misc as hm #################################################### @@ -424,7 +424,7 @@ def estimate_navigation_channels( floor_mask, idealized_height_image, m_per_pix, # find exit regions number_of_exits, exit_label_image = simple_connected_components(map_exits) - print 'number_of_exits =', number_of_exits + print('number_of_exits =', number_of_exits) distance_map = original_dist_transform.copy() distance_map[closed_floor_mask < 1] = 0 @@ -924,9 +924,9 @@ def split_paths(pix_path, max_segment_length_pix): new_path.append(list(p_mid)) new_path.append(p1) else: - print 'WARNING: split_paths given pix_path input with length <= 1.' - print ' returning pix_path without modification' - print ' pix_path =', pix_path + print('WARNING: split_paths given pix_path input with length <= 1.') + print(' returning pix_path without modification') + print(' pix_path =', pix_path) return pix_path return new_path, split_made @@ -955,9 +955,9 @@ def chop_path_at_location(pix_path, best_stopping_location): new_path = pix_path[:min_index+1] new_path.append(best_stopping_location) else: - print 'WARNING: chop_path_at_location given pix_path input with length <= 1.' - print ' returning pix_path without modification' - print ' pix_path =', pix_path + print('WARNING: chop_path_at_location given pix_path input with length <= 1.') + print(' returning pix_path without modification') + print(' pix_path =', pix_path) return pix_path return new_path diff --git a/stretch_funmap/src/stretch_funmap/ros_max_height_image.py b/stretch_funmap/src/stretch_funmap/ros_max_height_image.py index 90dc600..51f11d1 100644 --- a/stretch_funmap/src/stretch_funmap/ros_max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/ros_max_height_image.py @@ -1,6 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -from __future__ import print_function +#from __future__ import print_function import sys import rospy @@ -30,9 +30,9 @@ from scipy.spatial.transform import Rotation from copy import deepcopy -from max_height_image import * +from stretch_funmap.max_height_image import * -import navigation_planning as na +import stretch_funmap.navigation_planning as na class ROSVolumeOfInterest(VolumeOfInterest): diff --git a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py index 93ed607..a22f716 100755 --- a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py @@ -1,8 +1,8 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -from __future__ import print_function +#from __future__ import print_function -import max_height_image as mh +import stretch_funmap.max_height_image as mh import numpy as np import scipy.ndimage as nd import scipy.signal as si @@ -11,9 +11,9 @@ import skimage as sk from skimage.morphology import convex_hull_image import math import hello_helpers.hello_misc as hm -import navigation_planning as na +import stretch_funmap.navigation_planning as na -from numba_height_image import numba_create_segment_image_uint8 +from stretch_funmap.numba_height_image import numba_create_segment_image_uint8 import hello_helpers.fit_plane as fp