USBLManager.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *   * Neither the name of the LABUST 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 OWNER 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.
 *
 *  Author: Dula Nad
 *  Created: 02.04.2013.
 *********************************************************************/
#ifndef USBLMANAGER_HPP_
#define USBLMANAGER_HPP_
#include <labust/tritech/DiverMsg.hpp>

#include <nodelet/nodelet.h>
#include <auv_msgs/NavSts.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float64MultiArray.h>
#include <ros/ros.h>

#include <boost/thread.hpp>
#include <boost/function.hpp>

#include <string>
#include <queue>
#include <map>
#include <ctype.h>

namespace labust
{
        namespace tritech
        {
                struct AsciiInternal
                {
                        enum {digit_diff=48, alpha_diff=55};
                        enum {alpha_limit_int=36, digit_limit_int=10};
                        enum {char_size = 6};
                        enum {point=36, 
                              comma, 
                              question_mark, 
                              exclamation_mark, 
                              space, 
                              newline = 62, 
                              zero_char};
                        static char ascii2Int(char c)
                        {
                                if (isalpha(c)) return (toupper(c)-alpha_diff);
                                if (isdigit(c)) return (c-digit_diff);
                                if (c == '.') return (point);
                                if (c == ',') return (comma);
                                if (c == '?') return (question_mark);
                                if (c == '!') return (exclamation_mark);
                                if (c == ' ') return (space);
                                if (c == '\n') return (newline);
                                return zero_char;
                        }
                        static char int2Ascii(char c)
                        {
                                if (c<digit_limit_int) return (c+digit_diff);
                                if (c<alpha_limit_int) return (c+alpha_diff);
                                if (c == point) return '.';
                                if (c == comma) return ',';
                                if (c == question_mark) return '?';
                                if (c == exclamation_mark) return '!';
                                if (c == space) return ' ';
                                if (c == newline) return '\n';
                                return 0;
                        }
                };

                class USBLManager : public nodelet::Nodelet
                {
                        enum {idle=0,initDiver,waitForReply,transmission, lastStateNum};
                        enum {kmlNotSent=0, kmlSent, kmlWaitValidation, kmlSentAndValid};
                        //enum {kmlNotSent=0, kmlSent, kmlWaitValidation, kmlSentAndValid};
                public:
                        USBLManager();
                        ~USBLManager();

                        void onInit();
                        inline void changeState(int next)
                        {
                                lastState = state;
                                state = next;
                                NODELET_INFO("Change state: %d -> %d",lastState,next);
                        }

                protected:
                        void onNavMsg(const auv_msgs::NavSts::ConstPtr nav);
                        void onIncomingMsg(const std_msgs::String::ConstPtr msg);
                        void onIncomingText(const std_msgs::String::ConstPtr msg);
                        void onUSBLTimeout(const std_msgs::Bool::ConstPtr msg);
                        void onIncomingDefaults(const std_msgs::Int32::ConstPtr msg);
                        void onIncomingKML(const std_msgs::Float64MultiArray::ConstPtr msg);
                        void onIncomingForceState(const std_msgs::Int32::ConstPtr msg);

                        void run();

                        void init_diver();
                        void send_msg();
                        void incoming_def_txt();
                        void incoming_kml();

                        void kml_send();

                        int msgToInt(int len);
                        std::string intToMsg(int len);
                        void resendLastPackage();
                        void publishDiverOrigin();

                        ros::Publisher outgoing, auto_mode, diverText,
                        diverDefaults, retKml, diverOrigin, outCurState;
                        ros::Subscriber navData, incoming, intext,
                        timeoutNotification, inDefaults, inKml, forceState;
                        //DiverMsg encoder;
                        boost::thread worker;
                        int state, lastState;
                        bool validNav;
                        DiverMsg outgoing_msg, incoming_msg;
                        std_msgs::String outgoing_package, incoming_package;
                        std::queue<char> textBuffer;
                        std::queue<char> defaultMsgs;
                        std::queue< std::pair<double, double> > kmlBuffer;
                        std::vector< std::pair<int, int> > kmlVec;
                        std::vector<int> kmlValidIdx;
                        std::pair<int, int> kmlEndValidation;
                        int initValidation;
                        bool newMessage;
                        std::map<int, boost::function<void(void)> > dispatch;
                        int timeout, emptyIterations;
                        double diverOriginLat, diverOriginLon;
                        int lastKmlIdxSent;
                };
        }
}

/* USBLMANAGER_HPP_ */
#endif