sensor.h

/*
 * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
 *               http://gearbox.sf.net/
 * Copyright (c) 2008-2010 Geoffrey Biggs
 *
 * hokuyo_aist Hokuyo laser scanner driver.
 *
 * This distribution is licensed to you under the terms described in the
 * LICENSE file included in this distribution.
 *
 * This work is a product of the National Institute of Advanced Industrial
 * Science and Technology, Japan. Registration number: H22PRO-1086.
 *
 * This file is part of hokuyo_aist.
 *
 * This software is licensed under the Eclipse Public License -v 1.0 (EPL). See
 * http://www.opensource.org/licenses/eclipse-1.0.txt
 */

#ifndef SENSOR_H__
#define SENSOR_H__

#include <string>

#if defined(WIN32)
    typedef unsigned char           uint8_t;
    typedef unsigned int            uint32_t;
    #if defined(HOKUYO_AIST_STATIC)
        #define HOKUYO_AIST_EXPORT
    #elif defined(HOKUYO_AIST_EXPORTS)
        #define HOKUYO_AIST_EXPORT       __declspec(dllexport)
    #else
        #define HOKUYO_AIST_EXPORT       __declspec(dllimport)
    #endif
#else
    #include <stdint.h>
    #define HOKUYO_AIST_EXPORT
#endif

namespace flexiport
{
    class Port;
}

namespace hokuyo_aist
{

enum MultiechoMode
{
    ME_OFF,
    ME_FRONT,
    ME_MIDDLE,
    ME_REAR,
    ME_AVERAGE
};


HOKUYO_AIST_EXPORT inline char const* multiecho_mode_to_string(MultiechoMode mode)
{
    switch(mode)
    {
        case ME_OFF:
            return "Off";
        case ME_FRONT:
            return "Front";
        case ME_MIDDLE:
            return "Middle";
        case ME_REAR:
            return "Rear";
        case ME_AVERAGE:
            return "Average";
        default:
            return "Unknown";
    }
}


typedef struct IPAddr
{
    unsigned int first;
    unsigned int second;
    unsigned int third;
    unsigned int fourth;
} IPAddr;


class HOKUYO_AIST_EXPORT Sensor
{
    public:
        Sensor();
        Sensor(std::ostream& err_output);
        ~Sensor();

        void open(std::string port_options);

        unsigned int open_with_probing(std::string port_options);

        void close();

        bool is_open() const;

        void set_power(bool on);

        void set_baud(unsigned int baud);

        void set_ip(IPAddr const& addr, IPAddr const& subnet,
                IPAddr const& gateway);

        void reset();

        void semi_reset();

        void set_motor_speed(unsigned int speed);

        void set_high_sensitivity(bool on);

        void get_sensor_info(SensorInfo& info);

        unsigned int get_time();

        unsigned int get_raw_time();

        long long calibrate_time(unsigned int skew_sleep_time=0,
                unsigned int samples=10);

        long long time_offset() const { return time_offset_; }

        void set_time_offset(long long time_offset)
            { time_offset_ = time_offset; }
        float drift_rate() const { return time_drift_rate_; }
        void set_drift_rate(float drift_rate)
            { time_drift_rate_ = drift_rate; }

        float skew_alpha() const { return time_skew_alpha_; }
        void set_skew_alpha(float alpha) { time_skew_alpha_ = alpha; }

        unsigned int get_ranges(ScanData& data, int start_step = -1,
                int end_step = -1, unsigned int cluster_count = 1);

        unsigned int get_ranges_by_angle(ScanData& data, double start_angle,
                double end_angle, unsigned int cluster_count = 1);

        unsigned int get_ranges_intensities(ScanData& data,
                int start_step = -1, int end_step = -1,
                unsigned int cluster_count = 1);

        unsigned int get_ranges_intensities_by_angle(ScanData& data,
                double start_angle, double end_angle,
                unsigned int cluster_count = 1);

        unsigned int get_new_ranges(ScanData& data, int start_step = -1,
                int end_step = -1, unsigned int cluster_count = 1);

        unsigned int get_new_ranges_by_angle(ScanData& data,
                double start_angle, double end_angle,
                unsigned int cluster_count = 1);

        unsigned int get_new_ranges_intensities(ScanData& data,
                int start_step = -1, int end_step = -1,
                unsigned int cluster_count = 1);

        unsigned int get_new_ranges_intensities_by_angle(ScanData& data,
                double start_angle, double end_angle,
                unsigned int cluster_count = 1);

        uint8_t scip_version() const            { return scip_version_; }

        void set_verbose(bool verbose)          { verbose_ = verbose; }

        void ignore_unknowns(bool ignore)       { ignore_unknowns_ = ignore; }

        void set_multiecho_mode(MultiechoMode mode) { multiecho_mode_ = mode; }

        double step_to_angle(unsigned int step);
        unsigned int angle_to_step(double angle);

    private:
        flexiport::Port* port_;
        std::ostream& err_output_;

        uint8_t scip_version_;
        LaserModel model_;
        bool verbose_, enable_checksum_workaround_,
             ignore_unknowns_;
        MultiechoMode multiecho_mode_;
        double min_angle_, max_angle_, resolution_;
        int first_step_, last_step_, front_step_;
        unsigned int max_range_;
        unsigned int time_resolution_;
        long long time_offset_;
        unsigned int last_timestamp_;
        unsigned int wrap_count_;
        float time_drift_rate_;
        float time_skew_alpha_;

        void clear_read_buffer();
        int read_line(char* buffer, int expected_length=-1);
        int read_line_with_check(char* buffer, int expected_length=-1,
                bool has_semicolon=false);
        bool read_data_block(char* buffer, int& block_size);
        void skip_lines(int count);
        int send_command(char const* cmd, char const* param, int param_length,
                char const* extra_ok);

        void enter_timing_mode();
        void leave_timing_mode();
        unsigned int get_timing_mode_time(unsigned long long* reception_time=0);
        unsigned long long get_computer_time();
        unsigned int wrap_timestamp(unsigned int timestamp);
        unsigned long long offset_timestamp(unsigned int timestamp);
        unsigned int step_to_time_offset(int start_step);

        void find_model(char const* buffer);
        void get_and_set_scip_version();
        void get_defaults();
        void process_vv_line(char const* buffer, SensorInfo& info);
        void process_pp_line(char const* buffer, SensorInfo& info);
        void process_ii_line(char const* buffer, SensorInfo& info);

        uint32_t process_echo_buffer(int const* buffer, int num_echos);
        void read_2_byte_range_data(ScanData& data, unsigned int num_steps);
        void read_3_byte_range_data(ScanData& data, unsigned int num_steps);
        void read_3_byte_range_and_intensity_data(ScanData& data,
                unsigned int num_steps);

        int confirm_checksum(char const* buffer, int length,
                int expected_sum);
}; // class Sensor

} // namespace hokuyo_aist

#endif // SENSOR_H__