HighResRangeFinder.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */

#ifndef SIMULATION_MARSHIGHRESRANGEFINDER_TASK_HPP
#define SIMULATION_MARSHIGHRESRANGEFINDER_TASK_HPP

#include "mars/HighResRangeFinderBase.hpp"
#include <base/samples/DistanceImage.hpp>

namespace mars {

    class HighResRangeFinder : public HighResRangeFinderBase
    {
        friend class HighResRangeFinderBase;
    protected:
        struct Camera {
            Camera(long id, mars::sim::CameraSensor* cam, double rot_y) : 
                    sensor_id(id), camera_sensor(cam), name()
            {
                width = camera_sensor->getConfig().width;
                height = camera_sensor->getConfig().height;
                image = new base::samples::DistanceImage(width, height);
                image->setSize(width, height);
                camera_sensor->getCameraInfo(&cam_info);
                image->setIntrinsic(cam_info.scale_x, cam_info.scale_y, 
                        cam_info.center_x, cam_info.center_y );
                cam_sensor_info = camera_sensor->getConfig();
                // Degree to rad.
                // INVERTS ROTATION, has to be done to match the viewing direction of the MARS camera.
                rot_y = (-rot_y / 180.0) * M_PI;
                Eigen::AngleAxis<double> rot(rot_y, Eigen::Vector3d(0.0, 1.0, 0.0));
                orientation = rot;
            }
            
            ~Camera() {
                delete image; image = NULL;
            }
            
            long sensor_id;
            mars::sim::CameraSensor* camera_sensor;
            int width;
            int height;
            mars::interfaces::cameraStruct cam_info;
            mars::sim::CameraConfigStruct cam_sensor_info;
            base::samples::DistanceImage* image;
            // Rotation of the camera around the y-axis within the camera frame.
            Eigen::Quaternion<double, Eigen::DontAlign> orientation;
            
            double pixel_per_rad_horizontal;
            double pixel_per_rad_vertical;
            double lower_pixel;
            double upper_pixel;
            double left_pixel;
            double right_pixel;
            double v_steps;
            double h_steps;
            std::string name;
        };
    
        std::vector<Camera*> cameras;
        
        virtual bool addCamera(::std::string const & name, double orientation);
        
        void calcCamParameters(Camera* camera);

    public:
        HighResRangeFinder(std::string const& name = "mars::HighResRangeFinder");

        HighResRangeFinder(std::string const& name, RTT::ExecutionEngine* engine);

         ~HighResRangeFinder();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
        
        virtual void getData();
    };
}

#endif