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 init_path():
# logitude, latitute, altitute, orientation (+deg)
gpsPub = rospy.Publisher("/fix_path_init", NavSatFix, queue_size=10)
wpSub = rospy.Subscriber("/gps_path_init", Odometry, utm_cb)
navStatus = NavSatStatus()
navStatus.service = 1
navStatus.status = 1
# waypoints hardcoded for now, may want to spec with argument in future
num_wp = gps_waypoints.shape[0]
hdop = 1.5
position_covariance_type = 1
global wp_num
for i in range(0, num_wp):
# send gps waypoint for conversion
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base_link'
msgOut = NavSatFix()
msgOut.position_covariance[0] = hdop**2
msgOut.position_covariance[4] = hdop**2
msgOut.position_covariance[8] = (2 * hdop)**2
msgOut.longitude = gps_waypoints[i][0]
msgOut.latitude = gps_waypoints[i][1]
msgOut.altitude = gps_waypoints[i][2]
msgOut.position_covariance_type = position_covariance_type
msgOut.status = navStatus
msgOut.header = header
wp_num = i
gpsPub.publish(msgOut)
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 publish_gps(event):
global origin_lat, origin_lon, first
#It may take a second or two to get good data
#print gpsd.fix.latitude,', ',gpsd.fix.longitude,' Time: ',gpsd.utc
# os.system('clear')
# print
# print ' GPS reading'
# print '----------------------------------------'
# print 'latitude ' , gpsd.fix.latitude
# print 'longitude ' , gpsd.fix.longitude
# print 'time utc ' , gpsd.utc,' + ', gpsd.fix.time
# print 'altitude (m)' , gpsd.fix.altitude
# print 'epx ' , gpsd.fix.epx
# print 'epv ' , gpsd.fix.epv
# print 'ept ' , gpsd.fix.ept
# print 'speed (m/s) ' , gpsd.fix.speed
# print 'climb ' , gpsd.fix.climb
# print 'track ' , gpsd.fix.track
# print 'mode ' , gpsd.fix.mode
# print
# print 'sats ' , gpsd.satellites
navsat = NavSatFix()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "base_link"
navsat.header = header
navsat_status = NavSatStatus()
navsat_status.status = 0
navsat_status.service = 1
navsat.status = navsat_status
navsat.latitude = gpsd.fix.latitude
navsat.longitude = gpsd.fix.longitude
navsat.altitude = gpsd.fix.altitude
navsat.position_covariance_type = 2
navsat.position_covariance = [2.5, 0, 0,
0, 2.5, 0,
0, 0, 2.5]
if not origin_lat and not origin_lon:
origin_lat = gpsd.fix.latitude
origin_lon = gpsd.fix.longitude
# print ('Odometry: ')
# print (x, y)
pub_navsat.publish(navsat)
def read_rtk_gps():
s = socket.socket() # Create a socket object
# these are parameters loaded onto the ros parameter servers
host = rospy.get_param('~host', '172.20.10.5')
# these are parameters loaded onto the ros parameter servers
port = rospy.get_param('~port', 3000)
s.connect((host, port))
rospy.init_node('rtk_gps_node')
pub_navsatfix = rospy.Publisher("/rtk_fix", NavSatFix, queue_size=0)
navsatfix = NavSatFix()
while True:
GPS_msgs = s.recv(1024)
msgs = re.split('\r\n', GPS_msgs)
for msg in msgs:
if msg[:6] == "$GPGGA":
print msg
data = msg.split(',')
navsatfix.status = NavSatStatus(status=int(data[6]))
navsatfix.latitude = float(data[2][:2])
navsatfix.longitude = float(data[4][:3])
navsatfix.altitude = float(data[9])
navsatfix.header.stamp = rospy.Time.now()
pub_navsatfix.publish(navsatfix)
def gpsTopicPublush(
timestamp,
latitude,
longitude,
status, #将得到的信息组装成 NavSatStatus 消息类型
satellite_num,
location_accuracy,
altitude):
# Header
header_ins = Header(frame_id="gps_link", stamp=rospy.Time.now())
# NavSatStatus
if (status is '0'):
gps_status = -1
else:
gps_status = 0
navsatstatus_ins = NavSatStatus(status=gps_status)
navstafix_ins = NavSatFix(
header=header_ins,
status=navsatstatus_ins,
latitude=float(latitude),
longitude=float(longitude),
altitude=float(altitude),
)
global gps_pub
gps_pub.publish(navstafix_ins)
def callback( packet ) :
if rospy.is_shutdown() :
sys.exit()
message = GPSFix()
message.header.stamp = rospy.Time.now()
message.header.frame_id = 'base_link'
message.latitude = packet[ 'latitude' ] * 180 / math.pi
message.longitude = packet[ 'longitude' ] * 180 / math.pi
message.altitude = packet[ 'height' ]
message.time = packet[ 'time' ]
message.pdop = packet[ 'PDOP' ]
message.hdop = packet[ 'HDOP' ]
message.vdop = packet[ 'VDOP' ]
message.tdop = packet[ 'TDOP' ]
message.position_covariance = [ packet[ 'sigma-E' ], packet[ 'cov-EN' ] , 0,
packet[ 'cov-EN' ], packet[ 'sigma-N' ], 0,
0, 0, packet[ 'sigma-Up'] ]
if packet[ 'cov-EN' ] != 0 :
message.position_covariance_type = 3
elif packet[ 'sigma-E' ] != 0 :
message.position_covariance_type = 2
else :
message.position_covariance_type = 0
if debug :
pp.pprint( packet )
pp.pprint( message )
message.status.status = (0 if (message.position_covariance_type > 0) else -1)
message.status.position_source = 1
message.status.header = message.header
publisherGPSFix.publish( message )
if message.status >= 0 :
publisherNavSatFix.publish( NavSatFix( message.header , NavSatStatus( message.status.status, 3 ), message.latitude, message.longitude,
message.altitude, message.position_covariance, message.position_covariance_type ) )
def timepulse_callback(self, channel):
self.get_logger().info(f"{time.time()} Timepulse trigger")
gps_msg = NavSatFix()
timeref_msg = TimeReference()
msg_hdr = Header()
system_time = self.get_clock().now().to_msg()
msg_hdr.frame_id = 'base_link' # center of the plane
try:
ubx = self.ubp.read()
except IOError:
self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
return
while ubx:
if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
# <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>
msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)
fix_stat = NavSatStatus()
if ubx.fixType == 0:
self.get_logger().warning(f"No fix yet.")
break
fix_stat.service = SERVICE_GPS
gps_msg.status = fix_stat
gps_msg.header = msg_hdr
gps_msg.latitude = float(ubx.lat)/10000000
gps_msg.longitude = float(ubx.lon)/10000000
gps_msg.altitude = float(ubx.height)/1000
timeref_msg.header = msg_hdr
timeref_msg.time_ref = system_time
timeref_msg.source = "GPS"
self.fix_pub.publish(gps_msg)
self.time_pub.publish(timeref_msg)
self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
return
else:
self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
ubx = self.ubp.read()
def pub(self):
msg_nav_sat_fix = NavSatFix()
msg_nav_sat_fix.header.stamp = rospy.Time.now() + rospy.Duration(random.gauss(0., 0.35))
msg_nav_sat_stat = NavSatStatus()
self.pub_nav_fix.publish(msg_nav_sat_fix)
self.nav_fix_freq.tick(msg_nav_sat_fix.header.stamp)
self.pub_nav_stat.publish(msg_nav_sat_stat)
def publish(self):
header = Header()
header.stamp = rospy.Time.now()
navStatus = NavSatStatus()
navStatus.status = (self.fixStatus - 1)
navStatus.service = (0b1111)
gpsMsg = NavSatFix()
gpsMsg.header = header
gpsMsg.status = navStatus
gpsMsg.latitude = self.lat
gpsMsg.longitude = self.long
gpsMsg.altitude = self.alt
gpsMsg.position_covariance = self.covariance
gpsMsg.position_covariance_type = self.covariance_type
self.navSatPub.publish(gpsMsg)
self.headingPub.publish(self.heading)
def mainloop():
rospy.init_node('roscopter')
while not rospy.is_shutdown():
rospy.sleep(0.001)
msg = master.recv_match(blocking=False)
if not msg:
continue
#print msg.get_type()
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
msg_type = msg.get_type()
if msg_type == "RC_CHANNELS_RAW":
pub_rc.publish([
msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw,
msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw
])
if msg_type == "HEARTBEAT":
pub_state.publish(
msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
msg.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED,
mavutil.mode_string_v10(msg))
if msg_type == "VFR_HUD":
pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading,
msg.throttle, msg.alt, msg.climb)
if msg_type == "GPS_RAW_INT":
fix = NavSatStatus.STATUS_NO_FIX
if msg.fix_type >= 3:
fix = NavSatStatus.STATUS_FIX
pub_gps.publish(
NavSatFix(
latitude=msg.lat / 1e07,
longitude=msg.lon / 1e07,
altitude=msg.alt / 1e03,
status=NavSatStatus(status=fix,
service=NavSatStatus.SERVICE_GPS)))
#pub.publish(String("MSG: %s"%msg))
if msg_type == "ATTITUDE":
pub_attitude.publish(msg.roll, msg.pitch, msg.yaw,
msg.rollspeed, msg.pitchspeed,
msg.yawspeed)
if msg_type == "LOCAL_POSITION_NED":
print "Local Pos: (%f %f %f) , (%f %f %f)" % (
msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)
if msg_type == "RAW_IMU":
pub_raw_imu.publish(Header(), msg.time_usec, msg.xacc,
msg.yacc, msg.zacc, msg.xgyro, msg.ygyro,
msg.zgyro, msg.xmag, msg.ymag, msg.zmag)
def write_rtk_gps(timestamp_ns, T_UTM_S, utm_zone, lat_deg, lon_deg, alt_m,
status, bag):
ros_timestamp = nanoseconds_to_ros_timestamp(timestamp_ns)
ros_status = NavSatStatus()
if status == 'INS_SOLUTION_GOOD':
ros_status.status = NavSatStatus.STATUS_FIX
else:
ros_status.status = NavSatStatus.STATUS_FIX
ros_status.service = NavSatStatus.SERVICE_GPS
fix = NavSatFix()
fix.status = ros_status
fix.latitude = lat_deg
fix.longitude = lon_deg
fix.altitude = alt_m
fix.header.frame_id = "WGS"
fix.header.stamp = ros_timestamp
rospose = Odometry()
rospose.child_frame_id = "S"
rospose.header.frame_id = "UTM"
rospose.header.stamp = ros_timestamp
rospose.pose.pose.position.x = T_UTM_S.getPosition()[0]
rospose.pose.pose.position.y = T_UTM_S.getPosition()[1]
rospose.pose.pose.position.z = T_UTM_S.getPosition()[2]
rospose.pose.pose.orientation.x = T_UTM_S.getRotation().x()
rospose.pose.pose.orientation.y = T_UTM_S.getRotation().y()
rospose.pose.pose.orientation.z = T_UTM_S.getRotation().z()
rospose.pose.pose.orientation.w = T_UTM_S.getRotation().w()
rospose.twist.twist.linear.x = 0.0
rospose.twist.twist.linear.y = 0.0
rospose.twist.twist.linear.z = 0.0
rospose.twist.twist.angular.x = 0.0
rospose.twist.twist.angular.y = 0.0
rospose.twist.twist.angular.z = 0.0
bag.write('/gps_rtk', rospose, t=ros_timestamp)
bag.write('/gps_rtk_fix', fix, t=ros_timestamp)
def write_gps(gps, i, bag):
utime = gps[i, 0]
mode = gps[i, 1]
lat = gps[i, 3]
lng = gps[i, 4]
alt = gps[i, 5]
timestamp = microseconds_to_ros_timestamp(utime)
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = gps[i, 2]
fix = NavSatFix()
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = alt
track = Float64()
track.data = gps[i, 6]
speed = Float64()
speed.data = gps[i, 7]
bag.write('/gps_fix', fix, t=timestamp)
bag.write('/gps_track', track, t=timestamp)
bag.write('/gps_speed', speed, t=timestamp)
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 __init__(self, **kwargs):
assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
"Invalid arguments passed to constructor: %r" % kwargs.keys()
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
from sensor_msgs.msg import NavSatStatus
self.status = kwargs.get('status', NavSatStatus())
self.latitude = kwargs.get('latitude', float())
self.longitude = kwargs.get('longitude', float())
self.altitude = kwargs.get('altitude', float())
self.position_covariance = kwargs.get(
'position_covariance',
list([float() for x in range(9)])
)
self.position_covariance_type = kwargs.get('position_covariance_type', int())
def __init__(self):
rospy.init_node('applanix_publisher')
# Parameters
self.publish_tf = rospy.get_param('~publish_tf', False)
self.odom_frame = rospy.get_param('~odom_frame', 'odom_combined')
self.base_frame = rospy.get_param('~base_frame', 'base_footprint')
self.zero_start = rospy.get_param('~zero_start', False)
origin_param = rospy.get_param('/gps_origin', None)
self.origin = Point()
if origin_param is not None and origin_param != "None":
self.zero_start = False
self.origin.x = origin_param["east"]
self.origin.y = origin_param["north"]
self.origin.z = origin_param["alt"]
elif not self.zero_start:
origin_param = {
"east": self.origin.x,
"north": self.origin.y,
"alt": self.origin.z,
}
rospy.set_param('/gps_origin', origin_param)
# Topic publishers
self.pub_imu = rospy.Publisher('imu_data', Imu, queue_size=5)
self.pub_odom = rospy.Publisher('gps_odom', Odometry, queue_size=5)
self.pub_origin = rospy.Publisher('origin', Pose, queue_size=5)
self.pub_navsatfix = rospy.Publisher('gps_fix',
NavSatFix,
queue_size=5)
self.pub_navsatstatus = rospy.Publisher('gps_status',
NavSatStatus,
queue_size=5)
if self.publish_tf:
self.tf_broadcast = tf.TransformBroadcaster()
# Init nav status
self.nav_status = NavSatStatus(
) # We need this for the NavSatFix broadcaster
self.nav_status.status = NavSatStatus.STATUS_NO_FIX
self.nav_status.service = NavSatStatus.SERVICE_GPS
self.init = False # If we've been initialized
# Subscribed topics
rospy.Subscriber('nav', NavigationSolution, self.navigation_handler)
rospy.Subscriber('status/gnss/primary', GNSSStatus,
self.status_handler)
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
from sensor_msgs.msg import NavSatStatus
self.status = kwargs.get('status', NavSatStatus())
self.latitude = kwargs.get('latitude', float())
self.longitude = kwargs.get('longitude', float())
self.altitude = kwargs.get('altitude', float())
if 'position_covariance' not in kwargs:
self.position_covariance = numpy.zeros(9, dtype=numpy.float64)
else:
self.position_covariance = numpy.array(kwargs.get('position_covariance'), dtype=numpy.float64)
assert self.position_covariance.shape == (9, )
self.position_covariance_type = kwargs.get('position_covariance_type', int())
def __init__(self):
# Parameters
self.publish_tf = rospy.get_param('~publish_tf', False)
self.odom_frame = rospy.get_param('~odom_frame', 'odom_combined')
self.base_frame = rospy.get_param('~base_frame', 'base_footprint')
self.zero_start = rospy.get_param(
'~zero_start', False
) # If this is True, UTM will be pub'd wrt. our first recv'd coordinate
self.keep_az = 0 # want delta az not abs
self.init_az = False # have we initialised the azimuth
self.imu_rate = rospy.get_param('~rate')
# IMU scale factors, needed for RAWIMU log only
self.imu_scale = {
'gyro': RAD(720.0 / pow(2.0, 31.0)),
'accel': 200.0 / pow(2.0, 31.0)
} # ADIS16488
# Topic publishers
self.pub_imu = rospy.Publisher('imu/data', Imu, queue_size=1)
self.pub_odom = rospy.Publisher('odom', Odometry, queue_size=1)
self.pub_origin = rospy.Publisher('origin', Pose, queue_size=1)
self.pub_navsatfix = rospy.Publisher('navsat/fix',
NavSatFix,
queue_size=1)
self.pub_navsatstatus = rospy.Publisher('navsat/status',
NavSatStatus,
queue_size=1)
if self.publish_tf:
self.tf_broadcast = tf.TransfromBroadcaster()
# Init nav status
self.nav_status = NavSatStatus(
) # We need this for the NavSatFix broadcaster
self.nav_status.status = NavSatStatus.STATUS_NO_FIX
self.nav_status.service = NavSatStatus.SERVICE_GPS
self.init = False # If we've been initialized
self.origin = Point() # Where we've started
# Subscribed topics
rospy.Subscriber('config', AllMsgs, self.everything_handler)
def __init__(self):
# Subscriber
self.pose_sub = rospy.Subscriber('pose_real', PoseStamped, self.update_pose)
self.twist_sub = rospy.Subscriber('twist_real', TwistStamped, self.update_twist)
# Publisher
self.gpsfix_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=1)
self.gpsfix = NavSatFix()
self.gpsvel_pub = rospy.Publisher('/gps/vel', TwistStamped, queue_size=1)
self.gpsvel = TwistStamped()
# Configuration initiale du gps
self.gpsfix.status = NavSatStatus(status=NavSatStatus.STATUS_FIX,
service=NavSatStatus.SERVICE_GPS) # gps simple
self.gpsfix.position_covariance = [1, 1, 1,
1, 1, 1,
1, 1, 1]
self.gpsfix.position_covariance_type = self.gpsfix.COVARIANCE_TYPE_UNKNOWN
self.lon_offset = -4
self.lat_offset = 48.5
def talker():
ser = serial.Serial("/dev/ttyTHS0", 9600, timeout=1)
sio = io.TextIOWrapper(io.BufferedReader(ser))
pub = rospy.Publisher("gps_data", NavSatFix, queue_size=1)
rospy.init_node("gps")
msg = NavSatFix()
msg.status = NavSatStatus(0, 0x1 | 0x2) # Unaugmented fix, GPS & GLONASS
msg.header.frame_id = "gps_link"
rospy.loginfo("GPS node ready!")
while not rospy.is_shutdown():
try:
line = sio.readline()
if not line:
continue
gpsmsg = pynmea2.parse(line)
if type(gpsmsg) == pynmea2.GGA:
qual = gpsmsg.gps_qual
if qual > 0: # Valid gps fix?
msg.status.status = (qual - 1 if qual <= 2 else 2
) # See sensor_msgs/NavSatStatus
msg.latitude = gpsmsg.latitude
msg.longitude = gpsmsg.longitude
msg.altitude = gpsmsg.altitude
except serial.SerialException as e:
rospy.logerr("Device error: {}".format(e))
break
except pynmea2.ParseError as e:
rospy.logerr("Parse error: {}".format(e))
continue
except UnicodeDecodeError:
continue
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rospy.sleep(0.01)
def __init__(self):
# Subscriber
self.pose_sub = rospy.Subscriber('simu/pose_real', PoseStamped, self.update_pose)
self.twist_sub = rospy.Subscriber('simu/twist_real', TwistStamped, self.update_twist)
# Publisher
self.gpsfix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1)
self.gpsfix = NavSatFix()
self.gpsvel_pub = rospy.Publisher('vel', TwistStamped, queue_size=1)
self.gpsvel = TwistStamped()
# Configuration initiale du gps
self.CONVERSION_FACTOR_GPS = 1852 # m/min d'angle
self.lat_origin = 60.0
self.lon_origin = 0.0
self.gpsfix.status = NavSatStatus(status=NavSatStatus.STATUS_FIX,
service=NavSatStatus.SERVICE_GPS) # gps simple
self.gpsfix.position_covariance = [1, 1, 1,
1, 1, 1,
1, 1, 1]
self.gpsfix.position_covariance_type = self.gpsfix.COVARIANCE_TYPE_UNKNOWN
def write_gps(gps, i, bag):
utime = gps[i, 0]
mode = gps[i, 1]
lat = gps[i, 3]
lng = gps[i, 4]
alt = gps[i, 5]
timestamp = rospy.Time.from_sec(utime / 1e6)
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = gps[i, 2]
fix = NavSatFix()
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = alt
track = Float64()
track.data = gps[i, 6]
speed = Float64()
speed.data = gps[i, 7]
bag.write('gps_fix', fix, t=timestamp)
bag.write('gps_track', track, t=timestamp)
bag.write('gps_speed', speed, t=timestamp)
# print("package image...")
# img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/'
# img_list = os.listdir(img_path)
# i_img = 0
# for img_name in img_list:
# i_img = i_img + 1
# print(i_img)
# img_cv = cv2.imread(os.path.join(img_path, img_name), -1)
# br = CvBridge()
# img = Image()
# img = br.cv2_to_imgmsg(img_cv, "bgr8")
# img.header.seq = i_img
# img.header.frame_id = 'camImage'
# bag.write('/camera/image', img)
img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/'
img_list = os.listdir(img_path)
img_cv = cv2.imread(os.path.join(img_path, img_list[i]), -1)
img_cv = cv2.resize(img_cv, IMAGEDIM, interpolation=cv2.INTER_AREA)
br = CvBridge()
img = Image()
img = br.cv2_to_imgmsg(img_cv, "bgr8")
def handle_gps(self, data, timestamp):
"""
Date gps_Date MM/DD/YYYY or DD/MM/YYYY
Time gps_Time HH:MM:SS.SSS
Lat & Lon gps_Lat,gps_Long Degrees-7
Altitude gps_Alt mm
Altitude MSL gps_AltMSL mm
SIV gps_SIV Count
Fix Type gps_FixType 0-5
Carrier Soln. gps_CarrierSolution 0-2
Ground Speed gps_GroundSpeed mm/s
Heading gps_Heading Degrees-5
PDOP gps_pDOP 10-2 (dimensionless)
Time Of Week gps_iTOW ms
Lat = Latitude
Lon = Longitude
MSL = Metres above Sea Level
SIV = Satellites In View
PDOP = Positional Dilution Of Precision
Fix Type:
0: No
1: Dead Reckoning Only
2: 2D
3: 3D
4: GNSS + Dead Reckoning
5: Time Only
Carrier Solution:
0: No
1: Float Solution
2: Fixed Solution
"""
gps_msg = NavSatFix()
gps_msg.header.frame_id = self.tf_prefix+"world"
gps_msg.header.stamp = timestamp
gps_msg.status = NavSatStatus(status=0, service=1)
gps_msg.latitude = float(data[self.data_headers["GPS"]["Lat"]])
gps_msg.longitude = float(data[self.data_headers["GPS"]["Long"]])
gps_msg.altitude = float(data[self.data_headers["GPS"]["Alt"]])
# COVARIANCE_TYPE_UNKNOWN = 0, COVARIANCE_TYPE_APPROXIMATED = 1
# COVARIANCE_TYPE_DIAGONAL_KNOWN = 2, COVARIANCE_TYPE_KNOWN = 3
gps_msg.position_covariance = [0.0] * 9
gps_msg.position_covariance_type = 0
self.gps_pub.publish(gps_msg)
# Time Reference
time_msg = TimeReference()
time_msg.header.stamp = timestamp
gps_time = datetime.strptime("{} {}".format(data[self.data_headers["GPS"]["Date"]], data[self.data_headers["GPS"]["Time"]]),
"%d/%m/%Y %H:%M:%S.%f")
total_secs = (gps_time - epoch).total_seconds()
time_msg.time_ref.secs = int(total_secs)
time_msg.time_ref.nsecs = total_secs-int(total_secs)
time_msg.source = "GPS"
self.time_ref.publish(time_msg)
return True
def __init__(self):
super(GPSClass, self).__init__()
self.__gps_msg = NavSatFix()
self.__status = NavSatStatus()
def gps_to_navsat(self, gps_list, i):
""" converts gps data to ROS NavSatFix messages
:param gps_list: list, containing gps data
:param i: row counter
:return: fill bag with navsat, track, speed, timestamp
"""
# load data from list
utime = gps_list[i, 0]
mode = gps_list[i, 1]
num_satss = gps_list[i, 2]
lat = gps_list[i, 3]
lng = gps_list[i, 4]
alt = gps_list[i, 5]
track_raw = gps_list[i, 6]
speed_raw = gps_list[i, 7]
# create ros timestamp
timestamp = rospy.Time.from_sec(utime / 1e6)
# get gps and base link
gps_link = self.gps_frame
base_link = self.body_frame
# fill NavSat message
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = num_satss
navsat = NavSatFix()
navsat.header.stamp = timestamp
navsat.header.frame_id = gps_link
navsat.status = status
navsat.latitude = np.rad2deg(lat)
navsat.longitude = np.rad2deg(lng)
navsat.altitude = alt
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
track = Float64()
track.data = track_raw
speed = Float64()
speed.data = speed_raw
# create base_link gps_link static transformer
gps_static_transform_stamped = geometry_msgs.msg.TransformStamped()
gps_static_transform_stamped.header.stamp = timestamp
gps_static_transform_stamped.header.frame_id = base_link
gps_static_transform_stamped.child_frame_id = gps_link
gps_static_transform_stamped.transform.translation.x = 0
gps_static_transform_stamped.transform.translation.y = 0.25
gps_static_transform_stamped.transform.translation.z = 0.51
quat = tf.transformations.quaternion_from_euler(0, 0, 0)
gps_static_transform_stamped.transform.rotation.x = quat[0]
gps_static_transform_stamped.transform.rotation.y = quat[1]
gps_static_transform_stamped.transform.rotation.z = quat[2]
gps_static_transform_stamped.transform.rotation.w = quat[3]
# publish static transform
tf_static_msg = tf2_msgs.msg.TFMessage([gps_static_transform_stamped])
return navsat, track, speed, timestamp, tf_static_msg
def main(argv):
if len(sys.argv) < 2:
print 'Please specify data directory file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
print("loading files...")
bag = rosbag.Bag(sys.argv[2], 'w')
gps = np.loadtxt(sys.argv[1] + "sensor_data/2012-01-08/gps.csv",
delimiter=",")
imu_100hz = np.loadtxt(sys.argv[1] +
"sensor_data/2012-01-08/imu_100hz.csv",
delimiter=",")
ral_seq = 0
bap_seq = 0
img_seq = 0
imu_seq = 0
cal = -1
gps_seq = 0
# IMAGE_COUNT = 81169
STREET_VIEW = 113
print("package gps and image...")
print("Packaging GPS and image")
for gps_data in gps:
utime = int(gps_data[0])
mode = int(gps_data[1])
timestamp = rospy.Time.from_sec(utime / 1e6)
lat = float(gps_data[3])
lng = float(gps_data[4])
alt = float(gps_data[5])
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = float(gps_data[2])
fix = NavSatFix()
fix.header.seq = gps_seq
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = np.rad2deg(alt)
track = Float64()
track.data = float(gps_data[6])
speed = Float64()
speed.data = float(gps_data[7])
bag.write('/gps', fix, t=timestamp)
bag.write('/gps_track', track, t=timestamp)
bag.write('/gps_speed', speed, t=timestamp)
# write aerial image
if gps_seq <= MAVIMAGE:
img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam5/'
img_list = os.listdir(img_path)
img_list.sort()
img_cv = cv2.imread(os.path.join(img_path, img_list[gps_seq]), -1)
img_cv = cv2.resize(img_cv, MAVDIM, interpolation=cv2.INTER_AREA)
# 顺时针旋转90度
trans_img = cv2.transpose(img_cv)
img_cv = cv2.flip(trans_img, 1)
br = CvBridge()
Img = Image()
Img = br.cv2_to_imgmsg(img_cv, "bgr8")
Img.header.seq = int(gps_seq)
print(gps_seq)
Img.header.stamp = timestamp
Img.header.frame_id = 'camera'
bag.write('/image/cam5', Img, t=timestamp)
gps_seq = gps_seq + 1
print('packaging imu...')
for imu_data in imu_100hz:
imu_seq = imu_seq + 1
utime = int(imu_data[0])
timestamp = rospy.Time.from_sec(utime / 1e6)
imu = Imu()
imu.header.seq = imu_seq
imu.header.stamp = timestamp
imu.header.frame_id = '/Imu'
imu.linear_acceleration.x = float(imu_data[5])
imu.linear_acceleration.y = float(imu_data[6])
imu.linear_acceleration.z = float(imu_data[7])
imu.linear_acceleration_covariance = np.zeros(9)
imu.angular_velocity.x = float(imu_data[8])
imu.angular_velocity.y = float(imu_data[9])
imu.angular_velocity.z = float(imu_data[10])
imu.angular_velocity_covariance = np.zeros(9)
imu.orientation.w = float(imu_data[1])
imu.orientation.x = float(imu_data[2])
imu.orientation.y = float(imu_data[3])
imu.orientation.z = float(imu_data[4])
bag.write('/Imu', imu, t=timestamp)
bag.close()
return 0
if __name__ == "__main__":
rospy.init_node("fake_waterlinked", log_level=rospy.INFO)
heading_offset = rospy.get_param(
"~heading"
) # heading is given by waterlinked GPS in degrees CW from North
master_datum = rospy.get_param(
"~datum") # Master located (latitude, longitude)
lat, lon, alt = master_datum + [0.0] if len(
master_datum) < 3 else master_datum
master_gps = rospy.Publisher('gps_datum', NavSatFix, queue_size=5)
master_msg = NavSatFix()
master_msg.altitude = alt
master_msg.longitude = lon
master_msg.latitude = lat
master_msg.status = NavSatStatus()
master_msg.status.status = master_msg.status.STATUS_FIX
master_msg.status.service = master_msg.status.SERVICE_GPS
master_msg.position_covariance_type = master_msg.COVARIANCE_TYPE_UNKNOWN
master_msg.position_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0]
pose_pub = rospy.Publisher('waterlinked/pose_with_cov_stamped',
PoseWithCovarianceStamped,
queue_size=5)
buff = Buffer()
TransformListener(buff)
rospy.Subscriber("mavros/global_position/global", NavSatFix, handle_gps,
(pose_pub, buff))
map_to_waterlink = TransformStamped(
Header(0, rospy.Time.now(), 'map'), 'waterlinked',
Transform(
None,
def main(args):
if len(sys.argv) < 2:
print 'Please specify gps file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
gps = np.loadtxt(sys.argv[1], delimiter=",")
utimes = gps[:, 0]
modes = gps[:, 1]
num_satss = gps[:, 2]
lats = gps[:, 3]
lngs = gps[:, 4]
alts = gps[:, 5]
tracks = gps[:, 6]
speeds = gps[:, 7]
bag = rosbag.Bag(sys.argv[2], 'w')
try:
for i, utime in enumerate(utimes):
timestamp = rospy.Time.from_sec(utime / 1e6)
status = NavSatStatus()
if modes[i] == 0 or modes[i] == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = num_satss[i]
fix = NavSatFix()
fix.status = status
fix.latitude = np.rad2deg(lats[i])
fix.longitude = np.rad2deg(lngs[i])
fix.altitude = alts[i]
track = Float64()
track.data = tracks[i]
speed = Float64()
speed.data = speeds[i]
bag.write('fix', fix, t=timestamp)
bag.write('track', track, t=timestamp)
bag.write('speed', speed, t=timestamp)
finally:
bag.close()
return 0
def mainloop():
global gps_msg, autonomous_enable
rospy.init_node('casy_rover')
if opts.enable_watchdog:
rospy.Timer(rospy.Duration(1/opts.watchdog_rate), watchdog_timer_cb)
# SEND IF YOU DESIRE A LIST OF ALL PARAMS (TODO: Publish params to a topic)
#master.mav.param_request_list_send(master.target_system, master.target_component)
# Permette di settare un parametro come in mission planner
#master.param_set_send('FS_ACTION', 0, 2)
# Riceve una lista di tutti i parametri disponibili sotto forma di messaggi PARAM_VALUE
#master.param_fetch_all()
r = rospy.Rate(opts.rate)
while not rospy.is_shutdown():
r.sleep()
msg = master.recv_match(blocking=False)
if not msg:
continue
# Parse incoming message
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
msg_type = msg.get_type()
if msg_type == "RC_CHANNELS_RAW" :
# If using Autonomous Control safety switch, use specified channel
# to enable or disable autonomous control. Autonomous control
# allows mode, rc, and waypoint controls.
if (opts.enable_autonomous_safety_switch):
if (msg.chan6_raw >= 1500):
autonomous_enable = True
elif (msg.chan6_raw < 1500):
autonomous_enable = False
# Give control back to controller
master.mav.rc_channels_override_send(master.target_system,
master.target_component, 0, 0, 0, 0, 0, 0, 0, 0)
pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw,
msg.chan4_raw, msg.chan5_raw, msg.chan6_raw,
msg.chan7_raw, msg.chan8_raw])
elif msg_type == "HEARTBEAT":
pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED,
mavutil.mode_string_v10(msg))
state_msg.armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
state_msg.mode = mavutil.mode_string_v10(msg)
elif msg_type == "VFR_HUD":
pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb)
elif msg_type == "GPS_RAW_INT":
fix = NavSatStatus.STATUS_NO_FIX
if msg.fix_type >=3:
fix=NavSatStatus.STATUS_FIX
header = Header()
header.frame_id = 'base_link'# '/gps'
header.stamp = rospy.Time.now()
#rospy.loginfo("Hdop is %d", msg.eph)
#rospy.loginfo("Vdop is %d", msg.epv)
sigma = math.sqrt((3.04 * msg.eph**2)**2 + 3.57**2)
position_covariance = [0] * 9
position_covariance[0] = sigma #9999
position_covariance[4] = sigma #9999
position_covariance[8] = sigma #9999
pub_gps.publish(NavSatFix(header = header,
latitude = msg.lat/1e07,
longitude = msg.lon/1e07,
altitude = msg.alt/1e03,
position_covariance=position_covariance,
position_covariance_type=NavSatFix.COVARIANCE_TYPE_APPROXIMATED,
status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS)
))
gps_msg.latitude = msg.lat
gps_msg.longitude = msg.lon
elif msg_type == "LOCAL_POSITION_NED" :
rospy.loginfo("Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz))
elif msg_type == "RAW_IMU" :
pub_raw_imu.publish (Header(), msg.time_usec,
msg.xacc, msg.yacc, msg.zacc,
msg.xgyro, msg.ygyro, msg.zgyro,
msg.xmag, msg.ymag, msg.zmag)
elif msg_type == "SYS_STATUS":
status_msg = casy_rover.msg.Status()
status_msg.header.stamp = rospy.Time.now()
status_msg.battery_voltage = msg.voltage_battery
status_msg.battery_current = msg.current_battery
status_msg.battery_remaining = msg.battery_remaining
status_msg.sensors_enabled = msg.onboard_control_sensors_enabled
pub_status.publish(status_msg)
elif msg_type == "GLOBAL_POSITION_INT":
header = Header()
header.stamp = rospy.Time.now()
filtered_pos_msg.header = header
filtered_pos_msg.latitude = msg.lat
filtered_pos_msg.longitude = msg.lon
filtered_pos_msg.altitude = msg.alt
filtered_pos_msg.relative_altitude = msg.relative_alt
filtered_pos_msg.ground_x_speed = msg.vx
filtered_pos_msg.ground_y_speed = msg.vy
filtered_pos_msg.ground_z_speed = msg.vz
filtered_pos_msg.heading = msg.hdg
pub_filtered_pos.publish(filtered_pos_msg)
elif msg_type == "COMMAND_ACK":
rospy.loginfo ("COMMAND_ACK: Command Message ACK with result - " + str(msg.result))
elif msg_type == "STATUSTEXT":
rospy.loginfo ("STATUSTEXT: Status severity is %d. Text Message is %s" %(msg.severity, msg.text))
elif msg_type == "PARAM_VALUE":
rospy.loginfo ("PARAM_VALUE: ID = %s, Value = %d, Type = %d, Count = %d, Index = %d"
%(msg.param_id, msg.param_value, msg.param_type, msg.param_count, msg.param_index))