38 #include <sensor_msgs/Image.h>
39 #include <sensor_msgs/CameraInfo.h>
40 #include <image_transport/image_transport.h>
43 #include <tf/transform_listener.h>
45 #include <ros/package.h>
54 ROS_ERROR(
"Signal %d caught...",sig);
55 cout<<
"Shuting down road_recognition"<<endl;
62 void Callback(
const sensor_msgs::CameraInfoPtr& cam_info)
64 tf::TransformListener
lt;
65 tf::StampedTransform trans_cam;
67 string target_ref =
"/atc/vehicle/center_bumper";
68 string ref = cam_info->header.frame_id;
73 lt.lookupTransform( ref.c_str() , target_ref.c_str() , ros::Time(0), trans_cam);
76 catch (tf::TransformException ex)
78 ROS_INFO(
"Transformation not found: entered catch for 1.5 seconds!");
79 if(lt.waitForTransform( ref.c_str() , target_ref.c_str(),ros::Time(0), ros::Duration(1.5)))
83 lt.lookupTransform( ref.c_str() , target_ref.c_str() , ros::Time(0), trans_cam);
86 catch (tf::TransformException ex)
88 ROS_ERROR(
"Transformation not found: tansforrmation not found after waiting 1.5 seconds\n.%s",ex.what());
93 ROS_ERROR(
"Transformation not found: Could not find valid transform after waiting 1.5 seconds\n.%s",ex.what());
101 q = trans_cam.getRotation();
102 double height = trans_cam.getOrigin().z() * 1000;
104 tf::Matrix3x3(q).getRPY(R, P, Y);
107 cout<<
"---> Camera configurations"<<endl;
108 cout<<
"imge size "<<endl<<
"\t"<<cam_info->height<<
" x "<<cam_info->width<<endl;
109 cout<<
"distortion_model:"<<endl<<
"\t"<<cam_info->distortion_model<<endl;
110 cout<<
"focal lengthstrans_cam"<<endl<<
"\t fx ="<<cam_info->K[0]<<
" fy = "<<cam_info->K[4]<<endl;
111 cout<<
"principal point"<<endl<<
"\t cx ="<<cam_info->K[2]<<
" cy = "<<cam_info->K[5]<<endl;
112 cout<<
"cameraHeight = "<<endl;
113 cout<<
"pitch ="<<P<<endl;
114 cout<<
"yaw = "<<Y<<endl;
117 P = (P + (M_PI/2)) * ( 180/M_PI );
118 Y = (Y + (M_PI/2)) * ( 180/M_PI );
122 std::string file_path=ros::package::getPath(
"caltech_lanes")+
"/config/CameraInfo.conf";
123 cout<<
"file_path:"<<file_path<<endl;
124 ofstream myfile(file_path.c_str(),ios::trunc);
125 if (myfile.is_open())
128 myfile<<
"\n# focal length\n";
129 myfile<<
"focalLengthX "<<cam_info->K[0]<<
"\n";
130 myfile<<
"focalLengthY "<<cam_info->K[4]<<
"\n";
131 myfile<<
"\n# optical center \n";
132 myfile<<
"opticalCenterX "<<cam_info->K[2]<<
"\n";
133 myfile<<
"opticalCenterY "<<cam_info->K[5]<<
"\n";
134 myfile<<
"\n# height of the camera in mm\n";
135 myfile<<
"cameraHeight "<<height<<
"\n";
136 myfile<<
"\n# pitch of the camera\n";
137 myfile<<
"pitch "<<P<<
"\n";
138 myfile<<
"\n# yaw of the camera\n";
139 myfile<<
"yaw "<<Y<<
"\n";
140 myfile<<
"\n# imag width and height\n";
141 myfile<<
"imageWidth "<<cam_info->width<<
"\n";
142 myfile<<
"imageHeight "<<cam_info->height<<
"\n";
146 cout<<
"---> Camera configurations Updated"<<endl;
151 cout <<
"Unable to open file"<<endl;
159 int main(
int argc,
char **argv)
161 ros::init(argc, argv,
"camera_configurations");
164 ros::NodeHandle nh(
"~");
166 nh.param(
"cam_info",messages,
string(
"/snr/scam/wide/left/camera_info"));
169 cout<<
"cam_info: "<<messages<<endl;
173 ROS_INFO(
"messages = %s",messages.c_str());
174 ros::Subscriber sub = nh.subscribe(messages.c_str(), 1,
Callback);
int main(int argc, char **argv)
void Callback(const sensor_msgs::CameraInfoPtr &cam_info)