static int run(void) { int i; int *grey = (int *)malloc(sizeof(int)*width); int speed[2]={150,150}; ////default fixed speed, whther Epuck is on line or not const unsigned char *image; // 1. Get the sensors values image = wb_camera_get_image(cam); for (i = 0; i < width; i++) { grey[i] = 255-wb_camera_image_get_grey(image, width, i, 0); ////0 = black, 255 = white, so 255-0=255 in grey[i] = black (value representation is now reversed ////capture just topmost row for the width of the camera vision } // 2. Behavior-based robotic: // call the modules in the right order lem(grey,width); lfm(grey, width); llm(grey, width); //utm(); // 3. Send the values to actuators wb_differential_wheels_set_speed( speed[LEFT]+lfm_speed[LEFT],//+utm_speed[LEFT], speed[RIGHT]+lfm_speed[RIGHT]//+utm_speed[RIGHT] ); free(grey); return TIME_STEP; }
void report_step_state_to_supervisor(){ // send message to supervisor int width = wb_camera_get_width(CameraTop); int height = wb_camera_get_height(CameraTop); // read rgb pixel values from the camera const unsigned char *image = wb_camera_get_image(CameraTop); // printf("player\n"); //printf_ByteArray(image, width*height); wb_emitter_send(emitter, image, height * width * 8); //free(data_values); }
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 }
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 }