基于DJI-OSDK-ROS 3.4版本demo_camera_gimbal.cpp程序阅读注释

/** @file demo_camera_gimbal.cpp
 *  @version 3.3
 *  @date May, 2017
 *
 *  @brief
 *  demo sample of how to use camera and gimbal APIs
 *
 *  @copyright 2017 DJI. All rights reserved.
 *
 */
#include <dji_sdk_demo/demo_camera_gimbal.h>

// global variables
geometry_msgs::Vector3Stamped gimbal_angle;  
/*该消息类型
std_msg/Header header
uint32 seq
time stamp
string frame_id

geometry_msgs/Vector3 vector
float64 x
float64 y
float64 z
*/
ros::Subscriber         gimbal_angle_subscriber;  
ros::Publisher          gimbal_angle_cmd_publisher;
ros::Publisher          gimbal_speed_cmd_publisher;
ros::ServiceClient      drone_activation_service;
ros::ServiceClient      camera_action_service;

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "sdk_demo_camera_gimbal"); //定义该节点类型为sdk_demo_camera_gimbal
  ros::NodeHandle nh;                              //节点句柄为nh

  
  /* 订阅叫做dji_sdk/gimbal_angle的话题，该话题的消息类型为geometry_msgs::Vector3Stamped
    回调函数为gimbalAngleCallback（）
	订阅当前云台的角度，如果当前没有云台，则返回0
  */
  gimbal_angle_subscriber = nh.subscribe<geometry_msgs::Vector3Stamped>
    ("dji_sdk/gimbal_angle", 10, &gimbalAngleCallback);
  /* 发布叫做dji_sdk/gimbal_angle_cmd的话题
	Gimbal控制命令:控制云台的pitch和yaw(单位:0.1 deg)。模式:0 -增量控制，角度参考是当前云台的位置
	。1 -绝对控制，角度参考与DJI go App中的配置有关。
  */
  gimbal_angle_cmd_publisher = nh.advertise<dji_sdk::Gimbal>
    ("dji_sdk/gimbal_angle_cmd", 10);
	
	
   /* 发布叫做dji_sdk/gimbal_speed_cmd的话题，该话题的消息类型为geometry_msgs::Vector3Stamped
	   Gimbal speed命令:控制滚动俯仰和偏航角(单位:0.1 deg/ sec)的变化。
  */
  gimbal_speed_cmd_publisher = nh.advertise<geometry_msgs::Vector3Stamped>
    ("dji_sdk/gimbal_speed_cmd", 10);
	/* 
	激活飞机客户端
	该服务使用app ID和密钥对激活无人机。激活参数应该在启动文件中指定
	Response	
    bool result	true--succeed	false--invalid action
	*/
  drone_activation_service  = nh.serviceClient<dji_sdk::Activation>
    ("dji_sdk/activation");
	
	/* 
	相机动作客户端
	Request请求	
    Uint8 Camera_action	0--Shoot Photo	1--Start video taking	2--Stop video taking
    Response回应	
    bool result	true--succeed	false--invalid action
	*/
  camera_action_service     = nh.serviceClient<dji_sdk::CameraAction>
    ("dji_sdk/camera_action");

  // Activate
  if (activate().result) {     // 激活成功   显示激活成功  否则显示激活失败  
    ROS_INFO("Activated successfully");   
  } else {
    ROS_WARN("Failed activation");
    return -1;
  }

  // Display interactive prompt
  std::cout               
    << "| Available commands:                                             |"
    << std::endl;
  std::cout
    << "| [a] Exercise gimbal and camera control                         |"
    << std::endl;
  char inputChar;
  std::cin >> inputChar;           // 从键盘输入一个值存入inputeChar中

  switch(inputChar) {             // 若这个值为a，则执行云台相机控制程序，否则退出
    case 'a':
      gimbalCameraControl();
      break;
    default:
      break;
  }

  ros::spin();

  return 0;
}

bool
gimbalCameraControl()
{
  int responseTimeout = 0;

  GimbalContainer               gimbal;
  RotationAngle                 initialAngle;
  RotationAngle                 currentAngle;
  geometry_msgs::Vector3Stamped gimbalSpeed;

  // Get Gimbal initial values
  ros::spinOnce();
  initialAngle.roll = gimbal_angle.vector.y;   //获取云台初始角度值
  initialAngle.pitch = gimbal_angle.vector.x;
  initialAngle.yaw = gimbal_angle.vector.z;
  ROS_INFO("Please note that the gimbal yaw angle you see "
             "is w.r.t absolute North, and depends on your "
             "magnetometer calibration.");
  ROS_INFO("Initial Gimbal rotation angle: [ %f, %f, %f ] deg",   //显示角度值
           initialAngle.roll,
           initialAngle.pitch,
           initialAngle.yaw);

  // Re-set Gimbal to initial values
  gimbal = GimbalContainer(0,0,0,2,1,initialAngle);
  doSetGimbalAngle(&gimbal);

  ROS_INFO("Setting new Gimbal rotation angle to [0,20,180] deg"
             "using incremental control:");
  // Get current gimbal data to calc precision error in post processing
  ros::spinOnce();
  currentAngle.roll = gimbal_angle.vector.y;
  currentAngle.pitch = gimbal_angle.vector.x;
  currentAngle.yaw = gimbal_angle.vector.z;

  gimbal = GimbalContainer(0,20,180,2,0,initialAngle,currentAngle);
  doSetGimbalAngle(&gimbal);

  ros::spinOnce();
  currentAngle.roll = gimbal_angle.vector.y;
  currentAngle.pitch = gimbal_angle.vector.x;
  currentAngle.yaw = gimbal_angle.vector.z;

  displayResult(¤tAngle);

  // Take picture
  ROS_INFO("Ensure SD card is present.");
  ROS_INFO("Taking picture..");
  if (takePicture()) {
    ROS_INFO("Picture taken, Check DJI GO App or SD card for a new picture.");
  }else{
    ROS_WARN("Failed taking picture");
  }

  ROS_INFO("Setting new Gimbal rotation angle to [0,-50, 0] deg"
             "using absolute control:");
  gimbal = GimbalContainer(0,-50,0,2,1,initialAngle);
  doSetGimbalAngle(&gimbal);


  ros::spinOnce();
  currentAngle.roll = gimbal_angle.vector.y;
  currentAngle.pitch = gimbal_angle.vector.x;
  currentAngle.yaw = gimbal_angle.vector.z;
  displayResult(¤tAngle);

  // Start video: We will keep the video doing for the duration of the speed control.
  ROS_INFO("Ensure SD card is present.");
  ROS_INFO("Starting video..");
  if (startVideo()) {
    ROS_INFO("Start recording video");
  }else{
    ROS_WARN("Failed recording video");
    return false;
  }

  // Speed control
  ROS_INFO("Gimbal Speed Description: \n\n"
            "Roll  - unit rad/s input rate [-pi, pi]\n"
            "Pitch - unit rad/s input rate [-pi, pi]\n"
            "Yaw   - unit rad/s input rate [-pi, pi]\n\n");

  ROS_INFO("Setting Roll rate to 10 deg/s, Pitch rate to 5 deg/s, Yaw Rate to -20 deg/s.");
  gimbalSpeed.vector.y = DEG2RAD(10);
  gimbalSpeed.vector.x = DEG2RAD(5);
  gimbalSpeed.vector.z = DEG2RAD(-20);

  int speedControlDurationMs = 4000;
  int incrementMs = 100;
  for (int timer = 0; timer < speedControlDurationMs; timer+=incrementMs) {
    gimbal_speed_cmd_publisher.publish(gimbalSpeed);
    usleep(incrementMs*1000);
  }

  ros::spinOnce();
  currentAngle.roll = gimbal_angle.vector.y;
  currentAngle.pitch = gimbal_angle.vector.x;
  currentAngle.yaw = gimbal_angle.vector.z;
  displayResult(¤tAngle);

  // Reset the position
  ROS_INFO("Resetting position...");
  gimbal = GimbalContainer(0,0,0,2,1,initialAngle);
  doSetGimbalAngle(&gimbal);

  ros::spinOnce();
  currentAngle.roll = gimbal_angle.vector.y;
  currentAngle.pitch = gimbal_angle.vector.x;
  currentAngle.yaw = gimbal_angle.vector.z;
  displayResult(¤tAngle);

  // Stop the video
  ROS_INFO("Stopping video...");
  if (stopVideo()) {
    ROS_INFO("Stop recording video, Check DJI GO App or SD card for a new video.");
  }else{
    ROS_WARN("Failed stopping video");
    return false;
  }

  return true;
}

void
doSetGimbalAngle(GimbalContainer *gimbal)  //GimbalContainer(roll,pitch,yaw,duration,isAbsolute,initialAngle,currentAngle)
{                                          //Pitch angle, unit 0.1 degree, input range [-900,300]
  dji_sdk::Gimbal gimbal_angle_data;       //Roll angle, unit 0.1 degree, input range [-350,350]
  gimbal_angle_data.mode |= 0;             //Yaw angle, unit 0.1 degree , input range [-3200,3200]
  gimbal_angle_data.mode |= gimbal->isAbsolute;    //0,增量控制，角度参考和当前位置有关。1,绝对控制，角度参考和dji go的设置有关
  gimbal_angle_data.mode |= gimbal->yaw_cmd_ignore << 1;
  gimbal_angle_data.mode |= gimbal->roll_cmd_ignore << 2;
  gimbal_angle_data.mode |= gimbal->pitch_cmd_ignore << 3;
  gimbal_angle_data.ts    = gimbal->duration;       //单位:0.1s，例如20意味着你需要2秒到达所设置的角度
  gimbal_angle_data.roll  = DEG2RAD(gimbal->roll);  //度转弧度函数
  gimbal_angle_data.pitch = DEG2RAD(gimbal->pitch);
  gimbal_angle_data.yaw   = DEG2RAD(gimbal->yaw);

  gimbal_angle_cmd_publisher.publish(gimbal_angle_data);
  // Give time for gimbal to sync
  sleep(4);
}

void
displayResult(RotationAngle *currentAngle)
{
  ROS_INFO("New Gimbal rotation angle is [ %f, %f, %f ] deg",
           currentAngle->roll,
           currentAngle->pitch,
           currentAngle->yaw);
}

ServiceAck
activate()
{
  dji_sdk::Activation activation;
  drone_activation_service.call(activation);
  if(!activation.response.result) {
    ROS_WARN("ack.info: set = %i id = %i", activation.response.cmd_set, activation.response.cmd_id);
    ROS_WARN("ack.data: %i", activation.response.ack_data);
  }
  return {activation.response.result, activation.response.cmd_set,
          activation.response.cmd_id, activation.response.ack_data};
}

bool
takePicture()
{
  dji_sdk::CameraAction cameraAction;
  cameraAction.request.camera_action = 0;
  camera_action_service.call(cameraAction);
  return cameraAction.response.result;
}

bool
startVideo()
{
  dji_sdk::CameraAction cameraAction;
  cameraAction.request.camera_action = 1;
  camera_action_service.call(cameraAction);
  return cameraAction.response.result;
}

bool
stopVideo()
{
  dji_sdk::CameraAction cameraAction;
  cameraAction.request.camera_action = 2;
  camera_action_service.call(cameraAction);
  return cameraAction.response.result;
}

void
gimbalAngleCallback(const geometry_msgs::Vector3Stamped::ConstPtr& msg)
{
  gimbal_angle = *msg;
}