test_ros_cam_obs.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2014, Southwest Research Institute
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  *   http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 
00020 #include <ros/ros.h>
00021 #include <industrial_extrinsic_cal/ros_camera_observer.h>
00022 #include <yaml-cpp/yaml.h>
00023 #include<fstream>
00024 
00025 using industrial_extrinsic_cal::ROSCameraObserver;
00026 
00027 int main(int argc, char **argv)
00028 {
00029         ros::init(argc, argv, "my_node_name");
00030         ros::NodeHandle nh;
00031         std::string topic="/camera/rgb/image_rect_color";
00032 
00033          ROSCameraObserver cam_observer(topic);
00034 
00035          boost::shared_ptr<industrial_extrinsic_cal::Target> target (new industrial_extrinsic_cal::Target);
00036          industrial_extrinsic_cal::Roi roi;
00037          target->target_type=0;
00038          target->is_moving=false;
00039          target->target_name="checkerboard";
00040          target->checker_board_parameters.pattern_rows=11;
00041          target->checker_board_parameters.pattern_cols=11;
00042          //target->target_name="circlegrid";
00043          //target->circle_grid_parameters.pattern_rows=31;
00044          //target->circle_grid_parameters.pattern_cols=30;
00045          //target->circle_grid_parameters.is_symmetric=true;
00046          //target->pts[0]=0;
00047          //target->pose=NULL;
00048          roi.x_min=100;
00049          roi.y_min=70;
00050          roi.x_max=460;
00051          roi.y_max=430;
00052 
00053          if (cam_observer.addTarget(target , roi))
00054          {
00055                  ROS_INFO_STREAM("Added target successfully");
00056          }
00057          cam_observer.triggerCamera();
00058          industrial_extrinsic_cal::CameraObservations camera_obs;
00059          //cam_observer.getObservations(camera_obs);
00060          if (cam_observer.getObservations(camera_obs))
00061          {
00062                  ROS_INFO_STREAM("Success!");
00063          }
00064 /*
00065          YAML::Emitter out;
00066          out << YAML::BeginMap;
00067          //out << YAML::Key << "Points";
00068          //out << YAML::Value << "f";
00069          //out ;
00070          out << YAML::Key << "Points";
00071          out << YAML::Value;
00072          out << YAML::BeginMap;
00073          for (int i=0; i<camera_obs.observations.size(); i++)
00074          {
00075          out << YAML::Key << "Point_id" << YAML::Value << i;
00076          out << YAML::Key << "Pnts";
00077          out << YAML::Value << YAML::BeginSeq;
00078          out << camera_obs.observations.at(i).image_loc_x << camera_obs.observations.at(i).image_loc_y;
00079          out << YAML::EndSeq;
00080          }
00081          out << YAML::EndMap;
00082          out << YAML::EndMap;
00083 
00084          std::ofstream fout("observations.txt");
00085          fout << out.c_str();*/
00086 
00087 
00088          return 0;
00089 }


industrial_extrinsic_cal
Author(s): Chris Lewis
autogenerated on Wed Aug 26 2015 12:00:27