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'