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);
}
