00001 #include <termios.h>
00002 #include <signal.h>
00003 #include <math.h>
00004 #include <stdio.h>
00005 #include <stdlib.h>
00006 #include <ros/ros.h>
00007 #include "virtual_camera/VirtualCameraParameters.h"
00008
00009 #define KEYCODE_A 0x61
00010 #define KEYCODE_D 0x64
00011 #define KEYCODE_S 0x73
00012 #define KEYCODE_W 0x77
00013 #define KEYCODE_Q 0x71
00014 #define KEYCODE_E 0x65
00015 #define KEYCODE_X 0x78
00016 #define KEYCODE_C 0x63
00017 #define KEYCODE_V 0x76
00018 #define KEYCODE_B 0x62
00019 #define KEYCODE_R 0x72
00020 #define KEYCODE_F 0x66
00021 #define KEYCODE_M 0x6D
00022 #define KEYCODE_K 0x6B
00023 #define KEYCODE_1 0x31
00024 #define KEYCODE_2 0x32
00025 #define KEYCODE_3 0x33
00026 #define KEYCODE_4 0x34
00027 #define KEYCODE_5 0x35
00028 #define KEYCODE_6 0x36
00029
00030 #define KEYCODE_A_CAP 0x41
00031 #define KEYCODE_D_CAP 0x44
00032 #define KEYCODE_S_CAP 0x53
00033 #define KEYCODE_W_CAP 0x57
00034 #define KEYCODE_Q_CAP 0x51
00035 #define KEYCODE_E_CAP 0x45
00036 #define KEYCODE_R_CAP 0x52
00037 #define KEYCODE_F_CAP 0x46
00038
00039 #define KEYCODE_SPACE 0x20
00040
00041 #define MOVE_STEP 0.2
00042 #define APPROX_DIST_STEP 0.2
00043 #define PAN_STEP 5
00044 #define TILT_STEP 5
00045 #define ZOOM_H_STEP 4
00046 #define ZOOM_V_STEP 3
00047 #define MOVE_STEP_FAST 2
00048 #define PAN_STEP_FAST 20
00049 #define TILT_STEP_FAST 20
00050 #define ZOOM_H_STEP_FAST 16
00051 #define ZOOM_V_STEP_FAST 12
00052 #define APPROX_DIST_STEP_FAST 1
00053
00060
00061
00062
00063
00064 const int NUM_MODELS = 3;
00065 std::string models[NUM_MODELS] = {"pinhole", "cylindric", "spherical"};
00066 void (*resolutionHandlers[NUM_MODELS])();
00067 int model = 0;
00068 std::string origModel;
00069
00070
00071
00072
00073 int32_t viewportWidth = 320, viewportHeight = 240;
00074 double pan = 0, tilt = 0, horizontalFov = 60, verticalFov = 45;
00075 double origPan, origTilt, origHorizontalFov, origVerticalFov, origX, origY, origZ,
00076 origApproxDistance, origWidth, origHeight, origSynchronize;
00077 double pX = 0, pY = 0, pZ = 0;
00078 int synchronize = 0;
00079 double approxDistance = 3;
00080 double frameRate;
00086 int quality = 4;
00087
00088 int kfd = 0;
00089 double zoom = 1.0;
00090 struct termios cooked, raw;
00091 ros::Publisher pub;
00092 std::string parentFrameId, encoding, name, pubTopic;
00093
00099 void pinholeResolutions() {
00100 viewportWidth = 160*quality;
00101 viewportHeight = 120*quality;
00102 }
00103
00104 void sphericalResolutions() {
00105 viewportWidth = 200*quality;
00106 viewportHeight = 100*quality;
00107 }
00108
00109 void cylindricResolutions() {
00110 double k = 360./horizontalFov;
00111 if (k>12) k = 12;
00112 viewportHeight = std::floor(std::sqrt(20000/k)*quality);
00113 viewportWidth = std::floor(k*viewportHeight);
00114 }
00115
00120 void updateResolutions() {
00121 (*(resolutionHandlers[model]))();
00122 }
00123
00124
00125 void quit(int sig)
00126 {
00127 tcsetattr(kfd, TCSANOW, &cooked);
00128 exit(0);
00129 }
00130
00131 void setOriginalValues() {
00132 pan = origPan;
00133 tilt = origTilt;
00134 horizontalFov = origHorizontalFov;
00135 verticalFov = origVerticalFov;
00136 pX = origX;
00137 pY = origY;
00138 pZ = origZ;
00139 approxDistance = origApproxDistance;
00140 viewportWidth = origWidth;
00141 viewportHeight = origHeight;
00142 synchronize = origSynchronize;
00143 if (origModel == "cylindric") {
00144 model = 1;
00145 } else if (origModel == "spherical") {
00146 model = 2;
00147 } else {
00148 model = 0;
00149 }
00150
00151 }
00152
00153 void keyboardLoop()
00154 {
00155 char c;
00156 bool do_nothing = true;
00157
00158
00159 tcgetattr(kfd, &cooked);
00160 memcpy(&raw, &cooked, sizeof(struct termios));
00161 raw.c_lflag &=~ (ICANON | ECHO);
00162
00163 raw.c_cc[VEOL] = 1;
00164 raw.c_cc[VEOF] = 2;
00165 tcsetattr(kfd, TCSANOW, &raw);
00166
00167 ROS_INFO("Reading from keyboard");
00168 ROS_INFO("---------------------");
00169 ROS_INFO("a,d - pan");
00170 ROS_INFO("w,s - tilt");
00171 ROS_INFO("q,e - zoom");
00172 ROS_INFO("r,f - change distance");
00173 ROS_INFO("1,2 - move X");
00174 ROS_INFO("3,4 - move Y");
00175 ROS_INFO("5,6 - move Z");
00176 ROS_INFO("any of above with Shift - faster");
00177 ROS_INFO("Space - reset to the default position");
00178 ROS_INFO("x,c,v,b - resolution (x=ultra-low, c=low, v=medium, b=high)");
00179 ROS_INFO("m - cycle through the camera models");
00180 ROS_INFO("k - turn synchronization on/off");
00181
00182 while(true) {
00183
00184 if(read(kfd, &c, 1) < 0)
00185 {
00186 perror("read():");
00187 exit(-1);
00188 }
00189
00190 do_nothing = false;
00191
00192 switch(c)
00193 {
00194 case KEYCODE_W:
00195 tilt += TILT_STEP;
00196 break;
00197 case KEYCODE_S:
00198 tilt -= TILT_STEP;
00199 break;
00200 case KEYCODE_A:
00201 pan += PAN_STEP;
00202 break;
00203 case KEYCODE_D:
00204 pan -= PAN_STEP;
00205 break;
00206 case KEYCODE_Q:
00207 horizontalFov += ZOOM_H_STEP;
00208 verticalFov += ZOOM_V_STEP;
00209 updateResolutions();
00210 break;
00211 case KEYCODE_E:
00212 horizontalFov -= ZOOM_H_STEP;
00213 verticalFov -= ZOOM_V_STEP;
00214 updateResolutions();
00215 break;
00216 case KEYCODE_X:
00217 quality = 1;
00218 updateResolutions();
00219 break;
00220 case KEYCODE_C:
00221 quality = 2;
00222 updateResolutions();
00223 break;
00224 case KEYCODE_V:
00225 quality = 4;
00226 updateResolutions();
00227 break;
00228 case KEYCODE_B:
00229 quality = 5;
00230 updateResolutions();
00231 break;
00232 case KEYCODE_M:
00233 ++model;
00234 model = model % NUM_MODELS;
00235 updateResolutions();
00236 break;
00237 case KEYCODE_K:
00238 synchronize = !synchronize;
00239 break;
00240 case KEYCODE_1:
00241 pX -= MOVE_STEP;
00242 break;
00243 case KEYCODE_2:
00244 pX += MOVE_STEP;
00245 break;
00246 case KEYCODE_3:
00247 pY -= MOVE_STEP;
00248 break;
00249 case KEYCODE_4:
00250 pY += MOVE_STEP;
00251 break;
00252 case KEYCODE_5:
00253 pZ -= MOVE_STEP;
00254 break;
00255 case KEYCODE_6:
00256 pZ += MOVE_STEP;
00257 break;
00258 case KEYCODE_R:
00259 approxDistance -= MOVE_STEP;
00260 break;
00261 case KEYCODE_F:
00262 approxDistance += MOVE_STEP;
00263 break;
00264
00265
00266 case KEYCODE_W_CAP:
00267 tilt += TILT_STEP_FAST;
00268 break;
00269 case KEYCODE_S_CAP:
00270 tilt -= TILT_STEP_FAST;
00271 break;
00272 case KEYCODE_A_CAP:
00273 pan += PAN_STEP_FAST;
00274 break;
00275 case KEYCODE_D_CAP:
00276 pan -= PAN_STEP_FAST;
00277 break;
00278 case KEYCODE_Q_CAP:
00279 horizontalFov += ZOOM_H_STEP_FAST;
00280 verticalFov += ZOOM_V_STEP_FAST;
00281 updateResolutions();
00282 break;
00283 case KEYCODE_E_CAP:
00284 horizontalFov -= ZOOM_H_STEP_FAST;
00285 verticalFov -= ZOOM_V_STEP_FAST;
00286 updateResolutions();
00287 break;
00288 case KEYCODE_R_CAP:
00289 approxDistance -= MOVE_STEP_FAST;
00290 break;
00291 case KEYCODE_F_CAP:
00292 approxDistance += MOVE_STEP_FAST;
00293 break;
00294
00295 case KEYCODE_SPACE:
00296 setOriginalValues();
00297 break;
00298
00299 default:
00300 do_nothing = true;
00301 break;
00302 }
00303
00304 if(!do_nothing) {
00305
00306 pan = (double) ((int) (pan < 0 ? 360+pan : pan) % 360);
00307 tilt = (double) ((int) (tilt < 0 ? 360+tilt : tilt) % 360);
00308
00309 if(horizontalFov < 4) horizontalFov = 4;
00310 else if(horizontalFov > 176) horizontalFov = 176;
00311
00312 if(verticalFov < 3) verticalFov = 3;
00313 else if(verticalFov > 132) verticalFov = 132;
00314
00315 virtual_camera::VirtualCameraParameters msg;
00316 msg.viewportWidth = viewportWidth;
00317 msg.viewportHeight = viewportHeight;
00318 msg.pan = pan;
00319 msg.tilt = tilt;
00320 msg.horizontalFov = horizontalFov;
00321 msg.verticalFov = verticalFov;
00322
00323 msg.parentFrameId = parentFrameId;
00324 msg.approxDistance = approxDistance;
00325 msg.posX = pX;
00326 msg.posY = pY;
00327 msg.posZ = pZ;
00328 msg.frameRate = frameRate;
00329 msg.encoding = encoding;
00330 msg.model = models[model];
00331 msg.synchronize = synchronize;
00332 pub.publish(msg);
00333
00334 ROS_INFO("*** keyboard_teleop: Sent new camera parameters ***");
00335 ROS_INFO("viewportWidth=%i viewportHeight=%i", viewportWidth, viewportHeight);
00336 ROS_INFO("pan=%f tilt=%f", pan, tilt);
00337 ROS_INFO("frame: %s, X: %.3f, Y: %.3f, Z: %.3f", parentFrameId.c_str(), pX, pY, pZ);
00338 ROS_INFO("Approx. distance: %.3f", approxDistance);
00339 ROS_INFO("horizontalFov=%f verticalFov=%f", horizontalFov, verticalFov);
00340 ROS_INFO("synchronization: %s", synchronize ? "on" : "off");
00341 ROS_INFO("Model: %s", models[model].c_str());
00342
00343 ros::spinOnce();
00344 }
00345 }
00346 }
00347
00348
00349 int main(int argc, char** argv)
00350 {
00351 ros::init(argc, argv, "keyboard_teleop");
00352 ros::NodeHandle n("~");
00353
00354 n.getParam("virtualCameraTopic", pubTopic);
00355 n.getParam("approxDistance", origApproxDistance);
00356 n.getParam("viewportWidth", origWidth);
00357 n.getParam("viewportHeight", origHeight);
00358 n.getParam("parentFrameId", parentFrameId);
00359 n.getParam("posX", origX);
00360 n.getParam("posY", origY);
00361 n.getParam("posZ", origZ);
00362 n.getParam("tilt", origTilt);
00363 n.getParam("pan", origPan);
00364 n.getParam("horizontalFov", origHorizontalFov);
00365 n.getParam("verticalFov", origVerticalFov);
00366 n.getParam("encoding", encoding);
00367 n.getParam("frameRate", frameRate);
00368 n.getParam("model", origModel);
00369 n.getParam("synchronize", origSynchronize);
00370
00371 resolutionHandlers[0] = pinholeResolutions;
00372 resolutionHandlers[1] = cylindricResolutions;
00373 resolutionHandlers[2] = sphericalResolutions;
00374
00375
00376 setOriginalValues();
00377 pub = n.advertise<virtual_camera::VirtualCameraParameters>(n.resolveName("/"+ pubTopic + "/input_params"), 1000);
00378 signal(SIGINT, quit);
00379 keyboardLoop();
00380 return(0);
00381 }
00382
00383
