Function cmdCallback

swarm_exploration/plan_manage/test/process_msg.cpp:107–152

}

void cmdCallback(const quadrotor_msgs::PositionCommandConstPtr& msg) {
  ROS_INFO_ONCE("start");
  Eigen::Vector3d pt, v;
  pt(0) = msg->position.x;
  pt(1) = msg->position.y;
  pt(2) = msg->position.z;
  v(0) = msg->velocity.x;
  v(1) = msg->velocity.y;
  v(2) = msg->velocity.z;
  auto cur_time = ros::Time::now();

  // if (pt(0) > 5 && pt(1) < 3) return;

  if (traj.size() > 0) {
    distance1 += (pt - traj.back()).norm();
  }

  traj.push_back(pt);
  vel.push_back(v);
  displayTrajWithColor(traj, 0.1, Eigen::Vector4d(1, 0, 0, 1), 0);

  double phi = msg->yaw;
  yaw.push_back(phi);
  yaw1.clear();
  yaw2.clear();
  for (int k = 0; k < 4; ++k) {
    int idx = yaw.size() - 1 - 30 * k;
    if (idx < 0) continue;
    double phi_k = yaw[idx];
    Eigen::Vector3d pt_k = traj[idx];
    Eigen::Matrix3d Rwb;
    Rwb << cos(phi_k), -sin(phi_k), 0, sin(phi_k), cos(phi_k), 0, 0, 0, 1;
    for (int i = 0; i < cam1.size(); ++i) {
      auto p1 = Rwb * cam1[i] + pt_k;
      auto p2 = Rwb * cam2[i] + pt_k;
      yaw1.push_back(p1);
      yaw2.push_back(p2);
    }
  }
  // displayLineList(yaw1, yaw2, 0.02, Eigen::Vector4d(0, 0, 0, 1), 0);

  // std::cout << v(0) << "," << v(1) << "," << v(2) << ",";
  if (v.norm() < 1e-3) {
    ROS_INFO("end, distance: %lf", distance1);
  }
}

bool endtraj = false;
void trajCallback(const visualization_msgs::MarkerConstPtr& msg) {