00001 /* 00002 * Copyright (C) 2014-2017 Fetch Robotics Inc. 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 #ifndef ROBOT_CALIBRATION_CAPTURE_PLANE_FINDER_H 00018 #define ROBOT_CALIBRATION_CAPTURE_PLANE_FINDER_H 00019 00020 #include <ros/ros.h> 00021 #include <robot_calibration/capture/depth_camera.h> 00022 #include <robot_calibration/capture/feature_finder.h> 00023 #include <robot_calibration_msgs/CalibrationData.h> 00024 #include <cv_bridge/cv_bridge.h> 00025 00026 namespace robot_calibration 00027 { 00028 00029 class PlaneFinder : public FeatureFinder 00030 { 00031 public: 00032 PlaneFinder(ros::NodeHandle & n); 00033 00034 bool find(robot_calibration_msgs::CalibrationData * msg); 00035 00036 private: 00037 void cameraCallback(const sensor_msgs::PointCloud2& cloud); 00038 bool waitForCloud(); 00039 00040 ros::Subscriber subscriber_; 00041 ros::Publisher publisher_; 00042 00043 bool waiting_; 00044 sensor_msgs::PointCloud2 cloud_; 00045 DepthCameraInfoManager depth_camera_manager_; 00046 00047 std::string camera_sensor_name_; 00048 double points_max_; 00049 double min_x_; 00050 double max_x_; 00051 double min_y_; 00052 double max_y_; 00053 double min_z_; 00054 double max_z_; 00055 std::string transform_frame_; 00056 }; 00057 00058 } // namespace robot_calibration 00059 00060 #endif // ROBOT_CALIBRATION_CAPTURE_PLANE_FINDER_H