ev3_ultrasonic_controller.h

/*
 * This file (Ev3UltraSonicController.h) is part of h4r_ev3_control.
 * Date: 10.12.2015
 *
 * Author: Christian Holl
 * http://github.com/Hacks4ROS
 *
 * h4r_ev3_control is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * h4r_ev3_control is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with ev3_control.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
#ifndef EV3_ULTRASONIC_CONTROLLER_H_
#define EV3_ULTRASONIC_CONTROLLER_H_

#include <controller_interface/controller.h>
#include <pluginlib/class_list_macros.h>
#include <realtime_tools/realtime_publisher.h>
#include <boost/shared_ptr.hpp>
#include <sensor_msgs/Range.h>
#include <std_msgs/Bool.h>
#include <limits>

#include <h4r_ev3_control/Ev3SensorInterface.h>

namespace ev3_control
{

class Ev3UltrasonicController: public controller_interface::Controller<
        Ev3SensorInterface>
{
private:
    std::string port_;
    Ev3Strings::Ev3UltrasonicMode mode_;
    Ev3SensorHandle handle_;
    H4REv3UltraSonicSensorSpecIface us_interface_;
    bool sensor_mode_needs_init_;

    double max_range_;
    double min_range_;
    std::string frame_id_;

    //Range Publisher
    typedef boost::shared_ptr<
            realtime_tools::RealtimePublisher<sensor_msgs::Range> > RtRangePublisherPtr;
    RtRangePublisherPtr realtime_range_publisher_;

    typedef boost::shared_ptr<
            realtime_tools::RealtimePublisher<std_msgs::Bool> > RtBoolPublisherPtr;
    RtBoolPublisherPtr realtime_bool_publisher_;


    ros::Time last_publish_time_;
    double publish_rate_;

public:
    Ev3UltrasonicController();
    virtual ~Ev3UltrasonicController();

    virtual bool init(Ev3SensorInterface* hw,
            ros::NodeHandle &root_nh,
            ros::NodeHandle& ctrl_nh);

    virtual void starting(const ros::Time& time);
    virtual void update(const ros::Time& time, const ros::Duration& /*period*/);
    virtual void stopping(const ros::Time& /*time*/){}

};

} /* namespace ev3_control */

#endif /* EV3_ULTRASONIC_CONTROLLER_H_ */