comment cleanup

I primarily removed commented code.

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.')

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

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

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}

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

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

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
 

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),

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

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
 

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

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:

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)

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}

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

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)

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()

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

stretch_funmap/nodes/funmap

@@ -1,7 +1,5 @@
 #!/usr/bin/env python3
 
-#from __future__ import print_function
-
 import rospy
 import actionlib
 

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)

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

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

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

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)

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

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])

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