def calculate_path(self):
# Only update path if robot is more than a 0.5m from the target
dist = Position(self.state[0, 0], self.state[1, 0]).distanceTo(self.goal_pos)
if not dist < 0.5 or self.global_path is None:
path = create_path(Position(self.state[0, 0], self.state[1, 0]), self.goal_pos, self.grid)
if path:
self.global_path = np.array(path)
self.current_index = 0 # new paths are created from robot's current position, so robot is at index 0
else:
print("Path could not be found, using old path")
def __init__(self, reference_point_x, goal=None, path=None, config=None):
self.config = config
self.global_path = path
self.local_path = None
if path is None:
self.goal_pos = goal
else:
self.goal_pos = Position(self.global_path[-1, 0], self.global_path[-1, 1]) # Used if path is manually set
self.grid = None
self.current_index = 0 # Fractional index of where robot's reference point is along path
self.index = 0
self.a = 0 # Coefficients of quadratic equation for the approximate path the robot is trying to follow
self.b = 0
self.c = 0
self.state = np.zeros((3, 1))
self.state_dot = np.zeros((3, 1))
self.reference_point_x = reference_point_x
self.target_angular_vel = 0
self.closest_point = None
self.alpha = 1
self.r = 100
self.last_s = 0
self.errors = []
self.error_dots = []
self.done = False
self.drive_backwards = -1 # -1 for backwards, 1 for forwards
def on_click(self, event):
self.object_id = -1
x, y = event.widget.winfo_pointerxy()
x, y = (x - self.win.winfo_rootx()) / self.SCALE, (
y - self.win.winfo_rooty()) / self.SCALE
x, y = x, self.arena_height - y
for i, obstacle in enumerate(self.obstacles):
if Position(x, y).distanceTo(
Position(obstacle.center_x,
obstacle.center_y)) < obstacle.getRadius():
self.object_id = i
break
if Position(x, y).distanceTo(self.goal) < 0.1:
self.object_id = len(self.obstacles)
def softwareTest():
goal = Position(0.5, 6.5)
obstacles = [
Obstacle(3, 2.0, 0.35),
Obstacle(0.5, 5, 0.35),
Obstacle(0.5, 1.5, 0.35)
]
tester = InteractivePathTester(goal, 3.78, 7.38, obstacles)
tester.update_path_and_draw()
def update_drag_position(self, event):
x, y = event.widget.winfo_pointerxy()
x, y = (x - self.win.winfo_rootx()) / self.SCALE, (
y - self.win.winfo_rooty()) / self.SCALE
x, y = x, self.arena_height - y
if 0 <= self.object_id < len(self.obstacles):
self.obstacles[self.object_id].setCenter(x, y)
self.update_path_and_draw()
elif self.object_id > 0:
self.goal = Position(x, y)
self.controller.set_goal(self.goal)
self.update_path_and_draw()
def go_to_goal(self, goal_msg):
goal = Position(goal_msg.x, goal_msg.y)
rospy.loginfo("Received ({}, {})".format(goal.x, goal.y))
self.state = State.FOLLOWING
self.controller.reset()
self.controller.set_drive_backwards(False)
self.controller.set_goal(goal)
self.controller.state = self.robot_state["state"]
self.controller.state_dot = self.robot_state["state_dot"]
self.controller.calculate_path()
self.publish_path()
rospy.loginfo("Going to ({}, {})".format(goal.x, goal.y))
frequency = 30
rate = rospy.Rate(frequency)
last_time = rospy.get_time()
last_pause_time = 0
while not (rospy.is_shutdown() or self.controller.done
or self.state == State.PREEMPTED):
time = rospy.get_time()
vel, angular_vel = 0, 0
if self.server.is_preempt_requested():
self.state = State.PREEMPTED
if self.step % int(
0.5 * frequency
) == 0 and self.state != State.PREEMPTED: # Every half second
self.controller.update_grid(self.grid)
if self.controller.is_path_blocked(
) and self.state == State.FOLLOWING:
self.state = State.BLOCKED_PAUSE
rospy.loginfo("Path blocked, regenerating")
last_pause_time = rospy.get_time()
rospy.loginfo("Currently at: {:.2f}".format(
self.controller.current_index))
if self.state == State.FOLLOWING:
vel, angular_vel = self.controller.get_target_vels(
self.robot_state["state"], self.robot_state["state_dot"],
time - last_time)
elif self.state == State.BLOCKED_PAUSE:
vel = 0
angular_vel = 0
if time - last_pause_time > 2:
self.controller.calculate_path()
self.publish_path()
self.state = State.FOLLOWING
elif self.state == State.PREEMPTED:
vel = 0
angular_vel = 0
# only used for target wheel speeds for now, not published directly
right_speed = (vel + angular_vel * effective_robot_width /
2) * 30 / (np.pi * wheel_radius)
left_speed = (vel - angular_vel * effective_robot_width /
2) * 30 / (np.pi * wheel_radius)
self.target_vels.append(vel)
self.target_ang_vels.append(angular_vel)
self.vels.append(self.robot_state["state_dot"][0, 0])
self.ang_vels.append(self.robot_state["state_dot"][2, 0])
self.target_wheel_speeds.append([right_speed, left_speed])
self.wheel_speeds.append(self.encoder_values)
self.publish_command_vel(vel, angular_vel)
self.publish_control_data()
self.draw()
self.step += 1
last_time = time
try:
rate.sleep() # Wait for desired time
except rospy.ROSInterruptException as e:
rospy.loginfo(str(e))
if rospy.is_shutdown():
result = GoToGoalResult(success=False)
self.server.set_aborted(result)
rospy.loginfo("Path Aborted")
elif self.state != State.PREEMPTED:
result = GoToGoalResult(success=True)
self.server.set_succeeded(result)
rospy.loginfo("Path Complete")
else:
result = GoToGoalResult(success=False)
self.server.set_preempted(result)
rospy.loginfo("Path Prempted")
def is_path_blocked(self):
for i in range(len(self.global_path) - 1):
if checkBlocked(Position(*self.global_path[i]), Position(*self.global_path[i+1]), self.grid):
return True
return False