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