00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00035 #include <iostream>
00036 #include <stdio.h>
00037 #include <ros/ros.h>
00038 #include <sensor_msgs/Image.h>
00039 #include <sensor_msgs/CameraInfo.h>
00040 #include <image_transport/image_transport.h>
00041 #include <signal.h>
00042 #include <fstream>
00043 #include <tf/transform_listener.h>
00044 #include <tf/tf.h>
00045 #include <ros/package.h>
00046 #include <math.h>
00047 using namespace std;
00048
00052 void sighandler(int sig)
00053 {
00054 ROS_ERROR("Signal %d caught...",sig);
00055 cout<<"Shuting down road_recognition"<<endl;
00056 exit(0);
00057 }
00058
00062 void Callback(const sensor_msgs::CameraInfoPtr& cam_info)
00063 {
00064 tf::TransformListener lt;
00065 tf::StampedTransform trans_cam;
00066
00067 string target_ref = "/atc/vehicle/center_bumper";
00068 string ref = cam_info->header.frame_id;
00069
00070
00071 try
00072 {
00073 lt.lookupTransform( ref.c_str() , target_ref.c_str() , ros::Time(0), trans_cam);
00074 ROS_INFO("Found");
00075 }
00076 catch (tf::TransformException ex)
00077 {
00078 ROS_INFO("Transformation not found: entered catch for 1.5 seconds!");
00079 if(lt.waitForTransform( ref.c_str() , target_ref.c_str(),ros::Time(0), ros::Duration(1.5)))
00080 {
00081 try
00082 {
00083 lt.lookupTransform( ref.c_str() , target_ref.c_str() , ros::Time(0), trans_cam);
00084 ROS_INFO("Found");
00085 }
00086 catch (tf::TransformException ex)
00087 {
00088 ROS_ERROR("Transformation not found: tansforrmation not found after waiting 1.5 seconds\n.%s",ex.what());
00089 }
00090 }
00091 else
00092 {
00093 ROS_ERROR("Transformation not found: Could not find valid transform after waiting 1.5 seconds\n.%s",ex.what());
00094 exit(0);
00095 }
00096
00097 };
00098
00099 double R,P,Y;
00100 tf::Quaternion q;
00101 q = trans_cam.getRotation();
00102 double height = trans_cam.getOrigin().z() * 1000;
00103
00104 tf::Matrix3x3(q).getRPY(R, P, Y);
00105
00106
00107 cout<<"---> Camera configurations"<<endl;
00108 cout<<"imge size "<<endl<<"\t"<<cam_info->height<<" x "<<cam_info->width<<endl;
00109 cout<<"distortion_model:"<<endl<<"\t"<<cam_info->distortion_model<<endl;
00110 cout<<"focal lengthstrans_cam"<<endl<<"\t fx ="<<cam_info->K[0]<<" fy = "<<cam_info->K[4]<<endl;
00111 cout<<"principal point"<<endl<<"\t cx ="<<cam_info->K[2]<<" cy = "<<cam_info->K[5]<<endl;
00112 cout<<"cameraHeight = "<<endl;
00113 cout<<"pitch ="<<P<<endl;
00114 cout<<"yaw = "<<Y<<endl;
00115
00116
00117 P = (P + (M_PI/2)) * ( 180/M_PI );
00118 Y = (Y + (M_PI/2)) * ( 180/M_PI );
00119
00120
00121
00122 std::string file_path=ros::package::getPath("caltech_lanes")+"/config/CameraInfo.conf";
00123 cout<<"file_path:"<<file_path<<endl;
00124 ofstream myfile(file_path.c_str(),ios::trunc);
00125 if (myfile.is_open())
00126 {
00127
00128 myfile<<"\n# focal length\n";
00129 myfile<<"focalLengthX "<<cam_info->K[0]<<"\n";
00130 myfile<<"focalLengthY "<<cam_info->K[4]<<"\n";
00131 myfile<<"\n# optical center \n";
00132 myfile<<"opticalCenterX "<<cam_info->K[2]<<"\n";
00133 myfile<<"opticalCenterY "<<cam_info->K[5]<<"\n";
00134 myfile<<"\n# height of the camera in mm\n";
00135 myfile<<"cameraHeight "<<height<<"\n";
00136 myfile<<"\n# pitch of the camera\n";
00137 myfile<<"pitch "<<P<<"\n";
00138 myfile<<"\n# yaw of the camera\n";
00139 myfile<<"yaw "<<Y<<"\n";
00140 myfile<<"\n# imag width and height\n";
00141 myfile<<"imageWidth "<<cam_info->width<<"\n";
00142 myfile<<"imageHeight "<<cam_info->height<<"\n";
00143
00144 myfile.close();
00145
00146 cout<<"---> Camera configurations Updated"<<endl;
00147 exit(0);
00148 }
00149 else
00150 {
00151 cout << "Unable to open file"<<endl;
00152 exit(0);
00153 }
00154 }
00155
00159 int main(int argc, char **argv)
00160 {
00161 ros::init(argc, argv, "camera_configurations");
00162
00163 string messages;
00164 ros::NodeHandle nh("~");
00165
00166 nh.param("cam_info",messages,string("/snr/scam/wide/left/camera_info"));
00167
00168
00169 cout<<"cam_info: "<<messages<<endl;
00170
00171
00172 signal(SIGINT, &sighandler);
00173 ROS_INFO("messages = %s",messages.c_str());
00174 ros::Subscriber sub = nh.subscribe(messages.c_str(), 1, Callback);
00175 ros::spin();
00176 return 0;
00177 }