static void reset(void) { int i; char text[5]="led0"; for(i=0;i<NB_LEDS;i++) { led[i]=wb_robot_get_device(text); text[3]++; wb_led_set(led[i],0); } // enable the camera cam = wb_robot_get_device("camera"); wb_camera_enable(cam,TIME_STEP_CAM); width = wb_camera_get_width(cam); height = wb_camera_get_height(cam); }
int main(int argc, char **argv) { printf("Comenzo todo\n"); wb_robot_init(); printf("Iniciando el sistema\n"); // Ros initialization ros::init(argc, argv, "Simulador"); ros::init(argc, argv, "Imagenes_de_Simulador"); ros::NodeHandle n,nh,nh2; // ros::Subscriber chatter_sub = n.subscribe("/auto_controller/command", 100, chatterCallback); ros::Subscriber chatter_sub = n.subscribe("/auto_controller/command", 100, chatterCallback); image_transport::ImageTransport it(nh); //OpenCV HighGUI call to destroy a display window on shut-down. image_transport::ImageTransport it2(nh2); image_transport::Publisher pub2 = it.advertise("/webot/camera", 1); //ros::spin(); // find front wheels left_front_wheel = wb_robot_get_device("left_front_wheel"); right_front_wheel = wb_robot_get_device("right_front_wheel"); wb_servo_set_position(left_front_wheel, INFINITY); wb_servo_set_position(right_front_wheel, INFINITY); // get steering motors left_steer = wb_robot_get_device("left_steer"); right_steer = wb_robot_get_device("right_steer"); // camera device camera = wb_robot_get_device("camera"); wb_camera_enable(camera, TIME_STEP); camera_width = wb_camera_get_width(camera); camera_height = wb_camera_get_height(camera); camera_fov = wb_camera_get_fov(camera); // initialize gps gps = wb_robot_get_device("gps"); wb_gps_enable(gps, TIME_STEP); // initialize display (speedometer) display = wb_robot_get_device("display"); speedometer_image = wb_display_image_load(display, "/root/research/PROYECTOTP/controllers/destiny_controller/speedometer.png"); wb_display_set_color(display, 0xffffff); // start engine speed=64.0; // km/h //print_help(); // allow to switch to manual control //wb_robot_keyboard_enable(TIME_STEP); // main loop printf("Iniciando el ciclo de peticiones\n"); // set_autodrive(false);z while (1) { const unsigned char *camera_image = wb_camera_get_image(camera); compute_gps_speed(); update_display(); //wb_camera_save_image (camera, "/tmp/imagen.png", 100); //cv::WImageBuffer3_b image(cvLoadImage("/tmp/imagen.png", CV_LOAD_IMAGE_COLOR)); IplImage* imagen=save_camera_image(camera_image); sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(imagen, "bgr8"); pub2.publish(msg); cvReleaseImage(&imagen ); ros::spinOnce(); // wb_servo_set_position(left_steer, 0); // wb_servo_set_position(right_steer, 0); wb_robot_step(TIME_STEP); } wb_robot_cleanup(); return 0; // ignored }
int main(int argc, char **argv) { printf("Comenzo todo\n"); wb_robot_init(); printf("Iniciando el sistema\n"); ros::init(argc, argv, "NEURONAL"); ros::NodeHandle n,nh,nh2; ros::Subscriber chatter_sub = n.subscribe("/auto_controller/command", 100, chatterCallback); // find front wheels left_front_wheel = wb_robot_get_device("left_front_wheel"); right_front_wheel = wb_robot_get_device("right_front_wheel"); wb_servo_set_position(left_front_wheel, INFINITY); wb_servo_set_position(right_front_wheel, INFINITY); // get steering motors left_steer = wb_robot_get_device("left_steer"); right_steer = wb_robot_get_device("right_steer"); // camera device camera = wb_robot_get_device("camera"); wb_camera_enable(camera, TIME_STEP); camera_width = wb_camera_get_width(camera); camera_height = wb_camera_get_height(camera); camera_fov = wb_camera_get_fov(camera); // initialize gps gps = wb_robot_get_device("gps"); wb_gps_enable(gps, TIME_STEP); // initialize display (speedometer) display = wb_robot_get_device("display"); speedometer_image = wb_display_image_load(display, "/root/research/PROYECTOTP/controllers/destiny_controller/speedometer.png"); wb_display_set_color(display, 0xffffff); // SICK sensor sick = wb_robot_get_device("lms291"); wb_camera_enable(sick, TIME_STEP); sick_width = wb_camera_get_width(sick); sick_range = wb_camera_get_max_range(sick); sick_fov = wb_camera_get_fov(sick); // start engine speed=64.0; // km/h // main loop printf("Iniciando el ciclo de peticiones\n"); // set_autodrive(false);z while (1) { const unsigned char *camera_image = wb_camera_get_image(camera); compute_gps_speed(); update_display(); ros::spinOnce(); wb_robot_step(TIME_STEP); } wb_robot_cleanup(); return 0; // ignored }
static void find_and_enable_devices() { // camera CameraTop = wb_robot_get_device("CameraTop"); CameraBottom = wb_robot_get_device("CameraBottom"); wb_camera_enable(CameraTop, 4 * time_step); wb_camera_enable(CameraBottom, 4 * time_step); // accelerometer accelerometer = wb_robot_get_device("accelerometer"); wb_accelerometer_enable(accelerometer, time_step); // gyro gyro = wb_robot_get_device("gyro"); wb_gyro_enable(gyro, time_step); // inertial unit inertial_unit = wb_robot_get_device("inertial unit"); wb_inertial_unit_enable(inertial_unit, time_step); // ultrasound sensors us[0] = wb_robot_get_device("USSensor1"); us[1] = wb_robot_get_device("USSensor2"); us[2] = wb_robot_get_device("USSensor3"); us[3] = wb_robot_get_device("USSensor4"); int i; for (i = 0; i < 4; i++) wb_distance_sensor_enable(us[i], time_step); // foot sensors fsr3d[0] = wb_robot_get_device("LFsr"); wb_touch_sensor_enable(fsr3d[0], time_step); fsr3d[1] = wb_robot_get_device("RFsr"); wb_touch_sensor_enable(fsr3d[1], time_step); // foot bumpers lfoot_lbumper = wb_robot_get_device("BumperLFootLeft"); lfoot_rbumper = wb_robot_get_device("BumperLFootRight"); rfoot_lbumper = wb_robot_get_device("BumperRFootLeft"); rfoot_rbumper = wb_robot_get_device("BumperRFootRight"); wb_touch_sensor_enable(lfoot_lbumper, time_step); wb_touch_sensor_enable(lfoot_rbumper, time_step); wb_touch_sensor_enable(rfoot_lbumper, time_step); wb_touch_sensor_enable(rfoot_rbumper, time_step); // There are 7 controlable LED groups in Webots leds[0] = wb_robot_get_device("ChestBoard/Led"); leds[1] = wb_robot_get_device("RFoot/Led"); leds[2] = wb_robot_get_device("LFoot/Led"); leds[3] = wb_robot_get_device("Face/Led/Right"); leds[4] = wb_robot_get_device("Face/Led/Left"); leds[5] = wb_robot_get_device("Ears/Led/Right"); leds[6] = wb_robot_get_device("Ears/Led/Left"); // get phalanx motor tags // the real Nao has only 2 motors for RHand/LHand // but in Webots we must implement RHand/LHand with 2x8 motors for (i = 0; i < PHALANX_MAX; i++) { char name[32]; sprintf(name, "LPhalanx%d", i + 1); lphalanx[i] = wb_robot_get_device(name); sprintf(name, "RPhalanx%d", i + 1); rphalanx[i] = wb_robot_get_device(name); } // shoulder pitch motors RShoulderPitch = wb_robot_get_device("RShoulderPitch"); LShoulderPitch = wb_robot_get_device("LShoulderPitch"); // shoulder pitch motors HeadYaw = wb_robot_get_device("HeadYaw"); HeadPitch = wb_robot_get_device("HeadPitch"); //LaserHead = wb_robot_get_device("URG-04LX-UG01"); //wb_camera_enable(LaserHead, 4 * time_step); // keyboard //wb_robot_keyboard_enable(10 * time_step); }