IplImage* save_camera_image(const unsigned char *image){
int imgPixel = 0;
int row;
int col;
int width = wb_camera_get_width(camera);
int height = wb_camera_get_height(camera);
// read rgb pixel values from the camera
//create image in OpenCV format with width and height of the webots camera image
IplImage* imgOpencv = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
//information about the imgOpencv
int widthStep = imgOpencv->widthStep;
int nChannels = imgOpencv->nChannels;
uchar* data = (uchar *)imgOpencv->imageData;
//counts the number of the channel that is being accessed
int numberChannel;
// redChannel = 2;
// greenChannel = 1;
// blueChannel = 0;
for(numberChannel = 0; numberChannel < 3; numberChannel++){
for(col = 0; col<height; col++){
for(row = 0; row<width; row++){
//printf("\n(%d,%d,%d)\n",numberChannel,row,col);
if(numberChannel == 0){
imgPixel = wb_camera_image_get_blue(image,width,row,col);
}
if(numberChannel == 1){
imgPixel = wb_camera_image_get_green(image,width,row,col);
}
if(numberChannel == 2){
imgPixel = wb_camera_image_get_red(image,width,row,col);
}
data[col * widthStep + row * nChannels + numberChannel] = imgPixel;
}
}
}
return imgOpencv;
}
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);
}
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
}
