rosaic_node.hpp

Go to the documentation of this file.

// *****************************************************************************
//
// © Copyright 2020, Septentrio NV/SA.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//    1. Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//    2. Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//    3. Neither the name of the copyright holder nor the names of its
//       contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE. 
//
// *****************************************************************************

// *****************************************************************************
//
// Boost Software License - Version 1.0 - August 17th, 2003
// 
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:

// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
// 
// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// *****************************************************************************

#ifndef ROSAIC_NODE_HPP
#define ROSAIC_NODE_HPP

// ROS includes
#include <ros/console.h>
// Boost includes
#include <boost/regex.hpp>
// ROSaic includes
#include <rosaic/communication/communication_core.hpp>

extern bool g_publish_gpgga;
extern bool g_publish_pvtcartesian;
extern bool g_publish_pvtgeodetic;
extern bool g_publish_poscovgeodetic;
extern bool g_publish_poscovcartesian;
extern bool g_publish_atteuler;
extern bool g_publish_attcoveuler;
extern bool g_publish_gpst;
extern bool g_publish_navsatfix;
extern ros::Timer g_reconnect_timer_;
extern boost::shared_ptr<ros::NodeHandle> g_nh;
extern const uint32_t g_ROS_QUEUE_SIZE;

namespace rosaic_node 
{
        io_comm_rx::Comm_IO IO;
        
        template <typename V, typename T>
        void checkRange(V val, T min, T max, std::string name) 
        {
                if(val < min || val > max) 
                {
                        std::stringstream ss;
                        ss << "Invalid settings: " << name << " must be in range [" << min << ", " << max << "].";
                        throw std::runtime_error(ss.str());
                }
        }

        template <typename V, typename T>
        void checkRange(std::vector<V> val, T min, T max, std::string name) 
        {
                for(size_t i = 0; i < val.size(); i++)  
                {
                        std::stringstream ss;
                        ss << name << "[" << i << "]";
                        checkRange(val[i], min, max, ss.str());
                }
        }
        
        template <typename U>
        bool getROSInt(const std::string& key, U &u) {
                int param;
                if (!g_nh->getParam(key, param)) return false;
                U min = std::numeric_limits<U>::lowest();
                U max = std::numeric_limits<U>::max();
                try
                {
                        checkRange((U) param, min, max, key);
                }
                catch (std::runtime_error& e) 
                {
                        std::ostringstream ss;
                        ss << e.what();
                        ROS_INFO("%s", ss.str().c_str());
                }
                u = (U) param;
                return true;
        }

        template <typename U>
        void getROSInt(const std::string& key, U &u, U default_val) 
        {
                if(!getROSInt(key, u))
                        u = default_val;
        }

        template <typename U>
        bool getROSInt(const std::string& key, std::vector<U> &u) 
        {
                std::vector<int> param;
                if (!g_nh->getParam(key, param)) return false;
                U min = std::numeric_limits<U>::lowest();
                U max = std::numeric_limits<U>::max();
                checkRange(param, min, max, key);
                u.insert(u.begin(), param.begin(), param.end());
                return true;
        }
        
        class ROSaicNode
        {
                public:
                
                        ROSaicNode();
                        
                        void getROSParams();
                        
                        void defineMessages();
                        
                        void configureRx();
                        
                        void initializeIO();
                        
                        void reconnect(const ros::TimerEvent& event);
                        
                        void connect();
                        

                        
                private:
                        std::string device_;
                        uint32_t baudrate_;
                        std::string hw_flow_control_;
                        std::string rx_serial_port_;
                        bool connected_;
                        std::string datum_;
                        uint32_t polling_period_pvt_;
                        uint32_t polling_period_rest_;
                        float reconnect_delay_s_;
                        float delta_e_;
                        float delta_n_;
                        float delta_u_;
                        std::string ant_type_;
                        std::string ant_serial_nr_;
                        std::string mode_;
                        std::string caster_;
                        uint32_t caster_port_;
                        std::string username_;
                        std::string password_;
                        std::string mountpoint_;
                        std::string ntrip_version_;
                        bool rx_has_internet_;
                        std::string rtcm_version_;
                        uint32_t rx_input_corrections_tcp_;
                        std::string rx_input_corrections_serial_;
                        ros::Timer reconnect_timer_;
                        std::string send_gga_;
                        bool publish_gpgga_;
                        bool publish_gprmc_;
                        bool publish_gpgsa_;
                        bool publish_gpgsv_;
                        bool publish_pvtcartesian_;
                        bool publish_pvtgeodetic_;
                        bool publish_poscovcartesian_;
                        bool publish_poscovgeodetic_;
                        bool publish_atteuler_;
                        bool publish_attcoveuler_;
                        boost::mutex connection_mutex_;
                        boost::condition_variable connection_condition_;
                        std::string tcp_host_;
                        std::string tcp_port_;
                        bool serial_;
        };
}

#endif // for ROSAIC_NODE_HPP