def talker():
print 'in talker'
pub = rospy.Publisher('GPS', NavSatFix)
rospy.init_node('GPStalker')
while not rospy.is_shutdown():
#Assuming that parse will return these values
time.sleep(1)
parse()
msg = NavSatFix()
Fix = NavSatStatus()
Fix.status = GPS.mode
Fix.service = GPS.numSat
msg.header.stamp = rospy.Time.now()
msg.status = Fix
msg.latitude = GPS.lat
msg.longitude = GPS.lon
msg.altitude = GPS.alt
msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
msg.position_covariance_type = 0
#covariance_type = 0 unknown
# = 1 approximated
# = 2 diagonal known
# = 3 known
pub.publish(msg)
def got_position(xx, yy, aa, stamp):
global EP
fix = NavSatFix()
fix.header.stamp = stamp
fix.header.frame_id = "sim_gps"
fix.status.status = 0
fix.status.service = 1
utm_e = base_xx + base_utm_e + xx + CONFIG["SIM_ERROR"] * cos((yy + time()) / EP)
utm_n = base_yy + base_utm_n + yy + CONFIG["SIM_ERROR"] * cos((utm_e / 3.14) / EP)
(lon, lat) = conv(utm_e, utm_n, inverse=True)
fix.latitude = lat
fix.longitude = lon
fix.altitude = 1.3
# print "got_position: %f %f" % (lat, lon)
gps.publish(fix)
pos.publish(Pose2D(utm_e, utm_n, aa))
tfBr.sendTransform(
(utm_e - base_xx, utm_n - base_yy, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/map",
"/gps",
)
def talker():
UDP_IP = "192.168.0.107"
UDP_PORT = 9002
sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10)
rospy.init_node('test_relay', anonymous=True)
rate = rospy.Rate(100) # 10hz
while not rospy.is_shutdown():
data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
print "received message:", data
data = re.sub("[A-Za-z]","",str(data))
result = str(data).split(":")
print result
rospy.loginfo(result)
msg = NavSatFix()
msg.header = Header()
msg.latitude = float(result[1])
msg.longitude = float(result[2])
#rospy.loginfo(msg)
pub.publish(msg)
def joy_callback(msg):
if msg.buttons[12] == 1:
gps_goal = NavSatFix()
gps_goal.latitude = 33.726526
gps_goal.longitude = -83.299984
gps_put.publish(gps_goal)
if msg.buttons[14] == 1:
client.cancel_all_goals()
def publish_gps(self):
if self.drone.positionGPS!=None:
gps_data = NavSatFix()
gps_data.latitude = self.drone.positionGPS[0]
gps_data.longitude = self.drone.positionGPS[1]#maybe reversed?
gps_data.altitude = self.drone.positionGPS[2]
gps_data.header.frame_id = self.unique_id
self.gps_pub.publish(gps_data)
def getMsg():
pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix)
pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped)
#Initialize the RTKLIB ROS node
rospy.init_node('rtklib_messages', anonymous=True)
#Define the publishing frequency of the node
rate = rospy.Rate(10)
#Create a socket
sock = socket.socket()
#Get the address of the local host
host = socket.gethostname()
#Connect to the RTKRCV server that is bound to port xxxx
port = 5801
sock.connect((host,port))
e2 = 6.69437999014e-3
a = 6378137.0
while not rospy.is_shutdown():
navsat = NavSatFix()
ecef_xyz = Point()
ecef_pose = Pose()
ecef_stampedPose = PoseStamped()
ecef_stampedPose =
navsat.header.stamp = rospy.Time.now()
#Get the position message from the RTKRCV server
msgStr = sock.recv(1024)
#Split the message
msg = msgStr.split()
navsat.latitude = float(msg[2])
navsat.longitude = float(msg[3])
navsat.altitude = float(msg[4])
N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2))
ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0)
ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0)
ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0)
ecef_pose.position = ecef_xyz
ecef_stampedPose.pose = ecef_pose
pub_navsat.publish(navsat)
pub_pose.publish(ecef_stampedPose)
rate.sleep()
def default(self, ci='unused'):
gps = NavSatFix()
gps.header = self.get_ros_header()
gps.latitude = self.data['x']
gps.longitude = self.data['y']
gps.altitude = self.data['z']
self.publish(gps)
def recieve_gps(self, data ): msg = NavSatFix() msg.latitude = data.vector.x msg.longitude = data.vector.y msg.altitude = data.vector.z msg.header.stamp = data.header.stamp msg.header.frame_id = data.header.frame_id self.publisher['gps'].publish(msg)
def default(self, ci='unused'):
""" Publish the data of the gps sensor as a custom ROS NavSatFix message """
gps = NavSatFix()
gps.header = self.get_ros_header()
# TODO ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html
gps.latitude = self.data['x']
gps.longitude = self.data['y']
gps.altitude = self.data['z']
self.publish(pose)
def address2geo(address):
gurl = 'https://maps.googleapis.com/maps/api/geocode/xml?address='+address
res = urllib.urlopen(gurl).read()
ans = NavSatFix()
i1 = res.find('lat')
i2 = res.find('/lat',i1)
ans.latitude = float(res[i1+4:i2-1])
i1 = res.find('lng')
i2 = res.find('/lng',i1)
ans.longitude = float(res[i1+4:i2-1])
return ans
def gps_fix_sim():
pub=rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10)
rospy.init_node('gps_sim', anonymous=True)
rate = rospy.Rate(2)
while not rospy.is_shutdown():
gps_fix = NavSatFix()
gps_fix.latitude = 36.6
gps_fix.longitude = -121.1
gps_fix.altitude = 1.0
pub.publish(gps_fix)
rate.sleep()
def pub_gps(msg_type, msg, bridge):
pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1)
fix = NavSatFix()
fix.header.stamp = bridge.project_ap_time(msg)
fix.header.frame_id = 'base_footprint'
fix.latitude = msg.lat/1e07
fix.longitude = msg.lon/1e07
# NOTE: absolute (MSL) altitude
fix.altitude = msg.alt/1e03
fix.status.status = NavSatStatus.STATUS_FIX
fix.status.service = NavSatStatus.SERVICE_GPS
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
pub.publish(fix)
def bestpos_handler(self, bestpos):
navsat = NavSatFix()
# TODO: The timestamp here should come from SPAN, not the ROS system time.
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
# Assume GPS - this isn't exposed
navsat.status.service = NavSatStatus.SERVICE_GPS
position_type_to_status = {
BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX,
BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX,
}
navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX)
# Position in degrees.
navsat.latitude = bestpos.latitude
navsat.longitude = bestpos.longitude
# Altitude in metres.
navsat.altitude = bestpos.altitude
navsat.position_covariance[0] = pow(2, bestpos.latitude_std)
navsat.position_covariance[4] = pow(2, bestpos.longitude_std)
navsat.position_covariance[8] = pow(2, bestpos.altitude_std)
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# Ship ito
self.pub_navsatfix.publish(navsat)
def convert_gps_to_pose(self, coords):
# create NavSatFix msg
nav_msg = NavSatFix()
nav_msg.header.stamp = rospy.get_rostime()
nav_msg.header.frame_id = 'odom'
# x is longitude, y is latitude
nav_msg.longitude = coords['x']
nav_msg.latitude = coords['y']
# convert using /latlong_goal_node/AddLatLongGoal service
# returns PoseStamped
response = self.latlong_service(nav_msg)
rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x,
response.goal_xy.pose.position.y))
return response.goal_xy
def calc_lat_long(init_pos,distance,brng):
'''
* calculate the latitude and longitude given distance and bearing
* distnace input in [m] brng in [rad]
'''
R = 6371.0 * 1000
p2 = NavSatFix()
lat1 = init_pos.latitude*PI/180.0
lon1 = init_pos.longitude*PI/180.0
p2.latitude = (asin( sin(lat1)*cos(distance/R) + cos(lat1)*sin(distance/R)*cos(brng)))
p2.longitude = (lon1 + atan2(sin(brng)*sin(distance/R)*cos(lat1),cos(distance/R)-sin(lat1)*sin(p2.latitude)))*180 / PI
p2.latitude = p2.latitude *180 / PI
return p2
def testAutoOriginFromNavSatFix(self):
rospy.init_node('test_initialize_origin')
nsf_pub = rospy.Publisher('fix', NavSatFix, queue_size=2)
origin_sub = self.subscribeToOrigin()
self.test_stamp = True
nsf_msg = NavSatFix()
nsf_msg.latitude = swri['latitude']
nsf_msg.longitude = swri['longitude']
nsf_msg.altitude = swri['altitude']
nsf_msg.header.frame_id = "/far_field"
nsf_msg.header.stamp = msg_stamp
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
nsf_pub.publish(nsf_msg)
r.sleep()
def test_telemetry_serializer(self):
"""Tests telemetry serializer."""
# Set up test data.
navsat = NavSatFix()
navsat.latitude = 38.149
navsat.longitude = -76.432
navsat.altitude = 30.48
compass = Float64(90.0)
json = serializers.TelemetrySerializer.from_msg(navsat, compass)
altitude_msl = serializers.meters_to_feet(navsat.altitude)
# Compare.
self.assertEqual(json["latitude"], navsat.latitude)
self.assertEqual(json["longitude"], navsat.longitude)
self.assertEqual(json["altitude_msl"], altitude_msl)
self.assertEqual(json["uas_heading"], compass.data)
def get_directions_service(req):
p = xy2geo(init_pos,req.start)
orig = "%f,%f"%(p.latitude,p.longitude)
p = xy2geo(init_pos,req.goal)
dest = "%f,%f"%(p.latitude,p.longitude)
print "From ", orig
print "To ", dest
print "--------"
direction_list,status = get_directions(orig,dest)
#print status
res = Path()
for wp in direction_list:
point = NavSatFix()
point.latitude = float(wp.start_location[0])
point.longitude = float(wp.start_location[1])
pos = geo2xy(init_pos,point)
res.poses.append(pos)
return res
def convert_to_pose(self, x, y):
if not self.latlong_service:
rospy.loginfo("Waiting for AddLatLongGoal service...")
rospy.wait_for_service('/latlong_goal_node/AddLatlongGoal')
rospy.loginfo("AddLatLongGoal service responded")
self.latlong_service = rospy.ServiceProxy('/latlong_goal_node/AddLatlongGoal', AddLatlongGoal)
# create NavSatFix msg
nav_msg = NavSatFix()
nav_msg.header.stamp = rospy.get_rostime()
nav_msg.header.frame_id = 'odom'
# x is longitude, y is latitude
nav_msg.longitude = float(x)
nav_msg.latitude = float(y)
# convert using /latlong_goal_node/AddLatLongGoal service
# returns PoseStamped
response = self.latlong_service(nav_msg)
rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x,
response.goal_xy.pose.position.y))
return response.goal_xy.pose.position.x, response.goal_xy.pose.position.y
def parse_gps(self, data):
'''
Given raw data from Jaguar GPS socket, parse and return NavSatFix message.
If bad/incomplete data, return None
'''
# use regex to parse
gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data)
gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data)
if gprmc_hit:
# Get estimated heading (not part of standard ROS navsatfix message)
gprmc = gprmc_hit.group(0).split(",")
try:
heading = float(gprmc[8])
except:
heading = float("NaN")
else:
while heading < -180.0:
heading += 360.0
while heading > 180.0:
heading -= 360.0
finally:
self.current_est_heading = heading
if gpgga_hit:
gpgga = gpgga_hit.group(0).split(",")
nav_msg = NavSatFix()
# Set header information
time = gpgga[1]
hrs = float(time[0:1])
mins = float(time[2:3])
secs = float(time[4:5])
nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs)
nav_msg.header.frame_id = "gps"
# Set GPS Status
status = NavSatStatus()
status.status = -1 if int(gpgga[6]) == 0 else 0
nav_msg.status = status
# Set longitude and latitude
lat_str = gpgga[2]
lon_str = gpgga[4]
lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0)
lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0)
nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs
nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs
# Set altitude (Positive is above the WGS 84 ellipsoid)
try:
nav_msg.altitude = float(gpgga[9])
except:
nav_msg.altitude = float("NaN")
# Set covariance type to unknown
nav_msg.position_covariance_type = 0
return nav_msg
else:
return None
def test_telemetry_serializer(self):
"""Tests telemetry serializer."""
# Set up test data.
navsat = NavSatFix()
navsat.latitude = 38.149
navsat.longitude = -76.432
altitude = Altitude()
altitude.amsl = 30.48
pose_stamped = PoseStamped()
pose_stamped.pose.orientation.w = 1.0
data = serializers.TelemetrySerializer.from_msg(navsat, altitude,
pose_stamped)
altitude_msl = serializers.meters_to_feet(altitude.amsl)
# Compare.
self.assertEqual(data["latitude"], navsat.latitude)
self.assertEqual(data["longitude"], navsat.longitude)
self.assertEqual(data["altitude_msl"], altitude_msl)
self.assertEqual(data["uas_heading"], 90.0)
def receiveGpsOverUdp():
pub = rospy.Publisher('/bozobox/gps/fix', NavSatFix)
rospy.init_node('gps_over_udp_receiver')
while not rospy.is_shutdown():
data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
#print "received message: >"+data+'<'
dataArray = data.split(',');
fix = NavSatFix()
fix.header.stamp.secs = int(dataArray[0])
fix.header.stamp.nsecs = int(dataArray[1])
fix.latitude = float(dataArray[2])
fix.longitude = float(dataArray[3])
fix.status.status = int(dataArray[4])
print fix.latitude
print fix.longitude
print fix.status.status
pub.publish(fix)
def callback(self): #Publish data at 1 Hz rate = rospy.Rate(1) #Define the RTKRCV server ports corresponding to each of the processing #Modes port = 5801 sock = socket.socket() host = socket.gethostname() sock.connect((host, port)) #WGS84 Parameters e2 = 6.69437999014e-3 a = 6378137.0 #At a rate of 1 hz, create an rtklib message for each socket and publish #the data over while not rospy.is_shutdown(): #Create and RTKLIB and a NavSatFix data structure rtk = rtklib() navsat = NavSatFix() #Set the data structure header time to the current system time navsat.header.stamp = rospy.Time.now() rtk.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = socket.recv(1024) #Split the message msg = msgStr.split() #Save the latitude, longitude and ellipsoid height navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) #Save the position covariance navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN #Compute the radius of curvature in the N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) #Compute and store the ECEF position rtk.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) rtk.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) rtk.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) rtk.delay = float(msg[13]) rtk.ftest = float(msg[14]) #Store the NavSatFix data rtk.state = navsat pubs[i].publish(rtk) rate.sleep()
def callback(sensorstr):
data = sensorstr.data
if data[0] == 'Z':
data = data.replace("Z","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 4:
try:
##data_list structure: lat, lon, heading, vel
float_list = [float(i) for i in data_list]
poseGPS = NavSatFix()
poseGPS.header.frame_id = "world"
poseGPS.header.stamp = rospy.Time.now()
poseGPS.status.status = 0
poseGPS.status.service = 1
poseGPS.latitude = float_list[0]
poseGPS.longitude = float_list[1]
poseGPS.altitude = 0.0
poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0]
poseGPS.position_covariance_type = 2
try:
pubGPS.publish(poseGPS)
log = "GPS Pose Data: %f %f Publish at Time: %s" % (float_list[0], float_list[1], rospy.get_time())
except:
log = "poseGPS Publisher Error"
except:
log = "GPS Data Error! Data : %s" % data
rospy.loginfo(log)
rospy.loginfo(log)
def ExtFix2Fix(data):
fix = NavSatFix()
fix.header = data.header
fix.status.status = data.status.status
fix.status.service = data.status.position_source
fix.latitude = data.latitude
fix.longitude = data.longitude
fix.altitude = data.altitude
fix.position_covariance = data.position_covariance
fix.position_covariance_type = data.position_covariance_type
return fix
def talker():
pub = rospy.Publisher("/gps/fix", NavSatFix, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
#gps config
gpsd.next()
os.system('clear')
gps_data = NavSatFix()
#status of gps
# gps_data.status.status = 0
# gps_data.status.service = 1
#time
gps_data.header.stamp.secs = round(time.time())
#position
gps_data.latitude = gpsd.fix.latitude
gps_data.longitude = gpsd.fix.longitude
gps_data.altitude = 0
# gps_data.position_covariance_type = 0
#publication ros
pub.publish(gps_data)
rate.sleep()
def talker():
# Subscribe to Vicon messages
viconTopic = rospy.get_param('topic')
rospy.Subscriber(viconTopic, TransformStamped, callback)
# Start a publisher for the GPS messages
pub = rospy.Publisher('GPS/position', NavSatFix) # FIXME
# Start the node
rospy.init_node('talker')
# Populate the NavSatFix message from the parameter server
statusMsg = NavSatStatus()
statusMsg.status = rospy.get_param('status', -1)
statusMsg.service = rospy.get_param('service', 1)
fixMsg = NavSatFix()
fixMsg.header.stamp = rospy.Time.now()
fixMsg.header.frame_id = "/world"
fixMsg.status = statusMsg
fixMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
#position could be modified with some gaussian noise added to it and then calculate the covariance matrix.
fixMsg.position_covariance_type = rospy.get_param('position_covariance_type', 0);
while not rospy.is_shutdown():
[fixMsg.longitude, fixMsg.latitude, fixMsg.altitude] = c.xyz2gps([parentFrameLong, parentFrameLat, parentFrameAlt], latestViconMsg.transform.translation.x, latestViconMsg.transform.translation.y, latestViconMsg.transform.translation.z, parentFrameAngle)
statusMsg.status = rospy.get_param('status', statusMsg.status)
statusMsg.service = rospy.get_param('service', statusMsg.service)
# put the sigma and calculate the cov matrix here
#rospy.loginfo([fixMsg.longitude, fixMsg.latitude, fixMsg.altitude])
pub.publish(fixMsg)
rospy.sleep(0.1)
def publish(self, data):
msg = NavSatFix()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.latitude = data.getLat()
msg.longitude = data.getLng()
msg.altitude = data.getMeters()
if data.getFix() == 1:
msg.status.status = 0
else:
msg.status.status = -1
msg.position_covariance_type = 1
msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
msg.status.service = 1
self._pub.publish(msg)
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def subCB(msg_in):
global pub
msg_out = NavSatFix()
msg_out.header = msg_in.header
msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_out.status.service = NavSatStatus.SERVICE_GPS
msg_out.latitude = msg_in.LLA.x
msg_out.longitude = msg_in.LLA.y
msg_out.altitude = msg_in.LLA.z
msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_out.position_covariance[1] = msg_in.PosUncerainty
pub.publish(msg_out)
def __init__(self, *args):
super(MavrosQuad, self).__init__(*args)
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.local_velocity = TwistStamped()
self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link'
self.gazebo_load_pose = Pose()
self.gazebo_load_twist = Twist()
self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0'
self.gazebo_quad_pose = Pose()
self.gazebo_quad_twist = Twist()
self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link'
self.gazebo_imu_pose = Pose()
self.gazebo_imu_twist = Twist()
self.q_input = np.array([0, 0, 0, 1])
self.T_input = 0
self.offboard_position_active = True
self.offboard_attitude_active = False
self.offboard_load_active = False
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos',
'local_pos', 'local_vel', 'mission_wp', 'state', 'imu'
]
}
# ROS services
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
rospy.logerr("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.local_vel_sub = rospy.Subscriber(
'mavros/local_position/velocity_local', TwistStamped,
self.local_velocity_callback)
self.imu_sub = rospy.Subscriber('/mavros/imu/data', Imu,
self.imu_callback)
self.gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates,
self.gazebo_callback)
# ROS Publishers
# Attitude
self.att = AttitudeTarget()
self.att_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.att_thread = Thread(target=self.send_att, args=())
self.att_thread.daemon = True
self.att_thread.start()
# Pose
self.pos = PoseStamped()
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
def Start():
rospy.init_node('cubigolf_localization', anonymous=True)
#Setup the publishers
imu_pub = rospy.Publisher("/cubigolf/imu", Imu, queue_size= 10)
navsat_pub = rospy.Publisher("/cubigolf/gps", NavSatFix, queue_size=10)
magneto_pub = rospy.Publisher("/cubigolf/magneticfield", MagneticField, queue_size=10)
odom_pub = rospy.Publisher("/cubigolf/odom", Odometry, queue_size=10)
freq = 100
r = rospy.Rate(freq) # 10hz
imu = Imu()
gps = NavSatFix()
odom = Odometry()
magneto = MagneticField()
wheelspeed = Float32()
steeringangle = Float32()
vel_oldx = vel_oldy = angle_old = 0
#Start deserializing the code coming in on the serial port
myDeco = UartPLCDeserializer('/dev/ttyUSB0')
deserializerThread = threading.Thread(target=myDeco.Main, args=())
deserializerThread.start()
while not rospy.is_shutdown():
#Populate IMU
imu.header.stamp = rospy.Time.now()
imu.orientation.x = myDeco.variables.imu_quatX['value']
imu.orientation.y = myDeco.variables.imu_quatY['value']
imu.orientation.z = myDeco.variables.imu_quatZ['value']
imu.orientation.w = myDeco.variables.imu_quatW['value']
imu.angular_velocity.x = myDeco.variables.imu_gyroX_rads['value']
imu.angular_velocity.y = myDeco.variables.imu_gyroY_rads['value']
imu.angular_velocity.z = myDeco.variables.imu_gyroZ_rads['value']
imu.linear_acceleration.x = myDeco.variables.imu_accX_ms2['value']
imu.linear_acceleration.y = myDeco.variables.imu_accY_ms2['value']
imu.linear_acceleration.z = myDeco.variables.imu_accZ_ms2['value']
imu_pub.publish(imu)
#Populate the magneto message
magneto.header.stamp = rospy.Time.now()
magneto.magnetic_field.x = myDeco.variables.imu_magX_ut['value']
magneto.magnetic_field.y = myDeco.variables.imu_magY_ut['value']
magneto.magnetic_field.z = myDeco.variables.imu_magZ_ut['value']
magneto_pub.publish(magneto)
#Populate navfix message
gps.header.stamp = rospy.Time.now()
gps.latitude = myDeco.variables.gps_lat_dms['value']
gps.longitude = myDeco.variables.gps_long_dms['value']
gps.altitude = myDeco.variables.gps_altitude_m['value']
print gps.latitude, gps.longitude, gps.altitude
#NMEA 0 = Invalid;1 = GPS fix, 2 = DGPS fix
#ROS NavSatStatus
#int8 STATUS_NO_FIX = -1 # unable to fix position
#int8 STATUS_FIX = 0 # unaugmented fix
#int8 STATUS_SBAS_FIX = 1 # with satellite-based augmentation
#int8 STATUS_GBAS_FIX = 2 # with ground-based augmentation
if myDeco.variables.gps_fix['value'] == 0:
gps.status.status = NavSatStatus.STATUS_NO_FIX
elif myDeco.variables.gps_fix['value'] == 1:
gps.status.status = NavSatStatus.STATUS_FIX
elif myDeco.variables.gps_fix['value'] == 2:
gps.status.status = NavSatStatus.STATUS_SBAS_FIX # TODO Check FIX assumptions
navsat_pub.publish(gps)
#TODO Define an odometry frame for an ackermann platfrom
odom.child_frame_id="base_link"
odom.twist.twist.linear.x = 0
odom.twist.twist.linear.y = 0
odom.child_frame_id = "odom"
odom.pose.pose.position.x = 0
odom.pose.pose.position.y = 0
odom.pose.pose.orientation.x = 0
odom.pose.pose.orientation.y = 0
odom.pose.pose.orientation.z = 0
odom.pose.pose.orientation.w = 1
odom_pub.publish(odom)
r.sleep()
def __init__(self, this_uav, NUM_UAV, D_GAIN):
print 'init'
print 'this uav = ' + str(this_uav)
print 'num uav = ' + str(NUM_UAV)
print 'dir_path = ' + str(os.path.dirname(os.path.realpath(__file__)))
print 'cwd = ' + str(os.getcwd())
global cur_pose
global cur_state
global cur_vel
global cur_globalPose
global command
cur_pose = [PoseStamped() for i in range(NUM_UAV)]
cur_globalPose = [NavSatFix() for i in range(NUM_UAV)]
cur_state = [State() for i in range(NUM_UAV)]
cur_vel = [TwistStamped() for i in range(NUM_UAV)]
command = Float64MultiArray()
command.data = [0, 0, 0, -1]
des_pose = PoseStamped()
des_vel = TwistStamped()
des_vel.header.frame_id = "map"
status = Int8()
status.data = -1
convergence = 0
t = 0 #time at start of rotation
ttR = 20 #time to rotate
comDisk = 0 #40 #size of communication disk between drones
#create pid controllers for R and Theta (Kp, Ki, Kd)
cmdR = pid.PID(50, 0, 0)
cmdTheta = pid.PID(50, 25, 0)
rospy.init_node('offboard_test' + str(this_uav), anonymous=True)
rate = rospy.Rate(100) #Hz
mode_proxy = rospy.ServiceProxy(
'mavros' + str(this_uav + 1) + '/set_mode', SetMode)
arm_proxy = rospy.ServiceProxy(
'mavros' + str(this_uav + 1) + '/cmd/arming', CommandBool)
paramPull_proxy = rospy.ServiceProxy(
'mavros' + str(this_uav + 1) + '/param/pull', ParamPull)
paramGet_proxy = rospy.ServiceProxy(
'mavros' + str(this_uav + 1) + '/param/get', ParamGet)
paramSet_proxy = rospy.ServiceProxy(
'mavros' + str(this_uav + 1) + '/param/set', ParamSet)
print('paramPull - \n' + str(paramPull_proxy(True)))
print('paramGet MAV_TYPE - \n' + str(paramGet_proxy("MAV_TYPE")))
print(
'______________________________________________________________________________'
)
#generating subscribers to ALL drones in the swarm
for i in range(NUM_UAV):
exec('def position_cb' + str(i) + '(msg): cur_pose[' + str(i) +
'] = msg')
exec('def globalPosition_cb' + str(i) + '(msg): cur_globalPose[' +
str(i) + '] = msg')
exec('def velocity_cb' + str(i) + '(msg): cur_vel[' + str(i) +
'] = msg')
exec('def state_cb' + str(i) + '(msg): cur_state[' + str(i) +
'] = msg')
rospy.Subscriber('/mavros' + str(i + 1) + '/local_position/pose',
PoseStamped,
callback=eval('position_cb' + str(i)))
rospy.Subscriber('/mavros' + str(i + 1) +
'/global_position/global',
NavSatFix,
callback=eval('globalPosition_cb' + str(i)))
rospy.Subscriber('/mavros' + str(i + 1) + '/state',
State,
callback=eval('state_cb' + str(i)))
rospy.Subscriber('/mavros' + str(i + 1) +
'/local_position/velocity',
TwistStamped,
callback=eval('velocity_cb' + str(i)))
#suscribe to sequencer
rospy.Subscriber('/sequencer/command',
Float64MultiArray,
callback=self.command_cb)
#publish status to sequencer
status_pub = rospy.Publisher('/sequencer/status' + str(this_uav),
Int8,
queue_size=10)
#publish position setpoints to FCU
pose_pub = rospy.Publisher('/mavros' + str(this_uav + 1) +
'/setpoint_position/local',
PoseStamped,
queue_size=10)
#publish velocity setpoints to FCU
vel_pub = rospy.Publisher('/mavros' + str(this_uav + 1) +
'/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
#2nd subscriber to uav-state [temporary]
rospy.Subscriber('/mavros' + str(this_uav + 1) + '/state',
State,
callback=self.state_cb)
#assigning identities for drone ahead and behind in formation
#only relevant to rotations in cmd 4
plus_uav = (this_uav + 1) % NUM_UAV
minus_uav = (this_uav - 1) % NUM_UAV
#GOTO initial holding pattern command = [0,0,25,0]
des_pose = cur_pose[this_uav]
des_pose.pose.position.x = 10 * math.sin(
(this_uav * 2 * math.pi) / NUM_UAV)
des_pose.pose.position.y = 10 * math.cos(
(this_uav * 2 * math.pi) / NUM_UAV)
des_pose.pose.position.z = 20
pose_pub.publish(des_pose)
status_pub.publish(status)
#....fix....double subscribe to status, check and set status in callback?
print cur_state[this_uav]
print '---------------arming-----------------------'
print '----WAITING FOR STANDBY STATUS---'
while True:
pose_pub.publish(des_pose)
if cur_state[this_uav].system_status == 3:
print cur_state[this_uav]
break
print 'INITIAL POSITION'
print cur_globalPose[this_uav]
while cur_state[this_uav].mode != 'OFFBOARD' and not cur_state[
this_uav].armed:
print 'intial arming'
mode_sent = False
success = False
pose_pub.publish(des_pose)
while not mode_sent:
rospy.wait_for_service('mavros' + str(this_uav + 1) +
'/set_mode',
timeout=None)
try:
mode_sent = mode_proxy(1, 'OFFBOARD')
except rospy.ServiceException as exc:
print exc
rate.sleep()
while not success:
rospy.wait_for_service('mavros' + str(this_uav + 1) +
'/cmd/arming',
timeout=None)
try:
success = arm_proxy(True)
except rospy.ServiceException as exc:
print exc
rate.sleep()
print 'mode_sent - ' + str(mode_sent)
print 'arm_sent - ' + str(success)
print 'armed?'
rate.sleep()
print cur_state[this_uav]
#wait for sequencer to connect to /sequencer/status#
nc = status_pub.get_num_connections()
print 'num_connections = ' + str(nc)
sys.stdout.flush()
while nc == 0:
nc = status_pub.get_num_connections()
rate.sleep()
print 'num_connections = ' + str(nc)
print 'FLAG'
print rospy.is_shutdown()
sys.stdout.flush()
rate.sleep()
print rospy.is_shutdown()
#MAIN LOOP
print 'Loop INIT Time - ' + str(time.clock())
while not rospy.is_shutdown():
#''' #hopefully temporary hack
while cur_state[this_uav].mode != 'OFFBOARD' and not cur_state[
this_uav].armed:
print '------------------rearming----------------------------'
mode_sent = False
success = False
pose_pub.publish(des_pose)
while not mode_sent:
rospy.wait_for_service('mavros' + str(this_uav + 1) +
'/set_mode',
timeout=None)
try:
mode_sent = mode_proxy(1, 'OFFBOARD')
except rospy.ServiceException as exc:
print exc
while not success:
rospy.wait_for_service('mavros' + str(this_uav + 1) +
'/cmd/arming',
timeout=None)
try:
success = arm_proxy(True)
except rospy.ServiceException as exc:
print exc
#temp end
#'''
if command.data[3] > -1 and command.data[3] < 4:
des_pose.pose.position.x = command.data[
0] + command.data[2] * math.sin(
(this_uav * 2 * math.pi) / NUM_UAV)
des_pose.pose.position.y = command.data[
1] + command.data[2] * math.cos(
(this_uav * 2 * math.pi) / NUM_UAV)
des_pose.pose.position.z = 25
convergence = math.sqrt(
math.pow(
des_pose.pose.position.x -
cur_pose[this_uav].pose.position.x, 2) + math.pow(
des_pose.pose.position.y -
cur_pose[this_uav].pose.position.y, 2) + math.pow(
des_pose.pose.position.z -
cur_pose[this_uav].pose.position.z, 2))
if convergence < .5 and status != Int8(command.data[3]):
status = Int8(command.data[3])
print 'Status Set - ' + str(status) + ' time - ' + str(
time.clock())
print 'Current Pose - ' + str(cur_pose[this_uav].pose)
status_pub.publish(status)
pose_pub.publish(des_pose)
if command.data[3] == 4:
#timing on rotation
if t == 0:
t = time.clock()
print t
if time.clock() > t + ttR:
status = Int8(command.data[3])
print 'Status Set - ' + str(status) + ' time - ' + str(
time.clock())
status_pub.publish(status)
ttR = 1000000000000
r = math.sqrt(
math.pow(cur_pose[this_uav].pose.position.x, 2) +
math.pow(cur_pose[this_uav].pose.position.y, 2))
theta = math.atan2(cur_pose[this_uav].pose.position.y,
cur_pose[this_uav].pose.position.x) % (
2 * math.pi) #from [-pi,pi] -> [0, 2pi]
theta_plus = math.atan2(
cur_pose[plus_uav].pose.position.y,
cur_pose[plus_uav].pose.position.x) % (2 * math.pi)
theta_minus = math.atan2(
cur_pose[minus_uav].pose.position.y,
cur_pose[minus_uav].pose.position.x) % (2 * math.pi)
#deal with wrap around
if theta_minus < theta: #yea looks backwards
dtheta_minus = (theta_minus + 2 * math.pi) - theta
else:
dtheta_minus = theta_minus - theta
if theta < theta_plus: #...again...backwards
dtheta_plus = (theta + 2 * math.pi) - theta_plus
else:
dtheta_plus = theta - theta_plus
if comDisk: #do stuff
distPlus = math.sqrt(
math.pow(
cur_pose[this_uav].pose.position.x -
cur_pose[plus_uav].pose.position.x, 2) + math.pow(
cur_pose[this_uav].pose.position.y -
cur_pose[plus_uav].pose.position.y, 2))
distMinus = math.sqrt(
math.pow(
cur_pose[this_uav].pose.position.x -
cur_pose[minus_uav].pose.position.x, 2) + math.pow(
cur_pose[this_uav].pose.position.y -
cur_pose[minus_uav].pose.position.y, 2))
if distPlus > comDisk or distMinus > comDisk:
print 'comDisk'
print ' distPlus - ' + str(distPlus)
print ' distMinus - ' + str(distMinus)
if distPlus > comDisk and distMinus > comDisk:
dtheta_plus = dtheta_minus = 2
elif distPlus > comDisk:
dtheta_plus = 2
dtheta_minus = 0
elif distMinus > comDisk:
dtheta_minus = 0
dtheta_plus = .5
thetaError = (dtheta_minus - dtheta_plus)
rError = command.data[2] - r
thetaDot = cmdTheta.update(thetaError) + 5
rDot = cmdR.update(rError)
if thetaDot < 0:
thetaDot = 0
des_vel.twist.linear.x = (rDot * math.cos(theta) -
r * thetaDot * math.sin(theta)) * 2
des_vel.twist.linear.y = (rDot * math.sin(theta) +
r * thetaDot * math.cos(theta)) * 2
des_vel.twist.linear.z = (
25 - cur_pose[this_uav].pose.position.z) * .5
vel_pub.publish(des_vel)
print theta
if command.data[3] == 5:
config = np.loadtxt('/simulation/config0_0.txt')
status = Int8(command.data[3])
print 'Status Set - ' + str(status) + ' time - ' + str(
time.clock())
status_pub.publish(status)
if command.data[3] > 5:
des_pose.pose.position.x = config[int(command.data[3] - 6),
(4 * this_uav)]
des_pose.pose.position.y = config[int(command.data[3] - 6),
(4 * this_uav) + 1]
des_pose.pose.position.z = 20 + (2 * this_uav)
convergence = math.sqrt(
math.pow(
des_pose.pose.position.x -
cur_pose[this_uav].pose.position.x, 2) + math.pow(
des_pose.pose.position.y -
cur_pose[this_uav].pose.position.y, 2) + math.pow(
des_pose.pose.position.z -
cur_pose[this_uav].pose.position.z, 2))
if convergence < .5 and status != Int8(command.data[3]):
status = Int8(command.data[3])
print 'Status Set - ' + str(status) + ' time - ' + str(
time.clock())
print 'Current Pose - ' + str(cur_pose[this_uav].pose)
print 'Cmd Pose - (' + str(
config[int(command.data[3] - 6),
(4 * this_uav)]) + ',' + str(
config[int(command.data[3] - 6),
(4 * this_uav) + 1]) + ')'
status_pub.publish(status)
pose_pub.publish(des_pose)
rate.sleep()
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
return
# Changing from NED from the Applanix to ENU in ROS
# Roll is still roll, since it's WRT to the x axis of the vehicle
# Pitch is -ve since axis goes the other way (+y to right vs left)
# Yaw (or heading) in Applanix is clockwise starting with North
# In ROS it's counterclockwise startin with East
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch),
RAD(90 - data.heading)).GetQuaternion()
# UTM conversion
utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude)
# Initialize starting point if we haven't yet
if not self.init and self.zero_start:
self.origin.x = utm_pos.easting
self.origin.y = utm_pos.northing
self.origin.z = data.altitude
self.init = True
origin_param = {
"east": self.origin.x,
"north": self.origin.y,
"alt": self.origin.z,
}
rospy.set_param('/gps_origin', origin_param)
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
p.position.z = self.origin.z
self.pub_origin.publish(p)
#
# Odometry
# TODO: Work out these covariances properly from DOP
#
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = utm_pos.easting - self.origin.x
odom.pose.pose.position.y = utm_pos.northing - self.origin.y
odom.pose.pose.position.z = data.altitude - self.origin.z
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /reference frame or /vehicle frame and
# NED to ENU conversion is needed here too
odom.twist.twist.linear.x = data.east_vel
odom.twist.twist.linear.y = data.north_vel
odom.twist.twist.linear.z = -data.down_vel
odom.twist.twist.angular.x = RAD(data.ang_rate_long)
odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z
q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion(
*orient).z, Quaternion(*orient).w
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp,
odom.child_frame_id,
odom.header.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
navsat.latitude = data.latitude
navsat.longitude = data.longitude
navsat.altitude = data.altitude
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.pub_navsatfix.publish(navsat)
#
# IMU
# TODO: Work out these covariances properly
#
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
imu.orientation = Quaternion(*orient)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates
imu.angular_velocity.x = RAD(data.ang_rate_long)
imu.angular_velocity.y = RAD(-data.ang_rate_trans)
imu.angular_velocity.z = RAD(-data.ang_rate_down)
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration
imu.linear_acceleration.x = data.long_accel
imu.linear_acceleration.y = data.trans_accel
imu.linear_acceleration.z = data.down_accel
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pass
def parse_novatelINSPVA(insHeader, insString):
#print "++++instring:",insString
gnssWeek = insString[0] # GNSS week
secondsFromWeek = insString[1] # Seconds from week start
latitude = insString[2] # Latitude (WGS84)
longitude = insString[3] # Longitude (WGS84)
heightMSL = insString[4] # Ellipsoidal Height (WGS84) [m]
velcnsY = float(
insString[5]
) # Velocity in northerly direction [m/s] (negative for south)
velcnsX = float(
insString[6]
) # Velocity in easterly direction [m/s] (negative for west)
velcnsZ = float(insString[7]) # Velocity in upward direction [m/s]
cnsYaw = float(insString[8]) * (
3.14159265 / 180
) # yaw/azimuth - Right-handed rotation from local level around Z-axis -- changed from degress to radians (pi/180 deg)
cnsRoll = float(insString[9]) * (
3.14159265 / 180
) # roll - (neg) Right-handed rotation from local level around y-axis -- changed from degress to radians (pi/180 deg)
cnsPitch = float(insString[10]) * (
3.14159265 / 180
) # pitch -Right-handed rotation from local level around x-axis -- changed from degress to radians (pi/180 deg)
inertialStatus = insString[11].split('*')[0] # Inertial status
#print "inertialStatus",inertialStatus
fix_msg = NavSatFix()
fix_msg.header.stamp = rospy.get_rostime()
fix_msg.header.frame_id = 'cns5000_frame'
fix_msg.latitude = float(latitude)
fix_msg.longitude = float(longitude)
fix_msg.altitude = float(heightMSL)
fix_msg.position_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 999.0
]
if (insHeader[4] == "FINESTEERING"):
fix_msg.position_covariance_type = 2
else:
fix_msg.position_covariance_type = 0
#print "lat, long, alt:" + str(fix_msg.latitude)+ " , "+ str(fix_msg.longitude)+" , " + str(fix_msg.altitude)
global curTime
global curRoll
global curYaw
global curPitch
global lastRoll
global lastYaw
global lastPitch
global curvelcnsX
global curvelcnsY
global curvelcnsZ
global lastvelcnsX
global lastvelcnsY
global lastvelcnsZ
#cns to imu coordinate sytem conversion --> cnsX = -imuY, cnsY = imuX, cnsZ = imuZ
#imuY comes in as negative of value --> cancels out negative conversion
imuYaw = -1 * cnsPitch + 1.57079632679 #rotate 90 deg, or pi/2 radians
#print(imuYaw)
imuPitch = cnsRoll
imuRoll = cnsYaw
lastYaw = curYaw
curYaw = imuYaw
lastPitch = curPitch
curPitch = imuPitch
lastRoll = curRoll
curRoll = imuRoll
lastvelcnsX = curvelcnsX
curvelcnsX = velcnsX
lastvelcnsY = curvelcnsY
curvelcnsY = velcnsY
lastvelcnsZ = curvelcnsZ
curvelcnsZ = velcnsZ
lastTime = curTime
curTime = rospy.Time.now()
deltime = (curTime - lastTime).to_sec()
if (deltime == 0):
deltime = 0.00000001
imu_msg = Imu()
imu_msg.header.stamp = curTime
imu_msg.header.frame_id = 'cns5000_frame'
imu_msg.linear_acceleration.x = (curvelcnsY -
lastvelcnsY) / deltime #*.05/pow(2,15)
imu_msg.linear_acceleration.y = (
curvelcnsY - lastvelcnsY) / deltime #*-1#*.05/pow(2,15)
imu_msg.linear_acceleration.z = (curvelcnsY -
lastvelcnsY) / deltime #*.05/pow(2,15)
imu_msg.linear_acceleration_covariance = [
0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1
]
#imu_msg.orientation_covariance.x = float(angcnsY)
#euler = Vector3(curRoll, curPitch, curYaw)
#euler = vector_norm(euler)
quaternion = tf.transformations.quaternion_from_euler(0, 0, curYaw)
#print(curRoll, curPitch, curYaw)
#type(pose) = geometry_msgs.msg.Pose
# DML Next two lines additions to normalize the quaternion
quat = numpy.array(
[quaternion[0], quaternion[1], quaternion[2], quaternion[3]])
quaternion[0], quaternion[1], quaternion[2], quaternion[
3] = quat / numpy.sqrt(numpy.dot(quat, quat))
imu_msg.orientation.x = quaternion[0]
imu_msg.orientation.y = quaternion[1]
imu_msg.orientation.z = quaternion[2]
imu_msg.orientation.w = quaternion[3]
imu_msg.orientation_covariance = [
0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1
]
imu_msg.angular_velocity.x = (curPitch - lastPitch) / deltime
imu_msg.angular_velocity.y = (curRoll - lastRoll) / deltime
imu_msg.angular_velocity.z = (curYaw - lastYaw) / deltime
imu_msg.angular_velocity_covariance = [
0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1
]
#velx = float(velEast)*.05/pow(2,15)
#vely = float(velNorth)*.05/pow(2,15)
#velz = float(velUp)*.05/pow(2,15)
#imu_msg = Vector3(velx,vely,velz,pitch,roll)
#imu_msg.orientation.w = 0.01
retrnList = [imu_msg, fix_msg]
return retrnList
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_VELECEF,
1)
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_POSECEF,
1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_RAW, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SFRB, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SVSI, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_ALM, 1)
ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_EPH, 1)
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_TIMEGPS,
5)
ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_CLOCK, 5)
while not rospy.is_shutdown():
msg = ubl.receive_message()
if msg is None:
pass
if msg.name() == "NAV_POSLLH":
outstr = str(msg).split(",")[1:]
outstr = "".join(outstr)
components = search(" Longitude={:d} Latitude={:d} Height={:d}",
outstr)
h = Header()
h.stamp = rospy.Time.now()
navmsg = NavSatFix()
navmsg.header = h
navmsg.latitude = float(components[1]) / 10000000.0
navmsg.longitude = float(components[0]) / 10000000.0
navmsg.altitude = float(components[2]) / 10000000.0
publisher.publish(navmsg)
import rospy
from sensor_msgs.msg import NavSatFix
import sys
if len(sys.argv) < 4:
print('usage: {} lat lng alt'.format(sys.argv[0]))
exit()
lat = float(sys.argv[1])
lng = float(sys.argv[2])
alt = float(sys.argv[3])
rospy.init_node("target_gps_broadcaster")
pub = rospy.Publisher('target_gps_fix', NavSatFix, queue_size=1)
rospy.sleep(1)
fix = NavSatFix()
fix.header.stamp = rospy.Time.now()
fix.header.frame_id = 'gps_frame'
fix.latitude = lat
fix.longitude = lng
fix.altitude = alt
fix.status.status = 0
fix.status.service = 1
pub.publish(fix)
def callback(self):
rate = rospy.Rate(10)
ports = [5801, 5802, 5803]
sockets = []
pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub]
for i in ports:
sock = socket.socket()
host = socket.gethostname()
sock.connect((host, i))
sockets.append(sock)
e2 = 6.69437999014e-3
a = 6378137.0
while not rospy.is_shutdown():
for i in range(len(sockets)):
navsat = NavSatFix()
ecef_xyz = Point()
ecef_pose = Pose()
#ecef_stampedPose = PoseStamped()
navsat.header.stamp = rospy.Time.now()
#Get the position message from the RTKRCV server
msgStr = sockets[i].recv(1024)
#Split the message
msg = msgStr.split()
navsat.latitude = float(msg[2])
navsat.longitude = float(msg[3])
navsat.altitude = float(msg[4])
navsat.position_covariance = [
float(msg[7]),
float(msg[10]),
float(msg[12]),
float(msg[10]),
float(msg[8]),
float(msg[11]),
float(msg[12]),
float(msg[11]),
float(msg[9])
]
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
N = 1.0 * a / np.sqrt(
1 - e2 * (np.sin(float(msg[2]) * np.pi / 180.0)**2))
ecef_xyz.x = (N + float(msg[4])) * np.cos(
float(msg[2]) * np.pi / 180.0) * np.cos(
float(msg[3]) * np.pi / 180.0)
ecef_xyz.y = (N + float(msg[4])) * np.cos(
float(msg[2]) * np.pi / 180.0) * np.sin(
float(msg[3]) * np.pi / 180.0)
ecef_xyz.z = (N * (1 - e2) + float(msg[4])) * np.sin(
float(msg[2]) * np.pi / 180.0)
ecef_pose.position = ecef_xyz
#ecef_stampedPose.pose = ecef_pose
pubs[i].publish(navsat)
rate.sleep()
def __init__(self, default_port='/dev/ttyUSB1'):
"""
@param default_port: default serial port to use for
establishing a connection to the UM7 IMU sensor.
This will be overridden by ~port param if
available.
"""
rospy.init_node('imu_um7')
self.port = rospy.get_param('~port', default_port)
self.frame_id = rospy.get_param('~frame_id', "/imu")
self.gps_frame_id = rospy.get_param('~gps_frame_id', "/gps")
self.throttle_rate = rospy.get_param('~throttle_rate', 10000)
self.reset_mag = rospy.get_param('~reset_mag', False)
self.reset_accel = rospy.get_param('~reset_accel', False)
self.mag_zero_x = rospy.get_param('~mag_zero_x', False)
self.mag_zero_y = rospy.get_param('~mag_zero_y', False)
self.mag_zero_z = rospy.get_param('~mag_zero_z', False)
rospy.loginfo("serial port: %s" % (self.port))
self.link = rospy.get_param('~link', 'imu_link')
rospy.loginfo("tf link: {}".format(self.link))
self.imu_data = Imu()
self.imu_data = Imu(header=rospy.Header(frame_id=self.link))
# These covariance calculations are based on those bound in the existing
# UM7 ROS package at:
# https://github.com/ros-drivers/um7
linear_acceleration_stdev = rospy.get_param(
"~linear_acceleration_stdev", 4.0 * 1e-3 * 9.80665)
angular_velocity_stdev = rospy.get_param("~angular_velocity_stdev",
np.deg2rad(0.06))
linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev
angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev
# From the UM7 datasheet for the dynamic accuracy from the EKF.
orientation_x_stdev = rospy.get_param("~orientation_x_stdev",
np.deg2rad(3.0))
orientation_y_stdev = rospy.get_param("~orientation_y_stdev",
np.deg2rad(3.0))
orientation_z_stdev = rospy.get_param("~orientation_z_stdev",
np.deg2rad(5.0))
orientation_x_covar = orientation_x_stdev * orientation_x_stdev
orientation_y_covar = orientation_y_stdev * orientation_y_stdev
orientation_z_covar = orientation_z_stdev * orientation_z_stdev
self.imu_data.orientation_covariance = [
orientation_x_covar, 0, 0, 0, orientation_y_covar, 0, 0, 0,
orientation_z_covar
]
self.imu_data.angular_velocity_covariance = [
angular_velocity_cov, 0, 0, 0, angular_velocity_cov, 0, 0, 0,
angular_velocity_cov
]
self.imu_data.linear_acceleration_covariance = [
linear_acceleration_cov, 0, 0, 0, linear_acceleration_cov, 0, 0, 0,
linear_acceleration_cov
]
# Set up the imu data message
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=1)
# Set up the Roll-Pitch-Yaw (rpy) message
self.rpy_data = Vector3Stamped()
self.rpy_pub = rospy.Publisher('imu/rpy', Vector3Stamped, queue_size=1)
# Set up the magnometer message
self.mag_data = Vector3Stamped()
self.mag_pub = rospy.Publisher('imu/mag', Vector3Stamped, queue_size=1)
# what data to get from the UM7
self.statevars = [
'health', 'roll', 'pitch', 'yaw', 'mag_proc_x', 'mag_proc_y',
'mag_proc_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z',
'gyro_proc_x', 'gyro_proc_y', 'gyro_proc_z', 'quat_a', 'quat_b',
'quat_c', 'quat_d', 'gps_latitude', 'gps_longitude', 'gps_altitude'
]
# Masks for parsing out the health data from the IMU
self.NUM_SATS_USED = 0b11111100000000000000000000000000
self.HDOP = 0b00000011111111110000000000000000 # Not used as of 05/23/19
self.NUM_SATS_IN_VIEW = 0b00000000000000001111110000000000
# Set up the GPS NavSatFix publisher
self.fix_data = NavSatFix()
self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1)
imu_connected = False
while not rospy.is_shutdown() and not imu_connected:
try:
self.driver = um7.UM7('s',
self.port,
self.statevars,
baud=115200)
imu_connected = True
rospy.loginfo("Imu initialization completed")
self.received = -1
if self.reset_mag:
self.driver.set_mag_reference()
if self.reset_accel:
cmd_seq.append(Um6Drv.CMD_SET_ACCEL_REF)
if self.mag_zero_x and self.mag_zero_y and self.mag_zero_z:
rospy.loginfo("Magnetometer calibration: %.3f %.3f %.3f",
self.mag_zero_x, self.mag_zero_y,
self.mag_zero_z)
except SerialException:
rospy.logwarn(
"Serial error communicating with IMU. Will retry in 2.0 seconds."
)
rospy.sleep(2.0)
def __init__(self):
# Initializing ros node with name position_control
rospy.init_node('position_controller', log_level=rospy.DEBUG)
# Creating an instance of NavSatFix message and edrone_cmd message
self.location = NavSatFix()
self.drone_cmd = edrone_cmd()
# Created a flag for changing the setpoints
self.targets_achieved = 0
# Counter for csv file
self.csv_counter = 0
# Counter Check if obstacle was deteceted
self.obstacle_count = 0
# Gripper check vaiable
self.gripper_data = False
# A multiplication factor to increase number of waypoints proportional to distance between final and initial
self.stride = 1 / 300.0
# Hardcoded initial target point
"""
Building 1: lat: 18.9990965928, long: 72.0000664814, alt: 10.75
Building 2: lat: 18.9990965925, long: 71.9999050292, alt: 22.2
Building 3: lat: 18.9993675932, long: 72.0000569892, alt: 10.7
"""
# 0:takeoff,1:transverse,2:landing,3:takeoff W/P,4:transverse W/P,5:landing W/P
self.states = 0
# List of target buildings
self.buiding_locations = []
# Initial Longitude , latitude and altitude
self.targets = [
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
]
# Variable to store scanned waypoints
self.scanned_target = [0.0, 0.0, 0.0]
# Marker ids
self.marker_id = 0
# To store bottom range finder values
self.range_finder_bottom = 0
# Variable for top range finder sensor data
# [1] right, [2] back, [3] left, [0,4] front w.r.t eyantra logo
self.range_finder_top_list = [0.0, 0.0, 0.0, 0.0, 0.0]
# Weights for left right sensor values
self.weights_lr = [1.3, -1.3]
# Counter for number of landings made
self.bottom_count = 0
# Offset altitude
self.offset_alt = 3.00
# Safety distances
self.safe_dist_lat = 21.555 / 105292.0089353767
self.safe_dist_long = 6 / 110692.0702932625
# To store the x and y co-ordinates in meters
self.err_x_m = 0.0
self.err_y_m = 0.0
# Number of waypoints initialized to -1
self.n = -1
# List to store waypoints Points calculated
self.points = []
# Check if height attained
self.start_to_check_for_obstacles = False
# Kp, Ki and Kd found out experimentally using PID tune
self.Kp = [88318, 88318, 1082 * 0.06]
self.Ki = [0.008 * 10, 0.008 * 10, 0.0 * 0.008]
self.Kd = [6606000, 6606000, 4476 * 0.3]
# Output, Error ,Cummulative Error and Previous Error of the PID equation in the form [Long, Lat, Alt]
self.error = [0.0, 0.0, 0.0]
self.ouput = [0.0, 0.0, 0.0]
self.cummulative_error = [0.0, 0.0, 0.0]
self.previous_error = [0.0, 0.0, 0.0]
self.max_cummulative_error = [1e-3, 1e-3, 100]
self.max_error = 0.00038256625782
self.assigned_error = 0.000118256625782 / 2
self.throttle = 0
self.base_pwm = 1500
# ----------------------------------------------------------------------------------------------------------
# Allowed errors in long.,and lat.
self.allowed_lon_error = 0.0000047487 / 6
self.allowed_lat_error = 0.000004517 / 6
# Checking if we have to scan or Land
self.scan = False
# Variable representing if the box has been grabbed
self.box_grabbed = 0
# Constraining the no of times location_error has been called
self.count = 0
# Time in which PID algorithm runs
self.pid_break_time = 0.060 # in seconds
self.x = 0
# Publishing servo-control messaages and altitude,longitude,latitude and zero error on errors /drone_command, /alt_error, /long_error, /lat_error, and current marker id
self.drone_pub = rospy.Publisher('/drone_command',
edrone_cmd,
queue_size=1)
self.alt_error = rospy.Publisher('/alt_error', Float32, queue_size=1)
self.long_error = rospy.Publisher('/long_error', Float32, queue_size=1)
self.lat_error = rospy.Publisher('/lat_error', Float32, queue_size=1)
self.zero_error = rospy.Publisher('/zero_error', Float32, queue_size=1)
self.curr_m_id = rospy.Publisher('/edrone/curr_marker_id',
Int32,
queue_size=1)
self.alt_diff = rospy.Publisher("/alt_diff", Float32, queue_size=1)
# -----------------------------------------------------------------------------------------------------------
# Subscribers for gps co-ordinates, and pid_tune GUI, gripper,rangefinder, custom location message and x,y errors in meters
rospy.Subscriber('/edrone/gps', NavSatFix, self.gps_callback)
rospy.Subscriber('/pid_tuning_altitude', PidTune,
self.altitude_set_pid)
rospy.Subscriber('/pid_tuning_roll', PidTune, self.long_set_pid)
rospy.Subscriber('/pid_tuning_pitch', PidTune, self.lat_set_pid)
rospy.Subscriber('/edrone/location_custom', location_custom,
self.scanQR)
rospy.Subscriber('/edrone/range_finder_bottom', LaserScan,
self.range_bottom)
rospy.Subscriber('/edrone/range_finder_top', LaserScan, self.range_top)
rospy.Subscriber('/edrone/gripper_check', String,
self.gripper_callback)
rospy.Subscriber('/edrone/err_x_m', Float32, self.handle_x_m_err)
rospy.Subscriber('/edrone/err_y_m', Float32, self.handle_y_m_err)
def spin_once(self):
def quat_from_orient(orient):
'''Build a quaternion from orientation data.'''
try:
w, x, y, z = orient['quaternion']
return (x, y, z, w)
except KeyError:
pass
try:
return quaternion_from_euler(pi*orient['roll']/180.,
pi*orient['pitch']/180, pi*orient['yaw']/180.)
except KeyError:
pass
try:
m = identity_matrix()
m[:3,:3] = orient['matrix']
return quaternion_from_matrix(m)
except KeyError:
pass
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp = data.get('Temp') # float
raw_data = data.get('RAW')
imu_data = data.get('Calib')
orient_data = data.get('Orient')
velocity_data = data.get('Vel')
position_data = data.get('Pos')
rawgps_data = data.get('RAWGPS')
status = data.get('Stat') # int
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
# fill information where it's due
# start by raw information that can be overriden
if raw_data: # TODO warn about data not calibrated
pub_imu = True
pub_vel = True
pub_mag = True
pub_temp = True
# acceleration
imu_msg.linear_acceleration.x = raw_data['accX']
imu_msg.linear_acceleration.y = raw_data['accY']
imu_msg.linear_acceleration.z = raw_data['accZ']
imu_msg.linear_acceleration_covariance = (0., )*9
# gyroscopes
imu_msg.angular_velocity.x = raw_data['gyrX']
imu_msg.angular_velocity.y = raw_data['gyrY']
imu_msg.angular_velocity.z = raw_data['gyrZ']
imu_msg.angular_velocity_covariance = (0., )*9
vel_msg.twist.angular.x = raw_data['gyrX']
vel_msg.twist.angular.y = raw_data['gyrY']
vel_msg.twist.angular.z = raw_data['gyrZ']
# magnetometer
mag_msg.vector.x = raw_data['magX']
mag_msg.vector.y = raw_data['magY']
mag_msg.vector.z = raw_data['magZ']
# temperature
# 2-complement decoding and 1/256 resolution
x = raw_data['temp']
if x&0x8000:
temp_msg.data = (x - 1<<16)/256.
else:
temp_msg.data = x/256.
if rawgps_data:
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
if temp is not None:
pub_temp = True
temp_msg.data = temp
if imu_data:
try:
x = imu_data['gyrX']
y = imu_data['gyrY']
z = imu_data['gyrZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.angular_velocity.x = v[0]
imu_msg.angular_velocity.y = v[1]
imu_msg.angular_velocity.z = v[2]
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = v[0]
vel_msg.twist.angular.y = v[1]
vel_msg.twist.angular.z = v[2]
pub_vel = True
except KeyError:
pass
try:
x = imu_data['accX']
y = imu_data['accY']
z = imu_data['accZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.linear_acceleration.x = v[0]
imu_msg.linear_acceleration.y = v[1]
imu_msg.linear_acceleration.z = v[2]
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
pub_imu = True
except KeyError:
pass
try:
x = imu_data['magX']
y = imu_data['magY']
z = imu_data['magZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
mag_msg.vector.x = v[0]
mag_msg.vector.y = v[1]
mag_msg.vector.z = v[2]
pub_mag = True
except KeyError:
pass
if velocity_data:
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
if orient_data:
pub_imu = True
orient_quat = quat_from_orient(orient_data)
imu_msg.orientation.x = orient_quat[0]
imu_msg.orientation.y = orient_quat[1]
imu_msg.orientation.z = orient_quat[2]
imu_msg.orientation.w = orient_quat[3]
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
if position_data:
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
if status is not None:
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
if pub_gps:
if status & 0b0100:
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
def spin_once(self):
'''Read data from device and publishes ROS messages.'''
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
press_msg = Float32()
pub_press = False
anin1_msg = UInt16()
pub_anin1 = False
anin2_msg = UInt16()
pub_anin2 = False
pub_diag = False
def fill_from_raw(raw_data):
'''Fill messages with information from 'raw' MTData block.'''
# don't publish raw imu data anymore
# TODO find what to do with that
pass
def fill_from_rawgps(rawgps_data):
'''Fill messages with information from 'rawgps' MTData block.'''
global pub_hps, xgps_msg, gps_msg
if rawgps_data['bGPS'] < self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT'] * 1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data[
'LON'] * 1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT'] * 1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2 * rawgps_data['Hacc'] * 1e-3
xgps_msg.err_vert = 2 * rawgps_data['Vacc'] * 1e-3
self.old_bGPS = rawgps_data['bGPS']
def fill_from_Temp(temp):
'''Fill messages with information from 'temperature' MTData block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = temp
def fill_from_Calib(imu_data):
'''Fill messages with information from 'calibrated' MTData block.'''
global pub_imu, imu_msg, pub_vel, vel_msg, pub_mag, mag_msg
try:
pub_imu = True
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0.,
0., radians(0.025), 0.,
0., 0., radians(0.025))
pub_vel = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
except KeyError:
pass
try:
pub_imu = True
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0.,
0.0004)
except KeyError:
pass
try:
pub_mag = True
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
except KeyError:
pass
def fill_from_Vel(velocity_data):
'''Fill messages with information from 'velocity' MTData block.'''
global pub_vel, vel_msg
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.'''
global pub_imu, imu_msg
pub_imu = True
if orient.has_key('quaternion'):
w, x, y, z = orient['quaternion']
elif orient.has_key('roll'):
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(orient['roll']),
radians(orient['pitch']),
radians(orient['yaw']))
elif orient.has_key('matrix'):
m = identity_matrix()
m[:3, :3] = orient['matrix']
x, y, z, w = quaternion_from_matrix(m)
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
def fill_from_Pos(position_data):
'''Fill messages with information from 'position' MTData block.'''
global pub_gps, xgps_msg, gps_msg
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
def fill_from_Stat(status):
'''Fill messages with information from 'status' MTData block.'''
global pub_diag, pub_gps, gps_msg, xgps_msg
pub_diag = True
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
def fill_from_Auxiliary(aux_data):
'''Fill messages with information from 'Auxiliary' MTData block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['Ain_1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['Ain_2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Temperature(o):
'''Fill messages with information from 'Temperature' MTData2 block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = o['Temp']
def fill_from_Timestamp(o):
'''Fill messages with information from 'Timestamp' MTData2 block.'''
global h
try:
# put timestamp from gps UTC time if available
y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
if f & 0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO find what to do with other kind of information
pass
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']),
radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
def fill_from_Pressure(o):
'''Fill messages with information from 'Pressure' MTData2 block.'''
global pub_press, press_msg
press_msg.data = o['Pressure']
def fill_from_Acceleration(o):
'''Fill messages with information from 'Acceleration' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
# FIXME not sure we should treat all in that same way
try:
x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
except KeyError:
pass
try:
x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
except KeyError:
pass
try:
x, y, z = o['accX'], o['accY'], o['accZ']
except KeyError:
pass
imu_msg.linear_acceleration.x = x
imu_msg.linear_acceleration.y = y
imu_msg.linear_acceleration.z = z
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0.,
0.0004)
def fill_from_Position(o):
'''Fill messages with information from 'Position' MTData2 block.'''
global pub_gps, xgps_msg, gps_msg
# TODO publish ECEF
try:
xgps_msg.latitude = gps_msg.latitude = o['lat']
xgps_msg.longitude = gps_msg.longitude = o['lon']
pub_gps = True
alt = o.get('altMsl', o.get('altEllipsoid', 0))
xgps_msg.altitude = gps_msg.altitude = alt
except KeyError:
pass
def fill_from_Angular_Velocity(o):
'''Fill messages with information from 'Angular Velocity' MTData2 block.'''
global pub_imu, imu_msg, pub_vel, vel_msg
try:
imu_msg.angular_velocity.x = o['gyrX']
imu_msg.angular_velocity.y = o['gyrY']
imu_msg.angular_velocity.z = o['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0.,
0., radians(0.025), 0.,
0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = o['gyrX']
vel_msg.twist.angular.y = o['gyrY']
vel_msg.twist.angular.z = o['gyrZ']
pub_vel = True
except KeyError:
pass
# TODO decide what to do with 'Delta q'
def fill_from_GPS(o):
'''Fill messages with information from 'GPS' MTData2 block.'''
global pub_gps, h, xgps_msg
try: # DOP
xgps_msg.gdop = o['gDOP']
xgps_msg.pdop = o['pDOP']
xgps_msg.hdop = o['hDOP']
xgps_msg.vdop = o['vDOP']
xgps_msg.tdop = o['tDOP']
pub_gps = True
except KeyError:
pass
try: # Time UTC
y, m, d, hr, mi, s, ns, f = o['year'], o['month'], o['day'],\
o['hour'], o['min'], o['sec'], o['nano'], o['valid']
if f & 0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO publish SV Info
def fill_from_SCR(o):
'''Fill messages with information from 'SCR' MTData2 block.'''
# TODO that's raw information
pass
def fill_from_Analog_In(o):
'''Fill messages with information from 'Analog In' MTData2 block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['analogIn1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['analogIn2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Magnetic(o):
'''Fill messages with information from 'Magnetic' MTData2 block.'''
global pub_mag, mag_msg
mag_msg.vector.x = o['magX']
mag_msg.vector.y = o['magY']
mag_msg.vector.z = o['magZ']
pub_mag = True
def fill_from_Velocity(o):
'''Fill messages with information from 'Velocity' MTData2 block.'''
global pub_vel, vel_msg
vel_msg.twist.linear.x = o['velX']
vel_msg.twist.linear.y = o['velY']
vel_msg.twist.linear.z = o['velZ']
pub_vel = True
def fill_from_Status(o):
'''Fill messages with information from 'Status' MTData2 block.'''
try:
status = o['StatusByte']
fill_from_Stat(status)
except KeyError:
pass
try:
status = o['StatusWord']
fill_from_Stat(status)
except KeyError:
pass
# TODO RSSI
def find_handler_name(name):
return "fill_from_%s" % (name.replace(" ", "_"))
# get data
data = self.mt.read_measurement()
# fill messages based on available data fields
for n, o in data:
try:
locals()[find_handler_name(n)](o)
except KeyError:
rospy.logwarn("Unknown MTi data packet: '%s', ignoring." % n)
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
if pub_press:
self.press_pub.publish(press_msg)
if pub_anin1:
self.analog_in1_pub.publish(anin1_msg)
if pub_anin2:
self.analog_in2_pub.publish(anin2_msg)
if pub_diag:
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
# publish string representation
self.str_pub.publish(str(data))
def add_sentence(self, nmea_string, frame_id, timestamp=None):
"""Public method to provide a new NMEA sentence to the driver.
Args:
nmea_string (str): NMEA sentence in string form.
frame_id (str): TF frame ID of the GPS receiver.
timestamp(rospy.Time, optional): Time the sentence was received.
If timestamp is not specified, the current time is used.
Returns:
bool: True if the NMEA string is successfully processed, False if there is an error.
"""
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
# else:
# rospy.logwarn("Valid sentence was: %s" % repr(nmea_string))
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
if not self.use_GNSS_time:
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with
# epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (2 * hdop *
self.alt_std_dev)**2 # FIXME
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * math.sin(
data['true_course'])
current_vel.twist.linear.y = data['speed'] * math.cos(
data['true_course'])
self.vel_pub.publish(current_vel)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide
# it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
elif 'SHR' in parsed_sentence:
data = parsed_sentence['SHR']
if data['heading']:
current_attitude = PoseWithCovarianceStamped()
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_attitude.header.stamp = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
else:
current_attitude.header.stamp = current_time
current_attitude.header.frame_id = frame_id
current_attitude.pose.pose.position.x = math.radians(
data['roll'])
current_attitude.pose.pose.position.y = math.radians(
data['pitch'])
current_attitude.pose.pose.position.z = math.radians(
data['heading'])
q = quaternion_from_euler(math.radians(data['roll']),
math.radians(data['pitch']),
math.radians(data['heading']))
current_attitude.pose.pose.orientation.x = q[0]
current_attitude.pose.pose.orientation.y = q[1]
current_attitude.pose.pose.orientation.z = q[2]
current_attitude.pose.pose.orientation.w = q[3]
current_attitude.pose.covariance[0] = data['gps_quality']
current_attitude.pose.covariance[1] = data['imu_state']
current_attitude.pose.covariance[21] = math.radians(
data['roll_accuracy'])
current_attitude.pose.covariance[28] = math.radians(
data['pitch_accuracy'])
current_attitude.pose.covariance[35] = math.radians(
data['heading_accuracy'])
self.attitude_pub.publish(current_attitude)
else:
return False
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0];
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2];
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as well.
if self.valid_fix:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif self.use_RMC and 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
if data['fix_valid']:
# Publish velocity
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
# Publish true course
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, data['true_course'])
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif not self.use_RMC and 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
else:
return False
def DataReader():
data = ReadFile(FILE_NAME)
# IMU
imu_msg = Imu()
imu_msg.orientation.x = data[0][1:-1,0]
imu_msg.orientation.y = data[0][1:-1,1]
imu_msg.orientation.z = data[0][1:-1,2]
imu_msg.orientation.w = data[0][1:-1,3]
imu_msg.linear_acceleration.x=data[0][1:-1,4]
imu_msg.linear_acceleration.y=data[0][1:-1,5]
imu_msg.linear_acceleration.z=data[0][1:-1,6]
imu_msg.angular_velocity.x=data[0][1:-1,7]
imu_msg.angular_velocity.y=data[0][1:-1,8]
imu_msg.angular_velocity.z=data[0][1:-1,9]
imu_time = data[0][1:-1,10]
imu_length = len(imu_msg.orientation.x)
imu_roll_angle=np.zeros(imu_length,dtype=float)
imu_pitch_angle=np.zeros(imu_length,dtype=float)
imu_yaw_angle=np.zeros(imu_length,dtype=float)
for i in range(imu_length):
x = imu_msg.orientation.x[i]
y = imu_msg.orientation.y[i]
z = imu_msg.orientation.z[i]
w = imu_msg.orientation.w[i]
imu_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI
imu_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI
imu_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI
# vehicle_state
vehicle_msg =Odometry()
vehicle_msg.pose.pose.position.x =data[1][1:-1,0]
vehicle_msg.pose.pose.position.y =data[1][1:-1,1]
vehicle_msg.pose.pose.position.z =data[1][1:-1,2]
vehicle_msg.pose.pose.orientation.x = data[1][1:-1,3]
vehicle_msg.pose.pose.orientation.y = data[1][1:-1,4]
vehicle_msg.pose.pose.orientation.z = data[1][1:-1,5]
vehicle_msg.pose.pose.orientation.w = data[1][1:-1,6]
vehicle_msg.twist.twist.linear.x= data[1][1:-1,7]
vehicle_msg.twist.twist.linear.y= data[1][1:-1,8]
vehicle_msg.twist.twist.linear.z= data[1][1:-1,9]
vehicle_msg.twist.twist.angular.x= data[1][1:-1,10]
vehicle_msg.twist.twist.angular.y= data[1][1:-1,11]
vehicle_msg.twist.twist.angular.z= data[1][1:-1,12]
vehicle_time = data[1][1:-1,13]
odom_length = len(vehicle_msg.pose.pose.orientation.x)
odom_roll_angle=np.zeros(odom_length,dtype=float)
odom_pitch_angle=np.zeros(odom_length,dtype=float)
odom_yaw_angle=np.zeros(odom_length,dtype=float)
for i in range(odom_length):
x = vehicle_msg.pose.pose.orientation.x[i]
y = vehicle_msg.pose.pose.orientation.y[i]
z = vehicle_msg.pose.pose.orientation.z[i]
w = vehicle_msg.pose.pose.orientation.w[i]
odom_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI
odom_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI
odom_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI
# GPS
pos_gps = NavSatFix()
pos_gps.latitude = data[2][1:-1,1]
pos_gps.longitude = data[2][1:-1,2]
pos_gps.altitude = data[2][1:-1,3]
gps_time = data[2][1:-1,4]
# chassis
chassis_msg_Steer = data[3][1:-1,0]
chassis_msg_Throttle = data[3][1:-1,1]
chassis_msg_Brake = data[3][1:-1,2]
chassis_msg_Wheel = data[3][1:-1,3]
chassis_msg_Speed = data[3][1:-1,4]
chassis_msg_BucketAngle = data[3][1:-1,5]
chassis_msg_EngineSpeed = data[3][1:-1,6]
chassis_msg_EngineTorque = data[3][1:-1,7]
chassis_time = data[3][1:-1,8]
plt.figure(1)
loc='center'
font_dict={'fontsize': 20,\
'fontweight' : 8.2,\
'verticalalignment': 'baseline',\
'horizontalalignment': loc}
plt.title('trajectory',fontdict=font_dict,loc=loc)
plt.plot(pos_gps.latitude,pos_gps.longitude, color='cyan', label='desired_trajectory')
plt.xlabel('latitude')
plt.ylabel('longitude')
plt.figure(2)
loc='center'
font_dict={'fontsize': 20,\
'fontweight' : 8.2,\
'verticalalignment': 'baseline',\
'horizontalalignment': loc}
plt.title('Pitch Angle Comparison',fontdict=font_dict,loc=loc)
plt.plot(vehicle_time,odom_pitch_angle, color='cyan', label='pitch angle from odom')
plt.plot(imu_time,-imu_pitch_angle, color='r', label='pitch angle from imu')
plt.legend()
plt.ylabel('pitch angle/degree')
plt.xlabel('time/s')
plt.figure(3)
loc='center'
font_dict={'fontsize': 20,\
'fontweight' : 8.2,\
'verticalalignment': 'baseline',\
'horizontalalignment': loc}
plt.title('trajectory',fontdict=font_dict,loc=loc)
plt.plot(vehicle_msg.pose.pose.position.x ,vehicle_msg.pose.pose.position.y, color='cyan', label='desired_trajectory')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()
import rospy
import math
import geopy
from geometry_msgs.msg import PoseStamped, Pose2D
from geopy.distance import VincentyDistance, vincenty
from sensor_msgs.msg import NavSatFix
# global local_target
local_target = Pose2D()
position_global = NavSatFix()
pose = PoseStamped()
def global_to_local(data):
# TODO: refactor
global local_target
# local_target = Pose2D()
lat = data.x
lon = data.y
print data
d = math.hypot(pose.pose.position.x, pose.pose.position.y)
bearing = math.degrees(
math.atan2(-pose.pose.position.x, -pose.pose.position.y))
if bearing < 0:
bearing += 360
cur = geopy.Point(position_global.latitude, position_global.longitude)
origin = VincentyDistance(meters=d).destination(cur, bearing)
from sensor_msgs.msg import NavSatFix, BatteryState
from inspector_software_uav.srv import *
from uav_abstraction_layer.srv import *
from uav_abstraction_layer.msg import *
import PAL_clients as pal
import geo
# Global variables
stop_flag = False
auto_download = False
loaded_mission = False
flight_status = UInt8()
current_pos = PointStamped()
current_gps_pos = NavSatFix()
battery_state = BatteryState()
current_vel = Vector3Stamped()
ual_state = UInt8()
last_ual_state = UInt8()
adl_state = String()
## Json files with mission information
mission_status_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_status.json'
mission_waypoints_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_wps.json'
mission_data_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_data.json'
print mission_waypoints_file
## PAL srv Clients ##
def setUp(self):
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.imu_data = Imu()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.mission_wp = WaypointList()
self.state = State()
self.mav_type = None
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state', 'imu'
]
}
# ROS services
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
self.fail("failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.imu_data_sub = rospy.Subscriber('mavros/imu/data', Imu,
self.imu_data_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
def waterlinked(base_url):
rospy.init_node('waterlinked', anonymous=True)
rate = rospy.Rate(40) # 40hz
raw_pub = rospy.Publisher('/waterlinked/acoustic_position/raw',
PointStamped, queue_size=120)
filtered_pub = \
rospy.Publisher('/waterlinked/acoustic_position/filtered',
PointStamped, queue_size=120)
global_pub = rospy.Publisher('/waterlinked/global_position',
NavSatFix, queue_size=120)
def publish_raw(data_raw):
raw_point = PointStamped()
raw_point.header.frame_id = "/map"
raw_point.header.stamp = rospy.Time.now()
raw_point.point.x = data_raw['x']
raw_point.point.y = data_raw['y']
raw_point.point.z = data_raw['z']
raw_pub.publish(raw_point)
def publish_filtered(data_filtered):
filtered_point = PointStamped()
filtered_point.header.frame_id = "/map"
filtered_point.header.stamp = rospy.Time.now()
filtered_point.point.x = data_filtered['x']
filtered_point.point.y = data_filtered['y']
filtered_point.point.z = data_filtered['z']
filtered_pub.publish(filtered_point)
last_raw, last_filtered = None, None
while not rospy.is_shutdown():
data_raw = get_acoustic_position_raw(base_url)
if last_raw is None:
publish_raw(data_raw)
last_raw = data_raw
else:
dist = (data_raw['x'] - last_raw['x']) ** 2 +\
(data_raw['y'] - last_raw['y']) ** 2 +\
(data_raw['z'] - last_raw['z']) ** 2
if dist > 1e-10:
publish_raw(data_raw)
last_raw = data_raw
data_filtered = get_acoustic_position_filtered(base_url)
if last_filtered is None:
publish_filtered(data_filtered)
last_filtered = data_filtered
else:
dist = (data_filtered['x'] - last_filtered['x']) ** 2 +\
(data_filtered['y'] - last_filtered['y']) ** 2 +\
(data_filtered['z'] - last_filtered['z']) ** 2
if dist > 1e-10:
publish_filtered(data_filtered)
last_filtered = data_filtered
data_global = get_global_position(base_url)
global_point = NavSatFix()
global_point.header.stamp = rospy.Time.now()
global_point.header.frame_id = "/map"
global_point.latitude = data_global['lat']
global_point.longitude = data_global['lon']
global_pub.publish(global_point)
rate.sleep()
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int8
from sensor_msgs.msg import NavSatFix
from sklearn.cluster import MeanShift
#from threading import Thread
lat_offset = 26.19
lon_offset = 91.69
gps_coordinates = []
box_coordinates = []
box_gps_msg = NavSatFix()
obj_found_1 = 56
def obj_number_cb1(data_temp):
print("obj data updated")
global obj_found_1
obj_found_1 = int(data_temp.data)
print(obj_found_1)
def gps_cb1(data):
global obj_found_1, lat_offset, lon_offset
print("gps called")
print("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ", obj_found_1)
if (obj_found_1 == 1):
print("*****************object found ******************")
gps_coordinates.append((int((data.latitude - lat_offset) * 10000000),
int((data.longitude - lon_offset) * 1000000)))
def control():
pass
def commuciation_check():
pass
def takeoff(height):
pass
if __name__ == '__main__':
self_position = NavSatFix()
neighbour_position = gpsPosition()
rospy.init_node('px4ros_leader_control', anonymous=True)
pub = rospy.Publisher('serial_data_send', String, queue_size=20)
rate = rospy.Rate(10) # 10hz
#subscribe info from mavros
rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, callback1)
#subscribe info from leader_control_decode
rospy.Subscriber("neighbour_position", gpsPosition, callback2)
while not rospy.is_shutdown():
#pub.publish(globalPosition)
#rospy.loginfo("%s",globalPosition)
def talker():
global global_g, deg_rad, Heading_HPD
ser.flush()
pub_m = rospy.Publisher('/imu/data_raw', Imu, queue_size=100)
pub = rospy.Publisher('comb', comb, queue_size=100)
pub_n = rospy.Publisher('comb_now', comb, queue_size=100)
pub_g = rospy.Publisher('gps', comb, queue_size=100)
pub_o = rospy.Publisher('/gps/fix', NavSatFix, queue_size=100)
rospy.init_node('comb_node', anonymous=True)
rate = rospy.Rate(100) # 10hz
imuMsg = Imu()
combMsg = comb()
gpsMsg = comb()
gpsmeanMsg = NavSatFix()
seq = 0
while not rospy.is_shutdown():
# $GTIMU,GPSWeek,GPSTime,GyroX,GyroY,GyroZ,AccX,AccY,AccZ,Tpr*cs<CR><LF>
if ser.read() != '$':
continue
line0 = ser.readline()
print line0
cs = line0[-4:-2]
print cs
for s_tmp in line0[6:-8]:
if s_tmp.isalpha():
continue
try:
cs1 = int(cs, 16)
except:
continue
cs2 = checksum(line0)
print("cs1,cs2", cs1, cs2)
if cs1 != cs2:
continue
if line0[0:5] == "GTIMU":
line = line0.replace("GTIMU,", "")
line = line.replace("\r\n", "")
if "\x00" in line:
continue
if not string.find('*', line):
continue
line = line.replace("*", ",")
words = string.split(line, ",")
if len(words) != 10:
continue
GyroX = string.atof(words[2])
GyroY = string.atof(words[3])
GyroZ = string.atof(words[4])
AccX = string.atof(words[5])
AccY = string.atof(words[6])
AccZ = string.atof(words[7])
imuMsg.linear_acceleration.x = AccX * global_g
imuMsg.linear_acceleration.y = AccY * global_g
imuMsg.linear_acceleration.z = AccZ * global_g
imuMsg.linear_acceleration_covariance[0] = 0.0001
imuMsg.linear_acceleration_covariance[4] = 0.0001
imuMsg.linear_acceleration_covariance[8] = 0.0001
imuMsg.angular_velocity.x = GyroX * deg_rad
imuMsg.angular_velocity.y = GyroY * deg_rad
imuMsg.angular_velocity.z = GyroZ * deg_rad
imuMsg.angular_velocity_covariance[0] = 0.0001
imuMsg.angular_velocity_covariance[4] = 0.0001
imuMsg.angular_velocity_covariance[8] = 0.0001
q = quaternion_from_euler(0, 0, 0)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'imu'
imuMsg.header.seq = seq
seq = seq + 1
pub_m.publish(imuMsg)
rate.sleep()
elif line0[0:5] == "GPFPD":
line = line0.replace("GPFPD,", "")
line = line.replace("\r\n", "")
if ("\x00" in line):
continue
if (not string.find('*', line)):
continue
line = line.replace("*", ",")
words = string.split(line, ",")
if len(words) != 16:
continue
GPSWeek = string.atoi(words[0])
GPSTime = string.atof(words[1])
Heading = string.atof(words[2])
Pitch = string.atof(words[3])
Roll = string.atof(words[4])
Latitude = string.atof(words[5])
Longitude = string.atof(words[6])
Altitude = string.atof(words[7])
Ve = string.atof(words[8])
Vn = string.atof(words[9])
Vu = string.atof(words[10])
Baseline = string.atof(words[11])
NSV1 = string.atoi(words[12])
NSV2 = string.atoi(words[13])
Status = words[14]
if Status == '03':
Vne = math.sqrt(Vn * Vn + Ve * Ve)
if (Vne > 0.3 and Ve < 0):
Heading = math.acos(Vn / Vne)
else:
Heading = 2 * 3.141592658 - math.acos(Vn / Vne)
combMsg.GPSWeek = GPSWeek
combMsg.GPSTime = GPSTime
combMsg.Heading = Heading
combMsg.Pitch = Pitch
combMsg.Roll = Roll
combMsg.Latitude = Latitude
combMsg.Longitude = Longitude
combMsg.Altitude = Altitude
combMsg.Ve = Ve
combMsg.Vn = Vn
combMsg.Vu = Vu
combMsg.Baseline = Baseline
combMsg.NSV1 = NSV1
combMsg.NSV2 = NSV2
combMsg.Status = Status
combMsg.header.stamp = rospy.Time.now()
combMsg.header.frame_id = 'gps'
combMsg.header.seq = seq
pub_n.publish(combMsg)
print("pub comb_now")
if Status == '03' or Status == '04' or Status == '05' or Status == '08' or Status == '0B':
pub.publish(combMsg)
print("pub comb")
seq += 1
rate.sleep()
# $GPHPD, GPSWeek, GPSTime, Heading, Pitch, Track, Latitude, Longitude, Altitude, Ve , Vn, Vu,Baseline, NSV1, NSV2*cs<CR><LF>
elif line0[0:5] == "GPHPD":
line = line0.replace("GPHPD,", "")
line = line.replace("\r\n", "")
if ("\x00" in line):
continue
if (not string.find('*', line)):
continue
line = line.replace("*", ",")
words = string.split(line, ",")
if len(words) != 16:
continue
Status = words[14]
GPSWeek = string.atoi(words[0])
GPSTime = string.atof(words[1])
Heading_HPD = string.atof(words[2])
Heading_HPD = 360 - Heading_HPD
Pitch = string.atof(words[3])
Track = string.atof(words[4])
Latitude = string.atof(words[5])
Longitude = string.atof(words[6])
Altitude = string.atof(words[7])
Ve = string.atof(words[8])
Vn = string.atof(words[9])
Vu = string.atof(words[10])
Baseline = string.atof(words[11])
NSV1 = string.atoi(words[12])
NSV2 = string.atoi(words[13])
gpsMsg.GPSWeek = GPSWeek
gpsMsg.GPSTime = GPSTime
gpsMsg.Heading = Heading_HPD
gpsMsg.Pitch = Pitch
gpsMsg.Roll = Track
gpsMsg.Latitude = Latitude
gpsMsg.Longitude = Longitude
gpsMsg.Altitude = Altitude
gpsMsg.Ve = Ve
gpsMsg.Vn = Vn
gpsMsg.Vu = Vu
gpsMsg.Baseline = Baseline
gpsMsg.NSV1 = NSV1
gpsMsg.NSV2 = NSV2
gpsMsg.Status = Status
gpsMsg.header.stamp = rospy.Time.now()
gpsMsg.header.frame_id = 'gps'
gpsMsg.header.seq = seq
pub_g.publish(gpsMsg)
print("pub gps")
gpsmeanMsg.header.stamp = rospy.Time.now()
gpsmeanMsg.header.frame_id = 'gps'
gpsmeanMsg.header.seq = seq
if Status == '05':
gpsmeanMsg.status.status = 2
covariance_xyz = 0.05**2
elif Status == '03':
gpsmeanMsg.status.status = 0
covariance_xyz = 5**2
else:
gpsmeanMsg.status.status = -1
covariance_xyz = 99999
gpsmeanMsg.status.service = 1
gpsmeanMsg.latitude = Latitude
gpsmeanMsg.longitude = Longitude
gpsmeanMsg.altitude = Latitude
gpsmeanMsg.position_covariance = [
covariance_xyz, 0, 0, 0, covariance_xyz, 0, 0, 0,
covariance_xyz
]
gpsmeanMsg.position_covariance_type = 3
pub_o.publish(gpsmeanMsg)
print("pub gps_mean")
seq += 1
rate.sleep()
else:
continue
def bestpos_handler(self, bestpos):
navsat = NavSatFix()
# TODO: The timestamp here should come from SPAN, not the ROS system time.
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
# Assume GPS - this isn't exposed
navsat.status.service = NavSatStatus.SERVICE_GPS
position_type_to_status = {
BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX,
BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP:
NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP:
NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING:
NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX,
}
navsat.status.status = position_type_to_status.get(
bestpos.position_type, NavSatStatus.STATUS_NO_FIX)
# Position in degrees.
navsat.latitude = bestpos.latitude
navsat.longitude = bestpos.longitude
# Altitude in metres.
navsat.altitude = bestpos.altitude
navsat.position_covariance[0] = pow(bestpos.latitude_std, 2)
navsat.position_covariance[4] = pow(bestpos.longitude_std, 2)
navsat.position_covariance[8] = pow(bestpos.altitude_std, 2)
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# Ship ito
self.pub_navsatfix.publish(navsat)
def setUp(self):
print('@ctrl_server@ setUp')
self.pos = PoseStamped()
self.altitude = Altitude()
self.extended_state = ExtendedState()
self.global_position = NavSatFix()
self.home_position = HomePosition()
self.local_position = PoseStamped()
self.misson_wp = WaypointList()
self.state = State()
self.scan = LaserScan()
self.mav_type = None
self.ready = False
# Target offset radius
self.radius = 0.25
self.sub_topics_ready = {
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state', 'scan'
]
}
# ROS services
service_timeout = 60
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/mission/push', service_timeout)
rospy.wait_for_service('mavros/mission/clear', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
print("@ctrl_server@ failed to connect to services")
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet)
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear',
WaypointClear)
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push',
WaypointPush)
# ROS subscribers
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude,
self.altitude_callback)
self.ext_state_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
self.extended_state_callback)
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',
NavSatFix,
self.global_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints',
WaypointList,
self.mission_wp_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.get_scan_srv = rospy.Subscriber('/laser/scan', LaserScan,
self.scan_callback)
# ROS publisers
self.pos_setpoint_pub = rospy.Publisher(
'mavros/setpoint_position/local', PoseStamped, queue_size=1)
# send setpoints in seperate thread
self.pos_thread = Thread(target=self.send_pos, args=())
self.pos_thread.daemon = True
self.pos_thread.start()
print('@ctrl_server@ setUp over')
pass
def add_sentence(self, nmea_string, frame_id, timestamp=None):
"""Public method to provide a new NMEA sentence to the driver.
Args:
nmea_string (str): NMEA sentence in string form.
frame_id (str): TF frame ID of the GPS receiver.
timestamp(rospy.Time, optional): Time the sentence was received.
If timestamp is not specified, the current time is used.
Returns:
bool: True if the NMEA string is successfully processed, False if there is an error.
"""
if not check_nmea_checksum(nmea_string):
#rospy.logwarn("Received a sentence with an invalid checksum. " +
# "Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
#rospy.logdebug(
# "Failed to parse NMEA sentence. Sentence was: %s" %
# nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
if not self.use_GNSS_time:
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
data = parsed_sentence['GGA']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1])
gps_qual = data['fix_type']
if gps_qual == 0:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_qual == 1:
current_fix.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
elif gps_qual == 9:
# Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
# http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
hdop = data['hdop']
current_fix.position_covariance[0] = hdop ** 2
current_fix.position_covariance[4] = hdop ** 2
current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
#rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1])
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide
# it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
def __init__(self, namespace='sbg_raw_data_simulator'):
"""
Initialize the publishers and subscribes to necessary channels
`self.gps` mimics the gps data of the sbg
`self.imu` mimics the imu data of the sbg
Raw imu simulator parameters in: eufs_description/sensors/imu.urdf.xacro
Raw gps simulator parameters in: eufs_description/sensors/gps.urdf.xacro
Parameters:
_imu_hz: the hz at which to broadcast the imu data, note that this
has implications for accuracy as the noise models are affected by
sample rate!
_gps_hz: the hz at which to broadcast the gps data, has no accuracy implications
Highly recommended to read the wiki
https://www.wiki.ed.ac.uk/display/EUF/SBG+Sensor+Simulation
to understand why the noise is produced the way it is.
"""
rospy.init_node("sbg_raw_data_simulator", anonymous=True)
# Create persistent messages
self.imu_msg = Imu()
self.gps_msg = NavSatFix()
# Create bias error accumulator, only relevant for the imu
self.imu_angular_velocity_bias = [0, 0, 0]
# Don't publish anything if not received messages yet
self.got_imu = False
self.got_gps = False
# Latitude & longitude to meters
# Approximations about Edinburgh 55.950191, -3.187566
# Values represent distance you have to travel to equal 1 meter.
self.latitude_approx = 0.000018
self.longitude_approx = 0.000016
# Publish at fixed rates
self.IMU_PUBLISH_RATE_HZ = rospy.get_param("~imu_hz", default=200)
self.GPS_PUBLISH_RATE_HZ = rospy.get_param("~gps_hz", default=5)
self.gps = rospy.Publisher("/imu/nav_sat_fix", NavSatFix, queue_size=1)
self.imu = rospy.Publisher("/imu/data", Imu, queue_size=1)
rospy.Timer(
rospy.Duration(1.0 / self.IMU_PUBLISH_RATE_HZ),
lambda x: self.imu.publish(self.imu_msg)
if self.got_imu and self.got_gps else None)
rospy.Timer(
rospy.Duration(1.0 / self.GPS_PUBLISH_RATE_HZ),
lambda x: self.gps.publish(self.gps_msg)
if self.got_imu and self.got_gps else None)
# Set calculated standard distribution information
# check https://www.wiki.ed.ac.uk/display/EUF/SBG+Sensor+Simulation
# for method and reasons.
# Note that we had trouble calculating bias constant so for now we're using
# the white noise constant for bias as well.
self.imu_angular_velocity_white_noise_std = math.sqrt(
0.00000625 * self.IMU_PUBLISH_RATE_HZ)
self.imu_angular_velocity_bias_std = math.sqrt(
1.476 * math.pow(10, -13) / self.IMU_PUBLISH_RATE_HZ)
self.imu_linear_acceleration_white_noise_std = math.sqrt(
0.000000312 * self.IMU_PUBLISH_RATE_HZ)
self.gps_position_white_noise_std = math.sqrt(39.4)
# Subscribe to channels
self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_receiver)
self.gps_sub = rospy.Subscriber("/gps", NavSatFix, self.gps_receiver)
class bt_missions:
rospy.init_node('aero_pose_python')
mavros.set_namespace()
rate = rospy.Rate(20)
orb_ = PoseStamped()
dirct_ = Float64()
points_ = PointCloud2()
home_ = HomePosition()
current_state = State()
current_ = NavSatFix()
alt_ = Float64()
fourcc = cv2.VideoWriter_fourcc('X', 'V', 'I', 'D')
out = cv2.VideoWriter('realsense.avi', fourcc, 20.0, (640, 480))
home_set_ = False
isContinue = True
orb_scale = False
target = [[24.982550663278158, 121.57233949235275],
[24.982091, 121.571846], [24.981758, 121.571963]]
index = 0
current_posx = 0
dist_first = True
start_heading = 0
p0 = 0
p1 = 0
brng = 0
dLon = 0
y = 0
x = 0
current_yaw = 0
current_alt = 0
dist = 0
pose_dist = 0
heading = 0
lat = 0
lon = 0
cnt = 0
alt_count = 0
alt_count2 = 0
ah_alt = 0
z1 = 0
z2 = 0
z3 = 0
z4 = 0
scale = 0
ah_flag = False
img_count = 0
query_count = 0
pose_flag = True
move_flag = False
record_img = np.array([])
set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
set_v_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position',
'local'),
PoseStamped,
queue_size=10)
def __init__(self):
self.orb_sub = rospy.Subscriber("/orb_slam2_mono/pose", PoseStamped,
self.orb_pose)
self.hdg_sub = rospy.Subscriber("/mavros/global_position/compass_hdg",
Float64, self.compass)
self.rel_alt = rospy.Subscriber("/mavros/global_position/rel_alt",
Float64, self.altitude)
self.home_sub = rospy.Subscriber("/mavros/home_position/home",
HomePosition, self.setHomeGeoPointCB)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global",
NavSatFix, self.current_position_cb)
self.state_sub = rospy.Subscriber(mavros.get_topic('state'), State,
self.state_cb)
self.img_sub = rospy.Subscriber("/camera/rgb/image_raw",
Image,
self.call,
buff_size=2**24,
queue_size=1)
self.point_sub = rospy.Subscriber("/orb_slam2_mono/map_points",
PointCloud2, self.pointcloud_cb)
#self.mono_sub = rospy.Subscriber("/camera/mono/image_raw", Image, self.mono_cb, buff_size = 2**24, queue_size = 1)
self.tree = self.correction
#(self.achieve | ((self.achieve_alt | self.up) >> (self.yaw_correction | self.correction) >> (self.get_scale | self.up_scale) >> self.forward)) >> (self.ah | (self.isdetect >> self.change_alt))
def orb_pose(self, pose):
bt_missions.orb_ = pose
def pointcloud_cb(self, points):
bt_missions.points_ = points
def compass(self, dirct):
bt_missions.dirct_ = dirct
bt_missions.heading = bt_missions.dirct_.data
def state_cb(self, state):
bt_missions.current_state = State()
bt_missions.current_state = state
def altitude(self, alt):
bt_missions.alt_ = alt
bt_missions.current_alt = bt_missions.alt_.data
rospy.loginfo("Received current altitude : %s" %
bt_missions.current_alt)
def setHomeGeoPointCB(self, home):
bt_missions.home_ = home
bt_missions.home_set_ = True
rospy.loginfo(
"Received Home (WGS84 datum): %lf, %lf, %lf" %
(bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude,
bt_missions.home_.geo.altitude))
def current_position_cb(self, current):
bt_missions.current_ = current
bt_missions.lat = bt_missions.current_.latitude
bt_missions.lon = bt_missions.current_.longitude
rospy.loginfo("Received current GPS : %lf, %lf" %
(bt_missions.lat, bt_missions.lon))
def call(self, callimage):
bt_missions.record_img = CvBridge().imgmsg_to_cv2(callimage, "bgr8")
bt_missions.out.write(bt_missions.record_img)
@condition
def ah(self):
if bt_missions.ah_flag == True:
return False
else:
return True
@condition
def isdetect(self):
bt_missions.query_count = bt_missions.query_count + 1
img1 = cv2.imread('query/' + str(bt_missions.query_count) + '.PNG',
cv2.IMREAD_GRAYSCALE) #queryImage
bt_missions.img_count = bt_missions.img_count + 1
cv2.imwrite('image/' + str(bt_missions.img_count) + '.PNG',
bt_missions.record_img)
img2 = cv2.imread('image/' + str(bt_missions.img_count) + '.PNG',
cv2.IMREAD_GRAYSCALE) #trainImage
# Initiate SIFT detector
sift = cv2.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
# BFMatcher with default params
bf = cv2.BFMatcher()
t = rospy.get_time()
while (rospy.get_time() - t <= 2):
matches = bf.knnMatch(des1, des2, k=2)
# Apply ratio test
good = []
for m, n in matches:
if m.distance < 0.55 * n.distance:
good.append([m])
print("threshold: %s" % len(good))
if len(good) >= 20:
bt_missions.index = bt_missions.index + 1
bt_missions.img_count = 0
return True
else:
return False
@condition
def lose_tracking(self):
if bt_missions.orb_.pose.position.z == 0:
return False
else:
return True
@condition
def get_scale(self):
if bt_missions.orb_scale == True:
return True
else:
return False
@condition
def achieve_alt(self):
if bt_missions.current_alt > 15:
print("altitude enough")
return True
else:
print("altitude not enough")
return False
@condition
def achieve(self):
if bt_missions.index == 0 and bt_missions.dist_first == True:
bt_missions.dist_first = False
#GPS distance
bt_missions.p0 = (bt_missions.home_.geo.latitude,
bt_missions.home_.geo.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print("current Gps dist: %s" % bt_missions.dist)
#Pose distance
bt_missions.pose_dist = bt_missions.scale * bt_missions.dist
print("current pose dist: %s" % bt_missions.pose_dist)
if bt_missions.pose_dist >= 0.02:
return False
elif bt_missions.pose_dist == 0:
return False
else:
bt_missions.index = bt_missions.index + 1
elif bt_missions.index == 0 and bt_missions.dist_first == False:
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print("current Gps dist: %s" % bt_missions.dist)
#Pose distance
bt_missions.pose_dist = bt_missions.scale * bt_missions.dist
print("current pose dist: %s" % bt_missions.pose_dist)
if bt_missions.pose_dist >= 0.02:
return False
elif bt_missions.pose_dist == 0:
return False
else:
print("achieve first point")
bt_missions.move_flag = False
rospy.loginfo("first point's GPS : %lf, %lf, %lf" %
(bt_missions.lat, bt_missions.lon,
bt_missions.current_alt))
bt_missions.index = bt_missions.index + 1
return True
elif bt_missions.index == 1 and bt_missions.dist_first == False:
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
print("current Gps dist: %s" % bt_missions.dist)
#Pose distance
bt_missions.pose_dist = bt_missions.scale * bt_missions.dist
print("current pose dist: %s" % bt_missions.pose_dist)
if bt_missions.pose_dist >= 0.02:
return False
else:
print("achieve key point")
bt_missions.move_flag = False
bt_missions.ah_flag = True
rospy.loginfo("keypoint point's GPS : %lf, %lf, %lf" %
(bt_missions.lat, bt_missions.lon,
bt_missions.current_alt))
return True
@condition
def yaw_correction(self):
if bt_missions.index == 0:
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.home_.geo.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.home_.geo.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
print("correction heading: %s" % bt_missions.current_yaw)
print("correction brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 -
bt_missions.brng) + 2.0 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2.0:
return False
else:
return True
else:
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.current_.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.current_.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.current_.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
print("correction heading: %s" % bt_missions.current_yaw)
print("correction brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 -
bt_missions.brng) + 2.0 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2.0:
return False
else:
return True
@action
def up_scale(self):
bt_missions.start_heading = bt_missions.heading
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.x = 0.0
msg.twist.linear.x = 0.0
msg.twist.angular.y = 0.0
msg.twist.linear.y = 0.0
msg.twist.angular.z = 0.0
msg.twist.linear.z = 0.0
t = rospy.get_time()
while rospy.get_time() - t <= 2:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.rate.sleep()
bt_missions.alt_count = bt_missions.current_alt
bt_missions.z1 = bt_missions.orb_.pose.position.z
print("pose z1 :%s" % bt_missions.z1)
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.z = 0.0
msg.twist.linear.z = 0.5
while bt_missions.current_alt - bt_missions.alt_count <= 5:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.rate.sleep()
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.x = 0.0
msg.twist.linear.x = 0.0
msg.twist.angular.y = 0.0
msg.twist.linear.y = 0.0
msg.twist.angular.z = 0.0
msg.twist.linear.z = 0.0
t = rospy.get_time()
while rospy.get_time() - t <= 2:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.rate.sleep()
bt_missions.alt_count2 = bt_missions.current_alt
print("current altitude : %s" % bt_missions.alt_count2)
bt_missions.z2 = bt_missions.orb_.pose.position.z
print("pose z2 :%s" % bt_missions.z2)
bt_missions.scale = math.fabs(bt_missions.z2 - bt_missions.z1) / (
bt_missions.alt_count2 - bt_missions.alt_count)
print("scale one: %s" % bt_missions.scale)
bt_missions.orb_scale = True
@action
def correction(self):
#orb lost tracking
if bt_missions.orb_.pose.position.z == 0 and bt_missions.orb_.pose.position.y == 0 and bt_missions.orb_.pose.position.x == 0:
bt_missions.current_yaw = bt_missions.heading
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.linear.z = 0
bt_missions.current_yaw = bt_missions.heading
if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180:
msg.twist.angular.z = 0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng - bt_missions.current_yaw) >= 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng - bt_missions.current_yaw) < 180:
msg.twist.angular.z = 0.1
bt_missions.current_yaw = bt_missions.heading
while (bt_missions.current_yaw >
(360 - bt_missions.brng) + 2.0 or bt_missions.current_yaw <
(360 - bt_missions.brng) - 2.0) and (
bt_missions.orb_.pose.position.z != 0
and bt_missions.orb_.pose.position.y != 0
and bt_missions.orb_.pose.position.x != 0):
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.current_yaw = bt_missions.heading
bt_missions.rate.sleep()
print("first correction heading : %s" % bt_missions.current_yaw)
if bt_missions.orb_.pose.position.z == 0 and bt_missions.orb_.pose.position.y == 0 and bt_missions.orb_.pose.position.x == 0:
print("orb lost tracking")
bt_missions.current_yaw = bt_missions.heading
bt_missions.dLon = (bt_missions.target[bt_missions.index - 1][1] -
bt_missions.current_.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index - 1][0])
bt_missions.x = math.cos(bt_missions.current_.latitude) * math.sin(
bt_missions.target[bt_missions.index - 1][0]) - math.sin(
bt_missions.current_.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.linear.z = 0
bt_missions.current_yaw = bt_missions.heading
if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180:
msg.twist.angular.z = 0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng - bt_missions.current_yaw) >= 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng - bt_missions.current_yaw) < 180:
msg.twist.angular.z = 0.1
bt_missions.current_yaw = bt_missions.heading
while bt_missions.current_yaw > (
360 -
bt_missions.brng) + 2.0 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2.0:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.current_yaw = bt_missions.heading
bt_missions.rate.sleep()
print("orb correction heading : %s" % bt_missions.current_yaw)
@action
def up(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.linear.z = 0.8
while bt_missions.current_alt <= 15:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.rate.sleep()
print("current altitude : %s" % bt_missions.current_alt)
@action
def forward(self):
bt_missions.move_flag = True
#body_frame
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.z = 0.0
print("forward")
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0.0
vel.velocity.y = 1.0
vel.velocity.z = 0.0
forward_t = rospy.get_time()
while rospy.get_time() - forward_t <= bt_missions.dist * 0.1:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.set_v_pub.publish(vel)
bt_missions.rate.sleep()
@action
def change_alt(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.linear.z = 0.8
while bt_missions.current_alt <= 35:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.rate.sleep()
print("current altitude : %s" % bt_missions.current_alt)
def run(self):
while True:
if bt_missions.isContinue == False:
break
bt_count = self.tree.blackboard(1)
bt_state = bt_count.tick()
print("bt state = %s\n" % bt_state)
while bt_count == RUNNING:
bt_state = bt_count.tick()
print("state = %s\n" % bt_state)
assert bt_state == SUCCESS or bt_state == FAILURE
def auto_demo():
rospy.init_node('autonomous_demo')
#Requirements
pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size =10)
pub2 = rospy.Publisher('/imu/data', Imu ,queue_size = 10)
pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size = 10)
pub4 = rospy.Publisher('/gps_waypoint_handler/status', String, queue_size = 10)
#Autonomous movement
#pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10)
#Image Detect
pub6 = rospy.Publisher('/image_detect_topic',String,queue_size=10)
#Image Reach
pub7 = rospy.Publisher('/image_reach_topic',String,queue_size=10)
rate = rospy.Rate(10) # 10hz
count = 0
while not rospy.is_shutdown():
print("0: All sensors are good.")
print("1: All sensors except encoder are good.")
print("2: Damaged Sensors")
print("3: Got Waypoint")
print("4: Did not get any waypoint")
print("5: Reached to way point")
print("6: Did not Reached to way point")
print("7: Detected Image")
print("8: Did not detect Image")
print("9: Reached Image")
print("10: Did not reached image")
imuMsg = Imu()
imuMsg.orientation.x = 5
imuMsg.orientation.y = 5
gpsMsg = NavSatFix()
gpsMsg.latitude = 5
gpsMsg.longitude = 5
encoderMsg = Odometry()
encoderMsg.pose.pose.position.x = 5
encoderMsg.pose.pose.position.y = 5
wpStatusMsgLow = GoalStatus()
wpStatusMsgLow.status = 3
wpStatusArray =[]
wpStatusArray.append(wpStatusMsgLow)
wpStatusMsg = GoalStatusArray()
wpStatusMsg.status_list = wpStatusArray
userInput = raw_input()
if userInput == "0": #All sensors are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
pub3.publish(encoderMsg)
elif userInput == "1": # All sensors except encoder are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
#pub3.publish("0")
elif userInput == "2": #Damaged Sensors
pub1.publish(gpsMsg)
#pub2.publish("0")
pub3.publish(encoderMsg)
elif userInput == "3": # Got Waypoint
pub4.publish("1")
#elif userInput == "4": #Did not get any waypoint
#pub4.publish("0")
elif userInput == "5": #Reached to way point
#pub5.publish(wpStatusMsg)
pub4.publish("2")
#elif userInput == "6": #Did not reached to way point
#pub5.publish("0")
elif userInput == "7": #Detected Image
pub6.publish("1")
elif userInput == "8": #Did not Detect Image
pub6.publish("0")
elif userInput == "9": #Reached Image
pub7.publish("1")
elif userInput == "10": #Did not reached image
pub7.publish("0")
else:
print("Invalid entry")
rate.sleep();
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
if gps_qual == 0:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_qual == 1:
current_fix.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
hdop = data['hdop']
current_fix.position_covariance[0] = hdop**2
current_fix.position_covariance[4] = hdop**2
current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import NavSatFix
from math import *
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
ob1 = Twist()
ob2 = NavSatFix()
ob3 = NavSatFix()
def arduino_map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
def cartesian_distance(lat1, lon1, lat2, lon2):
return sqrt((lat2 - lat1) ** 2 + (lon2 - lon1) ** 2)
def haversine(lat1, lon1, lat2, lon2):
# distance between latitudes
# and longitudes
dLat = (lat2 - lat1) * pi / 180.0
dLon = (lon2 - lon1) * pi / 180.0
# convert to radians
lat1 = lat1 * pi / 180.0
lat2 = lat2 * pi / 180.0
