├── rtklib ├── rospack_nosubdirs ├── .gitignore ├── start_rtklib.sh ├── rtklib.launch ├── CMakeLists.txt ├── manifest.xml ├── Makefile ├── enu_single.conf ├── scripts │ └── latlon_to_utm.py ├── enu_rtk.conf └── nodes │ └── enu_to_pose.py ├── rtkrcv ├── Makefile ├── manifest.xml ├── src │ ├── ros_options.cpp │ ├── main.cpp │ └── rtkrcv.cpp ├── CMakeLists.txt └── rtkrcv.launch ├── stack.xml ├── .gitignore └── README.md /rtklib/rospack_nosubdirs: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /rtkrcv/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /rtklib/.gitignore: -------------------------------------------------------------------------------- 1 | built 2 | include 3 | installed 4 | msg_gen/ 5 | src/ 6 | 7 | -------------------------------------------------------------------------------- /rtklib/start_rtklib.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | conf="${1:-"enu_rtk.conf"}" 3 | rtklib_dir=$(dirname $0) 4 | $rtklib_dir/bin/rtkrcv -s -o $rtklib_dir/$conf -p 3000 5 | -------------------------------------------------------------------------------- /stack.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Tools for working with Global Navigation Satellite Systems. 5 | 6 | 7 | Maintained by Fraunhofer IIS 8 | BSD 9 | 10 | http://ros.org/wiki/gnss 11 | 12 | 13 | -------------------------------------------------------------------------------- /rtklib/rtklib.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | 3 | # C extensions 4 | *.so 5 | 6 | # Packages 7 | *.egg 8 | *.egg-info 9 | dist 10 | build 11 | eggs 12 | parts 13 | bin 14 | var 15 | sdist 16 | develop-eggs 17 | .installed.cfg 18 | lib 19 | lib64 20 | 21 | # Installer logs 22 | pip-log.txt 23 | 24 | # Unit test / coverage reports 25 | .coverage 26 | .tox 27 | nosetests.xml 28 | 29 | # Translations 30 | *.mo 31 | 32 | # Mr Developer 33 | .mr.developer.cfg 34 | .project 35 | .pydevproject 36 | -------------------------------------------------------------------------------- /rtkrcv/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | GNSS RTK receiver node 5 | 6 | 7 | Manuel Stahl 8 | GPLv3 9 | 10 | http://ros.org/wiki/rtkrcv 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /rtklib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | rosbuild_init() 5 | 6 | find_package(LAPACK) 7 | find_package(BLAS) 8 | 9 | if(${LAPACK_FOUND} AND ${BLAS_FOUND}) 10 | add_definitions(-DLAPACK) 11 | endif() 12 | 13 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/rtklib) 14 | add_definitions(-DDLL) 15 | 16 | set(src ${CMAKE_CURRENT_SOURCE_DIR}/src/src) 17 | file(GLOB RTK_SOURCES "${src}/*.c" "${src}/rcv/*.c") 18 | rosbuild_add_library(rtk ${RTK_SOURCES}) 19 | target_link_libraries(rtk ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) 20 | -------------------------------------------------------------------------------- /rtkrcv/src/ros_options.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int loadrosopts(opt_t *opts) 7 | { 8 | ros::NodeHandle private_nh("~"); 9 | for (opt_t *opt = opts; opt->var != NULL; opt++) { 10 | std::string value; 11 | std::string key(opt->name); 12 | std::replace(key.begin(), key.end(), '-', '_'); 13 | private_nh.param(key, value, value); 14 | if (value.empty()) continue; 15 | if (!str2opt(opt, value.c_str())) 16 | trace(1,"loadrosopts: invalid option value '%s' (%s), %d\n", value.c_str(), opt->name, value.length()); 17 | } 18 | return 1; 19 | } 20 | -------------------------------------------------------------------------------- /rtklib/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A meta-package that downloads and installs Tomoji Takasu's RTKLIB locally. 5 | Patches from the website are applied. 6 | More about RTKLIB: http://www.rtklib.com/ 7 | 8 | 9 | Manuel Stahl 10 | GPLv3 11 | 12 | http://ros.org/wiki/rtklib 13 | http://www.rtklib.com/rtklib.jpg 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /rtklib/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk 2 | 3 | all: installed 4 | 5 | BIN_DIR = bin 6 | INC_DIR = include 7 | LIB_DIR = lib 8 | GIT_DIR = src 9 | GIT_URL = https://github.com/tomojitakasu/RTKLIB.git 10 | include $(shell rospack find mk)/git_checkout.mk 11 | 12 | built: $(GIT_DIR) patched 13 | cd $(GIT_DIR)/app && sh makeall.sh 14 | touch built 15 | 16 | installed: built 17 | mkdir -p $(BIN_DIR) 18 | ln -f $(GIT_DIR)/app/pos2kml/gcc/pos2kml $(BIN_DIR) 19 | ln -f $(GIT_DIR)/app/str2str/gcc/str2str $(BIN_DIR) 20 | ln -f $(GIT_DIR)/app/rnx2rtkp/gcc/rnx2rtkp $(BIN_DIR) 21 | ln -f $(GIT_DIR)/app/convbin/gcc/convbin $(BIN_DIR) 22 | ln -f $(GIT_DIR)/app/rtkrcv/gcc/rtkrcv $(BIN_DIR) 23 | mkdir -p $(INC_DIR)/rtklib 24 | ln -f $(GIT_DIR)/src/rtklib.h $(INC_DIR)/rtklib 25 | touch installed 26 | 27 | clean: 28 | $(RM) built 29 | $(RM) installed 30 | $(RM) -r $(GIT_DIR) 31 | 32 | wipe: clean 33 | $(RM) -r $(BIN_DIR) 34 | $(RM) -r $(INC_DIR) 35 | $(RM) -r $(LIB_DIR) 36 | 37 | -------------------------------------------------------------------------------- /rtkrcv/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int start_rtkrcv(int argc, char **argv); 5 | void stop_rtkrcv(); 6 | char get_solution(double &lat, double &lon, double &height); 7 | 8 | int 9 | main(int argc, char** argv) 10 | { 11 | std::string frame_id; 12 | ros::init(argc, argv, "rtklib"); 13 | 14 | ros::NodeHandle private_nh("~"); 15 | private_nh.param("frame_id", frame_id, std::string("antenna")); 16 | 17 | ros::NodeHandle nh; 18 | ros::Publisher pub = nh.advertise("fix", 5); 19 | 20 | if (!start_rtkrcv(argc, argv)) return(-1); 21 | 22 | ros::Rate r(1); // 1 Hz 23 | while (ros::ok()) { 24 | sensor_msgs::NavSatFix fix; 25 | fix.header.stamp = ros::Time::now(); 26 | fix.header.frame_id = frame_id; 27 | fix.status.service = fix.status.SERVICE_GPS | fix.status.SERVICE_GALILEO; 28 | fix.status.status = get_solution(fix.latitude, fix.longitude, fix.altitude); 29 | fix.position_covariance_type = fix.COVARIANCE_TYPE_UNKNOWN; 30 | pub.publish(fix); 31 | 32 | ros::spinOnce(); 33 | r.sleep(); 34 | } 35 | 36 | stop_rtkrcv(); 37 | 38 | return(0); 39 | } 40 | 41 | -------------------------------------------------------------------------------- /rtkrcv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | #set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #rosbuild_genmsg() 20 | #uncomment if you have defined services 21 | #rosbuild_gensrv() 22 | 23 | ################################################################################ 24 | # RTKLIB 25 | ################################################################################ 26 | 27 | #INCLUDE($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) 28 | 29 | rosbuild_find_ros_package(rtklib) 30 | include_directories(${rtklib_PACKAGE_PATH}/include) 31 | find_library(LIBRTK NAMES rtk PATHS ${rtklib_PACKAGE_PATH}/lib) 32 | message(${LIBRTK}) 33 | rosbuild_add_executable(${PROJECT_NAME} 34 | src/main.cpp 35 | src/ros_options.cpp 36 | src/rtkrcv.cpp) 37 | target_link_libraries(${PROJECT_NAME} ${LIBRTK}) 38 | -------------------------------------------------------------------------------- /rtklib/enu_single.conf: -------------------------------------------------------------------------------- 1 | # RTKNAVI options (v.2.4.1) 2 | 3 | pos1-posmode =single # (0:single,1:dgps,2:kinematic,3:static,4:movingbase,5:fixed,6:ppp-kine,7:ppp-static) 4 | pos1-frequency =l1 # (1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l2+l5+l6,5:l1+l2+l5+l6+l7) 5 | pos1-soltype =forward # (0:forward,1:backward,2:combined) 6 | pos1-elmask =5 # (deg) 7 | pos1-snrmask =0 # (dBHz) 8 | pos1-dynamics =off # (0:off,1:on) 9 | pos1-tidecorr =off # (0:off,1:on) 10 | pos1-ionoopt =brdc # (0:off,1:brdc,2:sbas,3:dual-freq,4:est-stec,5:ionex-tec,6:qzs-brdc,7:qzs-lex,8:vtec_sf,9:vtec_ef,10:gtec) 11 | pos1-tropopt =saas # (0:off,1:saas,2:sbas,3:est-ztd,4:est-ztdgrad) 12 | pos1-sateph =brdc # (0:brdc,1:precise,2:brdc+sbas,3:brdc+ssrapc,4:brdc+ssrcom) 13 | pos1-navsys =1 # (1:gps+2:sbas+4:glo+8:gal+16:qzs+32:comp) 14 | out-solformat =enu # (0:llh,1:xyz,2:enu,3:nmea) 15 | out-timesys =utc # (0:gpst,1:utc,2:jst) 16 | out-timeform =hms # (0:tow,1:hms) 17 | out-timendec =3 18 | out-degform =deg # (0:deg,1:dms) 19 | out-outstat =residual # (0:off,1:state,2:residual) 20 | 21 | inpstr1-type =tcpcli # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) 22 | inpstr1-path =:@localhost:10000/: 23 | inpstr1-format =skytraq # (0:rtcm2,1:rtcm3,2:oem4,3:oem3,4:ubx,5:ss2,6:hemis,7:skytraq,8:gw10,9:javad,15:sp3) 24 | outstr1-type =tcpsvr # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripsvr) 25 | outstr1-path =:@:3333/: 26 | outstr1-format =enu # (0:llh,1:xyz,2:enu,3:nmea) 27 | -------------------------------------------------------------------------------- /rtkrcv/rtkrcv.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | gnss 2 | ==== 3 | 4 | GNSS stack for ROS 5 | 6 | Tools for working with Global Navigation Satellite Systems. 7 | 8 | 9 | Usage 10 | ----- 11 | 12 | The node `enu_to_pose` in `nodes/enu_to_pose.py` needs a network connection to 13 | a rtklib instance. rtklib is built in this package with the Makefile (use 14 | `rosmake`). rtklib then should supply the solution output on the network 15 | connection configured in the `enu_to_pose`-node. Examples for rtklib with 16 | solution output on the default network port and input of data from another 17 | stream are included as examples. 18 | 19 | The launch file `rtklib.launch` starts an rtklib instance as well as the 20 | `enu_to_pose`-node. For testing purposes it is helpful to start rtklib 21 | manually with the rtknavi-GUI or manually with the simple shell wrapper script 22 | `start_rtklib.sh`. 23 | 24 | Example step-by-step instructions: 25 | 26 | 27 | * Add package to your ROS workspace with `rosinstall` 28 | * Build rtklib with `rosmake` 29 | * Supply a pre-recorded skytraq measurements file to the port specified in 30 | `enu_single.conf`, e.g. `cat | nc -l -p 10000` 31 | * Start rtklib and `enu_to_pose`-node with `roslaunch rtklib.launch` 32 | * Now, the rostopic `/enu` should show ENU pose informations. If the quality 33 | of the provided GNSS signal is sufficient, coordinate transforms are 34 | published to the ROS system 35 | 36 | 37 | Advanced example procedure for debugging including intermediate testing steps: 38 | 39 | * Provide GNSS measurements (see above) 40 | * Start rtklib with `./start_rtklib.sh enu_single.conf` using these 41 | measurements 42 | * Test rtklib solution by connecting to the output port manually and checking 43 | the solution for a valid position `nc localhost 3333` 44 | * Ensure a roscore process is running: Call `roscore` 45 | * Start the node with `rosrun rtklib enu_to_pose.py` 46 | 47 | 48 | Example output 49 | -------------- 50 | 51 | rtklib processes the GNSS measurements into position solutions in the ENU 52 | coordinate system and provides these informations along with quality 53 | information and other data on an output port, if configured, as can be seen in 54 | the example configuration files. Each line should look like the following 55 | example: 56 | 57 | 2012/08/23 12:49:30.402 863028.7989 4720911.2174 -2194093.2995 5 9 3.0176 5.8136 3.4867 2.6834 3.1125 1.7950 0.00 0.0 58 | 59 | The `enu_to_pose` node provides coordinate transforms and prints log messages 60 | to the console. The position from rtklib is provided in the rostopic `/enu` 61 | and looks like this: 62 | 63 | header: 64 | seq: 565 65 | stamp: 66 | secs: 1376397269 67 | nsecs: 369039058 68 | frame_id: /map 69 | pose: 70 | position: 71 | x: 863027.3836 72 | y: 4720911.2261 73 | z: -2194086.6277 74 | orientation: 75 | x: 0.0 76 | y: 0.0 77 | z: 0.0 78 | w: 0.0 79 | -------------------------------------------------------------------------------- /rtklib/scripts/latlon_to_utm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import pyproj 4 | import math 5 | import sys 6 | from optparse import OptionParser 7 | from math import atan2, cos, sin, sqrt 8 | 9 | 10 | def pythagoras(a, b): 11 | return sqrt(a**2 + b**2) 12 | 13 | 14 | class WGS84(): 15 | A = 6378137.0 16 | recf = 298.257223563 17 | B = A * (recf - 1.0) / recf 18 | eSquared = ((A * A) - (B * B)) / (A * A) 19 | e2Squared = ((A * A) - (B * B)) / (B * B) 20 | tn = (A - B) / (A + B) 21 | ap = A * (1.0 - tn + 5.0 * ((tn * tn) - (tn * tn * tn)) / 4.0 + 81.0 * ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 64.0) 22 | bp = 3.0 * A * (tn - (tn * tn) + 7.0 * ((tn * tn * tn) - (tn * tn * tn * tn)) / 8.0 + 55.0 * (tn * tn * tn * tn * tn) / 64.0) / 2.0 23 | cp = 15.0 * A * ((tn * tn) - (tn * tn * tn) + 3.0 * ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 4.0) / 16.0 24 | dp = 35.0 * A * ((tn * tn * tn) - (tn * tn * tn * tn) + 11.0 * (tn * tn * tn * tn * tn) / 16.0) / 48.0 25 | ep = 315.0 * A * ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 512.0 26 | 27 | 28 | def ecef_to_utm(x, y, z): 29 | xyz = (x, y, z) 30 | a2 = WGS84.A * WGS84.A 31 | b2 = WGS84.B * WGS84.B 32 | p = pythagoras(xyz[0], xyz[1]) 33 | theta = atan2((xyz[2] * WGS84.A) , (p * WGS84.B)) 34 | esq = 1.0 - (b2 / a2) 35 | epsq = (a2 / b2 - 1.0) 36 | sth = sin(theta) 37 | sth3 = sth**3 38 | cth = cos(theta) 39 | cth3 = cth**3 40 | lat = atan2((xyz[2] + epsq * WGS84.B * sth3) , (p - esq * WGS84.A * cth3)) 41 | if (lat > (math.pi/2)): 42 | lat = math.pi - lat 43 | lon = atan2(xyz[1],xyz[0]) 44 | hgt = (p / cos(lat)) - (a2 / sqrt(a2 * cos(lat) * cos(lat) + b2 * sin(lat) * sin(lat))) 45 | lat = lat * (180/math.pi) 46 | lon = lon * (180/math.pi) 47 | return latlon_to_utm(lat, lon) 48 | 49 | 50 | def latlon_to_utm(latitude, longitude, height=0): 51 | """calculate UTM projection""" 52 | lon_6 = math.floor(longitude / 6.0) 53 | utmXZone = (longitude <= 0.0) and (30 + lon_6) or (31 + lon_6) 54 | p = pyproj.Proj(proj='utm', zone=utmXZone, ellps='WGS84') 55 | easting, northing = p(longitude, latitude) 56 | return (easting, northing) 57 | 58 | if __name__ == "__main__": 59 | parser = OptionParser() 60 | parser.add_option('--latitude', help='Latitude value in float degrees [-90.0:90]') 61 | parser.add_option('--longitude', help='Longitude value in float degrees [-180.0:180]') 62 | parser.add_option('-x', help='ECEF-X') 63 | parser.add_option('-y', help='ECEF-Y') 64 | parser.add_option('-z', help='ECEF-Z') 65 | (options, args) = parser.parse_args() 66 | if options.latitude and options.longitude: 67 | en = latlon_to_utm(float(options.latitude), float(options.longitude)) 68 | elif options.x and options.y and options.z: 69 | en = ecef_to_utm(float(options.x), float(options.y), float(options.z)) 70 | else: 71 | print("Select either latitude/longitude or x, y, z") 72 | sys.exit(1) 73 | print("easting northing: %f %f" % (en[0], en[1])) 74 | -------------------------------------------------------------------------------- /rtklib/enu_rtk.conf: -------------------------------------------------------------------------------- 1 | # rtkrcv options (2010/08/12 07:12:16, v.2.4.0) 2 | 3 | ant2-postype =xyz # (0:llh,1:xyz,2:single,3:posfile,4:rinexhead,5:rtcm) 4 | ant2-pos1 =0 # (deg|m) 5 | ant2-pos2 =0 # (deg|m) 6 | ant2-pos3 =0 # (m|m) 7 | 8 | console-passwd =admin 9 | console-timetype =utc # (0:gpst,1:utc,2:jst,3:tow) 10 | console-soltype =dms # (0:dms,1:deg,2:xyz,3:enu,4:pyl) 11 | console-solflag =0 # (0:off,1:std+2:age/ratio/ns) 12 | 13 | inpstr1-type =serial # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) 14 | inpstr2-type =tcpcli # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) 15 | inpstr1-path =ttyUSB0:115200:8:n:1:off 16 | inpstr2-path =:@localhost:10000/: 17 | inpstr1-format =skytraq # (0:rtcm2,1:rtcm3,2:oem4,3:oem3,4:ubx,5:ss2,6:hemis,7:skytraq,14:sp3) 18 | inpstr2-format =skytraq # (0:rtcm2,1:rtcm3,2:oem4,3:oem3,4:ubx,5:ss2,6:hemis,7:skytraq,14:sp3) 19 | 20 | outstr1-type =tcpsvr # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripsvr) 21 | outstr1-path =:@:3333/: 22 | outstr1-format =enu # (0:llh,1:xyz,2:enu,3:nmea) 23 | 24 | logstr1-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripsvr) 25 | logstr2-type =file # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripsvr) 26 | logstr1-path =rov_%Y%m%d%h%M.log 27 | logstr2-path =ref_%Y%m%d%h%M.log 28 | 29 | misc-navmsgsel =all # (0:all,1:rover,1:base,2:corr) 30 | file-cmdfile1 =src/data/skytraq_raw_5hz.cmd 31 | 32 | pos1-posmode =kinematic # (0:single,1:dgps,2:kinematic,3:static,4:movingbase,5:fixed,6:ppp-kine,7:ppp-static) 33 | pos1-frequency =l1 # (1:l1,2:l1+l2,3:l1+l2+l5) 34 | pos1-soltype =forward # (0:forward,1:backward,2:combined) 35 | pos1-elmask =15 # (deg) 36 | pos1-snrmask =35 # (dBHz) 37 | pos1-dynamics =on # (0:off,1:on) 38 | pos1-tidecorr =off # (0:off,1:on) 39 | pos1-ionoopt =brdc # (0:off,1:brdc,2:sbas,3:dual-freq,4:est-stec) 40 | pos1-tropopt =saas # (0:off,1:saas,2:sbas,3:est-ztd,4:est-ztdgrad) 41 | pos1-sateph =brdc # (0:brdc,1:precise,2:brdc+sbas,3:brdc+ssrapc,4:brdc+ssrcom) 42 | pos1-exclsats = # (prn ...) 43 | pos2-arthres =2.5 44 | pos2-arlockcnt =0 45 | pos2-arelmask =0 # (deg) 46 | pos2-aroutcnt =5 47 | pos2-arminfix =10 48 | pos2-slipthres =0.05 # (m) 49 | pos2-maxage =30 # (s) 50 | pos2-rejionno =30 # (m) 51 | pos2-niter =1 52 | pos2-baselen =0 # (m) 53 | pos2-basesig =0 # (m) 54 | out-solformat =enu # (0:llh,1:xyz,2:enu,3:nmea) 55 | out-outhead =on # (0:off,1:on) 56 | out-outopt =on # (0:off,1:on) 57 | out-timesys =utc # (0:gpst,1:utc,2:jst) 58 | out-timeform =hms # (0:tow,1:hms) 59 | out-timendec =3 60 | out-outstat =residual # (0:off,1:state,2:residual) 61 | 62 | stats-errratio =100 63 | stats-errphase =0.003 # (m) 64 | stats-errphaseel =0.003 # (m) 65 | stats-errphasebl =0 # (m/10km) 66 | stats-errdoppler =1 # (Hz) 67 | stats-stdbias =30 # (m) 68 | stats-stdiono =0.03 # (m) 69 | stats-stdtrop =0.3 # (m) 70 | stats-prnaccelh =1 # (m/s^2) 71 | stats-prnaccelv =0.1 # (m/s^2) 72 | stats-prnbias =0.0001 # (m) 73 | stats-prniono =0.001 # (m) 74 | stats-prntrop =0.0001 # (m) 75 | stats-clkstab =5e-12 # (s/s) 76 | 77 | misc-timeinterp =off # (0:off,1:on) 78 | misc-sbasatsel =0 # (0:all) 79 | -------------------------------------------------------------------------------- /rtklib/nodes/enu_to_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | roslib.load_manifest('rtklib') 4 | import rospy 5 | import sys 6 | import socket 7 | import re 8 | import time 9 | import calendar 10 | 11 | from sensor_msgs.msg import TimeReference 12 | from geometry_msgs.msg import PoseStamped,TransformStamped 13 | from tf.msg import tfMessage 14 | from tf import transformations 15 | import tf 16 | 17 | 18 | #Add the tf_prefix to the given frame id 19 | def addTFPrefix(frame_id): 20 | prefix = "" 21 | prefix_param = rospy.search_param("tf_prefix") 22 | if prefix_param: 23 | prefix = rospy.get_param(prefix_param) 24 | if prefix[0] != "/": 25 | prefix = "/%s" % prefix 26 | 27 | return "%s/%s" % (prefix, frame_id) 28 | 29 | 30 | if __name__ == "__main__": 31 | # Init publisher 32 | rospy.init_node('enu_to_pose') 33 | posepub = rospy.Publisher('enu', PoseStamped) 34 | timepub = rospy.Publisher('time_reference', TimeReference) 35 | tfpub = tf.TransformBroadcaster() 36 | tflisten = tf.TransformListener() 37 | 38 | # Init rtklib port 39 | host = rospy.get_param('~host', 'localhost') 40 | port = rospy.get_param('~port', 3333) 41 | global_frame_id = rospy.get_param('~global_frame_id', '/map') 42 | odom_frame_id = rospy.get_param('~odom_frame_id', '/odom') 43 | base_frame_id = rospy.get_param('~base_frame_id', '/base_link') 44 | publish_tf = rospy.get_param('~publish_tf', True) 45 | 46 | # Quality parameters 47 | accept_quality = rospy.get_param('~quality','1,2,4,5,6') 48 | accept_quality = [int(x) for x in accept_quality.split(',')] 49 | accept_num_sats = int(rospy.get_param('~min_sat', 5)) 50 | accept_ratio = float(rospy.get_param('~min_ratio', 1.0)) 51 | 52 | enu = PoseStamped() 53 | enu.header.frame_id = '/map' 54 | time_ref_source = rospy.get_param('~time_ref_source', global_frame_id) 55 | gpstime = TimeReference() 56 | gpstime.source = time_ref_source 57 | trans_corr = (0.0, 0.0, 0.0) 58 | 59 | try: 60 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 61 | sock.connect((host, port)) 62 | file = sock.makefile() 63 | 64 | while not rospy.is_shutdown(): 65 | data = file.readline() 66 | time_now = rospy.get_rostime() 67 | fields = re.split('\s+', data) 68 | if fields[0] == '%': continue 69 | assert len(fields) is 16 70 | 71 | quality = int(fields[5]) 72 | nr_sats = int(fields[6]) 73 | ratio = float(fields[14]) 74 | 75 | # Pose 76 | enu.header.stamp = time_now 77 | enu.pose.position.x = float(fields[2]) 78 | enu.pose.position.y = float(fields[3]) 79 | enu.pose.position.z = float(fields[4]) 80 | 81 | # Timeref 82 | # Expects time as UTC 83 | gpstime.header.stamp = time_now 84 | t = time.strptime(fields[0] + ' ' + fields[1], "%Y/%m/%d %H:%M:%S.%f") 85 | gpstime.time_ref = rospy.Time.from_sec(calendar.timegm(t)) 86 | 87 | posepub.publish(enu) 88 | timepub.publish(gpstime) 89 | 90 | if not publish_tf: continue 91 | 92 | # Transform 93 | if quality in accept_quality and nr_sats >= accept_num_sats and ratio >= accept_ratio: 94 | try: 95 | tflisten.waitForTransform(odom_frame_id, base_frame_id, time_now, rospy.Duration(1.0)) 96 | (trans,rot) = tflisten.lookupTransform(odom_frame_id, base_frame_id, time_now) 97 | except tf.Exception: 98 | print("Catched tf.Exception, i.e. no transform received in time, using zero-values, (just for testing)") 99 | (trans, rot) = ((0,0,0), (0,0,0,0.1)) 100 | 101 | trans_gps = (float(fields[2]), float(fields[3]), float(fields[4])) 102 | trans_corr = (trans_gps[0] - trans[0], trans_gps[1] - trans[1], trans_gps[2] - trans[2]) 103 | rot = (0.0, 0.0, 0.0, 1.0) 104 | tfpub.sendTransform(trans_corr, rot, rospy.get_rostime(), odom_frame_id, global_frame_id) 105 | 106 | except rospy.ROSInterruptException: 107 | sock.close() 108 | 109 | -------------------------------------------------------------------------------- /rtkrcv/src/rtkrcv.cpp: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | * rtkrcv.cpp : based on rtk-gps/gnss receiver console app 3 | * 4 | * Copyright (C) 2009-2013 by T.TAKASU, All rights reserved. 5 | * Copyright (C) 2012 by M.STAHL, All rights reserved. 6 | * 7 | * notes : 8 | * current version does not support win32 without pthread library 9 | * 10 | * version : $Revision:$ $Date:$ 11 | * history : 2009/12/13 1.0 new 12 | * 2010/07/18 1.1 add option -m 13 | * 2010/08/12 1.2 fix bug on ftp/http 14 | * 2011/01/22 1.3 add option misc-proxyaddr,misc-fswapmargin 15 | * 2011/08/19 1.4 fix bug on size of arg solopt arg for rtksvrstart() 16 | * 2012/11/25 use as template for ROS node * 2012/11/03 1.5 fix bug on setting output format 17 | * 2013/06/30 1.6 add "nvs" option for inpstr*-format 18 | * 2013/09/07 update template for ROS node *-----------------------------------------------------------------------------*/ 19 | #ifndef WIN32 20 | #define _POSIX_C_SOURCE 2 21 | #endif 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #ifdef WIN32 28 | #else 29 | #include 30 | #include 31 | #include 32 | #endif 33 | #include 34 | 35 | #define MAXSTR 1024 /* max length of a string */ 36 | #define MAXBUFF 1024 /* max input buffer */ 37 | #define MAXRCVCMD 4096 /* max receiver command */ 38 | #define NAVIFILE "rtkrcv.nav" /* navigation save file */ 39 | #define STATFILE "rtkrcv.stat" /* solution status file */ 40 | #define TRACEFILE "rtkrcv.trace" /* trace file */ 41 | #define INTKEEPALIVE 1000 /* keep alive interval (ms) */ 42 | 43 | #define MIN(x,y) ((x)<(y)?(x):(y)) 44 | #define MAX(x,y) ((x)>(y)?(x):(y)) 45 | #define SQRT(x) ((x)<=0.0?0.0:sqrt(x)) 46 | 47 | /* type definition -----------------------------------------------------------*/ 48 | typedef struct { /* virtual terminal type */ 49 | int type; /* type (0:stdio,1:remote,2:device) */ 50 | int state; /* state(0:close,1:open) */ 51 | int in,out; /* input/output file descriptor */ 52 | int nbuff; /* number of data */ 53 | char buff[MAXBUFF]; /* input buffer */ 54 | pthread_t svr; /* input server */ 55 | pthread_t parent; /* parent thread */ 56 | pthread_mutex_t lock; /* lock flag */ 57 | pthread_cond_t event; /* event flag */ 58 | } vt_t; 59 | 60 | /* global variables ----------------------------------------------------------*/ 61 | static rtksvr_t svr; /* rtk server struct */ 62 | static stream_t moni; /* monitor stream */ 63 | 64 | static FILE *logfp =NULL; /* log file pointer */ 65 | 66 | static char passwd[MAXSTR]="admin"; /* login password */ 67 | static int timetype =0; /* time format (0:gpst,1:utc,2:jst,3:tow) */ 68 | static int soltype =0; /* sol format (0:dms,1:deg,2:xyz,3:enu,4:pyl) */ 69 | static int solflag =2; /* sol flag (1:std+2:age/ratio/ns) */ 70 | static int strtype[]={ /* stream types */ 71 | STR_SERIAL,STR_NONE,STR_NONE,STR_NONE,STR_NONE,STR_NONE,STR_NONE,STR_NONE 72 | }; 73 | static char strpath[8][MAXSTR]={""}; /* stream paths */ 74 | static int strfmt[]={ /* stream formats */ 75 | STRFMT_UBX,STRFMT_RTCM3,STRFMT_SP3,SOLF_LLH,SOLF_NMEA 76 | }; 77 | static int svrcycle =10; /* server cycle (ms) */ 78 | static int timeout =10000; /* timeout time (ms) */ 79 | static int reconnect =10000; /* reconnect interval (ms) */ 80 | static int nmeacycle =5000; /* nmea request cycle (ms) */ 81 | static int buffsize =32768; /* input buffer size (bytes) */ 82 | static int navmsgsel =0; /* navigation mesaage select */ 83 | static char proxyaddr[256]=""; /* http/ntrip proxy */ 84 | static int nmeareq =0; /* nmea request type (0:off,1:lat/lon,2:single) */ 85 | static double nmeapos[] ={0,0}; /* nmea position (lat/lon) (deg) */ 86 | static char rcvcmds[3][MAXSTR]={""}; /* receiver commands files */ 87 | static char startcmd[MAXSTR]=""; /* start command */ 88 | static char stopcmd [MAXSTR]=""; /* stop command */ 89 | static int modflgr[256] ={0}; /* modified flags of receiver options */ 90 | static int modflgs[256] ={0}; /* modified flags of system options */ 91 | static int moniport =0; /* monitor port */ 92 | static int keepalive =0; /* keep alive flag */ 93 | static int fswapmargin =30; /* file swap margin (s) */ 94 | 95 | static prcopt_t prcopt; /* processing options */ 96 | static solopt_t solopt[2]={{0}}; /* solution options */ 97 | static filopt_t filopt ={""}; /* file options */ 98 | 99 | /* receiver options table ----------------------------------------------------*/ 100 | #define TIMOPT "0:gpst,1:utc,2:jst,3:tow" 101 | #define CONOPT "0:dms,1:deg,2:xyz,3:enu,4:pyl" 102 | #define FLGOPT "0:off,1:std+2:age/ratio/ns" 103 | #define ISTOPT "0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http" 104 | #define OSTOPT "0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripsvr" 105 | #define FMTOPT "0:rtcm2,1:rtcm3,2:oem4,3:oem3,4:ubx,5:ss2,6:hemis,7:skytraq,8:gw10,9:javad,10:nvs,15:sp3" 106 | #define NMEOPT "0:off,1:latlon,2:single" 107 | #define SOLOPT "0:llh,1:xyz,2:enu,3:nmea" 108 | #define MSGOPT "0:all,1:rover,2:base,3:corr" 109 | 110 | static opt_t rcvopts[]={ 111 | {"console-passwd", 2, (void *)passwd, "" }, 112 | {"console-timetype",3, (void *)&timetype, TIMOPT }, 113 | {"console-soltype", 3, (void *)&soltype, CONOPT }, 114 | {"console-solflag", 0, (void *)&solflag, FLGOPT }, 115 | 116 | {"inpstr1-type", 3, (void *)&strtype[0], ISTOPT }, 117 | {"inpstr2-type", 3, (void *)&strtype[1], ISTOPT }, 118 | {"inpstr3-type", 3, (void *)&strtype[2], ISTOPT }, 119 | {"inpstr1-path", 2, (void *)strpath [0], "" }, 120 | {"inpstr2-path", 2, (void *)strpath [1], "" }, 121 | {"inpstr3-path", 2, (void *)strpath [2], "" }, 122 | {"inpstr1-format", 3, (void *)&strfmt [0], FMTOPT }, 123 | {"inpstr2-format", 3, (void *)&strfmt [1], FMTOPT }, 124 | {"inpstr3-format", 3, (void *)&strfmt [2], FMTOPT }, 125 | {"inpstr2-nmeareq", 3, (void *)&nmeareq, NMEOPT }, 126 | {"inpstr2-nmealat", 1, (void *)&nmeapos[0], "deg" }, 127 | {"inpstr2-nmealon", 1, (void *)&nmeapos[1], "deg" }, 128 | {"outstr1-type", 3, (void *)&strtype[3], OSTOPT }, 129 | {"outstr2-type", 3, (void *)&strtype[4], OSTOPT }, 130 | {"outstr1-path", 2, (void *)strpath [3], "" }, 131 | {"outstr2-path", 2, (void *)strpath [4], "" }, 132 | {"outstr1-format", 3, (void *)&strfmt [3], SOLOPT }, 133 | {"outstr2-format", 3, (void *)&strfmt [4], SOLOPT }, 134 | {"logstr1-type", 3, (void *)&strtype[5], OSTOPT }, 135 | {"logstr2-type", 3, (void *)&strtype[6], OSTOPT }, 136 | {"logstr3-type", 3, (void *)&strtype[7], OSTOPT }, 137 | {"logstr1-path", 2, (void *)strpath [5], "" }, 138 | {"logstr2-path", 2, (void *)strpath [6], "" }, 139 | {"logstr3-path", 2, (void *)strpath [7], "" }, 140 | 141 | {"misc-svrcycle", 0, (void *)&svrcycle, "ms" }, 142 | {"misc-timeout", 0, (void *)&timeout, "ms" }, 143 | {"misc-reconnect", 0, (void *)&reconnect, "ms" }, 144 | {"misc-nmeacycle", 0, (void *)&nmeacycle, "ms" }, 145 | {"misc-buffsize", 0, (void *)&buffsize, "bytes"}, 146 | {"misc-navmsgsel", 3, (void *)&navmsgsel, MSGOPT }, 147 | {"misc-proxyaddr", 2, (void *)proxyaddr, "" }, 148 | {"misc-fswapmargin",0, (void *)&fswapmargin, "s" }, 149 | 150 | {"misc-startcmd", 2, (void *)startcmd, "" }, 151 | {"misc-stopcmd", 2, (void *)stopcmd, "" }, 152 | 153 | {"file-cmdfile1", 2, (void *)rcvcmds[0], "" }, 154 | {"file-cmdfile2", 2, (void *)rcvcmds[1], "" }, 155 | {"file-cmdfile3", 2, (void *)rcvcmds[2], "" }, 156 | 157 | {"",0,NULL,""} 158 | }; 159 | 160 | /* discard space characters at tail ------------------------------------------*/ 161 | static void chop(char *str) 162 | { 163 | char *p; 164 | for (p=str+strlen(str)-1;p>=str&&!isgraph((int)*p);p--) *p='\0'; 165 | } 166 | /* thread to send keep alive for monitor port --------------------------------*/ 167 | static void *sendkeepalive(void *arg) 168 | { 169 | trace(3,"sendkeepalive: start\n"); 170 | 171 | while (keepalive) { 172 | strwrite(&moni,(unsigned char *)"\r",1); 173 | sleepms(INTKEEPALIVE); 174 | } 175 | trace(3,"sendkeepalive: stop\n"); 176 | return NULL; 177 | } 178 | /* open monitor port ---------------------------------------------------------*/ 179 | static int openmoni(int port) 180 | { 181 | pthread_t thread; 182 | char path[64]; 183 | 184 | trace(3,"openmomi: port=%d\n",port); 185 | 186 | sprintf(path,":%d",port); 187 | if (!stropen(&moni,STR_TCPSVR,STR_MODE_RW,path)) return 0; 188 | strsettimeout(&moni,timeout,reconnect); 189 | keepalive=1; 190 | pthread_create(&thread,NULL,sendkeepalive,NULL); 191 | return 1; 192 | } 193 | /* close monitor port --------------------------------------------------------*/ 194 | static void closemoni(void) 195 | { 196 | trace(3,"closemoni:\n"); 197 | keepalive=0; 198 | 199 | /* send disconnect message */ 200 | strwrite(&moni,(unsigned char *)MSG_DISCONN,strlen(MSG_DISCONN)); 201 | 202 | /* wait fin from clients */ 203 | sleepms(1000); 204 | 205 | strclose(&moni); 206 | } 207 | /* read remote console -------------------------------------------------------*/ 208 | static int readremote(vt_t *vt, char *buff, int nmax) 209 | { 210 | int n=0; 211 | 212 | trace(4,"readremote:\n"); 213 | 214 | if (!vt->state) return 0; 215 | pthread_cond_wait(&vt->event,&vt->lock); 216 | if (vt->state) { 217 | n=MIN(nmax,vt->nbuff); 218 | memcpy(buff,vt->buff,n); 219 | vt->nbuff=0; 220 | } 221 | pthread_mutex_unlock(&vt->lock); 222 | return n; 223 | } 224 | /* write remote console ------------------------------------------------------*/ 225 | static int writeremote(vt_t *vt, char *buff, int n) 226 | { 227 | char *p,*q,crlf[]="\r\n";; 228 | 229 | trace(4,"writeremote: n=%d\n",n); 230 | 231 | if (!vt->state) return 0; 232 | 233 | for (p=buff;pout,p,buff+n-p)out,p,q-p)out,crlf,2)<2) return 0; 240 | } 241 | return 1; 242 | } 243 | /* output to console ---------------------------------------------------------*/ 244 | static int outvt(vt_t *vt, char *buff, int n) 245 | { 246 | trace(4,"outvt: n=%d\n",n); 247 | 248 | if (logfp) fwrite(buff,n,1,logfp); 249 | if (vt->type==1) return writeremote(vt,buff,n); 250 | return write(vt->out,buff,n)==n; 251 | } 252 | /* input line from console ---------------------------------------------------*/ 253 | static int inpvt(vt_t *vt, char *buff, int nmax) 254 | { 255 | int n; 256 | 257 | trace(4,"inpvt:\n"); 258 | 259 | if (vt->type==1) n=readremote(vt,buff,nmax-1); 260 | else n=read(vt->in,buff,nmax-1); 261 | if (n<=0) { 262 | outvt(vt,"\n",1); 263 | return 0; 264 | } 265 | buff[n]='\0'; 266 | if (logfp) fprintf(logfp,"%s",buff); 267 | chop(buff); 268 | return 1; 269 | } 270 | /* printf to console ---------------------------------------------------------*/ 271 | static int printvt(vt_t *vt, const char *format, ...) 272 | { 273 | va_list ap; 274 | int n; 275 | char buff[MAXSTR]; 276 | 277 | trace(4,"prvt:\n"); 278 | 279 | if (!vt->state) { 280 | va_start(ap,format); 281 | vfprintf(stderr,format,ap); 282 | va_end(ap); 283 | return 1; 284 | } 285 | va_start(ap,format); 286 | n=vsprintf(buff,format,ap); 287 | va_end(ap); 288 | return outvt(vt,buff,n); 289 | } 290 | /* confirm overwrite ---------------------------------------------------------*/ 291 | static int confwrite(vt_t *vt, const char *file) 292 | { 293 | FILE *fp; 294 | char buff[MAXSTR],*p; 295 | 296 | strcpy(buff,file); 297 | if ((p=strstr(buff,"::"))) *p='\0'; /* omit options in path */ 298 | if (!vt->state||!(fp=fopen(buff,"r"))) return 1; /* no existing file */ 299 | fclose(fp); 300 | printvt(vt,"overwrite %-16s ? (y/n): ",buff); 301 | if (!inpvt(vt,buff,sizeof(buff))) return 0; 302 | return toupper((int)buff[0])=='Y'; 303 | } 304 | /* read receiver commands ----------------------------------------------------*/ 305 | static int readcmd(const char *file, char *cmd, int type) 306 | { 307 | FILE *fp; 308 | char buff[MAXSTR],*p=cmd; 309 | int i=0; 310 | 311 | trace(3,"readcmd: file=%s\n",file); 312 | 313 | if (!(fp=fopen(file,"r"))) return 0; 314 | 315 | while (fgets(buff,sizeof(buff),fp)) { 316 | if (*buff=='@') i=1; 317 | else if (i==type&&p+strlen(buff)+1pcvr[0]=opt->pcvr[1]=pcv0; 336 | if (!*filopt.rcvantp) return; 337 | 338 | if (readpcv(filopt.rcvantp,&pcvr)) { 339 | for (i=0;i<2;i++) { 340 | if (!*opt->anttype[i]) continue; 341 | if (!(pcv=searchpcv(0,opt->anttype[i],time,&pcvr))) { 342 | printvt(vt,"no antenna %s in %s",opt->anttype[i],filopt.rcvantp); 343 | continue; 344 | } 345 | opt->pcvr[i]=*pcv; 346 | } 347 | } 348 | else printvt(vt,"antenna file open error %s",filopt.rcvantp); 349 | 350 | if (readpcv(filopt.satantp,&pcvs)) { 351 | for (i=0;ipcvs[i]=*pcv; 354 | } 355 | } 356 | else printvt(vt,"antenna file open error %s",filopt.satantp); 357 | 358 | free(pcvr.pcv); free(pcvs.pcv); 359 | } 360 | /* start rtk server ----------------------------------------------------------*/ 361 | static int startsvr(vt_t *vt) 362 | { 363 | double pos[3],npos[3]; 364 | char s[3][MAXRCVCMD]={"","",""},*cmds[]={NULL,NULL,NULL}; 365 | char *ropts[]={"","",""}; 366 | char *paths[]={ 367 | strpath[0],strpath[1],strpath[2],strpath[3],strpath[4],strpath[5], 368 | strpath[6],strpath[7] 369 | }; 370 | int i,ret,stropt[8]={0}; 371 | 372 | trace(3,"startsvr:\n"); 373 | 374 | /* read start commads from command files */ 375 | for (i=0;i<3;i++) { 376 | if (!*rcvcmds[i]) continue; 377 | if (!readcmd(rcvcmds[i],s[i],0)) { 378 | printvt(vt,"no command file: %s\n",rcvcmds[i]); 379 | } 380 | else cmds[i]=s[i]; 381 | } 382 | /* confirm overwrite */ 383 | for (i=3;i<8;i++) { 384 | if (strtype[i]==STR_FILE&&!confwrite(vt,strpath[i])) return 0; 385 | } 386 | if (prcopt.refpos==4) { /* rtcm */ 387 | for (i=0;i<3;i++) prcopt.rb[i]=0.0; 388 | } 389 | pos[0]=nmeapos[0]*D2R; 390 | pos[1]=nmeapos[1]*D2R; 391 | pos[2]=0.0; 392 | pos2ecef(pos,npos); 393 | 394 | /* read antenna file */ 395 | readant(vt,&prcopt,&svr.nav); 396 | 397 | /* open geoid data file */ 398 | if (solopt[0].geoid>0&&!opengeoid(solopt[0].geoid,filopt.geoid)) { 399 | trace(2,"geoid data open error: %s\n",filopt.geoid); 400 | printvt(vt,"geoid data open error: %s\n",filopt.geoid); 401 | } 402 | for (i=0;*rcvopts[i].name;i++) modflgr[i]=0; 403 | for (i=0;*sysopts[i].name;i++) modflgs[i]=0; 404 | 405 | /* set stream options */ 406 | stropt[0]=timeout; 407 | stropt[1]=reconnect; 408 | stropt[2]=1000; 409 | stropt[3]=buffsize; 410 | stropt[4]=fswapmargin; 411 | strsetopt(stropt); 412 | 413 | if (strfmt[2]==8) strfmt[2]=STRFMT_SP3; 414 | 415 | /* set ftp/http directory and proxy */ 416 | strsetdir(filopt.tempdir); 417 | strsetproxy(proxyaddr); 418 | 419 | /* execute start command */ 420 | if (*startcmd&&(ret=system(startcmd))) { 421 | trace(2,"command exec error: %s (%d)\n",startcmd,ret); 422 | printvt(vt,"command exec error: %s (%d)\n",startcmd,ret); 423 | } 424 | solopt[0].posf=strfmt[3]; 425 | solopt[1].posf=strfmt[4]; 426 | 427 | /* start rtk server */ 428 | if (!rtksvrstart(&svr,svrcycle,buffsize,strtype,paths,strfmt,navmsgsel, 429 | cmds,ropts,nmeacycle,nmeareq,npos,&prcopt,solopt,&moni)) { 430 | trace(2,"rtk server start error\n"); 431 | printvt(vt,"rtk server start error\n"); 432 | return 0; 433 | } 434 | return 1; 435 | } 436 | /* stop rtk server -----------------------------------------------------------*/ 437 | static void stopsvr(vt_t *vt) 438 | { 439 | char s[3][MAXRCVCMD]={"","",""},*cmds[]={NULL,NULL,NULL}; 440 | int i,ret; 441 | 442 | trace(3,"stopsvr:\n"); 443 | 444 | if (!svr.state) return; 445 | 446 | /* read stop commads from command files */ 447 | for (i=0;i<3;i++) { 448 | if (!*rcvcmds[i]) continue; 449 | if (!readcmd(rcvcmds[i],s[i],1)) { 450 | printvt(vt,"no command file: %s\n",rcvcmds[i]); 451 | } 452 | else cmds[i]=s[i]; 453 | } 454 | /* stop rtk server */ 455 | rtksvrstop(&svr,cmds); 456 | 457 | /* execute stop command */ 458 | if (*stopcmd&&(ret=system(stopcmd))) { 459 | trace(2,"command exec error: %s (%d)\n",stopcmd,ret); 460 | printvt(vt,"command exec error: %s (%d)\n",stopcmd,ret); 461 | } 462 | if (solopt[0].geoid>0) closegeoid(); 463 | } 464 | 465 | /* rtkrcv main ----------------------------------------------------------------- 466 | * sysnopsis 467 | * rtkrcv [-m port][-t level][-r level] 468 | * 469 | * description 470 | * A ROS version of the real-time positioning AP by rtklib. To start 471 | * or stop RTK server, to configure options or to print solution/status, 472 | * login a console and input commands. As default, stdin/stdout are used for 473 | * the console. Use -p option for network login with telnet protocol. To show 474 | * the available commands, type ? or help on the console. The initial 475 | * processing options are loaded from ROS parameters. To configure the 476 | * processing options, create a launch file or use set, load or save command 477 | * on the console. 478 | * 479 | * option 480 | * -m port port number for monitor stream 481 | * -r level output solution status file (0:off,1:states,2:residuals) 482 | * -t level debug trace level (0:off,1-5:on) 483 | * 484 | *-----------------------------------------------------------------------------*/ 485 | 486 | static vt_t svr_vt={0}; 487 | static int outstat=0,rcv_trace=0; 488 | 489 | int loadrosopts(opt_t *opts); 490 | 491 | int start_rtkrcv(int argc, char **argv) 492 | { 493 | int i; 494 | 495 | for (i=1;i0) { 502 | traceopen(TRACEFILE); 503 | tracelevel(rcv_trace); 504 | } 505 | /* initialize rtk server and monitor port */ 506 | rtksvrinit(&svr); 507 | strinit(&moni); 508 | 509 | /* load options from ROS */ 510 | resetsysopts(); 511 | loadrosopts(rcvopts); 512 | loadrosopts(sysopts); 513 | getsysopts(&prcopt,solopt,&filopt); 514 | 515 | /* read navigation data */ 516 | if (!readnav(NAVIFILE,&svr.nav)) { 517 | fprintf(stderr,"no navigation data: %s\n",NAVIFILE); 518 | } 519 | if (outstat>0) { 520 | rtkopenstat(STATFILE,outstat); 521 | } 522 | /* open monitor port */ 523 | if (moniport>0&&!openmoni(moniport)) { 524 | fprintf(stderr,"monitor port open error: %d\n",moniport); 525 | return 0; 526 | } 527 | /* start rtk server */ 528 | if (!startsvr(&svr_vt)) return 0; 529 | return 1; 530 | } 531 | 532 | void stop_rtkrcv() { 533 | /* stop rtk server */ 534 | stopsvr(&svr_vt); 535 | 536 | if (moniport>0) closemoni(); 537 | 538 | if (outstat>0) rtkclosestat(); 539 | 540 | if (rcv_trace>0) traceclose(); 541 | 542 | /* save navigation data */ 543 | if (!savenav(NAVIFILE,&svr.nav)) { 544 | fprintf(stderr,"navigation data save error: %s\n",NAVIFILE); 545 | } 546 | } 547 | 548 | char get_solution(double &lat, double &lon, double &height) 549 | { 550 | rtksvrlock(&svr); 551 | const sol_t *sol = &svr.solbuf[svr.nsol-1]; 552 | const double *rb = svr.rtk.rb; 553 | double pos[3]={0},Qr[9],Qe[9]={0},dms1[3]={0},dms2[3]={0},bl[3]={0}; 554 | double enu[3]={0},pitch=0.0,yaw=0.0,len; 555 | int i; 556 | int status; 557 | 558 | if (sol->time.time == 0 || !sol->stat) { 559 | rtksvrunlock(&svr); 560 | return -1; // no fix 561 | } 562 | if (1 <= sol->stat && sol->stat <= 2) status = 0; // fix 563 | if (sol->stat == 3) status = 1; // sbas fix 564 | else status = 2; // differential fix 565 | 566 | if (norm(sol->rr,3) > 0.0 && norm(rb,3) > 0.0) { 567 | for (i=0;i<3;i++) bl[i]=sol->rr[i]-rb[i]; 568 | } 569 | len=norm(bl,3); 570 | Qr[0]=sol->qr[0]; 571 | Qr[4]=sol->qr[1]; 572 | Qr[8]=sol->qr[2]; 573 | Qr[1]=Qr[3]=sol->qr[3]; 574 | Qr[5]=Qr[7]=sol->qr[4]; 575 | Qr[2]=Qr[6]=sol->qr[5]; 576 | 577 | if (norm(sol->rr,3) > 0.0) { 578 | ecef2pos(sol->rr,pos); 579 | covenu(pos,Qr,Qe); 580 | lat = pos[0]*R2D; 581 | lon = pos[1]*R2D; 582 | if (solopt[0].height == 1) pos[2]-=geoidh(pos); /* geodetic */ 583 | height = pos[2]; 584 | } 585 | svr.nsol=0; 586 | rtksvrunlock(&svr); 587 | return status; 588 | } 589 | --------------------------------------------------------------------------------