def execute(self, userdata): #disable obstacle checking! rospy.sleep(10.0) #wait a sec for beacon pose to catch up target_point = deepcopy(userdata.platform_point) target_point.header.stamp = rospy.Time(0) yaw, distance = util.get_robot_strafe(self.tf_listener, target_point) move = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE, angle = yaw, distance = distance, velocity = 0.5) userdata.simple_move = move self.announcer.say("Execute ing final mount move") return 'final'
def check_beacon_pose(self, event): if not self.publish_beacon: return if not self.beacon_enabled: return try: #get distances and yaws from reality frame origin platform_origin = PointStamped(std_msg.Header(0, rospy.Time(0), 'platform'), Point(0,0,0)) angle_to_platform, dist_from_platform = util.get_robot_strafe(self.tf_listener, platform_origin) angle_to_robot = util.get_robot_yaw_from_origin(self.tf_listener, 'platform') except tf.Exception, exc: print("Transforms not available in publish beacon: {!s}".format(exc)) return
def execute(self, userdata): #wait a sec for beacon pose to adjust the localization filter saved_point_odom = None #wait for a maximum of one minute, then mount anyway timeout_time = rospy.Time.now() + rospy.Duration(60.0) in_tolerance_count = 0 while not rospy.core.is_shutdown_requested(): rospy.sleep(3.0) try: platform_point_odom = self.tf_listener.transformPoint(userdata.odometry_frame, userdata.platform_point) except tf.Exception: rospy.logwarn("LEVEL_TWO calculate_mount failed to transform platform point") if saved_point_odom is None: saved_point_odom = platform_point_odom else: correction_error = util.point_distance_2d(platform_point_odom.point, saved_point_odom.point) saved_point_odom = platform_point_odom if correction_error < userdata.beacon_mount_tolerance: in_tolerance_count += 1 else: in_tolerance_count = 0 #if we get 3 in_tolerance corrections, mount if in_tolerance_count > 2: break if rospy.Time.now() > timeout_time: self.announcer.say("Beacon correction did not converge.") break else: self.announcer.say("Correction. {:.3f}".format(correction_error)) yaw, distance = util.get_robot_strafe(self.tf_listener, userdata.platform_point) userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE, angle = yaw, distance = distance, velocity = 0.5) self.announcer.say("Initiate ing mount move.") return 'move'
def execute(self, userdata): if (len(userdata.point_list) > 0): rospy.loginfo("MOVE TO POINTS list: %s" %(userdata.point_list)) target_point = userdata.point_list[0] header = std_msg.Header(0, rospy.Time(0), userdata.odometry_frame) target_stamped = geometry_msg.PointStamped(header, target_point) try: yaw, distance = util.get_robot_strafe(self.tf_listener, target_stamped) except(tf.Exception): rospy.logwarn("MOVE_TO_POINTS failed to transform search point (%s) to base_link in 1.0 seconds", search_point.header.frame_id) return 'aborted' userdata.point_list.popleft() userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE, angle = yaw, distance = distance, velocity = userdata.velocity) return 'next_point' else: return 'complete' return 'aborted'
def execute(self, userdata): result = samplereturn_msg.GeneralExecutiveResult() result.result_string = 'approach failed' result.result_int = samplereturn_msg.NamedPoint.NONE userdata.action_result = result #default velocity for all moves is in simple_mover! #initial approach pursuit done at pursuit_velocity userdata.lighting_optimized = True userdata.search_count = 0 userdata.grab_count = 0 rospy.loginfo("SAMPLE_PURSUIT input_point: %s" % (userdata.action_goal.input_point)) #store the filter_id on entry, this instance of pursuit will only try to pick up this hypothesis userdata.latched_filter_id = userdata.action_goal.input_point.filter_id userdata.filter_changed = False point, pose = calculate_pursuit(self.tf_listener, userdata.action_goal.input_point, userdata.min_pursuit_distance, userdata.odometry_frame) try: approach_point = geometry_msg.PointStamped(pose.header, pose.pose.position) yaw, distance = util.get_robot_strafe(self.tf_listener, approach_point) robot_yaw = util.get_current_robot_yaw(self.tf_listener, userdata.odometry_frame) except tf.Exception: rospy.logwarn("PURSUE_SAMPLE failed to get base_link -> %s transform in 1.0 seconds", sample_frame) return 'aborted' rospy.loginfo("INITIAL_PURSUIT calculated with distance: {:f}, yaw: {:f} ".format(distance, yaw)) #this is the initial vfh goal pose goal = samplereturn_msg.VFHMoveGoal(target_pose = pose, move_velocity = userdata.pursuit_velocity, spin_velocity = userdata.spin_velocity, orient_at_target = True) userdata.pursuit_goal = goal #create return destination current_pose = util.get_current_robot_pose(self.tf_listener, userdata.odometry_frame) current_pose.header.stamp = rospy.Time(0) goal = samplereturn_msg.VFHMoveGoal(target_pose = current_pose, move_velocity = userdata.pursuit_velocity, spin_velocity = userdata.spin_velocity, orient_at_target = False) userdata.return_goal = goal #if distance to sample approach point is small, use a simple move if distance < userdata.simple_pursuit_threshold: facing_yaw = euler_from_orientation(pose.pose.orientation)[-1] dyaw = util.unwind(facing_yaw - robot_yaw) userdata.approach_strafe = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE, angle = yaw, distance = distance, velocity = userdata.pursuit_velocity) userdata.face_sample = SimpleMoveGoal(type=SimpleMoveGoal.SPIN, angle = dyaw, velocity = userdata.spin_velocity) return 'simple' #if further than threshold make a vfh move to approach point else: return 'goal'
def update_sectors(self, sectors, sector_angle): #sector_count and zero offset sector_count = len(sectors) zero_offset = np.rint(sector_count/2.0) #get current position from mover object robot_position = self._mover.current_position robot_yaw = self._mover.current_yaw yaw_to_target, distance_to_target = util.get_robot_strafe(self._tf, self._target_point_odom) target_index = int(round(yaw_to_target/sector_angle) + zero_offset) #if we are too far off the line between start point, and goal, initiate stop distance_off_course = util.point_distance_to_line(geometry_msg.Point(*robot_position), self._start_point, self._target_point_odom.point) valid, robot_cmap_coords = self.world2map(robot_position[:2], self.costmap_info) obstacle_density = np.zeros(len(sectors)) inverse_costs = np.zeros(len(sectors)) #nonzero_coords = np.transpose(np.nonzero(self.costmap)) for index in range(sector_count): #yaw in odom sector_yaw = robot_yaw + (index - zero_offset) * sector_angle #get start and end points for the bresenham line start = self.bresenham_point(robot_cmap_coords, self.min_obstacle_distance, sector_yaw, self.costmap_info.resolution) #if we are close to the target, truncate the check distance end = self.bresenham_point(robot_cmap_coords, min(self.max_obstacle_distance, (distance_to_target+self._goal_obstacle_radius)), sector_yaw, self.costmap_info.resolution) sector_line = bresenham.points(start, end) for coords in sector_line: cell_value = self.costmap[tuple(coords)] position = self.costmap_info.resolution * (coords - robot_cmap_coords) distance = np.linalg.norm(position) #m is the obstacle vector magnitude m = cell_value * (self.max_obstacle_distance - distance) obstacle_density[index] += m #set sectors above threshold density as blocked, those below as clear, and do #not change the ones in between if obstacle_density[index] >= self.threshold_high: sectors[index] = True inverse_costs[index] = 0 #inversely infinitely expensive if obstacle_density[index] <= self.threshold_low: sectors[index] = False #if sector is clear (it may not have been changed this time!) set its cost, making it a candidate sector if not sectors[index]: inverse_costs[index] = 1.0 / ( self.u_goal*np.abs(yaw_to_target - (index - zero_offset)*sector_angle) + self.u_current*np.abs(self.current_sector_index - index)*sector_angle) #START DEBUG CRAP if self.publish_debug: rospy.logdebug("TARGET SECTOR index: %d" % (target_index)) rospy.logdebug("SECTORS: %s" % (sectors)) rospy.logdebug("INVERSE_COSTS: %s" % (inverse_costs)) vfh_debug_array = [] vfh_debug_marker = vis_msg.Marker() vfh_debug_marker.pose = Pose() vfh_debug_marker.header = std_msg.Header(0, rospy.Time(0), self.odometry_frame) vfh_debug_marker.type = vis_msg.Marker.ARROW vfh_debug_marker.color = std_msg.ColorRGBA(0, 0, 0, 1) arrow_len = min(self.max_obstacle_distance, (distance_to_target + self._goal_obstacle_radius)) vfh_debug_marker.scale = geometry_msg.Vector3(arrow_len, .02, .02) vfh_debug_marker.lifetime = rospy.Duration(0.2) for index in range(sector_count): debug_marker = deepcopy(vfh_debug_marker) sector_yaw = robot_yaw + (index - zero_offset) * sector_angle quat_vals = tf.transformations.quaternion_from_euler(0, 0, sector_yaw) debug_marker.pose.orientation = geometry_msg.Quaternion(*quat_vals) debug_marker.pose.position = geometry_msg.Point(robot_position[0], robot_position[1], 0) if self.current_sector_index == index: debug_marker.color = std_msg.ColorRGBA(0.1, 1.0, 0.1, 1) elif not sectors[index]: debug_marker.color = std_msg.ColorRGBA(0.1, 0.1, 1.0, 1) else: debug_marker.color = std_msg.ColorRGBA(1.0, 0.1, 0.1, 1) debug_marker.id = self.marker_id self.marker_id += 1 vfh_debug_array.append(debug_marker) vfh_debug_msg = vis_msg.MarkerArray(vfh_debug_array) self.debug_marker_pub.publish(vfh_debug_msg) #load the state dictionary vfh_state = {} min_cost_index = np.argmax(inverse_costs) vfh_state['minimum_cost_index'] = min_cost_index vfh_state['minimum_cost_angle'] = (min_cost_index - zero_offset) * sector_angle vfh_state['target_index'] = target_index vfh_state['distance_to_target'] = distance_to_target vfh_state['distance_off_course'] = distance_off_course return sectors, vfh_state
def execute_movement(self, move_velocity, spin_velocity): ##### Begin rotate to clear section #if requested rotate until the forward sector is clear, then move #clear_distance straight ahead, then try to move to the target pose if self._rotate_to_clear: start_dyaw, d = util.get_robot_strafe(self._tf, self._target_point_odom) rot_sign = np.sign(start_dyaw) #first try rotating toward the target 120 degrees or so error = self._mover.execute_spin(rot_sign*self._clear_first_angle, max_velocity = spin_velocity, stop_function=self.clear_check) if self.exit_check(): return #if that doesn't work, spin all the way around if np.abs(error) < self._goal_orientation_tolerance: self._mover.execute_spin(-1*rot_sign*2*np.pi, max_velocity = spin_velocity, stop_function=self.clear_check) if self.exit_check(): return dyaw, d = util.get_robot_strafe(self._tf, self._target_point_odom) rospy.loginfo("VFH finished rotate_to-clear, start dyaw: {:f}, finish dyaw: {:f}".format(start_dyaw, dyaw)) if np.abs(start_dyaw - dyaw) < self._goal_orientation_tolerance: #we spun all the way around +/- a little, so we are trapped self._action_outcome == VFHMoveResult.BLOCKED else: #we found a clear spot, drive clear_distance meters ahead saved_odom_point = deepcopy(self._target_point_odom) #save the final goal for later header = std_msg.Header(0, rospy.Time(0), 'base_link') point = geometry_msg.Point(self._clear_distance, 0, 0) target_point_base = geometry_msg.PointStamped(header, point) self.set_path_points_odom(target_point_base) if self.publish_debug: self.publish_debug_path() self.vfh_running = True self._mover.execute_continuous_strafe(0.0, max_velocity = move_velocity, stop_function=self.is_stop_requested) self.vfh_running = False if self.exit_check(): return #reload previous goal self.set_path_points_odom(saved_odom_point) if self.publish_debug: self.publish_debug_path() ##### Begin normal movement section self._action_outcome = VFHMoveResult.ERROR #initialize outcome to error ##### Turn to face goal dyaw, d = util.get_robot_strafe(self._tf, self._target_point_odom) if np.abs(dyaw) > self._goal_orientation_tolerance: rospy.loginfo("VFH Rotating to point at goal by: %f.", dyaw) self._mover.execute_spin(dyaw, max_velocity = spin_velocity, stop_function=self.is_stop_requested) if self.exit_check(): return ##### Strafe to goal pose using VFH yaw, distance = util.get_robot_strafe(self._tf, self._target_point_odom) #start vfh and wait for one update self.vfh_running = True start_time = rospy.Time.now() while not self._as.is_preempt_requested(): rospy.sleep(0.1) if self.last_vfh_update > start_time: break #If we received a valid goal (not too short, or out of target window), then #the outcome should still be un-set (still ERROR). if self._action_outcome == VFHMoveResult.ERROR: rospy.loginfo("VFH Moving %f meters ahead.", distance) self._mover.execute_continuous_strafe(0.0, max_velocity = move_velocity, stop_function=self.is_stop_requested) self.vfh_running = False if self.exit_check(): return elif self._action_outcome == VFHMoveResult.BLOCKED: rospy.loginfo("VFH reporting STARTED_BLOCKED") #we found all sectors blocked before we even tried to move self._action_outcome = VFHMoveResult.STARTED_BLOCKED #if we got to the goal(ish), turn and face in requested orientation if self._action_outcome in [VFHMoveResult.COMPLETE, VFHMoveResult.MISSED_TARGET]: # turn to requested orientation dyaw = self.yaw_error() if self._orient_at_target and (np.abs(dyaw) > self._goal_orientation_tolerance): rospy.loginfo("VFH Rotating to goal yaw: %f.", dyaw) self._mover.execute_spin(dyaw, spin_velocity, stop_function=self.is_stop_requested) if self.exit_check(): return rospy.logdebug("Successfully completed goal.") self._as.set_succeeded(result = VFHMoveResult(outcome = self._action_outcome, position_error = self.position_error(), yaw_error = Float64(self.yaw_error())))