sick_scan_common.h

Go to the documentation of this file.

/*
 * Copyright (C) 2013, Osnabrück University
 * Copyright (C) 2017, Ing.-Buero Dr. Michael Lehning, Hildesheim
 * Copyright (C) 2017, SICK AG, Waldkirch
 * 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 Osnabrück University 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.
 *
 *  Created on: 24.05.2012
 *
 *      Authors:
 *         Jochen Sprickerhof <jochen@sprickerhof.de>
 *         Martin Günther <mguenthe@uos.de>
 *         Michael Lehning <michael.lehning@lehning.de>
 *
 * Based on the TiM communication example by SICK AG.
 *
 */

#ifndef SICK_SCAN_COMMON_H_
#define SICK_SCAN_COMMON_H_

#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <string.h>
#include <vector>

#include <boost/asio.hpp>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <sick_scan/sick_scan_common_nw.h>


#ifndef _MSC_VER
#include <dynamic_reconfigure/server.h>
#include <sick_scan/SickScanConfig.h>
#endif
#include "sick_scan/sick_generic_parser.h"
#include "sick_scan/sick_scan_common_nw.h"

void swap_endian(unsigned char *ptr, int numBytes);

namespace sick_scan
{

        class SickScanCommon
        {
        public:
                enum SOPAS_CMD
                {
                        CMD_DEVICE_IDENT_LEGACY,
                        CMD_DEVICE_IDENT,  // for MRS6124
                        CMD_SERIAL_NUMBER,
                        CMD_REBOOT,
                        CMD_WRITE_EEPROM,
                        CMD_FIRMWARE_VERSION,
                        CMD_DEVICE_STATE,
                        CMD_OPERATION_HOURS,
                        CMD_POWER_ON_COUNT,
                        CMD_LOCATION_NAME,
                        CMD_ACTIVATE_STANDBY,
                        CMD_SET_PARTICLE_FILTER,
                        CMD_SET_MEAN_FILTER,
                        CMD_ALIGNMENT_MODE,
                        CMD_APPLICATION_MODE,
      CMD_APPLICATION_MODE_FIELD_ON,
                        CMD_APPLICATION_MODE_FIELD_OFF,
                        CMD_APPLICATION_MODE_RANGING_ON,
                        CMD_SET_ACCESS_MODE_3,
                        CMD_SET_OUTPUT_RANGES,
                        CMD_GET_OUTPUT_RANGES,
                        CMD_RUN,
                        CMD_SET_PARTIAL_SCAN_CFG,
                        CMD_GET_PARTIAL_SCAN_CFG,
                        CMD_GET_PARTIAL_SCANDATA_CFG,
                        CMD_SET_PARTIAL_SCANDATA_CFG,
                        CMD_STOP_SCANDATA,
                        CMD_START_SCANDATA,
                        CMD_START_RADARDATA,
      CMD_ACTIVATE_NTP_CLIENT,
                        CMD_SET_NTP_INTERFACE_ETH,

      CMD_START_IMU_DATA, // start of IMU data
      CMD_STOP_IMU_DATA, // start of IMU data

        // start of radar specific commands
                        CMD_SET_TRANSMIT_RAWTARGETS_ON,  // transmit raw target for radar
                CMD_SET_TRANSMIT_RAWTARGETS_OFF, // do not transmit raw target for radar

      CMD_SET_TRANSMIT_OBJECTS_ON,  // transmit raw target for radar
      CMD_SET_TRANSMIT_OBJECTS_OFF, // do not transmit raw target for radar

                        CMD_SET_TRACKING_MODE_0,  // set radar tracking mode to "BASIC"
                        CMD_SET_TRACKING_MODE_1,  // set radar tracking mode to "TRAFFIC"

      CMD_LOAD_APPLICATION_DEFAULT, // load application default
      CMD_DEVICE_TYPE,
      CMD_ORDER_NUMBER,
      // end of radar specific commands
                        CMD_START_MEASUREMENT,
                        CMD_STOP_MEASUREMENT,
                        CMD_SET_ECHO_FILTER,
      CMD_SET_NTP_UPDATETIME,
                        CMD_SET_NTP_TIMEZONE,
            CMD_SET_IP_ADDR,
            CMD_SET_GATEWAY,
                        CMD_SET_NTP_SERVER_IP_ADDR,
                        CMD_SET_TO_COLA_A_PROTOCOL,  //         sWN EIHstCola 1  // Cola B      sWN EIHstCola 0  // Cola A 
                        CMD_SET_TO_COLA_B_PROTOCOL,  // 
                        // ML: Add above new CMD-Identifier
                        //
                        //
                        CMD_END // CMD_END is a tag for end of enum - never (re-)move it. It must be the last element.
                };
// --- START KEYWORD DEFINITIONS ---
#define PARAM_MIN_ANG "min_ang"
#define PARAM_MAX_ANG "max_ang"
#define PARAM_RES_ANG "res_ang"
// --- END KEYWORD DEFINITIONS ---


                SickScanCommon(SickGenericParser* parser);
                virtual ~SickScanCommon();

                int setParticleFilter(bool _active, int _particleThreshold);//actualy only 500 mm is working.
                std::string generateExpectedAnswerString(const std::vector<unsigned char> requestStr);
                int sendSopasAndCheckAnswer(std::string request, std::vector<unsigned char> *reply, int cmdId);
                int sendSopasAndCheckAnswer(std::vector<unsigned char> request, std::vector<unsigned char> *reply, int cmdId);

                int setAligmentMode(int _AligmentMode);
                int setMeanFilter(bool _active, int _numberOfScans);
                int setApplicationMode(bool _active, int _mode); //0=RANG (Ranging) 1=FEVL (Field Application).
                int ActivateStandBy(void);
                bool testSettingIpAddress();
                bool testsetParticleFilter();
                bool testsetMeanFilter();
                bool testsetAligmentMode();
                bool testsetActivateStandBy();
                bool testsetApplicationMode();
                int getReadTimeOutInMs();
                void setReadTimeOutInMs(int timeOutInMs);
                int getProtocolType(void);
                void setProtocolType(SopasProtocol cola_dialect_id);
                virtual int init();
                int loopOnce();
                void check_angle_range(SickScanConfig &conf);
                void update_config(sick_scan::SickScanConfig &new_config, uint32_t level = 0);

                double get_expected_frequency() const { return expectedFrequency_; }
                int convertAscii2BinaryCmd(const char *requestAscii, std::vector<unsigned char>* requestBinary);
                int init_cmdTables();


                virtual bool rebootScanner();


                bool changeIPandreboot(boost::asio::ip::address_v4 IpAdress);

                SickScanCommonNw m_nw;

                SickScanConfig* getConfigPtr()
                {
                        return(&config_);
                }

                // move back to private
                                /* FÜR MRS10000 brauchen wir einen Publish und eine NAchricht */
                                // Should we publish laser or point cloud?
                                // ros::Publisher cloud_pub_;
                                ros::Publisher cloud_pub_;
                                ros::Publisher cloud_radar_rawtarget_pub_;
                                ros::Publisher cloud_radar_track_pub_;
                                ros::Publisher radarScan_pub_;
                                ros::Publisher  imuScan_pub_;
                                // sensor_msgs::PointCloud cloud_;
                                sensor_msgs::PointCloud2 cloud_;
                // Dynamic Reconfigure
                SickScanConfig config_;
                protected:
                virtual int init_device() = 0;
                virtual int init_scanner();
                virtual int stop_scanner();
                virtual int close_device() = 0;


                virtual int sendSOPASCommand(const char* request, std::vector<unsigned char> * reply, int cmdLen = -1) = 0;

                virtual int get_datagram(ros::Time &recvTimeStamp, unsigned char *receiveBuffer, int bufferSize, int *actual_length,
                             bool isBinaryProtocol, int *numberOfRemainingFifoEntries) = 0;


                std::string replyToString(const std::vector<unsigned char> &reply);
                unsigned long convertBigEndianCharArrayToUnsignedLong(const unsigned char *vecArr);

                int checkForBinaryAnswer(const std::vector<unsigned char>* reply);

                bool isCompatibleDevice(const std::string identStr) const;

                bool dumpDatagramForDebugging(unsigned char *buffer, int bufLen);

                diagnostic_updater::Updater diagnostics_;




        private:
                SopasProtocol m_protocolId;
                // ROS
                ros::NodeHandle nh_;
                ros::Publisher pub_;
                ros::Publisher datagram_pub_;
                bool publish_datagram_;


                // Diagnostics
                diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>* diagnosticPub_;
                double expectedFrequency_;


#ifndef _MSC_VER
                dynamic_reconfigure::Server<sick_scan::SickScanConfig> dynamic_reconfigure_server_;
#endif
                // Parser
                SickGenericParser* parser_;
                std::vector<std::string> sopasCmdVec;
                std::vector<std::string> sopasCmdMaskVec;
                std::vector<std::string> sopasReplyVec;
                std::vector<std::vector<unsigned char> > sopasReplyBinVec;
                std::vector<std::string> sopasReplyStrVec;
                std::vector<std::string> sopasCmdErrMsg;
                std::vector<int> sopasCmdChain;

                int  outputChannelFlagId;
                bool checkForProtocolChangeAndMaybeReconnect(bool& useBinaryCmdNow);
                void setSensorIsRadar(bool _isRadar);
                bool getSensorIsRadar(void);
                bool setNewIpAddress(boost::asio::ip::address_v4 ipNewIPAddr, bool useBinaryCmd);
                bool setNTPServerAndStart(boost::asio::ip::address_v4 ipNewIPAddr, bool useBinaryCmd);

        int readTimeOutInMs;
private:
        bool sensorIsRadar;
        };

} /* namespace sick_scan */
#endif /* SICK_TIM3XX_COMMON_H_ */