def cast_temperature(self): rospy.init_node("Temperature", log_level=rospy.DEBUG) rate = rospy.Rate(2) vendor_of_A = rospy.get_param('/As_vendor') vendor_of_B = rospy.get_param('/Bs_vendor') vendor_of_C = rospy.get_param('/Cs_vendor') rospy.Subscriber("/"+vendor_of_A+"/A/pose_euclid", Pose, callback=self.callback_sensor_pose_A) rospy.Subscriber("/"+vendor_of_B+"/B/pose_euclid", Pose, callback=self.callback_sensor_pose_B) rospy.Subscriber("/"+vendor_of_C+"/C/pose_euclid", Pose, callback=self.callback_sensor_pose_C) pub_A = rospy.Publisher("A/temperature", data_class=Float32, queue_size=10) pub_B = rospy.Publisher("B/temperature", data_class=Float32, queue_size=10) pub_C = rospy.Publisher("C/temperature", data_class=Float32, queue_size=10) UDP_IP = os.environ.get("ROS_IP") simA = Simulator({'IP': UDP_IP, "port_send": 41001, "port_recv": 41101}) simB = Simulator({'IP': UDP_IP, "port_send": 42001, "port_recv": 42102}) while not rospy.is_shutdown(): xA, yA, zA = map(int, map(round, np.array(self._pose_A.position.__getstate__())[:])) xB, yB, zB = map(int, map(round, np.array(self._pose_B.position.__getstate__())[:])) xC, yC, zC = map(int, map(round, np.array(self._pose_C.position.__getstate__())[:])) # need to change this file to a generalized version of sensor reading, robot vendor and reading method if vendor_of_A=='solo': try: msg = str(self._serial.readline()) if msg.startswith("A_T:"): self._true_tmp_A = 1.8*float(msg[:-1].split(":")[1])+32. except Exception as e: rospy.logdebug("[A Temp Read Error]: {}".format(e.message)) elif vendor_of_A=='flightgear': self._true_tmp_A = 1.8 * float(simA.FGRecv()[38])+32. rospy.logdebug("Temp: A({},{},{}) {}".format(xA, yA, zA, self._true_tmp_A)) if self._dim == 3: pub_A.publish(self._true_tmp_A) pub_B.publish(self._true_tmp_B) pub_C.publish(self._true_tmp_C) if self._dim == 2: pub_A.publish(self._true_tmp_A) pub_B.publish(self._true_tmp_B) pub_C.publish(self._true_tmp_C) # rospy.logdebug("A_TMP[{}]@{}:{}".format(del_t.datetime.fromtimestamp(rospy.Time.now().to_time()).strftime( # "%H:%M:%S"), self._pose_A.position.__getstate__()[:], self._true_tmp_A)) rate.sleep()
def simple_goto(self): """ Continuously Listens to goal_gps topic and set controller to guide aircraft towards the goal """ try: s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) s.connect(("google.com", 80)) UDP_IP = s.getsockname()[0] s.close() except socket.error: rospy.logdebug("{}:Network connection unavailable...".format( self.tag)) exit(-1) sock_params = { 'IP': UDP_IP, "port_send": self._port_send, "port_recv": self._port_recv } sim = Simulator(sock_params) sensor = sensor_data() rospy.init_node(self._name) rate = rospy.Rate(80) rospy.Subscriber("/flightgear/{}/next_way_point_gps".format( self._name), data_class=geo_location, callback=self.callback_goal_gps) pub_sensor = rospy.Publisher('{}/sensor_data'.format(self._name), data_class=sensor_data, queue_size=1) i = 0 while not rospy.is_shutdown(): fg_data = sim.FGRecv() sensor.Pos_n = fg_data[0] sensor.Pos_e = fg_data[1] sensor.Pos_d = fg_data[2] sensor.V_n_ms = fg_data[3] sensor.V_e_ms = fg_data[4] sensor.V_d_ms = fg_data[5] sensor.u = fg_data[6] sensor.v = fg_data[7] sensor.w = fg_data[8] sensor.roll_deg = fg_data[9] sensor.pitch_deg = fg_data[10] sensor.yaw_deg = fg_data[11] sensor.p_body = fg_data[12] sensor.q_body = fg_data[13] sensor.r_body = fg_data[14] sensor.V_airspeed = fg_data[15] sensor.hour = fg_data[29] sensor.min = fg_data[30] sensor.sec = fg_data[31] sensor.Odometer = fg_data[32] sensor.CO2Density = fg_data[33] sensor.CO2alt_ft = fg_data[34] sensor.CO2lat_deg = fg_data[35] sensor.CO2lon_deg = fg_data[36] sensor.CO2heading_deg = fg_data[37] sensor.Temperature_degc = fg_data[38] sensor.Relative_humidity = fg_data[39] sensor.Pressure_inhg = fg_data[40] sensor.Dewpoint_degc = fg_data[41] sensor.wind_speed_kt = fg_data[42] sensor.wind_heading_deg = fg_data[43] self._sensor = sensor lat1 = float(sensor.Pos_n) lon1 = float(sensor.Pos_e) alt1 = float(sensor.Pos_d) heading = sensor.yaw_deg if np.isclose(self._goal.latitude, 36.1340362495, atol=1e-8) and np.isclose( self._goal.longitude, 97.0762336919, atol=1e-8): rospy.logerr("{} pid: goal {}".format(self._name, self._goal)) lat2 = self._goal.latitude lon2 = self._goal.longitude alt2 = self._goal.altitude ### Uclidian # GPS -> Distance based x1 = alt1 * cos(lat1) * sin(lon1) y1 = alt1 * sin(lat1) z1 = alt1 * cos(lat1) * cos(lon1) x2 = alt2 * cos(lat2) * sin(lon2) y2 = alt2 * sin(lat2) z2 = alt2 * cos(lat2) * cos(lon2) dist = sqrt((x2 - x1)**2 + (y2 - y1)**2 + (z2 - z1)**2) dlat = lat2 - lat1 dlon = lon2 - lon1 dalt = alt2 - alt1 Aaltitude = alt1 Oppsite = alt2 lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2]) dlon = lon2 - lon1 dlat = lat2 - lat1 a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2 # c = 2 * atan2(sqrt(a), sqrt(1-a)) c = 2. * asin(sqrt(a)) Base = 6371 * c + np.random.rand() * 0.1 Bearing = atan2( cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2 - lon1), sin(lon2 - lon1) * cos(lat2)) Bearing = degrees(Bearing) Bearing = (Bearing + 360) % 360 Bearing = (90 - Bearing + 360) % 360 Base2 = Base * 1000. distance = (Base * 2 + Oppsite * 2) / 2 Caltitude = Oppsite - Aaltitude a = Oppsite / Base b = atan(a) c = degrees(b) distance = distance / 1000. max_delta_lat, max_delta_lon = 0.00144194768199 / 10, 0.00177704227973 / 10 max_aileron = 0.6 max_elevator = 0.2 max_throttle = 0.005 kp_alerion = 0.006 / 360. # kp_alerion = 5.0/180. kp_throttle = 0.0 kp_elevator = 0.001 kd_alerion = 0.006 kd_throttle = 0.0 kd_elevator = 0.001 throttle = max_throttle der_alt = dalt - self._previous_error_alt output = kp_elevator * abs(dalt) + kd_elevator * der_alt if dalt > 0: elevator = max(-max_elevator, -output) if dalt < 0: elevator = min(max_elevator, output) # print "i={:05} alt={:3.2f} g={:3.2f} dalt={:3.2f} prvE={:3.2f} der={:1.2f} kp*dalt={:1.2f} kd*dalt={:1.2f} elv={:1.2f}".format( # i, alt1, alt2, dalt, self._previous_error_alt, der_alt, kp_elevator * dalt, kd_elevator * der_alt, output, elevator) self._previous_error_alt = dalt err_theta = ((Bearing - heading + 360) % 360) if err_theta < 180: der_theta = err_theta - self._previous_error_deg turn = kp_alerion * err_theta + kd_alerion * der_theta aileron = min(turn, max_aileron) if err_theta > 180: der_theta = (360. - err_theta) - self._previous_error_deg turn = kp_alerion * (360 - err_theta) + kd_alerion * der_theta aileron = max(-turn, -max_aileron) rospy.logdebug( "{} i={:05} yw={:3.2f} g={:3.2f} err={:3.2f} prv={:3.2f} der_theta={:1.2f} kp*err={:1.2f} kd*der={:1.2f} turn={:1.2f} ail={:1.2f}" .format(self.tag, i, heading, Bearing, err_theta, self._previous_error_deg, der_theta, kp_alerion * err_theta, kd_alerion * der_theta, turn, aileron)) commands = { "throttle": throttle, "elevator": elevator, "aileron": aileron, "rudder": 0.0 } sim.FGSend(commands, for_model="ufo") pub_sensor.publish(sensor) rate.sleep()
class flightgear_quad(object): def __init__(self, name, instance, server_id, server_ip, port_send, port_recv, dim=3, scale=16): """ :param port: port number of solo controller to connect :param altitude: in meters """ self._name = name self._instance = instance self._port_send = port_send self._port_recv = port_recv self._server_id = server_id self._server_ip = server_ip self._tag = "[fgqd_{}]".format(instance) self._goal_gps = Pose(Point(0., 0., 0.), Quaternion(*quaternion_from_euler(0., 0., 0.))) self._goal_euclid = Pose() self._dim = int(rospy.get_param("/dim")) self._scale = int(rospy.get_param("/scale")) self._space = tuple([self._scale for _ in range(self._dim)]) # cowboy cricket ground bowling end 36.133642, -97.076528 self._origin_lat = 36.169097 #36.1333333 self._origin_lon = -97.088101 #-97.0771 self._origin_alt = 5. # meter self._meters_per_alt = 4. self._meters_per_disposition = 4. self._meters_per_lat = 110961.03 # meters per degree of latitude for use near Stillwater self._meters_per_lon = 90037.25 # meters per degree of longitude self._tol_meter = .05 # drone to be considered reached a goal if it is withing tol_meter within the goal self._tol_lat = 1.e-6 self._tol_lon = 1.e-6 self._tol_alt = 0.5 self._max_lon = self._origin_lon + (self._meters_per_disposition * self._scale) / self._meters_per_lon self._max_lat = self._origin_lat + (self._meters_per_disposition * self._scale) / self._meters_per_lat self._max_alt = self._origin_alt + (self._meters_per_disposition * self._scale) self._pub_pose_gps = None self._pub_pose_euclid = None self._pub_distance_to_goal = None self._is_ready = False try: s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) s.connect(("google.com", 80)) UDP_IP = s.getsockname()[0] s.close() except socket.error: rospy.logdebug("{}[{}]:Network connection unavailable...".format(self._name,self.time_tag)) exit(-1) self._sim = Simulator({'IP': UDP_IP, "port_send": self._port_send, "port_recv": self._port_recv}) @property def time_tag(self): return dt.datetime.fromtimestamp(rospy.Time.now().to_time()).strftime("%H:%M:%S") @property def gps_loc(self): """ :rtype: Pose """ fg_data = self._sim.FGRecv() lat = float(fg_data[0]) lon = float(fg_data[1]) alt = 0.3048 * float(fg_data[2]) # meter pose = Pose() pose.position.y = lat pose.position.x = lon pose.position.z = alt return pose def arm_and_takeoff(self, start_at_euclid=None): """ Init ROS node. Arms vehicle and fly_grad to aTargetAltitude (in meters). """ rospy.init_node(self._name, log_level=rospy.DEBUG) rospy.logdebug("{}[{}] init node max (lon, lat, alt)=({},{},{})".format( self._tag, self.time_tag, self._max_lon, self._max_lat, self._max_alt)) rate = rospy.Rate(1) rospy.logdebug("{}:server_ip={} port_send={} port_recv={}".format(self._name, self._server_ip, self._port_send, self._port_recv)) # longitude EW = x axis and latitude NS = y axis # send solo to initial location FGthread( server_id = self._server_id, instance=self._instance, controller_hostIP=self._server_ip, freq_in=100, freq_out=100, vehicle='quad', lat=self._origin_lat, lon=self._origin_lon, alt=self._origin_alt, iheading=45, ivel=60, ithrottle=0) # 0.1 -> throttle pub_ready = rospy.Publisher('{}/ready'.format(self._name), data_class=Bool, queue_size=1) self._goal_euclid = Pose(Point(start_at_euclid[0], start_at_euclid[1], start_at_euclid[2]), Quaternion( *quaternion_from_euler(0., 0., 0.))) self._goal_gps = self.euclid_to_geo(NS=self._goal_euclid.position.y, EW=self._goal_euclid.position.x, UD=self._goal_euclid.position.z) rospy.logdebug("{}[{}]Sending to initial goal (x,y,z)=({}) (lon, lat, alt)=({},{},{}) tol=({},{},{})".format( self._tag, self.time_tag, start_at_euclid, self._goal_gps.position.y, self._goal_gps.position.x, self._goal_gps.position.z, self._tol_lon, self._tol_lat, self._tol_alt) ) rospy.sleep(3) pub_ready.publish(True) rospy.Subscriber("/UAV/{}/next_way_point_euclid".format(self._name), data_class=Pose, callback=self.callback_next_euclidean_way_point) self._pub_pose_gps = rospy.Publisher(self._name + '/pose_gps', data_class=Pose, queue_size=10) self._pub_pose_euclid = rospy.Publisher(self._name + '/pose_euclid', data_class=Pose, queue_size=10) pub_fly = rospy.Publisher("{}/fly_grad".format(self._name), data_class=String, queue_size=10) pub_fly.publish("fly_grad") while not rospy.is_shutdown(): pose_gps = self.gps_loc self._pub_pose_gps.publish(pose_gps) self._pub_pose_euclid.publish(self.pose_in_euclid()) if self._goal_gps is not None: simple_goto(lat=self._goal_gps.position.y, lon=self._goal_gps.position.x, alt=self._goal_gps.position.z, callasign=self._name, sim=self._sim) reached_lon = np.isclose(self._goal_gps.position.x, self.gps_loc.position.x, atol=self._tol_lon) reached_lat = np.isclose(self._goal_gps.position.y, self.gps_loc.position.y, atol=self._tol_lat) reached_alt = np.isclose(self._goal_gps.position.z, self.gps_loc.position.z, atol=self._tol_alt) dif_lon = self.gps_loc.position.x - self._goal_gps.position.x dif_lat = self.gps_loc.position.y - self._goal_gps.position.y dif_alt = self.gps_loc.position.z - self._goal_gps.position.z if reached_lat and reached_lon and reached_alt: pos_eu = self.pose_in_euclid() rospy.logdebug("{}[{}]Reached goal @(lon,lat,alt)=({},{},{}) goal({},{},{}) dif=({},{},{}) @(x,y,z)=({},{},{}) " "goal_eu=({},{},{})".format(self._tag, self.time_tag, self.gps_loc.position.x, self.gps_loc.position.y, self.gps_loc.position.z, self._goal_gps.position.x, self._goal_gps.position.y, self._goal_gps.position.z, dif_lon, dif_lat, dif_alt, pos_eu.position.x, pos_eu.position.y, pos_eu.position.z, self._goal_euclid.position.x, self._goal_euclid.position.y, self._goal_euclid.position.z)) self._goal_gps = None self._goal_euclid = None else: pos_eu = self.pose_in_euclid() rospy.logdebug("{}[{}]@(lon,lat,alt)=({},{},{}) goal({},{},{}) dif=({},{},{}) @(x,y,z)=({},{},{}) goal_eu=({},{},{})".format( self._tag, dt.datetime.fromtimestamp(rospy.Time.now().to_time()).strftime("%H:%M:%S"), self.gps_loc.position.x, self.gps_loc.position.y, self.gps_loc.position.z, self._goal_gps.position.x, self._goal_gps.position.y, self._goal_gps.position.z, dif_lon, dif_lat, dif_alt, pos_eu.position.x, pos_eu.position.y, pos_eu.position.z,self._goal_euclid.position.x, self._goal_euclid.position.y, self._goal_euclid.position.z)) pub_fly.publish("wait") else: rospy.logdebug("{}[{}]Waiting for new goal".format(self._tag, dt.datetime.fromtimestamp( rospy.Time.now().to_time()).strftime("%H:%M:%S"))) pub_fly.publish("fly_grad") rate.sleep() def euclid_to_geo(self, NS, EW, UD): """ Converts euclid NED coordinate and converts it to gps latitude and longitude. displacement in meters (N and E are positive, S and W are negative), and outputs the new lat/long CAUTION: the numbers below are set for use near Stillwater will change at other lattitudes :param NS: set as y axis of euclidean coordinate lat :param EW: set as x axis of euclidean coordinate lon :param UD: set as z axis of eculidean coordinate alt :rtype: Pose """ pose = Pose() lon = self._origin_lon + self._meters_per_disposition * EW / self._meters_per_lon lat = self._origin_lat + self._meters_per_disposition * NS / self._meters_per_lat alt = self._origin_alt + self._meters_per_alt * UD pose.position.x = lon pose.position.y = lat pose.position.z = alt return pose def pose_in_euclid(self): """ Converts euclid NED coordinate and converts it to gps latitude and longitude. displacement in meters (N and E are positive, S and W are negative), and outputs the new lat/long CAUTION: the numbers below are set for use near Stillwater will change at other lattitudes :param lon: set as y axis of euclidean coordinate lon :param lat: set as x axis of euclidean coordinate lat :return: Pose in euclid :rtype: Pose """ pose = Pose() lon = self.gps_loc.position.x lat = self.gps_loc.position.y alt = self.gps_loc.position.z pose.position.x = ((lon - self._origin_lon)/(self._max_lon - self._origin_lon)) * float(self._scale) pose.position.y = ((lat - self._origin_lat)/(self._max_lat - self._origin_lat)) * float(self._scale) pose.position.z = ((alt - self._origin_alt)/(self._max_alt - self._origin_alt)) * float(self._scale) if (lat < self._origin_lat or lon < self._origin_lon or alt >= self._max_alt or lon >= self._max_lon or lat >= self._max_lat) and self._is_ready: rospy.logdebug("{} Loiter because went out of boundary!!! psoe={} (lon,lat,alt)=({},{},{})".format( self._tag, pose.position.__getstate__(),self.gps_loc.position.x, self.gps_loc.position.y, self.gps_loc.position.z)) rospy.signal_shutdown("{} Went out of boundary".format(self._tag)) return pose def callback_next_euclidean_way_point(self, goal_euclid): """ :param goal_euclid: goal in euclidian coordinate :type goal_euclid: Pose :return: """ if goal_euclid is not None: self._goal_euclid = goal_euclid # longitude EW = x axis and latitude NS = y axis, E is +x, N is +y self._goal_gps = self.euclid_to_geo(NS=goal_euclid.position.y, EW=goal_euclid.position.x, UD=goal_euclid.position.z) # rospy.logdebug("{}[{}]New Goal (x,y,z)=({},{},{}) (lat,long,alt)=({},{},{})".format( # self._tag, dt.datetime.fromtimestamp(rospy.Time.now().to_time()).strftime("%H:%M:%S"), # self._goal_euclid.position.x, self._goal_euclid.position.y, self._goal_euclid.position.z, # self._goal_gps.position.y, self._goal_gps.position.x, self._goal_gps.position.z) # ) else: rospy.logdebug("{} No goal waypoint received yet.".format(self._tag))