Method Interface

src/simulator/test_interface/src/test_interface_node.cpp:22–61

};

Interface::Interface() {
  pub =
      nh.advertise<quadrotor_msgs::PositionCommand>("/planning/pos_cmd_1", 10);
  pub2 =
      nh.advertise<quadrotor_msgs::PositionCommand>("/planning/pos_cmd_2", 10);
  sub = nh.subscribe<geometry_msgs::PoseStamped>(
      "/quadrotor_1_pos_cmd", 10, &Interface::messageCallback, this);
  /// move_base_simple/goal
  sub2 = nh.subscribe<geometry_msgs::PoseStamped>(
      "/quadrotor_2_pos_cmd", 10, &Interface::messageCallback2, this);

  _n_seq = 0;

  /* kP */
  // double pos_gain[3] = { 5.7, 5.7, 6.2 };
  // double vel_gain[3] = { 3.4, 3.4, 4.0 };

  double pos_gain[3] = {7, 7, 6.2};
  double vel_gain[3] = {4, 4, 4.0};

  /* control parameter */
  cmd.kx[0] = pos_gain[0];
  cmd.kx[1] = pos_gain[1];
  cmd.kx[2] = pos_gain[2];

  cmd.kv[0] = vel_gain[0];
  cmd.kv[1] = vel_gain[1];
  cmd.kv[2] = vel_gain[2];

  /* control parameter */
  cmd2.kx[0] = pos_gain[0];
  cmd2.kx[1] = pos_gain[1];
  cmd2.kx[2] = pos_gain[2];

  cmd2.kv[0] = vel_gain[0];
  cmd2.kv[1] = vel_gain[1];
  cmd2.kv[2] = vel_gain[2];

  ros::spin();
}

void Interface::messageCallback(const geometry_msgs::PoseStampedConstPtr &msg) {
  // header