int main() {
const char *SERVO_NAMES[NUM_SERVOS] = {
"servo_r0",
"servo_r1",
"servo_r2",
"servo_l0",
"servo_l1",
"servo_l2"};
WbDeviceTag servos[NUM_SERVOS];
int elapsed = 0;
int state, i;
long double pos = 0,lastPos = 0; // int 害死了 0-6.28
int flag = 0;
const int dir[] = {1,1,1,1,1,1};//{1,1,1,-1,-1,-1}
// 增加在地面上的时间，就会更加稳定
const double rate = 1.0/6;// 1/4
wb_robot_init();
for (i = 0; i < NUM_SERVOS; i++) {
servos[i] = wb_robot_get_device(SERVO_NAMES[i]);
if (!servos[i]) {
printf("could not find servo: %s\n",SERVO_NAMES[i]);
}
}
pos = M_PI * rate ;
flag = 0;
while(wb_robot_step(TIME_STEP)!=-1) {
elapsed++;
state = (elapsed / 25 + 1) % NUM_STATES;
lastPos = pos;
if(flag) pos += 2*M_PI * rate;
else pos += 2*M_PI * (1-rate);
flag = !flag;
for (i = 0; i < 6; i+=2 ){
wb_servo_set_position(servos[i], dir[i]*pos);
}
for (i = 1; i < 6; i+=2 ) {
wb_servo_set_position(servos[i], dir[i]*lastPos);
}
}
wb_robot_cleanup();
return 0;
}
void chatterCallback(const boost::shared_ptr<geometry_msgs::Twist const>& msg)
{
//ROS_INFO("Received %f %f %f %f %f %f", msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z);
// speed=0;
//manual_steering = 0;
if(msg->linear.x==1)
{
printf("Acelerador Presionado\n");
speed=40.0; // km/h
}
if(msg->linear.x==5)
{
printf("Acelerador Presionado\n");
speed=-20.0; // km/h
}
if(msg->linear.x==-1)
{
printf("Freno Presionado \n");
speed=0;
}
manual_steering += msg->angular.z;
steering_angle = manual_steering * 0.05;
wb_servo_set_position(left_steer, steering_angle);
wb_servo_set_position(right_steer, steering_angle);
double turning_radius = AXLES_DIST / tan(steering_angle);
double differential_ratio = 1.0 - AXLE_LENGTH / turning_radius;
double left_speed = 2 * speed / (1 + differential_ratio);
double right_speed = left_speed * differential_ratio;
// set motor rotation speed
const double KMH_TO_RADS = 1000.0 / 3600.0 / WHEEL_DIAMETER * 2.0;
wb_servo_set_velocity(left_front_wheel, left_speed * KMH_TO_RADS);
wb_servo_set_velocity(right_front_wheel, right_speed * KMH_TO_RADS);
return;
}
int main(int argc, char **argv)
{
wb_robot_init();
int i;
WbDeviceTag servos[NSERVOS];
WbDeviceTag head_led;
for (i=0; i<NSERVOS; i++)
servos[i] = wb_robot_get_device(servo_names[i]);
head_led = wb_robot_get_device("HeadLed");
wb_led_set(head_led, 0x40C040);
do {
double t = wb_robot_get_time();
for (i=0; i<6; i++)
wb_servo_set_position(servos[i], amplitudes[i]*sin(frequency*t) + offsets[i]);
} while (wb_robot_step(TIME_STEP) != -1);
wb_robot_cleanup();
return 0;
}
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
}