class Controller:
stop = True # This is set to true when the stop button is pressed
def __init__(self):
self.cmdVelPub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.trackposSub = rospy.Subscriber("tracked_pos", Pose2D,
self.trackposCallback)
self.world = World(785, 575)
def trackposCallback(self, msg):
# This function is continuously called
if not self.stop:
twist = Twist()
# Change twist.linear.x to be your desired x velocity
velocity = self.world.getVelocity(msg)
twist.linear.x = velocity.x
# Change twist.linear.y to be your desired y velocity
twist.linear.y = velocity.y
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
self.cmdVelPub.publish(twist)
def start(self):
rospy.wait_for_service("apriltags_info")
try:
info_query = rospy.ServiceProxy("apriltags_info", apriltags_info)
resp = info_query()
for i in range(len(resp.polygons)):
# A polygon (access points using poly.points)
poly = resp.polygons[i]
# The polygon's id (just an integer, 0 is goal, all else is bad)
t_id = resp.ids[i]
polyPoints = []
for point in poly.points:
polyPoints.append(Point(point.x, point.y))
self.world.addTag(PolyField(t_id, polyPoints))
self.world.createFields()
PathFinder(self.world)
except Exception, e:
print "Exception: " + str(e)
finally:
class Controller:
stop = True # This is set to true when the stop button is pressed
def __init__(self):
self.cmdVelPub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.trackposSub = rospy.Subscriber("tracked_pos", Pose2D, self.trackposCallback)
self.world = World(785, 575)
def trackposCallback(self, msg):
# This function is continuously called
if not self.stop:
twist = Twist()
# Change twist.linear.x to be your desired x velocity
velocity = self.world.getVelocity(msg)
twist.linear.x = velocity.x
# Change twist.linear.y to be your desired y velocity
twist.linear.y = velocity.y
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
self.cmdVelPub.publish(twist)
def start(self):
rospy.wait_for_service("apriltags_info")
try:
info_query = rospy.ServiceProxy("apriltags_info", apriltags_info)
resp = info_query()
for i in range(len(resp.polygons)):
# A polygon (access points using poly.points)
poly = resp.polygons[i]
# The polygon's id (just an integer, 0 is goal, all else is bad)
t_id = resp.ids[i]
polyPoints = []
for point in poly.points:
polyPoints.append(Point(point.x, point.y))
self.world.addTag(PolyField(t_id, polyPoints))
self.world.createFields()
PathFinder(self.world)
except Exception, e:
print "Exception: " + str(e)
finally:
