From d16eb373a44db1cd0fe92f0439cec43971166ec6 Mon Sep 17 00:00:00 2001 From: Charlie Kemp Date: Thu, 3 Jun 2021 18:28:04 -0400 Subject: [PATCH] comment cleanup I primarily removed commented code. --- stretch_calibration/nodes/calibration.py | 6 +- .../nodes/collect_head_calibration_data | 2 - .../nodes/process_head_calibration_data | 3 - stretch_core/nodes/command_groups.py | 13 +---- stretch_core/nodes/d435i_accel_correction | 2 - stretch_core/nodes/d435i_configure | 2 - stretch_core/nodes/d435i_frustum_visualizer | 2 - stretch_core/nodes/detect_aruco_markers | 4 -- stretch_core/nodes/joint_trajectory_server.py | 3 +- stretch_core/nodes/keyboard.py | 4 +- stretch_core/nodes/keyboard_teleop | 5 -- stretch_core/nodes/rwlock.py | 2 +- stretch_core/nodes/stretch_driver | 55 ------------------- stretch_demos/nodes/clean_surface | 12 +--- stretch_demos/nodes/grasp_object | 2 - stretch_demos/nodes/handover_object | 3 - stretch_demos/nodes/hello_world | 15 ++--- stretch_demos/nodes/open_drawer | 5 -- stretch_funmap/nodes/funmap | 2 - .../stretch_funmap/manipulation_planning.py | 4 -- .../src/stretch_funmap/max_height_image.py | 2 - .../src/stretch_funmap/merge_maps.py | 2 - stretch_funmap/src/stretch_funmap/navigate.py | 2 - .../src/stretch_funmap/navigation_planning.py | 3 - .../src/stretch_funmap/numba_sample_ridge.py | 4 -- .../stretch_funmap/ros_max_height_image.py | 4 +- .../segment_max_height_image.py | 29 ++++------ 27 files changed, 27 insertions(+), 165 deletions(-) diff --git a/stretch_calibration/nodes/calibration.py b/stretch_calibration/nodes/calibration.py index ed8f275..58bb832 100644 --- a/stretch_calibration/nodes/calibration.py +++ b/stretch_calibration/nodes/calibration.py @@ -1,6 +1,4 @@ -#!/usr/bin/env python - -from __future__ import print_function +#!/usr/bin/env python3 from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist @@ -176,9 +174,7 @@ class Chain: self.chain_names = self.urdf.get_chain(start_name, end_name) self.chain = [] for name in self.chain_names: - #is_joint = self.urdf.joint_map.has_key(name) is_joint = name in self.urdf.joint_map - #is_link = self.urdf.link_map.has_key(name) is_link = name in self.urdf.link_map if is_joint and is_link: print('ERROR: a joint and a link have the same name. This is not supported.') diff --git a/stretch_calibration/nodes/collect_head_calibration_data b/stretch_calibration/nodes/collect_head_calibration_data index 03d0e75..9a434fc 100755 --- a/stretch_calibration/nodes/collect_head_calibration_data +++ b/stretch_calibration/nodes/collect_head_calibration_data @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry diff --git a/stretch_calibration/nodes/process_head_calibration_data b/stretch_calibration/nodes/process_head_calibration_data index d9a4745..9ea6b72 100755 --- a/stretch_calibration/nodes/process_head_calibration_data +++ b/stretch_calibration/nodes/process_head_calibration_data @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import calibration as ca from sensor_msgs.msg import JointState @@ -620,7 +618,6 @@ class HeadCalibrator: pan_backlash_correction = 0.0 if backlash_state['joint_head_tilt_looking_up']: - #tilt_backlash_correction = -math.pi/18.0 # 10 deg test tilt_backlash_correction = self.tilt_looking_up_offset else: tilt_backlash_correction = 0.0 diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index 9df22c2..b16cbfa 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -1,5 +1,4 @@ -#! /usr/bin/env python -from __future__ import print_function +#! /usr/bin/env python3 import numpy as np import hello_helpers.hello_misc as hm @@ -209,7 +208,6 @@ class HeadTiltCommandGroup(SimpleCommandGroup): if self.active: _, tilt_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state']) robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) - #if tilt_error > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset): if self.goal['position'] > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset): kwargs['backlash_state']['head_tilt_looking_up'] = True else: @@ -333,7 +331,6 @@ class GripperCommandGroup(SimpleCommandGroup): class TelescopingCommandGroup(SimpleCommandGroup): def __init__(self, range_m, wrist_extension_calibrated_retracted_offset): - #SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.005) SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008) self.wrist_extension_calibrated_retracted_offset = wrist_extension_calibrated_retracted_offset self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0'] @@ -501,9 +498,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup): # + If both rotation and translation commands received, then return goal failure. # + If only one received, check that the array length works with the index. # + Can both commands be provided and one have a null goal of some sort? If so, should consider handling this case. - # - # TO DO: remove legacy manipulation mode - # + robot_mode = kwargs['robot_mode'] self.active = False self.active_translate_mobile_base = False @@ -552,9 +547,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup): # + If both rotation and translation commands received, then return goal failure. # + If only one received, check that the array length works with the index. # + Can both commands be provided and one have a null goal of some sort? If so, should consider handling this case. - # - # TO DO: remove legacy manipulation mode - # + self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} self.goal_translate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} self.goal_rotate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} diff --git a/stretch_core/nodes/d435i_accel_correction b/stretch_core/nodes/d435i_accel_correction index 1f08821..618312d 100755 --- a/stretch_core/nodes/d435i_accel_correction +++ b/stretch_core/nodes/d435i_accel_correction @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import sys import rospy from sensor_msgs.msg import Imu diff --git a/stretch_core/nodes/d435i_configure b/stretch_core/nodes/d435i_configure index 5a5ff6a..f980344 100755 --- a/stretch_core/nodes/d435i_configure +++ b/stretch_core/nodes/d435i_configure @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import rospy import dynamic_reconfigure.client from std_srvs.srv import Trigger, TriggerResponse diff --git a/stretch_core/nodes/d435i_frustum_visualizer b/stretch_core/nodes/d435i_frustum_visualizer index 852e71a..132226d 100755 --- a/stretch_core/nodes/d435i_frustum_visualizer +++ b/stretch_core/nodes/d435i_frustum_visualizer @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import rospy import numpy as np diff --git a/stretch_core/nodes/detect_aruco_markers b/stretch_core/nodes/detect_aruco_markers index 7083d7d..da4c796 100755 --- a/stretch_core/nodes/detect_aruco_markers +++ b/stretch_core/nodes/detect_aruco_markers @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import sys import rospy import cv2 @@ -41,7 +39,6 @@ class ArucoMarker: bgr = id_color_image[0,0] self.id_color = [bgr[2], bgr[1], bgr[0]] - #self.frame_id = '/camera_color_optical_frame' self.frame_id = 'camera_color_optical_frame' self.info = marker_info.get(str(self.aruco_id), None) @@ -664,7 +661,6 @@ class DetectArucoNode: def publish_point_cloud(self): header = Header() - #header.frame_id = '/camera_color_optical_frame' header.frame_id = 'camera_color_optical_frame' header.stamp = rospy.Time.now() fields = [PointField('x', 0, PointField.FLOAT32, 1), diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 40d7ab2..52d5d05 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -1,5 +1,4 @@ -#! /usr/bin/env python -from __future__ import print_function +#! /usr/bin/env python3 import rospy import actionlib diff --git a/stretch_core/nodes/keyboard.py b/stretch_core/nodes/keyboard.py index 5d8c050..19fa1f9 100644 --- a/stretch_core/nodes/keyboard.py +++ b/stretch_core/nodes/keyboard.py @@ -1,6 +1,4 @@ -#!/usr/bin/env python - -from __future__ import print_function +#!/usr/bin/env python3 import sys, tty, termios diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop index 72be4dc..e1fe14b 100755 --- a/stretch_core/nodes/keyboard_teleop +++ b/stretch_core/nodes/keyboard_teleop @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -#from __future__ import print_function import math import keyboard as kb @@ -25,10 +24,6 @@ class GetKeyboardCommands: self.deliver_object_on = deliver_object_on self.step_size = 'medium' - # self.persistent_command_count = 0 - # self.prev_persistent_c = None - # self.persistent_start_s = time.time() - # self.max_persistent_delay_s = 1.0 self.rad_per_deg = math.pi/180.0 self.small_deg = 3.0 self.small_rad = self.rad_per_deg * self.small_deg diff --git a/stretch_core/nodes/rwlock.py b/stretch_core/nodes/rwlock.py index 43b0164..6974348 100644 --- a/stretch_core/nodes/rwlock.py +++ b/stretch_core/nodes/rwlock.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 """rwlock.py This write-preferring implementation of the readwrite lock was made available via the Creative Commons license at: diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index ef82344..126d05e 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -1,5 +1,4 @@ #! /usr/bin/env python3 -#from __future__ import print_function import yaml import numpy as np @@ -61,8 +60,6 @@ class StretchBodyNode: self.gripper_conversion = GripperConversion() - #self.mobile_base_manipulation_origin = None #{'x':None, 'y':None, 'theta':None} - self.robot_stop_lock = threading.Lock() self.stop_the_robot = False @@ -129,11 +126,6 @@ class StretchBodyNode: theta_vel = base_status['theta_vel'] pose_time_s = base_status['pose_time_s'] - # if self.robot_mode == 'manipulation': - # x = self.mobile_base_manipulation_origin['x'] - # x_vel = 0.0 - # x_effort = 0.0 - # assign relevant arm status to variables arm_status = robot_status['arm'] if self.backlash_state['wrist_extension_retracted']: @@ -286,18 +278,6 @@ class StretchBodyNode: velocities.append(gripper_finger_vel) efforts.append(gripper_finger_effort) - # # set virtual joint for mobile base translation - # joint_state.name.append('joint_mobile_base_translation') - # if self.robot_mode == 'manipulation': - # manipulation_translation = x_raw - self.mobile_base_manipulation_origin['x'] - # positions.append(manipulation_translation) - # velocities.append(x_vel_raw) - # efforts.append(x_effort_raw) - # else: - # positions.append(0.0) - # velocities.append(0.0) - # efforts.append(0.0) - # set joint_state joint_state.position = positions joint_state.velocity = velocities @@ -370,28 +350,6 @@ class StretchBodyNode: self.angular_velocity_radps = 0.0 self.change_mode('navigation', code_to_run) - # def turn_on_manipulation_mode(self): - # # Manipulation mode enables mobile base translation using - # # position control via joint_mobile_base_translation, and - # # disables velocity control of the mobile base. It also - # # updates the virtual prismatic joint named - # # joint_mobile_base_translation that relates 'floor_link' and - # # 'base_link'. This mode was originally created so that - # # MoveIt! could treat the robot like an arm. This mode does - # # not allow base rotation. - # def code_to_run(): - # self.robot.base.enable_pos_incr_mode() - # # get copy of the current robot status (uses lock held by the robot) - # robot_status = self.robot.get_status() - # # obtain odometry - # # assign relevant base status to variables - # base_status = robot_status['base'] - # x = base_status['x'] - # y = base_status['y'] - # theta = base_status['theta'] - # self.mobile_base_manipulation_origin = {'x':x, 'y':y, 'theta':theta} - # self.change_mode('manipulation', code_to_run) - def turn_on_position_mode(self): # Position mode enables mobile base translation and rotation # using position control with sequential incremental rotations @@ -440,13 +398,6 @@ class StretchBodyNode: message='Now in navigation mode.' ) - # def manipulation_mode_service_callback(self, request): - # self.turn_on_manipulation_mode() - # return TriggerResponse( - # success=True, - # message='Now in manipulation mode.' - # ) - def position_mode_service_callback(self, request): self.turn_on_position_mode() return TriggerResponse( @@ -500,8 +451,6 @@ class StretchBodyNode: self.turn_on_position_mode() elif mode == "navigation": self.turn_on_navigation_mode() -# elif mode == "manipulation": -# self.turn_on_manipulation_mode() self.broadcast_odom_tf = rospy.get_param('~broadcast_odom_tf', False) rospy.loginfo('broadcast_odom_tf = ' + str(self.broadcast_odom_tf)) @@ -587,10 +536,6 @@ class StretchBodyNode: self.joint_trajectory_action = JointTrajectoryAction(self) self.joint_trajectory_action.server.start() - # self.switch_to_manipulation_mode_service = rospy.Service('/switch_to_manipulation_mode', - # Trigger, - # self.manipulation_mode_service_callback) - self.switch_to_navigation_mode_service = rospy.Service('/switch_to_navigation_mode', Trigger, self.navigation_mode_service_callback) diff --git a/stretch_demos/nodes/clean_surface b/stretch_demos/nodes/clean_surface index fa71fe4..7590fa9 100755 --- a/stretch_demos/nodes/clean_surface +++ b/stretch_demos/nodes/clean_surface @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry @@ -96,9 +94,9 @@ class CleanSurfaceNode(hm.HelloNode): def trigger_clean_surface_callback(self, request): - tool_width_m = 0.08 #0.06 - tool_length_m = 0.08 #0.06 - step_size_m = 0.04 #0.06 #0.1 #0.02 + tool_width_m = 0.08 + tool_length_m = 0.08 + step_size_m = 0.04 min_extension_m = 0.01 max_extension_m = 0.5 @@ -127,7 +125,6 @@ class CleanSurfaceNode(hm.HelloNode): initial_wrist_position = self.wrist_position extension_m = start['start_wrist_extension_m'] - #start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0) start_extension_m = initial_wrist_position + extension_m pose = {'wrist_extension': start_extension_m} rospy.loginfo('Extend tool above surface.') @@ -154,7 +151,6 @@ class CleanSurfaceNode(hm.HelloNode): rospy.sleep(0.2) # wait for new lift position extension_m = start['end_wrist_extension_m'] - #end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0) end_extension_m = initial_wrist_position + extension_m pose = {'wrist_extension': end_extension_m} rospy.loginfo('Extend tool to end of surface.') @@ -172,7 +168,6 @@ class CleanSurfaceNode(hm.HelloNode): self.move_to_pose(pose) extension_m = m['start_wrist_extension_m'] - #start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0) start_extension_m = initial_wrist_position + extension_m start_extension_m = max(start_extension_m, min_extension_m) pose = {'wrist_extension': start_extension_m} @@ -180,7 +175,6 @@ class CleanSurfaceNode(hm.HelloNode): self.move_to_pose(pose) extension_m = m['end_wrist_extension_m'] - #end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0) end_extension_m = initial_wrist_position + extension_m end_extension_m = min(end_extension_m, max_extension_m) pose = {'wrist_extension': end_extension_m} diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index a982364..d8146e5 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry diff --git a/stretch_demos/nodes/handover_object b/stretch_demos/nodes/handover_object index dfaf91d..c681b98 100755 --- a/stretch_demos/nodes/handover_object +++ b/stretch_demos/nodes/handover_object @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry @@ -107,7 +105,6 @@ 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[:] new_frame_id = 'base_link' stamped_transform = self.tf2_buffer.lookup_transform(new_frame_id, old_frame_id, lookup_time, timeout_ros) diff --git a/stretch_demos/nodes/hello_world b/stretch_demos/nodes/hello_world index 139b030..cde2ab4 100755 --- a/stretch_demos/nodes/hello_world +++ b/stretch_demos/nodes/hello_world @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry @@ -37,7 +35,7 @@ class HelloWorldNode(hm.HelloNode): self.joint_states_lock = threading.Lock() self.move_base = nv.MoveBase(self) self.letter_height_m = 0.2 - self.letter_top_lift_m = 1.08 #1.05 + self.letter_top_lift_m = 1.08 def joint_states_callback(self, joint_states): with self.joint_states_lock: @@ -71,7 +69,7 @@ class HelloWorldNode(hm.HelloNode): wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states) extension_m = wrist_position + max_reach_m extension_m = min(extension_m, max_extension_m) - extension_contact_effort = 45.0 #42.0 #40.0 from funmap + extension_contact_effort = 45.0 pose = {'wrist_extension': (extension_m, extension_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) @@ -81,11 +79,11 @@ class HelloWorldNode(hm.HelloNode): wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states) max_extension_m = 0.5 min_extension_m = 0.01 - backoff_m = 0.1 #0.07 # back away 7cm from the surface + backoff_m = 0.1 extension_m = wrist_position - backoff_m extension_m = min(extension_m, max_extension_m) extension_m = max(min_extension_m, extension_m) - extension_contact_effort = 80.0 #40.0 from funmap # to avoid stopping due to contact + extension_contact_effort = 80.0 # to avoid stopping due to contact pose = {'wrist_extension': (extension_m, extension_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) rospy.sleep(1.0) # Give it time to backoff before the next move. Otherwise it may not backoff. @@ -264,10 +262,6 @@ class HelloWorldNode(hm.HelloNode): rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.') self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger) - # rospy.wait_for_service('/funmap/trigger_turn_off_contact_regulation') - # rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_turn_off_contact_regulation.') - # self.trigger_turn_off_contact_regulation_service = rospy.ServiceProxy('/funmap/trigger_turn_off_contact_regulation', Trigger) - rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): rate.sleep() @@ -276,7 +270,6 @@ class HelloWorldNode(hm.HelloNode): if __name__ == '__main__': try: parser = ap.ArgumentParser(description='Hello World demo for stretch.') - #parser.add_argument('--mapping_on', action='store_true', help='Turn on mapping control. For example, the space bar will trigger a head scan. This requires that the mapping node be run (funmap).') args, unknown = parser.parse_known_args() node = HelloWorldNode() node.main() diff --git a/stretch_demos/nodes/open_drawer b/stretch_demos/nodes/open_drawer index a1ccd4d..6ecc2b2 100755 --- a/stretch_demos/nodes/open_drawer +++ b/stretch_demos/nodes/open_drawer @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry @@ -74,7 +72,6 @@ class OpenDrawerNode(hm.HelloNode): use_correction = True if use_correction: # raise due to drop down after contact detection - #rospy.sleep(0.5) # wait for new lift position rospy.sleep(0.2) # wait for new lift position lift_m = self.lift_position + 0.015 pose = {'joint_lift': lift_m} @@ -89,8 +86,6 @@ class OpenDrawerNode(hm.HelloNode): pose = {'joint_lift': (lift_m, lift_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) - #rospy.sleep(1.0) # needed to correct for bounce after contact - use_correction = True if use_correction: # raise due to drop down after contact detection diff --git a/stretch_funmap/nodes/funmap b/stretch_funmap/nodes/funmap index eab4ab7..025fce6 100755 --- a/stretch_funmap/nodes/funmap +++ b/stretch_funmap/nodes/funmap @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import rospy import actionlib diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index e12a60b..0629c2a 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/stretch_funmap/src/stretch_funmap/manipulation_planning.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import numpy as np import scipy.ndimage as nd import scipy.signal as si @@ -99,7 +97,6 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text if use_dilation: kernel_width_pix = 3 iterations = 1 - #kernel_radius_pix = (kernel_width_pix - 1) / 2 kernel_radius_pix = (kernel_width_pix - 1) // 2 kernel = np.zeros((kernel_width_pix, kernel_width_pix), np.uint8) cv2.circle(kernel, (kernel_radius_pix, kernel_radius_pix), kernel_radius_pix, 255, -1) @@ -303,7 +300,6 @@ class ManipulationView(): if use_dilation: kernel_width_pix = 3 iterations = 1 - #kernel_radius_pix = (kernel_width_pix - 1) / 2 kernel_radius_pix = (kernel_width_pix - 1) // 2 kernel = np.zeros((kernel_width_pix, kernel_width_pix), np.uint8) cv2.circle(kernel, (kernel_radius_pix, kernel_radius_pix), kernel_radius_pix, 255, -1) diff --git a/stretch_funmap/src/stretch_funmap/max_height_image.py b/stretch_funmap/src/stretch_funmap/max_height_image.py index 7043ba0..e9b6071 100644 --- a/stretch_funmap/src/stretch_funmap/max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/max_height_image.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import sys import cv2 import numpy as np diff --git a/stretch_funmap/src/stretch_funmap/merge_maps.py b/stretch_funmap/src/stretch_funmap/merge_maps.py index 1cb76dd..ca1c8b6 100755 --- a/stretch_funmap/src/stretch_funmap/merge_maps.py +++ b/stretch_funmap/src/stretch_funmap/merge_maps.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import stretch_funmap.max_height_image as mh import numpy as np import scipy.ndimage as nd diff --git a/stretch_funmap/src/stretch_funmap/navigate.py b/stretch_funmap/src/stretch_funmap/navigate.py index 8b396eb..4890844 100644 --- a/stretch_funmap/src/stretch_funmap/navigate.py +++ b/stretch_funmap/src/stretch_funmap/navigate.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import numpy as np import ros_numpy as rn import stretch_funmap.ros_max_height_image as rm diff --git a/stretch_funmap/src/stretch_funmap/navigation_planning.py b/stretch_funmap/src/stretch_funmap/navigation_planning.py index 17201be..ed84ba2 100644 --- a/stretch_funmap/src/stretch_funmap/navigation_planning.py +++ b/stretch_funmap/src/stretch_funmap/navigation_planning.py @@ -397,7 +397,6 @@ def estimate_navigation_channels( floor_mask, idealized_height_image, m_per_pix, # create kernel for morphological operations kernel_width_pix = 11 - #kernel_radius_pix = (kernel_width_pix - 1) / 2 kernel_radius_pix = (kernel_width_pix - 1) // 2 kernel = np.zeros((kernel_width_pix, kernel_width_pix), np.uint8) cv2.circle(kernel, (kernel_radius_pix, kernel_radius_pix), kernel_radius_pix, 255, -1) @@ -624,9 +623,7 @@ def find_exits( floor_mask, max_height_image, m_per_pix, distance_map = cv2.distanceTransform(traversable_mask, cv2.DIST_L2, 5) # fill in floor mask holes - #kernel = np.ones((11,11), np.uint8) kernel_width_pix = 11 - #kernel_radius_pix = (kernel_width_pix - 1) / 2 kernel_radius_pix = (kernel_width_pix - 1) // 2 kernel = np.zeros((kernel_width_pix, kernel_width_pix), np.uint8) cv2.circle(kernel, (kernel_radius_pix, kernel_radius_pix), kernel_radius_pix, 255, -1) diff --git a/stretch_funmap/src/stretch_funmap/numba_sample_ridge.py b/stretch_funmap/src/stretch_funmap/numba_sample_ridge.py index b2a4d08..c6a66eb 100644 --- a/stretch_funmap/src/stretch_funmap/numba_sample_ridge.py +++ b/stretch_funmap/src/stretch_funmap/numba_sample_ridge.py @@ -40,10 +40,8 @@ def numba_sample_ridge(window_width, ridge_mask, distance_map, distance_threshol l_x += window_width l_y += window_width - #l_y = window_width/2 l_y = window_width//2 while l_y < l_y_max: - #l_x = window_width/2 l_x = window_width//2 while l_x < l_x_max: max_found = False @@ -115,10 +113,8 @@ def numba_sample_ridge_list(window_width, ridge_mask, distance_map, distance_thr sample_list = [] - #l_y = window_width/2 l_y = window_width//2 while l_y < l_y_max: - #l_x = window_width/2 l_x = window_width//2 while l_x < l_x_max: max_found = False 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 016fa0d..38cbeea 100644 --- a/stretch_funmap/src/stretch_funmap/ros_max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/ros_max_height_image.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import sys import rospy import cv2 @@ -91,7 +89,7 @@ class ROSVolumeOfInterest(VolumeOfInterest): marker.color.r = r marker.color.g = g marker.color.b = b - marker.color.a = 0.25 #0.33 #0.25 #0.5 + marker.color.a = 0.25 # find the middle of the volume of interest center_vec = np.array([self.x_in_m/2.0, self.y_in_m/2.0, self.z_in_m/2.0]) 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 8cacdc4..84005b4 100755 --- a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -#from __future__ import print_function - import stretch_funmap.max_height_image as mh import numpy as np import scipy.ndimage as nd @@ -63,8 +61,6 @@ def find_object_to_grasp(height_image, display_on=False): if display_on: rgb_image = height_image.rgb_image.copy() - #rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0] - #rgb_image[obstacle_selector] = (rgb_image[obstacle_selector]/2) + [0, 0, 127] rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]//2) + [0, 127, 0] rgb_image[obstacle_selector] = (rgb_image[obstacle_selector]//2) + [0, 0, 127] cv2.imshow('obstacles', rgb_image) @@ -73,8 +69,6 @@ def find_object_to_grasp(height_image, display_on=False): if display_on: rgb_image = height_image.rgb_image.copy() - #rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0] - #rgb_image[obstacle_mask > 0] = (rgb_image[obstacle_mask > 0]/2) + [0, 0, 127] rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]//2) + [0, 127, 0] rgb_image[obstacle_mask > 0] = (rgb_image[obstacle_mask > 0]//2) + [0, 0, 127] @@ -92,7 +86,6 @@ def find_object_to_grasp(height_image, display_on=False): # and other phenomena. kernel_width_pix = 5 #3 iterations = 3 #5 - #kernel_radius_pix = (kernel_width_pix - 1) / 2 kernel_radius_pix = (kernel_width_pix - 1) // 2 kernel = np.zeros((kernel_width_pix, kernel_width_pix), np.uint8) cv2.circle(kernel, (kernel_radius_pix, kernel_radius_pix), kernel_radius_pix, 255, -1) @@ -107,9 +100,7 @@ def find_object_to_grasp(height_image, display_on=False): # Process the candidate object points # Treat connected components of candidate object points as objects. Fit ellipses to these segmented objects. - #label_image, max_label_index = sk.measure.label(obstacles_on_surface, neighbors=8, background=0, return_num=True, connectivity=None) label_image, max_label_index = sk.measure.label(obstacles_on_surface, background=0, return_num=True, connectivity=2) - #region_properties = sk.measure.regionprops(label_image, intensity_image=None, cache=True, coordinates='xy') region_properties = sk.measure.regionprops(label_image, intensity_image=None, cache=True) if display_on: rgb_image = height_image.rgb_image.copy() @@ -218,7 +209,6 @@ def draw_grasp(rgb_image, grasp_target): surface_convex_hull_mask = grasp_target['surface_convex_hull_mask'] object_selector = grasp_target['object_selector'] object_ellipse = grasp_target['object_ellipse'] - #rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]/2) + [0, 127, 0] rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]//2) + [0, 127, 0] rgb_image[object_selector] = [0, 0, 255] draw_ellipse_axes(rgb_image, object_ellipse, color=[255, 255, 255]) @@ -268,9 +258,7 @@ def find_closest_flat_surface(height_image, robot_xy_pix, display_on=False): if remove_floor: segments_image[floor_mask > 0] = 0 - #label_image, max_label_index = sk.measure.label(segments_image, neighbors=8, background=0, return_num=True, connectivity=None) label_image, max_label_index = sk.measure.label(segments_image, background=0, return_num=True, connectivity=2) - #region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True, coordinates='xy') region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True) if display_on: color_label_image = sk.color.label2rgb(label_image, image=image_rgb, colors=None, alpha=0.3, bg_label=0, bg_color=(0, 0, 0), image_alpha=1, kind='overlay') @@ -439,8 +427,6 @@ def render_segments(segments_image, segment_info, output_key_image=False): font_scale = 1.1 * (size/100.0) line_width = 1 line_color = (0,0,0) - #cv2.putText(key_image_color, '%.3f m' % h, ((i*size) + size/10, size/2), - # font, font_scale, line_color, line_width, cv2.LINE_AA) cv2.putText(key_image_color, '%.3f m' % h, ((i*size) + size//10, size//2), font, font_scale, line_color, line_width, cv2.LINE_AA) else: @@ -1043,14 +1029,22 @@ def find_floor(segment_info, segments_image, verbose=False): for i in segment_info: height_m = segment_info[i]['height_m'] bin_value = segment_info[i]['bin_value'] + + # Old values changed on June 3, 2021 due to no floor segment + # being within the bounds, which resulted in an error. + #min_floor_m = -0.02 #max_floor_m = 0.1 + + # New values set on June 3, 2021. min_floor_m = -0.05 max_floor_m = 0.05 - print('segment = {0}, histogram bin_value = {1}, height_m = {2}, min_floor_m = {3}, max_floor_m = {4}'.format(i, bin_value, height_m, min_floor_m, max_floor_m)) + if verbose: + print('segment = {0}, histogram bin_value = {1}, height_m = {2}, min_floor_m = {3}, max_floor_m = {4}'.format(i, bin_value, height_m, min_floor_m, max_floor_m)) if (height_m >= min_floor_m) and (height_m <= max_floor_m): - print('found candidate floor segment!') + if verbose: + print('found candidate floor segment!') if (floor_id is None) and (bin_value > 0.0): floor_id = i max_bin_value = bin_value @@ -1067,7 +1061,8 @@ def find_floor(segment_info, segments_image, verbose=False): print('floor_id =', floor_id) print('max_bin_value =', max_bin_value) else: - print('segment_max_height_image.py : no floor segment found...') + if verbose: + print('segment_max_height_image.py : no floor segment found...') return floor_id, floor_mask