From 68e0cc65969dd63ce72345a9e501ea1779af2d67 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Fri, 9 Feb 2018 11:04:10 -0600 Subject: [PATCH 01/12] added RTCM, parameters The driver is now configured using standard ROS parameters. If a ROS topic is provided via the "rtcm" parameter, Strings on this topic will be passed to the device as RTCM corrections (packet ID 55). --- src/an_driver.cpp | 151 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 112 insertions(+), 39 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 8a574cd..0a63917 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -26,12 +26,19 @@ * DEALINGS IN THE SOFTWARE. */ +/* +2018.02.09 Kyler Laird +added RTCM handling, configuration parameters +*/ + #include +#include #include #include #include #include #include +#include #include "rs232/rs232.h" #include "an_packet_protocol.h" @@ -42,33 +49,96 @@ #include #include #include +#include #define RADIANS_TO_DEGREES (180.0/M_PI) + +int an_packet_transmit(an_packet_t *an_packet) +{ + an_packet_encode(an_packet); + return SendBuf(an_packet_pointer(an_packet), an_packet_size(an_packet)); +} + +void set_filter_options() +{ + an_packet_t *an_packet; + filter_options_packet_t filter_options_packet; + + /* initialise the structure by setting all the fields to zero */ + memset(&filter_options_packet, 0, sizeof(filter_options_packet_t)); + + filter_options_packet.permanent = TRUE; + filter_options_packet.vehicle_type = vehicle_type_car; + filter_options_packet.internal_gnss_enabled = TRUE; + filter_options_packet.atmospheric_altitude_enabled = TRUE; + filter_options_packet.velocity_heading_enabled = TRUE; + filter_options_packet.reversing_detection_enabled = TRUE; + filter_options_packet.motion_analysis_enabled = TRUE; + + an_packet = encode_filter_options_packet(&filter_options_packet); + + an_packet_transmit(an_packet); + + an_packet_free(&an_packet); +} + +void set_filter_options_x() +{ + //an_packet_t *an_packet = an_packet_allocate(17, 186); + + an_packet_t *an_packet = an_packet_allocate(4, 55); + memcpy(&an_packet->data[0], "test", 5 * sizeof(uint8_t)); + an_packet_transmit(an_packet); + an_packet_free(&an_packet); +} + +void handle_rtcm(const std_msgs::String::ConstPtr& msg) { + const char *rtcm_data; + uint32_t string_length = msg->data.length(); + + // ROS_INFO("RTCM: %d bytes", string_length); + + an_packet_t *an_packet = an_packet_allocate(string_length, packet_id_rtcm_corrections); + memcpy(&an_packet->data[0], &msg->data[0], string_length); + an_packet_transmit(an_packet); + an_packet_free(&an_packet); +} + int main(int argc, char *argv[]) { // Set up ROS node // - ros::init(argc, argv, "an_device_node"); - ros::NodeHandle nh; - - if(argc != 3) + ros::init(argc, argv, "an_device"); + ros::NodeHandle nh("~"); + + // Set up the COM port + std::string com_port_s; + nh.param("port", com_port_s, "/dev/ttyS0"); + char *com_port = (char *)com_port_s.c_str(); + + int baud_rate; + nh.param("baud", baud_rate, 115200); + + if (OpenComport(com_port, baud_rate)) { - printf("\nCannot start - not enough commnand line arguments. \nUsage: rosrun an_driver an_driver {port} {baud rate}. \nTry: rosrun an_driver an_driver /dev/ttyUSB0 115200\n"); - printf("Number of command line arguments detected: %i\n",argc); + ROS_INFO("Could not open serial port %s at %d baud.", com_port, baud_rate); exit(EXIT_FAILURE); } - - printf("\nYour Advanced Navigation ROS driver is currently running\nClose the Terminal window when done.\n"); - - // Set up the COM port - char* com_port = argv[1]; - int baud_rate = atoi(argv[2]); + ROS_INFO("port:%s@%d", com_port, baud_rate); + + // If an RTCM topic is provided, subscribe to it and pass corrections to device. + std::string rtcm_topic; + ros::Subscriber rtcm_sub; + if (nh.getParam("rtcm", rtcm_topic)) { + rtcm_sub = nh.subscribe(rtcm_topic.c_str(), 1000, handle_rtcm); + ROS_INFO("listening for RTCM on %s", rtcm_topic.c_str()); + } // Initialise Publishers and Topics // - ros::Publisher nav_sat_fix_pub=nh.advertise("an_device/NavSatFix",10); - ros::Publisher twist_pub=nh.advertise("an_device/Twist",10); - ros::Publisher imu_pub=nh.advertise("an_device/Imu",10); - ros::Publisher system_status_pub=nh.advertise("an_device/SystemStatus",10); - ros::Publisher filter_status_pub=nh.advertise("an_device/FilterStatus",10); + ros::Publisher nav_sat_fix_pub=nh.advertise("NavSatFix",10); + ros::Publisher twist_pub=nh.advertise("Twist",10); + ros::Publisher imu_pub=nh.advertise("Imu",10); + ros::Publisher system_status_pub=nh.advertise("SystemStatus",10); + ros::Publisher filter_status_pub=nh.advertise("FilterStatus",10); // Initialise messages sensor_msgs::NavSatFix nav_sat_fix_msg; @@ -126,16 +196,12 @@ int main(int argc, char *argv[]) { quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet; int bytes_received; - if (OpenComport(com_port, baud_rate)) - { - printf("Could not open serial port: %s \n",com_port); - exit(EXIT_FAILURE); - } an_decoder_initialise(&an_decoder); + // Loop continuously, polling for packets - while (1) + while (ros::ok()) { if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0) { @@ -145,6 +211,18 @@ int main(int argc, char *argv[]) { // decode all the packets in the buffer // while ((an_packet = an_packet_decode(&an_decoder)) != NULL) { + // acknowledgement packet // + if (an_packet->id == 0) + { + ROS_INFO("acknowledgement data: %d", an_packet->data[3]); + } + + // receiver information packet // + if (an_packet->id == 69) + { + ROS_INFO("receiver information: %d", an_packet->data[0]); + } + // system state packet // if (an_packet->id == packet_id_system_state) { @@ -153,21 +231,21 @@ int main(int argc, char *argv[]) { // NavSatFix nav_sat_fix_msg.header.stamp.sec=system_state_packet.unix_time_seconds; nav_sat_fix_msg.header.stamp.nsec=system_state_packet.microseconds*1000; - if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || - (system_state_packet.filter_status.b.gnss_fix_type == 2)) + if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || // 2D + (system_state_packet.filter_status.b.gnss_fix_type == 2)) // 3D { - nav_sat_fix_msg.status.status=0; + nav_sat_fix_msg.status.status=0; // no fix } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || - (system_state_packet.filter_status.b.gnss_fix_type == 5)) + else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || // SBAS + (system_state_packet.filter_status.b.gnss_fix_type == 5)) // Omnistar/Starfire { - nav_sat_fix_msg.status.status=1; + nav_sat_fix_msg.status.status=1; // SBAS } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || - (system_state_packet.filter_status.b.gnss_fix_type == 6) || - (system_state_packet.filter_status.b.gnss_fix_type == 7)) + else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || // differential + (system_state_packet.filter_status.b.gnss_fix_type == 6) || // RTK float + (system_state_packet.filter_status.b.gnss_fix_type == 7)) // RTK fixed { - nav_sat_fix_msg.status.status=2; + nav_sat_fix_msg.status.status=2; // GBAS } else { @@ -403,12 +481,7 @@ int main(int argc, char *argv[]) { filter_status_pub.publish(filter_status_msg); } } + ros::spinOnce(); } } - - - - - - From df6f014f2060ba48af9cb9c0eb6e4dcd96277a17 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Mon, 12 Mar 2018 12:41:21 -0500 Subject: [PATCH 02/12] yaw fix, RTCM, TF Yaw has been corrected to conform with REP 103. https://github.com/ros-drivers/advanced_navigation_driver/issues/3 RTCM corrections can be passed from a topic (as Strings). A UTM-based transform can be generated. --- src/an_driver.cpp | 151 ++++++++++++---------------------------------- 1 file changed, 39 insertions(+), 112 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 0a63917..8a574cd 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -26,19 +26,12 @@ * DEALINGS IN THE SOFTWARE. */ -/* -2018.02.09 Kyler Laird -added RTCM handling, configuration parameters -*/ - #include -#include #include #include #include #include #include -#include #include "rs232/rs232.h" #include "an_packet_protocol.h" @@ -49,96 +42,33 @@ added RTCM handling, configuration parameters #include #include #include -#include #define RADIANS_TO_DEGREES (180.0/M_PI) - -int an_packet_transmit(an_packet_t *an_packet) -{ - an_packet_encode(an_packet); - return SendBuf(an_packet_pointer(an_packet), an_packet_size(an_packet)); -} - -void set_filter_options() -{ - an_packet_t *an_packet; - filter_options_packet_t filter_options_packet; - - /* initialise the structure by setting all the fields to zero */ - memset(&filter_options_packet, 0, sizeof(filter_options_packet_t)); - - filter_options_packet.permanent = TRUE; - filter_options_packet.vehicle_type = vehicle_type_car; - filter_options_packet.internal_gnss_enabled = TRUE; - filter_options_packet.atmospheric_altitude_enabled = TRUE; - filter_options_packet.velocity_heading_enabled = TRUE; - filter_options_packet.reversing_detection_enabled = TRUE; - filter_options_packet.motion_analysis_enabled = TRUE; - - an_packet = encode_filter_options_packet(&filter_options_packet); - - an_packet_transmit(an_packet); - - an_packet_free(&an_packet); -} - -void set_filter_options_x() -{ - //an_packet_t *an_packet = an_packet_allocate(17, 186); - - an_packet_t *an_packet = an_packet_allocate(4, 55); - memcpy(&an_packet->data[0], "test", 5 * sizeof(uint8_t)); - an_packet_transmit(an_packet); - an_packet_free(&an_packet); -} - -void handle_rtcm(const std_msgs::String::ConstPtr& msg) { - const char *rtcm_data; - uint32_t string_length = msg->data.length(); - - // ROS_INFO("RTCM: %d bytes", string_length); - - an_packet_t *an_packet = an_packet_allocate(string_length, packet_id_rtcm_corrections); - memcpy(&an_packet->data[0], &msg->data[0], string_length); - an_packet_transmit(an_packet); - an_packet_free(&an_packet); -} - int main(int argc, char *argv[]) { // Set up ROS node // - ros::init(argc, argv, "an_device"); - ros::NodeHandle nh("~"); - - // Set up the COM port - std::string com_port_s; - nh.param("port", com_port_s, "/dev/ttyS0"); - char *com_port = (char *)com_port_s.c_str(); - - int baud_rate; - nh.param("baud", baud_rate, 115200); - - if (OpenComport(com_port, baud_rate)) + ros::init(argc, argv, "an_device_node"); + ros::NodeHandle nh; + + if(argc != 3) { - ROS_INFO("Could not open serial port %s at %d baud.", com_port, baud_rate); + printf("\nCannot start - not enough commnand line arguments. \nUsage: rosrun an_driver an_driver {port} {baud rate}. \nTry: rosrun an_driver an_driver /dev/ttyUSB0 115200\n"); + printf("Number of command line arguments detected: %i\n",argc); exit(EXIT_FAILURE); } - ROS_INFO("port:%s@%d", com_port, baud_rate); - - // If an RTCM topic is provided, subscribe to it and pass corrections to device. - std::string rtcm_topic; - ros::Subscriber rtcm_sub; - if (nh.getParam("rtcm", rtcm_topic)) { - rtcm_sub = nh.subscribe(rtcm_topic.c_str(), 1000, handle_rtcm); - ROS_INFO("listening for RTCM on %s", rtcm_topic.c_str()); - } + + printf("\nYour Advanced Navigation ROS driver is currently running\nClose the Terminal window when done.\n"); + + // Set up the COM port + char* com_port = argv[1]; + int baud_rate = atoi(argv[2]); // Initialise Publishers and Topics // - ros::Publisher nav_sat_fix_pub=nh.advertise("NavSatFix",10); - ros::Publisher twist_pub=nh.advertise("Twist",10); - ros::Publisher imu_pub=nh.advertise("Imu",10); - ros::Publisher system_status_pub=nh.advertise("SystemStatus",10); - ros::Publisher filter_status_pub=nh.advertise("FilterStatus",10); + ros::Publisher nav_sat_fix_pub=nh.advertise("an_device/NavSatFix",10); + ros::Publisher twist_pub=nh.advertise("an_device/Twist",10); + ros::Publisher imu_pub=nh.advertise("an_device/Imu",10); + ros::Publisher system_status_pub=nh.advertise("an_device/SystemStatus",10); + ros::Publisher filter_status_pub=nh.advertise("an_device/FilterStatus",10); // Initialise messages sensor_msgs::NavSatFix nav_sat_fix_msg; @@ -196,12 +126,16 @@ int main(int argc, char *argv[]) { quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet; int bytes_received; + if (OpenComport(com_port, baud_rate)) + { + printf("Could not open serial port: %s \n",com_port); + exit(EXIT_FAILURE); + } an_decoder_initialise(&an_decoder); - // Loop continuously, polling for packets - while (ros::ok()) + while (1) { if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0) { @@ -211,18 +145,6 @@ int main(int argc, char *argv[]) { // decode all the packets in the buffer // while ((an_packet = an_packet_decode(&an_decoder)) != NULL) { - // acknowledgement packet // - if (an_packet->id == 0) - { - ROS_INFO("acknowledgement data: %d", an_packet->data[3]); - } - - // receiver information packet // - if (an_packet->id == 69) - { - ROS_INFO("receiver information: %d", an_packet->data[0]); - } - // system state packet // if (an_packet->id == packet_id_system_state) { @@ -231,21 +153,21 @@ int main(int argc, char *argv[]) { // NavSatFix nav_sat_fix_msg.header.stamp.sec=system_state_packet.unix_time_seconds; nav_sat_fix_msg.header.stamp.nsec=system_state_packet.microseconds*1000; - if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || // 2D - (system_state_packet.filter_status.b.gnss_fix_type == 2)) // 3D + if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || + (system_state_packet.filter_status.b.gnss_fix_type == 2)) { - nav_sat_fix_msg.status.status=0; // no fix + nav_sat_fix_msg.status.status=0; } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || // SBAS - (system_state_packet.filter_status.b.gnss_fix_type == 5)) // Omnistar/Starfire + else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || + (system_state_packet.filter_status.b.gnss_fix_type == 5)) { - nav_sat_fix_msg.status.status=1; // SBAS + nav_sat_fix_msg.status.status=1; } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || // differential - (system_state_packet.filter_status.b.gnss_fix_type == 6) || // RTK float - (system_state_packet.filter_status.b.gnss_fix_type == 7)) // RTK fixed + else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || + (system_state_packet.filter_status.b.gnss_fix_type == 6) || + (system_state_packet.filter_status.b.gnss_fix_type == 7)) { - nav_sat_fix_msg.status.status=2; // GBAS + nav_sat_fix_msg.status.status=2; } else { @@ -481,7 +403,12 @@ int main(int argc, char *argv[]) { filter_status_pub.publish(filter_status_msg); } } - ros::spinOnce(); } } + + + + + + From 2604368dd68d2a09048fc83a2667a76cd81b0e80 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Mon, 12 Mar 2018 12:52:10 -0500 Subject: [PATCH 03/12] oops I uploaded the wrong file last time. See previous commit. --- src/an_driver.cpp | 278 +++++++++++++++++++++++++++++++++++++--------- 1 file changed, 224 insertions(+), 54 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 8a574cd..5aea691 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -26,12 +26,23 @@ * DEALINGS IN THE SOFTWARE. */ +/* +2018.02.09 Kyler Laird +added RTCM handling, configuration parameters + +2018.03.10 Kyler Laird +added REP compliance, transform +*/ + #include +#include +#include #include #include #include #include #include +#include #include "rs232/rs232.h" #include "an_packet_protocol.h" @@ -42,33 +53,158 @@ #include #include #include +#include #define RADIANS_TO_DEGREES (180.0/M_PI) + +int an_packet_transmit(an_packet_t *an_packet) +{ + an_packet_encode(an_packet); + return SendBuf(an_packet_pointer(an_packet), an_packet_size(an_packet)); +} + +void set_filter_options() +{ + an_packet_t *an_packet; + filter_options_packet_t filter_options_packet; + + /* initialise the structure by setting all the fields to zero */ + memset(&filter_options_packet, 0, sizeof(filter_options_packet_t)); + + filter_options_packet.permanent = TRUE; + filter_options_packet.vehicle_type = vehicle_type_car; + filter_options_packet.internal_gnss_enabled = TRUE; + filter_options_packet.atmospheric_altitude_enabled = TRUE; + filter_options_packet.velocity_heading_enabled = TRUE; + filter_options_packet.reversing_detection_enabled = TRUE; + filter_options_packet.motion_analysis_enabled = TRUE; + + an_packet = encode_filter_options_packet(&filter_options_packet); + + an_packet_transmit(an_packet); + + an_packet_free(&an_packet); +} + +void set_filter_options_x() +{ + //an_packet_t *an_packet = an_packet_allocate(17, 186); + + an_packet_t *an_packet = an_packet_allocate(4, 55); + memcpy(&an_packet->data[0], "test", 5 * sizeof(uint8_t)); + an_packet_transmit(an_packet); + an_packet_free(&an_packet); +} + +void handle_rtcm(const std_msgs::String::ConstPtr& msg) { + const char *rtcm_data; + uint32_t string_length = msg->data.length(); + + // ROS_INFO("RTCM: %d bytes", string_length); + + an_packet_t *an_packet = an_packet_allocate(string_length, packet_id_rtcm_corrections); + memcpy(&an_packet->data[0], &msg->data[0], string_length); + an_packet_transmit(an_packet); + an_packet_free(&an_packet); +} + + +// LatLong-UTM.c++ +// Conversions: LatLong to UTM; and UTM to LatLong; +// by Eugene Reimer, ereimer@shaw.ca, 2002-December; +// with LLtoUTM & UTMtoLL routines based on those by Chuck Gantz chuck.gantz@globalstar.com; +// with ellipsoid & datum constants from Peter H Dana website (http://www.colorado.edu/geography/gcraft/notes/datum/edlist.html); +// +// Usage: see the Usage() routine below; +// +// Copyright © 1995,2002,2010 Eugene Reimer, Peter Dana, Chuck Gantz. Released under the GPL; see http://www.gnu.org/licenses/gpl.html +// (Peter Dana's non-commercial clause precludes using the LGPL) + + +#include //2010-08-11: was +#include //2010-08-11: was + +#define fr 298.257223563 +#define a 6378137 +#define k0 0.9996 +const double PI = 4*atan(1); //Gantz used: PI=3.14159265; +const double deg2rad = PI/180; +const double ee = 2/fr-1/(fr*fr); +const double EE = ee/(1-ee); +double LongOriginRad; + +void LLtoUTM(int Zone, double LatRad, double LongRad, double& Northing, double& Easting) { + // converts LatLong to UTM coords; 3/22/95: by ChuckGantz chuck.gantz@globalstar.com, from USGS Bulletin 1532. + double N, T, C, A, M; + + N = a/sqrt(1-ee*sin(LatRad)*sin(LatRad)); + T = tan(LatRad)*tan(LatRad); + C = EE*cos(LatRad)*cos(LatRad); + A = cos(LatRad)*(LongRad-LongOriginRad); + + M= a*((1 - ee/4 - 3*ee*ee/64 - 5*ee*ee*ee/256 ) *LatRad + - (3*ee/8 + 3*ee*ee/32 + 45*ee*ee*ee/1024) *sin(2*LatRad) + + (15*ee*ee/256 + 45*ee*ee*ee/1024 ) *sin(4*LatRad) + - (35*ee*ee*ee/3072 ) *sin(6*LatRad)); + + Easting = k0*N*(A+(1-T+C)*A*A*A/6+(5-18*T+T*T+72*C-58*EE)*A*A*A*A*A/120) + 500000.0; + + Northing = k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 + + (61-58*T+T*T+600*C-330*EE)*A*A*A*A*A*A/720)); +} + + + + + + int main(int argc, char *argv[]) { // Set up ROS node // - ros::init(argc, argv, "an_device_node"); - ros::NodeHandle nh; - - if(argc != 3) + ros::init(argc, argv, "an_device"); + ros::NodeHandle nh("~"); + + // Set up the COM port + std::string com_port_s; + nh.param("port", com_port_s, "/dev/ttyS0"); + char *com_port = (char *)com_port_s.c_str(); + + int baud_rate; + nh.param("baud", baud_rate, 115200); + + if (OpenComport(com_port, baud_rate)) { - printf("\nCannot start - not enough commnand line arguments. \nUsage: rosrun an_driver an_driver {port} {baud rate}. \nTry: rosrun an_driver an_driver /dev/ttyUSB0 115200\n"); - printf("Number of command line arguments detected: %i\n",argc); + ROS_INFO("Could not open serial port %s at %d baud.", com_port, baud_rate); exit(EXIT_FAILURE); } - - printf("\nYour Advanced Navigation ROS driver is currently running\nClose the Terminal window when done.\n"); - - // Set up the COM port - char* com_port = argv[1]; - int baud_rate = atoi(argv[2]); + ROS_INFO("port:%s@%d", com_port, baud_rate); + + // If an RTCM topic is provided, subscribe to it and pass corrections to device. + std::string rtcm_topic; + ros::Subscriber rtcm_sub; + if (nh.getParam("rtcm", rtcm_topic)) { + rtcm_sub = nh.subscribe(rtcm_topic.c_str(), 1000, handle_rtcm); + ROS_INFO("listening for RTCM on %s", rtcm_topic.c_str()); + } + + // If a UTM Zone is provided, publish transforms. + // The zone is static to avoid problems due to changing when near a Zone boundary. + int utm_zone; + std::string tf_name; + tf::Transform transform; + if (nh.getParam("utm_zone", utm_zone)) { + LongOriginRad = (utm_zone*6 - 183) * deg2rad; + nh.param("tf_name", tf_name, "an_device"); // The default should be chosen better. + ROS_INFO("using UTM Zone %d to publish transform %s", utm_zone, tf_name.c_str()); + + } // Initialise Publishers and Topics // - ros::Publisher nav_sat_fix_pub=nh.advertise("an_device/NavSatFix",10); - ros::Publisher twist_pub=nh.advertise("an_device/Twist",10); - ros::Publisher imu_pub=nh.advertise("an_device/Imu",10); - ros::Publisher system_status_pub=nh.advertise("an_device/SystemStatus",10); - ros::Publisher filter_status_pub=nh.advertise("an_device/FilterStatus",10); + ros::Publisher nav_sat_fix_pub=nh.advertise("NavSatFix",10); + ros::Publisher twist_pub=nh.advertise("Twist",10); + ros::Publisher imu_pub=nh.advertise("Imu",10); + ros::Publisher system_status_pub=nh.advertise("SystemStatus",10); + ros::Publisher filter_status_pub=nh.advertise("FilterStatus",10); // Initialise messages sensor_msgs::NavSatFix nav_sat_fix_msg; @@ -126,16 +262,12 @@ int main(int argc, char *argv[]) { quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet; int bytes_received; - if (OpenComport(com_port, baud_rate)) - { - printf("Could not open serial port: %s \n",com_port); - exit(EXIT_FAILURE); - } an_decoder_initialise(&an_decoder); + // Loop continuously, polling for packets - while (1) + while (ros::ok()) { if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0) { @@ -145,6 +277,18 @@ int main(int argc, char *argv[]) { // decode all the packets in the buffer // while ((an_packet = an_packet_decode(&an_decoder)) != NULL) { + // acknowledgement packet // + if (an_packet->id == 0) + { + ROS_INFO("acknowledgement data: %d", an_packet->data[3]); + } + + // receiver information packet // + if (an_packet->id == 69) + { + ROS_INFO("receiver information: %d", an_packet->data[0]); + } + // system state packet // if (an_packet->id == packet_id_system_state) { @@ -153,21 +297,21 @@ int main(int argc, char *argv[]) { // NavSatFix nav_sat_fix_msg.header.stamp.sec=system_state_packet.unix_time_seconds; nav_sat_fix_msg.header.stamp.nsec=system_state_packet.microseconds*1000; - if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || - (system_state_packet.filter_status.b.gnss_fix_type == 2)) + if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || // 2D + (system_state_packet.filter_status.b.gnss_fix_type == 2)) // 3D { - nav_sat_fix_msg.status.status=0; + nav_sat_fix_msg.status.status=0; // no fix } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || - (system_state_packet.filter_status.b.gnss_fix_type == 5)) + else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || // SBAS + (system_state_packet.filter_status.b.gnss_fix_type == 5)) // Omnistar/Starfire { - nav_sat_fix_msg.status.status=1; + nav_sat_fix_msg.status.status=1; // SBAS } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || - (system_state_packet.filter_status.b.gnss_fix_type == 6) || - (system_state_packet.filter_status.b.gnss_fix_type == 7)) + else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || // differential + (system_state_packet.filter_status.b.gnss_fix_type == 6) || // RTK float + (system_state_packet.filter_status.b.gnss_fix_type == 7)) // RTK fixed { - nav_sat_fix_msg.status.status=2; + nav_sat_fix_msg.status.status=2; // GBAS } else { @@ -191,21 +335,19 @@ int main(int argc, char *argv[]) { // IMU imu_msg.header.stamp.sec=system_state_packet.unix_time_seconds; imu_msg.header.stamp.nsec=system_state_packet.microseconds*1000; + // Convert roll, pitch, yaw from radians to quaternion format // - float phi = system_state_packet.orientation[0] / 2.0f; - float theta = system_state_packet.orientation[1] / 2.0f; - float psi = system_state_packet.orientation[2] / 2.0f; - float sin_phi = sinf(phi); - float cos_phi = cosf(phi); - float sin_theta = sinf(theta); - float cos_theta = cosf(theta); - float sin_psi = sinf(psi); - float cos_psi = cosf(psi); - imu_msg.orientation.x=-cos_phi * sin_theta * sin_psi + sin_phi * cos_theta * cos_psi; - imu_msg.orientation.y=cos_phi * sin_theta * cos_psi + sin_phi * cos_theta * sin_psi; - imu_msg.orientation.z=cos_phi * cos_theta * sin_psi - sin_phi * sin_theta * cos_psi; - imu_msg.orientation.w=cos_phi * cos_theta * cos_psi + sin_phi * sin_theta * sin_psi; - + tf::Quaternion orientation; + orientation.setRPY( + system_state_packet.orientation[0], + system_state_packet.orientation[1], + PI / 2.0f - system_state_packet.orientation[2] // REP 103 + ); + imu_msg.orientation.x = orientation[0]; + imu_msg.orientation.y = orientation[1]; + imu_msg.orientation.z = orientation[2]; + imu_msg.orientation.w = orientation[3]; + imu_msg.angular_velocity.x=system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values imu_msg.angular_velocity.y=system_state_packet.angular_velocity[1]; imu_msg.angular_velocity.z=system_state_packet.angular_velocity[2]; @@ -213,6 +355,39 @@ int main(int argc, char *argv[]) { imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; + // transform + if (utm_zone) { + static tf::TransformBroadcaster transform_br; + double N, E; + + LLtoUTM( + utm_zone, + system_state_packet.latitude, + system_state_packet.longitude, + N, E + ); + + transform.setOrigin(tf::Vector3( + E, N, system_state_packet.height + // 0, 0, 0 // move to origin for testing + // E - 483713.0, N - 4526330.0, 0 // move to origin for testing + )); + + transform.setRotation(orientation); + + transform_br.sendTransform(tf::StampedTransform( + transform, + //ros::Time::now(), + ros::Time( + system_state_packet.unix_time_seconds, + system_state_packet.microseconds*1000 + ), + "world", // Is it reasonable to hardcode this? + tf_name + )); + //printf("time: %d\n", system_state_packet.microseconds); + } + // System Status system_status_msg.message = ""; system_status_msg.level = 0; // default OK state @@ -391,7 +566,7 @@ int main(int argc, char *argv[]) { imu_msg.orientation_covariance[8] = quaternion_orientation_standard_deviation_packet.standard_deviation[2]; } } - // Ensure that you free the an_packet when your done with it // + // Ensure that you free the an_packet when you're done with it // // or you will leak memory // an_packet_free(&an_packet); @@ -403,12 +578,7 @@ int main(int argc, char *argv[]) { filter_status_pub.publish(filter_status_msg); } } + ros::spinOnce(); } } - - - - - - From e494877f4bf71cf2f2e4bad5e29f2edec279c443 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Mon, 12 Mar 2018 12:53:34 -0500 Subject: [PATCH 04/12] RTCM, TF, REP 103 added instructions for RTCM and TF usage, notification of REP 103 compliance --- README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/README.md b/README.md index 81d7690..35175b4 100644 --- a/README.md +++ b/README.md @@ -15,3 +15,17 @@ If you require any assistance using this code, please email support@advancednavi Installation, build, device configuration, and execution instructions can be found in the file "Advanced Navigation ROS Driver Notes.txt". + +*** modifications by Kyler Laird *** + +Orientation now complies with REP 103. East is zero degrees. Degrees increment counter-clockwise. +https://github.com/ros-drivers/advanced_navigation_driver/issues/3#issuecomment-372348146 + +New parameters: + rtcm: Specify a topic name. Strings published to this topic will be passed to the device as RTCM corrections. + + utm_zone: Specify a UTM Zone number. This will be used for calculating the transform. + + tf_name: Specify the name of the transform to broadcast. + +These changes are all tentative and need testing. From e4e212e272cd99f2200866a638f48c479c3f5ba3 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Mon, 12 Mar 2018 12:54:37 -0500 Subject: [PATCH 05/12] formatting --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 35175b4..5a0f590 100644 --- a/README.md +++ b/README.md @@ -22,6 +22,7 @@ Orientation now complies with REP 103. East is zero degrees. Degrees increment https://github.com/ros-drivers/advanced_navigation_driver/issues/3#issuecomment-372348146 New parameters: + rtcm: Specify a topic name. Strings published to this topic will be passed to the device as RTCM corrections. utm_zone: Specify a UTM Zone number. This will be used for calculating the transform. From f21406e9319b8b68e9441cd5d65f69f2858c38ec Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Mon, 12 Mar 2018 18:29:43 -0500 Subject: [PATCH 06/12] parameter names, odometry I changed the parameter names again. I added a note about the new Odometry feature. --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 5a0f590..29cd8d1 100644 --- a/README.md +++ b/README.md @@ -23,10 +23,12 @@ https://github.com/ros-drivers/advanced_navigation_driver/issues/3#issuecomment- New parameters: - rtcm: Specify a topic name. Strings published to this topic will be passed to the device as RTCM corrections. + rtcm_topic: Specify a topic name. Strings published to this topic will be passed to the device as RTCM corrections. utm_zone: Specify a UTM Zone number. This will be used for calculating the transform. tf_name: Specify the name of the transform to broadcast. +If you specify a utm_zone, Odometry messages will be published to "odom". + These changes are all tentative and need testing. From c873ff66a2fce2a075b28b97515fa0dcb5558b9a Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Mon, 12 Mar 2018 18:31:01 -0500 Subject: [PATCH 07/12] parameter names, odometry I'm trying to standardize the parameter names (to be more like Robotnik's). I added Odometry message output if utm_zone is provided. --- src/an_driver.cpp | 46 +++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 43 insertions(+), 3 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 5aea691..c151467 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -54,6 +54,7 @@ added REP compliance, transform #include #include #include +#include #define RADIANS_TO_DEGREES (180.0/M_PI) @@ -182,7 +183,7 @@ int main(int argc, char *argv[]) { // If an RTCM topic is provided, subscribe to it and pass corrections to device. std::string rtcm_topic; ros::Subscriber rtcm_sub; - if (nh.getParam("rtcm", rtcm_topic)) { + if (nh.getParam("rtcm_topic", rtcm_topic)) { rtcm_sub = nh.subscribe(rtcm_topic.c_str(), 1000, handle_rtcm); ROS_INFO("listening for RTCM on %s", rtcm_topic.c_str()); } @@ -199,12 +200,14 @@ int main(int argc, char *argv[]) { } + // Initialise Publishers and Topics // ros::Publisher nav_sat_fix_pub=nh.advertise("NavSatFix",10); ros::Publisher twist_pub=nh.advertise("Twist",10); ros::Publisher imu_pub=nh.advertise("Imu",10); ros::Publisher system_status_pub=nh.advertise("SystemStatus",10); ros::Publisher filter_status_pub=nh.advertise("FilterStatus",10); + ros::Publisher odom_pub = nh.advertise("odom", 50); // Initialise messages sensor_msgs::NavSatFix nav_sat_fix_msg; @@ -354,7 +357,7 @@ int main(int argc, char *argv[]) { imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0]; imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; - + // transform if (utm_zone) { static tf::TransformBroadcaster transform_br; @@ -386,7 +389,44 @@ int main(int argc, char *argv[]) { tf_name )); //printf("time: %d\n", system_state_packet.microseconds); - } + + nav_msgs::Odometry odom; + odom.header.stamp.sec=system_state_packet.unix_time_seconds; + odom.header.stamp.nsec=system_state_packet.microseconds*1000; + + odom.header.frame_id= "world"; // fix this! + odom.child_frame_id = "ins"; // fix this! + + //set the position + odom.pose.pose.position.x = E; + odom.pose.pose.position.y = N; + odom.pose.pose.position.z = system_state_packet.height; + odom.pose.pose.orientation.x = orientation[0]; + odom.pose.pose.orientation.y = orientation[1]; + odom.pose.pose.orientation.z = orientation[2]; + odom.pose.pose.orientation.w = orientation[3]; + + // Is this correct??? + odom.pose.covariance={ + pow(system_state_packet.standard_deviation[1],2), 0.0, 0.0, + 0.0, pow(system_state_packet.standard_deviation[0],2), 0.0, + 0.0, 0.0, pow(system_state_packet.standard_deviation[2],2) + }; + + //set the velocity + odom.twist.twist.linear.x = system_state_packet.velocity[0]; + odom.twist.twist.linear.y = system_state_packet.velocity[1]; + odom.twist.twist.linear.z = system_state_packet.velocity[2]; + + // Set angular velocity. + odom.twist.twist.angular.x = system_state_packet.angular_velocity[0]; + odom.twist.twist.angular.y = system_state_packet.angular_velocity[1]; + odom.twist.twist.angular.z = system_state_packet.angular_velocity[2]; + + //publish the message + odom_pub.publish(odom); + + } // System Status system_status_msg.message = ""; From f45b9d4760f1c03b62cca11978462849da8e6920 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Wed, 14 Mar 2018 11:31:13 -0500 Subject: [PATCH 08/12] block in Linux Even at only 20 Hz, the driver always consumed 100% CPU time. I found that it was polling without any delay/throttling. I configured the Linux version of OpenComport() for blocking reads. CPU time dropped to 5%. --- src/rs232/rs232.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/rs232/rs232.c b/src/rs232/rs232.c index 2b30f66..b2d3f8d 100644 --- a/src/rs232/rs232.c +++ b/src/rs232/rs232.c @@ -101,7 +101,8 @@ int OpenComport(char *comport, int baudrate) break; } - Cport = open(comport, O_RDWR | O_NOCTTY | O_NDELAY); + // Cport = open(comport, O_RDWR | O_NOCTTY | O_NDELAY); + Cport = open(comport, O_RDWR | O_NOCTTY); if(Cport==-1) { perror("unable to open comport "); @@ -122,7 +123,8 @@ int OpenComport(char *comport, int baudrate) new_port_settings.c_oflag = 0; new_port_settings.c_lflag = 0; new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */ - new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ + // new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ + new_port_settings.c_cc[VTIME] = 1; /* block untill a timer expires (n * 100 mSec.) */ error = tcsetattr(Cport, TCSANOW, &new_port_settings); if(error==-1) { From 6631e71df8f4bdd67d242459d13ea87f33a928b7 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Wed, 14 Mar 2018 11:38:00 -0500 Subject: [PATCH 09/12] blocking serial port read I added a note about blocking serial port reads for Linux. --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 29cd8d1..aac86d4 100644 --- a/README.md +++ b/README.md @@ -31,4 +31,6 @@ New parameters: If you specify a utm_zone, Odometry messages will be published to "odom". +Under Linux, serial port reads are blocking. This reduces CPU usage from 100% to 5% (for 20 Hz on an i5). + These changes are all tentative and need testing. From cc1e4879fafd2b6c1078820da274ec3dfc43e41d Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Wed, 14 Mar 2018 12:11:15 -0500 Subject: [PATCH 10/12] simplified names Rather than specify topic and frame names using parameters, I named them relative to the current namespace. --- src/an_driver.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index c151467..539eaf0 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -180,34 +180,27 @@ int main(int argc, char *argv[]) { } ROS_INFO("port:%s@%d", com_port, baud_rate); - // If an RTCM topic is provided, subscribe to it and pass corrections to device. - std::string rtcm_topic; - ros::Subscriber rtcm_sub; - if (nh.getParam("rtcm_topic", rtcm_topic)) { - rtcm_sub = nh.subscribe(rtcm_topic.c_str(), 1000, handle_rtcm); - ROS_INFO("listening for RTCM on %s", rtcm_topic.c_str()); - } - + // If a UTM Zone is provided, publish transforms. // The zone is static to avoid problems due to changing when near a Zone boundary. int utm_zone; - std::string tf_name; + std::string tf_name = nh.getNamespace(); tf::Transform transform; if (nh.getParam("utm_zone", utm_zone)) { LongOriginRad = (utm_zone*6 - 183) * deg2rad; - nh.param("tf_name", tf_name, "an_device"); // The default should be chosen better. ROS_INFO("using UTM Zone %d to publish transform %s", utm_zone, tf_name.c_str()); } - // Initialise Publishers and Topics // + // Initialise Publishers, and Subscribers // ros::Publisher nav_sat_fix_pub=nh.advertise("NavSatFix",10); ros::Publisher twist_pub=nh.advertise("Twist",10); ros::Publisher imu_pub=nh.advertise("Imu",10); ros::Publisher system_status_pub=nh.advertise("SystemStatus",10); ros::Publisher filter_status_pub=nh.advertise("FilterStatus",10); ros::Publisher odom_pub = nh.advertise("odom", 50); + ros::Subscriber rtcm_sub = nh.subscribe("rtcm", 1000, handle_rtcm); // Initialise messages sensor_msgs::NavSatFix nav_sat_fix_msg; @@ -372,15 +365,12 @@ int main(int argc, char *argv[]) { transform.setOrigin(tf::Vector3( E, N, system_state_packet.height - // 0, 0, 0 // move to origin for testing - // E - 483713.0, N - 4526330.0, 0 // move to origin for testing )); transform.setRotation(orientation); transform_br.sendTransform(tf::StampedTransform( transform, - //ros::Time::now(), ros::Time( system_state_packet.unix_time_seconds, system_state_packet.microseconds*1000 @@ -388,7 +378,6 @@ int main(int argc, char *argv[]) { "world", // Is it reasonable to hardcode this? tf_name )); - //printf("time: %d\n", system_state_packet.microseconds); nav_msgs::Odometry odom; odom.header.stamp.sec=system_state_packet.unix_time_seconds; From 2cfb0a46d0da9845aba847ad4b5d6c3610000e68 Mon Sep 17 00:00:00 2001 From: Kyler Laird Date: Wed, 14 Mar 2018 12:11:54 -0500 Subject: [PATCH 11/12] simplified namespace --- README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index aac86d4..3d30abc 100644 --- a/README.md +++ b/README.md @@ -21,16 +21,16 @@ Installation, build, device configuration, and execution instructions can be fou Orientation now complies with REP 103. East is zero degrees. Degrees increment counter-clockwise. https://github.com/ros-drivers/advanced_navigation_driver/issues/3#issuecomment-372348146 -New parameters: - - rtcm_topic: Specify a topic name. Strings published to this topic will be passed to the device as RTCM corrections. - +new parameters: utm_zone: Specify a UTM Zone number. This will be used for calculating the transform. - tf_name: Specify the name of the transform to broadcast. - -If you specify a utm_zone, Odometry messages will be published to "odom". +new topics: + rtcm: Strings published to this topic will be passed to the device as RTCM corrections. + odom: Odometry from the device is published to this topic. (Depends on utm_zone.) +new frames: + ~: This frame has the name of the current name space and describes the position and orientation of the device. (Depends on utm_zone.) + Under Linux, serial port reads are blocking. This reduces CPU usage from 100% to 5% (for 20 Hz on an i5). These changes are all tentative and need testing. From f05316f35a51ae981dfcd265ad7a35b575d53c37 Mon Sep 17 00:00:00 2001 From: Ronald Ensing Date: Mon, 27 Aug 2018 11:52:42 +0200 Subject: [PATCH 12/12] Conform to ros conventions regarding frame id's and coordinate frames. --- Advanced Navigation ROS Driver Notes.txt | 2 +- CMakeLists.txt | 194 +------ README.md | 2 +- package.xml | 16 +- src/an_driver.cpp | 685 +++++++---------------- 5 files changed, 220 insertions(+), 679 deletions(-) diff --git a/Advanced Navigation ROS Driver Notes.txt b/Advanced Navigation ROS Driver Notes.txt index 5dddbda..9ef110a 100644 --- a/Advanced Navigation ROS Driver Notes.txt +++ b/Advanced Navigation ROS Driver Notes.txt @@ -42,7 +42,7 @@ ROS driver build instructions from: http://www.clearpathrobotics.com/assets/guid /*********************************************/ /* Device Configuration */ /*********************************************/ -To use this example code, your Advanced Navigation device should be configured to output anpp packets #20 and #27. +To use this example code, your Advanced Navigation device should be already configured to output anpp packets #20 and #27. /*********************************************/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 91cc74b..c7d82ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,204 +1,22 @@ cmake_minimum_required(VERSION 2.8.3) project(advanced_navigation_driver) -## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + diagnostic_msgs + nav_msgs roscpp + sensor_msgs std_msgs + tf ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +catkin_package() - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## 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 you 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 an_driver -# CATKIN_DEPENDS roscpp std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include +include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/an_driver.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/an_driver_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_an_driver.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) - -######################### -## Advanced Navigation ## -######################### add_executable(advanced_navigation_driver src/an_driver.cpp src/spatial_packets.c src/an_packet_protocol.c src/rs232/rs232.c) target_link_libraries(advanced_navigation_driver ${catkin_LIBRARIES}) diff --git a/README.md b/README.md index 3d30abc..1c9f9d9 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ Packet to Published Message Example Copyright 2017, Advanced Navigation -This is an example using the Advanced Navigation Spatial SDK to create a ROS driver that reads and decodes the anpp packets (in this case packet #20 and packet #27) and publishes the information as ROS topics / messages. +This is an example using the Advanced Navigation Spatial SDK to create a ROS driver that reads and decodes the anpp packets (in this case packet #20, #27, #36 and #40) and publishes the information as ROS topics / messages. It should work on all Advanced Navigation INS devices. diff --git a/package.xml b/package.xml index 244c990..196301f 100644 --- a/package.xml +++ b/package.xml @@ -1,19 +1,19 @@ - + advanced_navigation_driver 1.0.0 - The Advanced Navigation driver package for ROS + The Advanced Navigation driver package for ROS. support@advancednavigation.com.au MIT catkin - - roscpp - std_msgs - - roscpp - std_msgs + diagnostic_msgs + nav_msgs + roscpp + sensor_msgs + std_msgs + tf diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 539eaf0..2486ac0 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -19,147 +19,75 @@ * * 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 + * 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. */ -/* -2018.02.09 Kyler Laird -added RTCM handling, configuration parameters - -2018.03.10 Kyler Laird -added REP compliance, transform -*/ - #include -#include -#include +#include #include #include #include #include #include -#include + +#include +#include +#include +#include +#include #include "rs232/rs232.h" #include "an_packet_protocol.h" #include "spatial_packets.h" -#include -#include -#include -#include -#include -#include -#include #define RADIANS_TO_DEGREES (180.0/M_PI) - -int an_packet_transmit(an_packet_t *an_packet) -{ - an_packet_encode(an_packet); - return SendBuf(an_packet_pointer(an_packet), an_packet_size(an_packet)); +// Convert NED orientation (Advanced Navigation) to ENU orientation (ROS) +inline geometry_msgs::Quaternion nedToEnu(geometry_msgs::Quaternion const & in) { + tf::Quaternion enu(in.x, in.y, in.z, in.w); // x, y, z, w according to tf documentation + // To get from NED to ENU we have to: + // 1) Rotate 90 degrees around Z + // 2) Rotate the result 180 degrees around Y + tf::Transform z_90(tf::Quaternion(tf::Vector3(0, 0, 1), 0.5*M_PI)); + tf::Transform y_180(tf::Quaternion(tf::Vector3(0, 1, 0), M_PI)); + + enu = (y_180 * (z_90 * enu)); + geometry_msgs::Quaternion out; + out.x = enu.x(); + out.y = enu.y(); + out.z = enu.z(); + out.w = enu.w(); + return out; } -void set_filter_options() -{ - an_packet_t *an_packet; - filter_options_packet_t filter_options_packet; - - /* initialise the structure by setting all the fields to zero */ - memset(&filter_options_packet, 0, sizeof(filter_options_packet_t)); - - filter_options_packet.permanent = TRUE; - filter_options_packet.vehicle_type = vehicle_type_car; - filter_options_packet.internal_gnss_enabled = TRUE; - filter_options_packet.atmospheric_altitude_enabled = TRUE; - filter_options_packet.velocity_heading_enabled = TRUE; - filter_options_packet.reversing_detection_enabled = TRUE; - filter_options_packet.motion_analysis_enabled = TRUE; - - an_packet = encode_filter_options_packet(&filter_options_packet); - - an_packet_transmit(an_packet); - - an_packet_free(&an_packet); -} - -void set_filter_options_x() -{ - //an_packet_t *an_packet = an_packet_allocate(17, 186); - - an_packet_t *an_packet = an_packet_allocate(4, 55); - memcpy(&an_packet->data[0], "test", 5 * sizeof(uint8_t)); - an_packet_transmit(an_packet); - an_packet_free(&an_packet); -} - -void handle_rtcm(const std_msgs::String::ConstPtr& msg) { - const char *rtcm_data; - uint32_t string_length = msg->data.length(); - - // ROS_INFO("RTCM: %d bytes", string_length); - - an_packet_t *an_packet = an_packet_allocate(string_length, packet_id_rtcm_corrections); - memcpy(&an_packet->data[0], &msg->data[0], string_length); - an_packet_transmit(an_packet); - an_packet_free(&an_packet); +// Convert the packet status to a ros diagnostic message +inline void appendSystemStatus(diagnostic_msgs::DiagnosticArray & status_array, unsigned int const status, int const level, std::string const & failure_message = "", std::string const & success_message = "") { + if (status) { + diagnostic_msgs::DiagnosticStatus status_msg; + status_msg.level = level; // WARN=1 ERROR=2 + status_msg.message = failure_message; + status_array.status.push_back(status_msg); + } } -// LatLong-UTM.c++ -// Conversions: LatLong to UTM; and UTM to LatLong; -// by Eugene Reimer, ereimer@shaw.ca, 2002-December; -// with LLtoUTM & UTMtoLL routines based on those by Chuck Gantz chuck.gantz@globalstar.com; -// with ellipsoid & datum constants from Peter H Dana website (http://www.colorado.edu/geography/gcraft/notes/datum/edlist.html); -// -// Usage: see the Usage() routine below; -// -// Copyright © 1995,2002,2010 Eugene Reimer, Peter Dana, Chuck Gantz. Released under the GPL; see http://www.gnu.org/licenses/gpl.html -// (Peter Dana's non-commercial clause precludes using the LGPL) - - -#include //2010-08-11: was -#include //2010-08-11: was - -#define fr 298.257223563 -#define a 6378137 -#define k0 0.9996 -const double PI = 4*atan(1); //Gantz used: PI=3.14159265; -const double deg2rad = PI/180; -const double ee = 2/fr-1/(fr*fr); -const double EE = ee/(1-ee); -double LongOriginRad; - -void LLtoUTM(int Zone, double LatRad, double LongRad, double& Northing, double& Easting) { - // converts LatLong to UTM coords; 3/22/95: by ChuckGantz chuck.gantz@globalstar.com, from USGS Bulletin 1532. - double N, T, C, A, M; - - N = a/sqrt(1-ee*sin(LatRad)*sin(LatRad)); - T = tan(LatRad)*tan(LatRad); - C = EE*cos(LatRad)*cos(LatRad); - A = cos(LatRad)*(LongRad-LongOriginRad); - - M= a*((1 - ee/4 - 3*ee*ee/64 - 5*ee*ee*ee/256 ) *LatRad - - (3*ee/8 + 3*ee*ee/32 + 45*ee*ee*ee/1024) *sin(2*LatRad) - + (15*ee*ee/256 + 45*ee*ee*ee/1024 ) *sin(4*LatRad) - - (35*ee*ee*ee/3072 ) *sin(6*LatRad)); - - Easting = k0*N*(A+(1-T+C)*A*A*A/6+(5-18*T+T*T+72*C-58*EE)*A*A*A*A*A/120) + 500000.0; - - Northing = k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 - + (61-58*T+T*T+600*C-330*EE)*A*A*A*A*A*A/720)); +// Convert the packet status to a ros diagnostic message +inline diagnostic_msgs::DiagnosticStatus filterStatusToMsg(unsigned int const status, int const level, std::string const & failure_message = "", std::string const & success_message = "") { + diagnostic_msgs::DiagnosticStatus status_msg; + status_msg.message = success_message; + status_msg.level = 0; // default OK state + if (!status) { + status_msg.level = level; // WARN=1 ERROR=2 + status_msg.message = failure_message; + } + return status_msg; } - - - - - int main(int argc, char *argv[]) { // Set up ROS node // ros::init(argc, argv, "an_device"); @@ -180,87 +108,68 @@ int main(int argc, char *argv[]) { } ROS_INFO("port:%s@%d", com_port, baud_rate); - - // If a UTM Zone is provided, publish transforms. - // The zone is static to avoid problems due to changing when near a Zone boundary. - int utm_zone; - std::string tf_name = nh.getNamespace(); - tf::Transform transform; - if (nh.getParam("utm_zone", utm_zone)) { - LongOriginRad = (utm_zone*6 - 183) * deg2rad; - ROS_INFO("using UTM Zone %d to publish transform %s", utm_zone, tf_name.c_str()); - - } - + // Initialise Publishers and Topics // + ros::Publisher nav_sat_fix_pub=nh.advertise("nav_sat_fix",10); + ros::Publisher imu_pub=nh.advertise("imu",10); + ros::Publisher odom_pub=nh.advertise("odom",10); + ros::Publisher diagnostics_pub=nh.advertise("diagnostics",10); - // Initialise Publishers, and Subscribers // - ros::Publisher nav_sat_fix_pub=nh.advertise("NavSatFix",10); - ros::Publisher twist_pub=nh.advertise("Twist",10); - ros::Publisher imu_pub=nh.advertise("Imu",10); - ros::Publisher system_status_pub=nh.advertise("SystemStatus",10); - ros::Publisher filter_status_pub=nh.advertise("FilterStatus",10); - ros::Publisher odom_pub = nh.advertise("odom", 50); - ros::Subscriber rtcm_sub = nh.subscribe("rtcm", 1000, handle_rtcm); - - // Initialise messages + // Initialise gps message sensor_msgs::NavSatFix nav_sat_fix_msg; - nav_sat_fix_msg.header.stamp.sec=0; - nav_sat_fix_msg.header.stamp.nsec=0; - nav_sat_fix_msg.header.frame_id='0'; // fixed - nav_sat_fix_msg.status.status=0; - nav_sat_fix_msg.status.service=1; // fixed to GPS - nav_sat_fix_msg.latitude=0.0; - nav_sat_fix_msg.longitude=0.0; - nav_sat_fix_msg.altitude=0.0; - nav_sat_fix_msg.position_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + nav_sat_fix_msg.header.frame_id="an_device"; nav_sat_fix_msg.position_covariance_type=2; // fixed to variance on the diagonal - - geometry_msgs::Twist twist_msg; - twist_msg.linear.x=0.0; - twist_msg.linear.y=0.0; - twist_msg.linear.z=0.0; - twist_msg.angular.x=0.0; - twist_msg.angular.y=0.0; - twist_msg.angular.z=0.0; - + nav_sat_fix_msg.position_covariance = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + // Initialize wheel odometry message + nav_msgs::Odometry odom_msg; + odom_msg.header.frame_id = "odom"; + odom_msg.child_frame_id = "an_device"; + odom_msg.twist.covariance = { + 1.0, 0 , 0 , 0, 0, 0, + 0 , 1.0, 0 , 0, 0, 0, + 0 , 0 , 1.0, 0, 0, 0, + 0 , 0 , 0 , 0, 0, 0, + 0 , 0 , 0 , 0, 0, 0, + 0 , 0 , 0 , 0, 0, 0 + }; + + // Initialize imu message sensor_msgs::Imu imu_msg; - imu_msg.header.stamp.sec=0; - imu_msg.header.stamp.nsec=0; - imu_msg.header.frame_id='0'; // fixed - imu_msg.orientation.x=0.0; - imu_msg.orientation.y=0.0; - imu_msg.orientation.z=0.0; - imu_msg.orientation.w=0.0; - imu_msg.orientation_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - imu_msg.angular_velocity.x=0.0; - imu_msg.angular_velocity.y=0.0; - imu_msg.angular_velocity.z=0.0; - imu_msg.angular_velocity_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; // fixed - imu_msg.linear_acceleration.x=0.0; - imu_msg.linear_acceleration.y=0.0; - imu_msg.linear_acceleration.z=0.0; - imu_msg.linear_acceleration_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; // fixed - - diagnostic_msgs::DiagnosticStatus system_status_msg; - system_status_msg.level = 0; // default OK state - system_status_msg.name = "System Status"; - system_status_msg.message = ""; - - diagnostic_msgs::DiagnosticStatus filter_status_msg; - filter_status_msg.level = 0; // default OK state - filter_status_msg.name = "Filter Status"; - filter_status_msg.message = ""; - + imu_msg.header.frame_id = "an_device"; + imu_msg.orientation_covariance = { // Will be read from device + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0}; + imu_msg.angular_velocity_covariance = { // Cannot be read from device + 0.2, 0.0, 0.0, + 0.0, 0.2, 0.0, + 0.0, 0.0, 0.2}; + imu_msg.linear_acceleration_covariance = { // Cannot be read from device + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0}; + + // Status messages + diagnostic_msgs::DiagnosticArray diagnostics_msg; + // get data from com port // an_decoder_t an_decoder; an_packet_t *an_packet; - system_state_packet_t system_state_packet; - quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet; - int bytes_received; - - an_decoder_initialise(&an_decoder); + // packet types + system_state_packet_t system_state_packet; // Packet 20 + euler_orientation_standard_deviation_packet_t euler_orientation_standard_deviation_packet; // Packet 26 + body_velocity_packet_t body_velocity_packet; // Packet 36 + quaternion_orientation_packet_t quaternion_orientation_packet; // Packet 40 + + if (OpenComport(com_port, baud_rate)) + { + printf("Could not open serial port: %s \n",com_port); + exit(EXIT_FAILURE); + } + an_decoder_initialise(&an_decoder); + int bytes_received; // Loop continuously, polling for packets while (ros::ok()) @@ -269,345 +178,159 @@ int main(int argc, char *argv[]) { { // increment the decode buffer length by the number of bytes received // an_decoder_increment(&an_decoder, bytes_received); - + // decode all the packets in the buffer // while ((an_packet = an_packet_decode(&an_decoder)) != NULL) { - // acknowledgement packet // - if (an_packet->id == 0) + + // acknowledgement packet (0) // + if (an_packet->id == 0) { ROS_INFO("acknowledgement data: %d", an_packet->data[3]); } - // receiver information packet // - if (an_packet->id == 69) - { - ROS_INFO("receiver information: %d", an_packet->data[0]); - } - // system state packet // - if (an_packet->id == packet_id_system_state) + // system state packet (20) // + if (an_packet->id == packet_id_system_state) { if(decode_system_state_packet(&system_state_packet, an_packet) == 0) - { + { // NavSatFix nav_sat_fix_msg.header.stamp.sec=system_state_packet.unix_time_seconds; nav_sat_fix_msg.header.stamp.nsec=system_state_packet.microseconds*1000; - if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || // 2D - (system_state_packet.filter_status.b.gnss_fix_type == 2)) // 3D + if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || + (system_state_packet.filter_status.b.gnss_fix_type == 2)) { - nav_sat_fix_msg.status.status=0; // no fix + nav_sat_fix_msg.status.status=0; } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || // SBAS - (system_state_packet.filter_status.b.gnss_fix_type == 5)) // Omnistar/Starfire + else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || + (system_state_packet.filter_status.b.gnss_fix_type == 5)) { - nav_sat_fix_msg.status.status=1; // SBAS + nav_sat_fix_msg.status.status=1; } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || // differential - (system_state_packet.filter_status.b.gnss_fix_type == 6) || // RTK float - (system_state_packet.filter_status.b.gnss_fix_type == 7)) // RTK fixed + else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || + (system_state_packet.filter_status.b.gnss_fix_type == 6) || + (system_state_packet.filter_status.b.gnss_fix_type == 7)) { - nav_sat_fix_msg.status.status=2; // GBAS + nav_sat_fix_msg.status.status=2; } - else + else { nav_sat_fix_msg.status.status=-1; } nav_sat_fix_msg.latitude=system_state_packet.latitude * RADIANS_TO_DEGREES; nav_sat_fix_msg.longitude=system_state_packet.longitude * RADIANS_TO_DEGREES; nav_sat_fix_msg.altitude=system_state_packet.height; - nav_sat_fix_msg.position_covariance={pow(system_state_packet.standard_deviation[1],2), 0.0, 0.0, - 0.0, pow(system_state_packet.standard_deviation[0],2), 0.0, - 0.0, 0.0, pow(system_state_packet.standard_deviation[2],2)}; - - // Twist - twist_msg.linear.x=system_state_packet.velocity[0]; - twist_msg.linear.y=system_state_packet.velocity[1]; - twist_msg.linear.z=system_state_packet.velocity[2]; - twist_msg.angular.x=system_state_packet.angular_velocity[0]; - twist_msg.angular.y=system_state_packet.angular_velocity[1]; - twist_msg.angular.z=system_state_packet.angular_velocity[2]; - + nav_sat_fix_msg.position_covariance[0] = pow(system_state_packet.standard_deviation[1],2); + nav_sat_fix_msg.position_covariance[4] = pow(system_state_packet.standard_deviation[0],2); + nav_sat_fix_msg.position_covariance[8] = pow(system_state_packet.standard_deviation[2],2); + // IMU imu_msg.header.stamp.sec=system_state_packet.unix_time_seconds; imu_msg.header.stamp.nsec=system_state_packet.microseconds*1000; + imu_msg.linear_acceleration.x = system_state_packet.body_acceleration[0]; + imu_msg.linear_acceleration.y = system_state_packet.body_acceleration[1]; + imu_msg.linear_acceleration.z = system_state_packet.body_acceleration[2]; + imu_msg.angular_velocity.x = system_state_packet.angular_velocity[0]; + imu_msg.angular_velocity.y = system_state_packet.angular_velocity[1]; + imu_msg.angular_velocity.z = system_state_packet.angular_velocity[2]; - // Convert roll, pitch, yaw from radians to quaternion format // - tf::Quaternion orientation; - orientation.setRPY( - system_state_packet.orientation[0], - system_state_packet.orientation[1], - PI / 2.0f - system_state_packet.orientation[2] // REP 103 - ); - imu_msg.orientation.x = orientation[0]; - imu_msg.orientation.y = orientation[1]; - imu_msg.orientation.z = orientation[2]; - imu_msg.orientation.w = orientation[3]; - - imu_msg.angular_velocity.x=system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values - imu_msg.angular_velocity.y=system_state_packet.angular_velocity[1]; - imu_msg.angular_velocity.z=system_state_packet.angular_velocity[2]; - imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0]; - imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; - imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; - - // transform - if (utm_zone) { - static tf::TransformBroadcaster transform_br; - double N, E; - - LLtoUTM( - utm_zone, - system_state_packet.latitude, - system_state_packet.longitude, - N, E - ); - - transform.setOrigin(tf::Vector3( - E, N, system_state_packet.height - )); - - transform.setRotation(orientation); - - transform_br.sendTransform(tf::StampedTransform( - transform, - ros::Time( - system_state_packet.unix_time_seconds, - system_state_packet.microseconds*1000 - ), - "world", // Is it reasonable to hardcode this? - tf_name - )); - - nav_msgs::Odometry odom; - odom.header.stamp.sec=system_state_packet.unix_time_seconds; - odom.header.stamp.nsec=system_state_packet.microseconds*1000; - - odom.header.frame_id= "world"; // fix this! - odom.child_frame_id = "ins"; // fix this! - - //set the position - odom.pose.pose.position.x = E; - odom.pose.pose.position.y = N; - odom.pose.pose.position.z = system_state_packet.height; - odom.pose.pose.orientation.x = orientation[0]; - odom.pose.pose.orientation.y = orientation[1]; - odom.pose.pose.orientation.z = orientation[2]; - odom.pose.pose.orientation.w = orientation[3]; - - // Is this correct??? - odom.pose.covariance={ - pow(system_state_packet.standard_deviation[1],2), 0.0, 0.0, - 0.0, pow(system_state_packet.standard_deviation[0],2), 0.0, - 0.0, 0.0, pow(system_state_packet.standard_deviation[2],2) - }; - - //set the velocity - odom.twist.twist.linear.x = system_state_packet.velocity[0]; - odom.twist.twist.linear.y = system_state_packet.velocity[1]; - odom.twist.twist.linear.z = system_state_packet.velocity[2]; - - // Set angular velocity. - odom.twist.twist.angular.x = system_state_packet.angular_velocity[0]; - odom.twist.twist.angular.y = system_state_packet.angular_velocity[1]; - odom.twist.twist.angular.z = system_state_packet.angular_velocity[2]; - - //publish the message - odom_pub.publish(odom); + // Filter Status + diagnostics_msg.header.stamp.sec=system_state_packet.unix_time_seconds; + diagnostics_msg.header.stamp.nsec=system_state_packet.microseconds*1000; + diagnostics_msg.status.clear(); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.orientation_filter_initialised, 1, "Orientation Filter NOT Initialised", "Orientation Filter Initialised")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.ins_filter_initialised , 1, "Navigation Filter NOT Initialised", "Navigation Filter Initialised")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.heading_initialised , 1, "Heading NOT Initialised", "Heading Initialised")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.utc_time_initialised , 1, "UTC Time NOT Initialised", "UTC Time Initialised")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.event1_flag , 1, "Event 1 NOT Occurred", "Event 1 Occurred")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.event2_flag , 1, "Event 2 NOT Occurred", "Event 2 Occurred")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.internal_gnss_enabled , 1, "Internal GNSS NOT Enabled", "Internal GNSS Enabled")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.magnetic_heading_enabled , 1, "Magnetic Heading NOT Active", "Magnetic Heading Active")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.velocity_heading_enabled , 1, "Velocity Heading NOT Enabled", "Velocity Heading Enabled")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.atmospheric_altitude_enabled , 1, "Atmospheric Altitude NOT Enabled", "Atmospheric Altitude Enabled")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.external_position_active , 1, "External Position NOT Active", "External Position Active")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.external_velocity_active , 1, "External Velocity NOT Active", "External Velocity Active")); + diagnostics_msg.status.push_back(filterStatusToMsg(system_state_packet.filter_status.b.external_heading_active , 1, "External Heading NOT Active", "External Heading Active")); + + // System Status + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.system_failure , 2, "System Failure!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.system_failure , 2, "System Failure!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.accelerometer_sensor_failure, 2, "Accelerometer Sensor Failure!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.gyroscope_sensor_failure , 2, "Gyroscope Sensor Failure!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.magnetometer_sensor_failure , 2, "Magnetometer Sensor Failure!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.pressure_sensor_failure , 2, "Pressure Sensor Failure!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.gnss_failure , 2, "GNSS Failure!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.accelerometer_over_range , 2, "Accelerometer Over Range!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.gyroscope_over_range , 2, "Gyroscope Over Range!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.magnetometer_over_range , 2, "Magnetometer Over Range!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.pressure_over_range , 2, "Pressure Over Range!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.minimum_temperature_alarm , 2, "Minimum Temperature Alarm!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.maximum_temperature_alarm , 2, "Maximum Temperature Alarm!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.low_voltage_alarm , 2, "Low Voltage Alarm!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.high_voltage_alarm , 2, "High Voltage Alarm!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.gnss_antenna_disconnected , 2, "GNSS Antenna Disconnected!"); + appendSystemStatus(diagnostics_msg, system_state_packet.system_status.b.serial_port_overflow_alarm , 2, "Data Output Overflow Alarm!"); - } + } + } - // System Status - system_status_msg.message = ""; - system_status_msg.level = 0; // default OK state - if (system_state_packet.system_status.b.system_failure) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "0. System Failure! "; - } - if (system_state_packet.system_status.b.accelerometer_sensor_failure) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "1. Accelerometer Sensor Failure! "; - } - if (system_state_packet.system_status.b.gyroscope_sensor_failure) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "2. Gyroscope Sensor Failure! "; - } - if (system_state_packet.system_status.b.magnetometer_sensor_failure) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "3. Magnetometer Sensor Failure! "; - } - if (system_state_packet.system_status.b.pressure_sensor_failure) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "4. Pressure Sensor Failure! "; - } - if (system_state_packet.system_status.b.gnss_failure) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "5. GNSS Failure! "; - } - if (system_state_packet.system_status.b.accelerometer_over_range) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "6. Accelerometer Over Range! "; - } - if (system_state_packet.system_status.b.gyroscope_over_range) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "7. Gyroscope Over Range! "; - } - if (system_state_packet.system_status.b.magnetometer_over_range) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "8. Magnetometer Over Range! "; - } - if (system_state_packet.system_status.b.pressure_over_range) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "9. Pressure Over Range! "; - } - if (system_state_packet.system_status.b.minimum_temperature_alarm) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "10. Minimum Temperature Alarm! "; - } - if (system_state_packet.system_status.b.maximum_temperature_alarm) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "11. Maximum Temperature Alarm! "; - } - if (system_state_packet.system_status.b.low_voltage_alarm) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "12. Low Voltage Alarm! "; - } - if (system_state_packet.system_status.b.high_voltage_alarm) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "13. High Voltage Alarm! "; - } - if (system_state_packet.system_status.b.gnss_antenna_disconnected) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "14. GNSS Antenna Disconnected! "; - } - if (system_state_packet.system_status.b.serial_port_overflow_alarm) { - system_status_msg.level = 2; // ERROR state - system_status_msg.message = system_status_msg.message + "15. Data Output Overflow Alarm! "; - } - - // Filter Status - filter_status_msg.message = ""; - filter_status_msg.level = 0; // default OK state - if (system_state_packet.filter_status.b.orientation_filter_initialised) { - filter_status_msg.message = filter_status_msg.message + "0. Orientation Filter Initialised. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "0. Orientation Filter NOT Initialised. "; - } - if (system_state_packet.filter_status.b.ins_filter_initialised) { - filter_status_msg.message = filter_status_msg.message + "1. Navigation Filter Initialised. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "1. Navigation Filter NOT Initialised. "; - } - if (system_state_packet.filter_status.b.heading_initialised) { - filter_status_msg.message = filter_status_msg.message + "2. Heading Initialised. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "2. Heading NOT Initialised. "; - } - if (system_state_packet.filter_status.b.utc_time_initialised) { - filter_status_msg.message = filter_status_msg.message + "3. UTC Time Initialised. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "3. UTC Time NOT Initialised. "; - } - if (system_state_packet.filter_status.b.event1_flag) { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "7. Event 1 Occured. "; - } - else { - filter_status_msg.message = filter_status_msg.message + "7. Event 1 NOT Occured. "; - } - if (system_state_packet.filter_status.b.event2_flag) { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "8. Event 2 Occured. "; - } - else { - filter_status_msg.message = filter_status_msg.message + "8. Event 2 NOT Occured. "; - } - if (system_state_packet.filter_status.b.internal_gnss_enabled) { - filter_status_msg.message = filter_status_msg.message + "9. Internal GNSS Enabled. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "9. Internal GNSS NOT Enabled. "; - } - if (system_state_packet.filter_status.b.magnetic_heading_enabled) { - filter_status_msg.message = filter_status_msg.message + "10. Magnetic Heading Active. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "10. Magnetic Heading NOT Active. "; - } - if (system_state_packet.filter_status.b.velocity_heading_enabled) { - filter_status_msg.message = filter_status_msg.message + "11. Velocity Heading Enabled. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "11. Velocity Heading NOT Enabled. "; - } - if (system_state_packet.filter_status.b.atmospheric_altitude_enabled) { - filter_status_msg.message = filter_status_msg.message + "12. Atmospheric Altitude Enabled. "; - } - else { - filter_status_msg.message = filter_status_msg.message + "12. Atmospheric Altitude NOT Enabled. "; - filter_status_msg.level = 1; // WARN state - } - if (system_state_packet.filter_status.b.external_position_active) { - filter_status_msg.message = filter_status_msg.message + "13. External Position Active. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "13. External Position NOT Active. "; - } - if (system_state_packet.filter_status.b.external_velocity_active) { - filter_status_msg.message = filter_status_msg.message + "14. External Velocity Active. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "14. External Velocity NOT Active. "; - } - if (system_state_packet.filter_status.b.external_heading_active) { - filter_status_msg.message = filter_status_msg.message + "15. External Heading Active. "; - } - else { - filter_status_msg.level = 1; // WARN state - filter_status_msg.message = filter_status_msg.message + "15. External Heading NOT Active. "; - } + // euler orientation standard deviation packet (26) // + if (an_packet->id == packet_id_euler_orientation_standard_deviation) + { + if(decode_euler_orientation_standard_deviation_packet(&euler_orientation_standard_deviation_packet, an_packet) == 0) + { + imu_msg.orientation_covariance[0] = pow(euler_orientation_standard_deviation_packet.standard_deviation[0],2); + imu_msg.orientation_covariance[4] = pow(euler_orientation_standard_deviation_packet.standard_deviation[1],2); + imu_msg.orientation_covariance[8] = pow(euler_orientation_standard_deviation_packet.standard_deviation[2],2); } } - - // quaternion orientation standard deviation packet // - if (an_packet->id == packet_id_quaternion_orientation_standard_deviation) + + // body velocity packet (36) + if (an_packet->id == packet_id_body_velocity) { + if(decode_body_velocity_packet(&body_velocity_packet, an_packet) == 0) { + odom_msg.header.stamp.sec=system_state_packet.unix_time_seconds; + odom_msg.header.stamp.nsec=system_state_packet.microseconds*1000; + odom_msg.twist.twist.linear.x = body_velocity_packet.velocity[0]; + odom_msg.twist.twist.linear.y = body_velocity_packet.velocity[1]; // Should be close to zero + odom_msg.twist.twist.linear.z = body_velocity_packet.velocity[2]; // Should be close to zero + } + } + + + // quaternion orientation packet (40) // + if (an_packet->id == packet_id_quaternion_orientation) { - // copy all the binary data into the typedef struct for the packet // - // this allows easy access to all the different values // - if(decode_quaternion_orientation_standard_deviation_packet(&quaternion_orientation_standard_deviation_packet, an_packet) == 0) - { - // IMU - imu_msg.orientation_covariance[0] = quaternion_orientation_standard_deviation_packet.standard_deviation[0]; - imu_msg.orientation_covariance[4] = quaternion_orientation_standard_deviation_packet.standard_deviation[1]; - imu_msg.orientation_covariance[8] = quaternion_orientation_standard_deviation_packet.standard_deviation[2]; + if(decode_quaternion_orientation_packet(&quaternion_orientation_packet, an_packet) == 0) + { + imu_msg.orientation.x = quaternion_orientation_packet.orientation[1]; // AN reports in s (w?), x, y, z + imu_msg.orientation.y = quaternion_orientation_packet.orientation[2]; + imu_msg.orientation.z = quaternion_orientation_packet.orientation[3]; + imu_msg.orientation.w = quaternion_orientation_packet.orientation[0]; + imu_msg.orientation = nedToEnu(imu_msg.orientation); // AN reports in NED, ROS expects ENU } } - // Ensure that you free the an_packet when you're done with it // + + + + // receiver information packet (69) // + if (an_packet->id == 69) + { + ROS_INFO("receiver information: %d", an_packet->data[0]); + } + + + // Ensure that you free the an_packet when your done with it // // or you will leak memory // an_packet_free(&an_packet); // Publish messages // nav_sat_fix_pub.publish(nav_sat_fix_msg); - twist_pub.publish(twist_msg); + odom_pub.publish(odom_msg); imu_pub.publish(imu_msg); - system_status_pub.publish(system_status_msg); - filter_status_pub.publish(filter_status_msg); + diagnostics_pub.publish(diagnostics_msg); } } - ros::spinOnce(); } }