def teleport_cb(self, req):
print req
self.gps_coord = MapCoords(req.coordinates.latitude + 0.000001,
req.coordinates.longitude + 0.000001)
self.precord = self.gps_coord
rospy.sleep(2.0)
return True
def __init__(self, lat, lon, northang):
self.gps_coord = MapCoords(lat, lon)
self.northang = northang
self.cmd = Twist()
self.two_pi = math.pi * 2.0
self.timer = rospy.Timer(rospy.Duration(0.5), self.time_callback)
rospy.Subscriber("/cmd_vel", Twist, self.callback)
self.pub = rospy.Publisher("/fix", NavSatFix)
rospy.Service('/teleport_robot', TeleportRobot, self.teleport_cb)
rospy.spin()
self.timer.shutdown()
def executeCallback(self, goal):
rospy.loginfo("Navigating...")
print goal
goal_coord = MapCoords(goal.coords.latitude, goal.coords.longitude)
print "RESULT"
#print self.gps_coord - goal_coord
if not self.teleport:
result = self.navigate(goal_coord)
if not self.cancelled:
print result
self._result.success = result
self._as.set_succeeded(self._result)
else:
rospy.wait_for_service('/teleport_robot')
try:
print
teleport = rospy.ServiceProxy('/teleport_robot', TeleportRobot)
result = teleport(goal.coords)
print result
rospy.sleep(0.5)
self._result.success = True #result.success
self._as.set_succeeded(self._result)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
result = False
self._result.success = result
self._as.set_succeeded(self._result)
def gps_callback(self, data):
if not np.isnan(data.latitude):
self.gps_coord = MapCoords(data.latitude, data.longitude)
if not self.first_fix:
ang = self.last_coord - self.gps_coord
self.ang = math.radians(ang[1])
self.first_fix = False
self.last_coord = self.gps_coord
def gps_callback(self, data):
if not np.isnan(data.latitude):
gps_coord = MapCoords(data.latitude, data.longitude)
self.gps_canvas.draw_coordinate(gps_coord,
'red',
size=2,
thickness=1,
alpha=255)
def gps_callback(self, data):
if not np.isnan(data.latitude):
self.gps_canvas.clear_image()
gps_coord = MapCoords(data.latitude, data.longitude)
self.gps_canvas.draw_coordinate(gps_coord,
'black',
size=2,
thickness=2,
alpha=255)
if self.last_coord:
dist = gps_coord - self.last_coord
self.explodist += dist[0]
self.last_coord = gps_coord
class SImPos(object):
def __init__(self, lat, lon, northang):
self.gps_coord = MapCoords(lat, lon)
self.northang = northang
self.cmd = Twist()
self.two_pi = math.pi * 2.0
self.timer = rospy.Timer(rospy.Duration(0.5), self.time_callback)
rospy.Subscriber("/cmd_vel", Twist, self.callback)
self.pub = rospy.Publisher("/fix", NavSatFix)
rospy.Service('/teleport_robot', TeleportRobot, self.teleport_cb)
rospy.spin()
self.timer.shutdown()
def teleport_cb(self, req):
print req
self.gps_coord = MapCoords(req.coordinates.latitude + 0.000001,
req.coordinates.longitude + 0.000001)
self.precord = self.gps_coord
rospy.sleep(2.0)
return True
def callback(self, data):
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
self.cmd = data
def time_callback(self, event):
self.precord = self.gps_coord
self.northang = self.northang + (self.cmd.angular.z)
if self.northang > self.two_pi:
self.northang = self.northang - self.two_pi
if self.northang < -self.two_pi:
self.northang = self.northang + self.two_pi
northing_dif = (self.cmd.linear.x * 0.5) * math.cos(self.northang)
easting_dif = (self.cmd.linear.x * 0.5) * math.sin(self.northang)
if northing_dif != 0.0 or easting_dif != 0:
self.gps_coord = self.gps_coord._get_rel_point(
easting_dif, northing_dif)
#print self.cmd.linear.x, self.cmd.angular.z
#print self.northang, northing_dif, easting_dif #self.gps_coord.northing, self.gps_coord.easting
# print self.gps_coord
fix = NavSatFix()
fix.latitude = self.gps_coord.lat
fix.longitude = self.gps_coord.lon
self.pub.publish(fix)
def executeCallback(self, goal):
rospy.loginfo("Navigating...")
print goal
goal_coord = MapCoords(goal.coords.latitude, goal.coords.longitude)
print "RESULT"
print self.gps_coord - goal_coord
result = self.navigate(goal_coord)
if not self.cancelled:
print result
self._result.success = result
self._as.set_succeeded(self._result)
def godom_callback(self, data):
d = utm.to_latlon(data.pose.pose.position.x,
data.pose.pose.position.y,
self.centre.zone_number,
zone_letter=self.centre.zone_letter)
odom_coord = MapCoords(d[0], d[1])
diff = odom_coord - self.preodom
self.draw_compass(diff)
self.preodom = odom_coord
self.gps_canvas.draw_coordinate(odom_coord,
'yellow',
size=2,
thickness=1,
alpha=255)
def draw_coordinate(self,
lat,
lon,
colour='white',
size=6,
thickness=2,
alpha=128):
a = MapCoords(lat, lon)
# b = [255*x for x in mcolors.hex2color(mcolors.cnames[colour])]
# b = b[::-1]
# b.append(alpha)
self.canvas.draw_coordinate(a, colour, size=size, thickness=thickness)
self.refresh_image()
# def _draw_coordinate(self, coord, colour, size=6, thickness=2):
# mx, my =self._coord2pix(coord)
# cv2.circle(self.base_image, (int(mx), int(my)), size, colour, thickness)
def scan_callback(self, msg):
if msg.data == 'Do_reading':
print "generating reading"
gps_coord = MapCoords(self.last_coord.latitude,self.last_coord.longitude)
cx, cy = self.grid.get_cell_inds_from_coords(gps_coord)
hmm = KrigInfo()
hmm.header = std_msgs.msg.Header()
hmm.header.stamp = rospy.Time.now()
hmm.coordinates = self.last_coord
# hmm.coordinates.longitude = click_coord.lon
for i in self.grid.models:
mmh = KrigMsg()
mmh.model_name = i.name
mmh.measurement = i.output[cy][cx]
hmm.data.append(mmh)
self.data_pub.publish(hmm)
print hmm
