diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..24687ac --- /dev/null +++ b/.gitattributes @@ -0,0 +1 @@ +novatel_oem7/bin/liboem7decoder.a filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3986aec --- /dev/null +++ b/.gitignore @@ -0,0 +1,15 @@ +.bash_history +.bloom_logs +.catkin_workspace +.ros +.viminfo +build +devel +install +doc +src/CMakeLists.txt +*.pyc +*.deb +*.ddeb +debian +obj-* diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..09ed338 --- /dev/null +++ b/LICENSE @@ -0,0 +1,22 @@ +MIT + +Copyright (c) 2020 NovAtel Inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + diff --git a/README.md b/README.md new file mode 100644 index 0000000..e769544 --- /dev/null +++ b/README.md @@ -0,0 +1,638 @@ +# NovAtel OEM7 Driver +[**ROS**](www.ros.org) Driver for [**NovAtel**](www.novatel.com) OEM7 GNSS Receivers. + +
+
+

BETA Driver

+This driver is considered pre-release at this time. This is provided as-is. Please refer to the bundled license. +
+
+ + +## Getting Started + +### Prerequisites +* ROS Kinetic or Melodic, including gps-common and tf ROS packages. +* Obtain OEM7 receiver. + + +### Installation +#### Option A: Install binary package +``` +sudo apt-get install ros-${ROS_DISTRO}-novatel-oem7-driver +``` + +#### Option B: Build from source (docker) +These instructions assume that you are using Ubuntu 18.04. + +1. Install Docker, add the user you intend on using to the 'docker' group. +1. From the base directory of the repository, run `./docker/run-melodic-build.sh` (this may take >10mins) +1. From within your docker container (where the prompt from above should land), run `./build.sh -f` + +#### Option C: Build from source (local environment) +Here are approximate instructions for building this driver with your local ROS development environment. Please note this is for reference. The Docker approach is recommended. + +1. Install ROS with developer support to your environment +1. Install ROS dependencies gps-common, tf + Ex: `sudo apt install ros-melodic-gps-common ros-melodic-tf` +1. Run `source /opt/ros/melodic/setup.bash` +1. Set `ROS_DISTRO` environment variable (Ex: `ROS_DISTRO=melodic`) +1. Run `bash envsetup.sh` +1. Run build: `./build.sh -f` + + +## Basic Operation with OEM7 receiver + +### Operation using TCP/IP + +Use default TCP/IP port '3001'. Refer to the OEM7 receiver [manual](https://docs.novatel.com/OEM7/Content/Home.htm) for more details on ICOM ports. + +```shell +roslaunch novatel_oem7_driver oem7_net.launch oem7_ip_addr:=10.128.224.56 +``` + + +### Operation using UDP/IP + +By default (factory reset), OEM7 receivers do not support UDP connections. An ICOM port must be configured for UDP and **the configuration saved.** + +Using a terminal program, connect to OEM7 COM port as described in OEM7 manual and enter : +``` +ICOMCONFIG ICOM2 UDP ":3002" +SAVECONFIG +``` +Then +```shell +roslaunch novatel_oem7_driver oem7_net.launch oem7_if:=Oem7ReceiverUdp `oem7_ip_addr:=10.128.224.56 oem7_port:=3002 +``` +Refer to "ICOMCONFIG" command in OEM7 receiver manual. + + +### Operation Using Serial Port (TTY) + + +```shell +roslaunch novatel_oem7_driver oem7_tty.launch oem7_tty_name:=/dev/ttyUSB0 +``` + +To (optionally) set a specific baud rate, e.g. 115200, set **oem7_tty_baud**:=115200 +Ensure that the baud rate of receiver port matches; the driver does ***not*** automatically configure receiver baud rate. +Setting the baud rate is required for physical RS232 ports. + +In order to use USB ports, install OEM7 USB [drivers](https://docs.novatel.com/OEM7/Content/Operation/USB_Communications.htm). +Setting the baud rate is not required for USB serial ports. + + + +### ROS Parameter namespace layout + +The driver uses standard ROS relative parameter addressing, with some parameters private to the nodelets. + +The default namespace is outlined below. The namespace structure can be changed in the .launch and .xml files. + + +**/novatel/oem7** +Overall driver namespace; global driver paramerers. + +**/novatel/oem7/receivers** +OEM7 Receiver instances + +**/novatel/oem7/receivers/main** +OEM7 Receiver instance: interface (TCP/IP, TTY) parameters, initialization commands, published topics, etc. + + + + + + +## Customization + +### Configuring your own initialization commands + +The **/novatel/oem7/receivers/main/receiver_init_commands** parameter holds the list of commands to be executed on driver startup. +There are no semantics checking of any kind; it is the user's responsibility to ensure correctness. +By default, this list is populated from ***$(find novatel_oem7_driver)/config/std_init_commands.yaml*** + + +This list can be overriden using standard ROS parameter mechanisms. + + + +Notes: + +1. In order to be processed by the driver, logs must be in OEM7 "binary' format, e.g. "BESTPOS**B**" + + + +### Configuring topic parameters: + +Topic name and reference frame are configured by adding the following parameter under /novatel/oem7/receivers/main + +``` +$message_name{topic: $topic_name, frame_id:$id, queue_size: "$size"} +``` + +INSCONFIG +``` +INSCONFIG: {topic: /gps/insconfig, frame_id: gps, queue_size: "10"} +``` +The default configuration is populated from "$(find novatel_oem7_driver)/config/std_msg_topics.yaml" + + + +### Other Configuration Files + +The .launch files includes other configuration files: + + +$(find novatel_oem7_driver)/config/**std_oem7_raw_msgs.yaml** +The list of OEM7 messages to logged as blobs for postprocessing. Refer to novatel_oem7_msg/Oem7RawMsg.msg + + + +$(find novatel_oem7_driver)/config/**std_driver_config.xml** +$(find novatel_oem7_driver)/config/**std_msg_handlers.yaml** +Various driver plugins to be loaded; there is no need to modify these unless you are developing your own plugins or optimizing the driver. + + + +$(find novatel_oem7_driver)/config/**oem7_msgs.yaml** +A map of OEM7 message names / IDs. +Messages not in this list will be ignored by the driver. Typically, this does not need modification, unless you are optimizing the driver. + + + +### ROS Message Generation + +ROS messages are derived ("synthesized") from multiple OEM7 messages. +Consult documentation in $(find novatel_oem7_driver)/config/std_init_commands.yaml + + + +### Runtime Receiver Reconfiguration + +The driver implements ROS Service "**/novatel_oem7_driver/receivers/main/Oem7Cmd**" which sends OEM7 Abbreviated ASCII commands from Linux terminal using "rosservice" tool: +``` +rosservice call /novatel/oem7/receivers/main/Oem7Cmd "$oem7_abbreviated_ascii_command" +``` +OEM7 response is printed out verbatim. + +Example: +``` +rosservice call /novatel/oem7/receivers/main/Oem7Cmd "LOG BESTPOSB ONTIME 0.1" +rsp: "OK" +``` + + + +All standard OEM7 abbreviated ascii commands are supported. The commands are executed "as-is"; the syntax is not modified. + + + + +### Error Detection and Recovery + +There is no internal recovery mechanism; whenever the driver goes into an error state, message publishing stops. The user is responsible for detection of this condition. +Recovery is achieved by reloading the driver (Message and Config nodelets). + + + + + +### Built-in Self-Test (BIST) + +Standard driver installation supports BIST, which can be used to evaluate if the driver is operating normally. +BIST is lauched by using rostest instead of roslaunch, by specifying "oem7_bist:=true" parameter with your standard .launch file. + +For example, to run BIST with an oem7_net.launch file: + +``` +**rostest** --text novatel_oem7_driver oem7_net.launch oem7_ip_addr:=10.128.224.56 **oem7_bist:=true** + +BIST outputs topic statistics, similar to the "rostopic hz" command: + + +... topic: '/gps/gps', exp interval= 0.02. samples= 2945: + +mean, max, stdev + +Bag Recording interval: 0.0200001721311, 0.127890110016, 0.00589703421274 + +Message publish interval: 0.0200002559501, 0.119542121887, 0.00569331898041 + +Bag Rec - Message Pub Delta: 0.000302689346676, 0.0227742195129, 0.000916649615432 + + + +topic: '/novatel/oem7/inspva', exp interval= 0.02. samples= 2918: + +mean, max, stdev + +Bag Recording interval: 0.0199991217369, 0.0854535102844, 0.0049107521279 + +Message publish interval: 0.0199991563922, 0.0838820934296, 0.00478976290435 + +Bag Rec - Message Pub Delta: 0.000299763973647, 0.0198292732239, 0.000763679984502 + + + +topic: '/novatel/oem7/time', exp interval= 1.0. samples= 57: + +mean, max, stdev + +Bag Recording interval: 0.999989598989, 1.0087621212, 0.0045717348049 + +Message publish interval: 0.999993549926, 1.00874257088, 0.00458939860791 + +Bag Rec - Message Pub Delta: 0.000231772138361, 0.000780344009399, 0.000111371446655 + + + +topic: '/gps/fix', exp interval= 0.02. samples= 2886: + +mean, max, stdev + +Bag Recording interval: 0.0200002543021, 0.078773021698, 0.00453926745601 + +Message publish interval: 0.0200003352901, 0.0766448974609, 0.00433987262471 + +Bag Rec - Message Pub Delta: 0.000307747992227, 0.0275449752808, 0.00098472728123 + + + +topic: '/novatel/oem7/bestpos', exp interval= 0.1. samples= 573: + +mean, max, stdev + +Bag Recording interval: 0.100003437979, 0.144937515259, 0.0112068678576 + +Message publish interval: 0.100003776017, 0.144729137421, 0.0111951734664 + +Bag Rec - Message Pub Delta: 0.000227286137419, 0.00631713867188, 0.000422261912593 + + + +topic: '/gps/imu', exp interval= 0.01. samples= 5922: + +mean, max, stdev + +Bag Recording interval: 0.00999986636157, 0.127390623093, 0.00506395854586 + +Message publish interval: 0.0099998703077, 0.118784427643, 0.00485288752033 + +Bag Rec - Message Pub Delta: 0.000257168738917, 0.0433497428894, 0.00116386292041 + + + +topic: '/novatel/oem7/inspvax', exp interval= 1.0. samples= 59: + +mean, max, stdev + +Bag Recording interval: 0.999823825113, 1.02793908119, 0.00850038277189 + +Message publish interval: 0.999849241355, 1.02734613419, 0.00816920128965 + +Bag Rec - Message Pub Delta: 0.000262191740133, 0.00299835205078, 0.000491497024532 + + + +topic: '/novatel/oem7/bestvel', exp interval= 0.1. samples= 587: + +mean, max, stdev + +Bag Recording interval: 0.0998391059478, 0.159289360046, 0.0127196007736 + +Message publish interval: 0.0998426366585, 0.151807785034, 0.012558915178 + +Bag Rec - Message Pub Delta: 0.000218554290434, 0.00764131546021, 0.000507693107715 + + + +topic: '/novatel/oem7/corrimu', exp interval= 0.01. samples= 5746: + +mean, max, stdev + +Bag Recording interval: 0.00999978424882, 0.0556886196136, 0.00387268651204 + +Message publish interval: 0.00999978275482, 0.040646314621, 0.00370057604793 + +Bag Rec - Message Pub Delta: 0.000260806108575, 0.0385403633118, 0.000954403421047 + + + +topic: '/novatel/oem7/bestutm', exp interval= 1.0. samples= 58: + +mean, max, stdev + +Bag Recording interval: 0.999384821507, 1.0226817131, 0.00724017624648 + +Message publish interval: 0.999406969338, 1.02262759209, 0.00715263035281 + +Bag Rec - Message Pub Delta: 0.000181029582846, 0.00139737129211, 0.000200204160647 + + + +topic: '/novatel/oem7/insstdev', exp interval= 1.0. samples= 57: + +mean, max, stdev + +Bag Recording interval: 1.00011486241, 1.01120710373, 0.00461768586745 + +Message publish interval: 1.00011816195, 1.01120328903, 0.00446202626225 + +Bag Rec - Message Pub Delta: 0.000224979300248, 0.00264716148376, 0.000435574278454 + + + +ok + + + +---------------------------------------------------------------------- + +Ran 2 tests in 106.496s + + + +OK + +------------------------------------------------------------- + +SUMMARY: + +* RESULT: SUCCESS + +* TESTS: 2 + +* ERRORS: 0 [] + +* FAILURES: 0 [] +``` + + +### Sample Driver Output + +``` +SUMMARY +======== + +PARAMETERS +* /novatel/oem7/oem7_msgs/BDSEPHEMERIS: 1696 +* /novatel/oem7/oem7_msgs/BESTPOS: 42 +* /novatel/oem7/oem7_msgs/BESTUTM: 726 +* /novatel/oem7/oem7_msgs/BESTVEL: 99 +* /novatel/oem7/oem7_msgs/CORRIMUS: 2264 +* /novatel/oem7/oem7_msgs/GALFNAVEPHEMERIS: 1310 +* /novatel/oem7/oem7_msgs/GALINAVEPHEMERIS: 1309 +* /novatel/oem7/oem7_msgs/GLOEPHEMERIS: 723 +* /novatel/oem7/oem7_msgs/HEADING2: 1335 +* /novatel/oem7/oem7_msgs/IMURATECORRIMUS: 1362 +* /novatel/oem7/oem7_msgs/INSCONFIG: 1945 +* /novatel/oem7/oem7_msgs/INSPVAS: 508 +* /novatel/oem7/oem7_msgs/INSPVAX: 1465 +* /novatel/oem7/oem7_msgs/INSSTDEV: 2051 +* /novatel/oem7/oem7_msgs/INSUPDATESTATUS: 1825 +* /novatel/oem7/oem7_msgs/PSRDOP2: 1163 +* /novatel/oem7/oem7_msgs/RANGE: 43 +* /novatel/oem7/oem7_msgs/RAWEPHEM: 41 +* /novatel/oem7/oem7_msgs/RAWIMUSX: 1462 +* /novatel/oem7/oem7_msgs/RXSTATUS: 93 +* /novatel/oem7/oem7_msgs/TIME: 101 +* /novatel/oem7/receivers/main/BESTPOS/frame_id: gps +* /novatel/oem7/receivers/main/BESTPOS/topic: /novatel/oem7/bes... +* /novatel/oem7/receivers/main/BESTUTM/frame_id: gps +* /novatel/oem7/receivers/main/BESTUTM/topic: /novatel/oem7/bes... +* /novatel/oem7/receivers/main/BESTVEL/frame_id: gps +* /novatel/oem7/receivers/main/BESTVEL/topic: /novatel/oem7/bes... +* /novatel/oem7/receivers/main/CORRIMU/frame_id: gps +* /novatel/oem7/receivers/main/CORRIMU/topic: /novatel/oem7/cor... +* /novatel/oem7/receivers/main/GPSFix/frame_id: gps +* /novatel/oem7/receivers/main/GPSFix/topic: /gps/gps +* /novatel/oem7/receivers/main/HEADING2/frame_id: gps +* /novatel/oem7/receivers/main/HEADING2/topic: /novatel/oem7/hea... +* /novatel/oem7/receivers/main/IMU/frame_id: gps +* /novatel/oem7/receivers/main/IMU/topic: /gps/imu +* /novatel/oem7/receivers/main/INSCONFIG/frame_id: gps +* /novatel/oem7/receivers/main/INSCONFIG/queue_size: 10 +* /novatel/oem7/receivers/main/INSCONFIG/topic: /novatel/oem7/ins... +* /novatel/oem7/receivers/main/INSPVA/frame_id: gps +* /novatel/oem7/receivers/main/INSPVA/topic: /novatel/oem7/inspva +* /novatel/oem7/receivers/main/INSPVAX/frame_id: gps +* /novatel/oem7/receivers/main/INSPVAX/queue_size: 10 +* /novatel/oem7/receivers/main/INSPVAX/topic: /novatel/oem7/ins... +* /novatel/oem7/receivers/main/INSSTDEV/frame_id: gps +* /novatel/oem7/receivers/main/INSSTDEV/queue_size: 10 +* /novatel/oem7/receivers/main/INSSTDEV/topic: /novatel/oem7/ins... +* /novatel/oem7/receivers/main/NavSatFix/frame_id: gps +* /novatel/oem7/receivers/main/NavSatFix/topic: /gps/fix +* /novatel/oem7/receivers/main/Oem7RawMsg/frame_id: gps +* /novatel/oem7/receivers/main/Oem7RawMsg/queue_size: 200 +* /novatel/oem7/receivers/main/Oem7RawMsg/topic: /novatel/oem7/oem... +* /novatel/oem7/receivers/main/RXSTATUS/frame_id: gps +* /novatel/oem7/receivers/main/RXSTATUS/queue_size: 10 +* /novatel/oem7/receivers/main/RXSTATUS/topic: /novatel/oem7/rxs... +* /novatel/oem7/receivers/main/TIME/frame_id: gps +* /novatel/oem7/receivers/main/TIME/topic: /novatel/oem7/time +* /novatel/oem7/receivers/main/oem7_if: Oem7ReceiverTcp +* /novatel/oem7/receivers/main/oem7_ip_addr: 10.128.224.50 +* /novatel/oem7/receivers/main/oem7_msg_decoder: Oem7MessageDecoder +* /novatel/oem7/receivers/main/oem7_msg_handlers: ['BESTPOSHandler'... +* /novatel/oem7/receivers/main/oem7_port: 3001 +* /novatel/oem7/receivers/main/oem7_raw_msgs: ['BESTPOS', 'BEST... +* /novatel/oem7/receivers/main/receiver_init_commands: ['UNLOGALL THISPO... +* /novatel/oem7/supported_imus/0/name: Unknown +* /novatel/oem7/supported_imus/0/rate: 0 +* /novatel/oem7/supported_imus/1/name: Honeywell HG1700 AG1 +* /novatel/oem7/supported_imus/1/rate: 100 +* /novatel/oem7/supported_imus/11/name: Honeywell HG1700 ... +* /novatel/oem7/supported_imus/11/rate: 100 +* /novatel/oem7/supported_imus/12/name: Honeywell HG1700 ... +* /novatel/oem7/supported_imus/12/rate: 100 +* /novatel/oem7/supported_imus/13/name: iMAR ilMU-FSAS +* /novatel/oem7/supported_imus/13/rate: 200 +* /novatel/oem7/supported_imus/16/name: KVH CPT IMU +* /novatel/oem7/supported_imus/16/rate: 200 +* /novatel/oem7/supported_imus/19/name: Northrop Grumman ... +* /novatel/oem7/supported_imus/19/rate: 200 +* /novatel/oem7/supported_imus/20/name: Honeywell HG1930 ... +* /novatel/oem7/supported_imus/20/rate: 100 +* /novatel/oem7/supported_imus/26/name: Northrop Grumman ... +* /novatel/oem7/supported_imus/26/rate: 100 +* /novatel/oem7/supported_imus/27/name: Honeywell HG1900 ... +* /novatel/oem7/supported_imus/27/rate: 100 +* /novatel/oem7/supported_imus/28/name: Honeywell HG1930 ... +* /novatel/oem7/supported_imus/28/rate: 100 +* /novatel/oem7/supported_imus/31/name: Analog Devices AD... +* /novatel/oem7/supported_imus/31/rate: 200 +* /novatel/oem7/supported_imus/32/name: Sensonor STIM300 +* /novatel/oem7/supported_imus/32/rate: 125 +* /novatel/oem7/supported_imus/33/name: KVH1750 IMU +* /novatel/oem7/supported_imus/33/rate: 200 +* /novatel/oem7/supported_imus/4/name: Honeywell HG1700 ... +* /novatel/oem7/supported_imus/4/rate: 100 +* /novatel/oem7/supported_imus/41/name: Epson G320N +* /novatel/oem7/supported_imus/41/rate: 125 +* /novatel/oem7/supported_imus/45/name: KVH 1725 IMU? +* /novatel/oem7/supported_imus/45/rate: 200 +* /novatel/oem7/supported_imus/5/name: Honeywell HG1700 ... +* /novatel/oem7/supported_imus/5/rate: 100 +* /novatel/oem7/supported_imus/52/name: Litef microIMU +* /novatel/oem7/supported_imus/52/rate: 200 +* /novatel/oem7/supported_imus/56/name: Sensonor STIM300,... +* /novatel/oem7/supported_imus/56/rate: 125 +* /novatel/oem7/supported_imus/58/name: Honeywell HG4930 ... +* /novatel/oem7/supported_imus/58/rate: 200 +* /novatel/oem7/supported_imus/61/name: Epson G370N +* /novatel/oem7/supported_imus/61/rate: 200 +* /novatel/oem7/supported_imus/62/name: Epson G320N - 200Hz +* /novatel/oem7/supported_imus/62/rate: 200 +* /novatel/oem7/supported_imus/8/name: Northrop Grumman ... +* /novatel/oem7/supported_imus/8/rate: 200 +* /rosdistro: melodic +* /rosversion: 1.14.3 + +NODES +/novatel/oem7/ +driver (nodelet/nodelet) +/novatel/oem7/receivers/ +main (nodelet/nodelet) +/novatel/oem7/receivers/main/ +config (nodelet/nodelet) + +auto-starting new master +process[master]: started with pid [95] +ROS_MASTER_URI=[http://localhost:11311](http://localhost:11311/) + +setting /run_id to 3c58ff7a-5825-11ea-af66-0242ac110002 +process[rosout-1]: started with pid [106] +started core service [/rosout] +process[novatel/oem7/driver-2]: started with pid [113] +[ INFO] [1582672757.580037186]: Initializing nodelet with 1 worker threads. +process[novatel/oem7/receivers/main-3]: started with pid [114] +[ INFO] [1582672758.427979153]: Loading nodelet /novatel/oem7/receivers/main of type novatel_oem7_driver/Oem7MessageNodelet to manager /novatel/oem7/driver with the following remappings: +[ INFO] [1582672758.482685569]: /novatel/oem7/receivers/main: Oem7MessageNodelet v.0.1.5; Feb 25 2020 23:10:59 +[ INFO] [1582672758.491468200]: Oem7MessageDecoderLib version: 0.1.0 +[ INFO] [1582672758.501851170]: topic [/novatel/oem7/bestpos]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.506864595]: topic [/novatel/oem7/bestvel]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.520382178]: topic [/novatel/oem7/bestutm]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.522930675]: topic [/novatel/oem7/inspva]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.525848469]: topic [/gps/gps]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.531813975]: topic [/gps/fix]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.539888495]: topic [/gps/imu]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.546775428]: topic [/novatel/oem7/corrimu]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.561730288]: topic [/novatel/oem7/insstdev]: frame_id: 'gps'; q size: 10 +[ INFO] [1582672758.566235059]: topic [/novatel/oem7/inspvax]: frame_id: 'gps'; q size: 10 +[ INFO] [1582672758.568718145]: topic [/novatel/oem7/insconfig]: frame_id: 'gps'; q size: 10 +[ INFO] [1582672758.575187796]: topic [/novatel/oem7/heading2]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.586377975]: topic [/novatel/oem7/rxstatus]: frame_id: 'gps'; q size: 10 +[ INFO] [1582672758.597258756]: topic [/novatel/oem7/time]: frame_id: 'gps'; q size: 100 +[ INFO] [1582672758.608667707]: Oem7 Raw message 'BESTPOS' will be published. +[ INFO] [1582672758.608896692]: Oem7 Raw message 'BESTVEL' will be published. +[ INFO] [1582672758.609316891]: Oem7 Raw message 'INSPVAX' will be published. +[ INFO] [1582672758.609700340]: Oem7 Raw message 'INSUPDATESTATUS' will be published. +[ INFO] [1582672758.610101790]: Oem7 Raw message 'RAWIMUSX' will be published. +[ INFO] [1582672758.610435285]: Oem7 Raw message 'RAWEPHEM' will be published. +[ INFO] [1582672758.610800972]: Oem7 Raw message 'GLOEPHEMERIS' will be published. +[ INFO] [1582672758.611205708]: Oem7 Raw message 'BDSEPHEMERIS' will be published. +[ INFO] [1582672758.611586715]: Oem7 Raw message 'GALFNAVEPHEMERIS' will be published. +[ INFO] [1582672758.611931563]: Oem7 Raw message 'GALINAVEPHEMERIS' will be published. +[ INFO] [1582672758.612324055]: Oem7 Raw message 'RANGE' will be published. +[ INFO] [1582672758.612752046]: Oem7 Raw message 'HEADING2' will be published. +[ INFO] [1582672758.613338666]: Oem7 Raw message 'TIME' will be published. +[ INFO] [1582672758.619745586]: Oem7 Raw message 'RXSTATUS' will be published. +[ INFO] [1582672758.620168469]: Oem7 Raw message 'INSCONFIG' will be published. +[ INFO] [1582672758.625131610]: topic [/novatel/oem7/oem7raw]: frame_id: 'gps'; q size: 200 +[ INFO] [1582672758.645420418]: Oem7Net TCP['10.128.224.50' : 3001] +[ INFO] [1582672758.648501360]: Oem7Net socket open: '1; OS error= 0 +process[novatel/oem7/receivers/main/config-4]: started with pid [123] +[ INFO] [1582672759.522221729]: Loading nodelet /novatel/oem7/receivers/main/config of type novatel_oem7_driver/Oem7ConfigNodelet to manager /novatel/oem7/driver with the following remappings: +[ INFO] [1582672759.532702445]: /novatel/oem7/receivers/main/config: Oem7ConfigNodelet v.0.1.5; Feb 25 2020 23:10:59 +[ INFO] [1582672759.549198962]: AACmd 'UNLOGALL THISPORT' : 'OK' +[ INFO] [1582672759.555651969]: AACmd 'LOG INSCONFIGB ONCE' : 'OK' +[ INFO] [1582672759.557937765]: IMU: 'Epson G320N', rate= 125 +[ INFO] [1582672759.563950339]: AACmd 'LOG INSCONFIGB ONTIME 300' : 'OK' +[ INFO] [1582672759.570031172]: AACmd 'LOG RXSTATUSB ONCHANGED' : 'OK' +[ INFO] [1582672759.692688310]: AACmd 'LOG BESTPOSB ONTIME 0.1' : 'OK' +[ INFO] [1582672759.699932134]: AACmd 'LOG BESTVELB ONTIME 0.1' : 'OK' +[ INFO] [1582672759.708925767]: AACmd 'LOG BESTUTMB ONTIME 1' : 'OK' +[ INFO] [1582672759.717126078]: AACmd 'LOG HEADING2B ONNEW' : 'OK' +[ INFO] [1582672759.968983558]: AACmd 'LOG PSRDOP2B ONCHANGED' : 'OK' +[ INFO] [1582672759.979083113]: AACmd 'LOG INSPVASB ONTIME 0.02' : 'OK' +[ INFO] [1582672759.989194363]: AACmd 'LOG INSPVAXB ONTIME 1' : 'OK' +[ INFO] [1582672760.110518011]: AACmd 'LOG CORRIMUSB ONTIME 0.01' : 'OK' +[ INFO] [1582672760.117430487]: AACmd 'LOG INSSTDEVB ONTIME 1' : 'OK' +[ INFO] [1582672760.127582549]: AACmd 'LOG TIMEB ONTIME 1' : 'OK' +[ INFO] [1582672760.250652075]: AACmd 'LOG RAWIMUSXB ONNEW' : 'OK' +[ INFO] [1582672760.259886196]: AACmd 'LOG INSUPDATESTATUSB ONNEW' : 'OK' +[ INFO] [1582672760.268288028]: AACmd 'LOG RAWEPHEMB ONNEW' : 'OK' +[ INFO] [1582672760.441192318]: AACmd 'LOG GLOEPHEMERISB ONNEW' : 'OK' +[ INFO] [1582672760.452404856]: AACmd 'LOG BDSEPHEMERISB ONNEW' : 'OK' +[ INFO] [1582672760.463694659]: AACmd 'LOG GALFNAVEPHEMERISB ONNEW' : 'OK' +[ INFO] [1582672760.476838316]: AACmd 'LOG GALINAVEPHEMERISB ONNEW' : 'OK' +[ INFO] [1582672760.485394230]: AACmd 'LOG RANGEB ONTIME 1' : 'OK' +[ INFO] [1582672760.486592625]: Oem7 configuration completed. +[ INFO] [1582672793.168160450]: Log Statistics: +[ INFO] [1582672793.170725228]: Logs: 10000; discarded: 37 +[ INFO] [1582672793.171424835]: Log[RAWEPHEM](41):10 +[ INFO] [1582672793.171991321]: Log[BESTPOS](42):335 +[ INFO] [1582672793.172548012]: Log[RANGE](43):33 +[ INFO] [1582672793.173131571]: Log[RXSTATUS](93):1 +[ INFO] [1582672793.173925225]: Log[BESTVEL](99):335 +[ INFO] [1582672793.174466830]: Log[TIME](101):33 +[ INFO] [1582672793.175082019]: Log[INSPVAS](508):1660 +[ INFO] [1582672793.177104558]: Log[GLOEPHEMERIS](723):8 +[ INFO] [1582672793.177702006]: Log[BESTUTM](726):34 +[ INFO] [1582672793.178415540]: Log[PSRDOP2](1163):2 +[ INFO] [1582672793.182107115]: Log[HEADING2](1335):34 +[ INFO] [1582672793.182548463]: Log[RAWIMUSX](1462):4130 +[ INFO] [1582672793.183634882]: Log[INSPVAX](1465):33 +[ INFO] [1582672793.187078494]: Log[INSCONFIG](1945):2 +[ INFO] [1582672793.191874451]: Log[INSSTDEV](2051):33 +[ INFO] [1582672793.192099653]: Log[CORRIMUS](2264):3317 +``` + +#### Output Interpretation: + + +``` +[ INFO] [1582672759.692688310]: AACmd 'LOG BESTPOSB ONTIME 0.1' : 'OK' +``` +Initialization command sent, "OK" response received. + + + + +``` +[ INFO] [1582672759.557937765]: IMU: 'Epson G320N', rate= 12 +``` +Detected IMU and assumed data rate. + + + + +``` +Logs: 10000; discarded: 37 +``` +10000 logs received and processed; 37 logs or message fragments discarded. +Discarded fragments during receiver configuration are normal. However, continuously increasing number of discarded fragments during stable operation indicates misconfiguration. +Received OEM7 Message statistics are output periodically. + + + +``` +Log[BESTPOS](42):335 +``` +335 BESTPOS logs (ID=42) received. + + + +## Authors + +* [**NovAtel**](https://www.novatel.com), part of [**Hexagon**](https://hexagon.com) + + +## License + +This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details + + diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..de1b2fc --- /dev/null +++ b/build.sh @@ -0,0 +1,111 @@ +#!/bin/bash + +print_usage() +{ + echo "Build novatel_oem7_driver" + echo "Usage: " + echo " $0 -h" + echo " $0 [-c] [-d | -r] [-p] [-t]" + echo + echo "Where:" + echo " -h: Print this help message and exit." + echo " -c: Clean all intermediate files." + echo " -d: Build debug binaries" + echo " -r: Build release binaries; can't be combined with -d" + echo " -t: Run tests using local binaries." + echo " -p: Build .deb package(s)" + echo " -f: Full build, equivalent to '$0 -crtp'" + echo + echo "Examples" + echo " '$0' Build local binaries; used for local development." + echo " '$0 -f' Full build: Used for production artifacts." + echo " '$0 -r' Release build, no tests or .deb packages" + echo " '$0 -cd' Clean Debug build, no packages." +} + + + +build_deb_pkg() +{ + ./create-$ROS_DISTRO-package.sh +} + + +INSTALL= +CLEAN= +DEBUG_FLAG= +ROSDOC= +BUILD_DEB_PKG= +RUN_TESTS= + + +while getopts "hcdrpft" OPT; do + case $OPT in + h ) + print_usage + exit 0 + ;; + + c ) + CLEAN=clean + ;; + + d ) + INSTALL=install + DEBUG_FLAG=-DCMAKE_BUILD_TYPE=Debug + ;; + + r ) + INSTALL=install + DEBUG_FLAG= + ;; + + p ) + INSTALL=install + BUILD_DEB_PKG=build_deb_pkg + ;; + + t ) + RUN_TESTS=run_tests + ;; + + f ) + CLEAN=clean + INSTALL=install + BUILD_DEB_PKG=build_deb_pkg + RUN_TESTS=run_tests + ;; + + * ) + print_usage + exit 1 + ;; + esac +done + + + +if [[ $CLEAN ]]; +then + # Remove all intermediate and temporary files. + catkin_make clean + rm -rf .ros devel build doc install + rm -rf *.deb src/*.ddeb + rm -rf src/novatel_oem7_driver/debian src/novatel_oem7_driver/obj-* + rm -rf src/novatel_oem7_msgs/debian src/novatel_oem7_msgs/obj-* +fi + + + +set -e + +. ./envsetup.sh + +# Build local artifacts +catkin_make $DEBUG_FLAG $CLEAN $INSTALL $RUN_TESTS + +if [[ $INSTALL ]]; +then + rosdoc_lite src/novatel_oem7_driver -o doc + $BUILD_DEB_PKG +fi diff --git a/create-binary-package.sh b/create-binary-package.sh new file mode 100755 index 0000000..45a5237 --- /dev/null +++ b/create-binary-package.sh @@ -0,0 +1,16 @@ +#!/bin/sh + +UBUNTU_TGT_DISTRO=$1 +ROS_TGT_DISTRO=$2 + +rosdep init +rosdep update + +rm *.deb +rm src/*.deb +./create-deb.sh $UBUNTU_TGT_DISTRO $ROS_TGT_DISTRO src/novatel_oem7_msgs +./create-deb.sh $UBUNTU_TGT_DISTRO $ROS_TGT_DISTRO src/novatel_oem7_driver +mv src/*.deb . + + + diff --git a/create-deb.sh b/create-deb.sh new file mode 100755 index 0000000..45ba0d2 --- /dev/null +++ b/create-deb.sh @@ -0,0 +1,16 @@ +#!/bin/sh + +CALLED_FROM=$PWD +cd $3 + +rm -rf debian obj-* +bloom-generate rosdebian --os-name ubuntu --os-version $1 --ros-distro $2 + + +fakeroot -- debian/rules binary + +cd $CALLED_FROM + + + + diff --git a/create-kinetic-package.sh b/create-kinetic-package.sh new file mode 100755 index 0000000..059e3ba --- /dev/null +++ b/create-kinetic-package.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +./create-binary-package.sh xenial kinetic + diff --git a/create-melodic-package.sh b/create-melodic-package.sh new file mode 100755 index 0000000..cdf4bdb --- /dev/null +++ b/create-melodic-package.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +./create-binary-package.sh bionic melodic + diff --git a/create_eclipse_project.sh b/create_eclipse_project.sh new file mode 100755 index 0000000..39b8ef6 --- /dev/null +++ b/create_eclipse_project.sh @@ -0,0 +1,5 @@ +#!/bin/sh + +catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles" +awk -f $(rospack find mk)/eclipse.awk build/.project > build/.project_with_env && mv build/.project_with_env build/.project + diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..e5b1357 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,5 @@ +from ros:kinetic + + + + diff --git a/docker/Dockerfile.kinetic.build b/docker/Dockerfile.kinetic.build new file mode 100644 index 0000000..a592d5a --- /dev/null +++ b/docker/Dockerfile.kinetic.build @@ -0,0 +1,42 @@ +from ros:kinetic + +# Needed to deal with a recent key revocation, presumably melodic dockerfile will be updated eventually, and this can be remvoed. +#sudo apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 +#sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + +#sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +#sudo apt-get update && sudo apt-get install -y sudo ros-kinetic-desktop ros-kinetic-catkin build-essential gdb + +#sudo apt-get install dh-make +#rosdep init +#rosdep update +#sudo chown -R $( whoami) ~/.ros + + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y ros-kinetic-tf ros-kinetic-gps-umd ros-kinetic-rosdoc-lite dh-make python-bloom + +# Replace dash with bash +#RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +#---------------------------------- +# TODO - Not sure about this, since we want to run as your host user id. Might need to remove this build user. +# User management +RUN groupadd -g 1000 build +RUN useradd -u 1000 -g 1000 -G sudo,build -ms /bin/bash build + +USER build +#---------------------------------- + + +RUN rosdep init +RUN rosdep update + + +# bindmount source directory +# Make /home/build the working directory. This will be mapped to a host directory on your machine. +#WORKDIR /home/build + +# If no command is provided when the container is started, this will be run, giving you a shell inside the container as the 'build' user. +CMD "/bin/bash" diff --git a/docker/Dockerfile.kinetic.test b/docker/Dockerfile.kinetic.test new file mode 100644 index 0000000..dee04dc --- /dev/null +++ b/docker/Dockerfile.kinetic.test @@ -0,0 +1,24 @@ +from ros:kinetic + +# Needed to deal with a recent key revocation, presumably melodic dockerfile will be updated eventually, and this can be remvoed. +#sudo apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 +#sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + +#sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +#sudo apt-get update && sudo apt-get install -y sudo ros-kinetic-desktop ros-kinetic-catkin build-essential gdb + + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y ros-kinetic-tf ros-kinetic-gps-umd vim sudo +RUN apt install + +#---------------------------------- +# Create 'test:test' user/password in sudo group. +RUN useradd -m test && echo "test:test" | chpasswd && adduser test sudo +USER test +#---------------------------------- + + +# If no command is provided when the container is started, this will be run, giving you a shell inside the container as the 'test' user. +CMD "/bin/bash" diff --git a/docker/Dockerfile.melodic.build b/docker/Dockerfile.melodic.build new file mode 100644 index 0000000..79fdba5 --- /dev/null +++ b/docker/Dockerfile.melodic.build @@ -0,0 +1,38 @@ +from ros:melodic + +# Needed to deal with a recent key revocation, presumably melodic dockerfile will be updated eventually, and this can be remvoed. +#sudo apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 +#sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + +#sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +#sudo apt-get update && sudo apt-get install -y sudo ros-kinetic-desktop ros-kinetic-catkin build-essential gdb + +#sudo apt-get install dh-make +#rosdep init +#rosdep update +#sudo chown -R $( whoami) ~/.ros + + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y ros-melodic-rosdoc-lite ros-melodic-tf ros-melodic-gps-umd dh-make python-bloom vim + +# Replace dash with bash +#RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +#---------------------------------- +# TODO - Not sure about this, since we want to run as your host user id. Might need to remove this build user. +# User management +RUN groupadd -g 1000 build +RUN useradd -u 1000 -g 1000 -G sudo,build,dialout -ms /bin/bash build +USER build +#---------------------------------- + + + +# bindmount source directory +# Make /home/build the working directory. This will be mapped to a host directory on your machine. +#WORKDIR /home/build + +# If no command is provided when the container is started, this will be run, giving you a shell inside the container as the 'build' user. +CMD "/bin/bash" diff --git a/docker/Dockerfile.melodic.test b/docker/Dockerfile.melodic.test new file mode 100644 index 0000000..a754d56 --- /dev/null +++ b/docker/Dockerfile.melodic.test @@ -0,0 +1,22 @@ +from ros:melodic + +# Needed to deal with a recent key revocation, presumably melodic dockerfile will be updated eventually, and this can be remvoed. +#sudo apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 +#sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + +#sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y ros-melodic-tf ros-melodic-gps-umd vim sudo + +#---------------------------------- +# Create 'test:test' user/password in sudo group. +RUN useradd -m test && echo "test:test" | chpasswd && adduser test sudo +USER test +#---------------------------------- + + +# If no command is provided when the container is started, this will be run, giving you a shell inside the container as the 'test' user. +CMD "/bin/bash" diff --git a/docker/run-distro-build.sh b/docker/run-distro-build.sh new file mode 100755 index 0000000..f8ea4d2 --- /dev/null +++ b/docker/run-distro-build.sh @@ -0,0 +1,9 @@ +#!/bin/sh + +DISTRO=$1 +NAME=ros-$DISTRO-build +docker build -t $NAME - < docker/Dockerfile.$DISTRO.build + +BLDDIR=/home/build +docker run -ti --mount type=bind,source="$(pwd)",target=$BLDDIR -w $BLDDIR -u `id -u`:`id -g` --group-add sudo --rm $NAME:latest + diff --git a/docker/run-kinetic-build.sh b/docker/run-kinetic-build.sh new file mode 100755 index 0000000..e2e5212 --- /dev/null +++ b/docker/run-kinetic-build.sh @@ -0,0 +1,8 @@ +#!/bin/sh + +NAME=ros-kinetic-build +docker build -t $NAME - < docker/Dockerfile.kinetic.build + +BLDDIR=/home/build +docker run -ti --mount type=bind,source="$(pwd)",target=$BLDDIR -w $BLDDIR -u `id -u`:`id -g` --group-add sudo --rm $NAME:latest + diff --git a/docker/run-kinetic-test.sh b/docker/run-kinetic-test.sh new file mode 100755 index 0000000..473b339 --- /dev/null +++ b/docker/run-kinetic-test.sh @@ -0,0 +1,9 @@ +#!/bin/sh + +NAME=ros-kinetic-test +docker build -t $NAME - < docker/Dockerfile.kinetic.test + +CONT_TESTDIR=/home/test +HOST_TESTDIR=$(pwd)/docker-test +docker run -ti --mount type=bind,source=$HOST_TESTDIR,target=$CONT_TESTDIR -w $CONT_TESTDIR -u `id -u`:`id -g` --group-add sudo --rm $NAME:latest + diff --git a/docker/run-melodic-build.sh b/docker/run-melodic-build.sh new file mode 100755 index 0000000..71bd561 --- /dev/null +++ b/docker/run-melodic-build.sh @@ -0,0 +1,5 @@ +#!/bin/sh + +./docker/run-distro-build.sh melodic + + diff --git a/docker/run-melodic-test.sh b/docker/run-melodic-test.sh new file mode 100755 index 0000000..1e92379 --- /dev/null +++ b/docker/run-melodic-test.sh @@ -0,0 +1,9 @@ +#!/bin/sh + +NAME=ros-melodic-test +docker build -t $NAME - < docker/Dockerfile.melodic.test + +CONT_TESTDIR=/home/test +HOST_TESTDIR=$(pwd)/docker-test +docker run -ti --mount type=bind,source=$HOST_TESTDIR,target=$CONT_TESTDIR -w $CONT_TESTDIR -u `id -u`:`id -g` --group-add sudo --rm $NAME:latest + diff --git a/docker/run-test.sh b/docker/run-test.sh new file mode 100755 index 0000000..3b158b8 --- /dev/null +++ b/docker/run-test.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +docker build -t novatel_oem7_driver . +docker run -ti --mount type=bind,source="$(pwd)",target=/test --rm novatel_oem7_driver:latest diff --git a/envsetup.sh b/envsetup.sh new file mode 100644 index 0000000..6c18640 --- /dev/null +++ b/envsetup.sh @@ -0,0 +1,12 @@ + +source /opt/ros/$ROS_DISTRO/setup.bash + +# If no catkin environment is setup, create it +if [ ! -d ./devel ]; then + echo "catkin environment does not exist; creating it. This runs the local build once as well." + catkin_make +fi + +source ./devel/setup.bash + + diff --git a/novatel_oem7/bin/liboem7decoder.a b/novatel_oem7/bin/liboem7decoder.a new file mode 100644 index 0000000..8cdfa55 --- /dev/null +++ b/novatel_oem7/bin/liboem7decoder.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:460c465ff8ab73e44bf9ab2ba79d040ecc1bb9d61bb44a7e2ed7fc12b61e9c6e +size 111757832 diff --git a/novatel_oem7/include/oem7_message_decoder_lib.hpp b/novatel_oem7/include/oem7_message_decoder_lib.hpp new file mode 100644 index 0000000..bfdc3c2 --- /dev/null +++ b/novatel_oem7/include/oem7_message_decoder_lib.hpp @@ -0,0 +1,66 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_MESSAGE_DECODER__ +#define __OEM7_MESSAGE_DECODER__ + +#include + +#include +#include + + + +namespace novatel_oem7 +{ + typedef short version_element_t; + + class Oem7MessageDecoderLibIf + { + + public: + virtual ~Oem7MessageDecoderLibIf(){} + virtual bool readMessage(boost::shared_ptr&) = 0; + + }; + + class Oem7MessageDecoderLibUserIf + { + public: + virtual ~Oem7MessageDecoderLibUserIf(){} + virtual bool read( boost::asio::mutable_buffer, size_t&) = 0; + }; + + + boost::shared_ptr + GetOem7MessageDecoder(Oem7MessageDecoderLibUserIf*); + + void + GetOem7MessageDecoderLibVersion(version_element_t& major, version_element_t& minor, version_element_t& build); +} + + + + +#endif diff --git a/novatel_oem7/include/oem7_raw_message_if.hpp b/novatel_oem7/include/oem7_raw_message_if.hpp new file mode 100644 index 0000000..5f4a8ba --- /dev/null +++ b/novatel_oem7/include/oem7_raw_message_if.hpp @@ -0,0 +1,67 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_RAW_MESSAGE_IF_ +#define __OEM7_RAW_MESSAGE_IF_ + +#include +#include + +#include + +namespace novatel_oem7 +{ + class Oem7RawMessageIf + { + public: + + typedef boost::shared_ptr ConstPtr; + + enum Oem7MessageType + { + OEM7MSGTYPE_LOG, + OEM7MSGTYPE_RSP, + OOEM7MSGTYPE_CMD, + OEM7MSGTYPE_UNKNOWN = 1000 + }; + + enum Oem7MessageFormat + { + OEM7MSGFMT_BINARY, + OEM7MSGFMT_ASCII, + OEM7MSGFMT_ABASCII, + OEM7MSGFMT_UNKNOWN = 1000 + }; + + virtual ~Oem7RawMessageIf(){} + + virtual Oem7MessageType getMessageType() const = 0; + virtual Oem7MessageFormat getMessageFormat() const = 0; + virtual int getMessageId() const = 0; + virtual const uint8_t* getMessageData(size_t offset) const = 0; + virtual size_t getMessageDataLength() const = 0; + }; +} + +#endif diff --git a/src/novatel_oem7_driver/CMakeLists.txt b/src/novatel_oem7_driver/CMakeLists.txt new file mode 100644 index 0000000..0fb5cb3 --- /dev/null +++ b/src/novatel_oem7_driver/CMakeLists.txt @@ -0,0 +1,157 @@ +cmake_minimum_required(VERSION 2.8.3) +project(novatel_oem7_driver) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +# Without this line, .deb package build fails due to lpthread linking errors. +set(novatel_oem7_msgs_DIR ${CMAKE_CURRENT_LIST_DIR}/../../install/share/novatel_oem7_msgs/cmake) + + +set(BUILD_DEPS + nodelet + roscpp + std_msgs + nav_msgs + gps_common + sensor_msgs + tf + message_generation + novatel_oem7_msgs +) + + +find_package(catkin REQUIRED COMPONENTS ${BUILD_DEPS}) +find_package(Boost REQUIRED COMPONENTS system thread) +find_package(novatel_oem7_msgs REQUIRED) + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJET_NAME} + CATKIN_DEPENDS nodelet roscpp +) + +# Make package available as a macro to C++ +add_definitions("-D${PROJECT_NAME}_VERSION=\"${${PROJECT_NAME}_VERSION}\"") + +########### +## Build ## +########### + +include_directories(include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${novatel_oem7_msgs_INCLUDE_DIRS} + "../../novatel_oem7/include" + src +) + + + +## All components are plugins +add_library(${PROJECT_NAME} + src/oem7_log_nodelet.cpp + src/oem7_message_nodelet.cpp + src/oem7_config_nodelet.cpp + src/oem7_receiver_net.cpp + src/oem7_receiver_port.cpp + src/oem7_receiver_file.cpp + src/oem7_message_decoder.cpp + src/oem7_message_util.cpp + src/oem7_ros_messages.cpp + src/message_handler.cpp + src/bestpos_handler.cpp + src/ins_handler.cpp + src/align_handler.cpp + src/time_handler.cpp + src/receiverstatus_handler.cpp +) + + +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + +FIND_LIBRARY(OEM7_DECODER_LIB liboem7decoder.a "../../novatel_oem7/bin") +message(STATUS "Linking to Oem7 Decoder at: '${OEM7_DECODER_LIB}'") + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${OEM7_DECODER_LIB} +) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Export plugin header files. +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install( + FILES novatel_oem7_driver_nodelets.xml novatel_oem7_driver_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install( + DIRECTORY + launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install( + DIRECTORY + config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + + +############# +## Testing ## +############# + +# Built-in Self Test (BIST) +catkin_install_python(PROGRAMS test/oem7_bist.py test/oem7_message_test.py + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +# Instaleld into SHARED vs BINARY. BINARY does not allow BIST to be run using +# rostest novatel_oem7_driver your_launch_file.lauch" syntax. + + +find_package(rostest REQUIRED) +add_rostest(test/align.test) +add_rostest(test/bestpos.test) +add_rostest(test/ins1.test) +add_rostest(test/ins2.test) +add_rostest(test/rxstatus.test) +add_rostest(test/time.test) + + + +#message("includes:") +#get_property(incdirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) +#foreach(dir ${incdirs}) +# message(STATUS "incdir= '${dir}'") +#endforeach() diff --git a/src/novatel_oem7_driver/config/oem7_msgs.yaml b/src/novatel_oem7_driver/config/oem7_msgs.yaml new file mode 100644 index 0000000..e8fb6ea --- /dev/null +++ b/src/novatel_oem7_driver/config/oem7_msgs.yaml @@ -0,0 +1,26 @@ +# Oem7 messages; message names and IDs are as they appear in Oem7 manual. +# Correctness of message ID is critical for correct operation. +oem7_msgs: + # Standard messages - Do not remove. + BESTPOS: 42 + BESTVEL: 99 + BESTUTM: 726 + INSPVAS: 508 + INSPVAX: 1465 + INSSTDEV: 2051 + CORRIMUS: 2264 + IMURATECORRIMUS: 1362 + RANGE: 43 + RAWIMUSX: 1462 + RAWEPHEM: 41 + GLOEPHEMERIS: 723 + BDSEPHEMERIS: 1696 + GALINAVEPHEMERIS: 1309 + GALFNAVEPHEMERIS: 1310 + HEADING2: 1335 + TIME: 101 + RXSTATUS: 93 + INSUPDATESTATUS: 1825 + INSCONFIG: 1945 + PSRDOP2: 1163 + # More messages may be added in order to process Oem7 'raw' messages. diff --git a/src/novatel_oem7_driver/config/oem7_supported_imus.yaml b/src/novatel_oem7_driver/config/oem7_supported_imus.yaml new file mode 100644 index 0000000..96d63d7 --- /dev/null +++ b/src/novatel_oem7_driver/config/oem7_supported_imus.yaml @@ -0,0 +1,25 @@ +supported_imus: + "0" : { name: "Unknown", rate: "0" } + "1" : { name: "Honeywell HG1700 AG1", rate: "100" } + "4" : { name: "Honeywell HG1700 AG17", rate: "100" } + "5" : { name: "Honeywell HG1700 CA29", rate: "100" } + "8" : { name: "Northrop Grumman LN-200/LN-200C", rate: "200" } + "11" : { name: "Honeywell HG1700 AG58", rate: "100" } + "12" : { name: "Honeywell HG1700 AG62", rate: "100" } + "13" : { name: "iMAR ilMU-FSAS", rate: "200" } + "16" : { name: "KVH CPT IMU", rate: "200" } + "19" : { name: "Northrop Grumman Litef LCI-1", rate: "200" } + "20" : { name: "Honeywell HG1930 AA99", rate: "100" } + "26" : { name: "Northrop Grumman Litef ISA-100C", rate: "100" } + "27" : { name: "Honeywell HG1900 CA50", rate: "100" } + "28" : { name: "Honeywell HG1930 CA50", rate: "100" } + "31" : { name: "Analog Devices ADIS16488", rate: "200" } + "32" : { name: "Sensonor STIM300", rate: "125" } + "33" : { name: "KVH1750 IMU", rate: "200" } + "41" : { name: "Epson G320N", rate: "125" } + "45" : { name: "KVH 1725 IMU?", rate: "200" } + "52" : { name: "Litef microIMU" , rate: "200" } + "56" : { name: "Sensonor STIM300, Direct Connection", rate: "125" } + "58" : { name: "Honeywell HG4930 AN01", rate: "200" } + "61" : { name: "Epson G370N", rate: "200" } + "62" : { name: "Epson G320N - 200Hz", rate: "200" } \ No newline at end of file diff --git a/src/novatel_oem7_driver/config/std_driver_config.xml b/src/novatel_oem7_driver/config/std_driver_config.xml new file mode 100644 index 0000000..70d9914 --- /dev/null +++ b/src/novatel_oem7_driver/config/std_driver_config.xml @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/novatel_oem7_driver/config/std_init_commands.yaml b/src/novatel_oem7_driver/config/std_init_commands.yaml new file mode 100644 index 0000000..838cb88 --- /dev/null +++ b/src/novatel_oem7_driver/config/std_init_commands.yaml @@ -0,0 +1,90 @@ +# Oem7 Receiver initialization and configuration Commands, sent to Oem7 as-is. +# Refer to Oem7 Receiver documentation. +# +# +# Lines marked as "Do Not Remove" are needed to ensure correct operation. +# Some ROS messages are composed from a combination of Oem7 messages. +# It is critical to log individiual Oem7 message to ensure correct generation and field population. +# +# Message generation/composition as as follows: +# +# --------------------------------------------------------------------------------------------------- +# 'GPSFix': +# BESTPOSB +# BESTVELB +# INSPVASB (on IMU-enabled units only, not needed on non-IMU units) +# PSDROP2B (on IMU-enabled units only, not needed on non-IMU units) +# +# Output rate: the highest rate of: +# BESTPOSB, BESTVELB, INSPVASB. +# Typically, INSPVASB is logged at the highest rate and thus generates GPSFix +# +# ----------------------------------------------------------------------------------------------------- +# NavSatFix: published at the same rate as GPSFix; data derived from: +# BESTPOSB +# +# It is recommended to use GPSFix instead of NavSatFix. +# +# ----------------------------------------------------------------------------------------------------- +# 'Imu': +# INSPVASB +# INSSTDEVB +# CORRIMUSB or IMURATECORRIMUSB +# +# Output rate: the highest rate of: +# CORRIMUSB or IMURATECORRIMUSB + +# Commented out Oem7 commands are for example purposes +receiver_init_commands: +#----------------------------------------------------------------------------------------------------- +# Reset logging + +- "UNLOGALL THISPORT" +#- "FRESET" +#- "!PAUSE 10" +#- "SETINSPROFILE LAND" +#- "SETINSTRANSLATION ANT 1 +#- ... etc. + + +#------------------------------------------------------------------------------------------------------- +# Standard Logging Requests + +#---------------------------- +# Status & Config +- "LOG INSCONFIGB ONCE" # Log once to obtain initial configuration. Do not remove, needed for IMU messages. +- "LOG INSCONFIGB ONTIME 300" # Periodic update. +- "LOG RXSTATUSB ONCHANGED" + +# GNSS and INS Solutions + +# Do not remove these messages -------------------------------------------------------------------------- +- "LOG BESTPOSB ONTIME 0.1" +- "LOG BESTVELB ONTIME 0.1" +- "LOG BESTUTMB ONTIME 1" +- "LOG HEADING2B ONNEW" +- "LOG PSRDOP2B ONCHANGED" +- "LOG INSPVASB ONTIME 0.02" +- "LOG INSPVAXB ONTIME 1" +- "LOG CORRIMUSB ONTIME 0.01" +- "LOG INSSTDEVB ONTIME 1" +#---------------------------------------------------------------------------------------------------------- + +- "LOG TIMEB ONTIME 1" # Can be used as a 'heartbeat' message. + +# Post-Processing Messages; not directly used to generate ROS messages. +- "LOG RAWIMUSXB ONNEW" +- "LOG INSUPDATESTATUSB ONNEW" +- "LOG RAWEPHEMB ONNEW" +- "LOG GLOEPHEMERISB ONNEW" +- "LOG BDSEPHEMERISB ONNEW" +- "LOG GALFNAVEPHEMERISB ONNEW" +- "LOG GALINAVEPHEMERISB ONNEW" +- "LOG RANGEB ONTIME 1" + + + + + + + diff --git a/src/novatel_oem7_driver/config/std_msg_handlers.yaml b/src/novatel_oem7_driver/config/std_msg_handlers.yaml new file mode 100644 index 0000000..fe3e24a --- /dev/null +++ b/src/novatel_oem7_driver/config/std_msg_handlers.yaml @@ -0,0 +1,7 @@ +oem7_msg_handlers: +- "BESTPOSHandler" +- "INSHandler" +- "ALIGNHandler" +- "RXSTATUSHandler" +- "TimeHandler" + \ No newline at end of file diff --git a/src/novatel_oem7_driver/config/std_msg_topics.yaml b/src/novatel_oem7_driver/config/std_msg_topics.yaml new file mode 100644 index 0000000..eaee01c --- /dev/null +++ b/src/novatel_oem7_driver/config/std_msg_topics.yaml @@ -0,0 +1,20 @@ +# ROS-standard +IMU: {topic: /gps/imu, frame_id: gps} +GPSFix: {topic: /gps/gps, frame_id: gps} +NavSatFix: {topic: /gps/fix, frame_id: gps} + +# Oem7-specific +Oem7RawMsg: {topic: /novatel/oem7/oem7raw, frame_id: gps, queue_size: "200"} +BESTPOS: {topic: /novatel/oem7/bestpos, frame_id: gps} +BESTUTM: {topic: /novatel/oem7/bestutm, frame_id: gps} +BESTVEL: {topic: /novatel/oem7/bestvel, frame_id: gps} +CORRIMU: {topic: /novatel/oem7/corrimu, frame_id: gps} +HEADING2: {topic: /novatel/oem7/heading2, frame_id: gps} +INSPVA: {topic: /novatel/oem7/inspva, frame_id: gps} +INSPVAX: {topic: /novatel/oem7/inspvax, frame_id: gps, queue_size: "10"} +INSSTDEV: {topic: /novatel/oem7/insstdev, frame_id: gps, queue_size: "10"} +INSCONFIG: {topic: /novatel/oem7/insconfig, frame_id: gps, queue_size: "10"} +RXSTATUS: {topic: /novatel/oem7/rxstatus, frame_id: gps, queue_size: "10"} +TIME: {topic: /novatel/oem7/time, frame_id: gps} + + diff --git a/src/novatel_oem7_driver/config/std_oem7_raw_msgs.yaml b/src/novatel_oem7_driver/config/std_oem7_raw_msgs.yaml new file mode 100644 index 0000000..3969445 --- /dev/null +++ b/src/novatel_oem7_driver/config/std_oem7_raw_msgs.yaml @@ -0,0 +1,17 @@ +oem7_raw_msgs: +- BESTPOS +- BESTVEL +- INSPVAX +- INSUPDATESTATUS +- RAWIMUSX +- RAWEPHEM +- GLOEPHEMERIS +- BDSEPHEMERIS +- GALFNAVEPHEMERIS +- GALINAVEPHEMERIS +- RANGE +- HEADING2 +- TIME +- RXSTATUS +- INSCONFIG + diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_decoder_if.hpp b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_decoder_if.hpp new file mode 100644 index 0000000..b635508 --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_decoder_if.hpp @@ -0,0 +1,85 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_MESSAGE_PARSER_IF_HPP__ +#define __OEM7_MESSAGE_PARSER_IF_HPP__ + + +#include +#include +#include +#include + +#include +#include + +#include "novatel_oem7_msgs/Oem7RawMsg.h" + +namespace novatel_oem7_driver +{ + /** + * Interface implemented by Oem7RawMessageParserIf users, in order to receiver message callbacks. + */ + class Oem7MessageDecoderUserIf + { + public: + virtual ~Oem7MessageDecoderUserIf(){}; + + /** + * Called when new message is available. + */ + virtual void onNewMessage(boost::shared_ptr) = 0; + }; + + + /** + * Interface for accessing Oem7 message decoder. + * The user is responsible for initializing it, and calling service from its preferred context. + * Any callbacks on Oem7RawMessageParserUserIf are made from the service context. + */ + class Oem7MessageDecoderIf + { + public: + virtual ~Oem7MessageDecoderIf(){} + + /** + * Initializes the parser. + * @return true on success + */ + virtual bool initialize( + ros::NodeHandle& nh, /**< [in] handle of the owner node. Parser uses it to access ROS environment. */ + Oem7ReceiverIf* recvr, /**< [in] Receiver interface used for data input */ + Oem7MessageDecoderUserIf* user /**< [in] Interface to receiver message callbacks */ + ) = 0; + + /** + * Message decoder service loop; blocks as long as input is available. + * Returns when no more input is available; or when ros::ok() returns false. + */ + virtual void service() = 0; + }; +} + + +#endif diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_handler_if.hpp b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_handler_if.hpp new file mode 100644 index 0000000..d3225e9 --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_handler_if.hpp @@ -0,0 +1,64 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_MESSAGE_HANDLER_IF_HPP__ +#define __OEM7_MESSAGE_HANDLER_IF_HPP__ + + +#include + +#include + +#include "oem7_raw_message_if.hpp" +using novatel_oem7::Oem7RawMessageIf; + +namespace novatel_oem7_driver +{ + /** + * Interface implemented by modules handling Oem7RawMessageIf messages + */ + class Oem7MessageHandlerIf + { + public: + virtual ~Oem7MessageHandlerIf(){}; + + /** + * Initializes the handler + */ + virtual void initialize(ros::NodeHandle&) = 0; + + /** + * @return a vector of Oem7 message IDs to be handled by this Handler. + */ + virtual const std::vector& getMessageIds() = 0; + + /** + * Handle a message + */ + virtual void handleMsg(Oem7RawMessageIf::ConstPtr msg) = 0; + }; +} + + +#endif diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_ids.h b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_ids.h new file mode 100644 index 0000000..d90d31f --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_ids.h @@ -0,0 +1,50 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_MESSAGE_IDS_HPP__ +#define __OEM7_MESSAGE_IDS_HPP__ + +/*! \file + * Oem7 Message ID definitions. + * Refer to Oem7 manual. + */ + +namespace novatel_oem7_driver +{ + const int BESTPOS_OEM7_MSGID = 42; + const int BESTUTM_OEM7_MSGID = 726; + const int BESTVEL_OEM7_MSGID = 99; + const int CORRIMUS_OEM7_MSGID = 2264; + const int HEADING2_OEM7_MSGID = 1335; + const int IMURATECORRIMU_OEM7_MSGID = 1362; + const int INSCONFIG_OEM7_MSGID = 1945; + const int INSPVAS_OEM7_MSGID = 508; + const int INSPVAX_OEM7_MSGID = 1465; + const int INSSTDEV_OEM7_MSGID = 2051; + const int PSRDOP2_OEM7_MSGID = 1163; + const int RXSTATUS_OEM7_MSGID = 93; + const int TIME_OEM7_MSGID = 101; +} + +#endif diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_util.hpp b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_util.hpp new file mode 100644 index 0000000..5bbbccf --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_message_util.hpp @@ -0,0 +1,77 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_MESSAGE_UTIL_HPP__ +#define __OEM7_MESSAGE_UTIL_HPP__ + +#include "novatel_oem7_msgs/Oem7Header.h" +#include "novatel_oem7_msgs/Oem7RawMsg.h" + +#include "oem7_raw_message_if.hpp" +using novatel_oem7::Oem7RawMessageIf; + +#include "oem7_messages.h" + +#include + +namespace novatel_oem7_driver +{ + + void initializeOem7MessageUtil(ros::NodeHandle& nh); + int getOem7MessageId(const std::string& msg_name); + const std::string& getOem7MessageName(int msg_id); + + /** + * Populates Oem7 Binary message header from raw message + * + */ + void getOem7Header( + const Oem7RawMessageIf::ConstPtr& raw_msg, ///< [in] Raw binary message + novatel_oem7_msgs::Oem7Header::Type& hdr ///< [out] Oem7 Message Header + ); + + /** + * Populates Oem7 Binary message header from 'short' raw message + * + */ + void getOem7ShortHeader( + const Oem7RawMessageIf::ConstPtr& raw_msg, ///< [in] Raw binary message + novatel_oem7_msgs::Oem7Header::Type& hdr ///< [out] Oem7 Message Header + ); + + size_t Get_INSCONFIG_NumTranslations(const INSCONFIG_FixedMem* insconfig); + + const INSCONFIG_TranslationMem* Get_INSCONFIG_Translation(const INSCONFIG_FixedMem* insconfig, size_t idx); + + size_t Get_INSCONFIG_NumRotations(const INSCONFIG_FixedMem* insconfig); + + const INSCONFIG_RotationMem* Get_INSCONFIG_Rotation(const INSCONFIG_FixedMem* insconfig, size_t idx); + + + size_t Get_PSRDOP2_NumSystems(const PSRDOP2_FixedMem* psrdop2); + const PSRDOP2_SystemMem* Get_PSRDOP2_System(const PSRDOP2_FixedMem* psrdop2, size_t idx); +} + + +#endif diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_messages.h b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_messages.h new file mode 100644 index 0000000..5cd3068 --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_messages.h @@ -0,0 +1,417 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +/*! \file + * Binary format definitions for Oem7 messages. + * In the future, this file will be autogenerated. + * All Oem7 messages are 4-byte aligned, allowing simple casting into structs. + * Consult Oem7 manual for details. + */ + +#ifndef __OEM7_MESSAGES_H_ +#define __OEM7_MESSAGES_H_ + +#include + + +#define ASSERT_MSG "Consult Oem7 manual" + +namespace novatel_oem7_driver +{ + typedef uint32_t oem7_enum_t; + typedef uint32_t oem7_bool_t; + typedef uint8_t oem7_hex_t; + typedef char oem7_char_t; + + static_assert(sizeof(oem7_char_t) == 1, ASSERT_MSG); + static_assert(sizeof(double) == 8, ASSERT_MSG); + static_assert(sizeof(float) == 4, ASSERT_MSG); + + struct __attribute__((packed)) + Oem7MessageCommonHeaderMem + { + char sync1; + char sync2; + char sync3; + + uint8_t message_length; + uint16_t message_id; + }; + + struct __attribute__((packed)) + Oem7MessageHeaderMem + { + char sync1; + char sync2; + char sync3; + + uint8_t header_length; + uint16_t message_id; + char message_type; + uint8_t port_address; + uint16_t message_length; + uint16_t sequence; + uint8_t idle_time; + uint8_t time_status; + uint16_t gps_week; + int32_t gps_milliseconds; + uint32_t recevier_status; + uint16_t reserved; + uint16_t recevier_version; + }; + + struct __attribute__((packed)) + Oem7MessgeShortHeaderMem + { + char sync1; + char sync2; + char sync3; + + uint8_t message_length; + uint16_t message_id; + uint16_t gps_week; + int32_t gps_milliseconds; + }; + + + struct __attribute__((packed)) + BESTPOSMem + { + oem7_enum_t sol_stat; + oem7_enum_t pos_type; + double lat; + double lon; + double hgt; + float undulation; + oem7_enum_t datum_id; + float lat_stdev; + float lon_stdev; + float hgt_stdev; + oem7_char_t stn_id[4]; + float diff_age; + float sol_age; + uint8_t num_svs; + uint8_t num_sol_svs; + uint8_t num_sol_l1_svs; + uint8_t num_sol_multi_svs; + oem7_hex_t reserved; + oem7_hex_t ext_sol_stat; + uint8_t galileo_beidou_sig_mask; + uint8_t gps_glonass_sig_mask; + }; + static_assert(sizeof(BESTPOSMem) == 72, ASSERT_MSG); + + + struct __attribute__((packed)) + BESTVELMem + { + uint32_t sol_stat; + uint32_t vel_type; + float latency; + float diff_age; + double hor_speed; + double track_gnd; + double ver_speed; + float reserved; + }; + static_assert(sizeof(BESTVELMem) == 44, ASSERT_MSG); + + + struct __attribute__((packed)) + INSPVASmem + { + uint32_t gnss_week; + double seconds; + double latitude; + double longitude; + double height; + double north_velocity; + double east_velocity; + double up_velocity; + double roll; + double pitch; + double azimuth; + oem7_enum_t status; + }; + static_assert(sizeof(INSPVASmem) == 88, ASSERT_MSG); + + struct __attribute__((packed)) + CORRIMUSMem + { + uint32_t imu_data_count; + double pitch_rate; + double roll_rate; + double yaw_rate; + double lateral_acc; + double longitudinal_acc; + double vertical_acc; + uint32_t reserved1; + uint32_t reserved2; + }; + static_assert(sizeof(CORRIMUSMem) == 60, ASSERT_MSG); + + struct __attribute__((packed)) + IMURATECORRIMUSMem + { + uint32_t week; + double seconds; + double pitch_rate; + double roll_rate; + double yaw_rate; + double lateral_acc; + double longitudinal_acc; + double vertical_acc; + }; + static_assert(sizeof(IMURATECORRIMUSMem) == 60, ASSERT_MSG); + + struct __attribute__((packed)) + INSSTDEVMem + { + float latitude_stdev; + float longitude_stdev; + float height_stdev; + float north_velocity_stdev; + float east_velocity_stdev; + float up_velocity_stdev; + float roll_stdev; + float pitch_stdev; + float azimuth_stdev; + uint32_t ext_sol_status; + uint16_t time_since_last_update; + uint16_t reserved1; + uint32_t reserved2; + uint32_t reserved3; + }; + static_assert(sizeof(INSSTDEVMem) == 52, ASSERT_MSG); + + + struct __attribute__((packed)) + INSCONFIG_FixedMem + { + oem7_enum_t imu_type; + uint8_t mapping; + uint8_t initial_alignment_velocity; + uint16_t heave_window; + oem7_enum_t profile; + oem7_hex_t enabled_updates[4]; + oem7_enum_t alignment_mode; + oem7_enum_t relative_ins_output_frame; + oem7_bool_t relative_ins_output_direction; + oem7_hex_t ins_receiver_status[4]; + uint8_t ins_seed_enabled; + uint8_t ins_seed_validation; + uint16_t reserved_1; + uint32_t reserved_2; + uint32_t reserved_3; + uint32_t reserved_4; + uint32_t reserved_5; + uint32_t reserved_6; + uint32_t reserved_7; + }; + static_assert(sizeof(INSCONFIG_FixedMem) == 60, ASSERT_MSG); + + struct __attribute__((packed)) + INSCONFIG_TranslationMem + { + uint32_t translation; + uint32_t frame; + float x_offset; + float y_offset; + float z_offset; + float x_uncertainty; + float y_uncertainty; + float z_uncertainty; + uint32_t translation_source; + }; + + + struct __attribute__((packed)) + INSCONFIG_RotationMem + { + uint32_t rotation; + uint32_t frame; + float x_rotation; + float y_rotation; + float z_rotation; + float x_rotation_stdev; + float y_rotation_stdev; + float z_rotation_stdev; + uint32_t rotation_source; + }; + + + struct __attribute__((packed)) + INSPVAXMem + { + oem7_enum_t ins_status; + oem7_enum_t pos_type; + double latitude; + double longitude; + double height; + float undulation; + double north_velocity; + double east_velocity; + double up_velocity; + double roll; + double pitch; + double azimuth; + float latitude_stdev; + float longitude_stdev; + float height_stdev; + float north_velocity_stdev; + float east_velocity_stdev; + float up_velocity_stdev; + float roll_stdev; + float pitch_stdev; + float azimuth_stdev; + uint32_t extended_status; + uint16_t time_since_update; + }; + static_assert(sizeof(INSPVAXMem) == 126, ASSERT_MSG); + + struct __attribute__((packed)) + HEADING2Mem + { + oem7_enum_t sol_status; + oem7_enum_t pos_type; + float length; + float heading; + float pitch; + float reserved; + float heading_stdev; + float pitch_stdev; + oem7_char_t rover_stn_id[4]; + oem7_char_t master_stn_id[4]; + uint8_t num_sv_tracked; + uint8_t num_sv_in_sol; + uint8_t num_sv_obs; + uint8_t num_sv_multi; + uint8_t sol_source; + uint8_t ext_sol_status; + uint8_t galileo_beidou_sig_mask; + uint8_t gps_glonass_sig_mask; + }; + static_assert(sizeof(HEADING2Mem) == 48, ASSERT_MSG); + + struct __attribute__((packed)) + BESTUTMMem + { + oem7_enum_t sol_stat; + oem7_enum_t pos_type; + uint32_t lon_zone_number; + uint32_t lat_zone_letter; + double northing; + double easting; + double height; + float undulation; + uint32_t datum_id; + float northing_stddev; + float easting_stddev; + float height_stddev; + char stn_id[4]; + float diff_age; + float sol_age; + uint8_t num_svs; + uint8_t num_sol_svs; + uint8_t num_sol_ggl1_svs; + uint8_t num_sol_multi_svs; + uint8_t reserved; + uint8_t ext_sol_stat; + uint8_t galileo_beidou_sig_mask; + uint8_t gps_glonass_sig_mask; + }; + static_assert(sizeof(BESTUTMMem) == 80, ASSERT_MSG); + + struct __attribute__((packed)) + RXSTATUSMem + { + uint32_t error; + uint32_t num_status_codes; + uint32_t rxstat; + uint32_t rxstat_pri_mask; + uint32_t rxstat_set_mask; + uint32_t rxstat_clr_mask; + uint32_t aux1_stat; + uint32_t aux1_stat_pri; + uint32_t aux1_stat_set; + uint32_t aux1_stat_clr; + uint32_t aux2_stat; + uint32_t aux2_stat_pri; + uint32_t aux2_stat_set; + uint32_t aux2_stat_clr; + uint32_t aux3_stat; + uint32_t aux3_stat_pri; + uint32_t aux3_stat_set; + uint32_t aux3_stat_clr; + uint32_t aux4_stat; + uint32_t aux4_stat_pri; + uint32_t aux4_stat_set; + uint32_t aux4_stat_clr; + }; + static_assert(sizeof(RXSTATUSMem) == 88, ASSERT_MSG); + + + struct __attribute__((packed)) + TIMEMem + { + uint32_t clock_status; + double offset; + double offset_std; + double utc_offset; + uint32_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_min; + uint32_t utc_msec; + uint32_t utc_status; + }; + static_assert(sizeof(TIMEMem) == 44, ASSERT_MSG); + + + struct __attribute__((packed)) + PSRDOP2_FixedMem + { + float gdop; + float pdop; + float hdop; + float vdop; + }; + static_assert(sizeof(PSRDOP2_FixedMem) == 16, ASSERT_MSG); + + struct __attribute__((packed)) + PSRDOP2_SystemMem + { + uint32_t system; + float tdop; + }; + + + const std::size_t OEM7_BINARY_MSG_HDR_LEN = sizeof(Oem7MessageHeaderMem); + const std::size_t OEM7_BINARY_MSG_SHORT_HDR_LEN = sizeof(Oem7MessgeShortHeaderMem); + + + +} +#endif diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_receiver_if.hpp b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_receiver_if.hpp new file mode 100644 index 0000000..17f63cd --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_receiver_if.hpp @@ -0,0 +1,50 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_RECEIVER_INTERFACE_HPP__ +#define __OEM7_RECEIVER_INTERFACE_HPP__ + + +#include +#include +#include + + + + +namespace novatel_oem7_driver +{ + class Oem7ReceiverIf + { + public: + virtual ~Oem7ReceiverIf(){}; + virtual bool initialize(ros::NodeHandle&) = 0; + + virtual bool read( boost::asio::mutable_buffer, size_t&) = 0; + virtual bool write(boost::asio::const_buffer ) = 0; + }; +} + + +#endif diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_ros_messages.hpp b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_ros_messages.hpp new file mode 100644 index 0000000..7f2ff9a --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/oem7_ros_messages.hpp @@ -0,0 +1,53 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_ROS_MESSAGES_HPP__ +#define __OEM7_ROS_MESSAGES_HPP__ + +#include + +#include "oem7_raw_message_if.hpp" +using novatel_oem7::Oem7RawMessageIf; + +#include "novatel_oem7_driver/ros_messages.hpp" +#include "novatel_oem7_driver/oem7_message_ids.h" + +namespace novatel_oem7_driver +{ + + template + void + MakeROSMessage(const Oem7RawMessageIf::ConstPtr& msg, boost::shared_ptr& rosmsg); + + void + GetDOPFromPSRDOP2( + const Oem7RawMessageIf::ConstPtr& msg, + uint32_t system_to_use, + double& gdop, + double& pdop, + double& hdop, + double& vdop, + double& tdop); +} +#endif diff --git a/src/novatel_oem7_driver/include/novatel_oem7_driver/ros_messages.hpp b/src/novatel_oem7_driver/include/novatel_oem7_driver/ros_messages.hpp new file mode 100644 index 0000000..5a4eb69 --- /dev/null +++ b/src/novatel_oem7_driver/include/novatel_oem7_driver/ros_messages.hpp @@ -0,0 +1,47 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __ROS_MESSAGES_HPP__ +#define __ROS_MESSAGES_HPP__ + +#include + + +namespace novatel_oem7_driver +{ + + uint32_t GetNextMsgSequenceNumber(); + + template + void + SetROSHeader( + const std::string& frame_id, + boost::shared_ptr& msg) + { + msg->header.frame_id = frame_id; + msg->header.stamp = ros::Time::now(); + msg->header.seq = GetNextMsgSequenceNumber(); + } +} +#endif diff --git a/src/novatel_oem7_driver/launch/oem7_gps_file.launch b/src/novatel_oem7_driver/launch/oem7_gps_file.launch new file mode 100644 index 0000000..be27f0f --- /dev/null +++ b/src/novatel_oem7_driver/launch/oem7_gps_file.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/src/novatel_oem7_driver/launch/oem7_net.launch b/src/novatel_oem7_driver/launch/oem7_net.launch new file mode 100644 index 0000000..dbd8978 --- /dev/null +++ b/src/novatel_oem7_driver/launch/oem7_net.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/novatel_oem7_driver/launch/oem7_tty.launch b/src/novatel_oem7_driver/launch/oem7_tty.launch new file mode 100644 index 0000000..2ec27d2 --- /dev/null +++ b/src/novatel_oem7_driver/launch/oem7_tty.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/novatel_oem7_driver/novatel_oem7_driver_nodelets.xml b/src/novatel_oem7_driver/novatel_oem7_driver_nodelets.xml new file mode 100644 index 0000000..f08c4da --- /dev/null +++ b/src/novatel_oem7_driver/novatel_oem7_driver_nodelets.xml @@ -0,0 +1,25 @@ + + + + + Oem7 Log Nodelet. + + + + + + Oem7 Message Nodelet. + + + + + + Oem7 Config Nodelet. + + + + + + + + diff --git a/src/novatel_oem7_driver/novatel_oem7_driver_plugins.xml b/src/novatel_oem7_driver/novatel_oem7_driver_plugins.xml new file mode 100644 index 0000000..4716acf --- /dev/null +++ b/src/novatel_oem7_driver/novatel_oem7_driver_plugins.xml @@ -0,0 +1,66 @@ + + + + + Oem7 Receiver TCP interface. + + + + + + Oem7 Receiver UDP interface. + + + + + + GPS file produced by Oem7 receiver. + + + + + + Oem7 Receiver serial port interface + + + + + + + + Standard Oem7 Message Decoder + + + + + + Standard BESTPOS Handler. + + + + + + Standard INS/IMU Handler. + + + + + + ALIGN Handler. + + + + + + Time-related messages. + + + + + + Receiver Status (RXSTATUS) messages. + + + + + diff --git a/src/novatel_oem7_driver/package.xml b/src/novatel_oem7_driver/package.xml new file mode 100644 index 0000000..569483b --- /dev/null +++ b/src/novatel_oem7_driver/package.xml @@ -0,0 +1,25 @@ + + + novatel_oem7_driver + 0.1.5 + NovAtel Oem7 ROS Driver + + NovAtel Support + + MIT + + http://www.novatel.com + + catkin + nodelet + roscpp + nodelet + roscpp + nodelet + roscpp + + + + + + diff --git a/src/novatel_oem7_driver/src/align_handler.cpp b/src/novatel_oem7_driver/src/align_handler.cpp new file mode 100644 index 0000000..b3e4260 --- /dev/null +++ b/src/novatel_oem7_driver/src/align_handler.cpp @@ -0,0 +1,85 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + + +#include + + +#include + +#include +#include + +#include + + +namespace novatel_oem7_driver +{ + + /*** + * Handler of ALIGH-related messages + */ + class ALIGNHandler: public Oem7MessageHandlerIf + { + Oem7RosPublisher HEADING2_pub_; + + void publishHEADING2( + Oem7RawMessageIf::ConstPtr msg) + { + boost::shared_ptr heading2; + MakeROSMessage(msg, heading2); + HEADING2_pub_.publish(heading2); + } + + public: + ALIGNHandler() + { + } + + ~ALIGNHandler() + { + } + + void initialize(ros::NodeHandle& nh) + { + HEADING2_pub_.setup("HEADING2", nh); + } + + const std::vector& getMessageIds() + { + static const std::vector MSG_IDS({HEADING2_OEM7_MSGID}); + return MSG_IDS; + } + + void handleMsg(Oem7RawMessageIf::ConstPtr msg) + { + ROS_DEBUG_STREAM("ALIGN < [id= " << msg->getMessageId() << "]"); + + publishHEADING2(msg); + } + }; +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::ALIGNHandler, novatel_oem7_driver::Oem7MessageHandlerIf) diff --git a/src/novatel_oem7_driver/src/bestpos_handler.cpp b/src/novatel_oem7_driver/src/bestpos_handler.cpp new file mode 100644 index 0000000..7bb9e06 --- /dev/null +++ b/src/novatel_oem7_driver/src/bestpos_handler.cpp @@ -0,0 +1,428 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include + + +#include +#include + +#include "novatel_oem7_msgs/SolutionStatus.h" +#include "novatel_oem7_msgs/PositionOrVelocityType.h" +#include "novatel_oem7_msgs/BESTPOS.h" +#include "novatel_oem7_msgs/BESTUTM.h" +#include "novatel_oem7_msgs/BESTVEL.h" +#include "novatel_oem7_msgs/INSPVA.h" + +#include "gps_common/GPSFix.h" +#include "sensor_msgs/NavSatFix.h" + +#include +#include + + +namespace novatel_oem7_driver +{ + + + /*** + * Derive ROS GPS Status from Oem7 BESTPOS + * + * @return ROS status. + */ + int16_t ToROSGPSStatus(const novatel_oem7_msgs::BESTPOS::Ptr bestpos) + { + if(bestpos->pos_type.type == novatel_oem7_msgs::PositionOrVelocityType::NONE) + return gps_common::GPSStatus::STATUS_NO_FIX; + + return gps_common::GPSStatus::STATUS_FIX; + } + + /*** + * Convert GPS time to seconds + * + * @return seconds + */ + double MakeGpsTime_Seconds(uint16_t gps_week, uint32_t gps_milliseconds) + { + static const double SECONDS_IN_GPS_WEEK = 604800.0; + static const double MILLISECONDS_IN_SECOND = 1000.0; + + return gps_week * SECONDS_IN_GPS_WEEK + + gps_milliseconds / MILLISECONDS_IN_SECOND; + } + + /*** + * Converts covariance form GPSFix to NavSatFix + * @return NavSatFix covariance + */ + uint8_t GpsFixCovTypeToNavSatFixCovType(uint8_t covariance_type) + { + switch(covariance_type) + { + case gps_common::GPSFix::COVARIANCE_TYPE_APPROXIMATED: + return sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED; + + case gps_common::GPSFix::COVARIANCE_TYPE_DIAGONAL_KNOWN: + return sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + case gps_common::GPSFix::COVARIANCE_TYPE_KNOWN: + return sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN; + + case gps_common::GPSFix::COVARIANCE_TYPE_UNKNOWN: + return sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + + default: + ROS_ERROR_STREAM("Unknown GPSFix covariance type: " << covariance_type); + return sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + } + } + + + /*** Generates NavSatFix object from GpsFix + */ + void GpsFixToNavSatFix(const gps_common::GPSFix::Ptr gpsfix, sensor_msgs::NavSatFix::Ptr navsatfix) + { + navsatfix->latitude = gpsfix->latitude; + navsatfix->longitude = gpsfix->longitude; + navsatfix->altitude = gpsfix->altitude; + + navsatfix->position_covariance[0] = gpsfix->position_covariance[0]; + navsatfix->position_covariance[4] = gpsfix->position_covariance[4]; + navsatfix->position_covariance[8] = gpsfix->position_covariance[8]; + + navsatfix->position_covariance_type = GpsFixCovTypeToNavSatFixCovType(gpsfix->position_covariance_type); + } + + /*** + * Handler of position-related messages. Synthesizes ROS messagse GPSFix and NavSatFix from native Oem7 Messages. + */ + class BESTPOSHandler: public Oem7MessageHandlerIf + { + Oem7RosPublisher BESTPOS_pub_; + Oem7RosPublisher BESTUTM_pub_; + Oem7RosPublisher BESTVEL_pub_; + Oem7RosPublisher INSPVA_pub_; + + Oem7RosPublisher GPSFix_pub_; + Oem7RosPublisher NavSatFix_pub_; + + boost::shared_ptr bestpos_; + boost::shared_ptr bestvel_; + boost::shared_ptr inspva_; + + boost::shared_ptr gpsfix_; + + Oem7RawMessageIf::ConstPtr psrdop2_; + + int64_t last_bestpos_; + int64_t last_bestvel_; + int64_t last_inspva_; + + int32_t bestpos_period_; + int32_t bestvel_period_; + int32_t inspva_period_; + + + /*** + * @return true if the specified period is the shortest in all messages. + */ + bool isShortestPeriod(int32_t period) + { + return period <= bestpos_period_ && + period <= bestvel_period_ && + period <= inspva_period_; + } + + /*** + * Updates message period + */ + template + void updatePeriod( + const T& msg, + int64_t& last_msg_msec, + int32_t& msg_period) + { + int64_t cur_msg_msec = GPSTimeToMsec(msg->nov_header); + if(last_msg_msec > 0) + { + int32_t period = cur_msg_msec - last_msg_msec; + if(period >= 0) + { + msg_period = period; + } + else // Could be input corruption; do not update anything. + { + ROS_ERROR_STREAM("updatePeriod: msg= " << msg->nov_header.message_id << "; per= " << period << "; ignored."); + } + } + + last_msg_msec = cur_msg_msec; + } + + void publishBESTPOS(Oem7RawMessageIf::ConstPtr msg) + { + MakeROSMessage(msg, bestpos_); + updatePeriod(bestpos_, last_bestpos_, bestpos_period_); + + + BESTPOS_pub_.publish(bestpos_); + } + + void publishBESTVEL(Oem7RawMessageIf::ConstPtr msg) + { + MakeROSMessage(msg, bestvel_); + updatePeriod(bestvel_, last_bestvel_, bestvel_period_); + BESTVEL_pub_.publish(bestvel_); + } + + void publishBESTUTM(Oem7RawMessageIf::ConstPtr msg) + { + boost::shared_ptr bestutm; + MakeROSMessage(msg, bestutm); + BESTUTM_pub_.publish(bestutm); + } + + void publishINSVPA(Oem7RawMessageIf::ConstPtr msg) + { + MakeROSMessage(msg, inspva_); + updatePeriod(inspva_, last_inspva_, inspva_period_); + + INSPVA_pub_.publish(inspva_); + } + + void publishGPSFix() + { + gpsfix_.reset(new gps_common::GPSFix); + + gpsfix_->status.position_source = gps_common::GPSStatus::SOURCE_NONE; + gpsfix_->status.orientation_source = gps_common::GPSStatus::SOURCE_NONE; + gpsfix_->status.motion_source = gps_common::GPSStatus::SOURCE_NONE; + + // When INS is available, BESTPOS is the best of GNSS + INS, so it is always preferred. + if(bestpos_) + { + gpsfix_->latitude = bestpos_->lat; + gpsfix_->longitude = bestpos_->lon; + gpsfix_->altitude = bestpos_->hgt; + + // Convert stdev to diagonal covariance + gpsfix_->position_covariance[0] = std::pow(bestpos_->lon_stdev, 2); + gpsfix_->position_covariance[4] = std::pow(bestpos_->lat_stdev, 2); + gpsfix_->position_covariance[8] = std::pow(bestpos_->hgt_stdev, 2); + gpsfix_->position_covariance_type = gps_common::GPSFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + gpsfix_->time = MakeGpsTime_Seconds( + bestpos_->nov_header.gps_week_number, + bestpos_->nov_header.gps_week_milliseconds); + + + gpsfix_->status.satellites_used = bestpos_->num_sol_svs; + gpsfix_->status.status = ToROSGPSStatus(bestpos_); + + gpsfix_->status.position_source = gps_common::GPSStatus::SOURCE_GPS; + } + + if(bestvel_) + { + gpsfix_->track = bestvel_->trk_gnd; + gpsfix_->speed = bestvel_->hor_speed; + gpsfix_->climb = bestvel_->ver_speed; + + if(gpsfix_->time == 0.0) // Not populated yet + { + gpsfix_->time = MakeGpsTime_Seconds( + bestvel_->nov_header.gps_week_number, + bestvel_->nov_header.gps_week_milliseconds); + } + + if(bestvel_->vel_type.type == novatel_oem7_msgs::PositionOrVelocityType::DOPPLER_VELOCITY) + { + gpsfix_->status.motion_source != gps_common::GPSStatus::SOURCE_DOPPLER; + } + else + { + gpsfix_->status.motion_source |= gps_common::GPSStatus::SOURCE_POINTS; + } + } + + + if(inspva_ ) + { + if(!bestpos_) // BESTPOS is not available + { + gpsfix_->latitude = inspva_->latitude; + gpsfix_->longitude = inspva_->longitude; + gpsfix_->altitude = inspva_->height; + + // TODO: compute variance form INSSTDEV instead? + gpsfix_->position_covariance_type = gps_common::GPSFix::COVARIANCE_TYPE_UNKNOWN; + } + + gpsfix_->pitch = inspva_->pitch; + gpsfix_->roll = inspva_->roll; + //gpsfix->dip: not populated. + + gpsfix_->status.position_source |= (gps_common::GPSStatus::SOURCE_GYRO | gps_common::GPSStatus::SOURCE_ACCEL); + gpsfix_->status.orientation_source = (gps_common::GPSStatus::SOURCE_GYRO | gps_common::GPSStatus::SOURCE_ACCEL); + gpsfix_->status.motion_source = (gps_common::GPSStatus::SOURCE_GYRO | gps_common::GPSStatus::SOURCE_ACCEL); + + if(gpsfix_->time == 0.0) // Not populated yet + { + gpsfix_->time = MakeGpsTime_Seconds( + inspva_->nov_header.gps_week_number, + inspva_->nov_header.gps_week_milliseconds); + } + } + + if(psrdop2_) + { + GetDOPFromPSRDOP2( + psrdop2_, + 0, // GPS + gpsfix_->gdop, + gpsfix_->pdop, + gpsfix_->hdop, + gpsfix_->vdop, + gpsfix_->tdop); + } + + GPSFix_pub_.publish(gpsfix_); + } + + void publishNavSatFix() + { + if(!gpsfix_) + { + return; + } + + boost::shared_ptr navsatfix(new sensor_msgs::NavSatFix); + + // Derive from GPSFix. + GpsFixToNavSatFix(gpsfix_, navsatfix); + + NavSatFix_pub_.publish(navsatfix); + } + + void publishROSMessages() + { + publishGPSFix(); // Must be published first, since other message may be derived from it. + publishNavSatFix(); + } + + + + public: + BESTPOSHandler(): + last_bestpos_(0), + last_bestvel_(0), + last_inspva_(0), + bestpos_period_(INT_MAX), + bestvel_period_(INT_MAX), + inspva_period_( INT_MAX) + { + } + + ~BESTPOSHandler() + { + } + + void initialize(ros::NodeHandle& nh) + { + BESTPOS_pub_.setup("BESTPOS", nh); + BESTVEL_pub_.setup("BESTVEL", nh); + BESTUTM_pub_.setup("BESTUTM", nh); + INSPVA_pub_.setup( "INSPVA", nh); + GPSFix_pub_.setup( "GPSFix", nh); + NavSatFix_pub_.setup( "NavSatFix", nh); + } + + const std::vector& getMessageIds() + { + static const std::vector MSG_IDS({BESTPOS_OEM7_MSGID, BESTVEL_OEM7_MSGID, BESTUTM_OEM7_MSGID, INSPVAS_OEM7_MSGID, PSRDOP2_OEM7_MSGID}); + return MSG_IDS; + } + + void handleMsg(Oem7RawMessageIf::ConstPtr msg) + { + ROS_DEBUG_STREAM("BESTPOS < [id=" << msg->getMessageId() << "] periods (BP BV PVA):" << + bestpos_period_ << " " << + bestvel_period_ << " " << + inspva_period_); + + // It is assumed all the messages are logged at reasonable rates. + // BESTPOS and BESTVEL are always logged together. + // On units with IMU, INSPVA would trigger publishing of ROS messages. + // On non-IMU units, BESTVEL be. + + if(msg->getMessageId() == BESTPOS_OEM7_MSGID) + { + publishBESTPOS(msg); + + if(isShortestPeriod(bestpos_period_)) + { + publishROSMessages(); + } + } + + if(msg->getMessageId() == BESTVEL_OEM7_MSGID) + { + publishBESTVEL(msg); + + if(isShortestPeriod(bestvel_period_)) + { + publishROSMessages(); + } + } + + if(msg->getMessageId() == BESTUTM_OEM7_MSGID) + { + publishBESTUTM(msg); + } + + if(msg->getMessageId() == INSPVAS_OEM7_MSGID) + { + publishINSVPA(msg); + + if(isShortestPeriod(inspva_period_)) + { + publishROSMessages(); + } + } + + if(msg->getMessageId() == PSRDOP2_OEM7_MSGID) + { + psrdop2_ = msg; + } + } + }; + +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::BESTPOSHandler, novatel_oem7_driver::Oem7MessageHandlerIf) diff --git a/src/novatel_oem7_driver/src/ins_handler.cpp b/src/novatel_oem7_driver/src/ins_handler.cpp new file mode 100644 index 0000000..19c6d2e --- /dev/null +++ b/src/novatel_oem7_driver/src/ins_handler.cpp @@ -0,0 +1,296 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include + +#include + + +#include + +#include + +#include "sensor_msgs/Imu.h" +#include "novatel_oem7_msgs/CORRIMU.h" +#include "novatel_oem7_msgs/IMURATECORRIMU.h" +#include "novatel_oem7_msgs/INSSTDEV.h" +#include "novatel_oem7_msgs/INSCONFIG.h" +#include "novatel_oem7_msgs/INSPVA.h" +#include "novatel_oem7_msgs/INSPVAX.h" + +#include +#include + +#include +#include + +namespace +{ + typedef unsigned int imu_type_t; ///< Type of IMU used + typedef int imu_rate_t; ///< IMU message rate + + const imu_type_t IMU_TYPE_UNKNOWN = 0; +} + + + +namespace novatel_oem7_driver +{ + /*** + * Converts degrees to Radians + * + * @return radians + */ + inline double degreesToRadians(double degrees) + { + return degrees * M_PI / 180.0; + } + + const double DATA_NOT_AVAILABLE = -1.0; ///< Used to initialized unpopulated fields. + + class INSHandler: public Oem7MessageHandlerIf + { + ros::NodeHandle nh_; + + Oem7RosPublisher imu_pub_; + Oem7RosPublisher corrimu_pub_; + Oem7RosPublisher insstdev_pub_; + Oem7RosPublisher inspvax_pub_; + Oem7RosPublisher insconfig_pub_; + + boost::shared_ptr inspva_; + boost::shared_ptr corrimu_; + boost::shared_ptr insstdev_; + + int imu_rate_; + std::string frame_id_; + + typedef std::map imu_config_map_t; + imu_config_map_t imu_config_map; + + + void getImuParam(imu_type_t imu_type, const std::string& name, std::string& param) + { + std::string ns = ros::this_node::getNamespace(); + std::string param_name = ns + "/supported_imus/" + std::to_string(imu_type) + "/" + name; + if(!nh_.getParam(param_name, param)) + { + ROS_FATAL_STREAM("INS: IMU type= " << imu_type << " is not supported."); + } + } + + int getImuRate(imu_type_t imu_type) + { + std::string rate; + getImuParam(imu_type, "rate", rate); + + return std::stoi(rate); + } + + void getImuDescription(imu_type_t imu_type, std::string& desc) + { + getImuParam(imu_type, "name", desc); + } + + + void processInsConfigMsg(Oem7RawMessageIf::ConstPtr msg) + { + boost::shared_ptr insconfig; + MakeROSMessage(msg, insconfig); + insconfig_pub_.publish(insconfig); + + if(imu_rate_ == 0) + { + std::string imu_desc; + getImuDescription(insconfig->imu_type, imu_desc); + + imu_rate_ = getImuRate(insconfig->imu_type); + + ROS_LOG_STREAM(imu_rate_ == 0 ? ::ros::console::levels::Error : + ::ros::console::levels::Info, + ROSCONSOLE_DEFAULT_NAME, + "IMU: '" << imu_desc << "', rate= " << imu_rate_); + } + } + + void publishInsPVAXMsg(Oem7RawMessageIf::ConstPtr msg) + { + boost::shared_ptr inspvax; + MakeROSMessage(msg, inspvax); + + inspvax_pub_.publish(inspvax); + } + + void publishCorrImuMsg(Oem7RawMessageIf::ConstPtr msg) + { + MakeROSMessage(msg, corrimu_); + corrimu_pub_.publish(corrimu_); + } + + + void publishImuMsg() + { + if(!imu_pub_.isEnabled()) + { + return; + } + + boost::shared_ptr imu(new sensor_msgs::Imu); + + if(inspva_) + { + imu->orientation = tf::createQuaternionMsgFromRollPitchYaw( + degreesToRadians(inspva_->roll), + -degreesToRadians(inspva_->pitch), + -degreesToRadians(inspva_->azimuth)); + } + else + { + ROS_WARN_THROTTLE(10, "/IMU: PVA not available; 'IMU' not published."); + return; + } + + if(insstdev_) + { + imu->orientation_covariance[0] = std::pow(insstdev_->pitch_stdev, 2); + imu->orientation_covariance[4] = std::pow(insstdev_->roll_stdev, 2); + imu->orientation_covariance[8] = std::pow(insstdev_->azimuth_stdev, 2); + } + + if(corrimu_ && imu_rate_ > 0) + { + imu->angular_velocity.x = corrimu_->pitch_rate * imu_rate_; + imu->angular_velocity.y = corrimu_->roll_rate * imu_rate_; + imu->angular_velocity.z = corrimu_->yaw_rate * imu_rate_; + + imu->linear_acceleration.x = corrimu_->lateral_acc * imu_rate_; + imu->linear_acceleration.y = corrimu_->longitudinal_acc * imu_rate_; + imu->linear_acceleration.z = corrimu_->vertical_acc * imu_rate_; + + + imu->angular_velocity_covariance[0] = 1e-3; + imu->angular_velocity_covariance[4] = 1e-3; + imu->angular_velocity_covariance[8] = 1e-3; + + + imu->linear_acceleration_covariance[0] = 1e-3; + imu->linear_acceleration_covariance[4] = 1e-3; + imu->linear_acceleration_covariance[8] = 1e-3; + } + else + { + imu->angular_velocity_covariance[0] = DATA_NOT_AVAILABLE; + imu->linear_acceleration_covariance[0] = DATA_NOT_AVAILABLE; + } + + imu_pub_.publish(imu); + } + + void publishInsStDevMsg(Oem7RawMessageIf::ConstPtr msg) + { + MakeROSMessage(msg, insstdev_); + insstdev_pub_.publish(insstdev_); + } + + + public: + INSHandler(): + imu_rate_(0) + { + } + + ~INSHandler() + { + } + + void initialize(ros::NodeHandle& nh) + { + nh_ = nh; + + imu_pub_.setup( "IMU", nh); + corrimu_pub_.setup< novatel_oem7_msgs::CORRIMU>( "CORRIMU", nh); + insstdev_pub_.setup< novatel_oem7_msgs::INSSTDEV>( "INSSTDEV", nh); + inspvax_pub_.setup< novatel_oem7_msgs::INSPVAX>( "INSPVAX", nh); + insconfig_pub_.setup("INSCONFIG", nh); + + nh.getParam("imu_rate", imu_rate_); // User rate override + if(imu_rate_ > 0) + { + ROS_INFO_STREAM("INS: IMU rate overriden to " << imu_rate_); + } + } + + const std::vector& getMessageIds() + { + static const std::vector MSG_IDS( + { + CORRIMUS_OEM7_MSGID, + IMURATECORRIMU_OEM7_MSGID, + INSPVAS_OEM7_MSGID, + INSPVAX_OEM7_MSGID, + INSSTDEV_OEM7_MSGID, + INSCONFIG_OEM7_MSGID + } + ); + return MSG_IDS; + } + + void handleMsg(Oem7RawMessageIf::ConstPtr msg) + { + ROS_DEBUG_STREAM("INS < [id= " << msg->getMessageId() << "]"); + + if(msg->getMessageId()== INSPVAS_OEM7_MSGID) + { + MakeROSMessage(msg, inspva_); // Cache + } + else if(msg->getMessageId() == INSSTDEV_OEM7_MSGID) + { + publishInsStDevMsg(msg); + } + else if(msg->getMessageId() == CORRIMUS_OEM7_MSGID || + msg->getMessageId() == IMURATECORRIMU_OEM7_MSGID) + { + publishCorrImuMsg(msg); + + publishImuMsg(); + } + else if(msg->getMessageId() == INSCONFIG_OEM7_MSGID) + { + processInsConfigMsg(msg); + } + else if(msg->getMessageId() == INSPVAX_OEM7_MSGID) + { + publishInsPVAXMsg(msg); + } + else + { + assert(false); + } + } + }; + +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::INSHandler, novatel_oem7_driver::Oem7MessageHandlerIf) diff --git a/src/novatel_oem7_driver/src/message_handler.cpp b/src/novatel_oem7_driver/src/message_handler.cpp new file mode 100644 index 0000000..0924117 --- /dev/null +++ b/src/novatel_oem7_driver/src/message_handler.cpp @@ -0,0 +1,81 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include "message_handler.hpp" + +#include +#include + + + +namespace novatel_oem7_driver +{ + /** + * Constructs the handler by loading the plugins based on user-defined parameters + */ + MessageHandler::MessageHandler(ros::NodeHandle& nh): + msg_handler_loader_("novatel_oem7_driver", "novatel_oem7_driver::Oem7MessageHandlerIf") + { + // Load the plugins and create the dispatch table. + std::vector msg_handler_names; + nh.getParam("oem7_msg_handlers", msg_handler_names); + for(const auto& name : msg_handler_names) + { + MessageHandlerShPtr msg_handler = msg_handler_loader_.createInstance(name); + + msg_handler->initialize(nh); + + for(int msg_id: msg_handler->getMessageIds()) + { + MessageHandlerMap::iterator itr = msg_handler_map_.find(msg_id); + if(itr == msg_handler_map_.end()) + { + msg_handler_map_[msg_id].reset(new MsgHandlerList); + } + + msg_handler_map_[msg_id]->push_back(msg_handler); + } + } + } + + /** + * Dispatches raw messages to plugins for decoding. + */ + void MessageHandler::handleMessage(Oem7RawMessageIf::ConstPtr raw_msg) + { + MessageHandlerMap::iterator itr = msg_handler_map_.find(raw_msg->getMessageId()); + if(itr == msg_handler_map_.end()) + { + ROS_DEBUG_STREAM("No handler for message ID= " << raw_msg->getMessageId()); + return; + } + + MessageHandlerListPtr& msg_handler_list = itr->second; + for(auto& h: *msg_handler_list) + { + h->handleMsg(raw_msg); + } + } +} + diff --git a/src/novatel_oem7_driver/src/message_handler.hpp b/src/novatel_oem7_driver/src/message_handler.hpp new file mode 100644 index 0000000..f220981 --- /dev/null +++ b/src/novatel_oem7_driver/src/message_handler.hpp @@ -0,0 +1,64 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __MESSAGE_HANDLER_HPP__ +#define __MESSAGE_HANDLER_HPP__ + +#include + +#include +#include + +#include "oem7_raw_message_if.hpp" +using novatel_oem7::Oem7RawMessageIf; + +#include "novatel_oem7_driver/oem7_message_decoder_if.hpp" +#include "novatel_oem7_driver/oem7_message_handler_if.hpp" + +#include + +namespace novatel_oem7_driver +{ + /** + * Encapsulates a collection of message handling plugins, where a message + * a messages is handled by 0 or more plugins, matching the message on ID. + */ + class MessageHandler + { + pluginlib::ClassLoader msg_handler_loader_; ///< Plugin loader + + typedef boost::shared_ptr MessageHandlerShPtr; + typedef std::list MsgHandlerList; + typedef boost::scoped_ptr MessageHandlerListPtr; + typedef std::map MessageHandlerMap; + MessageHandlerMap msg_handler_map_; ///< Dispatch map for raw messages. + + public: + MessageHandler(ros::NodeHandle& nh); + + void handleMessage(Oem7RawMessageIf::ConstPtr raw_msg); + }; +} + +#endif diff --git a/src/novatel_oem7_driver/src/oem7_config_nodelet.cpp b/src/novatel_oem7_driver/src/oem7_config_nodelet.cpp new file mode 100644 index 0000000..5e7a90f --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_config_nodelet.cpp @@ -0,0 +1,149 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "novatel_oem7_msgs/Oem7AbasciiCmd.h" + +#include + +namespace +{ + + /** + * @return true if the string has the specified prefix + */ + bool isPrefix(const std::string& prefix, const std::string& str) + { + auto const diff_pos = std::mismatch(prefix.begin(), prefix.end(), str.begin()); + return diff_pos.first == prefix.end(); + } + +} + +namespace novatel_oem7_driver +{ + /** + * Nodelet which configures Oem7 receiver. + * Sends Oem7 commands using 'Oem7Cmd' service. The commands are obtained from Global parameters. + * + */ + class Oem7ConfigNodelet : public nodelet::Nodelet + { + ros::Timer serviceCbTimer_; /**< Timer used to execute main service callback. */ + ros::ServiceClient client_; /** Oem7Cmd service */ + + public: + + /** + * Initializes the Config nodelet: Connects to Oem7 Cmd service and launches the configuration. + */ + void onInit() + { + NODELET_INFO_STREAM(getName() << ": Oem7ConfigNodelet v." << novatel_oem7_driver_VERSION << "; " + << __DATE__ << " " << __TIME__); + + serviceCbTimer_ = getNodeHandle().createTimer(ros::Duration(0.0), &Oem7ConfigNodelet::serviceLoopCb, this, true); + + client_ = getNodeHandle().serviceClient("Oem7Cmd"); + } + + /** + * Service loop, obtains and sends the configuration commands sequentially, waiting for a response from a previous command before sending the next one. + */ + void serviceLoopCb(const ros::TimerEvent& event) + { + client_.waitForExistence(); + + std::vector receiver_init_commands; + getNodeHandle().getParam("receiver_init_commands", receiver_init_commands); + for(const auto& cmd : receiver_init_commands) + { + issueConfigCmd(cmd); + } + + NODELET_INFO_STREAM("Oem7 configuration completed."); + client_.shutdown(); + } + + /** + * Executes Driver-specific command, like PAUSE. + * + * @return true when the provided command is a recongized internal command. + */ + bool executeInternalCommand(const std::string& cmd) + { + static const std::string CMD_PAUSE("!PAUSE"); + if(isPrefix(CMD_PAUSE, cmd)) + { + std::stringstream ss(cmd); + std::string token; + ss >> token; // Prefix + ss >> token; // Period + int pause_period_sec = 0; + if(std::stringstream(token) >> pause_period_sec) + { + ROS_INFO_STREAM("Sleeping for " << pause_period_sec << " seconds...."); + ros::Duration(pause_period_sec).sleep(); + ROS_INFO_STREAM("... done sleeping."); + } + else + { + ROS_ERROR_STREAM("Invalid Driver command syntax: '" << cmd << "'"); + } + + return true; + } + else // Not a recognized internal command + { + return false; + } + } + + /** + * Issues Oem7 configuration command + */ + void issueConfigCmd(const std::string& cmd /**< The command to issue */) + { + if(!executeInternalCommand(cmd)) + { + novatel_oem7_msgs::Oem7AbasciiCmd oem7_cmd; + oem7_cmd.request.cmd = cmd; + + if(client_.call(oem7_cmd)) // BLOCKS with no timeout. + { + NODELET_DEBUG_STREAM("Config: '" << cmd << "' : Rsp: '" << oem7_cmd.response.rsp << "'"); + } + else + { + NODELET_ERROR_STREAM("Config '" << cmd << "' not executed."); + } + } + } + }; +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7ConfigNodelet, nodelet::Nodelet); diff --git a/src/novatel_oem7_driver/src/oem7_driver_util.hpp b/src/novatel_oem7_driver/src/oem7_driver_util.hpp new file mode 100644 index 0000000..bf05b7c --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_driver_util.hpp @@ -0,0 +1,48 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include "novatel_oem7_msgs/Oem7Header.h" + + +namespace novatel_oem7_driver +{ + /*** Converts GSP time to Milliseconds + * @return milliseconds + */ + static inline int64_t GPSTimeToMsec(uint32_t gps_week_no, uint32_t week_msec) + { + static const int64_t GPS_MSEC_IN_WEEK = 7 * 24 * 60 * 60 * 1000; + return GPS_MSEC_IN_WEEK * gps_week_no + week_msec; + } + + /*** + * Covers GPSTime obtained from header to milliseconds. + * @return milliseconds + */ + static inline int64_t GPSTimeToMsec(const novatel_oem7_msgs::Oem7Header& hdr) + { + return GPSTimeToMsec(hdr.gps_week_number, hdr.gps_week_milliseconds); + } +} diff --git a/src/novatel_oem7_driver/src/oem7_log_nodelet.cpp b/src/novatel_oem7_driver/src/oem7_log_nodelet.cpp new file mode 100644 index 0000000..a42f7b1 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_log_nodelet.cpp @@ -0,0 +1,118 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include + +#include "novatel_oem7_msgs/Oem7RawMsg.h" +#include "novatel_oem7_driver/oem7_messages.h" + +#include + +namespace novatel_oem7_driver +{ + + /* + * Adapter: novatel_oem7_msgs::Oem7RawMsg and Oem7RawMsgIf + */ + class RawMsgAdapter: public Oem7RawMessageIf + { + public: + const novatel_oem7_msgs::Oem7RawMsg::ConstPtr msg_; + + RawMsgAdapter(const novatel_oem7_msgs::Oem7RawMsg::ConstPtr& msg): + msg_(msg) + { + } + + Oem7MessageType getMessageType() const + { + assert(false); + return Oem7RawMessageIf::OEM7MSGTYPE_UNKNOWN; + } + + Oem7MessageFormat getMessageFormat() const + { + assert(false); + return Oem7RawMessageIf::OEM7MSGFMT_UNKNOWN; + } + + int getMessageId() const + { + const Oem7MessageCommonHeaderMem* mem = + reinterpret_cast(getMessageData(0)); + return mem->message_id; + } + + const uint8_t* getMessageData(size_t offset) const + { + return const_cast(msg_->message_data.data()); // FIXME + } + + size_t getMessageDataLength() const + { + return msg_->message_data.size(); + } + }; + /* + * Nodelet responsible for decoding raw Oem7 messages and generating specific ROS and novatel_oem7_msg messages. + * Subscribes to "oem7_raw_msg", and loads plugins which advertise specific messages. + * Raw oem7 messages are dispatched to plugins for decoding. + */ + class Oem7LogNodelet : public nodelet::Nodelet + { + boost::scoped_ptr msg_handler_; + + ros::Subscriber oem7_raw_msg_sub_; + + + public: + Oem7LogNodelet() + { + } + + void onInit() + { + ros::NodeHandle nh = getNodeHandle(); + ros::NodeHandle priv_nh = getPrivateNodeHandle(); + msg_handler_.reset(new MessageHandler(priv_nh)); + + oem7_raw_msg_sub_ = nh.subscribe("oem7_raw_msg", 100, &Oem7LogNodelet::oem7RawMsgCb, this); + } + + /** + * Dispatches raw messages for handling + */ + void oem7RawMsgCb(const novatel_oem7_msgs::Oem7RawMsg::ConstPtr& msg) + { + boost::shared_ptr raw_msg = boost::make_shared(msg); + msg_handler_->handleMessage(raw_msg); + } + }; +} + + +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7LogNodelet, nodelet::Nodelet); diff --git a/src/novatel_oem7_driver/src/oem7_message_decoder.cpp b/src/novatel_oem7_driver/src/oem7_message_decoder.cpp new file mode 100644 index 0000000..c867269 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_message_decoder.cpp @@ -0,0 +1,123 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include + + +#include +#include "oem7_message_decoder_lib.hpp" + + + + +namespace novatel_oem7_driver +{ + + /*** + * Parser for Oem7 messages. Obtains serial input from Oem7RawMessageParserIf; makes callbacks on Oem7RawMessageParserUserIf when Oem7 messages. + */ + class Oem7MessageDecoder: public Oem7MessageDecoderIf, public novatel_oem7::Oem7MessageDecoderLibUserIf + { + ros::NodeHandle nh_; // ROS Node Handle. + + Oem7MessageDecoderUserIf* user_; //< Parser user callback interface + + Oem7ReceiverIf* recvr_; + + boost::shared_ptr decoder_; //< NovAtel message decoder + + + public: + + /** + * Initializes the decoder + */ + bool initialize( + ros::NodeHandle& nh, + Oem7ReceiverIf* recvr, + Oem7MessageDecoderUserIf* user) + { + nh_ = nh; + user_ = user; + recvr_ = recvr; + + novatel_oem7::version_element_t major, minor, build; + novatel_oem7::GetOem7MessageDecoderLibVersion(major, minor, build); + + ROS_INFO_STREAM("Oem7MessageDecoderLib version: " << major << "." << minor << "." << build); + + decoder_ = novatel_oem7::GetOem7MessageDecoder(this); + + return true; + } + + virtual bool read( boost::asio::mutable_buffer buf, size_t& s) + { + return recvr_->read(buf, s); + } + + + /* + * Parser service loop. + * Drive the parser forward to keep reading from its input stream and making message callbacks into user. + * Blocks until: + * The system is shut down / ros::ok() returns false + * No more input available (as a permanent condition), + */ + void service() + { + try + { + while(!ros::isShuttingDown()) + { + boost::shared_ptr msg; + if(decoder_->readMessage(msg)) + { + if(msg) + { + user_->onNewMessage(msg); + } + // else: No messages available now; keep retrying until we get one or decoder gives up. + } + else + { + ROS_WARN("Decoder: no more messages available."); + break; + } + } + } + catch(std::exception const& ex) + { + ROS_ERROR_STREAM("Decoder exception: " << ex.what()); + } + } + }; + +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7MessageDecoder, novatel_oem7_driver::Oem7MessageDecoderIf) diff --git a/src/novatel_oem7_driver/src/oem7_message_nodelet.cpp b/src/novatel_oem7_driver/src/oem7_message_nodelet.cpp new file mode 100644 index 0000000..17c97f6 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_message_nodelet.cpp @@ -0,0 +1,385 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include + +#include +#include +#include +#include + +#include "novatel_oem7_msgs/Oem7AbasciiCmd.h" +#include "novatel_oem7_msgs/Oem7RawMsg.h" + + +#include + + + +#include +#include + +#include +#include +#include + +#include + + +namespace novatel_oem7_driver +{ + /** + * Nodelet publishing raw oem7 messages and issuing oem7 commands. + * Loads plugins responsible for obtaining byte input from the Oem7 receiver, and decoding it into raw oem7 messages. + * Implements a service allowing Oem7 Abbreviated ASCII commands to be sent to the receiver. + */ + class Oem7MessageNodelet : + public Oem7MessageDecoderUserIf, + public nodelet::Nodelet + { + boost::mutex nodelet_mtx_; ///< Protects nodelet internal state + + double publish_delay_sec_; ///< Delay after publishing each message; used to throttle output with static data sources. + + Oem7RosPublisher oem7rawmsg_pub_; ///< Publishes raw Oem7 messages. + + ros::CallbackQueue timer_queue_; ///< Dedicated queue for command requests. + boost::shared_ptr timer_spinner_; ///< 1 thread servicing the command queue. + + // Command service + boost::interprocess::interprocess_semaphore rsp_sem_; ///< Signaling between Oem7 commands / response handler + std::string rsp_; ///< The latest response from Oem7 receiver. + ros::CallbackQueue queue_; //< Dedicated queue for command requests. + boost::shared_ptr aspinner_; ///< 1 thread servicing the command queue. + ros::ServiceServer oem7_cmd_srv_; ///< Oem7 command service. + + + ros::Timer timer_; ///< One time service callback. + + pluginlib::ClassLoader recvr_loader_; + pluginlib::ClassLoader oem7_msg_decoder_loader; + + std::set raw_msg_pub_; ///< Set of raw messages to publish. + + boost::shared_ptr msg_handler_; ///< Dispatches individual messages for handling. + + // Log statistics + long total_log_count_; ///< Total number of logs received + + typedef std::map log_count_map_t; + log_count_map_t log_counts_; ///< Indidividual log counts. + long discarded_msg_num_; ///< Number of messages received and discarded by the driver. + + + boost::shared_ptr msg_decoder; ///< Message Decoder plugin + boost::shared_ptr recvr_; ///< Oem7 Receiver Interface plugin + + + + /** + * Wraps actual initializating for exception handling. + */ + void onInit() + { + try + { + onInitImpl(); + } + catch(std::exception const& ex) + { + NODELET_FATAL_STREAM("Fatal: " << ex.what()); + + ros::shutdown(); + } + } + + /** + * Loads plugins, sets up threading/callbacks, advertises messages and services. + */ + void onInitImpl() + { + NODELET_INFO_STREAM(getName() << ": Oem7MessageNodelet v." << novatel_oem7_driver_VERSION << "; " + << __DATE__ << " " << __TIME__); + + boost::lock_guard guard(nodelet_mtx_); + + + initializeOem7MessageUtil(getNodeHandle()); + + getNodeHandle().setCallbackQueue(&timer_queue_); + + getPrivateNodeHandle().getParam("oem7_publish_delay", publish_delay_sec_); + if(publish_delay_sec_ > 0) + { + NODELET_WARN_STREAM("Publish Delay: " << publish_delay_sec_ << " seconds. Is this is a test?"); + } + // Load plugins + + // Load Oem7Receiver + std::string oem7_if_name; + getPrivateNodeHandle().getParam("oem7_if", oem7_if_name); + recvr_ = recvr_loader_.createInstance(oem7_if_name); + recvr_->initialize(getPrivateNodeHandle()); + + // Load Oem7 Message Decoder + std::string msg_decoder_name; + getPrivateNodeHandle().getParam("oem7_msg_decoder", msg_decoder_name); + msg_decoder = oem7_msg_decoder_loader.createInstance(msg_decoder_name); + msg_decoder->initialize(getPrivateNodeHandle(), recvr_.get(), this); + + + msg_handler_.reset(new MessageHandler(getPrivateNodeHandle())); + + // Oem7 raw messages to publish. + std::vector oem7_raw_msgs; + bool ok = getPrivateNodeHandle().getParam("oem7_raw_msgs", oem7_raw_msgs); + for(const auto& msg : oem7_raw_msgs) + { + int raw_msg_id = getOem7MessageId(msg); + if(raw_msg_id == 0) + { + NODELET_ERROR_STREAM("Unknown Oem7 message '" << msg ); + } + else + { + NODELET_INFO_STREAM("Oem7 Raw message '" << msg << "' will be published." ); + raw_msg_pub_.insert(raw_msg_id); + } + } + + oem7rawmsg_pub_.setup("Oem7RawMsg", getPrivateNodeHandle()); + + timer_spinner_.reset(new ros::AsyncSpinner(1, &timer_queue_)); //< 1 thread servicing the command queue. + timer_spinner_->start(); + + timer_ = getNodeHandle().createTimer(ros::Duration(0.0), &Oem7MessageNodelet::serviceLoopCb, this, true); + + aspinner_.reset(new ros::AsyncSpinner(1, &queue_)); + aspinner_->start(); + + ros::AdvertiseServiceOptions ops = ros::AdvertiseServiceOptions::create( + "Oem7Cmd", + boost::bind(&Oem7MessageNodelet::serviceOem7AbasciiCb, this, _1, _2), + ros::VoidConstPtr(), + &queue_); + oem7_cmd_srv_ = getPrivateNodeHandle().advertiseService(ops); + } + + + /** + * Called to request O7AbasciiCmd service + */ + bool serviceOem7AbasciiCb(novatel_oem7_msgs::Oem7AbasciiCmd::Request& req, novatel_oem7_msgs::Oem7AbasciiCmd::Response& rsp) + { + NODELET_DEBUG_STREAM("AACmd: cmd '" << req.cmd << "'"); + + // Protect this with semaphore vs race with init code + nodelet_mtx_.lock(); + rsp_.clear(); + nodelet_mtx_.unlock(); + + for(int attempt = 0; + attempt < 10; + attempt++) + { + recvr_->write(boost::asio::buffer(req.cmd)); + + static const std::string NEWLINE("\n"); + recvr_->write(boost::asio::buffer(NEWLINE)); + + boost::system_time const timeout = boost::get_system_time() + boost::posix_time::milliseconds(3 * 1000); + if(rsp_sem_.timed_wait(timeout)) + { + break; + } + NODELET_ERROR_STREAM("Attempt " << attempt << ": timed out waiting for response."); + } + + nodelet_mtx_.lock(); + rsp.rsp = rsp_; + rsp_.clear(); + nodelet_mtx_.unlock(); + + if(rsp.rsp == "OK") + { + NODELET_INFO_STREAM("AACmd '" << req.cmd << "' : " << "'" << rsp.rsp << "'"); + } + else + { + NODELET_ERROR_STREAM("AACmd '" << req.cmd << "' : " << "'" << rsp.rsp << "'"); + } + + return true; + } + + /** + * Outputs Log statistics to ROS console. + */ + void outputLogStatistics() + { + NODELET_INFO("Log Statistics:"); + NODELET_INFO_STREAM("Logs: " << total_log_count_ << "; discarded: " << discarded_msg_num_); + + for(log_count_map_t::iterator itr = log_counts_.begin(); + itr != log_counts_.end(); + itr++) + { + int id = itr->first; + long count = itr->second; + + NODELET_INFO_STREAM("Log[" << getOem7MessageName(id) << "](" << id << "):" << count); + } + } + + /* + * Update Log statistics for a particular message + */ + void updateLogStatistics(const Oem7RawMessageIf::ConstPtr& raw_msg) + { + total_log_count_++; + + if(log_counts_.find(raw_msg->getMessageId()) == log_counts_.end()) + { + log_counts_[raw_msg->getMessageId()] = 0; + } + log_counts_[raw_msg->getMessageId()]++; + + + if((total_log_count_ % 10000) == 0) + { + outputLogStatistics(); + } + } + + /** + * Called by ROS decoder with new raw messages + */ + void onNewMessage(Oem7RawMessageIf::ConstPtr raw_msg) + { + + + NODELET_DEBUG_STREAM("onNewMsg: fmt= " << raw_msg->getMessageFormat() + << " type= " << raw_msg->getMessageType()); + + // Discard all unknown messages; this is normally when dealing with responses to ASCII commands. + if(raw_msg->getMessageFormat() == Oem7RawMessageIf::OEM7MSGFMT_UNKNOWN) + { + ++discarded_msg_num_; + NODELET_DEBUG_STREAM("Discarded: [ID: " << raw_msg->getMessageType() << + " T: " << raw_msg->getMessageType() << + " F: " << raw_msg->getMessageFormat() << + " L: " << raw_msg->getMessageDataLength() << + "]"); + } + else + { + if(raw_msg->getMessageFormat() == Oem7RawMessageIf::OEM7MSGFMT_ASCII || + raw_msg->getMessageFormat() == Oem7RawMessageIf::OEM7MSGFMT_ABASCII) + { + std::string msg(raw_msg->getMessageData(0), raw_msg->getMessageData(raw_msg->getMessageDataLength())); + + NODELET_DEBUG_STREAM(">---------------------------" << std::endl + << msg << std::endl + << "<---------------------------"); + } + + if(raw_msg->getMessageType() == Oem7RawMessageIf::OEM7MSGTYPE_RSP) // Response + { + std::string rsp(raw_msg->getMessageData(0), raw_msg->getMessageData(raw_msg->getMessageDataLength())); + + + nodelet_mtx_.lock(); + rsp_ = rsp; + nodelet_mtx_.unlock(); + + rsp_sem_.post(); + } + else // Log + { + if(raw_msg->getMessageFormat() == Oem7RawMessageIf::OEM7MSGFMT_BINARY) // binary + { + updateLogStatistics(raw_msg); + + msg_handler_->handleMessage(raw_msg); + + + if(raw_msg_pub_.find(raw_msg->getMessageId()) != raw_msg_pub_.end()) + { + novatel_oem7_msgs::Oem7RawMsg::Ptr oem7_raw_msg(new novatel_oem7_msgs::Oem7RawMsg); + oem7_raw_msg->message_data.insert( + oem7_raw_msg->message_data.end(), + raw_msg->getMessageData(0), + raw_msg->getMessageData(raw_msg->getMessageDataLength())); + + assert(oem7_raw_msg->message_data.size() == raw_msg->getMessageDataLength()); + + oem7rawmsg_pub_.publish(oem7_raw_msg); + } + + if(publish_delay_sec_ > 0) + { + sleep(publish_delay_sec_); + } + } + } + } + } + + + /** + * Service loop; drives Oem7 message decoder. onNewMessage is called on this thread. + */ + void serviceLoopCb(const ros::TimerEvent& event) + { + msg_decoder->service(); + + outputLogStatistics(); + + NODELET_WARN("No more input from Decoder; Oem7MessageNodelet finished."); + } + + + public: + Oem7MessageNodelet(): + recvr_loader_( "novatel_oem7_driver", "novatel_oem7_driver::Oem7ReceiverIf"), + oem7_msg_decoder_loader("novatel_oem7_driver", "novatel_oem7_driver::Oem7MessageDecoderIf"), + rsp_sem_(0), + total_log_count_(0), + discarded_msg_num_(0), + publish_delay_sec_(0) + { + } + + ~Oem7MessageNodelet() + { + NODELET_DEBUG("~Oem7MessageNodelet"); + } + }; +} + + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7MessageNodelet, nodelet::Nodelet) diff --git a/src/novatel_oem7_driver/src/oem7_message_util.cpp b/src/novatel_oem7_driver/src/oem7_message_util.cpp new file mode 100644 index 0000000..1d688d3 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_message_util.cpp @@ -0,0 +1,167 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include + +#include "novatel_oem7_driver/oem7_messages.h" + +namespace +{ + bool is_initialized = false; + + std::map oem7_msg_id_map; ///< Mapping of Oem7 Message Names to IDs + std::map oem7_msg_name_map; ///< Mapping of Oem7 Message IDs to Names. + +} + +namespace novatel_oem7_driver +{ + enum GPSReferenceTimeStatus + { + GPS_REFTIME_STATUS_UNKNOWN = 20 // Refer to Oem7 manual. + }; + + void initializeOem7MessageUtil(ros::NodeHandle& nh) + { + if(is_initialized) + return; + + const std::string ns(ros::this_node::getNamespace()); + nh.getParam(ns + "/oem7_msgs", oem7_msg_id_map); + for(const auto& msg_itr : oem7_msg_id_map) + { + ROS_DEBUG_STREAM("Oem7 Message: " << msg_itr.first << ":" << msg_itr.second); + oem7_msg_name_map[msg_itr.second] = msg_itr.first; + } + + is_initialized = true; + } + + int getOem7MessageId(const std::string& msg_name) + { + return oem7_msg_id_map[msg_name]; + } + + const std::string& getOem7MessageName(int msg_id) + { + return oem7_msg_name_map[msg_id]; + } + + + /** + * Gets Oem7 Binary message header from raw message + * + */ + void getOem7Header( + const Oem7RawMessageIf::ConstPtr& raw_msg, + novatel_oem7_msgs::Oem7Header::Type& hdr + ) + { + const Oem7MessageHeaderMem* hdr_mem = reinterpret_cast(raw_msg->getMessageData(0)); + + hdr.message_id = hdr_mem->message_id; + hdr.message_type = hdr_mem->message_type; + hdr.sequence_number = hdr_mem->sequence; + hdr.time_status = hdr_mem->time_status; + hdr.gps_week_number = hdr_mem->gps_week; + hdr.gps_week_milliseconds = hdr_mem->gps_milliseconds; + } + + void getOem7ShortHeader( + const Oem7RawMessageIf::ConstPtr& raw_msg, ///< [in] Raw binary message + novatel_oem7_msgs::Oem7Header::Type& hdr ///< [out] Oem7 Message Header + ) + { + const Oem7MessgeShortHeaderMem* hdr_mem = reinterpret_cast(raw_msg->getMessageData(0)); + + hdr.message_id = hdr_mem->message_id; + hdr.message_type = novatel_oem7_msgs::Oem7Header::OEM7MSGTYPE_LOG; // Always log + hdr.sequence_number = 0; // Not available; assume it's a single log. + hdr.time_status = GPS_REFTIME_STATUS_UNKNOWN; + hdr.gps_week_number = hdr_mem->gps_week; + hdr.gps_week_milliseconds = hdr_mem->gps_milliseconds; + } + + + size_t Get_INSCONFIG_NumTranslations(const INSCONFIG_FixedMem* insconfig) + { + const uint8_t* mem = reinterpret_cast(insconfig) + sizeof(INSCONFIG_FixedMem); + return *reinterpret_cast(mem); // Safe because all fields are guaranteed to be aligned. + } + + const INSCONFIG_TranslationMem* Get_INSCONFIG_Translation(const INSCONFIG_FixedMem* insconfig, size_t idx) + { + const uint8_t* mem = reinterpret_cast(insconfig) + + sizeof(INSCONFIG_FixedMem) + + sizeof(uint32_t) + + sizeof(INSCONFIG_TranslationMem) * idx; + + return reinterpret_cast(mem); + } + + size_t Get_INSCONFIG_NumRotations(const INSCONFIG_FixedMem* insconfig) + { + const uint8_t* mem = reinterpret_cast(insconfig) + + sizeof(INSCONFIG_FixedMem) + + sizeof(uint32_t) + + sizeof(INSCONFIG_TranslationMem) * Get_INSCONFIG_NumTranslations(insconfig); + + return *reinterpret_cast(mem); + } + + + const INSCONFIG_RotationMem* Get_INSCONFIG_Rotation(const INSCONFIG_FixedMem* insconfig, size_t idx) + { + const uint8_t* mem = reinterpret_cast(insconfig) + + sizeof(INSCONFIG_FixedMem) + + sizeof(uint32_t) + + sizeof(INSCONFIG_TranslationMem) * Get_INSCONFIG_NumTranslations(insconfig) + + sizeof(uint32_t) + + sizeof(INSCONFIG_RotationMem) * idx; + + return reinterpret_cast(mem); + } + + + size_t Get_PSRDOP2_NumSystems(const PSRDOP2_FixedMem* psrdop2) + { + const uint8_t* mem = reinterpret_cast(psrdop2) + sizeof(PSRDOP2_FixedMem); + return *reinterpret_cast(mem); // Safe because all fields are guaranteed to be aligned. + } + + const PSRDOP2_SystemMem* Get_PSRDOP2_System(const PSRDOP2_FixedMem* psrdop2, size_t idx) + { + const uint8_t* mem = reinterpret_cast(psrdop2) + + sizeof(PSRDOP2_FixedMem) + + sizeof(uint32_t) + + sizeof(PSRDOP2_SystemMem) * idx; + + return reinterpret_cast(mem); + } +} + + + + + diff --git a/src/novatel_oem7_driver/src/oem7_receiver.hpp b/src/novatel_oem7_driver/src/oem7_receiver.hpp new file mode 100644 index 0000000..5c00b40 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_receiver.hpp @@ -0,0 +1,175 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +#include + + + +namespace novatel_oem7_driver +{ + /** + * Common functionality related to boost::asio + */ + template + class Oem7Receiver: public Oem7ReceiverIf + { + boost::asio::io_service io_; + + enum + { + DEFAULT_MAX_NUM_IO_ERRORS = 7 + }; + + + + protected: + ros::NodeHandle nh_; + + T endpoint_; ///< boost::asio communication endoint; socket, serial port, etc. + + int max_num_io_errors_; ///< Number of consecutive io errors before declaring failure and quitting. + int num_io_errors_; ///< Number of consecuitive io errors. + + bool in_error_state() + { + if(num_io_errors_ >= max_num_io_errors_) + { + ROS_ERROR_STREAM("Oem7Receiver: Max Num IO errors exceeded: " << max_num_io_errors_); + return true; + } + else + { + return false; + } + } + + + /** + * Attempt to opena the endpoint. + */ + virtual void endpoint_try_open() = 0; + + /** + * Read some data from the endpoint. + */ + virtual size_t endpoint_read( boost::asio::mutable_buffer buf, boost::system::error_code& err) = 0; + + + /** + * Write some data to the endpoint + */ + virtual size_t endpoint_write(boost::asio::const_buffer buf, boost::system::error_code& err) = 0; + + /** + * Close the endpoint; delay to avoid tight re-open loop + */ + void endpoint_close() + { + boost::system::error_code err; + endpoint_.close(err); + ROS_ERROR_STREAM("Oem7Receiver: close error: " << err.value()); + sleep(1.0); + } + + public: + Oem7Receiver(): + io_(), + endpoint_(io_), + max_num_io_errors_(DEFAULT_MAX_NUM_IO_ERRORS), + num_io_errors_(0) + { + } + + virtual ~Oem7Receiver() + { + } + + virtual bool initialize(ros::NodeHandle& h) + { + nh_ = h; + + this->nh_.getParam("oem7_max_io_errors", max_num_io_errors_); + + return true; + } + + virtual bool read( boost::asio::mutable_buffer buf, size_t& rlen) + { + while(!ros::isShuttingDown() && !in_error_state()) + { + endpoint_try_open(); + + boost::system::error_code err; + size_t len = endpoint_read(buf, err); + if(err.value() == boost::system::errc::success) + { + num_io_errors_ = 0; // Reset error counter + + rlen = len; + return true; + } + // else: error condition + + + num_io_errors_++; + + ROS_ERROR_STREAM("Oem7Receiver: read error: " << err.value() + <<"; endpoint open: " << endpoint_.is_open() + <<" errors/max: " << num_io_errors_ + <<"/" << max_num_io_errors_); + endpoint_close(); + } + + return false; + } + + virtual bool write(boost::asio::const_buffer buf) + { + if(in_error_state() || ros::isShuttingDown()) + return false; + + endpoint_try_open(); + + boost::system::error_code err; + endpoint_write(buf, err); + if(err.value() != boost::system::errc::success) + { + num_io_errors_++; + + ROS_ERROR_STREAM("Oem7Receiver: write error: " << err.value() << "; endpoint open: " << endpoint_.is_open()); + endpoint_close(); + return false; + } + + return true; + } +}; + +} + + diff --git a/src/novatel_oem7_driver/src/oem7_receiver_file.cpp b/src/novatel_oem7_driver/src/oem7_receiver_file.cpp new file mode 100644 index 0000000..2bf7711 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_receiver_file.cpp @@ -0,0 +1,133 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +#include +#include + + +namespace novatel_oem7_driver +{ + /** + * 'Virtual' Oem7 interface, where input is read from a file. The contents of the file is any relevant receiver output. + */ + class Oem7ReceiverFile: public Oem7ReceiverIf + { + std::ifstream oem7_file_; ///< Input + + size_t num_byte_read_; ///< Total number of bytes read from file. + + + public: + Oem7ReceiverFile(): + num_byte_read_(0) + { + } + + ~Oem7ReceiverFile() + { + } + + /** + * Prepares the input file for reading. The file is opened as binary/read only. + */ + virtual bool initialize(ros::NodeHandle& nh) + { + std::string oem7_file_name;; + nh.getParam("oem7_file_name", oem7_file_name); + + ROS_INFO_STREAM("Oem7File['" << oem7_file_name << "']"); + + oem7_file_.open(oem7_file_name, std::ios::in | std::ios::binary); + int errno_value = errno; // Cache errno locally, in case any ROS calls /macros affect it. + if(!oem7_file_) + { + ROS_ERROR_STREAM("Could not open '" << oem7_file_name << "'; error= " << errno_value << " '" + << strerror(errno_value) << "'"); + return false; + } + + return true; + } + + /** + * Reads input from file. + * + */ + virtual bool read( boost::asio::mutable_buffer buf, size_t& rlen) + { + if(!oem7_file_) + { + ROS_ERROR_STREAM("Error accessing file." ); + return false; + } + + // Workaround for automated testing: + // delay reporting logs so that 'rosbag record' has a chance to subscribe to the topic after they are published. + // Otherwise it is likely to miss messages. + if(num_byte_read_ == 0) + { + sleep(3); // Use absolute sleep, as this is not related to ROS internal timing. + } + + oem7_file_.read(boost::asio::buffer_cast(buf), boost::asio::buffer_size(buf)); + int errno_value = errno; // Cache errno locally, in case any ROS calls /macros affect it. + + rlen = oem7_file_.gcount(); + num_byte_read_ += rlen; + + if(oem7_file_.eof()) + { + ROS_INFO_STREAM("No more input available. Read " << num_byte_read_ << " bytes." ); + + return false; + } + + if(!oem7_file_) + { + ROS_ERROR_STREAM("Error " << errno_value << " reading input: '" << strerror(errno_value) << "'" ); + + return false; + } + + return true; + } + + /** + * Takes no action. + * + * @return false always. + */ + virtual bool write(boost::asio::const_buffer buf) + { + return false; + } + }; +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7ReceiverFile, novatel_oem7_driver::Oem7ReceiverIf) diff --git a/src/novatel_oem7_driver/src/oem7_receiver_net.cpp b/src/novatel_oem7_driver/src/oem7_receiver_net.cpp new file mode 100644 index 0000000..7d5efd0 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_receiver_net.cpp @@ -0,0 +1,89 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include + +#include + + +namespace novatel_oem7_driver +{ + + + template + class Oem7ReceiverNet: public Oem7Receiver + { + void endpoint_try_open() + { + if(this->endpoint_.is_open()) + { + return; + } + + + std::string recvr_ip_addr; + this->nh_.getParam("oem7_ip_addr", recvr_ip_addr); + + int recvr_port; + this->nh_.getParam("oem7_port", recvr_port); + + ROS_INFO_STREAM("Oem7Net " << (T::v4().protocol() == IPPROTO_TCP ? "TCP" : "UDP") << + "['" << recvr_ip_addr << "' : " << recvr_port << "]"); + + boost::system::error_code err; + + this->endpoint_.close(err); // Doesn't matter if we fail. + this->endpoint_.connect(typename T::endpoint(boost::asio::ip::address::from_string(recvr_ip_addr), recvr_port), err); + // Proceed regardless; successful connection does not guarantee subsequent operations will succeed. + + ROS_INFO_STREAM("Oem7Net socket open: '" << this->endpoint_.is_open() << "; OS error= " << err.value()); + + static const std::string CONN_PRIMER("\r\n"); + endpoint_write(boost::asio::buffer(CONN_PRIMER), err); + } + + virtual size_t endpoint_read(boost::asio::mutable_buffer buf, boost::system::error_code& err) + { + boost::array bufs = {buf}; + return this->endpoint_.receive(bufs, 0, err); + } + + virtual size_t endpoint_write(boost::asio::const_buffer buf, boost::system::error_code& err) + { + const boost::array bufs = {buf}; + return this->endpoint_.send(bufs, 0, err); + } + }; + + class Oem7ReceiverTcp: public Oem7ReceiverNet{}; + class Oem7ReceiverUdp: public Oem7ReceiverNet{}; +} + + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7ReceiverTcp, novatel_oem7_driver::Oem7ReceiverIf) +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7ReceiverUdp, novatel_oem7_driver::Oem7ReceiverIf) diff --git a/src/novatel_oem7_driver/src/oem7_receiver_port.cpp b/src/novatel_oem7_driver/src/oem7_receiver_port.cpp new file mode 100644 index 0000000..25cd59e --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_receiver_port.cpp @@ -0,0 +1,90 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include + +#include + + +namespace novatel_oem7_driver +{ + /** + * Serial port (tty) implementation. + */ + class Oem7ReceiverPort: public Oem7Receiver + { + void endpoint_try_open() + { + if(endpoint_.is_open()) + { + return; + } + + std::string recvr_tty_name; + nh_.getParam("oem7_tty_name", recvr_tty_name); + + int baud_rate = 0; // Optional parameter + nh_.getParam("oem7_tty_baud", baud_rate); + ROS_INFO_STREAM("Oem7SerialPort['" << recvr_tty_name << "' : " << baud_rate << "]"); + + + boost::system::error_code err; + // We proceed regardless of the error code. + // Successful connection does not guarantee subsequent operations will succeed. + // Attempting operations on closed ports is harmless. + + endpoint_.close(err); + endpoint_.open(recvr_tty_name, err); + ROS_INFO_STREAM("Oem7SerialPort open: '" << endpoint_.is_open() << "; err: " << err); + + if(baud_rate > 0) + { + boost::asio::serial_port_base::baud_rate baud_option(baud_rate); + endpoint_.set_option(baud_option, err); + ROS_INFO_STREAM("Oem7SerialPort set_option baud_rate: '" << baud_option.value() << " : " << err); + } + } + + virtual size_t endpoint_read(boost::asio::mutable_buffer buf, boost::system::error_code& err) + { + boost::array bufs = {buf}; + return endpoint_.read_some(bufs, err); + } + + virtual size_t endpoint_write(boost::asio::const_buffer buf, boost::system::error_code& err) + { + boost::array bufs = {buf}; + return endpoint_.write_some(bufs, err); + } +}; + +} + + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7ReceiverPort, novatel_oem7_driver::Oem7ReceiverIf) + diff --git a/src/novatel_oem7_driver/src/oem7_ros_messages.cpp b/src/novatel_oem7_driver/src/oem7_ros_messages.cpp new file mode 100644 index 0000000..7db1781 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_ros_messages.cpp @@ -0,0 +1,598 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// +// +// The names of messages are ROS messages, not Oem7 messages. These may be identical, but not necessarily. +// More than one Oem7 message may be used to generate the same ROS message +// + +#include "novatel_oem7_driver/oem7_ros_messages.hpp" + +#include "novatel_oem7_driver/oem7_message_ids.h" +#include "novatel_oem7_driver/oem7_messages.h" +#include "novatel_oem7_driver/oem7_message_util.hpp" + + +#include "novatel_oem7_msgs/HEADING2.h" +#include "novatel_oem7_msgs/BESTPOS.h" +#include "novatel_oem7_msgs/BESTVEL.h" +#include "novatel_oem7_msgs/BESTUTM.h" +#include "novatel_oem7_msgs/INSPVA.h" +#include "novatel_oem7_msgs/INSPVAX.h" +#include "novatel_oem7_msgs/INSCONFIG.h" +#include "novatel_oem7_msgs/INSSTDEV.h" +#include "novatel_oem7_msgs/CORRIMU.h" +#include "novatel_oem7_msgs/RXSTATUS.h" +#include "novatel_oem7_msgs/TIME.h" + + + +#define arr_size(_arr_) (sizeof(_arr_) / sizeof(_arr_[0])) + +namespace novatel_oem7_driver +{ + +/** + * Generate ROS message sequence number + * @return sequence number + */ +uint32_t GetNextMsgSequenceNumber() +{ + static uint32_t sequence_number = 0; + + return ++sequence_number; +} + +/* + * Populates Oem7header from raw message + */ +void +SetOem7Header( + const Oem7RawMessageIf::ConstPtr& msg, ///< in: raw message + const std::string& name, ///< message name + novatel_oem7_msgs::Oem7Header::Type& oem7_hdr ///< header to populate + ) +{ + getOem7Header(msg, oem7_hdr); + oem7_hdr.message_name = name; +} + +/* + * Populates Oem7header from a 'short' raw message + */ +void +SetOem7ShortHeader( + const Oem7RawMessageIf::ConstPtr& msg, ///< in: short raw message + const std::string& name, ///< message name + novatel_oem7_msgs::Oem7Header::Type& oem7_hdr ///< header to populate + ) +{ + getOem7ShortHeader(msg, oem7_hdr); + oem7_hdr.message_name = name; +} + +// Template specializations from specific messages + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& heading2) +{ + assert(msg->getMessageId() == HEADING2_OEM7_MSGID); + + const HEADING2Mem* mem = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + heading2.reset(new novatel_oem7_msgs::HEADING2); + + heading2->sol_status.status = mem->sol_status; + heading2->pos_type.type = mem->pos_type; + heading2->length = mem->length; + heading2->heading = mem->heading; + heading2->pitch = mem->pitch; + heading2->reserved = mem->reserved; + heading2->heading_stdev = mem->heading_stdev; + heading2->pitch_stdev = mem->pitch_stdev; + heading2->rover_stn_id.assign( mem->rover_stn_id, arr_size(mem->rover_stn_id)); + heading2->master_stn_id.assign(mem->master_stn_id, arr_size(mem->rover_stn_id)); + heading2->num_sv_tracked = mem->num_sv_tracked; + heading2->num_sv_in_sol = mem->num_sv_in_sol; + heading2->num_sv_obs = mem->num_sv_obs; + heading2->num_sv_multi = mem->num_sv_multi; + heading2->sol_source.source = mem->sol_source; + heading2->ext_sol_status.status = mem->ext_sol_status; + heading2->galileo_beidou_sig_mask = mem->galileo_beidou_sig_mask; + heading2->gps_glonass_sig_mask = mem->gps_glonass_sig_mask; + + static const std::string name = "HEADING2"; + SetOem7Header(msg, name, heading2->nov_header); +} + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& bestpos) +{ + assert(msg->getMessageId() == BESTPOS_OEM7_MSGID); + + const BESTPOSMem* bp = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + bestpos.reset(new novatel_oem7_msgs::BESTPOS); + + bestpos->sol_status.status = bp->sol_stat; + bestpos->pos_type.type = bp->pos_type; + bestpos->lat = bp->lat; + bestpos->lon = bp->lon; + bestpos->hgt = bp->hgt; + bestpos->undulation = bp->undulation; + bestpos->datum_id = bp->datum_id; + bestpos->lat_stdev = bp->lat_stdev; + bestpos->lon_stdev = bp->lon_stdev; + bestpos->hgt_stdev = bp->hgt_stdev; + bestpos->stn_id.assign( bp->stn_id, arr_size(bp->stn_id)); + bestpos->diff_age = bp->diff_age; + bestpos->sol_age = bp->sol_age; + bestpos->num_svs = bp->num_svs; + bestpos->num_sol_svs = bp->num_sol_svs; + bestpos->num_sol_l1_svs = bp->num_sol_l1_svs; + bestpos->reserved = bp->reserved; + bestpos->ext_sol_stat.status = bp->ext_sol_stat; + bestpos->galileo_beidou_sig_mask= bp->galileo_beidou_sig_mask; + bestpos->gps_glonass_sig_mask = bp->gps_glonass_sig_mask; + + static const std::string name = "BESTPOS"; + SetOem7Header(msg, name, bestpos->nov_header); +} + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& bestvel) +{ + assert(msg->getMessageId() == BESTVEL_OEM7_MSGID); + + const BESTVELMem* bv = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + bestvel.reset(new novatel_oem7_msgs::BESTVEL); + + bestvel->sol_status.status = bv->sol_stat; + bestvel->vel_type.type = bv->vel_type; + bestvel->latency = bv->latency; + bestvel->diff_age = bv->diff_age; + bestvel->hor_speed = bv->hor_speed; + bestvel->trk_gnd = bv->track_gnd; + bestvel->ver_speed = bv->ver_speed; + bestvel->reserved = bv->reserved; + + static const std::string name = "BESTVEL"; + SetOem7Header(msg, name, bestvel->nov_header); +} + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& bestutm) +{ + assert(msg->getMessageId() == BESTUTM_OEM7_MSGID); + + const BESTUTMMem* mem = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + bestutm.reset(new novatel_oem7_msgs::BESTUTM); + + bestutm->pos_type.type = mem->pos_type;; + bestutm->lon_zone_number = mem->lon_zone_number; + bestutm->lat_zone_letter = mem->lat_zone_letter; + bestutm->northing = mem->northing; + bestutm->easting = mem->easting; + bestutm->height = mem->height; + bestutm->undulation = mem->undulation; + bestutm->datum_id = mem->datum_id; + bestutm->northing_stddev = mem->northing_stddev; + bestutm->easting_stddev = mem->easting_stddev; + bestutm->height_stddev = mem->height_stddev; + bestutm->stn_id.assign( mem->stn_id, arr_size(mem->stn_id)); + bestutm->diff_age = mem->diff_age; + bestutm->sol_age = mem->sol_age; + bestutm->num_svs = mem->num_svs; + bestutm->num_sol_svs = mem->num_sol_svs; + bestutm->num_sol_ggl1_svs = mem->num_sol_ggl1_svs; + bestutm->num_sol_multi_svs = mem->num_sol_multi_svs; + bestutm->reserved = mem->reserved; + bestutm->ext_sol_stat.status = mem->ext_sol_stat; + bestutm->galileo_beidou_sig_mask= mem->galileo_beidou_sig_mask; + bestutm->gps_glonass_sig_mask = mem->gps_glonass_sig_mask; + + static const std::string name = "BESTUTM"; + SetOem7Header(msg, name, bestutm->nov_header); + } + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& pva) +{ + assert(msg->getMessageId() == INSPVAS_OEM7_MSGID); + + const INSPVASmem* pvamem = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_SHORT_HDR_LEN)); + pva.reset(new novatel_oem7_msgs::INSPVA); + + pva->latitude = pvamem->latitude; + pva->longitude = pvamem->longitude; + pva->height = pvamem->height; + pva->north_velocity = pvamem->north_velocity; + pva->east_velocity = pvamem->east_velocity; + pva->up_velocity = pvamem->up_velocity; + pva->roll = pvamem->roll; + pva->pitch = pvamem->pitch; + pva->azimuth = pvamem->azimuth; + pva->status.status = pvamem->status; + + static const std::string name = "INSPVA"; + SetOem7ShortHeader(msg, name, pva->nov_header); +} + + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& insconfig) +{ + assert(msg->getMessageId()== INSCONFIG_OEM7_MSGID); + + const INSCONFIG_FixedMem* insconfigmem = + reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + insconfig.reset(new novatel_oem7_msgs::INSCONFIG); + + insconfig->imu_type = insconfigmem->imu_type; + insconfig->mapping = insconfigmem->mapping; + insconfig->initial_alignment_velocity = insconfigmem->initial_alignment_velocity; + insconfig->heave_window = insconfigmem->heave_window; + insconfig->profile = insconfigmem->profile; + + std::copy( + insconfigmem->enabled_updates, + insconfigmem->enabled_updates + arr_size(insconfigmem->enabled_updates), + insconfig->enabled_updates.begin()); + + insconfig->alignment_mode.mode = insconfigmem->alignment_mode; + insconfig->relative_ins_output_frame.frame = insconfigmem->relative_ins_output_frame; + insconfig->relative_ins_output_direction = insconfigmem->relative_ins_output_direction; + + std::copy( + insconfigmem->ins_receiver_status, + insconfigmem->ins_receiver_status + arr_size(insconfigmem->ins_receiver_status), + insconfig->ins_receiver_status.status.begin()); + + insconfig->ins_seed_enabled = insconfigmem->ins_seed_enabled; + insconfig->ins_seed_validation = insconfigmem->ins_seed_validation; + insconfig->reserved_1 = insconfigmem->reserved_1; + insconfig->reserved_2 = insconfigmem->reserved_2; + insconfig->reserved_3 = insconfigmem->reserved_3; + insconfig->reserved_4 = insconfigmem->reserved_4; + insconfig->reserved_5 = insconfigmem->reserved_5; + insconfig->reserved_6 = insconfigmem->reserved_6; + insconfig->reserved_7 = insconfigmem->reserved_7; + + insconfig->translations.reserve(Get_INSCONFIG_NumTranslations(insconfigmem)); + for(size_t idx = 0; + idx < Get_INSCONFIG_NumTranslations(insconfigmem); + idx++) + { + const INSCONFIG_TranslationMem* trmem = Get_INSCONFIG_Translation(insconfigmem, idx); + novatel_oem7_msgs::Translation& tr = insconfig->translations[idx]; + + tr.translation.type = trmem->translation; + tr.frame.frame = trmem->frame; + tr.x_offset = trmem->x_offset; + tr.y_offset = trmem->y_offset; + tr.z_offset = trmem->z_offset; + tr.x_uncertainty = trmem->x_uncertainty; + tr.y_uncertainty = trmem->y_uncertainty; + tr.z_uncertainty = trmem->z_uncertainty; + tr.translation_source.status = trmem->translation_source; + } + + insconfig->rotations.reserve(Get_INSCONFIG_NumRotations(insconfigmem)); + for(size_t idx = 0; + idx < Get_INSCONFIG_NumRotations(insconfigmem); + idx++) + { + const INSCONFIG_RotationMem* rtmem = Get_INSCONFIG_Rotation(insconfigmem, idx); + novatel_oem7_msgs::Rotation& rt = insconfig->rotations[idx]; + rt.rotation.offset = rtmem->rotation; + rt.frame.frame = rtmem->frame; + rt.x_rotation = rtmem->x_rotation; + rt.y_rotation = rtmem->y_rotation; + rt.z_rotation = rtmem->z_rotation; + rt.x_rotation_stdev = rtmem->x_rotation_stdev; + rt.y_rotation_stdev = rtmem->y_rotation_stdev; + rt.z_rotation_stdev = rtmem->z_rotation_stdev; + rt.rotation_source.status = rtmem->rotation_source; + } + + static const std::string name = "INSCONFIG"; + SetOem7Header(msg, name, insconfig->nov_header); +} + + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& inspvax) +{ + assert(msg->getMessageId() == INSPVAX_OEM7_MSGID); + + const INSPVAXMem* mem = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + inspvax.reset(new novatel_oem7_msgs::INSPVAX); + + inspvax->ins_status.status = mem->ins_status; + inspvax->pos_type.type = mem->pos_type; + inspvax->latitude = mem->latitude; + inspvax->longitude = mem->longitude; + inspvax->height = mem->height; + inspvax->undulation = mem->undulation; + inspvax->north_velocity = mem->north_velocity; + inspvax->east_velocity = mem->east_velocity; + inspvax->up_velocity = mem->up_velocity; + inspvax->roll = mem->roll; + inspvax->pitch = mem->pitch; + inspvax->azimuth = mem->azimuth; + inspvax->latitude_stdev = mem->latitude_stdev; + inspvax->longitude_stdev = mem->longitude_stdev; + inspvax->height_stdev = mem->height_stdev; + inspvax->north_velocity_stdev = mem->north_velocity_stdev; + inspvax->east_velocity_stdev = mem->east_velocity_stdev; + inspvax->up_velocity_stdev = mem->up_velocity_stdev; + inspvax->roll_stdev = mem->roll_stdev; + inspvax->pitch_stdev = mem->pitch_stdev; + inspvax->azimuth_stdev = mem->azimuth_stdev; + inspvax->ext_sol_status.status = mem->extended_status; + + static const std::string name = "INSPVAX"; + SetOem7Header(msg, name, inspvax->nov_header); +} + + + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& insstdev) +{ + assert(msg->getMessageId() == INSSTDEV_OEM7_MSGID); + + const INSSTDEVMem* raw = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_SHORT_HDR_LEN)); + insstdev.reset(new novatel_oem7_msgs::INSSTDEV); + + insstdev->latitude_stdev = raw->latitude_stdev; + insstdev->longitude_stdev = raw->longitude_stdev; + insstdev->height_stdev = raw->height_stdev; + insstdev->north_velocity_stdev = raw->north_velocity_stdev; + insstdev->east_velocity_stdev = raw->east_velocity_stdev; + insstdev->up_velocity_stdev = raw->up_velocity_stdev; + insstdev->roll_stdev = raw->roll_stdev; + insstdev->pitch_stdev = raw->pitch_stdev; + insstdev->azimuth_stdev = raw->azimuth_stdev; + insstdev->ext_sol_status.status = raw->ext_sol_status; + insstdev->time_since_last_update = raw->time_since_last_update; + insstdev->reserved1 = raw->reserved1; + insstdev->reserved2 = raw->reserved2; + insstdev->reserved3 = raw->reserved3; + + static const std::string name = "INSSTDEV"; + SetOem7Header(msg, name, insstdev->nov_header); +} + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& corrimu) +{ + corrimu.reset(new novatel_oem7_msgs::CORRIMU); + + if(msg->getMessageId() == CORRIMUS_OEM7_MSGID) + { + const CORRIMUSMem* raw = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_SHORT_HDR_LEN)); + corrimu->imu_data_count = raw->imu_data_count; + corrimu->pitch_rate = raw->pitch_rate; + corrimu->roll_rate = raw->roll_rate; + corrimu->yaw_rate = raw->yaw_rate; + corrimu->lateral_acc = raw->lateral_acc; + corrimu->longitudinal_acc = raw->longitudinal_acc; + corrimu->vertical_acc = raw->vertical_acc; + } + else if(msg->getMessageId() == IMURATECORRIMU_OEM7_MSGID) + { + const IMURATECORRIMUSMem* raw = + reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_SHORT_HDR_LEN)); + corrimu->imu_data_count = 0; // FIXME + corrimu->pitch_rate = raw->pitch_rate; + corrimu->roll_rate = raw->roll_rate; + corrimu->yaw_rate = raw->yaw_rate; + corrimu->lateral_acc = raw->lateral_acc; + corrimu->longitudinal_acc = raw->longitudinal_acc; + corrimu->vertical_acc = raw->vertical_acc; + } + else + { + assert(false); + } + + static const std::string name = "CORRIMU"; + SetOem7ShortHeader(msg, name, corrimu->nov_header); +} + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& time) +{ + assert(msg->getMessageId()== TIME_OEM7_MSGID); + + const TIMEMem* mem = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + time.reset(new novatel_oem7_msgs::TIME); + + time->clock_status = mem->clock_status; + time->offset = mem->offset; + time->offset_std = mem->offset_std; + time->utc_offset = mem->utc_offset; + time->utc_year = mem->utc_year; + time->utc_month = mem->utc_month; + time->utc_day = mem->utc_day; + time->utc_hour = mem->utc_hour; + time->utc_min = mem->utc_min; + time->utc_msec = mem->utc_msec; + time->utc_status = mem->utc_status; + + static const std::string name = "TIME"; + SetOem7Header(msg, name, time->nov_header); +} + +template<> +void +MakeROSMessage( + const Oem7RawMessageIf::ConstPtr& msg, + boost::shared_ptr& rxstatus) +{ + assert(msg->getMessageId() == RXSTATUS_OEM7_MSGID); + + const RXSTATUSMem* mem = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + rxstatus.reset(new novatel_oem7_msgs::RXSTATUS); + + rxstatus->error = mem->error; + rxstatus->num_status_codes = mem->num_status_codes; + rxstatus->rxstat = mem->rxstat; + rxstatus->rxstat_pri_mask = mem->rxstat_pri_mask; + rxstatus->rxstat_set_mask = mem->rxstat_set_mask; + rxstatus->rxstat_clr_mask = mem->rxstat_clr_mask; + rxstatus->aux1_stat = mem->aux1_stat; + rxstatus->aux1_stat_pri = mem->aux1_stat_pri; + rxstatus->aux1_stat_set = mem->aux1_stat_set; + rxstatus->aux1_stat_clr = mem->aux1_stat_clr; + rxstatus->aux2_stat = mem->aux2_stat; + rxstatus->aux2_stat_pri = mem->aux2_stat_pri; + rxstatus->aux2_stat_set = mem->aux2_stat_set; + rxstatus->aux2_stat_clr = mem->aux2_stat_clr; + rxstatus->aux3_stat = mem->aux3_stat; + rxstatus->aux3_stat_pri = mem->aux3_stat_pri; + rxstatus->aux3_stat_set = mem->aux3_stat_set; + rxstatus->aux3_stat_clr = mem->aux3_stat_clr; + rxstatus->aux4_stat = mem->aux4_stat; + rxstatus->aux4_stat_pri = mem->aux4_stat_pri; + rxstatus->aux4_stat_set = mem->aux4_stat_set; + rxstatus->aux4_stat_clr = mem->aux4_stat_clr; + + + static const std::string name = "RXSTATUS"; + SetOem7Header(msg, name, rxstatus->nov_header); +}; + + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + +template +void +MakeROSMessage(const Oem7RawMessageIf::ConstPtr&, boost::shared_ptr&); + + +//--------------------------------------------------------------------------------------------------------------- +/*** + * Obtains DOP values from Oem7 PSRDOP2 message + */ +void +GetDOPFromPSRDOP2( + const Oem7RawMessageIf::ConstPtr& msg, + uint32_t system_to_use, + double& gdop, + double& pdop, + double& hdop, + double& vdop, + double& tdop) +{ + const PSRDOP2_FixedMem* mem = reinterpret_cast(msg->getMessageData(OEM7_BINARY_MSG_HDR_LEN)); + + gdop = mem->gdop; + pdop = mem->pdop; + hdop = mem->hdop; + vdop = mem->vdop; + + const size_t num_sys = Get_PSRDOP2_NumSystems(mem); + + for(int i = 0; + i < num_sys; + i++) + { + const PSRDOP2_SystemMem* sys = Get_PSRDOP2_System(mem, i); + if(sys->system == system_to_use) + { + tdop = sys->tdop; + break; + } + } +} + + + + + +} diff --git a/src/novatel_oem7_driver/src/oem7_ros_publisher.hpp b/src/novatel_oem7_driver/src/oem7_ros_publisher.hpp new file mode 100644 index 0000000..54a26a2 --- /dev/null +++ b/src/novatel_oem7_driver/src/oem7_ros_publisher.hpp @@ -0,0 +1,106 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef __OEM7_ROS_PUBLISHER_HPP__ +#define __OEM7_ROS_PUBLISHER_HPP__ + + +#include + + +namespace novatel_oem7_driver +{ + +/** + * Encapsulates ROS message publisher, configured and enabled based on ROS parameters. + */ +class Oem7RosPublisher +{ + ros::Publisher ros_pub_; ///< ROS publisher + + std::string frame_id_; ///< Configurable frame ID. + +public: + + template + void setup(const std::string& name, ros::NodeHandle& nh) + { + typedef std::map message_config_map_t; + + message_config_map_t message_config_map; + nh.getParam(name, message_config_map); + + message_config_map_t::iterator topic_itr = message_config_map.find("topic"); + if(topic_itr == message_config_map.end()) + { + ROS_WARN_STREAM("Message '" << name << "' will not be published."); + return; + } + + int queue_size = 100; // default size + + message_config_map_t::iterator q_size_itr = message_config_map.find("queue_size"); + if(q_size_itr != message_config_map.end()) + { + std::stringstream ss(q_size_itr->second); + ss >> queue_size; + } + + message_config_map_t::iterator frame_id_itr = message_config_map.find("frame_id"); + if(frame_id_itr != message_config_map.end()) + { + frame_id_ = frame_id_itr->second; + } + + ROS_INFO_STREAM("topic [" << topic_itr->second << "]: frame_id: '" << frame_id_ << "'; q size: " << queue_size); + ros_pub_ = nh.advertise(topic_itr->second, queue_size); + } + + /** + * @return true if the publisher is enabled + */ + bool isEnabled() + { + return !ros_pub_.getTopic().empty(); + } + + /** + * Publish a message on this publisher. The message is ignored when the publisher is disabled. + */ + template + void publish(boost::shared_ptr& msg) + { + if(!isEnabled()) + { + return; + } + + SetROSHeader(frame_id_, msg); + ros_pub_.publish(msg); + } + +}; + +} +#endif diff --git a/src/novatel_oem7_driver/src/receiverstatus_handler.cpp b/src/novatel_oem7_driver/src/receiverstatus_handler.cpp new file mode 100644 index 0000000..53040a7 --- /dev/null +++ b/src/novatel_oem7_driver/src/receiverstatus_handler.cpp @@ -0,0 +1,342 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// +#include +#include + +#include + + +#include + + +#include + +#include "novatel_oem7_msgs/RXSTATUS.h" + + +namespace novatel_oem7_driver +{ + typedef std::vector str_vector_t; + + + /*** Oem7 Receiver errors strings - refer to Oem7 manual */ + const str_vector_t RECEIVER_ERROR_STRS + { + "DRAM", + "Invalid FW", + "ROM", + "", + "ESN Access", + "AuthCode", + "", + "Supply Voltage", + "", + "Temperature", + "MINOS", + "PLL RF", + "", + "", + "", + "NVM", + "Software resource limit exceeded", + "Model invalid for this receiver", + "", + "", + "Remote loading has begun", + "Export restriction", + "Safe Mode", + "", + "", + "", + "", + "", + "", + "", + "", + "Component hardware failure" + }; + + /** Oem7 receiver status strings - refer to Oem7 manual */ + const str_vector_t RECEIVER_STATUS_STRS + { + "Temperature", + "Voltage Supply", + "Primary antenna not powered", + "LNA Failure", + "Primary antenna open circuit", + "Primary antenna short circuit", + "CPU overload", + "COM port tx buffer overrun", + "", + "", + "Link overrun", + "Input overrun", + "Aux transmit overrun", + "Antenna gain out of range", + "Jammer detected", + "INS reset", + "IMU communication failure", + "GPS almanac invalid", + "Position solution invalid", + "Position fixed", + "Clock steering disabled", + "Clock model invalid", + "External oscillator locked", + "Software resource warning", + "", + "Interpret Status/Error Bits as Oem7 format", + "Tracking mode: HDR", + "Digital filtering enabled", + "Aux3 event", + "Aux2 event", + "Aux1 event" + }; + + /** Auxiliary 1 Status strings - refer to Oem7 manual. */ + const str_vector_t AUX1_STATUS_STRS + { + "Jammer detected on RF1", + "Jammer detected on RF2", + "Jammer detected on RF3", + "Position averaging On", + "Jammer detected on RF4", + "Jammer detected on RF5", + "Jammer detected on RF6", + "USB not connected", + "USB1 buffer overrun", + "USB2 buffer overrun", + "USB3 buffer overrun", + "", + "Profile Activation Error", + "Throttled Ethernet Reception", + "", + "", + "", + "", + "Ethernet not connected", + "ICOM1 buffer overrun", + "ICOM2 buffer overrun", + "ICOM3 buffer overrun", + "NCOM1 buffer overrun", + "NCOM2 buffer overrun", + "NCOM3 buffer overrun", + "", + "", + "", + "", + "", + "", + "IMU measurement outlier detected" + }; + + /** Auxiliary 2 Status strings - refer to Oem7 manual. */ + const str_vector_t AUX2_STATUS_STRS + { + "SPI Communication Failure", + "I2C Communication Failure", + "COM4 buffer overrun", + "COM5 buffer overrun", + "", + "", + "", + "", + "", + "COM1 buffer overrun", + "COM2 buffer overrun", + "COM3 buffer overrun", + "PLL RF1 unlock", + "PLL RF2 unlock", + "PLL RF3 unlock", + "PLL RF4 unlock", + "PLL RF5 unlock", + "PLL RF6 unlock", + "CCOM1 buffer overrun", + "CCOM2 buffer overrun", + "CCOM3 buffer overrun", + "CCOM4 buffer overrun", + "CCOM5 buffer overrun", + "CCOM6 buffer overrun", + "ICOM4 buffer overrun", + "ICOM5 buffer overrun", + "ICOM6 buffer overrun", + "ICOM7 buffer overrun", + "Secondary antenna not powered", + "Secondary antenna open circuit", + "Secondary antenna short circuit", + "Reset loop detected", + }; + + /** Auxiliary 3 Status strings - refer to Oem7 manual. */ + const str_vector_t AUX3_STATUS_STRS + { + "SCOM buffer overrun", + "WCOM1 buffer overrun", + "FILE buffer overrun", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "Web content is corrupt or does not exist", + "RF Calibration Data is present and in error", + "RF Calibration data exists and has no errors" + }; + + const str_vector_t AUX4_STATUS_STRS + { + "<60% of available satellites are tracked well", + "<15% of available satellites are tracked well", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "", + "Clock freewheeling due to bad position integrity", + "", + "Usable RTK Corrections: < 60%", + "Usable RTK Corrections: < 15%", + "Bad RTK Geometry", + "", + "", + "Long RTK Baseline", + "Poor RTK COM link", + "Poor ALIGN COM link", + "GLIDE Not Active", + "Bad PDP Geometry", + "No TerraStar Subscription", + "", + "", + "", + "Bad PPP Geometry", + "", + "No INS Alignment", + "INS not converged" + }; + + + void + get_status_info( + uint32_t bitmask, + const str_vector_t& str_map, + str_vector_t& str_list, + std::vector& bit_list) + { + for(int bit = 0; + bit < (sizeof(bitmask) * 8); + bit++) + { + if(bitmask & (1 << bit)) + { + bit_list.push_back(bit); + if(str_map[bit].length() > 0) + { + str_list.push_back(str_map[bit]); + } + } + } + } + + + /*** Handles RXSTATUS messages */ + class ReceiverStatusHandler: public Oem7MessageHandlerIf + { + Oem7RosPublisher RXSTATUS_pub_; + + std::string frame_id_; + + public: + ReceiverStatusHandler() + { + static const size_t NUM_BITS = sizeof(uint32_t) * 8; + assert(RECEIVER_ERROR_STRS.size() == NUM_BITS); + assert(AUX1_STATUS_STRS.size() == NUM_BITS); + assert(AUX2_STATUS_STRS.size() == NUM_BITS); + assert(AUX3_STATUS_STRS.size() == NUM_BITS); + assert(AUX4_STATUS_STRS.size() == NUM_BITS); + } + + ~ReceiverStatusHandler() + { + } + + void initialize(ros::NodeHandle& nh) + { + RXSTATUS_pub_.setup("RXSTATUS", nh); + } + + const std::vector& getMessageIds() + { + static const std::vector MSG_IDS({RXSTATUS_OEM7_MSGID}); + return MSG_IDS; + } + + void handleMsg(Oem7RawMessageIf::ConstPtr msg) + { + boost::shared_ptr rxstatus; + MakeROSMessage(msg, rxstatus); + + // Populate status strings: + get_status_info(rxstatus->error, RECEIVER_ERROR_STRS, rxstatus->error_strs, rxstatus->error_bits); + get_status_info(rxstatus->rxstat, RECEIVER_STATUS_STRS, rxstatus->rxstat_strs, rxstatus->rxstat_bits); + get_status_info(rxstatus->aux1_stat, AUX1_STATUS_STRS, rxstatus->aux1_stat_strs, rxstatus->aux1_stat_bits); + get_status_info(rxstatus->aux2_stat, AUX2_STATUS_STRS, rxstatus->aux2_stat_strs, rxstatus->aux2_stat_bits); + get_status_info(rxstatus->aux3_stat, AUX3_STATUS_STRS, rxstatus->aux3_stat_strs, rxstatus->aux3_stat_bits); + get_status_info(rxstatus->aux4_stat, AUX4_STATUS_STRS, rxstatus->aux4_stat_strs, rxstatus->aux4_stat_bits); + + RXSTATUS_pub_.publish(rxstatus); + } + }; + + + +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::ReceiverStatusHandler, novatel_oem7_driver::Oem7MessageHandlerIf) diff --git a/src/novatel_oem7_driver/src/time_handler.cpp b/src/novatel_oem7_driver/src/time_handler.cpp new file mode 100644 index 0000000..b9c0f11 --- /dev/null +++ b/src/novatel_oem7_driver/src/time_handler.cpp @@ -0,0 +1,77 @@ +//////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2020 NovAtel Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +#include + +#include + +#include "novatel_oem7_msgs/TIME.h" + +namespace novatel_oem7_driver +{ + class TimeHandler: public Oem7MessageHandlerIf + { + Oem7RosPublisher TIME_pub_; + + + void publishTIME(Oem7RawMessageIf::ConstPtr msg) + { + boost::shared_ptr time; + MakeROSMessage(msg, time); + TIME_pub_.publish(time); + } + + public: + TimeHandler() + { + } + + ~TimeHandler() + { + } + + void initialize(ros::NodeHandle& nh) + { + TIME_pub_.setup("TIME", nh); + } + + const std::vector& getMessageIds() + { + static const std::vector MSG_IDS({TIME_OEM7_MSGID}); + return MSG_IDS; + } + + void handleMsg(Oem7RawMessageIf::ConstPtr msg) + { + publishTIME(msg); + } + }; +} + +#include +PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::TimeHandler, novatel_oem7_driver::Oem7MessageHandlerIf) diff --git a/src/novatel_oem7_driver/test/align.bag b/src/novatel_oem7_driver/test/align.bag new file mode 100644 index 0000000..26cc721 Binary files /dev/null and b/src/novatel_oem7_driver/test/align.bag differ diff --git a/src/novatel_oem7_driver/test/align.gps b/src/novatel_oem7_driver/test/align.gps new file mode 100644 index 0000000..1869287 Binary files /dev/null and b/src/novatel_oem7_driver/test/align.gps differ diff --git a/src/novatel_oem7_driver/test/align.test b/src/novatel_oem7_driver/test/align.test new file mode 100644 index 0000000..d7f9463 --- /dev/null +++ b/src/novatel_oem7_driver/test/align.test @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/src/novatel_oem7_driver/test/bestpos.bag b/src/novatel_oem7_driver/test/bestpos.bag new file mode 100644 index 0000000..f15df47 Binary files /dev/null and b/src/novatel_oem7_driver/test/bestpos.bag differ diff --git a/src/novatel_oem7_driver/test/bestpos.gps b/src/novatel_oem7_driver/test/bestpos.gps new file mode 100644 index 0000000..a87386f Binary files /dev/null and b/src/novatel_oem7_driver/test/bestpos.gps differ diff --git a/src/novatel_oem7_driver/test/bestpos.test b/src/novatel_oem7_driver/test/bestpos.test new file mode 100644 index 0000000..4b78768 --- /dev/null +++ b/src/novatel_oem7_driver/test/bestpos.test @@ -0,0 +1,12 @@ + + + + + + + diff --git a/src/novatel_oem7_driver/test/ins1.bag b/src/novatel_oem7_driver/test/ins1.bag new file mode 100644 index 0000000..e572c75 Binary files /dev/null and b/src/novatel_oem7_driver/test/ins1.bag differ diff --git a/src/novatel_oem7_driver/test/ins1.gps b/src/novatel_oem7_driver/test/ins1.gps new file mode 100644 index 0000000..77d1f28 Binary files /dev/null and b/src/novatel_oem7_driver/test/ins1.gps differ diff --git a/src/novatel_oem7_driver/test/ins1.test b/src/novatel_oem7_driver/test/ins1.test new file mode 100644 index 0000000..8207c4d --- /dev/null +++ b/src/novatel_oem7_driver/test/ins1.test @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/src/novatel_oem7_driver/test/ins2.bag b/src/novatel_oem7_driver/test/ins2.bag new file mode 100644 index 0000000..1f23ad3 Binary files /dev/null and b/src/novatel_oem7_driver/test/ins2.bag differ diff --git a/src/novatel_oem7_driver/test/ins2.gps b/src/novatel_oem7_driver/test/ins2.gps new file mode 100644 index 0000000..891651d Binary files /dev/null and b/src/novatel_oem7_driver/test/ins2.gps differ diff --git a/src/novatel_oem7_driver/test/ins2.test b/src/novatel_oem7_driver/test/ins2.test new file mode 100644 index 0000000..dd82627 --- /dev/null +++ b/src/novatel_oem7_driver/test/ins2.test @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/src/novatel_oem7_driver/test/oem7_bist.py b/src/novatel_oem7_driver/test/oem7_bist.py new file mode 100755 index 0000000..394957a --- /dev/null +++ b/src/novatel_oem7_driver/test/oem7_bist.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +import unittest +import rospy +import os, sys + +import oem7_message_test + +PKG = 'novatel_oem7_driver' +NAME = 'oem7_bist' + +class Oem7BIST(unittest.TestCase): + def __init__(self, *args): + super(self.__class__, self).__init__(*args) + rospy.init_node(NAME) + + + def test_1_recording(self): + + if os.path.exists(self.bist_bag): + os.remove(self.bist_bag) + + delay_sec = float(rospy.get_param('~duration', 60.)) + print("Sleeping for {0} sec to allow topics to be recorded...".format(delay_sec)) + rospy.sleep(delay_sec) + print("..done") + + def test_2_analysis(self): + oem7_message_test.analyze_hz(self.bist_bag, output_csv = False) + # Set output_csv = True to dump topic statistics to .csv file. + + +if __name__ == '__main__': + + Oem7BIST.bist_bag = sys.argv[1] + + import rostest + rostest.run(PKG, NAME, Oem7BIST, sys.argv) + diff --git a/src/novatel_oem7_driver/test/oem7_decoder_test.py b/src/novatel_oem7_driver/test/oem7_decoder_test.py new file mode 100644 index 0000000..5ee8e14 --- /dev/null +++ b/src/novatel_oem7_driver/test/oem7_decoder_test.py @@ -0,0 +1,87 @@ +import unittest +import rostest +import rosbag + +import os + +import traceback +from docutils.nodes import topic + +def get_topic_list(bag_name): + """ Return a list of topics stored in this bag """ + bag = rosbag.Bag(bag_name, 'r') + return bag.get_type_and_topic_info()[1].keys() + +def make_msg_gen(bag_name, topic): + """ Generates a sequence of messages in the topic from the bag """ + bag = rosbag.Bag(bag_name, 'r') + for top, msg, t in bag.read_messages(): + if top == topic: + yield msg + + + +def compare(ref_msg, uut_msg): + # Supress seqno, timestamp + ref_msg.header.seq = 0 + uut_msg.header.seq = 0 + + ref_msg.header.stamp = None + uut_msg.header.stamp = None + + if(ref_msg != uut_msg): + print("Messages do not match:") + print("Reference:------------") + print(ref_msg) + print("\n\n\n") + print("UUT-------------") + print(uut_msg) + return False; + + return True + + +def verify_bag_equivalency(ref_bag, uut_bag): + """ + Verifies that two bags contain semantically identical sequence of messages. + """ + + ref_topics = get_topic_list(ref_bag) + print(ref_topics) + for topic in ref_topics: + msgno = 0 + uut_gen = make_msg_gen(uut_bag, topic) + ref_gen = make_msg_gen(ref_bag, topic) + for ref_msg in ref_gen: + uut_msg = next(uut_gen) + if not compare(ref_msg, uut_msg): + print("\n\n") + print("Topic: {} Msg No: {}".format(topic, msgno)) + assert False + + msgno += 1 + + print("Verified {} '{}' messages".format(msgno, topic)) + # Check for presence of unexpected messages + unexpected_messages = 0 + try: + while True: + uut_top, uut_msg, uut_t = next(uut_gen) + print("Unexpected message") + print(uut_msg) + unexpected_messages += 1 + + except StopIteration: + pass # Normal + + except: + traceback.print_exc() + assert(False) + + assert(unexpected_messages == 0) + + + + + + \ No newline at end of file diff --git a/src/novatel_oem7_driver/test/oem7_message_test.py b/src/novatel_oem7_driver/test/oem7_message_test.py new file mode 100644 index 0000000..20edae2 --- /dev/null +++ b/src/novatel_oem7_driver/test/oem7_message_test.py @@ -0,0 +1,121 @@ +""" +Message Hz analysis. + +Existing ros_comm hz tests anaylyzes a single topic at a time; +This script analyzes multiple topics in a Bag. +""" + +import os +import traceback + +import numpy as np + +import rosbag +from docutils.nodes import topic + + + +def get_topic_list(bag_name): + """ Return a list of topics stored in this bag """ + bag = rosbag.Bag(bag_name, 'r') + return bag.get_type_and_topic_info()[1].keys() + +def make_msg_gen(bag_name, topic): + """ Generates a sequence of messages in the topic from the bag """ + bag = rosbag.Bag(bag_name, 'r') + for top, msg, time in bag.read_messages(): + if top == topic: + yield time, msg + + +def make_timestamp_gen(msg_itr): + for t, m in msg_itr: + yield t.to_sec(), m.header.stamp.to_sec() + + + + +def analyze_topic_hz(name, topic, exp_int, output_csv): + """ + Analyzes message interval for a particular topic. + """ + + bag_name = name + ".bag" + bag_path = os.path.expanduser("~") + "/.ros/" + bag_name + + msg_gen = make_msg_gen(bag_path, topic) + ts_gen = make_timestamp_gen(msg_gen) + + # Array of timestamps: Bag ts (recording time), Message ts (generation time), Delta: rec ts - gen ts + ts_arr_ = [[bag_ts, msg_ts, bag_ts - msg_ts] for bag_ts, msg_ts in ts_gen] # sigh... + + if len(ts_arr_) < 3: + print("Topic: {}: Insufficient data: {} samples".format(topic, len(ts_arr_))) + return False + + # Convert to numpy array + ts_arr = np.array(ts_arr_) + + # Delta: time between message generated by the driver and bag recording (i.e. publishign overhead) + d_arr = ts_arr[:, -1] # Allow rows in the Delta column + d_mean = np.mean(d_arr) + d_max = np.max( d_arr) + d_std = np.std( d_arr) + + ROWS = 0 # Axis + # Interval: time between individual message handling: generation by driver; bag recording. + int_arr = np.diff(ts_arr[:,:-1], axis = ROWS) # Intervals between Bag timestampts, publish timestamps, + # i.e. message arrival interval: ['bag interval', 'publish interval'] + + int_mean = np.mean(int_arr, axis = ROWS) + int_max = np.amax(int_arr, axis = ROWS) + int_std = np.std( int_arr, axis = ROWS) + + BAG = 0 + MSG = 1 + print("topic: '{}', exp interval= {}. samples= {}: ".format(topic, exp_int, len(ts_arr_))) + print(" mean, max, stdev") + print(" Bag Recording interval: {}, {}, {}".format(int_mean[BAG], int_max[BAG], int_std[BAG])) + print(" Message publish interval: {}, {}, {}".format(int_mean[MSG], int_max[MSG], int_std[MSG])) + print(" Bag Rec - Message Pub Delta: {}, {}, {}".format(d_mean, d_max, d_std)) + + if output_csv: + # Dump into .csv for additional post-processing. + topic_filename = topic.replace("/", "_") + np.savetxt(name + "." + topic_filename + ".csv", int_arr, delimiter=",") + + +""" +Topic configuration; refer to std_init_commands.yaml. +""" +NS = "/novatel/oem7/" +topic_config = \ + { + # Topic : Rate + NS + "bestpos": 0.1, + NS + "bestvel": 0.1, + NS + "bestutm": 1.0, + NS + "time": 1.0, + NS + "corrimu": 0.01, + NS + "inspva": 0.02, + NS + "inspvax": 1.0, + NS + "insstdev": 1.0, + "/gps/gps": 0.02, # Same rate as INSPVA + "/gps/fix": 0.02, # Same rate as gps + "/gps/imu": 0.01 # Same as corrimu + } + + + +def analyze_hz(bag_name, output_csv): + """ + Tests message frequency. + Analyzes message intervals based on Message timestamp and Bag recording timestamp. + """ + + for topic in topic_config: + analyze_topic_hz(bag_name, topic, topic_config[topic], output_csv) + print "" + + + diff --git a/src/novatel_oem7_driver/test/oem7_publish_test.py b/src/novatel_oem7_driver/test/oem7_publish_test.py new file mode 100755 index 0000000..80c9f9c --- /dev/null +++ b/src/novatel_oem7_driver/test/oem7_publish_test.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +import unittest +import rospy +import sys +import os + +import oem7_decoder_test +#from __builtin__ import None + +PKG = 'novatel_oem7_driver' +NAME = 'publishtest' + +class Oem7PublishTest(unittest.TestCase): + def __init__(self, *args): + + super(self.__class__, self).__init__(*args) + rospy.init_node(NAME) + + def test_1_record_uut(self): + """ + Purge existing bag, record new one. + + Fails if stale file cannot be purged. + """ + + if os.path.exists(self.uut_bag): + os.remove(self.uut_bag) + + delay_sec = float(rospy.get_param('~delay_sec', 10.)) + rospy.sleep(delay_sec) + + def test_2_verify_recorded_topics(self): + oem7_decoder_test.verify_bag_equivalency(self.ref_bag, self.uut_bag) + + +if __name__ == '__main__': + import rostest + + Oem7PublishTest.ref_bag = sys.argv[1] + ".bag" + Oem7PublishTest.uut_bag = sys.argv[2] + "-test.bag" + + rostest.run(PKG, NAME, Oem7PublishTest, sysargs = sys.argv) + diff --git a/src/novatel_oem7_driver/test/publish_test.xml b/src/novatel_oem7_driver/test/publish_test.xml new file mode 100644 index 0000000..b3d3140 --- /dev/null +++ b/src/novatel_oem7_driver/test/publish_test.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/novatel_oem7_driver/test/rxstatus.bag b/src/novatel_oem7_driver/test/rxstatus.bag new file mode 100644 index 0000000..beabc08 Binary files /dev/null and b/src/novatel_oem7_driver/test/rxstatus.bag differ diff --git a/src/novatel_oem7_driver/test/rxstatus.gps b/src/novatel_oem7_driver/test/rxstatus.gps new file mode 100644 index 0000000..f71a8ff Binary files /dev/null and b/src/novatel_oem7_driver/test/rxstatus.gps differ diff --git a/src/novatel_oem7_driver/test/rxstatus.test b/src/novatel_oem7_driver/test/rxstatus.test new file mode 100644 index 0000000..b79c496 --- /dev/null +++ b/src/novatel_oem7_driver/test/rxstatus.test @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/novatel_oem7_driver/test/time.bag b/src/novatel_oem7_driver/test/time.bag new file mode 100644 index 0000000..984f181 Binary files /dev/null and b/src/novatel_oem7_driver/test/time.bag differ diff --git a/src/novatel_oem7_driver/test/time.gps b/src/novatel_oem7_driver/test/time.gps new file mode 100644 index 0000000..b43dda6 Binary files /dev/null and b/src/novatel_oem7_driver/test/time.gps differ diff --git a/src/novatel_oem7_driver/test/time.test b/src/novatel_oem7_driver/test/time.test new file mode 100644 index 0000000..7e1c760 --- /dev/null +++ b/src/novatel_oem7_driver/test/time.test @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/novatel_oem7_msgs/CMakeLists.txt b/src/novatel_oem7_msgs/CMakeLists.txt new file mode 100644 index 0000000..8e6217c --- /dev/null +++ b/src/novatel_oem7_msgs/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 2.8.3) +project(novatel_oem7_msgs) + +set(MSG_DEPS + std_msgs) + +find_package(catkin REQUIRED COMPONENTS + message_generation + ${MSG_DEPS} +) + +add_service_files( + FILES + Oem7AbasciiCmd.srv +) + +add_message_files(DIRECTORY msg FILES + Oem7RawMsg.msg + Oem7Header.msg + BESTPOS.msg + BESTUTM.msg + BESTVEL.msg + HEADING2.msg + INSCONFIG.msg + INSPVA.msg + INSPVAX.msg + INSSTDEV.msg + CORRIMU.msg + IMURATECORRIMU.msg + RXSTATUS.msg + TIME.msg + INSExtendedSolutionStatus.msg + INSFrame.msg + INSReceiverStatus.msg + INSResetType.msg + INSSourceStatus.msg + INSUpdate.msg + BestExtendedSolutionStatus.msg + InertialSolutionStatus.msg + INSAlignmentMode.msg + INSOutputFrame.msg + PositionOrVelocityType.msg + Rotation.msg + RotationalOffset.msg + SolutionStatus.msg + SolutionSource.msg + Translation.msg + TranslationOffset.msg +) +generate_messages(DEPENDENCIES ${MSG_DEPS}) +catkin_package( + CATKIN_DEPENDS message_runtime ${MSG_DEPS} +) diff --git a/src/novatel_oem7_msgs/msg/BESTPOS.msg b/src/novatel_oem7_msgs/msg/BESTPOS.msg new file mode 100644 index 0000000..2405d53 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/BESTPOS.msg @@ -0,0 +1,28 @@ +Header header +Oem7Header nov_header +SolutionStatus sol_status +PositionOrVelocityType pos_type +float64 lat +float64 lon +float64 hgt +float32 undulation +uint32 datum_id +float32 lat_stdev +float32 lon_stdev +float32 hgt_stdev +string stn_id +float32 diff_age +float32 sol_age +uint8 num_svs +uint8 num_sol_svs +uint8 num_sol_l1_svs +uint8 reserved +BestExtendedSolutionStatus ext_sol_stat +uint8 galileo_beidou_sig_mask +uint8 gps_glonass_sig_mask + + + + + + diff --git a/src/novatel_oem7_msgs/msg/BESTUTM.msg b/src/novatel_oem7_msgs/msg/BESTUTM.msg new file mode 100644 index 0000000..4d7f17f --- /dev/null +++ b/src/novatel_oem7_msgs/msg/BESTUTM.msg @@ -0,0 +1,26 @@ +Header header +Oem7Header nov_header +SolutionStatus sol_status +PositionOrVelocityType pos_type +uint32 lon_zone_number +uint32 lat_zone_letter +float64 northing +float64 easting +float64 height +float32 undulation +uint32 datum_id +float32 northing_stddev +float32 easting_stddev +float32 height_stddev +string stn_id +float32 diff_age +float32 sol_age +uint8 num_svs +uint8 num_sol_svs +uint8 num_sol_ggl1_svs +uint8 num_sol_multi_svs +uint8 reserved +BestExtendedSolutionStatus ext_sol_stat +uint8 galileo_beidou_sig_mask +uint8 gps_glonass_sig_mask + diff --git a/src/novatel_oem7_msgs/msg/BESTVEL.msg b/src/novatel_oem7_msgs/msg/BESTVEL.msg new file mode 100644 index 0000000..40250d1 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/BESTVEL.msg @@ -0,0 +1,11 @@ +Header header +Oem7Header nov_header +SolutionStatus sol_status +PositionOrVelocityType vel_type +float32 latency +float32 diff_age +float64 hor_speed +float64 trk_gnd +float64 ver_speed +float32 reserved + diff --git a/src/novatel_oem7_msgs/msg/BestExtendedSolutionStatus.msg b/src/novatel_oem7_msgs/msg/BestExtendedSolutionStatus.msg new file mode 100644 index 0000000..5f8227f --- /dev/null +++ b/src/novatel_oem7_msgs/msg/BestExtendedSolutionStatus.msg @@ -0,0 +1,26 @@ +# Bit 0 +uint8 RTK_SOLUTION_VERIFIED = 1 +uint8 PDP_SOLUTION_IS_GLIDE = 1 + +# Bits 1-3, mask 0xE +uint8 KLOBUCHAR_BROADCAST = 2 # 0x2 +uint8 SBAS_BROADCAST = 4 # 0x4 +uint8 MULTI_FREQUENCY_COMPUTED = 6 # 0x6 +uint8 PSRDIFF_CORRECTION = 8 # 0x8 +uint8 NOVATEL_BLENDED_IONO_VALUE = 10 #0xA + +# Bit 4 +uint8 RTK_ASSIST_ACTIVE = 16 # 0x10 + +# Bit 5 +uint8 ANTENNA_INFORMATION_IS_MISSING = 32 # 0x20 + +# Bit 6 reserved +uint8 RESERVED = 64 # 0x40 + +# Bit 7 +uint8 POSITION_INCLUDES_TERRAIN_COMPENSATION_CORRECTIONS = 128 # 0x80 + + + +uint8 status \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/CORRIMU.msg b/src/novatel_oem7_msgs/msg/CORRIMU.msg new file mode 100644 index 0000000..ac43ce8 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/CORRIMU.msg @@ -0,0 +1,11 @@ +Header header +Oem7Header nov_header +uint32 imu_data_count +float64 pitch_rate +float64 roll_rate +float64 yaw_rate +float64 lateral_acc +float64 longitudinal_acc +float64 vertical_acc +uint32 reserved1 +uint32 reserved2 \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/HEADING2.msg b/src/novatel_oem7_msgs/msg/HEADING2.msg new file mode 100644 index 0000000..cd78e27 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/HEADING2.msg @@ -0,0 +1,24 @@ +Header header +Oem7Header nov_header +SolutionStatus sol_status +PositionOrVelocityType pos_type +float32 length +float32 heading +float32 pitch +float32 reserved +float32 heading_stdev +float32 pitch_stdev +string rover_stn_id +string master_stn_id +uint8 num_sv_tracked +uint8 num_sv_in_sol +uint8 num_sv_obs +uint8 num_sv_multi +SolutionSource sol_source +BestExtendedSolutionStatus ext_sol_status +uint8 galileo_beidou_sig_mask +uint8 gps_glonass_sig_mask + + + + diff --git a/src/novatel_oem7_msgs/msg/IMURATECORRIMU.msg b/src/novatel_oem7_msgs/msg/IMURATECORRIMU.msg new file mode 100644 index 0000000..299d12e --- /dev/null +++ b/src/novatel_oem7_msgs/msg/IMURATECORRIMU.msg @@ -0,0 +1,10 @@ +Header header +Oem7Header nov_header +uint32 week +float64 seconds +float64 pitch_rate +float64 roll_rate +float64 yaw_rate +float64 lateral_acc +float64 longitudinal_acc +float64 vertical_acc \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSAlignmentMode.msg b/src/novatel_oem7_msgs/msg/INSAlignmentMode.msg new file mode 100644 index 0000000..6634eb7 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSAlignmentMode.msg @@ -0,0 +1,9 @@ + + +uint32 UNAIDED = 0 +uint32 AIDED_TRANSFER = 2 +uint32 AUTOMATIC = 3 +uint32 STATIC = 4 +uint32 KINEMATIC = 5 + +uint32 mode diff --git a/src/novatel_oem7_msgs/msg/INSCONFIG.msg b/src/novatel_oem7_msgs/msg/INSCONFIG.msg new file mode 100644 index 0000000..4b8c628 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSCONFIG.msg @@ -0,0 +1,33 @@ +Header header +Oem7Header nov_header +uint32 imu_type +uint8 mapping +uint8 initial_alignment_velocity +uint16 heave_window +uint32 profile +uint8[4] enabled_updates +INSAlignmentMode alignment_mode +INSOutputFrame relative_ins_output_frame +bool relative_ins_output_direction +INSReceiverStatus ins_receiver_status +uint8 ins_seed_enabled +uint8 ins_seed_validation +uint16 reserved_1 +uint32 reserved_2 +uint32 reserved_3 +uint32 reserved_4 +uint32 reserved_5 +uint32 reserved_6 +uint32 reserved_7 +uint32 number_of_translations +Translation[] translations +uint32 number_of_rotations +Rotation[] rotations + + + + + + + + diff --git a/src/novatel_oem7_msgs/msg/INSExtendedSolutionStatus.msg b/src/novatel_oem7_msgs/msg/INSExtendedSolutionStatus.msg new file mode 100644 index 0000000..11ddca6 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSExtendedSolutionStatus.msg @@ -0,0 +1,56 @@ + +uint32 POSITION_UPDATE = 1 # 0x00000001 +uint32 PHASE_UPDATE = 2 # 0x00000002 +uint32 ZERO_VELOCITY_UPDATE = 4 # 0x00000004 +uint32 WHEEL_SEONSOR_UPDATE = 8 # 0x00000008 +uint32 ALIGN_UPDATE = 16 # 0x00000010 +uint32 EXTERNAL_POSITION_UPDATE = 32 # 0x00000020 +uint32 INS_SOLUTIN_CONVERGENCE_FLAG = 64 # 0x00000040 +uint32 DOPPLER_UPDATE = 128 # 0x00000080 +uint32 PSEUDORANGE_UPDATE = 256 # 0x00000100 +uint32 VELOCITY_UPDATE = 512 # 0x00000200 +uint32 RESERVED_1 = 1024 # 0x00000400 +uint32 DEAD_RECONING_UPDATE = 2048 # 0x00000800 +uint32 PHASE_WIND_UP_UPDATE = 4096 # 0x00001000 +uint32 COURSE_OVER_GROUND_UPDATE = 8192 # 0x00002000 +uint32 EXTERNAL_VELOCITY_UPDATE = 16384 # 0x00004000 +uint32 EXTERNAL_ATTITUDE_UPDATE = 32768 # 0x00008000 +uint32 EXTERNAL_HEADING_UPDATE = 65535 # 0x00010000 +uint32 EXTERNAL_HEIGHT_UPDATE = 131072 # 0x00020000 +uint32 RESERVED_2 = 262144 # 0x00040000 +uint32 RESERVED_3 = 524288 # 0x00080000 +uint32 ROVER_POSITION_UPDATE = 1048576 # 0x00100000 +uint32 ROVER_POSITION_UPDATE_TYPE = 2097152 # 0x00200000 + +uint32 RESERVED_4 = 4194304 # 0x00400000 +uint32 RESERVED_5 = 8388608 # 0x00800000 +uint32 TURN_ON_BIASES_ESTIMATED = 16777216 # 0x01000000 +uint32 ALIGNMENT_DIRECTION_VERIFIED = 33554432 # 0x02000000 +uint32 ALIGNMENT_INDICATION_1 = 67108864 # 0x04000000 +uint32 ALIGNMENT_INDICATION_2 = 134217728 # 0x08000000 +uint32 ALIGNMENT_INDICATION_3 = 268435456 # 0x10000000 +uint32 NVM_SEED_INDICATION_1 = 538870912 # 0x20000000 +uint32 NVM_SEED_INDICATION_2 = 1073741824 # 0x40000000 +uint32 NVM_SEED_INDICATION_3 = 2147483648 # 0x80000000 + + + +# Alignment indication +uint32 ALIGNMENT_INCOMPLETE_ALIGNMENT = 0 +uint32 ALIGNMENT_STATIC = 1 +uint32 ALIGNMENT_KINETMATIC = 2 +uint32 ALIGNMENT_DUAL_ANTENNA = 3 +uint32 ALIGNMENT_USER_COMMAND = 4 +uint32 ALIGNMENT_NVM_SEED = 5 + +# NVM Seed Indication +uint32 NVM_SEED_INACTIVE = 0 +uint32 NVM_SEED_STORED_INVALID = 1 +uint32 NVM_SEED_PENDING_VALIDATION = 2 +uint32 NVM_SEED_INJECTED = 4 +uint32 NVM_SEEED_DATA_IGNORED = 5 +uint32 NVM_SEED_ERROR_MODEL_DATA_INJECTED = 6 + + + +uint32 status diff --git a/src/novatel_oem7_msgs/msg/INSFrame.msg b/src/novatel_oem7_msgs/msg/INSFrame.msg new file mode 100644 index 0000000..84bdf04 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSFrame.msg @@ -0,0 +1,4 @@ +uint32 IMUBODY = 0 +uint32 VEHICLE = 1 + +uint32 frame \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSOutputFrame.msg b/src/novatel_oem7_msgs/msg/INSOutputFrame.msg new file mode 100644 index 0000000..2a7fbb0 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSOutputFrame.msg @@ -0,0 +1,7 @@ + +uint32 ROVER = 1 +uint32 MASTER = 2 +uint32 ECEF = 3 +uint32 LOCALEVEL = 4 + +uint32 frame \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSPVA.msg b/src/novatel_oem7_msgs/msg/INSPVA.msg new file mode 100644 index 0000000..9177f9d --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSPVA.msg @@ -0,0 +1,12 @@ +Header header +Oem7Header nov_header +float64 latitude +float64 longitude +float64 height +float64 north_velocity +float64 east_velocity +float64 up_velocity +float64 roll +float64 pitch +float64 azimuth +InertialSolutionStatus status \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSPVAX.msg b/src/novatel_oem7_msgs/msg/INSPVAX.msg new file mode 100644 index 0000000..070c29f --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSPVAX.msg @@ -0,0 +1,25 @@ +Header header +Oem7Header nov_header +InertialSolutionStatus ins_status +PositionOrVelocityType pos_type +float64 latitude +float64 longitude +float64 height +float32 undulation +float64 north_velocity +float64 east_velocity +float64 up_velocity +float64 roll +float64 pitch +float64 azimuth +float32 latitude_stdev +float32 longitude_stdev +float32 height_stdev +float32 north_velocity_stdev +float32 east_velocity_stdev +float32 up_velocity_stdev +float32 roll_stdev +float32 pitch_stdev +float32 azimuth_stdev +INSExtendedSolutionStatus ext_sol_status +uint16 time_since_update \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSReceiverStatus.msg b/src/novatel_oem7_msgs/msg/INSReceiverStatus.msg new file mode 100644 index 0000000..d7144fa --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSReceiverStatus.msg @@ -0,0 +1,10 @@ + +uint8 INS_RESET_BYTE = 0 +# Refer to INSResetType for values + + +uint8 BYTE_1 = 1 +uint8 IMU_COMMUNICATION_ERROR = 1 + +# Bytes 3-4 are reserved +uint8[4] status \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSResetType.msg b/src/novatel_oem7_msgs/msg/INSResetType.msg new file mode 100644 index 0000000..42ba216 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSResetType.msg @@ -0,0 +1,21 @@ +uint8 INITIALIZED = 0 +uint8 INS_TIMING_ERROR = 1 +uint8 INVALID_SOLUTION = 2 +uint8 INS_ORIENT_CHANGED = 3 +uint8 INS_USER_COMMAND = 4 +uint8 INS_CALIBRATION = 5 +uint8 INIT_ATTITUDE_RECEIVED = 6 +uint8 ALIGNMENT_MODE_CHANGED = 8 +uint8 EXPT_RESTRICTS_EXCEEDED = 9 +uint8 DATA_GAP = 10 +uint8 RECEIVED_IMU_SPECS = 11 +uint8 ACCEL_RESTRICTIONS_EXCEEDED = 12 +uint8 ROTATION_RESTRICTIONS_EXCEEDED = 13 +uint8 RAWIMU_STATUS_INVALID = 14 +uint8 IMU_CONFIGURED = 15 +uint8 IMU_UNUSPPORTED = 16 +uint8 INS_SEED_INVALID = 17 +uint8 INS_SEED_VALIDATION_FAILED = 18 +uint8 USER_COMMAND_RESTART = 19 + +uint8 type \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSSTDEV.msg b/src/novatel_oem7_msgs/msg/INSSTDEV.msg new file mode 100644 index 0000000..379348a --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSSTDEV.msg @@ -0,0 +1,16 @@ +Header header +Oem7Header nov_header +float32 latitude_stdev +float32 longitude_stdev +float32 height_stdev +float32 north_velocity_stdev +float32 east_velocity_stdev +float32 up_velocity_stdev +float32 roll_stdev +float32 pitch_stdev +float32 azimuth_stdev +INSExtendedSolutionStatus ext_sol_status +uint16 time_since_last_update +uint16 reserved1 +uint32 reserved2 +uint32 reserved3 \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSSourceStatus.msg b/src/novatel_oem7_msgs/msg/INSSourceStatus.msg new file mode 100644 index 0000000..b38c734 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSSourceStatus.msg @@ -0,0 +1,11 @@ +uint32 FROM_NVM = 1 +uint32 CALIBRATING = 2 +uint32 CALIBRATED = 3 +uint32 FROM_COMMAND = 4 +uint32 RESET = 5 +uint32 FROM_DUAL_ANT = 6 +uint32 INS_CONVERSING = 7 +uint32 INSUFFICIENT_SPEED = 8 +uint32 HIGH_ROTATION = 9 + +uint32 status \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/INSUpdate.msg b/src/novatel_oem7_msgs/msg/INSUpdate.msg new file mode 100644 index 0000000..a1e78ea --- /dev/null +++ b/src/novatel_oem7_msgs/msg/INSUpdate.msg @@ -0,0 +1,4 @@ + + + +uint8[4] update \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/InertialSolutionStatus.msg b/src/novatel_oem7_msgs/msg/InertialSolutionStatus.msg new file mode 100644 index 0000000..0082f05 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/InertialSolutionStatus.msg @@ -0,0 +1,14 @@ + +uint32 INS_INACTIVE = 0 +uint32 INS_ALIGNING = 1 +uint32 INS_HIGH_VARIANCE = 2 +uint32 INS_SOLUTION_GOOD = 3 +uint32 INS_SOLUTION_FREE = 6 +uint32 INS_ALIGNMENT_COMPLETE = 7 +uint32 DETERMINING_ORIENTATION = 8 +uint32 WAITING_INITIAL_POS = 9 +uint32 WAITING_AZIMUTH = 10 +uint32 INITIALIZING_BIASES = 11 +uint32 MOTION_DETECT = 12 + +uint32 status \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/Oem7Header.msg b/src/novatel_oem7_msgs/msg/Oem7Header.msg new file mode 100644 index 0000000..24e7d93 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/Oem7Header.msg @@ -0,0 +1,11 @@ +uint16 OEM7MSGTYPE_LOG = 0 + + +string message_name +uint16 message_id +uint8 message_type +uint32 sequence_number +uint8 time_status +uint16 gps_week_number +uint32 gps_week_milliseconds + diff --git a/src/novatel_oem7_msgs/msg/Oem7RawMsg.msg b/src/novatel_oem7_msgs/msg/Oem7RawMsg.msg new file mode 100644 index 0000000..c45cf5f --- /dev/null +++ b/src/novatel_oem7_msgs/msg/Oem7RawMsg.msg @@ -0,0 +1,3 @@ +Header header +uint8[] message_data + diff --git a/src/novatel_oem7_msgs/msg/PositionOrVelocityType.msg b/src/novatel_oem7_msgs/msg/PositionOrVelocityType.msg new file mode 100644 index 0000000..897ad39 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/PositionOrVelocityType.msg @@ -0,0 +1,33 @@ +uint32 NONE = 0 +uint32 FIXEDPOS = 1 +uint32 FIXEDHEIGHT = 2 +uint32 DOPPLER_VELOCITY = 8 +uint32 SINGLE = 16 +uint32 PSRDIFF = 17 +uint32 WAAS = 18 +uint32 PROPAGATED = 19 +uint32 L1_FLOAT = 32 +uint32 NARROW_FLOAT = 34 +uint32 L1_INT = 48 +uint32 WIDE_INT = 49 +uint32 NARROW_INT = 50 +uint32 RTK_DIRECT_INS = 51 +uint32 INS_SBAS = 52 +uint32 INS_PSRSP = 53 +uint32 INS_PSRDIFF = 54 +uint32 INS_RTKFLOAT = 55 +uint32 INS_RTKFIXED = 56 +uint32 PPP_CONVERGING = 68 +uint32 PPP = 69 +uint32 OPERATIONAL = 70 +uint32 WARNING = 71 +uint32 OUT_OF_BOUNDS = 72 +uint32 INS_PPP_CONVERGING = 73 +uint32 INS_PPP = 74 +uint32 PPP_BASIC_CONVERGING = 77 +uint32 PPP_BASIC = 78 +uint32 INS_PPP_BASIC_CONVERGING = 79 +uint32 INS_PPP_BASIC = 80 + + +uint32 type \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/RXSTATUS.msg b/src/novatel_oem7_msgs/msg/RXSTATUS.msg new file mode 100644 index 0000000..ddb4b4f --- /dev/null +++ b/src/novatel_oem7_msgs/msg/RXSTATUS.msg @@ -0,0 +1,36 @@ +Header header +Oem7Header nov_header +uint32 error +uint32 num_status_codes +uint32 rxstat +uint32 rxstat_pri_mask +uint32 rxstat_set_mask +uint32 rxstat_clr_mask +uint32 aux1_stat +uint32 aux1_stat_pri +uint32 aux1_stat_set +uint32 aux1_stat_clr +uint32 aux2_stat +uint32 aux2_stat_pri +uint32 aux2_stat_set +uint32 aux2_stat_clr +uint32 aux3_stat +uint32 aux3_stat_pri +uint32 aux3_stat_set +uint32 aux3_stat_clr +uint32 aux4_stat +uint32 aux4_stat_pri +uint32 aux4_stat_set +uint32 aux4_stat_clr +uint8[] error_bits +string[] error_strs +uint8[] rxstat_bits +string[] rxstat_strs +uint8[] aux1_stat_bits +string[] aux1_stat_strs +uint8[] aux2_stat_bits +string[] aux2_stat_strs +uint8[] aux3_stat_bits +string[] aux3_stat_strs +uint8[] aux4_stat_bits +string[] aux4_stat_strs diff --git a/src/novatel_oem7_msgs/msg/Rotation.msg b/src/novatel_oem7_msgs/msg/Rotation.msg new file mode 100644 index 0000000..bae5230 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/Rotation.msg @@ -0,0 +1,9 @@ +RotationalOffset rotation +INSFrame frame +float32 x_rotation +float32 y_rotation +float32 z_rotation +float32 x_rotation_stdev +float32 y_rotation_stdev +float32 z_rotation_stdev +INSSourceStatus rotation_source \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/RotationalOffset.msg b/src/novatel_oem7_msgs/msg/RotationalOffset.msg new file mode 100644 index 0000000..3307a14 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/RotationalOffset.msg @@ -0,0 +1,10 @@ +uint32 USER = 4 +uint32 MARK1 = 5 +uint32 MARK2 = 6 +uint32 ALIGN = 8 +uint32 MARK3 = 9 +uint32 MARK4 = 10 +uint32 RBV = 11 +uint32 RBM = 12 + +uint32 offset \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/SolutionSource.msg b/src/novatel_oem7_msgs/msg/SolutionSource.msg new file mode 100644 index 0000000..f3777d8 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/SolutionSource.msg @@ -0,0 +1,9 @@ +uint8 RESERVED1 = 3 # 0x3 + +uint8 SOURCE_ANTENNA_MASK = 12 # 0xC +uint8 PRIMARY_ANTENNA = 0 +uint8 SECONDARY_ANTENNA = 0 + +uint8 RESERVED2 = 240 # 0xF0 + +uint8 source \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/SolutionStatus.msg b/src/novatel_oem7_msgs/msg/SolutionStatus.msg new file mode 100644 index 0000000..94e90ba --- /dev/null +++ b/src/novatel_oem7_msgs/msg/SolutionStatus.msg @@ -0,0 +1,17 @@ +uint32 SOL_COMPUTED = 0 +uint32 INSUFFICIENT_OBS = 1 +uint32 NO_CONVERGECE = 2 +uint32 SINGULARITY = 3 +uint32 COV_TRACE = 4 +uint32 TEST_DIST = 5 +uint32 COLD_START = 6 +uint32 V_H_LIMIT = 7 +uint32 VARIANCE = 8 +uint32 RESIDUALS = 1 +uint32 INTEGRITY_WARNING = 13 +uint32 PENDING = 18 +uint32 INVALID_FIX = 19 +uint32 UNAUTHORIZED = 20 +uint32 INVALID_RATE = 22 + +uint32 status diff --git a/src/novatel_oem7_msgs/msg/TIME.msg b/src/novatel_oem7_msgs/msg/TIME.msg new file mode 100644 index 0000000..b872f4a --- /dev/null +++ b/src/novatel_oem7_msgs/msg/TIME.msg @@ -0,0 +1,16 @@ +Header header +Oem7Header nov_header +uint32 clock_status +float64 offset +float64 offset_std +float64 utc_offset +uint32 utc_year +uint8 utc_month +uint8 utc_day +uint8 utc_hour +uint8 utc_min +uint32 utc_msec +uint32 utc_status + + + diff --git a/src/novatel_oem7_msgs/msg/Translation.msg b/src/novatel_oem7_msgs/msg/Translation.msg new file mode 100644 index 0000000..9cb3773 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/Translation.msg @@ -0,0 +1,9 @@ +TranslationOffset translation +INSFrame frame +float32 x_offset +float32 y_offset +float32 z_offset +float32 x_uncertainty +float32 y_uncertainty +float32 z_uncertainty +INSSourceStatus translation_source \ No newline at end of file diff --git a/src/novatel_oem7_msgs/msg/TranslationOffset.msg b/src/novatel_oem7_msgs/msg/TranslationOffset.msg new file mode 100644 index 0000000..7979b15 --- /dev/null +++ b/src/novatel_oem7_msgs/msg/TranslationOffset.msg @@ -0,0 +1,11 @@ +uint32 ANT1 = 1 +uint32 ANT2 = 2 +uint32 EXTERNAL = 3 +uint32 USER = 4 +uint32 MARK1 = 5 +uint32 MARK2 = 6 +uint32 GIMBAL = 7 +uint32 MARK3 = 9 +uint32 MARK4 = 10 + +uint32 type diff --git a/src/novatel_oem7_msgs/package.xml b/src/novatel_oem7_msgs/package.xml new file mode 100644 index 0000000..d4bcd3c --- /dev/null +++ b/src/novatel_oem7_msgs/package.xml @@ -0,0 +1,20 @@ + + + novatel_oem7_msgs + 0.1.4 + + Messages for NovAtel Oem7 family of receivers. + + + NovAtel Support + http://www.novatel.com + MIT + + TODO + + catkin + message_generation + std_msgs + message_runtime + + diff --git a/src/novatel_oem7_msgs/srv/Oem7AbasciiCmd.srv b/src/novatel_oem7_msgs/srv/Oem7AbasciiCmd.srv new file mode 100644 index 0000000..19be656 --- /dev/null +++ b/src/novatel_oem7_msgs/srv/Oem7AbasciiCmd.srv @@ -0,0 +1,4 @@ +string cmd +--- +string rsp +