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))