├── .circleci └── config.yml ├── .gitignore ├── README.md ├── novatel_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg │ ├── Ack.msg │ ├── BESTPOS.msg │ ├── CORRIMUDATA.msg │ ├── CommonFooter.msg │ ├── CommonHeader.msg │ ├── INSCOV.msg │ └── INSPVAX.msg └── package.xml └── novatel_span_driver ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch ├── example.launch └── example_for_usb.launch ├── package.xml ├── scripts └── driver ├── setup.py ├── src └── novatel_span_driver │ ├── __init__.py │ ├── bridge.py │ ├── data.py │ ├── diagnostics.py │ ├── handlers.py │ ├── mapping.py │ ├── monitor.py │ ├── port.py │ ├── publisher.py │ ├── translator.py │ └── wheel_velocity.py └── test ├── propak6-basic-fix.pcap.gz ├── propak6-basic-fix.test ├── propak6-ins-ppp.pcap.gz └── propak6-ins-ppp.test /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | jobs: 3 | kinetic: 4 | docker: 5 | - image: ros:kinetic-perception 6 | steps: 7 | - checkout 8 | - run: 9 | name: Set Up Container 10 | command: | 11 | apt-get update -qq && apt-get install -y python-catkin-tools 12 | source `find /opt/ros -name setup.bash | sort | head -1` 13 | rosdep install --from-paths . --ignore-src -y 14 | cd .. 15 | catkin init 16 | catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release 17 | - run: 18 | name: Build 19 | command: | 20 | cd .. 21 | catkin build --limit-status-rate 0.5 22 | - run: 23 | name: Lint 24 | command: | 25 | cd .. 26 | catkin build --no-deps novatel_span_driver --make-args roslint 27 | - run: 28 | name: Run Tests 29 | command: | 30 | source `find /opt/ros -name setup.bash | sort | head -1` 31 | cd .. 32 | catkin run_tests --limit-status-rate 0.5 33 | catkin_test_results 34 | working_directory: ~/src 35 | 36 | melodic: 37 | docker: 38 | - image: ros:melodic-perception 39 | steps: 40 | - checkout 41 | - run: 42 | name: Set Up Container 43 | command: | 44 | apt-get update -qq && apt-get install -y python-catkin-tools 45 | source `find /opt/ros -name setup.bash | sort | head -1` 46 | rosdep install --from-paths . --ignore-src -y 47 | cd .. 48 | catkin init 49 | catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release 50 | - run: 51 | name: Build 52 | command: | 53 | cd .. 54 | catkin build --limit-status-rate 0.5 55 | - run: 56 | name: Lint 57 | command: | 58 | cd .. 59 | catkin build --no-deps novatel_span_driver --make-args roslint 60 | - run: 61 | name: Run Tests 62 | command: | 63 | source `find /opt/ros -name setup.bash | sort | head -1` 64 | cd .. 65 | catkin run_tests --limit-status-rate 0.5 66 | catkin_test_results 67 | working_directory: ~/src 68 | 69 | workflows: 70 | version: 2 71 | ros_build: 72 | jobs: 73 | - kinetic 74 | - melodic 75 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | *.py[oc] 3 | *.sw[op] 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # novatel_span_driver 2 | 3 | [![CircleCI](https://circleci.com/gh/ros-drivers/novatel_span_driver.svg?style=svg)](https://circleci.com/gh/ros-drivers/novatel_span_driver) 4 | 5 | This ROS package connects via Ethernet to a [NovAtel](http://www.novatel.com/) receiver running 6 | [SPAN](http://www.novatel.com/span). 7 | 8 | Please see the ROS Wiki for details: http://wiki.ros.org/novatel_span_driver 9 | -------------------------------------------------------------------------------- /novatel_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package novatel_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2018-04-26) 6 | ------------------ 7 | * Adding new maintainers. 8 | * Contributors: Joshua Whitley 9 | 10 | 1.0.0 (2014-11-25) 11 | ------------------ 12 | * Fix velx/y mixup, add diagnostic publisher. 13 | * BESTPOSB -> BESTPOS, add diagnostic constants. 14 | * Update messages to include header, fix header to match documentation. 15 | * Contributors: Mike Purvis 16 | 17 | 0.2.0 (2014-10-29) 18 | ------------------ 19 | * Remove a bunch of unused messages. 20 | * Add WHEELVELOCITY msg. 21 | * Contributors: Mike Purvis 22 | 23 | 0.1.0 (2014-10-24) 24 | ------------------ 25 | * Package mostly unchanged from work done by NovAtel. 26 | * Contributors: Mike Purvis 27 | -------------------------------------------------------------------------------- /novatel_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(novatel_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | message_generation 7 | std_msgs 8 | nav_msgs 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | Ack.msg 14 | CommonFooter.msg 15 | CommonHeader.msg 16 | BESTPOS.msg 17 | CORRIMUDATA.msg 18 | INSCOV.msg 19 | INSPVAX.msg 20 | ) 21 | 22 | generate_messages( 23 | DEPENDENCIES 24 | geometry_msgs 25 | std_msgs 26 | nav_msgs 27 | ) 28 | 29 | catkin_package( 30 | CATKIN_DEPENDS geometry_msgs message_runtime nav_msgs std_msgs 31 | ) 32 | 33 | -------------------------------------------------------------------------------- /novatel_msgs/msg/Ack.msg: -------------------------------------------------------------------------------- 1 | uint16 transaction 2 | uint16 id 3 | 4 | uint16 RESPONSE_NOT_APPLICABLE=0 5 | uint16 RESPONSE_ACCEPTED=1 6 | uint16 RESPONSE_ACCEPTED_TOO_LONG=2 7 | uint16 RESPONSE_ACCEPTED_TOO_SHORT=3 8 | uint16 RESPONSE_PARAM_ERROR=4 9 | uint16 RESPONSE_NOT_APPLICABLE_IN_CURRENT_STATE=5 10 | uint16 RESPONSE_DATA_NOT_AVAILABLE=6 11 | uint16 RESPONSE_MESSAGE_START_ERROR=7 12 | uint16 RESPONSE_MESSAGE_END_ERROR=8 13 | uint16 RESPONSE_BYTE_COUNT_ERROR=9 14 | uint16 RESPONSE_CHECKSUM_ERROR=10 15 | uint16 response_code 16 | 17 | uint8 PARAMS_NO_CHANGE=0 18 | uint8 PARAMS_CHANGE=1 19 | uint8 params_status 20 | 21 | uint8[32] error_parameter_name 22 | -------------------------------------------------------------------------------- /novatel_msgs/msg/BESTPOS.msg: -------------------------------------------------------------------------------- 1 | # message 42 2 | novatel_msgs/CommonHeader header 3 | 4 | # Table 21 in the SPAN on OEM6 manual. 5 | # See: http://www.novatel.com/assets/Documents/Manuals/OM-20000144UM.pdf#page=99 6 | uint32 solution_status 7 | uint32 SOLUTION_STATUS_SOLUTION_COMPUTED=0 8 | uint32 SOLUTION_STATUS_INSUFFICIENT_OBSERVATIONS=1 9 | uint32 SOLUTION_STATUS_NO_CONVERGENCE=2 10 | uint32 SOLUTION_STATUS_SINGULARITY_AT_PARAMETERS_MATRIX=3 11 | uint32 SOLUTION_STATUS_COVARIANCE_TRACE_EXCEEDS_MAXIMUM=4 12 | uint32 SOLUTION_STATUS_TEST_DISTANCE_EXCEEDED=5 13 | uint32 SOLUTION_STATUS_COLD_START=6 14 | uint32 SOLUTION_STATUS_VELOCITY_OR_HEIGHT_LIMIT_EXCEEDED=7 15 | uint32 SOLUTION_STATUS_VARIANCE_EXCEEDS_LIMITS=8 16 | uint32 SOLUTION_STATUS_RESIDUALS_TOO_LARGE=9 17 | uint32 SOLUTION_STATUS_INTEGRITY_WARNING=13 18 | uint32 SOLUTION_STATUS_PENDING=18 19 | uint32 SOLUTION_STATUS_INVALID_FIX=19 20 | uint32 SOLUTION_STATUS_INVALID_RATE=22 21 | 22 | # Table 22 in the SPAN on OEM6 manual. 23 | # http://www.novatel.com/assets/Documents/Manuals/OM-20000144UM.pdf#page=100 24 | uint32 position_type 25 | uint32 POSITION_TYPE_NONE=0 26 | uint32 POSITION_TYPE_FIXED=1 27 | uint32 POSITION_TYPE_FIXEDHEIGHT=2 28 | uint32 POSITION_TYPE_FLOATCONV=4 29 | uint32 POSITION_TYPE_WIDELANE=5 30 | uint32 POSITION_TYPE_NARROWLANE=6 31 | uint32 POSITION_TYPE_DOPPLER_VELOCITY=8 32 | uint32 POSITION_TYPE_SINGLE=16 33 | uint32 POSITION_TYPE_PSRDIFF=17 34 | uint32 POSITION_TYPE_WAAS=18 35 | uint32 POSITION_TYPE_PROPAGATED=19 36 | uint32 POSITION_TYPE_OMNISTAR=20 37 | uint32 POSITION_TYPE_L1_FLOAT=32 38 | uint32 POSITION_TYPE_IONOFREE_FLOAT=33 39 | uint32 POSITION_TYPE_NARROW_FLOAT=34 40 | uint32 POSITION_TYPE_L1_INT=48 41 | uint32 POSITION_TYPE_WIDE_INT=49 42 | uint32 POSITION_TYPE_NARROW_INT=50 43 | uint32 POSITION_TYPE_RTK_DIRECT_INS=51 44 | uint32 POSITION_TYPE_INS_SBAS=52 45 | uint32 POSITION_TYPE_INS_PSRSP=53 46 | uint32 POSITION_TYPE_INS_PSRDIFF=54 47 | uint32 POSITION_TYPE_INS_RTKFLOAT=55 48 | uint32 POSITION_TYPE_INS_RTKFIXED=56 49 | uint32 POSITION_TYPE_INS_OMNISTAR=57 50 | uint32 POSITION_TYPE_INS_OMNISTAR_HP=58 51 | uint32 POSITION_TYPE_INS_OMNISTAR_XP=59 52 | uint32 POSITION_TYPE_OMNISTAR_HP=64 53 | uint32 POSITION_TYPE_OMNISTAR_XP=65 54 | uint32 POSITION_TYPE_PPP_CONVERGING=68 55 | uint32 POSITION_TYPE_PPP=69 56 | uint32 POSITION_TYPE_INS_PPP_CONVERGING=73 57 | uint32 POSITION_TYPE_INS_PPP=74 58 | 59 | float64 latitude 60 | float64 longitude 61 | float64 altitude 62 | 63 | float32 undulation 64 | uint32 datum_id 65 | 66 | float32 latitude_std 67 | float32 longitude_std 68 | float32 altitude_std 69 | 70 | char[4] stn_id 71 | 72 | float32 diff_age 73 | float32 sol_age 74 | 75 | uint8 svs 76 | uint8 sol_svs 77 | uint8 l1_svs 78 | uint8 mult_svs 79 | uint8 reserved 80 | uint8 extsolstat 81 | uint8 gal_sig_mask 82 | uint8 sig_mask 83 | 84 | -------------------------------------------------------------------------------- /novatel_msgs/msg/CORRIMUDATA.msg: -------------------------------------------------------------------------------- 1 | # message 812 2 | # Contains the RAWIMU data corrected for gravity, 3 | # the earth's rotation and estimated sensor errors. 4 | novatel_msgs/CommonHeader header 5 | 6 | int32 week 7 | float64 gpssec 8 | 9 | # Gyro values are instantaneous and in radians, must 10 | # multiply by datarate to get rad/s. 11 | float64 pitch_rate 12 | float64 roll_rate 13 | float64 yaw_rate 14 | 15 | float64 x_accel 16 | float64 y_accel 17 | float64 z_accel 18 | -------------------------------------------------------------------------------- /novatel_msgs/msg/CommonFooter.msg: -------------------------------------------------------------------------------- 1 | uint32 checksum 2 | 3 | -------------------------------------------------------------------------------- /novatel_msgs/msg/CommonHeader.msg: -------------------------------------------------------------------------------- 1 | # On the wire, this header is preceeded by three sync bytes, 2 | # which are 0xAA 0x44 0x12, and a uint8 which is the header length. 3 | 4 | # Message ID of the log being output. 5 | uint16 id 6 | 7 | # Measurement source, format, response bit. 8 | uint8 msg_type 9 | 10 | uint8 port_addr 11 | uint16 length 12 | uint16 sequence 13 | 14 | uint8 idle_time 15 | uint8 time_status 16 | 17 | uint16 gps_week 18 | uint32 gps_week_seconds 19 | 20 | # Table 3 in the SPAN on OEM6 manual. 21 | # See: http://www.novatel.com/assets/Documents/Manuals/OM-20000144UM.pdf#page=13 22 | uint32 receiver_status 23 | uint32 RECEIVER_STATUS_ERROR=1 24 | uint32 RECEIVER_STATUS_TEMPERATURE_WARNING=2 25 | uint32 RECEIVER_STATUS_VOLTAGE_SUPPLY_WARNING=4 26 | uint32 RECEIVER_STATUS_ANTENNA_UNPOWERED=8 27 | uint32 RECEIVER_STATUS_LNA_FAILURE=16 28 | uint32 RECEIVER_STATUS_ANTENNA_OPEN=32 29 | uint32 RECEIVER_STATUS_ANTENNA_SHORTED=64 30 | uint32 RECEIVER_STATUS_CPU_OVERLOADED=128 31 | uint32 RECEIVER_STATUS_COM1_BUFFER_OVERRUN=256 32 | uint32 RECEIVER_STATUS_COM2_BUFFER_OVERRUN=512 33 | uint32 RECEIVER_STATUS_COM3_BUFFER_OVERRUN=1024 34 | uint32 RECEIVER_STATUS_LINK_OVERLOAD=2048 35 | uint32 RECEIVER_STATUS_AUX_TRANSMIT_OVERRUN=8192 36 | uint32 RECEIVER_STATUS_AGC_OUT_OF_RANGE=16384 37 | uint32 RECEIVER_STATUS_INS_RESET=65536 38 | uint32 RECEIVER_STATUS_ALMANAC_INVALID=262144 39 | uint32 RECEIVER_STATUS_POSITION_SOLUTION_INVALID=524288 40 | uint32 RECEIVER_STATUS_POSITION_NOT_FIXED=1048576 41 | uint32 RECEIVER_STATUS_CLOCK_STEERING_DISABLED=2097152 42 | uint32 RECEIVER_STATUS_CLOCK_MODEL_INVALID=4194304 43 | uint32 RECEIVER_STATUS_EXTERNAL_OSCILLATOR_LOCKED=8388608 44 | uint32 RECEIVER_STATUS_SOFTWARE_RESOURCE_WARNING=16777216 45 | uint32 RECEIVER_STATUS_AUXILIARY3_EVENT=536870912 46 | uint32 RECEIVER_STATUS_AUXILIARY2_EVENT=1073741824 47 | uint32 RECEIVER_STATUS_AUXILIARY1_EVENT=2147483648 48 | 49 | uint16 reserved 50 | uint16 software_version 51 | 52 | -------------------------------------------------------------------------------- /novatel_msgs/msg/INSCOV.msg: -------------------------------------------------------------------------------- 1 | # message 264 2 | novatel_msgs/CommonHeader header 3 | 4 | int32 week 5 | float64 gpssec 6 | 7 | float64 pos11 8 | float64 pos12 9 | float64 pos13 10 | float64 pos21 11 | float64 pos22 12 | float64 pos23 13 | float64 pos31 14 | float64 pos32 15 | float64 pos33 16 | float64 att11 17 | float64 att12 18 | float64 att13 19 | float64 att21 20 | float64 att22 21 | float64 att23 22 | float64 att31 23 | float64 att32 24 | float64 att33 25 | float64 vel11 26 | float64 vel12 27 | float64 vel13 28 | float64 vel21 29 | float64 vel22 30 | float64 vel23 31 | float64 vel31 32 | float64 vel32 33 | float64 vel33 34 | -------------------------------------------------------------------------------- /novatel_msgs/msg/INSPVAX.msg: -------------------------------------------------------------------------------- 1 | # message 1465 2 | novatel_msgs/CommonHeader header 3 | 4 | # Table 29 in the SPAN on OEM6 manual: 5 | # See: http://www.novatel.com/assets/Documents/Manuals/OM-20000144UM.pdf#page=121 6 | uint32 ins_status 7 | uint32 INS_STATUS_INACTIVE=0 8 | uint32 INS_STATUS_ALIGNING=1 9 | uint32 INS_STATUS_HIGH_VARIANCE=2 10 | uint32 INS_STATUS_SOLUTION_GOOD=3 11 | uint32 INS_STATUS_SOLUTION_FREE=6 12 | uint32 INS_STATUS_ALIGNMENT_COMPLETE=7 13 | uint32 INS_STATUS_DETERMINING_ORIENTATION=8 14 | uint32 INS_STATUS_WAITING_INITIALPOS=9 15 | 16 | # Table 30 in the SPAN on OEM6 manual: 17 | # See: http://www.novatel.com/assets/Documents/Manuals/OM-20000144UM.pdf#page=124 18 | uint32 position_type 19 | uint32 POSITION_TYPE_NONE=0 20 | uint32 POSITION_TYPE_SBAS=52 21 | uint32 POSITION_TYPE_PSEUDORANGE_SINGLE_POINT=53 22 | uint32 POSITION_TYPE_PSEUDORANGE_DIFFERENTIAL=54 23 | uint32 POSITION_TYPE_RTK_FLOAT=55 24 | uint32 POSITION_TYPE_RTK_FIXED=56 25 | uint32 POSITION_TYPE_OMNISTAR=57 26 | uint32 POSITION_TYPE_OMNISTAR_HP=58 27 | uint32 POSITION_TYPE_OMNISTAR_XP=59 28 | uint32 POSITION_TYPE_PPP_CONVERGING=73 29 | uint32 POSITION_TYPE_PPP=74 30 | 31 | float64 latitude 32 | float64 longitude 33 | float64 altitude 34 | 35 | float32 undulation 36 | 37 | float64 north_velocity 38 | float64 east_velocity 39 | float64 up_velocity 40 | 41 | float64 roll 42 | float64 pitch 43 | float64 azimuth 44 | 45 | float32 latitude_std 46 | float32 longitude_std 47 | float32 altitude_std 48 | 49 | float32 north_velocity_std 50 | float32 east_velocity_std 51 | float32 up_velocity_std 52 | 53 | float32 roll_std 54 | float32 pitch_std 55 | float32 azimuth_std 56 | 57 | uint32 extended_status 58 | uint32 EXTENDED_STATUS_POSITION_UPDATE_APPLIED=1 59 | uint32 EXTENDED_STATUS_PHASE_UPDATE_APPLIED=2 60 | uint32 EXTENDED_STATUS_ZUPT_APPLIED=4 61 | uint32 EXTENDED_STATUS_WHEEL_SENSOR_APPLIED=8 62 | uint32 EXTENDED_STATUS_HEADING_UPDATE_APPLIED=16 63 | uint32 EXTENDED_STATUS_INS_SOLUTION_CONVERGED=64 64 | 65 | uint16 seconds_since_update 66 | -------------------------------------------------------------------------------- /novatel_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | novatel_msgs 4 | 1.1.0 5 | ROS messages which represent raw Novatel SPAN data. 6 | 7 | Mike Purvis 8 | Ian Colwell 9 | Josh Whitley 10 | 11 | NovAtel Support 12 | 13 | BSD 14 | 15 | catkin 16 | nav_msgs 17 | std_msgs 18 | message_generation 19 | geometry_msgs 20 | std_msgs 21 | message_runtime 22 | geometry_msgs 23 | nav_msgs 24 | 25 | -------------------------------------------------------------------------------- /novatel_span_driver/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package novatel_span_driver 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2018-04-26) 6 | ------------------ 7 | * Merge pull request `#27 `_ from michaelhsmart/ellipsoidal_from_orthometric 8 | Fix orthometric to ellipsoidal conversion 9 | Merged based on recommendations from @mikepurvis and @icolwell. 10 | * Deleted extra ')' (`#31 `_) 11 | * Fix ellipsoidal altitude to use correct conversion from orthometric + undulation. 12 | * Merge pull request `#10 `_ from prclibo/fix_rpy 13 | possibly fixed rpy interpretation 14 | * Merge pull request `#20 `_ from astuff/master 15 | Adding new maintainers. 16 | * Latch navsat/origin so that it is available to nodes that start after the NovAtel driver (`#12 `_) 17 | * Fix bad == when publishing IMU message (`#19 `_) 18 | * Merge pull request `#9 `_ from lemiant/patch-1 19 | Update diagnostics.py 20 | diagnostic_msgs has not been imported, must use DiagnosticStatus directly. 21 | * Contributors: AnkilP, Joshua Whitley, Michael Smart, Mike Purvis, P. J. Reed, lemiant, libo24 22 | 23 | 1.0.0 (2014-11-25) 24 | ------------------ 25 | * Add capture from INS PPP, default value for IMU rate, fix test, add docs to example launcher. 26 | * Throttle wheelvelocity commands to 1Hz, as recommended by Novatel. 27 | * Fix velx/y mixup, add diagnostic publisher. 28 | * BESTPOSB -> BESTPOS, add diagnostic constants and shim class. 29 | * Treat altitude as positive-up. 30 | * Update messages to include header, fix header to match documentation. 31 | * Add wheel velocity handler. 32 | * Contributors: Mike Purvis 33 | 34 | 0.2.0 (2014-10-29) 35 | ------------------ 36 | * Re-work the publisher class. 37 | * Add a pcap for a SPAN fix. 38 | * Tweaks to topic names. 39 | * Add working rostest, fixes from pepify. 40 | * Add roslaunch file check for example launcher. 41 | * Add roslint test. 42 | * Contributors: Mike Purvis 43 | 44 | 0.1.0 (2014-10-24) 45 | ------------------ 46 | * Consolidation and reorganization of novatelcentral driver prepared by NovAtel 47 | and published here: https://github.com/NovAtel-inc/rosnovatel 48 | * Contributors: Mike Purvis 49 | -------------------------------------------------------------------------------- /novatel_span_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(novatel_span_driver) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roslaunch roslint rostest) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package() 9 | 10 | 11 | install(PROGRAMS scripts/driver 12 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 13 | 14 | install(DIRECTORY launch 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 16 | 17 | roslint_python() 18 | 19 | if(CATKIN_ENABLE_TESTING) 20 | add_rostest(test/propak6-basic-fix.test) 21 | add_rostest(test/propak6-ins-ppp.test) 22 | 23 | roslaunch_add_file_check(launch/example.launch) 24 | endif() 25 | -------------------------------------------------------------------------------- /novatel_span_driver/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | log_request: 13 | inspvaxb: 0.02 14 | bestposb: 0.02 15 | corrimudatab: 0.02 16 | inscovb: 1 17 | # The example setup commands below apply to a Clearpath Robotics Husky running 18 | # SPAN with dual antennas. Run the SPAN setup wizard on your own platform to generate 19 | # an appropriate config. 20 | # command: 21 | # connectimu: COM3 IMU_KVH_COTS 22 | # setimuorientation: 5 23 | # vehiclebodyrotation: 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 24 | # applyvehiclebodyrotation: enable 25 | # setimutoantoffset: -0.519200 -0.131500 0.258600 0.001000 0.001000 0.001000 26 | # setimutoantoffset2: 0.580800 -0.131500 0.258600 0.001000 0.001000 0.001000 27 | # setinsoffset: 0.030800 -0.294200 -0.104100 28 | # alignmentmode: AUTOMATIC 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 43 | 44 | 45 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /novatel_span_driver/launch/example_for_usb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | log_request: 13 | inspvaxb: 0.02 14 | bestposb: 0.02 15 | corrimudatab: 0.02 16 | inscovb: 1 17 | # The example setup commands below apply to a Clearpath Robotics Husky running 18 | # SPAN with dual antennas. Run the SPAN setup wizard on your own platform to generate 19 | # an appropriate config. 20 | # command: 21 | # connectimu: COM3 IMU_KVH_COTS 22 | # setimuorientation: 5 23 | # vehiclebodyrotation: 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 24 | # applyvehiclebodyrotation: enable 25 | # setimutoantoffset: -0.519200 -0.131500 0.258600 0.001000 0.001000 0.001000 26 | # setimutoantoffset2: 0.580800 -0.131500 0.258600 0.001000 0.001000 0.001000 27 | # setinsoffset: 0.030800 -0.294200 -0.104100 28 | # alignmentmode: AUTOMATIC 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 43 | 44 | 45 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /novatel_span_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | novatel_span_driver 4 | 1.1.0 5 | Python driver for NovAtel SPAN devices. 6 | 7 | Mike Purvis 8 | Ian Colwell 9 | Josh Whitley 10 | 11 | NovAtel Support 12 | 13 | BSD 14 | 15 | catkin 16 | 17 | roslaunch 18 | roslint 19 | rostest 20 | 21 | diagnostic_msgs 22 | diagnostic_updater 23 | geodesy 24 | novatel_msgs 25 | python-serial 26 | rospy 27 | sensor_msgs 28 | std_msgs 29 | tf 30 | 31 | python-pcapy 32 | python-impacket 33 | 34 | -------------------------------------------------------------------------------- /novatel_span_driver/scripts/driver: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file driver 7 | # authors Mike Purvis 8 | # copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. 9 | # 10 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 11 | # the following conditions are met: 12 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 13 | # following disclaimer. 14 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 15 | # following disclaimer in the documentation and/or other materials provided with the distribution. 16 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 17 | # products derived from this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 20 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 22 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 23 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | import rospy 28 | 29 | from novatel_span_driver import bridge 30 | from novatel_span_driver import diagnostics 31 | from novatel_span_driver import publisher 32 | from novatel_span_driver import wheel_velocity 33 | 34 | 35 | def main(): 36 | rospy.init_node('novatel_span_driver') 37 | 38 | bridge.init() 39 | 40 | pub = publisher.NovatelPublisher() 41 | 42 | diag = diagnostics.NovatelDiagnostics() 43 | 44 | if rospy.get_param('~enable_wheel_velocity', False): 45 | vel = wheel_velocity.NovatelWheelVelocity(bridge.ports['data'].sock) 46 | 47 | rospy.spin() 48 | 49 | 50 | if __name__ == '__main__': 51 | try: 52 | main() 53 | except rospy.ROSInterruptException: 54 | pass 55 | -------------------------------------------------------------------------------- /novatel_span_driver/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | setup(**generate_distutils_setup( 5 | packages=['novatel_span_driver'], 6 | package_dir={'': 'src'} 7 | )) 8 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/novatel_span_driver/4dfd8830b73cf8b4d9ac8d6148b0de3f6990e9b9/novatel_span_driver/src/novatel_span_driver/__init__.py -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/bridge.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @bridge.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # ROS 30 | import rospy 31 | from novatel_msgs.msg import * 32 | from std_msgs.msg import String 33 | 34 | # Package modules 35 | from novatel_span_driver.data import DataPort 36 | from novatel_span_driver.monitor import Monitor 37 | 38 | # Standard 39 | import socket 40 | import serial 41 | import struct 42 | from cStringIO import StringIO 43 | import time 44 | 45 | from novatel_span_driver import translator 46 | 47 | DEFAULT_IP = '198.161.73.9' 48 | DEFAULT_PORT = 3001 49 | DEFAULT_DEV_NO = '/dev/ttyUSB0' 50 | DEFAULT_BAUDRATE = 115200 51 | 52 | SOCKET_TIMEOUT = 100.0 53 | socks = [] 54 | ports = {} 55 | monitor = Monitor(ports) 56 | 57 | 58 | def init(): 59 | # Pass this parameter to use pcap data rather than a socket to a device. 60 | # For testing the node itself--to exercise downstream algorithms, use a bag. 61 | pcap_file_name = rospy.get_param('~pcap_file', False) 62 | 63 | if not pcap_file_name: 64 | connect_type = rospy.get_param('~connect_type', False) 65 | if not connect_type: 66 | rospy.logfatal("Failed to fetch parameter: connect_type, please configure it as 'TCP' or 'SERIAL'") 67 | exit(1) 68 | 69 | sock = create_sock('data', connect_type) 70 | else: 71 | sock = create_test_sock(pcap_file_name) 72 | 73 | ports['data'] = DataPort(sock) 74 | 75 | configure_receiver(sock) 76 | 77 | for name, port in ports.items(): 78 | port.start() 79 | rospy.loginfo("Port %s thread started." % name) 80 | monitor.start() 81 | 82 | rospy.on_shutdown(shutdown) 83 | 84 | 85 | def create_sock(name, connect_type): 86 | try: 87 | if "TCP" == connect_type: 88 | ip = rospy.get_param('~ip', DEFAULT_IP) 89 | port = rospy.get_param('~port', DEFAULT_PORT) 90 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 91 | ip_port = (ip, port) 92 | sock.connect(ip_port) 93 | rospy.loginfo("Successfully connected to %%s port at %s:%d" % ip_port % name) 94 | elif "SERIAL" == connect_type: 95 | dev_no = rospy.get_param('~dev_no', DEFAULT_DEV_NO) 96 | baud = rospy.get_param('~baudrate', DEFAULT_BAUDRATE) 97 | sock = serial.Serial(port=dev_no, baudrate=baud, timeout=SOCKET_TIMEOUT, rtscts=True, dsrdtr=True) 98 | rospy.loginfo("Successfully connected to %%s port at %s:%d" % (dev_no, baud) % name) 99 | 100 | # make methods dynamically for make serial obj be compatible with socket obj 101 | from types import MethodType 102 | sock.recv = MethodType(serial.Serial.read, sock, serial.Serial) 103 | sock.send = MethodType(serial.Serial.write, sock, serial.Serial) 104 | sock.settimeout = MethodType(lambda *args, **kwargs: None, sock, serial.Serial) 105 | sock.shutdown = MethodType(lambda *args, **kwargs: None, sock, serial.Serial) 106 | else: 107 | rospy.logfatal("Connect type: %s isn't supported yet, please configure it as 'TCP' or 'SERIAL'" 108 | % connect_type) 109 | exit(1) 110 | except (socket.error, serial.SerialException) as e: 111 | rospy.logfatal("Couldn't connect to port at %s:%s" % (name, str(e))) 112 | exit(1) 113 | sock.settimeout(SOCKET_TIMEOUT) 114 | socks.append(sock) 115 | return sock 116 | 117 | 118 | def create_test_sock(pcap_filename): 119 | rospy.sleep(0.1) 120 | 121 | try: 122 | import pcapy 123 | except ImportError: 124 | import pure_pcapy as pcapy 125 | 126 | from StringIO import StringIO 127 | from impacket import ImpactDecoder 128 | 129 | body_list = [] 130 | if pcap_filename.endswith("gz"): 131 | # From: http://projects.honeynet.org/honeysnap/changeset/35/trunk/honeysnap/__init__.py 132 | import tempfile 133 | import gzip 134 | tmph, tmpf = tempfile.mkstemp() 135 | tmph = open(tmpf, 'wb') 136 | gfile = gzip.open(pcap_filename) 137 | tmph.write(gfile.read()) 138 | gfile.close() 139 | tmph.close() 140 | pcap_filename = tmpf 141 | 142 | cap = pcapy.open_offline(pcap_filename) 143 | decoder = ImpactDecoder.EthDecoder() 144 | 145 | while True: 146 | header, payload = cap.next() 147 | if not header: 148 | break 149 | try: 150 | tcp = decoder.decode(payload).child().child() 151 | body_list.append(tcp.child().get_packet()) 152 | except AttributeError: 153 | print(decoder.decode(payload)) 154 | raise 155 | 156 | data_io = StringIO(''.join(body_list)) 157 | 158 | class MockSocket(object): 159 | 160 | def recv(self, byte_count): 161 | rospy.sleep(0.002) 162 | data = data_io.read(byte_count) 163 | if data == "": 164 | rospy.signal_shutdown("Test completed.") 165 | return data 166 | 167 | def settimeout(self, timeout): 168 | pass 169 | 170 | return MockSocket() 171 | 172 | 173 | def configure_receiver(port): 174 | receiver_config = rospy.get_param('~configuration', None) 175 | 176 | if receiver_config is not None: 177 | imu_connect = receiver_config.get('imu_connect', None) 178 | if imu_connect is not None: 179 | rospy.loginfo("Sending IMU connection string to SPAN system.") 180 | port.send('connectimu ' + imu_connect['port'] + ' ' + imu_connect['type'] + '\r\n') 181 | 182 | logger = receiver_config.get('log_request', []) 183 | rospy.loginfo("Enabling %i log outputs from SPAN system." % len(logger)) 184 | for log in logger: 185 | port.send('log ' + log + ' ontime ' + str(logger[log]) + '\r\n') 186 | 187 | commands = receiver_config.get('command', []) 188 | rospy.loginfo("Sending %i user-specified initialization commands to SPAN system." % len(commands)) 189 | for cmd in commands: 190 | port.send(cmd + ' ' + str(commands[cmd]) + '\r\n') 191 | 192 | 193 | def shutdown(): 194 | monitor.finish.set() 195 | monitor.join() 196 | rospy.loginfo("Thread monitor finished.") 197 | for name, port in ports.items(): 198 | port.finish.set() 199 | port.join() 200 | rospy.loginfo("Port %s thread finished." % name) 201 | for sock in socks: 202 | sock.shutdown(socket.SHUT_RDWR) 203 | sock.close() 204 | rospy.loginfo("Sockets closed.") 205 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/data.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @data.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import rospy 30 | import novatel_msgs.msg 31 | 32 | from port import Port 33 | from novatel_span_driver.mapping import msgs 34 | from handlers import MessageHandler 35 | import translator 36 | 37 | from cStringIO import StringIO 38 | from threading import Lock 39 | 40 | 41 | class DataPort(Port): 42 | def run(self): 43 | # Set up handlers for translating different novatel messages as they arrive. 44 | handlers = {} 45 | pkt_counters = {} 46 | pkt_times = {} 47 | 48 | for msg_id in msgs.keys(): 49 | handlers[msg_id] = MessageHandler(*msgs[msg_id]) 50 | pkt_counters[msg_id] = 0 51 | pkt_times[msg_id] = 0 52 | 53 | bad_pkts = set() 54 | pkt_id = None 55 | 56 | while not self.finish.is_set(): 57 | try: 58 | header, pkt_str = self.recv() 59 | if header is not None: 60 | handlers[header.id].handle(StringIO(pkt_str), header) 61 | 62 | except ValueError as e: 63 | # Some problem in the recv() routine. 64 | rospy.logwarn(str(e)) 65 | continue 66 | 67 | except KeyError as e: 68 | if header.id not in handlers and header.id not in pkt_counters: 69 | rospy.logwarn("No handler for message id %d" % header.id) 70 | 71 | except translator.TranslatorError: 72 | if header.id not in bad_pkts: 73 | rospy.logwarn("Error parsing %s.%d" % header.id) 74 | bad_pkts.add(pkt) 75 | 76 | if header.id not in pkt_counters: 77 | pkt_counters[header.id] = 0 78 | else: 79 | pkt_counters[header.id] += 1 80 | pkt_times[header.id] = header.gps_week_seconds # only track times of msgs that are part of novatel msgs 81 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/diagnostics.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @publisher.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import rospy 30 | import diagnostic_updater 31 | 32 | from diagnostic_msgs.msg import DiagnosticStatus 33 | from novatel_msgs.msg import BESTPOS, INSPVAX 34 | 35 | 36 | class NovatelDiagnostics(object): 37 | def __init__(self): 38 | self.last_bestpos = None 39 | self.last_inspvax = None 40 | rospy.Subscriber("novatel_data/bestpos", BESTPOS, self.bestpos_callback) 41 | rospy.Subscriber("novatel_data/inspvax", INSPVAX, self.inspvax_callback) 42 | 43 | self.updater = diagnostic_updater.Updater() 44 | self.updater.setHardwareID("none") 45 | self.updater.add("Novatel SPAN", self.produce_diagnostics) 46 | 47 | def bestpos_callback(self, msg): 48 | self.last_bestpos = msg 49 | self.updater.setHardwareID("firmware-%d" % msg.header.software_version) 50 | self.updater.update() 51 | 52 | def inspvax_callback(self, msg): 53 | self.last_inspvax = msg 54 | self.updater.update() 55 | 56 | @staticmethod 57 | def get_status_string(msg, field): 58 | value = getattr(msg, field) 59 | matching_status = [x[len(field) + 1:] for x in dir(msg) if x.startswith(field.upper()) and 60 | value == getattr(msg, x)] 61 | if len(matching_status) != 1: 62 | return "No matching constant" 63 | return matching_status[0] 64 | 65 | @staticmethod 66 | def get_status_bitfield(msg, field): 67 | value = getattr(msg, field) 68 | matching_statuses = [x[len(field) + 1:] for x in dir(msg) if x.startswith(field.upper()) and 69 | value & getattr(msg, x)] 70 | return ', '.join(matching_statuses) 71 | 72 | def produce_diagnostics(self, stat): 73 | if self.last_bestpos: 74 | stat.add("GNSS Solution Status", 75 | self.get_status_string(self.last_bestpos, "solution_status")) 76 | stat.add("GNSS Position Type", 77 | self.get_status_string(self.last_bestpos, "position_type")) 78 | self.last_bestpos = None 79 | 80 | if self.last_inspvax: 81 | if self.last_inspvax.ins_status != INSPVAX.INS_STATUS_SOLUTION_GOOD: 82 | stat.summary(DiagnosticStatus.WARN, "INS Solution not GOOD.") 83 | elif self.last_inspvax.position_type != INSPVAX.POSITION_TYPE_PPP: 84 | stat.summary(DiagnosticStatus.WARN, "INS Position type not PPP.") 85 | else: 86 | stat.summary(DiagnosticStatus.OK, "INS Solution GOOD, PPP fix present.") 87 | 88 | stat.add("INS Solution Status", 89 | self.get_status_string(self.last_inspvax, "ins_status")) 90 | stat.add("INS Position Type", 91 | self.get_status_string(self.last_inspvax, "position_type")) 92 | stat.add("INS Extended Status", 93 | self.get_status_bitfield(self.last_inspvax, "extended_status")) 94 | stat.add("Seconds since last ZUPT or position update.", 95 | self.last_inspvax.seconds_since_update) 96 | stat.add("Receiver Status", 97 | self.get_status_bitfield(self.last_inspvax.header, "receiver_status")) 98 | self.last_inspvax = None 99 | else: 100 | stat.summary(DiagnosticStatus.ERROR, 101 | "No INSPVAX logs received from Novatel system.") 102 | 103 | return stat 104 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/handlers.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @handlers.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import translator 30 | import rospy 31 | 32 | from novatel_msgs.msg import Ack 33 | 34 | 35 | class Handler: 36 | def handle(self, buff, header): 37 | raise NotImplementedError 38 | 39 | 40 | class MessageHandler(Handler): 41 | def __init__(self, name, data_class): 42 | self.publisher = rospy.Publisher("novatel_data/" + name, data_class, queue_size=1) 43 | self.message = self.publisher.data_class() 44 | 45 | def handle(self, buff, header): 46 | self.message.translator().deserialize(buff) 47 | self.message.header = header 48 | self.publisher.publish(self.message) 49 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/mapping.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @mapping.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | from novatel_msgs.msg import * 30 | 31 | msgs = { 32 | 0: ("ack", Ack), 33 | 42: ("bestpos", BESTPOS), 34 | 264: ("inscov", INSCOV), 35 | 812: ("corrimudata", CORRIMUDATA), 36 | 1465: ("inspvax", INSPVAX) 37 | } 38 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/monitor.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @monitor.py 7 | # authors Mike Purvis 8 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 9 | # 10 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 11 | # the following conditions are met: 12 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 13 | # following disclaimer. 14 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 15 | # following disclaimer in the documentation and/or other materials provided with the distribution. 16 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 17 | # products derived from this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 20 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 22 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 23 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | import rospy 28 | import threading 29 | 30 | 31 | class Monitor(threading.Thread): 32 | def __init__(self, ports): 33 | super(Monitor, self).__init__() 34 | self.ports = ports 35 | self.finish = threading.Event() 36 | 37 | def run(self): 38 | while not self.finish: 39 | rospy.sleep(1.0) 40 | for name, port in ports.items(): 41 | if not port.is_alive(): 42 | rospy.logerr("Port %s thread died. Signalling node shutdown." % name) 43 | rospy.signal_shutdown("Node lost thread.") 44 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/port.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @port.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import rospy 30 | import novatel_msgs.msg as msg 31 | 32 | # Node source 33 | from translator import Translator 34 | 35 | # Python 36 | import threading 37 | import socket 38 | import serial 39 | import struct 40 | from cStringIO import StringIO 41 | 42 | 43 | class Port(threading.Thread): 44 | 45 | """ Common base class for DataPort and ControlPort. Provides functionality to 46 | recv/send novatel-formatted packets from the socket. Could in future 47 | support LoggingPort and DisplayPort.""" 48 | checksum_struct = struct.Struct(" 0 and not bytes_before_sync.startswith("\r\n 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import rospy 30 | import tf 31 | import geodesy.utm 32 | 33 | from novatel_msgs.msg import BESTPOS, CORRIMUDATA, INSCOV, INSPVAX 34 | from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus 35 | from nav_msgs.msg import Odometry 36 | from geometry_msgs.msg import Quaternion, Point, Pose, Twist 37 | 38 | from math import radians, pow 39 | 40 | # FIXED COVARIANCES 41 | # TODO: Work these out... 42 | IMU_ORIENT_COVAR = [1e-3, 0, 0, 43 | 0, 1e-3, 0, 44 | 0, 0, 1e-3] 45 | 46 | IMU_VEL_COVAR = [1e-3, 0, 0, 47 | 0, 1e-3, 0, 48 | 0, 0, 1e-3] 49 | 50 | IMU_ACCEL_COVAR = [1e-3, 0, 0, 51 | 0, 1e-3, 0, 52 | 0, 0, 1e-3] 53 | 54 | NAVSAT_COVAR = [1, 0, 0, 55 | 0, 1, 0, 56 | 0, 0, 1] 57 | 58 | POSE_COVAR = [1, 0, 0, 0, 0, 0, 59 | 0, 1, 0, 0, 0, 0, 60 | 0, 0, 1, 0, 0, 0, 61 | 0, 0, 0, 0.1, 0, 0, 62 | 0, 0, 0, 0, 0.1, 0, 63 | 0, 0, 0, 0, 0, 0.1] 64 | 65 | TWIST_COVAR = [1, 0, 0, 0, 0, 0, 66 | 0, 1, 0, 0, 0, 0, 67 | 0, 0, 1, 0, 0, 0, 68 | 0, 0, 0, 0.1, 0, 0, 69 | 0, 0, 0, 0, 0.1, 0, 70 | 0, 0, 0, 0, 0, 0.1] 71 | 72 | 73 | class NovatelPublisher(object): 74 | """ Subscribes to the directly-translated messages from the SPAN system 75 | and repackages the resultant data as standard ROS messages. """ 76 | 77 | def __init__(self): 78 | # Parameters 79 | self.publish_tf = rospy.get_param('~publish_tf', False) 80 | self.odom_frame = rospy.get_param('~odom_frame', 'odom_combined') 81 | self.base_frame = rospy.get_param('~base_frame', 'base_link') 82 | 83 | # When True, UTM odom x, y pose will be published with respect to the 84 | # first coordinate received. 85 | self.zero_start = rospy.get_param('~zero_start', False) 86 | 87 | self.imu_rate = rospy.get_param('~rate', 100) 88 | 89 | # Topic publishers 90 | self.pub_imu = rospy.Publisher('imu/data', Imu, queue_size=1) 91 | self.pub_odom = rospy.Publisher('navsat/odom', Odometry, queue_size=1) 92 | self.pub_origin = rospy.Publisher('navsat/origin', Pose, queue_size=1, latch=True) 93 | self.pub_navsatfix = rospy.Publisher('navsat/fix', NavSatFix, queue_size=1) 94 | 95 | if self.publish_tf: 96 | self.tf_broadcast = tf.TransformBroadcaster() 97 | 98 | self.init = False # If we've been initialized 99 | self.origin = Point() # Where we've started 100 | self.orientation = [0] * 4 # Empty quaternion until we hear otherwise 101 | self.orientation_covariance = IMU_ORIENT_COVAR 102 | 103 | # Subscribed topics 104 | rospy.Subscriber('novatel_data/bestpos', BESTPOS, self.bestpos_handler) 105 | rospy.Subscriber('novatel_data/corrimudata', CORRIMUDATA, self.corrimudata_handler) 106 | rospy.Subscriber('novatel_data/inscov', INSCOV, self.inscov_handler) 107 | rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.inspvax_handler) 108 | 109 | def bestpos_handler(self, bestpos): 110 | navsat = NavSatFix() 111 | 112 | # TODO: The timestamp here should come from SPAN, not the ROS system time. 113 | navsat.header.stamp = rospy.Time.now() 114 | navsat.header.frame_id = self.odom_frame 115 | 116 | # Assume GPS - this isn't exposed 117 | navsat.status.service = NavSatStatus.SERVICE_GPS 118 | 119 | position_type_to_status = { 120 | BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX, 121 | BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX, 122 | BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX, 123 | BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX, 124 | BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX, 125 | BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX, 126 | BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX, 127 | BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX, 128 | BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, 129 | BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX, 130 | BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX, 131 | BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX, 132 | BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX, 133 | BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX, 134 | BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX, 135 | BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX, 136 | BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX, 137 | BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX, 138 | BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX, 139 | BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX, 140 | BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX, 141 | BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX, 142 | BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX, 143 | BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX, 144 | BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX, 145 | BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX, 146 | BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX, 147 | BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX, 148 | BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX, 149 | BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, 150 | BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX, 151 | BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX, 152 | BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX, 153 | } 154 | navsat.status.status = position_type_to_status.get(bestpos.position_type, 155 | NavSatStatus.STATUS_NO_FIX) 156 | 157 | # Position in degrees. 158 | navsat.latitude = bestpos.latitude 159 | navsat.longitude = bestpos.longitude 160 | 161 | # Altitude in metres. 162 | navsat.altitude = bestpos.altitude + bestpos.undulation 163 | navsat.position_covariance[0] = pow(2, bestpos.latitude_std) 164 | navsat.position_covariance[4] = pow(2, bestpos.longitude_std) 165 | navsat.position_covariance[8] = pow(2, bestpos.altitude_std) 166 | navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN 167 | 168 | # Ship ito 169 | self.pub_navsatfix.publish(navsat) 170 | 171 | def inspvax_handler(self, inspvax): 172 | # Convert the latlong to x,y coordinates and publish an Odometry 173 | try: 174 | utm_pos = geodesy.utm.fromLatLong(inspvax.latitude, inspvax.longitude) 175 | except ValueError: 176 | # Probably coordinates out of range for UTM conversion. 177 | return 178 | 179 | if not self.init and self.zero_start: 180 | self.origin.x = utm_pos.easting 181 | self.origin.y = utm_pos.northing 182 | self.origin.z = inspvax.altitude 183 | self.pub_origin.publish(position=self.origin) 184 | 185 | odom = Odometry() 186 | odom.header.stamp = rospy.Time.now() 187 | odom.header.frame_id = self.odom_frame 188 | odom.child_frame_id = self.base_frame 189 | odom.pose.pose.position.x = utm_pos.easting - self.origin.x 190 | odom.pose.pose.position.y = utm_pos.northing - self.origin.y 191 | odom.pose.pose.position.z = inspvax.altitude - self.origin.z 192 | 193 | # Orientation 194 | # Save this on an instance variable, so that it can be published 195 | # with the IMU message as well. 196 | self.orientation = tf.transformations.quaternion_from_euler( 197 | radians(inspvax.roll), 198 | radians(inspvax.pitch), 199 | -radians(inspvax.azimuth), 'syxz') 200 | odom.pose.pose.orientation = Quaternion(*self.orientation) 201 | odom.pose.covariance[21] = self.orientation_covariance[0] = pow(inspvax.pitch_std, 2) 202 | odom.pose.covariance[28] = self.orientation_covariance[4] = pow(inspvax.roll_std, 2) 203 | odom.pose.covariance[35] = self.orientation_covariance[8] = pow(inspvax.azimuth_std, 2) 204 | 205 | # Twist is relative to vehicle frame 206 | odom.twist.twist.linear.x = inspvax.east_velocity 207 | odom.twist.twist.linear.y = inspvax.north_velocity 208 | odom.twist.twist.linear.z = inspvax.up_velocity 209 | TWIST_COVAR[0] = pow(2, inspvax.east_velocity_std) 210 | TWIST_COVAR[7] = pow(2, inspvax.north_velocity_std) 211 | TWIST_COVAR[14] = pow(2, inspvax.up_velocity_std) 212 | odom.twist.covariance = TWIST_COVAR 213 | 214 | self.pub_odom.publish(odom) 215 | 216 | # Odometry transform (if required) 217 | if self.publish_tf: 218 | self.tf_broadcast.sendTransform( 219 | (odom.pose.pose.position.x, odom.pose.pose.position.y, 220 | odom.pose.pose.position.z), 221 | self.orientation, 222 | odom.header.stamp, odom.child_frame_id, odom.header.frame_id) 223 | 224 | # Mark that we've received our first fix, and set origin if necessary. 225 | self.init = True 226 | 227 | def corrimudata_handler(self, corrimudata): 228 | # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body 229 | imu = Imu() 230 | imu.header.stamp = rospy.Time.now() 231 | imu.header.frame_id = self.base_frame 232 | 233 | # Populate orientation field with one from inspvax message. 234 | imu.orientation = Quaternion(*self.orientation) 235 | imu.orientation_covariance = self.orientation_covariance 236 | 237 | # Angular rates (rad/s) 238 | # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz 239 | imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate 240 | imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate 241 | imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate 242 | imu.angular_velocity_covariance = IMU_VEL_COVAR 243 | 244 | # Linear acceleration (m/s^2) 245 | imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate 246 | imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate 247 | imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate 248 | imu.linear_acceleration_covariance = IMU_ACCEL_COVAR 249 | 250 | self.pub_imu.publish(imu) 251 | 252 | def inscov_handler(self, inscov): 253 | # TODO: Supply this data in the IMU and Odometry messages. 254 | pass 255 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/translator.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @translator.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import roslib 30 | import roslib.message 31 | import roslib.msgs 32 | 33 | import genpy 34 | import rospy 35 | import struct 36 | from itertools import izip 37 | from cStringIO import StringIO 38 | 39 | 40 | class EndOfBuffer(BaseException): 41 | pass 42 | 43 | 44 | class TranslatorError(ValueError): 45 | pass 46 | 47 | 48 | class Handler(object): 49 | def field(self, msg): 50 | return getattr(msg, self.name) 51 | 52 | def preserialize(self, msg): 53 | pass 54 | 55 | 56 | class SubMessageHandler(Handler): 57 | def __init__(self, field): 58 | self.name = field.name 59 | self.msg_cls = roslib.message.get_message_class(field.type) 60 | 61 | def deserialize(self, buff, msg): 62 | self.field(msg).translator().deserialize(buff) 63 | 64 | def serialize(self, buff, msg): 65 | self.field(msg).translator().serialize(buff) 66 | 67 | 68 | class FixedFieldsHandler(Handler): 69 | def __init__(self, fields): 70 | struct_strs = ['<'] 71 | 72 | # serial vs ethernet 73 | def pattern(field): 74 | try: 75 | return genpy.base.SIMPLE_TYPES_DICT[field.type] 76 | except KeyError: 77 | if field.base_type in ['uint8', 'char'] and field.array_len is not None: 78 | return "%is" % field.array_len 79 | else: 80 | raise 81 | 82 | struct_strs.extend([pattern(f) for f in fields]) 83 | self.struct = struct.Struct(''.join(struct_strs)) 84 | self.names = [f.name for f in fields] 85 | self.size = self.struct.size 86 | 87 | def serialize(self, buff, msg): 88 | buff.write(self.struct.pack(*[getattr(msg, name) for name in self.names])) 89 | 90 | def deserialize(self, buff, msg): 91 | st = buff.read(self.struct.size) 92 | if st == '': 93 | return 94 | values = self.struct.unpack(st) 95 | for name, value in izip(self.names, values): 96 | setattr(msg, name, value) 97 | 98 | 99 | class SubMessageArrayHandler(Handler): 100 | struct_uint16 = struct.Struct(' 0: 170 | self.handlers.append(FixedFieldsHandler(fixed_fields)) 171 | fixed_fields = [] 172 | 173 | # Handle this other type. 174 | if field.type == 'string' or (field.base_type == 'uint8' and field.is_array): 175 | self.handlers.append(VariableStringHandler(field)) 176 | elif field.is_array: 177 | self.handlers.append(SubMessageArrayHandler(field)) 178 | else: 179 | self.handlers.append(SubMessageHandler(field)) 180 | 181 | if len(fixed_fields) > 0: 182 | self.handlers.append(FixedFieldsHandler(fixed_fields)) 183 | 184 | if len(self.handlers) == 1 and hasattr(self.handlers[0], 'size'): 185 | self.size = self.handlers[0].size 186 | 187 | 188 | class TranslatorProxy: 189 | def __init__(self, translator, msg): 190 | self.translator = translator 191 | self.size = translator.size 192 | self.msg = msg 193 | 194 | def deserialize(self, buff): 195 | try: 196 | for handler in self.translator.handlers: 197 | handler.deserialize(buff, self.msg) 198 | except struct.error as e: 199 | raise TranslatorError(e) 200 | 201 | def serialize(self, buff): 202 | try: 203 | for handler in self.translator.handlers: 204 | handler.serialize(buff, self.msg) 205 | except struct.error as e: 206 | raise TranslatorError(e) 207 | 208 | def preserialize(self): 209 | for handler in self.translator.handlers: 210 | handler.preserialize(self.msg) 211 | 212 | 213 | def translator(self): 214 | if not hasattr(self.__class__, "_translator"): 215 | self.__class__._translator = Translator(self.__class__) 216 | return TranslatorProxy(self.__class__._translator, self) 217 | 218 | roslib.message.Message.translator = translator 219 | -------------------------------------------------------------------------------- /novatel_span_driver/src/novatel_span_driver/wheel_velocity.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD) 5 | # 6 | # file @wheel_velocity.py 7 | # authors Mike Purvis 8 | # NovAtel 9 | # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved. 10 | # Copyright (c) 2014, NovAtel Inc., All rights reserved. 11 | # 12 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 13 | # the following conditions are met: 14 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 15 | # following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 17 | # following disclaimer in the documentation and/or other materials provided with the distribution. 18 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 19 | # products derived from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 22 | # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 24 | # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 25 | # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import rospy 30 | from nav_msgs.msg import Odometry 31 | from math import pi 32 | 33 | 34 | class NovatelWheelVelocity(object): 35 | """ Subscribes to a platform's odom topic and sends SPAN 36 | wheelvelocity messages about the platform's linear movement. """ 37 | 38 | def __init__(self, port): 39 | self.port = port 40 | 41 | # The Odometry message doesn't expose this information to us, so we 42 | # make up a fake wheel which is rotated and ticked based on how far 43 | # the odom topic tells us we traveled. 44 | self.fake_wheel_diameter = rospy.get_param("~fake_wheel/diameter", 0.33) 45 | self.fake_wheel_ticks = rospy.get_param("~fake_wheel/ticks", 1000) 46 | 47 | # SPAN wants to know how much delay is associated with our velocity report. 48 | # This is specified in milliseconds. 49 | self.latency = rospy.get_param("~wheel_velocity_latency", 100) 50 | max_frequency = rospy.get_param("~wheel_velocity_max_frequency", 1.0) 51 | self.minimum_period = rospy.Duration(1.0 / max_frequency) 52 | 53 | # Send setup command. 54 | self.circumference = self.fake_wheel_diameter * pi 55 | cmd = 'setwheelparameters %d %f %f' % ( 56 | self.fake_wheel_ticks, 57 | self.circumference, 58 | self.circumference / self.fake_wheel_ticks) 59 | 60 | rospy.logdebug("Sending: %s" % cmd) 61 | self.port.send(cmd) 62 | 63 | self.cumulative_ticks = 0 64 | self.last_received_stamp = None 65 | self.last_sent = None 66 | rospy.Subscriber('odom', Odometry, self.odom_handler) 67 | 68 | def odom_handler(self, odom): 69 | if self.last_received_stamp: 70 | # Robot's linear velocity in m/s. 71 | velocity = abs(odom.twist.twist.linear.x) 72 | velocity_ticks = velocity * self.fake_wheel_ticks / self.circumference 73 | 74 | period = (odom.header.stamp - self.last_received_stamp).to_sec() 75 | self.cumulative_ticks += velocity_ticks * period 76 | 77 | cmd = 'wheelvelocity %d %d %d 0 %f 0 0 %d \r\n' % ( 78 | self.latency, 79 | self.fake_wheel_ticks, 80 | int(velocity_ticks), 81 | velocity_ticks, 82 | self.cumulative_ticks) 83 | 84 | if not self.last_sent or (odom.header.stamp - self.last_sent) > self.minimum_period: 85 | rospy.logdebug("Sending: %s" % repr(cmd)) 86 | self.port.send(cmd) 87 | self.last_sent = odom.header.stamp 88 | 89 | self.last_received_stamp = odom.header.stamp 90 | -------------------------------------------------------------------------------- /novatel_span_driver/test/propak6-basic-fix.pcap.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/novatel_span_driver/4dfd8830b73cf8b4d9ac8d6148b0de3f6990e9b9/novatel_span_driver/test/propak6-basic-fix.pcap.gz -------------------------------------------------------------------------------- /novatel_span_driver/test/propak6-basic-fix.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | topic: /navsat/fix 9 | hz: 10.0 10 | hzerror: 5.0 11 | test_duration: 5.0 12 | wait_time: 5.0 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /novatel_span_driver/test/propak6-ins-ppp.pcap.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/novatel_span_driver/4dfd8830b73cf8b4d9ac8d6148b0de3f6990e9b9/novatel_span_driver/test/propak6-ins-ppp.pcap.gz -------------------------------------------------------------------------------- /novatel_span_driver/test/propak6-ins-ppp.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | topic: /navsat/odom 14 | hz: 12.5 15 | hzerror: 5.0 16 | test_duration: 5.0 17 | wait_time: 5.0 18 | 19 | 20 | 21 | --------------------------------------------------------------------------------