node.hpp

Go to the documentation of this file.

/*
 * Copyright 2016 Delft Robotics B.V.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "camera_pose_calibration.hpp"

#include <opencv2/opencv.hpp>

#include <camera_pose_calibration/CalibrateFile.h>
#include <camera_pose_calibration/CalibrateCall.h>
#include <camera_pose_calibration/CalibrateTopic.h>
#include <visualization_msgs/Marker.h>

#include <pcl_ros/point_cloud.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>


namespace camera_pose_calibration {

class CameraPoseCalibrationNode {
public:
        CameraPoseCalibrationNode();

protected:
        ros::NodeHandle node_handle;

        image_transport::ImageTransport image_transport;

        image_transport::Publisher detected_pattern_publisher;

        tf::TransformListener transform_listener;

        tf::TransformBroadcaster transform_broadcaster;

        ros::Publisher cloud_publisher;

        ros::Publisher target_cloud_publisher;

        ros::Publisher transformed_target_cloud_publisher;

        ros::Publisher source_cloud_publisher;

        ros::Publisher projected_source_cloud_publisher;

        ros::Publisher calibration_plane_marker_publisher;

        ros::ServiceServer calibrate_server_call;

        ros::ServiceServer calibrate_server_topic;

        ros::ServiceServer calibrate_server_file;

        ros::Timer tf_timer;

        tf::StampedTransform calibration_transform;

        bool publish_transform;

        double publish_rate;

        bool calibrated;

        void parseInput();

        visualization_msgs::Marker createCalibrationPlaneMarker(pcl::PointCloud<pcl::PointXYZ>::ConstPtr points, uint8_t pattern_width, uint8_t pattern_height);

        void onTfTimeout(ros::TimerEvent const &);

        bool calibrate(
                sensor_msgs::Image const & sensor_msgs_image,
                sensor_msgs::PointCloud2 const & sensor_msgs_cloud,
                std::string const & tag_frame,
                std::string const & target_frame,
                double const & point_cloud_scale_x,
                double const & point_cloud_scale_y,
                camera_pose_calibration::PatternParameters const & pattern,
                geometry_msgs::Transform & transform
        );

        bool onCalibrateFile(camera_pose_calibration::CalibrateFile::Request & req, camera_pose_calibration::CalibrateFile::Response & res);

        bool onCalibrateTopic(camera_pose_calibration::CalibrateTopic::Request & req, camera_pose_calibration::CalibrateTopic::Response & res);

        bool onCalibrateCall(camera_pose_calibration::CalibrateCall::Request & req, camera_pose_calibration::CalibrateCall::Response & res);
};

}