Hello everyone，
I use Raspberry Pi 4B to issue commands to control the drone’s fixed point flight, and use offboard mode. In addition, I installed a camera on the drone and wanted to recognize the logo and fly towards it. Therefore, I customized a message(“Marker_Deviation.msg”) about the coordinates of the logo and published it through ROS. I have added code to receive this message in the official offboard codes.
But there was a problem.I can execute code related to offsboard control and successfully receive custom information.I can execute code related to offboard control and successfully receive custom information. But my drone is unable to enter offboard mode and the buzzer sounds ’ Notify Negative Tone '.There is nothing displayed in my QGC, I want to check the flight log, but there is nothing.
When I removed the code related to receiving “Marker_Deviation.msg”, the drone could enter offboard mode normally and successfully take off. I don’t know why adding custom information would affect my control of drones. I hope to receive your help. I would like to express my gratitude in advance here.
The following is a code image related to “Marker_Deviation.msg” information. When I remove this part of the code, the drone can take off normally。
The custom message is shown in the figure:
The control code is as follows：
/**
-
@file offb_node.cpp
-
@brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
-
Stack and tested in Gazebo SITL
*/#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> #include <sensor_msgs/Imu.h> #include <nav_msgs/Odometry.h> #include <tf2/LinearMath/Quaternion.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include "usb_cam/Marker_Deviation.h" #include "usb_cam/UAV_Original_Position.h" mavros_msgs::State current_state; geometry_msgs::PoseStamped pose_pub,pose_sub; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; } void local_position_pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ pose_sub = *msg; //ROS_INFO("x:%.2f,y:%.2f,z:%.2f", pose_sub.pose.position.x, pose_sub.pose.position.y, pose_sub.pose.position.z); } void Marker_Deviation_Pose_cb(const usb_cam::Marker_Deviation::ConstPtr& msg){ usb_cam::Marker_Deviation Marker_Deviation_Pose; Marker_Deviation_Pose = *msg; ROS_INFO("x:%d,y:%d", Marker_Deviation_Pose.Deviation_x,Marker_Deviation_Pose.Deviation_y); } int main(int argc, char **argv) { ros::init(argc, argv, "offb_node"); ros::NodeHandle nh; ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Subscriber local_position_pose_sub = nh.subscribe<geometry_msgs::PoseStamped> ("mavros/local_position/pose", 10, local_position_pose_cb); ros::Subscriber Marker_Deviation_Pose_Sub = nh.subscribe<usb_cam::Marker_Deviation> ("img_dev_msg_topic", 5, Marker_Deviation_Pose_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 10); ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> ("mavros/set_mode"); //the setpoint publishing rate MUST be faster than 2Hz //20HZ ros::Rate rate(50.0); // wait for FCU connection while(ros::ok() && !current_state.connected){ ros::spinOnce(); rate.sleep(); } mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "OFFBOARD"; mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; ros::Time last_request = ros::Time::now(); usb_cam::UAV_Original_Position Original_Position; Original_Position.x=pose_sub.pose.position.x; Original_Position.y=pose_sub.pose.position.y; Original_Position.z=pose_sub.pose.position.z; pose_pub.pose.position.x = Original_Position.x+0; pose_pub.pose.position.y = Original_Position.y+0; pose_pub.pose.position.z = Original_Position.z+0.45; //pose_pub.pose.position.z = Original_Position.z+0.8; int First_Pose_Move_Flag=1; int Landing_Flag=0; while(ros::ok()){ if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ ROS_INFO("Offboard enabled"); } last_request = ros::Time::now(); } else { if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){ ROS_INFO("Vehicle armed"); } last_request = ros::Time::now(); } } if(pose_sub.pose.position.z>Original_Position.z+0.6||pose_sub.pose.position.x>Original_Position.x+0.4||pose_sub.pose.position.x<Original_Position.x-0.4||pose_sub.pose.position.y>Original_Position.y+0.4||pose_sub.pose.position.y<Original_Position.y-0.4) { ROS_INFO("Warn!!!"); break; } ros::spinOnce(); rate.sleep(); } return 0; }