def publish_goal(self, pose, lookahead_distance=0.0, stop_at_goal=False): goal = Goal() goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = "odom" lookahead = tf_conversions.Frame( tf_conversions.Vector(lookahead_distance, 0, 0)) # add the offset for navigating to the goal: # (offset * odom).Inverse() * goal = odom.Invserse() * pose # goal = (offset * odom) * odom.Inverse() * pose # goal = offset * pose original_pose_frame = tf_conversions.fromMsg(pose) pose_frame = self.zero_odom_offset * original_pose_frame original_pose_frame_lookahead = original_pose_frame * lookahead pose_frame_lookahead = pose_frame * lookahead goal.pose.pose.position.x = pose_frame_lookahead.p.x() goal.pose.pose.position.y = pose_frame_lookahead.p.y() goal.pose.pose.orientation = tf_conversions.toMsg( pose_frame_lookahead).orientation goal.stop_at_goal.data = stop_at_goal self.goal_pub.publish(goal) self.goal_plus_lookahead = tf_conversions.toMsg( original_pose_frame_lookahead) if self.publish_gt_goals: try: trans = self.tfBuffer.lookup_transform('map', 'odom', rospy.Time()) trans_frame = tf_conversions.Frame( tf_conversions.Rotation(trans.rotation.x, trans.rotation.y, trans.rotation.z, trans.rotation.w), tf_conversions.Vector(trans.translation.x, trans.translation.y, trans.translation.z)) goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = "map" goal_pose.pose = tf_conversions.toMsg(trans_frame * pose_frame_lookahead) self.goal_pose_pub.publish(goal_pose) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print('Could not lookup transform from /map to /odom') pass
def remove_anchor_pose(cls, anchor, data): # Convert anchor pose into a PyKDL.Frame: simplifies anchor_frame = tf_conversions.fromMsg(anchor.pose) anchor_frame.M = PyKDL.Rotation() # NO ROTATION def subtract_pose(point, verbose=False): p = copy.deepcopy(point) # Find the difference in poses. NOTE we do not change anything other # than the pose (twist & wrench stay the same). point_frame = tf_conversions.fromMsg(point.pose) delta = anchor_frame.Inverse() * point_frame p.pose = tf_conversions.toMsg(delta) if verbose: print('{} {} {} -> {} {} {}'.format(point.pose.position.x, point.pose.position.y, point.pose.position.z, p.pose.position.x, p.pose.position.y, p.pose.position.z)) return p parsed = [subtract_pose(x) for x in data] # Create an identity translation/rotation for the new anchor pose. anchor_new = copy.deepcopy(anchor) identity = tf_conversions.Frame() anchor_new.pose = tf_conversions.toMsg(identity) return (anchor_new, parsed)
def get_corrected_goal_offset(goal1, goal2, rotation_correction, correction_length): '''apply rotation and length corrections to the offset from goal1 to goal2''' goal_offset = goal1.Inverse() * goal2 goal_offset = tf_conversions.Frame( tf_conversions.Rotation.RotZ(rotation_correction)) * goal_offset goal_offset.p *= correction_length return goal_offset
def integrate_corrected_poses(poses, origin, theta_corrections, path_corrections): corrected_poses = [poses.frames[0]] for i in range(1,len(poses.frames)): new_pose = poses.frames[i] old_pose = poses.frames[i-1] current_goal = corrected_poses[-1] pose_offset = old_pose.Inverse() * new_pose pose_offset.p = tf_conversions.Rotation.RotZ(current_goal.M.GetRPY()[2]) * pose_offset.p pose_offset_corrected = pose_offset if theta_corrections is not None and len(theta_corrections) > i: pose_offset_corrected = tf_conversions.Frame(tf_conversions.Rotation.RotZ(theta_corrections[i])) * pose_offset_corrected if path_corrections is not None and len(path_corrections) > i: pose_offset_corrected.p *= path_corrections[i] corrected_poses.append(tf_conversions.Frame(pose_offset_corrected.M * current_goal.M, pose_offset_corrected.p + current_goal.p)) origin_offset = poses.frames[0].Inverse() * origin corrected_poses = [pose * origin_offset for pose in corrected_poses] return poses_to_np(corrected_poses)
def get_offset_px(goal, pose): visual_feature_range = 1#np.random.uniform(3.0, 5.0) visual_feature_angle = 0#np.random.uniform(-math.radians(10), math.radians(10)) visual_feature_offset = tf_conversions.Frame() visual_feature_offset.M.DoRotZ(visual_feature_angle) visual_feature_offset.p.x(visual_feature_range) visual_feature = goal * visual_feature_offset offset = pose.Inverse() * visual_feature return -localiser.rad_to_px(visual_feature_angle - math.atan2(offset.p.y(), offset.p.x()))
def make_new_goal(self, rotation_correction=0.0, path_correction=1.0): old_goal_index = self.goal_index old_goal_frame_world = tf_conversions.fromMsg( self.poses[old_goal_index]) current_goal_frame_odom = tf_conversions.fromMsg(self.goal) state = self.update_goal_index() if state == GOAL_STATE.finished: print('Localiser stopping. Reached final goal.') self.running = False return if state == GOAL_STATE.restart: # don't have a big offset from the end of the path, back to the start old_goal_frame_world = tf_conversions.Frame() new_goal_frame_world = tf_conversions.fromMsg( self.poses[self.goal_index]) turning_goal = is_turning_goal(old_goal_frame_world, new_goal_frame_world) goal_offset = get_corrected_goal_offset(old_goal_frame_world, new_goal_frame_world, rotation_correction, path_correction) new_goal = current_goal_frame_odom * goal_offset sum_path_correction_ratio = ( GOAL_DISTANCE_SPACING + self.sum_path_correction) / GOAL_DISTANCE_SPACING self.update_goal(new_goal, True, turning_goal) self.save_data_at_goal(self.last_odom_pose, new_goal, new_goal_frame_world, self.sum_theta_correction, sum_path_correction_ratio) self.goal_number += 1 print('[%d] theta [%f]\tpath [%f]' % (old_goal_index, math.degrees( self.sum_theta_correction), sum_path_correction_ratio)) if turning_goal: print('turning goal:') self.sum_theta_correction = 0 self.sum_path_correction = 0.0
def np_to_frame(pose_tuple): return tf_conversions.Frame( tf_conversions.Rotation.RotZ(pose_tuple[2]), tf_conversions.Vector(pose_tuple[0], pose_tuple[1], 0))
LOOKAHEAD_DISTANCE * math.sin(goal[2]), 0 ]) turning_goal = False actual_goals_odom = [] actual_goals_world = [] update_locations = [] x_errors = [] continuous_offsets = [] continuous_expected_offsets = [] continuous_path_offsets = [] odom = Odometry() odom.pose.pose.position.x = 0.0 odom.pose.pose.position.y = 0.0 q = tf_conversions.Frame().M.RotZ(math.radians(0)).GetQuaternion() odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] current_frame_odom = tf_conversions.fromMsg(odom.pose.pose) world = Pose() world.position.x = odom.pose.pose.position.x world.position.y = odom.pose.pose.position.y world.orientation.z = odom.pose.pose.orientation.z world.orientation.w = odom.pose.pose.orientation.w current_frame_world = tf_conversions.fromMsg(world) N = 10000 xs = [] ys = [] thetas = []
def start(self): if self.global_localisation_init: if self.last_odom_pose is None or self.last_image is None: print( 'Global localisation - waiting for first odom and image messages' ) while self.last_odom_pose is None or self.last_image is None: time.sleep(0.2) # sleep lets subscribers be processed offsets, correlations = self.calculate_image_pose_offset( 0, len(self.poses)) best_match = np.argmax(correlations) if best_match == 0 or (best_match == 1 and correlations[0] > correlations[2]): goal_pose = tf_conversions.Frame() odom_pose = tf_conversions.fromMsg(self.last_odom_pose) self.zero_odom_offset = tf_conversions.fromMsg( self.last_odom_pose) if self.poses[0].position.x == 0 and self.poses[ 0].position.y == 0 and tf_conversions.fromMsg( self.poses[0]).M.GetRPY()[2] == 0: # the first pose is at 0,0,0 so we ignore it and set the first goal to the next pose print('Localiser: starting at goal 1, goal 0 = [0,0,0]') self.goal_index = 1 else: self.goal_index = 0 else: if best_match == len(self.poses) - 1: self.goal_index = len(self.poses) - 2 elif correlations[best_match + 1] >= correlations[best_match - 1]: self.goal_index = best_match else: self.goal_index = best_match - 1 # goal = offset.Inverse() * odom # offset.Inverse() = goal * odom.Inverse() # offset = (goal * odom.Inverse()).Inverse() goal_pose = tf_conversions.fromMsg(self.poses[self.goal_index - 1]) odom_pose = tf_conversions.fromMsg(self.last_odom_pose) self.zero_odom_offset = (goal_pose * odom_pose.Inverse()).Inverse() self.last_odom_pose = tf_conversions.toMsg( self.zero_odom_offset.Inverse() * tf_conversions.fromMsg(self.last_odom_pose)) print( 'Global localisation - best match at pose %d [correlation = %f]' % (self.goal_index, correlations[best_match])) if correlations[best_match] < self.min_init_correlation: print( 'Stop, best match [%.2f] did not exceed minimum correlation ([%.2f])' % (correlations[best_match], self.min_init_correlation)) return else: if self.last_odom_pose is None: print('Global localisation - waiting for first odom message') while self.last_odom_pose is None: time.sleep(0.2) # sleep lets subscribers be processed goal_pose = tf_conversions.Frame() odom_pose = tf_conversions.fromMsg(self.last_odom_pose) self.zero_odom_offset = tf_conversions.fromMsg(self.last_odom_pose) if self.poses[0].position.x == 0 and self.poses[ 0].position.y == 0 and tf_conversions.fromMsg( self.poses[0]).M.GetRPY()[2] == 0: # the first pose is at 0,0,0 so we ignore it and set the first goal to the next pose print('Localiser: starting at goal 1, goal 0 = [0,0,0]') self.goal_index = 1 else: self.goal_index = 0 self.update_goal(tf_conversions.fromMsg(self.poses[self.goal_index])) self.running = True
def setup_parameters(self): # Teach Repeat Params self.rotation_correction_gain = rospy.get_param( '~rotation_correction_gain', 0.01) self.path_correction_gain = rospy.get_param('~path_correction_gain', 0.01) # Wait for ready self.ready = not rospy.get_param('/wait_for_ready', False) self.global_localisation_init = rospy.get_param( '~global_localisation_init', False) self.min_init_correlation = rospy.get_param('~min_init_correlation', 0.0) self.running = False self.mutex = threading.Lock() # Odom self.load_dir = os.path.expanduser( rospy.get_param('/data_load_dir', '~/miro/data')) if self.load_dir[-1] != '/': self.load_dir += '/' self.sum_theta_correction = 0.0 self.sum_path_correction = 0.0 self.stop_at_end = rospy.get_param('~stop_at_end', True) self.discrete_correction = rospy.get_param('~discrete-correction', False) # Load pose data pose_files = [ self.load_dir + f for f in os.listdir(self.load_dir) if f[-9:] == '_pose.txt' ] pose_files.sort() self.poses = load_poses(pose_files) self.goal_index = 0 self.goal = tf_conversions.toMsg(tf_conversions.Frame()) self.last_goal = tf_conversions.toMsg(tf_conversions.Frame()) self.goal_plus_lookahead = tf_conversions.toMsg(tf_conversions.Frame()) self.last_odom_pose = None self.zero_odom_offset = None global GOAL_DISTANCE_SPACING, LOOKAHEAD_DISTANCE_RATIO, TURNING_TARGET_RANGE_DISTANCE_RATIO, GOAL_THETA_TOLERANCE GOAL_DISTANCE_SPACING = rospy.get_param('/goal_pose_seperation', 0.2) LOOKAHEAD_DISTANCE_RATIO = rospy.get_param('/lookahead_distance_ratio', 0.65) TURNING_TARGET_RANGE_DISTANCE_RATIO = rospy.get_param( '/turning_target_range_distance_ratio', 0.5) GOAL_THETA_TOLERANCE = rospy.get_param('/goal_theta_tolerance', 5) # Image self.resize = image_processing.make_size( height=rospy.get_param('/image_resize_height', None), width=rospy.get_param('/image_resize_width', None)) if self.resize[1] is not None: global IMAGE_WIDTH IMAGE_WIDTH = self.resize[1] if self.resize[0] is None and self.resize[1] is None: self.resize = None self.last_image = None self.patch_size = image_processing.parse_patch_size_parameter( rospy.get_param('/patch_size', (9, 9))) global CORR_SUB_SAMPLING, FIELD_OF_VIEW_DEG self.image_recognition_threshold = rospy.get_param( '/image_recognition_threshold', 0.1) CORR_SUB_SAMPLING = rospy.get_param('/image_subsampling', 1) FIELD_OF_VIEW_DEG = rospy.get_param('/image_field_of_view_width_deg', 2 * 60.6 + 2 * 27.0) FIELD_OF_VIEW_RAD = math.radians(FIELD_OF_VIEW_DEG) self.search_range = rospy.get_param('~search-range', 1) # data saving self.save_dir = os.path.expanduser( rospy.get_param('/data_save_dir', '~/miro/data/follow-straight_tests/5')) self.save_full_res_images = rospy.get_param('/save_full_res_images', True) self.save_full_res_image_at_goal = rospy.get_param( '/save_full_res_image_at_goal', True) self.last_full_res_image = None self.save_gt_data = rospy.get_param('/save_gt_data', True) self.publish_gt_goals = rospy.get_param('/publish_gt_goals', True) if self.save_dir[-1] != '/': self.save_dir += '/' if not os.path.isdir(self.save_dir): os.makedirs(self.save_dir) if not os.path.isdir(self.save_dir + 'norm/'): os.makedirs(self.save_dir + 'norm/') if not os.path.isdir(self.save_dir + 'pose/'): os.makedirs(self.save_dir + 'pose/') if not os.path.isdir(self.save_dir + 'offset/'): os.makedirs(self.save_dir + 'offset/') if not os.path.isdir(self.save_dir + 'correction/'): os.makedirs(self.save_dir + 'correction/') if self.save_full_res_images or self.save_full_res_image_at_goal: if not os.path.isdir(self.save_dir + 'full/'): os.makedirs(self.save_dir + 'full/') self.goal_number = 0 self.save_params()