diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e56969..4b0119a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 @@ -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 @@ -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) diff --git a/include/px4ctrl_terminal.h b/include/px4ctrl_terminal.h index af212c7..5dc4f85 100644 --- a/include/px4ctrl_terminal.h +++ b/include/px4ctrl_terminal.h @@ -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." <> Control_Mode) { if (valid_modes.find(Control_Mode) != valid_modes.end()) { valid_Control_Mode = true; @@ -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; } @@ -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); } } diff --git a/package.xml b/package.xml index 9ff354f..a03dd69 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ px4ctrl - 3.6.3 + 4.0.0 A ROS package as a bridge to estimate and control drone with MAVROS @@ -18,8 +18,6 @@ std_msgs message_filters - message_generation - message_runtime diff --git a/src/px4ctrl_node.cpp b/src/px4ctrl_node.cpp index 59f9e28..e98aabd 100755 --- a/src/px4ctrl_node.cpp +++ b/src/px4ctrl_node.cpp @@ -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) { @@ -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) { @@ -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; } @@ -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); } @@ -235,8 +236,6 @@ int main(int argc, char **argv){ } break; - - break; } // 【Land】 降落。当前位置原地降落，降落后会自动上锁，且切换为mannual模式 @@ -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); @@ -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); } @@ -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); } diff --git a/src/px4ctrl_terminal.cpp b/src/px4ctrl_terminal.cpp index e1a7619..b51a677 100755 --- a/src/px4ctrl_terminal.cpp +++ b/src/px4ctrl_terminal.cpp @@ -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"); @@ -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);