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