int main(int argc, char **argv) { ros::init(argc, argv, "mono_odometer"); if (ros::names::remap("image").find("rect") == std::string::npos) { ROS_WARN("mono_odometer needs rectified input images. The used image " "topic is '%s'. Are you sure the images are rectified?", ros::names::remap("image").c_str()); } std::string transport = argc > 1 ? argv[1] : "raw"; viso2_ros::MonoOdometer odometer(transport); ros::spin(); return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "stereo_odometer"); if (ros::names::remap("stereo") == "stereo") { ROS_WARN("'stereo' has not been remapped! Example command-line usage:\n" "\t$ rosrun viso2_ros stereo_odometer stereo:=narrow_stereo image:=image_rect"); } if (ros::names::remap("image").find("rect") == std::string::npos) { ROS_WARN("stereo_odometer needs rectified input images. The used image " "topic is '%s'. Are you sure the images are rectified?", ros::names::remap("image").c_str()); } std::string transport = argc > 1 ? argv[1] : "raw"; viso2_ros::StereoOdometer odometer(transport); ros::spin(); return 0; }
void main(void){ init_config(); if(input(IR_RECEIVER)){ fprintf(SERIAL, "Conexión no iniciada.\n\r\n\r"); } while(input(IR_RECEIVER)); fprintf(SERIAL, "Conexión iniciada.\n\r\n\r"); RBIF=0; // Se limpia la bandera de interrupcion por cambio de estado de RB. enable_interrupts(GLOBAL); // Permiso Global de interrupciones. while(1){ adcPwmEnhanced(52.0); //52 es el valor de CCPRxL:CCPxCON<5:4> cuando el "duty" esta al 100%. //Si hay algo que notificar entonces se realiza por medio de una comunicacion serial. if(notification){ notification=false; //Se desactiva la notificacion serial. serialNotification(); } //Si el enlace esta bloqueado da vuelta, sino camina hacia adelante. if(really_blocked){ driverStop(); driverLeft(); } else{ driverForward(); } //Calculo para la distancia recorrida y velocidad (odometro). if(sec_odom){ sec_odom=false; odometer(2, 0.01); } } //</while> } //</main>