基于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 variablesgeometry_msgs::Vector3Stamped gimbal_angle;  /*该消息类型std_msg/Header headeruint32 seqtime stampstring frame_idgeometry_msgs/Vector3 vectorfloat64 xfloat64 yfloat64 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;intmain(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 resulttrue--succeedfalse--invalid action*/  drone_activation_service  = nh.serviceClient<dji_sdk::Activation>    ("dji_sdk/activation");/* 相机动作客户端Request请求    Uint8 Camera_action0--Shoot Photo1--Start video taking2--Stop video taking    Response回应    bool resulttrue--succeedfalse--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;}boolgimbalCameraControl(){  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;}voiddoSetGimbalAngle(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);}voiddisplayResult(RotationAngle *currentAngle){  ROS_INFO("New Gimbal rotation angle is [ %f, %f, %f ] deg",           currentAngle->roll,           currentAngle->pitch,           currentAngle->yaw);}ServiceAckactivate(){  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};}booltakePicture(){  dji_sdk::CameraAction cameraAction;  cameraAction.request.camera_action = 0;  camera_action_service.call(cameraAction);  return cameraAction.response.result;}boolstartVideo(){  dji_sdk::CameraAction cameraAction;  cameraAction.request.camera_action = 1;  camera_action_service.call(cameraAction);  return cameraAction.response.result;}boolstopVideo(){  dji_sdk::CameraAction cameraAction;  cameraAction.request.camera_action = 2;  camera_action_service.call(cameraAction);  return cameraAction.response.result;}voidgimbalAngleCallback(const geometry_msgs::Vector3Stamped::ConstPtr& msg){  gimbal_angle = *msg;}