def push_until_fail(self): global shutdown_requested hit_time = time.time() # push forever while time.time() - hit_time < 2: if self.callbacks.middle_bumper_pressed: hit_time = time.time() self.move_command(0.2, True) if shutdown_requested: return 'done4' box_position = trig.get_point(self.callbacks.bot_map_position, 0.405, self.callbacks.bot_map_heading) if trig.get_distance(box_position, self.callbacks.box_target_position) < 0.15: self.led1_pub.publish(1) # green self.led2_pub.publish(3) # red self.sound_pub.publish(1) time.sleep(5) self.led1_pub.publish(0) # off self.led2_pub.publish(0) # off return 'return' self.twist.linear.x = 0 self.cmd_vel_pub.publish(self.twist) return 'reverse'
def execute(self, userdata): global shutdown_requested distance_from_box = trig.get_distance( self.callbacks.box_target_position, self.target1.target_pose.pose.position) distance_from_bot = trig.get_distance( self.callbacks.bot_map_position, self.target1.target_pose.pose.position) start_position = self.callbacks.bot_odom_position end_position = start_position while trig.get_distance(start_position, end_position) < 0.4: if distance_from_box > distance_from_bot: self.twist.angular.z = -0.4 else: self.twist.angular.z = 0.4 self.twist.linear.x = -0.4 self.cmd_vel_pub.publish(self.twist) end_position = self.callbacks.bot_odom_position if shutdown_requested: return 'done4' distance_from_box = trig.get_distance( self.callbacks.box_target_position, self.target1.target_pose.pose.position) distance_from_bot = trig.get_distance( self.callbacks.bot_map_position, self.target1.target_pose.pose.position) self.clear_costmap() if distance_from_box > distance_from_bot: self.client.send_goal(self.target1) self.client.wait_for_result() self.client.send_goal(self.target2) self.client.wait_for_result() self.client.send_goal(self.target3) self.client.wait_for_result() print("Goal reached") return 'success4'
def execute(self, userdata): global shutdown_requested if shutdown_requested: return 'done4' get_point_heading = trig.get_heading_between_points( self.callbacks.box_position, self.callbacks.box_target_position) # Move around box if bot is closer to goal than box box_distance_to_goal = trig.get_distance( self.callbacks.bot_map_position, self.callbacks.box_target_position) bot_distance_to_goal = trig.get_distance( self.callbacks.bot_map_position, self.callbacks.box_target_position) offset = 0 self.clear_costmap() if bot_distance_to_goal < box_distance_to_goal: offset = 0.32 print("Going to intermediate goal") left_get_point_heading = get_point_heading - 90 if left_get_point_heading < 0: left_get_point_heading += 360 left_goal_position = trig.get_point(self.callbacks.box_position, 0.8, left_get_point_heading) right_goal_position = trig.get_point( self.callbacks.box_position, 0.8, (get_point_heading + 90) % 360) #print("------------------") #print("left") #print(left_goal_position) #print("right") #print(right_goal_position) #print("real") #print(self.callbacks.bot_position) #print("------------------") bot_distance_to_left = trig.get_distance( self.callbacks.bot_map_position, left_goal_position) bot_distance_to_right = trig.get_distance( self.callbacks.bot_map_position, right_goal_position) if bot_distance_to_left < bot_distance_to_right: print("Going to left side") #print(left_goal_position) self.goal.target_pose.header.frame_id = "map" self.goal.target_pose.header.stamp = rospy.Time.now() self.goal.target_pose.pose.position.x = left_goal_position.x self.goal.target_pose.pose.position.y = left_goal_position.y else: print("Going to right side") #print(right_goal_position) self.goal.target_pose.header.frame_id = "map" self.goal.target_pose.header.stamp = rospy.Time.now() self.goal.target_pose.pose.position.x = right_goal_position.x self.goal.target_pose.pose.position.y = right_goal_position.y #print(self.callbacks.box_position) goal_yaw = math.radians(90 - 180) goal_quaternion = quaternion_from_euler(0.0, 0.0, goal_yaw) #print(goal_quaternion) self.goal.target_pose.pose.orientation.x = goal_quaternion[0] self.goal.target_pose.pose.orientation.y = goal_quaternion[1] self.goal.target_pose.pose.orientation.z = goal_quaternion[2] self.goal.target_pose.pose.orientation.w = goal_quaternion[3] print("sending intermediate goal") self.client.send_goal(self.goal) self.client.wait_for_result() if self.callbacks.move_base_status == 4: return 'return' self.clear_costmap() # Move to proper pushing location goal_position = trig.get_point(self.callbacks.box_position, offset + 0.5, get_point_heading) goal_heading = (get_point_heading + 180) % 360 goal_yaw = math.radians(goal_heading - 180) goal_quaternion = quaternion_from_euler(0.0, 0.0, goal_yaw) #print(goal_quaternion) self.goal.target_pose.header.frame_id = "map" self.goal.target_pose.header.stamp = rospy.Time.now() self.goal.target_pose.pose.position.x = goal_position.x self.goal.target_pose.pose.position.y = goal_position.y self.goal.target_pose.pose.orientation.x = goal_quaternion[0] self.goal.target_pose.pose.orientation.y = goal_quaternion[1] self.goal.target_pose.pose.orientation.z = goal_quaternion[2] self.goal.target_pose.pose.orientation.w = goal_quaternion[3] if shutdown_requested: return 'done4' print("Going to main goal") self.client.send_goal(self.goal) self.client.wait_for_result() print(self.callbacks.move_base_status) if self.callbacks.move_base_status == 4: return 'return' if shutdown_requested: return 'done4' else: return 'push'
def execute(self, userdata): global shutdown_requested self.client.send_goal(self.target) self.client.wait_for_result() print("Goal reached") target_heading = (self.callbacks.bot_odom_heading + 350) % 360 turning = True previous_difference = None while turning: if shutdown_requested: return 'done4' difference = trig.minimum_angle_between_headings( target_heading, self.callbacks.bot_odom_heading) if previous_difference is None: self.twist.angular.z = 0.4 self.cmd_vel_pub.publish(self.twist) else: if abs(difference) < 1 or ( self.callbacks.box_position is not None and self.callbacks.box_target_position is not None): turning = False self.twist.angular.z = 0 self.cmd_vel_pub.publish(self.twist) else: self.twist.angular.z = 0.4 self.cmd_vel_pub.publish(self.twist) if previous_difference != difference: previous_difference = difference if self.callbacks.box_position is not None: self.led2_pub.publish(3) # red self.sound_pub.publish(1) time.sleep(1) if self.callbacks.box_target_position is not None: self.led1_pub.publish(1) # Green self.sound_pub.publish(1) time.sleep(1) self.led2_pub.publish(0) # Off self.led1_pub.publish(0) # Off if self.callbacks.box_target_position is None or self.callbacks.box_position is None: return 'return' # Can't find box and/or box target, so forget about pushing boxes else: global box2_position global box8_position box_to_8_distance = trig.get_distance(self.callbacks.box_position, box8_position) box_target_to_8_distance = trig.get_distance( self.callbacks.box_target_position, box8_position) if box_to_8_distance < box_target_to_8_distance: return 'box8' else: return 'box2'