使用velocityAndYawRateCtrl进行速度控制模式飞行时，报错：Exception thrown wh...
Completed
-
我的调用调用代码流程，大致如下，代码经过了一定程度的删减：
#include <vector>
#include "dji_htkg/flight_control_region.h"
#include "dji_osdk_ros/DroneTaskControl.h"
#include "dji_osdk_ros/common_type.h"
#include "dji_htkg/common/djiconfig.h"
#include "dji_htkg/common/tcpSendPic.h"
#include "dji_htkg/FlightMoveOffset.h"#include "dji_htkg/common/dji_share_memory.h"
#include <dji_osdk_ros/FlightTaskControl.h>
#include <dji_osdk_ros/ObtainControlAuthority.h>#include <dji_osdk_ros/SetJoystickMode.h>
#include <dji_osdk_ros/JoystickAction.h>
#include <dji_osdk_ros/EmergencyBrake.h>#define ROOM_TEST_STATUS 0
const float deg2rad = C_PI / 180.0;
const float rad2deg = 180.0 / C_PI;ros::ServiceClient drone_task_service;
//ros::ServiceClient query_version_service;ros::ServiceClient set_joystick_mode_client;
ros::ServiceClient joystick_action_client;
ros::ServiceClient emergency_brake_client;
sensor_msgs::NavSatFix current_gps;//速度控制模式函数
void velocityAndYawRateCtrl(const dji_osdk_ros::JoystickCommand &offsetDesired, uint32_t timeMs)
{
double originTime = 0;
double currentTime = 0;
uint64_t elapsedTimeInMs = 0;dji_osdk_ros::SetJoystickMode joystickMode;
dji_osdk_ros::JoystickAction joystickAction;joystickMode.request.horizontal_mode = joystickMode.request.HORIZONTAL_VELOCITY;
joystickMode.request.vertical_mode = joystickMode.request.VERTICAL_VELOCITY;
joystickMode.request.yaw_mode = joystickMode.request.YAW_RATE;
joystickMode.request.horizontal_coordinate = joystickMode.request.HORIZONTAL_GROUND;
joystickMode.request.stable_mode = joystickMode.request.STABLE_ENABLE;
set_joystick_mode_client.call(joystickMode);joystickAction.request.joystickCommand.x = offsetDesired.x;
joystickAction.request.joystickCommand.y = offsetDesired.y;
joystickAction.request.joystickCommand.z = offsetDesired.z;
joystickAction.request.joystickCommand.yaw = offsetDesired.yaw;originTime = ros::Time::now().toSec();
currentTime = originTime;
elapsedTimeInMs = (currentTime - originTime) * 1000;while (elapsedTimeInMs <= timeMs)
{
currentTime = ros::Time::now().toSec();
elapsedTimeInMs = (currentTime - originTime) * 1000;
joystick_action_client.call(joystickAction);
}
}
//无人机巡航路径
void cruise_step(int cruise_finish)
{
float xCmd, yCmd, zCmd;double xOffsetRemaining, yOffsetRemaining, zOffsetRemaining;
double x_Remaining, y_Remaining;
ROS_INFO("cruise_finish=====%d", cruise_finish);if (cruise_finish == 1)
{
xOffsetRemaining = waypoint_gps[cruise_flag][0] - current_gps.longitude; //经度差
yOffsetRemaining = waypoint_gps[cruise_flag][1] - current_gps.latitude; //纬度差
zOffsetRemaining = 0;
y_Remaining = yOffsetRemaining * deg2rad * C_EARTH;
x_Remaining = xOffsetRemaining * deg2rad * C_EARTH * cos(deg2rad*waypoint_gps[cruise_flag][1]);ROS_INFO("xOffsetRemaining==%lf,yOffsetRemaining==%lf", xOffsetRemaining, yOffsetRemaining);
}ROS_INFO("xcmd==%lf,ycmd==%lf", x_Remaining, y_Remaining);
float velocity_expect = 10; //单位：米/秒
float velocity_y, velocity_x;
int thread_v_to_offset = 10; //速度控制模式切换到偏移量控制模式的距离阈值if (std::abs(y_Remaining) > thread_v_to_offset) //当偏差较大时，根据速度进行控制
{
if (y_Remaining > 0)
velocity_y = velocity_expect;
else
velocity_y = -velocity_expect;uint32_t time = std::abs(y_Remaining) * 1000 / velocity_expect; //单位：毫秒
double velocity_x = 1000 * x_Remaining / time;dji_osdk_ros::EmergencyBrake emergency_brake;
velocityAndYawRateCtrl({ velocity_y, velocity_x, 0, 0 }, time); //速度模式控制飞行
emergency_brake_client.call(emergency_brake);
ros::Duration(2).sleep();ROS_INFO("velocity_expect=%lf, velocity_y=%lf, velocity_x=%lf, time=%d", velocity_expect, velocity_y, velocity_x, time);
}
}//回调函数
void gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
current_gps = *msg;
int cruise_finish = 1;cruise_step(cruise_finish);
}////////////////////////
// 主函数
////////////////////////
int main()
{
ros::init(argc, argv, "flight_control_region");
ros::NodeHandle nh;ros::Subscriber gpsSub = nh.subscribe("dji_osdk_ros/gps_position", 10, &gps_callback); //订阅
emergency_brake_client = nh.serviceClient<dji_osdk_ros::EmergencyBrake>("emergency_brake");
set_joystick_mode_client = nh.serviceClient<dji_osdk_ros::SetJoystickMode>("set_joystick_mode");
joystick_action_client = nh.serviceClient<dji_osdk_ros::JoystickAction>("joystick_action");return 0;
}
Please sign in to leave a comment.
Comments
4 comments