From 5d71e46ae0db2f09a4db80f83976c337ac6be4c3 Mon Sep 17 00:00:00 2001 From: Michael Zemb Date: Wed, 8 Sep 2021 11:57:57 +0200 Subject: [PATCH] Feature #44 - odometry output --- CMakeLists.txt | 5 +- config/example/ellipse_E_default.yaml | 8 +- config/example/ellipse_N_default.yaml | 20 +- include/sbg_driver/config_store.h | 51 ++++- include/sbg_driver/message_publisher.h | 9 +- include/sbg_driver/message_wrapper.h | 140 +++++++++++++- launch/examples/sbg_ellipseN.launch | 2 +- package.xml | 1 + src/config_store.cpp | 35 ++++ src/message_publisher.cpp | 74 +++++++- src/message_wrapper.cpp | 248 +++++++++++++++++++++++++ 11 files changed, 571 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 90fbbe2..6897afe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,9 @@ find_package(catkin REQUIRED COMPONENTS std_srvs geometry_msgs message_generation + gps_common + tf2_ros + tf2_msgs ) ################################################ @@ -147,4 +150,4 @@ install(DIRECTORY config/ ############# ## Testing ## -############# \ No newline at end of file +############# diff --git a/config/example/ellipse_E_default.yaml b/config/example/ellipse_E_default.yaml index f9f507b..24ef11c 100644 --- a/config/example/ellipse_E_default.yaml +++ b/config/example/ellipse_E_default.yaml @@ -15,14 +15,14 @@ driver: frequency: 400 # Configuration of the device with ROS. -confWithRos: false +confWithRos: true uartConf: # Port Name portName: "/dev/ttyUSB0" # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600) - baudRate: 115200 + baudRate: 921600 # Port Id # 0 PORT_A: Main communication interface. Full duplex. @@ -246,7 +246,7 @@ output: # Ros standard output: # Note: If true publish ROS standard messages. - ros_standard: false + ros_standard: true # Frame convention: # Note: If true messages are expressed in the ENU convention. @@ -267,7 +267,7 @@ output: # Includes roll, pitch, yaw and their accuracies on each axis log_ekf_euler: 8 # Includes the 4 quaternions values - log_ekf_quat: 0 + log_ekf_quat: 8 # Position and velocities in NED coordinates with the accuracies on each axis log_ekf_nav: 8 # Heave, surge and sway and accelerations on each axis for up to 4 points diff --git a/config/example/ellipse_N_default.yaml b/config/example/ellipse_N_default.yaml index 07bb765..434d455 100644 --- a/config/example/ellipse_N_default.yaml +++ b/config/example/ellipse_N_default.yaml @@ -14,6 +14,16 @@ driver: # it can lead to miss data frame and less accurate time stamping. frequency: 400 +odometry: + # Enable ROS odometry messages. + enable: true + # Publish odometry transforms. + publishTf: true + # Odometry frame IDs. + odomFrameId: "odom" + baseFrameId: "base_link" + initFrameId: "map" + # Configuration of the device with ROS. confWithRos: false @@ -22,7 +32,7 @@ uartConf: portName: "/dev/ttyUSB0" # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600) - baudRate: 115200 + baudRate: 921600 # Port Id # 0 PORT_A: Main communication interface. Full duplex. @@ -246,7 +256,7 @@ output: # Ros standard output: # Note: If true publish ROS standard messages. - ros_standard: false + ros_standard: true # Frame convention: # Note: If true messages are expressed in the ENU convention. @@ -265,11 +275,11 @@ output: # Includes IMU status, acc., gyro, temp delta speeds and delta angles values log_imu_data: 8 # Includes roll, pitch, yaw and their accuracies on each axis - log_ekf_euler: 0 + log_ekf_euler: 8 # Includes the 4 quaternions values - log_ekf_quat: 0 + log_ekf_quat: 8 # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 0 + log_ekf_nav: 8 # Heave, surge and sway and accelerations on each axis for up to 4 points log_ship_motion: 0 # Provides UTC time reference diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index a2773bb..db4b2b5 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -115,6 +115,12 @@ class ConfigStore std::string m_frame_id_; bool m_use_enu_; + bool m_odom_enable_; + bool m_odom_publish_tf_; + std::string m_odom_frame_id_; + std::string m_odom_base_frame_id_; + std::string m_odom_init_frame_id_; + //---------------------------------------------------------------------// //- Private methods -// //---------------------------------------------------------------------// @@ -152,6 +158,13 @@ class ConfigStore */ void loadDriverParameters(const ros::NodeHandle& ref_node_handle); + /*! + * Load odometry parameters. + * + * \param[in] ref_node_handle ROS nodeHandle. + */ + void loadOdomParameters(const ros::NodeHandle& ref_node_handle); + /*! * Load interface communication parameters. * @@ -445,6 +458,43 @@ class ConfigStore */ bool getUseEnu(void) const; + /*! + * Get odom enable. + * + * \return True if the odometry is enabled. + */ + bool getOdomEnable(void) const; + + /*! + * Get odom publish_tf. + * + * \return If true publish odometry transforms. + */ + bool getOdomPublishTf(void) const; + + /*! + * Get the odometry frame ID. + * + * \return Odometry frame ID. + */ + const std::string &getOdomFrameId(void) const; + + + /*! + * Get the odometry base frame ID. + * + * \return Odometry base frame ID. + */ + const std::string &getOdomBaseFrameId(void) const; + + + /*! + * Get the odometry init frame ID. + * + * \return Odometry init frame ID. + */ + const std::string &getOdomInitFrameId(void) const; + /*! * Get the time reference. * @@ -452,7 +502,6 @@ class ConfigStore */ TimeReference getTimeReference(void) const; - //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h index af25b2a..9ca0fe9 100644 --- a/include/sbg_driver/message_publisher.h +++ b/include/sbg_driver/message_publisher.h @@ -80,6 +80,7 @@ class MessagePublisher ros::Publisher m_velocity_pub_; ros::Publisher m_utc_reference_pub_; ros::Publisher m_nav_sat_fix_pub_; + ros::Publisher m_odometry_pub_; MessageWrapper m_message_wrapper_; uint32_t m_max_messages_; @@ -111,8 +112,9 @@ class MessagePublisher * Define standard ROS publishers. * * \param[in] ref_ros_node_handle Ros NodeHandle to advertise the publisher. + * \param[in] odom_enable If true, enable odometry messages. */ - void defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle); + void defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle, bool odom_enable); /*! * Publish a received SBG IMU log. @@ -131,6 +133,11 @@ class MessagePublisher */ void processRosImuMessage(void); + /*! + * Process a ROS odometry standard message. + */ + void processRosOdoMessage(void); + /*! * Publish a received SBG Magnetic log. * diff --git a/include/sbg_driver/message_wrapper.h b/include/sbg_driver/message_wrapper.h index 99578d6..05263c7 100644 --- a/include/sbg_driver/message_wrapper.h +++ b/include/sbg_driver/message_wrapper.h @@ -52,6 +52,10 @@ #include #include #include +#include +#include +#include +#include // SbgRos message headers #include "sbg_driver/SbgStatus.h" @@ -74,6 +78,14 @@ namespace sbg { +typedef struct _UTM0 +{ + double easting; + double northing; + double altitude; + int zone; +} UTM0; + /*! * Class to wrap the SBG logs into ROS messages. */ @@ -81,12 +93,21 @@ class MessageWrapper { private: - ros::Time m_ros_processing_time_; - sbg_driver::SbgUtcTime m_last_sbg_utc_; - bool m_first_valid_utc_; - std::string m_frame_id_; - bool m_use_enu_; - TimeReference m_time_reference_; + ros::Time m_ros_processing_time_; + sbg_driver::SbgUtcTime m_last_sbg_utc_; + bool m_first_valid_utc_; + std::string m_frame_id_; + bool m_use_enu_; + TimeReference m_time_reference_; + UTM0 m_utm0_; + tf2_ros::TransformBroadcaster m_tf_broadcaster_; + tf2_ros::StaticTransformBroadcaster m_static_tf_broadcaster_; + + bool m_odom_enable_; + bool m_odom_publish_tf_; + std::string m_odom_frame_id_; + std::string m_odom_base_frame_id_; + std::string m_odom_init_frame_id_; //---------------------------------------------------------------------// //- Internal methods -// @@ -108,6 +129,14 @@ class MessageWrapper */ float wrapAngle360(float angle_deg) const; + /*! + * Compute UTM zone meridian. + * + * \param[in] zone UTM Zone. + * \return Meridian angle, in degrees. + */ + double computeMeridian(std::string& utm_zone) const; + /*! * Create a ROS message header. * @@ -254,6 +283,36 @@ class MessageWrapper */ const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg::SbgVector3f& body_vel, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const; + /*! + * Fill a transformation. + * + * \param[in] ref_parent_frame_id Parent frame ID. + * \param[in] ref_child_frame_id Child frame ID. + * \param[in] ref_pose Pose. + * \param[out] ref_transform_stamped Stamped transformation. + */ + void fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::Pose &ref_pose, geometry_msgs::TransformStamped &ref_transform_stamped); + + /*! + * Set UTM initial position. + * + * \param[in] Lat Latitude, in degrees. + * \param[in] Long Longitude, in degrees. + * \param[in] altitude Altitude, in meters. + */ + void initUTM(double Lat, double Long, double altitude); + + /*! + * Convert latitude and longitude to a position relative to UTM initial position. + * + * \param[in] Lat Latitude, in degrees. + * \param[in] Long Longitude, in degrees. + * \param[in] zoneNumber UTM zone number. + * \param[out] UTMNorthing UTM northing, in meters. + * \param[out] UTMEasting UTM easting, in meters. + */ + void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const; + public: //---------------------------------------------------------------------// @@ -290,6 +349,41 @@ class MessageWrapper */ void setUseEnu(bool enu); + /*! + * Set odom enable. + * + * \param[in] odom_enable If true enable odometry. + */ + void setOdomEnable(bool odom_enable); + + /*! + * Set odom publish_tf. + * + * \param[in] publish_tf If true publish odometry transforms. + */ + void setOdomPublishTf(bool publish_tf); + + /*! + * Set the odometry frame ID. + * + * \param[in] ref_frame_id Odometry frame ID. + */ + void setOdomFrameId(const std::string &ref_frame_id); + + /*! + * Set the odometry base frame ID. + * + * \param[in] ref_frame_id Odometry base frame ID. + */ + void setOdomBaseFrameId(const std::string &ref_frame_id); + + /*! + * Set the odometry init frame ID. + * + * \param[in] ref_frame_id Odometry init frame ID. + */ + void setOdomInitFrameId(const std::string &ref_frame_id); + //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// @@ -439,6 +533,38 @@ class MessageWrapper */ const sensor_msgs::Imu createRosImuMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg, const sbg_driver::SbgEkfQuat& ref_sbg_quat_msg) const; + /*! + * Create a ROS standard odometry message from SBG messages. + * + * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. + * \param[in] ref_sbg_ekf_nav_msg SBG-ROS Ekf Nav message. + * \param[in] ref_sbg_ekf_quat_msg SBG-ROS Ekf Quaternion message. + * \param[in] ref_sbg_ekf_euler_msg SBG-ROS Ekf Euler message. + * \return ROS standard odometry message. + */ + const nav_msgs::Odometry createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_sbg_ekf_nav_msg, const sbg_driver::SbgEkfQuat &ref_sbg_ekf_quat_msg, const sbg_driver::SbgEkfEuler &ref_sbg_ekf_euler_msg); + + /*! + * Create a ROS standard odometry message from SBG messages. + * + * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. + * \param[in] ref_sbg_ekf_nav_msg SBG-ROS Ekf Nav message. + * \param[in] ref_sbg_ekf_euler_msg SBG-ROS Ekf Euler message. + * \return ROS standard odometry message. + */ + const nav_msgs::Odometry createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_sbg_ekf_nav_msg, const sbg_driver::SbgEkfEuler &ref_sbg_ekf_euler_msg); + + /*! + * Create a ROS standard odometry message from SBG messages and tf2 quaternion. + * + * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. + * \param[in] ref_sbg_ekf_nav_msg SBG-ROS Ekf Nav message. + * \param[in] orientation Orientation as a Tf2 quaternion. + * \param[in] ref_sbg_ekf_euler_msg SBG-ROS Ekf Euler message. + * \return ROS standard odometry message. + */ + const nav_msgs::Odometry createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_sbg_ekf_nav_msg, const tf2::Quaternion &ref_orientation, const sbg_driver::SbgEkfEuler &ref_sbg_ekf_euler_msg); + /*! * Create a ROS standard Temperature message from SBG message. * @@ -458,7 +584,7 @@ class MessageWrapper /*! * Create a ROS standard TwistStamped message from SBG messages. * - * \param[in] ref_sbg_ekf_euler_msg SBG-ROS Ekf Euler message. + * \param[in] ref_sbg_ekf_vel_msg SBG-ROS Ekf velocity message. * \param[in] ref_sbg_ekf_nav_msg SBG-ROS Ekf Nav message. * \param[in] ref_sbg_imu_msg SBG-ROS IMU message. * \return ROS standard TwistStamped message. diff --git a/launch/examples/sbg_ellipseN.launch b/launch/examples/sbg_ellipseN.launch index e027360..a3138aa 100644 --- a/launch/examples/sbg_ellipseN.launch +++ b/launch/examples/sbg_ellipseN.launch @@ -2,4 +2,4 @@ - \ No newline at end of file + diff --git a/package.xml b/package.xml index 66ac6d3..8e7dcef 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,7 @@ std_srvs geometry_msgs message_generation + gps_common roscpp sensor_msgs diff --git a/src/config_store.cpp b/src/config_store.cpp index 7ff5222..a359f27 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -28,6 +28,15 @@ void ConfigStore::loadDriverParameters(const ros::NodeHandle& ref_node_handle) m_rate_frequency_ = getParameter(ref_node_handle, "driver/frequency", 400); } +void ConfigStore::loadOdomParameters(const ros::NodeHandle& ref_node_handle) +{ + ref_node_handle.param ("odometry/enable" , m_odom_enable_ , false); + ref_node_handle.param ("odometry/publishTf", m_odom_publish_tf_ , false); + ref_node_handle.param("odometry/odomFrameId", m_odom_frame_id_ , "odom"); + ref_node_handle.param("odometry/baseFrameId", m_odom_base_frame_id_ , "base_link"); + ref_node_handle.param("odometry/initFrameId", m_odom_init_frame_id_ , "map"); +} + void ConfigStore::loadCommunicationParameters(const ros::NodeHandle& ref_node_handle) { ref_node_handle.param("confWithRos", m_configure_through_ros_, false); @@ -336,6 +345,31 @@ sbg::TimeReference ConfigStore::getTimeReference(void) const return m_time_reference_; } +bool ConfigStore::getOdomEnable(void) const +{ + return m_odom_enable_; +} + +bool ConfigStore::getOdomPublishTf(void) const +{ + return m_odom_publish_tf_; +} + +const std::string &ConfigStore::getOdomFrameId(void) const +{ + return m_odom_frame_id_; +} + +const std::string &ConfigStore::getOdomBaseFrameId(void) const +{ + return m_odom_base_frame_id_; +} + +const std::string &ConfigStore::getOdomInitFrameId(void) const +{ + return m_odom_init_frame_id_; +} + //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// @@ -343,6 +377,7 @@ sbg::TimeReference ConfigStore::getTimeReference(void) const void ConfigStore::loadFromRosNodeHandle(const ros::NodeHandle& ref_node_handle) { loadDriverParameters(ref_node_handle); + loadOdomParameters(ref_node_handle); loadCommunicationParameters(ref_node_handle); loadSensorParameters(ref_node_handle); loadImuAlignementParameters(ref_node_handle); diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp index 9437735..898af1e 100644 --- a/src/message_publisher.cpp +++ b/src/message_publisher.cpp @@ -207,7 +207,7 @@ void MessagePublisher::initPublisher(ros::NodeHandle& ref_ros_node_handle, SbgEC } } -void MessagePublisher::defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle) +void MessagePublisher::defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle, bool odom_enable) { if (m_sbgImuData_pub_ && m_sbgEkfQuat_pub_) { @@ -284,6 +284,18 @@ void MessagePublisher::defineRosStandardPublishers(ros::NodeHandle& ref_ros_node { ROS_WARN("SBG_DRIVER - [Publisher] SBG GPS Pos data output are not configured, the NavSatFix publisher can not be defined."); } + + if (odom_enable) + { + if (m_sbgImuData_pub_ && m_sbgEkfNav_pub_ && m_sbgEkfEuler_pub_) + { + m_odometry_pub_ = ref_ros_node_handle.advertise("imu/odometry", m_max_messages_); + } + else + { + ROS_WARN("SBG_DRIVER - [Publisher] SBG IMU, SBG EKF NAV and SBG EKF Euler outputs are not configured, the odometry publisher can not be defined."); + } + } } void MessagePublisher::publishIMUData(const SbgBinaryLogData &ref_sbg_log) @@ -300,6 +312,7 @@ void MessagePublisher::publishIMUData(const SbgBinaryLogData &ref_sbg_log) processRosImuMessage(); processRosVelMessage(); + processRosOdoMessage(); } void MessagePublisher::processRosVelMessage(void) @@ -328,6 +341,55 @@ void MessagePublisher::processRosImuMessage(void) } } +void MessagePublisher::processRosOdoMessage(void) +{ + if (m_odometry_pub_) + { +#if 1 +#warning "EKF NAV values hack for testing" +static float x = 0; +m_sbg_ekf_nav_message_ = m_sbg_ekf_nav_message_; +m_sbg_ekf_nav_message_.latitude = 48.9107896; +m_sbg_ekf_nav_message_.longitude = 2.1652421; +m_sbg_ekf_nav_message_.altitude = 30.0 + x; +m_sbg_ekf_nav_message_.undulation = 0.0; +m_sbg_ekf_nav_message_.velocity.x = 0.0; +m_sbg_ekf_nav_message_.velocity.y = 0.0; +m_sbg_ekf_nav_message_.velocity.z = 0.0; +m_sbg_ekf_nav_message_.status.position_valid = true; +x += 0.01; +if (x > 10) +{ + x = 0; +} +#endif + if (m_sbg_ekf_nav_message_.status.position_valid) + { + if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_nav_message_.time_stamp) + { + /* + * Odometry message can be generated from quaternion or euler angles. + * Quaternion is prefered if they are available. + */ + if (m_sbgEkfQuat_pub_) + { + if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_quat_message_.time_stamp) + { + m_odometry_pub_.publish(m_message_wrapper_.createRosOdoMessage(m_sbg_imu_message_, m_sbg_ekf_nav_message_, m_sbg_ekf_quat_message_, m_sbg_ekf_euler_message_)); + } + } + else + { + if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_euler_message_.time_stamp) + { + m_odometry_pub_.publish(m_message_wrapper_.createRosOdoMessage(m_sbg_imu_message_, m_sbg_ekf_nav_message_, m_sbg_ekf_euler_message_)); + } + } + } + } + } +} + void MessagePublisher::publishMagData(const SbgBinaryLogData &ref_sbg_log) { sbg_driver::SbgMag sbg_mag_message; @@ -425,6 +487,12 @@ void MessagePublisher::initPublishers(ros::NodeHandle& ref_ros_node_handle, cons m_message_wrapper_.setUseEnu(ref_config_store.getUseEnu()); + m_message_wrapper_.setOdomEnable(ref_config_store.getOdomEnable()); + m_message_wrapper_.setOdomPublishTf(ref_config_store.getOdomPublishTf()); + m_message_wrapper_.setOdomFrameId(ref_config_store.getOdomFrameId()); + m_message_wrapper_.setOdomBaseFrameId(ref_config_store.getOdomBaseFrameId()); + m_message_wrapper_.setOdomInitFrameId(ref_config_store.getOdomInitFrameId()); + for (const ConfigStore::SbgLogOutput &ref_output : ref_output_modes) { initPublisher(ref_ros_node_handle, ref_output.message_id, ref_output.output_mode, getOutputTopicName(ref_output.message_id)); @@ -432,7 +500,7 @@ void MessagePublisher::initPublishers(ros::NodeHandle& ref_ros_node_handle, cons if (ref_config_store.checkRosStandardMessages()) { - defineRosStandardPublishers(ref_ros_node_handle); + defineRosStandardPublishers(ref_ros_node_handle, ref_config_store.getOdomEnable()); } } @@ -484,6 +552,7 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ m_sbg_ekf_euler_message_ = m_message_wrapper_.createSbgEkfEulerMessage(ref_sbg_log.ekfEulerData); m_sbgEkfEuler_pub_.publish(m_sbg_ekf_euler_message_); processRosVelMessage(); + processRosOdoMessage(); } break; @@ -501,6 +570,7 @@ void MessagePublisher::publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_ case SBG_ECOM_LOG_EKF_NAV: publishEkfNavigationData(ref_sbg_log); + processRosOdoMessage(); break; case SBG_ECOM_LOG_SHIP_MOTION: diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp index 6aa7fc8..9e3e35a 100644 --- a/src/message_wrapper.cpp +++ b/src/message_wrapper.cpp @@ -1,6 +1,15 @@ // File header #include "message_wrapper.h" +// ROS headers +#include +#include +#include +#include +#include +#include +#include + // Project headers #include @@ -16,6 +25,10 @@ using sbg::MessageWrapper; MessageWrapper::MessageWrapper(void): m_first_valid_utc_(false) { + m_utm0_.easting = 0.0; + m_utm0_.northing = 0.0; + m_utm0_.altitude = 0.0; + m_utm0_.zone = 0; } //---------------------------------------------------------------------// @@ -52,6 +65,12 @@ float MessageWrapper::wrapAngle360(float angle_deg) const return angle_deg; } +double MessageWrapper::computeMeridian(std::string &utm_zone) const +{ + int zone_number = std::atoi(utm_zone.substr(0,2).c_str()); + return (zone_number == 0) ? 0.0 : (zone_number - 1) * 6.0 - 177.0; +} + const std_msgs::Header MessageWrapper::createRosHeader(uint32_t device_timestamp) const { std_msgs::Header header; @@ -355,6 +374,96 @@ const sbg_driver::SbgAirDataStatus MessageWrapper::createAirDataStatusMessage(co return air_data_status_message; } +void MessageWrapper::initUTM(double Lat, double Long, double altitude) +{ + int zoneNumber; + + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (Long+180)-int((Long+180)/360)*360-180; + + zoneNumber = int((LongTemp + 180)/6) + 1; + + if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) + { + zoneNumber = 32; + } + + // Special zones for Svalbard + if( Lat >= 72.0 && Lat < 84.0 ) + { + if( LongTemp >= 0.0 && LongTemp < 9.0 ) zoneNumber = 31; + else if( LongTemp >= 9.0 && LongTemp < 21.0 ) zoneNumber = 33; + else if( LongTemp >= 21.0 && LongTemp < 33.0 ) zoneNumber = 35; + else if( LongTemp >= 33.0 && LongTemp < 42.0 ) zoneNumber = 37; + } + + m_utm0_.zone = zoneNumber; + m_utm0_.altitude = altitude; + LLtoUTM(Lat, Long, m_utm0_.zone, m_utm0_.northing, m_utm0_.easting); + + ROS_INFO("initialized from lat:%f long:%f UTM zone%d: easting:%fm (%dkm) northing:%fm (%dkm)" + , Lat, Long, m_utm0_.zone, m_utm0_.easting, (int)(m_utm0_.easting)/1000, m_utm0_.northing, (int)(m_utm0_.northing)/1000); +} + +/* + * Modification of gps_common::LLtoUTM() to use a constant UTM zone. + */ +void MessageWrapper::LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const +{ + const double RADIANS_PER_DEGREE = M_PI/180.0; + + // WGS84 Parameters + const double WGS84_A = 6378137.0; // major axis + const double WGS84_E = 0.0818191908; // first eccentricity + + // UTM Parameters + const double UTM_K0 = 0.9996; // scale factor + const double UTM_E2 = (WGS84_E*WGS84_E); // e^2 + + double a = WGS84_A; + double eccSquared = UTM_E2; + double k0 = UTM_K0; + + double LongOrigin; + double eccPrimeSquared; + double N, T, C, A, M; + + // Make sure the longitude is between -180.00 .. 179.9 + double LongTemp = (Long+180)-int((Long+180)/360)*360-180; + + double LatRad = Lat*RADIANS_PER_DEGREE; + double LongRad = LongTemp*RADIANS_PER_DEGREE; + double LongOriginRad; + + // +3 puts origin in middle of zone + LongOrigin = (zoneNumber - 1)*6 - 180 + 3; + LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; + + eccPrimeSquared = (eccSquared)/(1-eccSquared); + + N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); + T = tan(LatRad)*tan(LatRad); + C = eccPrimeSquared*cos(LatRad)*cos(LatRad); + A = cos(LatRad)*(LongRad-LongOriginRad); + + M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad + - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) + + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) + - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); + + UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); + + UTMNorthing = (double)(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*eccPrimeSquared)*A*A*A*A*A*A/720))); + + if(Lat < 0) + { + UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere + } +} + //---------------------------------------------------------------------// //- Parameters -// //---------------------------------------------------------------------// @@ -374,6 +483,31 @@ void MessageWrapper::setUseEnu(bool enu) m_use_enu_ = enu; } +void MessageWrapper::setOdomEnable(bool odom_enable) +{ + m_odom_enable_ = odom_enable; +} + +void MessageWrapper::setOdomPublishTf(bool publish_tf) +{ + m_odom_publish_tf_ = publish_tf; +} + +void MessageWrapper::setOdomFrameId(const std::string &ref_frame_id) +{ + m_odom_frame_id_ = ref_frame_id; +} + +void MessageWrapper::setOdomBaseFrameId(const std::string &ref_frame_id) +{ + m_odom_base_frame_id_ = ref_frame_id; +} + +void MessageWrapper::setOdomInitFrameId(const std::string &ref_frame_id) +{ + m_odom_init_frame_id_ = ref_frame_id; +} + //---------------------------------------------------------------------// //- Operations -// //---------------------------------------------------------------------// @@ -862,6 +996,120 @@ const sensor_msgs::Imu MessageWrapper::createRosImuMessage(const sbg_driver::Sbg return imu_ros_message; } +void MessageWrapper::fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::Pose &ref_pose, geometry_msgs::TransformStamped &refTransformStamped) +{ + tf2::Quaternion q; + + refTransformStamped.header.stamp = ros::Time::now(); + refTransformStamped.header.frame_id = ref_parent_frame_id; + refTransformStamped.child_frame_id = ref_child_frame_id; + + refTransformStamped.transform.translation.x = ref_pose.position.x; + refTransformStamped.transform.translation.y = ref_pose.position.y; + refTransformStamped.transform.translation.z = ref_pose.position.z; + refTransformStamped.transform.rotation.x = ref_pose.orientation.x; + refTransformStamped.transform.rotation.y = ref_pose.orientation.y; + refTransformStamped.transform.rotation.z = ref_pose.orientation.z; + refTransformStamped.transform.rotation.w = ref_pose.orientation.w; + + m_tf_broadcaster_.sendTransform(refTransformStamped); + ROS_INFO("transform %s -> %s (pos:%f,%f,%f)", refTransformStamped.header.frame_id.c_str(), refTransformStamped.child_frame_id.c_str() + , refTransformStamped.transform.translation.x, refTransformStamped.transform.translation.y, refTransformStamped.transform.translation.z); +} + +const nav_msgs::Odometry MessageWrapper::createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_ekf_nav_msg, const sbg_driver::SbgEkfQuat &ref_ekf_quat_msg, const sbg_driver::SbgEkfEuler &ref_ekf_euler_msg) +{ + tf2::Quaternion orientation(ref_ekf_quat_msg.quaternion.x, ref_ekf_quat_msg.quaternion.y, ref_ekf_quat_msg.quaternion.z, ref_ekf_quat_msg.quaternion.w); + + return createRosOdoMessage(ref_sbg_imu_msg, ref_ekf_nav_msg, orientation, ref_ekf_euler_msg); +} + +const nav_msgs::Odometry MessageWrapper::createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_ekf_nav_msg, const sbg_driver::SbgEkfEuler &ref_ekf_euler_msg) +{ + tf2::Quaternion orientation; + + // Compute orientation quaternion from euler angles (already converted from NED to ENU if needed). + orientation.setRPY(ref_ekf_euler_msg.angle.x, ref_ekf_euler_msg.angle.y, ref_ekf_euler_msg.angle.z); + + return createRosOdoMessage(ref_sbg_imu_msg, ref_ekf_nav_msg, orientation, ref_ekf_euler_msg); +} + +const nav_msgs::Odometry MessageWrapper::createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_ekf_nav_msg, const tf2::Quaternion &ref_orientation, const sbg_driver::SbgEkfEuler &ref_ekf_euler_msg) +{ + nav_msgs::Odometry odo_ros_msg; + double utm_x, utm_y; + std::string utm_zone; + geometry_msgs::TransformStamped transform; + + // The pose message provides the position and orientation of the robot relative to the frame specified in header.frame_id + odo_ros_msg.header = createRosHeader(ref_sbg_imu_msg.time_stamp); + odo_ros_msg.header.frame_id = m_odom_frame_id_; + tf2::convert(ref_orientation, odo_ros_msg.pose.pose.orientation); + + // Convert latitude and longitude to UTM coordinates. + if (m_utm0_.zone == 0) + { + initUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, ref_ekf_nav_msg.altitude); + + if (m_odom_publish_tf_) + { + // Publish UTM initial transformation. + geometry_msgs::Pose pose; + pose.position.x = m_utm0_.easting; + pose.position.y = m_utm0_.northing; + pose.position.z = m_utm0_.altitude; + fillTransform(m_odom_init_frame_id_, m_odom_frame_id_, pose, transform); + m_static_tf_broadcaster_.sendTransform(transform); + } + } + + LLtoUTM(ref_ekf_nav_msg.latitude, ref_ekf_nav_msg.longitude, m_utm0_.zone, utm_y, utm_x); + odo_ros_msg.pose.pose.position.x = utm_x - m_utm0_.easting; + odo_ros_msg.pose.pose.position.y = utm_y - m_utm0_.northing; + odo_ros_msg.pose.pose.position.z = ref_ekf_nav_msg.altitude - m_utm0_.altitude; + + // Compute convergence angle. + double longitudeRad = sbgDegToRadD(ref_ekf_nav_msg.latitude); + double latitudeRad = sbgDegToRadD(ref_ekf_nav_msg.longitude); + double central_meridian = sbgDegToRadD(computeMeridian(utm_zone)); + double convergence_angle = atan(tan(longitudeRad - central_meridian) * sin(latitudeRad)); + // ROS_INFO("zone:%s convergence_angle:%f", utm_zone.c_str(), convergence_angle); + + // Convert position standard deviations to UTM frame. + double std_east = ref_ekf_nav_msg.position_accuracy.x; + double std_north = ref_ekf_nav_msg.position_accuracy.y; + double std_x = std_east * cos(convergence_angle) - std_north * sin(convergence_angle); + double std_y = std_north * sin(convergence_angle) + std_east * cos(convergence_angle); + double std_z = ref_ekf_nav_msg.position_accuracy.z; + odo_ros_msg.pose.covariance[0*6 + 0] = std_x * std_x; + odo_ros_msg.pose.covariance[1*6 + 1] = std_y * std_y; + odo_ros_msg.pose.covariance[2*6 + 2] = std_z * std_z; + + // The twist message gives the linear and angular velocity relative to the frame defined in child_frame_id + odo_ros_msg.child_frame_id = m_frame_id_; + odo_ros_msg.twist.twist.linear.x = ref_ekf_nav_msg.velocity.x; + odo_ros_msg.twist.twist.linear.y = ref_ekf_nav_msg.velocity.y; + odo_ros_msg.twist.twist.linear.z = ref_ekf_nav_msg.velocity.z; + odo_ros_msg.twist.twist.angular.x = ref_ekf_euler_msg.angle.x; + odo_ros_msg.twist.twist.angular.y = ref_ekf_euler_msg.angle.y; + odo_ros_msg.twist.twist.angular.z = ref_ekf_euler_msg.angle.z; + odo_ros_msg.twist.covariance[0*6 + 0] = ref_ekf_nav_msg.velocity_accuracy.x; + odo_ros_msg.twist.covariance[1*6 + 1] = ref_ekf_nav_msg.velocity_accuracy.y; + odo_ros_msg.twist.covariance[2*6 + 2] = ref_ekf_nav_msg.velocity_accuracy.z; + odo_ros_msg.twist.covariance[3*6 + 3] = ref_ekf_euler_msg.accuracy.x; + odo_ros_msg.twist.covariance[4*6 + 4] = ref_ekf_euler_msg.accuracy.y; + odo_ros_msg.twist.covariance[5*6 + 5] = ref_ekf_euler_msg.accuracy.z; + + if (m_odom_publish_tf_) + { + // Publish odom transformation. + fillTransform(odo_ros_msg.header.frame_id, m_odom_base_frame_id_, odo_ros_msg.pose.pose, transform); + m_tf_broadcaster_.sendTransform(transform); + } + + return odo_ros_msg; +} + const sensor_msgs::Temperature MessageWrapper::createRosTemperatureMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg) const { sensor_msgs::Temperature temperature_message;