.. _program_listing_file__tmp_ws_src_nmea_hardware_interface_include_nmea_hardware_interface_gps_hardware_interface.hpp: Program Listing for File gps_hardware_interface.hpp =================================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/nmea_hardware_interface/include/nmea_hardware_interface/gps_hardware_interface.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // Copyright (c) 2021 OUXT Polaris // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef NMEA_HARDWARE_INTERFACE__GPS_HARDWARE_INTERFACE_HPP_ #define NMEA_HARDWARE_INTERFACE__GPS_HARDWARE_INTERFACE_HPP_ // Headers in ROS #include #if defined(GALACTIC) || defined(HUMBLE) #include #else #include #endif #include #include #include #include #if defined(GALACTIC) || defined(HUMBLE) #include #else #include #endif #include #include #include #include #include #include #include #include #include #include #include #include namespace nmea_hardware_interface { class GPSHardwareInterface #if defined(GALACTIC) || defined(HUMBLE) : public hardware_interface::SensorInterface #else : public hardware_interface::BaseInterface #endif { public: RCLCPP_SHARED_PTR_DEFINITIONS(GPSHardwareInterface) #if defined(GALACTIC) || defined(HUMBLE) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; #else hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override; #endif ~GPSHardwareInterface(); std::vector export_state_interfaces() override; // #ifndef GALACTIC // hardware_interface::return_type start() override; // hardware_interface::return_type stop() override; // #endif hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; private: std::string joint_; nmea_msgs::msg::Sentence sentence_; size_t size_; boost::thread togeopose_thread_; std::string device_file_; int baud_rate_; std::string frame_id_; boost::asio::io_service io_; std::shared_ptr port_ptr_; boost::thread io_thread_; void readSentence(); boost::array buf_; void connectSerialPort(); bool connected_ = false; void timerCallback(); rclcpp::TimerBase::SharedPtr timer_; boost::optional validate(std::string sentence); bool validatecheckSum(std::string sentence); std::string getHexString(uint8_t value); void nmea_to_geopose(); std::string calculateChecksum(std::string sentence); geographic_msgs::msg::GeoPoint geopoint_; geometry_msgs::msg::Quaternion quat_; geographic_msgs::msg::GeoPose geopose_; bool isGprmcSentence(nmea_msgs::msg::Sentence sentence); bool isGphdtSentence(nmea_msgs::msg::Sentence sentence); std::vector split(const std::string & s, char delim); std::vector splitChecksum(std::string str); boost::optional> splitSentence(nmea_msgs::msg::Sentence sentence); }; } // namespace nmea_hardware_interface #endif // NMEA_HARDWARE_INTERFACE__GPS_HARDWARE_INTERFACE_HPP_