Skip to content

Commit

Permalink
[v4.0.0] [new feature]: safely Offboard via mavros
Browse files Browse the repository at this point in the history
  • Loading branch information
@HuaYuXiao
HuaYuXiao committed Aug 7, 2024
1 parent fd0eb85 commit 2096f79
Show file tree
Hide file tree
Showing 5 changed files with 46 additions and 109 deletions.
19 changes: 0 additions & 19 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@ set(PYTHON_EXECUTABLE "/usr/bin/python3.8")
find_package(Boost REQUIRED COMPONENTS system)
#find_package(PythonInterp REQUIRED 3)
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
mavros
message_filters
Expand All @@ -26,20 +25,6 @@ find_package(catkin REQUIRED COMPONENTS
easondrone_msgs
)

################################################
## Declare ROS messages, services and actions ##
################################################

## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
geometry_msgs
nav_msgs
sensor_msgs
std_msgs
easondrone_msgs
)

catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS message_runtime
Expand All @@ -56,10 +41,6 @@ include_directories(
include
)

###############################
## 声明可执行cpp文件 ##
###############################

##px4ctrl_node.cpp
add_executable(px4ctrl_node src/px4ctrl_node.cpp)
add_dependencies(px4ctrl_node ${PROJECT_NAME}_gencpp)
Expand Down
63 changes: 7 additions & 56 deletions include/px4ctrl_terminal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,54 +29,6 @@ easondrone_msgs::ControlCommand Command_to_pub;
//发布
ros::Publisher easondrone_ctrl_pub_;


void generate_com(int Move_mode, float state_desired[4]){
//# Move_mode 2-bit value:
//# 0 for position, 1 for vel, 1st for xy, 2nd for z.
//# xy position xy velocity
//# z position 0b00(0) 0b10(2)
//# z velocity 0b01(1) 0b11(3)

if(Move_mode == easondrone_msgs::PositionReference::XYZ_ACC){
cout << "ACC control not support yet." <<endl;
}

if((Move_mode & 0b10) == 0) //xy channel
{
Command_to_pub.Reference_State.position_ref[0] = state_desired[0];
Command_to_pub.Reference_State.position_ref[1] = state_desired[1];
Command_to_pub.Reference_State.velocity_ref[0] = 0;
Command_to_pub.Reference_State.velocity_ref[1] = 0;
}
else{
Command_to_pub.Reference_State.position_ref[0] = 0;
Command_to_pub.Reference_State.position_ref[1] = 0;
Command_to_pub.Reference_State.velocity_ref[0] = state_desired[0];
Command_to_pub.Reference_State.velocity_ref[1] = state_desired[1];
}

if((Move_mode & 0b01) == 0) //z channel
{
Command_to_pub.Reference_State.position_ref[2] = state_desired[2];
Command_to_pub.Reference_State.velocity_ref[2] = 0;
}
else{
Command_to_pub.Reference_State.position_ref[2] = 0;
Command_to_pub.Reference_State.velocity_ref[2] = state_desired[2];
}

Command_to_pub.Reference_State.acceleration_ref[0] = 0;
Command_to_pub.Reference_State.acceleration_ref[1] = 0;
Command_to_pub.Reference_State.acceleration_ref[2] = 0;

if(Command_to_pub.Reference_State.Yaw_Rate_Mode == 1){
Command_to_pub.Reference_State.yaw_rate_ref = state_desired[3];
}
else{
Command_to_pub.Reference_State.yaw_ref = state_desired[3]/180.0*M_PI;
}
}

void mainloop(){
int Control_Mode = 0;
bool valid_Control_Mode = false;
Expand All @@ -91,12 +43,12 @@ void mainloop(){
bool valid_yaw_input = false;

while(ros::ok()){

while (!valid_Control_Mode){
string msg = "-------- EasonDrone Terminal --------";
cout_color(msg, BLUE_COLOR);
cout << "Please choose the Command.Mode: " << endl;
cout << " 0 Arm | 1 Offboard | 2 Takeoff | 3 Move " << endl;
cout << " 4 Hold | 5 Land | 6 Manual | 7 Disarm " << endl;
cout << "--------------------------------" << endl;
cout << "Enter command to mavros: " << endl;
cout << "| 0 Arm | 1 Offboard | 2 Takeoff | 3 Move |" << endl;
cout << "| 4 Hold | 5 Land | 6 Manual | 7 Disarm |" << endl;
if (cin >> Control_Mode) {
if (valid_modes.find(Control_Mode) != valid_modes.end()) {
valid_Control_Mode = true;
Expand Down Expand Up @@ -275,7 +227,7 @@ void mainloop(){
Command_to_pub.Reference_State.Move_frame = Move_frame;
// yaw_rate control
// Command_to_pub.Reference_State.Yaw_Rate_Mode = 1;
generate_com(Move_mode, state_desired);
// generate_com(Move_mode, state_desired);

break;
}
Expand Down Expand Up @@ -314,8 +266,7 @@ void mainloop(){
Command_to_pub.header.stamp = ros::Time::now();
easondrone_ctrl_pub_.publish(Command_to_pub);

string msg = "-------- Mission Received --------\n";
cout_color(msg, GREEN_COLOR);
cout_color("Command publish success!", GREEN_COLOR);
}
}

Expand Down
4 changes: 1 addition & 3 deletions package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>px4ctrl</name>
<version>3.6.3</version>
<version>4.0.0</version>
<description>
A ROS package as a bridge to estimate and control drone with MAVROS
</description>
Expand All @@ -18,8 +18,6 @@
<depend>std_msgs</depend>
<depend>message_filters</depend>

<build_depend>message_generation</build_depend>

<exec_depend>message_runtime</exec_depend>

<export>
Expand Down
47 changes: 29 additions & 18 deletions src/px4ctrl_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,10 +129,10 @@ int main(int argc, char **argv){
arm_cmd.request.value = true;

pos_setpoint.coordinate_frame = 1;
pos_setpoint.position.x = 0;
pos_setpoint.position.y = 0;
pos_setpoint.position.z = 0;
pos_setpoint.yaw = 0;
pos_setpoint.position.x = odom_pos_(0);
pos_setpoint.position.y = odom_pos_(1);
pos_setpoint.position.z = odom_pos_(2);
pos_setpoint.yaw = odom_yaw_;

if (arming_client.call(arm_cmd) &&
arm_cmd.response.success) {
Expand All @@ -156,10 +156,10 @@ int main(int argc, char **argv){
offb_set_mode.request.custom_mode = "OFFBOARD";

pos_setpoint.coordinate_frame = 1;
pos_setpoint.position.x = 0;
pos_setpoint.position.y = 0;
pos_setpoint.position.z = 0;
pos_setpoint.yaw = 0;
pos_setpoint.position.x = odom_pos_(0);
pos_setpoint.position.y = odom_pos_(1);
pos_setpoint.position.z = odom_pos_(2);
pos_setpoint.yaw = odom_yaw_;

if (set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent) {
Expand Down Expand Up @@ -208,11 +208,6 @@ int main(int argc, char **argv){
case easondrone_msgs::ControlCommand::Move:{
cout << "FSM_EXEC_STATE: Move" << endl;

//对于机体系的指令,需要转换成ENU坐标系执行,且同一ID号内,只执行一次.
if(Command_Now.Reference_State.Move_frame != easondrone_msgs::PositionReference::ENU_FRAME){
Body_to_ENU();
}

break;
}

Expand All @@ -223,6 +218,12 @@ int main(int argc, char **argv){
if (current_state.mode != "AUTO.LOITER") {
offb_set_mode.request.custom_mode = "AUTO.LOITER";

pos_setpoint.coordinate_frame = 1;
pos_setpoint.position.x = odom_pos_(0);
pos_setpoint.position.y = odom_pos_(1);
pos_setpoint.position.z = odom_pos_(2);
pos_setpoint.yaw = odom_yaw_;

if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
cout_color("AUTO.LOITER response sent", YELLOW_COLOR);
}
Expand All @@ -235,8 +236,6 @@ int main(int argc, char **argv){
}

break;

break;
}

// 【Land】 降落。当前位置原地降落，降落后会自动上锁，且切换为mannual模式
Expand All @@ -247,10 +246,10 @@ int main(int argc, char **argv){
offb_set_mode.request.custom_mode = "AUTO.LAND";

pos_setpoint.coordinate_frame = 1;
pos_setpoint.position.x = 0;
pos_setpoint.position.y = 0;
pos_setpoint.position.x = odom_pos_(0);
pos_setpoint.position.y = odom_pos_(1);
pos_setpoint.position.z = 0;
pos_setpoint.yaw = 0;
pos_setpoint.yaw = odom_yaw_;

if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
cout_color("AUTO.LAND response sent", YELLOW_COLOR);
Expand All @@ -272,6 +271,12 @@ int main(int argc, char **argv){
if (current_state.mode != "MANUAL") {
offb_set_mode.request.custom_mode = "MANUAL";

pos_setpoint.coordinate_frame = 1;
pos_setpoint.position.x = odom_pos_(0);
pos_setpoint.position.y = odom_pos_(1);
pos_setpoint.position.z = odom_pos_(2);
pos_setpoint.yaw = odom_yaw_;

if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
cout_color("MANUAL response sent", YELLOW_COLOR);
}
Expand All @@ -293,6 +298,12 @@ int main(int argc, char **argv){
if (current_state.armed) {
arm_cmd.request.value = false;

pos_setpoint.coordinate_frame = 1;
pos_setpoint.position.x = odom_pos_(0);
pos_setpoint.position.y = odom_pos_(1);
pos_setpoint.position.z = 0;
pos_setpoint.yaw = odom_yaw_;

if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
cout_color("Disarm response success", YELLOW_COLOR);
}
Expand Down
22 changes: 9 additions & 13 deletions src/px4ctrl_terminal.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,12 @@
/***************************************************************************************************************************
* terminal_control.cpp
*
* Author: Qyp
* Edited by: Eason Hua
* Update Time: 2024.08.06
*
* Introduction: test function for sending ControlCommand.msg
***************************************************************************************************************************/
/*
px4ctrl_terminal.cpp
Author: Eason Hua
Update Time: 2024.08.07
Introduction: test function for sending ControlCommand.msg
*/

#include "px4ctrl_terminal.h"


//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>　主函数　<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv){
ros::init(argc, argv, "px4ctrl_terminal");
Expand All @@ -21,9 +17,9 @@ int main(int argc, char **argv){
("/easondrone/control_command", 10);

// 初始化命令
Command_to_pub.Mode = easondrone_msgs::ControlCommand::Move;
Command_to_pub.Reference_State.Move_mode = easondrone_msgs::PositionReference::XYZ_POS;
Command_to_pub.Reference_State.Move_frame = easondrone_msgs::PositionReference::ENU_FRAME;
Command_to_pub.Mode = easondrone_msgs::ControlCommand::Hold;
Command_to_pub.Reference_State.Move_mode = easondrone_msgs::PositionReference::XYZ_POS;
Command_to_pub.Reference_State.Move_frame = easondrone_msgs::PositionReference::ENU_FRAME;

//固定的浮点显示
cout.setf(ios::fixed);
Expand Down

0 comments on commit 2096f79

Please sign in to comment.