cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

whitespace

testing/funmap_changes
Binit Shah 2 years ago
parent
969c16fca7
commit
a268a790db
3 changed files with 24 additions and 41 deletions
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +17
    -28
      stretch_funmap/src/stretch_funmap/mapping.py
  2. +2
    -2
      stretch_funmap/src/stretch_funmap/max_height_image.py
  3. +5
    -11
      stretch_funmap/src/stretch_funmap/ros_max_height_image.py

+ 17
- 28
stretch_funmap/src/stretch_funmap/mapping.py View File

@ -6,7 +6,6 @@ import stretch_funmap.ros_max_height_image as rm
from actionlib_msgs.msg import GoalStatus from actionlib_msgs.msg import GoalStatus
import rospy import rospy
import hello_helpers.hello_misc as hm import hello_helpers.hello_misc as hm
import stretch_funmap.ros_max_height_image as rm
import ros_numpy import ros_numpy
import yaml import yaml
import stretch_funmap.navigation_planning as na import stretch_funmap.navigation_planning as na
@ -262,9 +261,9 @@ class HeadScan:
# manipulable surfaces. Also, when the top of the viewing frustum # manipulable surfaces. Also, when the top of the viewing frustum
# is parallel to the ground it will be at or close to the top of # is parallel to the ground it will be at or close to the top of
# the volume of interest. # the volume of interest.
robot_head_above_ground = 1.13 robot_head_above_ground = 1.13
# How far below the expected floor height the volume of interest # How far below the expected floor height the volume of interest
# should extend is less clear. Sunken living rooms and staircases # should extend is less clear. Sunken living rooms and staircases
# can go well below the floor and a standard stair step should be # can go well below the floor and a standard stair step should be
@ -283,9 +282,9 @@ class HeadScan:
# will be classified as traversable floor. This risk is mitigated # will be classified as traversable floor. This risk is mitigated
# by separate point cloud based obstacle detection while moving # by separate point cloud based obstacle detection while moving
# and cliff sensors. # and cliff sensors.
lowest_distance_below_ground = 0.05 #5cm lowest_distance_below_ground = 0.05 #5cm
total_height = robot_head_above_ground + lowest_distance_below_ground total_height = robot_head_above_ground + lowest_distance_below_ground
# 8m x 8m region # 8m x 8m region
voi_side_m = voi_side_m voi_side_m = voi_side_m
@ -296,13 +295,12 @@ class HeadScan:
m_per_pix = 0.006 m_per_pix = 0.006
pixel_dtype = np.uint8 pixel_dtype = np.uint8
self.max_height_im = rm.ROSMaxHeightImage(voi, m_per_pix, pixel_dtype, use_camera_depth_image=True) self.max_height_im = rm.ROSMaxHeightImage(voi, m_per_pix, pixel_dtype, use_camera_depth_image=True)
self.max_height_im.create_blank_rgb_image() self.max_height_im.create_blank_rgb_image()
self.max_height_im.print_info() self.max_height_im.print_info()
def make_robot_footprint_unobserved(self): def make_robot_footprint_unobserved(self):
# replace robot points with unobserved points # replace robot points with unobserved points
self.max_height_im.make_robot_footprint_unobserved(self.robot_xy_pix[0], self.robot_xy_pix[1], self.robot_ang_rad) self.max_height_im.make_robot_footprint_unobserved(self.robot_xy_pix[0], self.robot_xy_pix[1], self.robot_ang_rad)
@ -310,7 +308,7 @@ class HeadScan:
def make_robot_mast_blind_spot_unobserved(self): def make_robot_mast_blind_spot_unobserved(self):
# replace robot points with unobserved points # replace robot points with unobserved points
self.max_height_im.make_robot_mast_blind_spot_unobserved(self.robot_xy_pix[0], self.robot_xy_pix[1], self.robot_ang_rad) self.max_height_im.make_robot_mast_blind_spot_unobserved(self.robot_xy_pix[0], self.robot_xy_pix[1], self.robot_ang_rad)
def capture_point_clouds(self, node, pose, capture_params): def capture_point_clouds(self, node, pose, capture_params):
head_settle_time = capture_params['head_settle_time'] head_settle_time = capture_params['head_settle_time']
num_point_clouds_per_pan_ang = capture_params['num_point_clouds_per_pan_ang'] num_point_clouds_per_pan_ang = capture_params['num_point_clouds_per_pan_ang']
@ -321,7 +319,7 @@ class HeadScan:
head_settle_time = head_settle_time head_settle_time = head_settle_time
num_point_clouds_per_pan_ang = 1 num_point_clouds_per_pan_ang = 1
time_between_point_clouds = time_between_point_clouds time_between_point_clouds = time_between_point_clouds
node.move_to_pose(pose) node.move_to_pose(pose)
rospy.sleep(head_settle_time) rospy.sleep(head_settle_time)
settle_time = rospy.Time.now() settle_time = rospy.Time.now()
@ -351,22 +349,20 @@ class HeadScan:
if not_finished: if not_finished:
rospy.sleep(time_between_point_clouds) rospy.sleep(time_between_point_clouds)
def execute(self, head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node, look_at_self=True): def execute(self, head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node, look_at_self=True):
scan_start_time = time.time() scan_start_time = time.time()
pose = {'joint_head_pan': far_right_pan, 'joint_head_tilt': head_tilt} pose = {'joint_head_pan': far_right_pan, 'joint_head_tilt': head_tilt}
node.move_to_pose(pose) node.move_to_pose(pose)
pan_left = np.linspace(far_right_pan, far_left_pan, num_pan_steps) pan_left = np.linspace(far_right_pan, far_left_pan, num_pan_steps)
for pan_ang in pan_left: for pan_ang in pan_left:
pose = {'joint_head_pan': pan_ang} pose = {'joint_head_pan': pan_ang}
self.capture_point_clouds(node, pose, capture_params) self.capture_point_clouds(node, pose, capture_params)
# look at the ground right around the robot to detect any # look at the ground right around the robot to detect any
# nearby obstacles # nearby obstacles
if look_at_self: if look_at_self:
# Attempt to pick a head pose that sees around the robot, # Attempt to pick a head pose that sees around the robot,
# but doesn't see the mast, which can introduce noise. # but doesn't see the mast, which can introduce noise.
@ -378,7 +374,7 @@ class HeadScan:
scan_end_time = time.time() scan_end_time = time.time()
scan_duration = scan_end_time - scan_start_time scan_duration = scan_end_time - scan_start_time
rospy.loginfo('The head scan took {0} seconds.'.format(scan_duration)) rospy.loginfo('The head scan took {0} seconds.'.format(scan_duration))
##################################### #####################################
# record robot pose information and potentially useful transformations # record robot pose information and potentially useful transformations
self.robot_xy_pix, self.robot_ang_rad, self.timestamp = self.max_height_im.get_robot_pose_in_image(node.tf2_buffer) self.robot_xy_pix, self.robot_ang_rad, self.timestamp = self.max_height_im.get_robot_pose_in_image(node.tf2_buffer)
@ -396,8 +392,7 @@ class HeadScan:
self.make_robot_mast_blind_spot_unobserved() self.make_robot_mast_blind_spot_unobserved()
self.make_robot_footprint_unobserved() self.make_robot_footprint_unobserved()
def execute_full(self, node, fast_scan=False): def execute_full(self, node, fast_scan=False):
far_right_pan = -3.6 far_right_pan = -3.6
far_left_pan = 1.45 far_left_pan = 1.45
@ -416,13 +411,12 @@ class HeadScan:
self.execute(head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node) self.execute(head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node)
def execute_front(self, node, fast_scan=False): def execute_front(self, node, fast_scan=False):
far_right_pan = -1.2 far_right_pan = -1.2
far_left_pan = 1.2 far_left_pan = 1.2
head_tilt = -0.8 head_tilt = -0.8
num_pan_steps = 3 num_pan_steps = 3
capture_params = { capture_params = {
'fast_scan': fast_scan, 'fast_scan': fast_scan,
'head_settle_time': 0.5, 'head_settle_time': 0.5,
@ -432,15 +426,12 @@ class HeadScan:
self.execute(head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node) self.execute(head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node)
def execute_minimal(self, node, fast_scan=False): def execute_minimal(self, node, fast_scan=False):
far_right_pan = 0.1 far_right_pan = 0.1
far_left_pan = 0.1 far_left_pan = 0.1
head_tilt = -0.8 head_tilt = -0.8
num_pan_steps = 1 num_pan_steps = 1
look_at_self = True
capture_params = { capture_params = {
'fast_scan': fast_scan, 'fast_scan': fast_scan,
'head_settle_time': 0.5, 'head_settle_time': 0.5,
@ -448,10 +439,9 @@ class HeadScan:
'time_between_point_clouds': 1.0/15.0 # point clouds at 15 Hz, so this should help obtain distinct clouds 'time_between_point_clouds': 1.0/15.0 # point clouds at 15 Hz, so this should help obtain distinct clouds
} }
self.execute(head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node, look_at_self)
def save( self, base_filename, save_visualization=True ):
self.execute(head_tilt, far_left_pan, far_right_pan, num_pan_steps, capture_params, node)
def save(self, base_filename, save_visualization=True):
print('HeadScan: Saving to base_filename =', base_filename) print('HeadScan: Saving to base_filename =', base_filename)
# save scan to disk # save scan to disk
max_height_image_base_filename = base_filename + '_mhi' max_height_image_base_filename = base_filename + '_mhi'
@ -471,12 +461,11 @@ class HeadScan:
'image_to_base_link_mat' : self.image_to_base_link_mat.tolist(), 'image_to_base_link_mat' : self.image_to_base_link_mat.tolist(),
'map_to_image_mat' : self.map_to_image_mat.tolist(), 'map_to_image_mat' : self.map_to_image_mat.tolist(),
'map_to_base_mat' : self.map_to_base_mat.tolist()} 'map_to_base_mat' : self.map_to_base_mat.tolist()}
with open(base_filename + '.yaml', 'w') as fid: with open(base_filename + '.yaml', 'w') as fid:
yaml.dump(data, fid) yaml.dump(data, fid)
print('Finished saving.') print('Finished saving.')
@classmethod @classmethod
def from_file(self, base_filename): def from_file(self, base_filename):
print('HeadScan.from_file: base_filename =', base_filename) print('HeadScan.from_file: base_filename =', base_filename)

+ 2
- 2
stretch_funmap/src/stretch_funmap/max_height_image.py View File

@ -335,11 +335,11 @@ class MaxHeightImage:
self.transform_original_to_corrected = transform_to_corrected self.transform_original_to_corrected = transform_to_corrected
self.transform_corrected_to_original = np.linalg.inv(transform_to_corrected) self.transform_corrected_to_original = np.linalg.inv(transform_to_corrected)
def save( self, base_filename, save_visualization=True ):
def save(self, base_filename, save_visualization=True):
print('MaxHeightImage saving to base_filename =', base_filename) print('MaxHeightImage saving to base_filename =', base_filename)
max_pix = None max_pix = None
if save_visualization:
if save_visualization:
# Save uint8 png image for visualization purposes. This would # Save uint8 png image for visualization purposes. This would
# be sufficient for uint8 pixel_dtypes, but does not work for # be sufficient for uint8 pixel_dtypes, but does not work for
# uint16. # uint16.

+ 5
- 11
stretch_funmap/src/stretch_funmap/ros_max_height_image.py View File

@ -106,14 +106,13 @@ class ROSVolumeOfInterest(VolumeOfInterest):
return marker return marker
class ROSMaxHeightImage(MaxHeightImage): class ROSMaxHeightImage(MaxHeightImage):
@classmethod @classmethod
def from_file( self, base_filename ): def from_file( self, base_filename ):
data, image, rgb_image, camera_depth_image = MaxHeightImage.load_serialization(base_filename) data, image, rgb_image, camera_depth_image = MaxHeightImage.load_serialization(base_filename)
m_per_pix = data['m_per_pix'] m_per_pix = data['m_per_pix']
m_per_height_unit = data['m_per_height_unit'] m_per_height_unit = data['m_per_height_unit']
image_origin = np.array(data['image_origin']) image_origin = np.array(data['image_origin'])
@ -130,7 +129,6 @@ class ROSMaxHeightImage(MaxHeightImage):
return max_height_image return max_height_image
def get_points_to_image_mat(self, ros_frame_id, tf2_buffer, lookup_time=None, timeout_s=None): def get_points_to_image_mat(self, ros_frame_id, tf2_buffer, lookup_time=None, timeout_s=None):
# This returns a matrix that transforms a point in the # This returns a matrix that transforms a point in the
# provided ROS frame to a point in the image. However, it does # provided ROS frame to a point in the image. However, it does
@ -160,7 +158,6 @@ class ROSMaxHeightImage(MaxHeightImage):
else: else:
return None, None return None, None
def get_image_to_points_mat(self, ros_frame_id, tf2_buffer, lookup_time=None, timeout_s=None): def get_image_to_points_mat(self, ros_frame_id, tf2_buffer, lookup_time=None, timeout_s=None):
# This returns a matrix that transforms a point in the image # This returns a matrix that transforms a point in the image
# to a point in the provided ROS frame. However, it does not # to a point in the provided ROS frame. However, it does not
@ -190,7 +187,6 @@ class ROSMaxHeightImage(MaxHeightImage):
else: else:
return None, None return None, None
def get_robot_pose_in_image(self, tf2_buffer): def get_robot_pose_in_image(self, tf2_buffer):
robot_to_image_mat, timestamp = self.get_points_to_image_mat('base_link', tf2_buffer) robot_to_image_mat, timestamp = self.get_points_to_image_mat('base_link', tf2_buffer)
r0 = np.array([0.0, 0.0, 0.0, 1.0]) r0 = np.array([0.0, 0.0, 0.0, 1.0])
@ -211,7 +207,7 @@ class ROSMaxHeightImage(MaxHeightImage):
image_to_point_mat, timestamp = self.get_image_to_points_mat(xyz_frame_id, tf2_buffer) image_to_point_mat, timestamp = self.get_image_to_points_mat(xyz_frame_id, tf2_buffer)
p = np.matmul(image_to_point_mat, np.array([xyz_pix[0], xyz_pix[1], xyz_pix[2], 1.0]))[:3] p = np.matmul(image_to_point_mat, np.array([xyz_pix[0], xyz_pix[1], xyz_pix[2], 1.0]))[:3]
return p return p
def make_robot_footprint_unobserved(self, robot_x_pix, robot_y_pix, robot_ang_rad): def make_robot_footprint_unobserved(self, robot_x_pix, robot_y_pix, robot_ang_rad):
# replace robot points with unobserved points # replace robot points with unobserved points
na.draw_robot_footprint_rectangle(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.image, value=0) na.draw_robot_footprint_rectangle(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.image, value=0)
@ -220,7 +216,6 @@ class ROSMaxHeightImage(MaxHeightImage):
if self.rgb_image is not None: if self.rgb_image is not None:
na.draw_robot_footprint_rectangle(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.rgb_image, value=0) na.draw_robot_footprint_rectangle(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.rgb_image, value=0)
def make_robot_mast_blind_spot_unobserved(self, robot_x_pix, robot_y_pix, robot_ang_rad): def make_robot_mast_blind_spot_unobserved(self, robot_x_pix, robot_y_pix, robot_ang_rad):
# replace mast blind spot wedge points with unobserved points # replace mast blind spot wedge points with unobserved points
na.draw_robot_mast_blind_spot_wedge(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.image, value=0) na.draw_robot_mast_blind_spot_wedge(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.image, value=0)
@ -228,7 +223,7 @@ class ROSMaxHeightImage(MaxHeightImage):
na.draw_robot_mast_blind_spot_wedge(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.camera_depth_image, value=0) na.draw_robot_mast_blind_spot_wedge(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.camera_depth_image, value=0)
if self.rgb_image is not None: if self.rgb_image is not None:
na.draw_robot_mast_blind_spot_wedge(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.rgb_image, value=0) na.draw_robot_mast_blind_spot_wedge(robot_x_pix, robot_y_pix, robot_ang_rad, self.m_per_pix, self.rgb_image, value=0)
def from_points_with_tf2(self, points, points_frame_id, tf2_buffer, points_timestamp=None, timeout_s=None): def from_points_with_tf2(self, points, points_frame_id, tf2_buffer, points_timestamp=None, timeout_s=None):
# points should be a numpy array with shape = (N, 3) where N # points should be a numpy array with shape = (N, 3) where N
# is the number of points. So it has the following structure: # is the number of points. So it has the following structure:
@ -273,7 +268,6 @@ class ROSMaxHeightImage(MaxHeightImage):
else: else:
rospy.logwarn('ROSMaxHeightImage.from_rgb_points_with_tf2: failed to update the image likely due to a failure to lookup the transform using TF2. points_frame_id = {0}, points_timestamp = {1}, timeout_s = {2}'.format(points_frame_id, points_timestamp, timeout_s)) rospy.logwarn('ROSMaxHeightImage.from_rgb_points_with_tf2: failed to update the image likely due to a failure to lookup the transform using TF2. points_frame_id = {0}, points_timestamp = {1}, timeout_s = {2}'.format(points_frame_id, points_timestamp, timeout_s))
def to_point_cloud(self, color_map=None): def to_point_cloud(self, color_map=None):
points = self.to_points(color_map) points = self.to_points(color_map)
point_cloud = ros_numpy.msgify(PointCloud2, points, stamp=self.last_update_time, frame_id=self.voi.frame_id) point_cloud = ros_numpy.msgify(PointCloud2, points, stamp=self.last_update_time, frame_id=self.voi.frame_id)

Write Preview
Loading…
Cancel
Save