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 }