void nav_callback(const projeto::QuadStatus status) { struct timeval stop, start; gettimeofday(&start, NULL); //do stuff double timestamp = status.header.stamp.toSec(); theta = Vector3d(status.theta.x, status.theta.y, status.theta.z); Vector3d x_new(status.position.x, status.position.y, status.position.z); Vector3d vel(status.vel.x, status.vel.y, status.vel.z); //theta(0) = degree_to_rad(msg_in.rotX); //theta(1) = degree_to_rad(msg_in.rotY); //theta(2) = degree_to_rad(msg_in.rotZ); //ROS_INFO("I heard ax: [%f] ay: [%f] az: [%f]", vx_, vy_, vz_); //Vector3d velV (vx_, vy_, vz_); //Quaternion<double> rotQ = rotation(theta); //Matrix3d R = rotQ.matrix(); //Vector3d vel = R * velV; //pthread_mutex_lock(&mutex_1); //float dt = timestamp - previous_tm; //geting dt in secs if (x_new(0) < -10 || x_new(0) > 10 || x_new(1) < -10 || x_new(1) > 10) { f_vector_print("theta", theta); f_vector_print("x", x); f_vector_print("x_new", x_new); return; } x = x_new; previous_tm = timestamp; // pthread_mutex_unlock(&mutex_1); Vector3d future_position; vector<Vector3d> trajectory = predict_trajectory2(vel, x, timestamp, timestamp + TIME_AHEAD, TRAJECTORY_DT, future_position); float short_dist = MAX_DIST; nav_msgs::GridCells gcells; vector<geometry_msgs::Point> obstaclerepo; if (!production_mode) { sensor_msgs::PointCloud pc; pc.header.frame_id = "/nav"; pc.header.stamp = ros::Time(); pc.channels.resize(1); pc.channels[0].name="trajectory"; pc.channels[0].values.resize(trajectory.size()); pc.points.resize(trajectory.size()); int i = 0; for (vector<Vector3d>::iterator it=trajectory.begin(); it!=trajectory.end(); ++it) { Vector3d pos = *it; pc.channels[0].values[i] = 0; pc.points[i].x = pos(0); pc.points[i].y = pos(1); pc.points[i].z = pos(2); i++; } pub_pc2.publish(pc); gcells.header.frame_id = "/nav"; gcells.header.stamp = ros::Time(); gcells.cell_width = OCTREE_RESOLUTION; gcells.cell_height = OCTREE_RESOLUTION; } if (control_mode) { // cout << "TIMESTAMP " << timestamp << endl; // cout << "CONTADOR " << contador << endl; // cout << "LIMIT " << CONTROL_LIMIT << endl; if (timestamp < contador + CONTROL_LIMIT) { double u_x = pid_x.getCommand(pos_obj(0) - x(0), timestamp); double u_y = pid_y.getCommand(pos_obj(1) - x(1), timestamp); double u_z = pid_z.getCommand(pos_obj(2) - x(2), timestamp); double u_yaw = pid_yaw.getCommand(yaw_obj - theta(2), timestamp); double cx = within(cos(theta(2)) * u_x + sin(theta(2)) * u_y, -1, 1); double cy = within(-sin(theta(2)) * u_x + cos(theta(2)) * u_y, -1, 1); double cz = within(u_z, -1, 1); double cyaw = within(u_yaw, -1, 1); // f_vector_print("objetivo", pos_obj); // f_vector_print("posicao", x); cout << "SENDING AUTOMATIC CONTROLS" << endl; Command cmd(cx, cy, cz, cyaw); send_velocity_command(cmd); } else { control_mode = 0; cout << "CONTROL MODE OFF" << endl; send_collision_mode_msg(false); pid_x.reset(); pid_y.reset(); pid_z.reset(); pid_yaw.reset(); } } else { OcTreeKey bbxMinKey, bbxMaxKey; //Vector3d lim1 = x + R * Vector3d(0, -DELTA_VOL/2, -1.0); //Vector3d lim2 = x + R * Vector3d(DELTA_VOL, DELTA_VOL/2, 1.0); point3d min_vol = point3d(x(0)-DELTA_VOL/2, x(1) -DELTA_VOL/2, x(2)-1.0); point3d max_vol = point3d(x(0)+DELTA_VOL/2, x(1) +DELTA_VOL/2, x(2)+1.0); tree.coordToKeyChecked(min_vol, bbxMinKey); tree.coordToKeyChecked(max_vol, bbxMaxKey); for(OcTree::leaf_bbx_iterator it = tree.begin_leafs_bbx(bbxMinKey, bbxMaxKey), end_bbx = tree.end_leafs_bbx(); it!= end_bbx; ++it){ //cout << "passei" << endl; point3d coords = it.getCoordinate(); Vector3d wrapped_coords = Vector3d(coords(0), coords(1), coords(2)); //if (!in_perimeter(x, wrapped_coords, 0.5)) { if (!production_mode) { geometry_msgs::Point cell; cell.x = coords(0); cell.y = coords(1); cell.z = coords(2); obstaclerepo.push_back(cell); } if (it->getValue() > OCCUPIED_PROB) { for (vector<Vector3d>::iterator it=trajectory.begin(); it!=trajectory.end(); ++it) { Vector3d pos = *it; if (there_will_be_collision(pos, wrapped_coords)) { float dist = (wrapped_coords - x).norm(); cout << "DIST " << dist << endl; if (dist < short_dist) { short_dist = dist; } break; } } } //manipulate node, e.g.: //cout << "Node center: " << it.getCoordinate() << endl; //cout << "Node size: " << it.getSize() << endl; //cout << "Node value: " << it->getValue() << endl; if (short_dist < MAX_DIST) { float ttc = short_dist/vel.norm(); if (ttc < TTC_LIMIT) { pos_obj = x; yaw_obj = atan2(sin(theta(2)),cos(theta(2))); send_collision_mode_msg(true); control_mode = 1; contador = timestamp; cout << "CONTROL MODE ON" << endl; pid_x.reset(); pid_y.reset(); pid_z.reset(); pid_yaw.reset(); } } } //} } if (!production_mode) { int count_cells = 0; gcells.cells.resize(obstaclerepo.size()); while (!obstaclerepo.empty()) { gcells.cells[count_cells++] = obstaclerepo.back(); obstaclerepo.pop_back(); } //pub_grid_cell.publish(gcells); } gettimeofday(&stop, NULL); previous_vel = vel; previous_theta = theta; //previous_omega = omega; gettimeofday(&stop, NULL); //cout << "time took: "<< stop.tv_sec - start.tv_sec << "." << (stop.tv_usec - start.tv_usec)/1000000 << "s" << endl; }
void MotorControl::stop_turn() { pid_interval_ticker.detach(); pid->reset(); }