class GlobalPlanner:
matrix = None
def __init__(self, precision):
rospy.init_node("global_planner", anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param("~position")
startX, startY = positions["startX"], positions["startY"]
targetX, targetY = positions["targetX"], positions["targetY"]
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
# do not crop if target outside of maze
self.map_loader = MapLoader(start, target)
self.map_matrix = self.map_loader.loadMap()
GlobalPlanner.matrix = self.map_matrix
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
self.precision = precision
# Calculate path
t = time.time()
self.path_finder = AStar(self.map_matrix)
raw_path = self.path_finder.search()
print(time.time() - t)
Cell.start = self.path_finder.start
self.path = [Cell(r, c) for r, c in raw_path]
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# self.pid_pub = rospy.Publisher("/pid_err", Float64, queue_size=10)
# Setup subscribers
_ = rospy.Subscriber("/odom", Odometry, self.odom_callback)
# odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.odom_callback)
def odom_callback(self, msg):
self.pose.x = msg.pose.pose.position.x
self.pose.y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(_, _, self.pose.theta) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def next_pose(self):
try:
self.path_index += 1
self.goal = self.path[self.path_index].pose()
rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x,
self.goal.y)
except IndexError:
rospy.logwarn("REACHED END OF PATH!")
def run(self):
rate = rospy.Rate(10) # 10hz
speed = Twist()
goto = GotoController()
goto.set_max_linear_acceleration(.05)
goto.set_max_angular_acceleration(.2)
goto.set_forward_movement_only(True)
while not rospy.is_shutdown():
speed_pose = goto.get_velocity(self.pose, self.goal, 0)
# self.pid_pub.publish(goto.desiredAngVel)
speed.linear.x = speed_pose.xVel
speed.angular.z = speed_pose.thetaVel
self.cmd_vel_pub.publish(speed)
if goto.get_goal_distance(self.pose,
self.goal) <= .05: # 5 cm precision
self.next_pose()
rate.sleep()
class ProSolver:
matrix = None
def __init__(self):
rospy.init_node('maze_pro_solver', anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param('~position')
startX, startY = positions['startX'], positions['startY']
targetX, targetY = positions['targetX'], positions['targetY']
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
ProSolver.matrix = self.map_matrix
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
Cell.start = self.path_finder.start
self.path = [Cell(r, c) for r, c in raw_path]
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Setup subscribers
#odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped,
self.odom_callback)
def odom_callback(self, msg):
self.pose.x = msg.pose.pose.position.x
self.pose.y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(_, _, self.pose.theta) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def next_pose(self):
try:
self.path_index += 1
self.goal = self.path[self.path_index].pose()
rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x,
self.goal.y)
except IndexError:
rospy.logwarn("REACHED END OF PATH!")
def run(self):
rate = rospy.Rate(10) # 10hz
speed = Twist()
while not rospy.is_shutdown():
# Calculate command
ang_speed = 0.4
inc_x = self.goal.x - self.pose.x
inc_y = self.goal.y - self.pose.y
angle_to_goal = atan2(inc_y, inc_x)
real_angle = self.pose.theta
# Normalize angle_diff
if angle_to_goal < 0:
angle_to_goal += 2 * pi
real_angle += 2 * pi
if real_angle < 0:
real_angle += 2 * pi
angle_to_goal += 2 * pi
angle_diff = angle_to_goal - real_angle
if (inc_x**2 + inc_y**2)**0.5 < 0.05:
speed.linear.x = 0.0
speed.angular.z = 0.0
self.next_pose()
elif abs(angle_diff) > 0.2: # increase tolerance?
speed.linear.x = 0.0
if angle_diff < 0:
speed.angular.z = -ang_speed
else:
speed.angular.z = +ang_speed
else:
speed.linear.x = 0.08
speed.angular.z = 0.0
self.cmd_vel_pub.publish(speed)
rate.sleep()
class ProSolver:
def __init__(self):
rospy.init_node('maze_pro_solver', anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param('~position')
startX, startY = positions['startX'], positions['startY']
targetX, targetY = positions['targetX'], positions['targetY']
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
self.path = [
Cell(r - self.path_finder.start.row,
c - self.path_finder.start.column) for r, c in raw_path
] # move rows to correct starting position
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5)
# Setup subscribers
odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def odom_callback(self, msg):
self.pose.x = msg.pose.pose.position.x
self.pose.y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(_, _, self.pose.theta) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def next_pose(self):
try:
self.path_index += 1
self.goal = self.path[self.path_index].pose()
except IndexError:
rospy.logwarn("REACHED END OF PATH!")
def run(self):
rate = rospy.Rate(10) # 10hz
speed = Twist()
while not rospy.is_shutdown():
# Calculate command
# Do other stuff
inc_x = self.goal.x - self.pose.x
inc_y = self.goal.y - self.pose.y
angle_to_goal = atan2(inc_y, inc_x)
if (inc_x**2 + inc_y**2)**0.5 < 0.05:
speed.linear.x = 0.0
speed.angular.z = 0.0
self.next_pose()
elif abs(angle_to_goal - self.pose.theta) > 0.02:
speed.linear.x = 0.0
if (self.pose.theta < angle_to_goal):
if (self.pose.theta < -0.2 and angle_to_goal > 0):
if (abs(self.pose.theta > (pi / 2))
and abs(angle_to_goal > (pi / 2))):
speed.angular.z = 0.3
else:
speed.angular.z = -0.3
else:
speed.angular.z = 0.3
elif (self.pose.theta > angle_to_goal):
if (angle_to_goal < -0.2 and self.pose.theta > 0):
if (abs(self.pose.theta > (pi / 2))
or abs(angle_to_goal > (pi / 2))):
speed.angular.z = 0.3
else:
speed.angular.z = -0.3
else:
speed.angular.z = -0.3
else:
speed.linear.x = 0.08
speed.angular.z = 0.0
self.cmd_vel_pub.publish(speed)
rate.sleep()
