From 5ec27a07d16c70812fd501fcd2e3bda5255f9bd9 Mon Sep 17 00:00:00 2001 From: Tonser Date: Mon, 9 Nov 2020 04:31:13 +0800 Subject: [PATCH] added OffboardManager --- src/px4_swarm_control/CMakeLists.txt | 9 +- .../src/OffboardManager/OffboardManager.cpp | 739 ++++++++++++++++++ .../src/OffboardManager/OffboardManager.h | 275 +++++++ .../src/test/test_offboard_manager.cpp | 9 + 4 files changed, 1029 insertions(+), 3 deletions(-) create mode 100644 src/px4_swarm_control/src/OffboardManager/OffboardManager.cpp create mode 100644 src/px4_swarm_control/src/OffboardManager/OffboardManager.h create mode 100644 src/px4_swarm_control/src/test/test_offboard_manager.cpp diff --git a/src/px4_swarm_control/CMakeLists.txt b/src/px4_swarm_control/CMakeLists.txt index a1f3b16..338e76c 100644 --- a/src/px4_swarm_control/CMakeLists.txt +++ b/src/px4_swarm_control/CMakeLists.txt @@ -42,12 +42,15 @@ add_executable(uwb_ros_node src/UWBRosNode.cpp include/MsgLink/UWBManager.cpp ) + add_dependencies(uwb_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(uwb_ros_node ${catkin_LIBRARIES} ) -add_executable(lcm_ros_node - src/LcmRosNode.cpp - ) +add_executable(test_offboard_manager src/test/test_offboard_manager.cpp src/OffboardManager/OffboardManager.cpp src/OffboardManager/OffboardManager.h ) +add_dependencies(test_offboard_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_offboard_manager ${catkin_LIBRARIES} ) + +add_executable(lcm_ros_node src/LcmRosNode.cpp ) add_dependencies(lcm_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(lcm_ros_node ${catkin_LIBRARIES} lcm) diff --git a/src/px4_swarm_control/src/OffboardManager/OffboardManager.cpp b/src/px4_swarm_control/src/OffboardManager/OffboardManager.cpp new file mode 100644 index 0000000..0dc8be9 --- /dev/null +++ b/src/px4_swarm_control/src/OffboardManager/OffboardManager.cpp @@ -0,0 +1,739 @@ +// +// Created by sundx on 2020/11/9. +// + +#include "OffboardManager.h" +#include +#include "ros/ros.h" + +OffboardManager::OffboardManager(ros::NodeHandle &n) { + node_name = ros::this_node::getName(); + pkg_path = ros::package::getPath("px4_gcs"); + updateParams(n); + initState(); +} + +void +OffboardManager::initRosSubPubManager(ros::NodeHandle &n){ + sp_raw_local_pub = n.advertise( + mavros_id + "/setpoint_raw/local", 1); + current_drone_ready_pub = n.advertise( + mavros_id + "/ready", 1); + cmd_request = n.serviceClient(mavros_id + "/set_mode"); + arming_client = n.serviceClient(mavros_id + "/cmd/arming"); + local_pose_sub = n.subscribe( + mavros_id + "/local_position/pose", 1, &OffboardManager::chatterCallback_local_pose, this); + vision_pose_sub = n.subscribe( + mavros_id + "/vision_pose/pose", 1, &OffboardManager::chatterCallback_local_pose, this); + local_vel_sub = n.subscribe( + mavros_id + "/local_position/velocity", 1, &OffboardManager::chatterCallback_local_vel, this); + mode_sub = n.subscribe( + mavros_id + "/state", 1, &OffboardManager::chatterCallback_mode, this); + lan_sub = n.subscribe( + mavros_id + "/extended_state", 1, &OffboardManager::chatterCallback_extend_state, this); + swarm_state_sub = n.subscribe( + "/swarm/state", 1, &OffboardManager::swarm_state_cb, this); + swarm_ready_bool.resize(swarm_num); + swarm_ready_sub.resize(swarm_num); + for (int j = 0; j < swarm_num; ++j) { + swarm_ready_bool[j] = false; + string topic = "/uav" + to_string(j) + "/mavros/ready"; + if (swarm_num <= 1) { + topic = "/mavros/ready"; + } + swarm_ready_sub[j] = n.subscribe(topic, 1, + boost::bind(&OffboardManager::dronei_ready_cb, this, _1, j)); + } + current_drone_ready_msg.data = false; + ros::spinOnce(); +} + +void +OffboardManager::swarm_state_cb(const px4_gcs::swarm_state &msg){ + nodes = msg.nodes_id; + ready = msg.ready; + traj_ready = msg.traj_ready; + if(isMaster&&id!=msg.master_id){ + isMaster = false; + } + // swarm state + if(!msg.active.empty()){ + if(active.empty()){ + active = msg.active; + }else{ + if(active.size()==msg.active.size()){ + for (int j = 0; j < active.size(); ++j) { + if(active[j]!=msg.active[j]){ + swarm_state_changed = true; + break; + } + } + }else{ + swarm_state_changed = true; + } + } + + // master live + if(msg.active[msg.master_id]){ + isMaster = (id == msg.master_id); + }else{ + isMaster = true; + for (int j = 0; j < msg.active.size()&&j csvManager(readfilepath); + vector> trajectory; + csvManager.readCSV(trajectory); + if(trajectory[0][2]==trajectory.size()){ + ROS_INFO_STREAM("loaded trajectory successful !! size: "<< trajectory.size() << " X " << trajectory[0].size()); + } + + //>>>>>>>>>>>>>>>traj process<<<<<<<<<<<<<<< + ROS_INFO("Now process trajectory"); + TrajManager trajManager; + for (auto & j : trajectory) { + vector curTraj(3); + curTraj[0] = j[3]; + curTraj[1] = j[4]; + curTraj[2] = takeoff_height; + trajManager.traj.push_back(curTraj); + } + traj = trajManager.traj; + + //>>>>>>>>>>>>>>恢复状态准备起飞<<<<<<<<<<<<<<<<< + swarm_state_changed = false; + current_drone_ready_msg.data = true; + current_drone_ready_pub.publish(current_drone_ready_msg); + init_pose[0] = cur_pos[0]; + init_pose[1] = cur_pos[1]; + init_pose[2] = cur_pos[2]; + state.currentRunState = run_hold; + state.nextRunState = run_traj; + state.current_run_finished = false; + }else{ + ROS_INFO_STREAM_THROTTLE(1, "swarm state changed waiting traj ready!!!"); + current_drone_ready_msg.data = false; + current_drone_ready_pub.publish(current_drone_ready_msg); + moveToRelativeNow(0,0,0); + return; + } + } + + if (state.current_run_finished) { + state.current_run_finished = false; + state.currentRunState = state.nextRunState; + ROS_INFO_STREAM("ID: " << id << " " << " CURRENT RUN: " << printState(state.currentRunState)); + } + current_drone_ready_pub.publish(current_drone_ready_msg); // publish ready msg + switch (state.currentRunState) { + case run_takeoff: + if (use_takeoff) { + takeoff(takeoff_height); + } else { + take_off_finished = true; + } + state.current_run_finished = take_off_finished; + current_drone_ready_msg.data = take_off_finished; + current_drone_ready_pub.publish(current_drone_ready_msg); + state.nextRunState = run_hold; + break; + case run_hold : + if (!swarm_ready()) { + holdInitPos(takeoff_height); + record_pose(); + current_drone_ready_msg.data = take_off_finished; + current_drone_ready_pub.publish(current_drone_ready_msg); + } + state.current_run_finished = swarm_ready(); + state.nextRunState = run_traj; + break; + case run_traj: + if (!fly_traj) { + fly_rect(); + } + if (fly_traj && !traj_finished) { + fly_traj_base(init_pose[0], init_pose[1], takeoff_height); + } + state.current_run_finished = traj_finished; + state.nextRunState = run_hold_traj_end; + break; + + case run_hold_traj_end: + fly_traj_count_base(init_pose[0], init_pose[1], takeoff_height, (int) traj.size() - 1);// 飞向航迹末端点悬停 + if (cur_err_dist < 0.3) { + fly_finished = true; + } + state.current_run_finished = fly_finished; + state.nextRunState = use_land ? run_land : run_hold_traj_end; + break; + case run_land: + if (use_land) { + land(); + if (state.armState == disarmed) { + state.current_run_finished = true; + ROS_INFO("Land FINISHED!! set stabilized mode disarmed!!"); + break; + } + } else { + fly_traj_count_base(init_pose[0], init_pose[1], land_height, (int) traj.size() - 1);// 飞向航迹末端点悬停 + } + break; + default: + break; + } +} + + +void +OffboardManager::try_ready_takeoff() { + if (using_gazebo&&init_process_finished) { + for (int i = 0; i < 10; i++) + { + holdZeroPosition(); + } + setOffboard(); + setArmed(); + } +} + + +void +OffboardManager::fly_traj_count_base(float base_x, float base_y, float base_z, int _control_count) { + _target[0] = traj_scale_x * (traj[_control_count][0] - global_init_pose_x) + base_x ; + _target[1] = traj_scale_y * (traj[_control_count][1] - global_init_pose_y) + base_y ; + if (use_traj_z) _target[2] = traj_scale_z * traj[_control_count][2] + base_z; + else _target[2] = takeoff_height; + moveto(_target[0], _target[1], _target[2]); +} + + +void +OffboardManager::fly_traj_base(float base_x, float base_y, float base_z) { + ROS_INFO_STREAM("ID: " << id << " FLY_TRAJ......" << (float) control_count_time * 100 / traj.size() << " %"); + if (control_count_time < traj.size()) { + fly_traj_count_base(base_x, base_y, base_z, control_count_time); + if (traj_speed >= 0) { + if (traj_speed == 0)traj_speed = 1; + for (int j = 0; j < traj_speed; ++j) { + control_count_time++; + } + } else { + traj_speed_control++; + if (traj_speed_control + traj_speed >= 0) { + traj_speed_control = 1; + control_count_time++; + } + } + } else { + traj_finished = true; + control_count_time = 0; + ROS_INFO("TRAJ FINISHED!!"); + record_pose(); + } +} + + +void +OffboardManager::fly_rect() { + switch (move_stage) { + case 0: + if (control_count_time < control_time) { + moveto(-1 * rect_x / 2, -1 * rect_y / 2, takeoff_height); + control_count_time++; + } else { + record_pose(); + control_count_time = 0; + move_stage++; + ROS_INFO("Now fly traj 2"); + } + + break; + case 1: + if (control_count_time < control_time) { + moveto(-1 * rect_x / 2, rect_y / 2, takeoff_height); + control_count_time++; + } else { + record_pose(); + control_count_time = 0; + move_stage++; + ROS_INFO("Now fly traj 3"); + + } + break; + case 2: + if (control_count_time < control_time) { + moveto(rect_x / 2, rect_y / 2, takeoff_height); + control_count_time++; + } else { + record_pose(); + control_count_time = 0; + move_stage++; + ROS_INFO("Now fly traj 4"); + + } + break; + case 3: + if (control_count_time < control_time) { + moveto(rect_x / 2, -1 * rect_y / 2, takeoff_height); + control_count_time++; + } else { + record_pose(); + control_count_time = 0; + move_stage++; + ROS_INFO("Now fly back"); + } + break; + case 4: + if (control_count_time < control_time) { + moveto(init_pose[0], init_pose[1], takeoff_height); + control_count_time++; + } else { + record_pose(); + control_count_time = 0; + move_stage++; + ROS_INFO("TRAJ FINISHED!!"); + } + break; + default: + fly_finished = true; + traj_finished = true; + break; + } +} + +bool +OffboardManager::readCSV(const string &filename, vector> &traj) { + ifstream traj_file(filename); + if (traj_file.fail()) { + ROS_INFO_STREAM("ID: " << id << " can not find this file" << filename); + return false; + } + + string line; + while (getline(traj_file, line)) { + istringstream in_line(line); + vector dataInLine; + string word; + while (getline(in_line, word, ',')) { + dataInLine.push_back(strtof(word.c_str(), nullptr)); + } + if (!dataInLine.empty()) traj.push_back(dataInLine); + } + + if (!traj.empty()) { + return true; + } else { + ROS_INFO_STREAM("ID: " << id << " Found nothing in the file" << filename); + return false; + } +} + +void +OffboardManager::updateParams(ros::NodeHandle &nh) { + id = readParam(nh, "id"); + swarm_num = readParam(nh, "swarm_drone_num"); + rate = readParam(nh, "rate"); + traj_speed = readParam(nh, "traj_speed"); + resample_num = readParam(nh, "resample_num"); + global_init_pose_y = readParam(nh, "global_init_pose_y"); + global_init_pose_x = readParam(nh, "global_init_pose_x"); + takeoff_height = readParam(nh, "takeoff_height"); + land_height = readParam(nh, "land_height"); + land_speed = readParam(nh, "land_speed"); + offset_y = readParam(nh, "offset_y"); + mavros_id = readParam(nh, "mavros_id"); + readfilepath = readParam(nh, "readfilepath"); + using_position_methord = readParam(nh, "using_position_methord"); + using_gazebo = readParam(nh, "using_gazebo"); + fly_traj = readParam(nh, "fly_traj"); + use_resample = readParam(nh, "use_resample"); + use_traj_z = readParam(nh, "use_traj_z"); + use_takeoff = readParam(nh, "use_takeoff"); + use_land = readParam(nh, "use_land"); + traj_scale = readParam(nh, "traj_scale"); + use_local_head_target = readParam(nh, "use_local_head_target"); + traj_scale_x = readParam(nh, "traj_scale_x"); + traj_scale_y = readParam(nh, "traj_scale_y"); + traj_scale_z = readParam(nh, "traj_scale_z"); + ROS_INFO_STREAM("ID: " << id << " mavros_id: " + mavros_id); +} + +void +OffboardManager::chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg) { + cur_pos[0] = msg.pose.position.x; + cur_pos[1] = msg.pose.position.y; + cur_pos[2] = msg.pose.position.z; + tf::quaternionMsgToTF(msg.pose.orientation, cur_att); + double roll = 0; double pitch = 0; double yaw = 0; + tf::Matrix3x3(cur_att).getRPY(roll, pitch, yaw); + cur_angle[0] = (float)roll; + cur_angle[1] = (float)pitch; + cur_angle[2] = (float)yaw; + // ROS_INFO_STREAM("eular angle: RPY"<data; +} + +void +OffboardManager::chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg) { + cur_vel[0] = msg.twist.linear.x; + cur_vel[1] = msg.twist.linear.y; + cur_vel[2] = msg.twist.linear.z; + yaw_rate = msg.twist.angular.z; +} + +void +OffboardManager::chatterCallback_mode(const mavros_msgs::State &msg) { + if (msg.armed == 1) { + state.armState = armed; + } else { + state.armState = disarmed; + } + if (msg.mode == "OFFBOARD") { + state.flyMode = offboard; + } else if (msg.mode == "STABILIZED") { + state.flyMode = stabilized; + } else if (msg.mode == "ALTCTL") { + state.flyMode = altctl; + } else if (msg.mode == "POSCTL") { + state.flyMode = posctl; + } else{ + state.flyMode = stabilized; + } +} + + +void +OffboardManager::takeoff(float height) { + if (!take_off_finished) { + state.airState = takeoffing; + if (fabs(cur_pos[2] - height) < 0.1) { + take_off_finished = true; + return; + } + moveto(init_pose[0], init_pose[1], height); + } +} + +void +OffboardManager::holdInitPos(float height) { + moveto(init_pose[0], init_pose[1], height); +} + +void +OffboardManager::land(float target_x, float target_y) { + if (state.airState == landed) { + if (state.armState == armed) { + setStabilized(); + setDisarmed(); + } + } + if (!land_finished) { + ROS_INFO("landing now"); + state.airState = landing; + // + if ((cur_pos[2] > -5 || state.airState == landing) && (state.airState != landed)) { + moveto(target_x, target_y, cur_pos[2] - land_speed); + } else { + land_finished = true; + state.airState = landed; + } + } +} + +void +OffboardManager::land() { + if (state.airState == landed) { + if (state.armState == armed) { + setDisarmed(); + usleep(100000);//100ms + } + } + if (!land_finished) { + ROS_INFO("landing now"); + state.airState = landing; + // + if ((cur_pos[2] > -5 || state.airState == landing) && (state.airState != landed)) { + moveToRelativeNow(0,0,-100); + } else { + land_finished = true; + state.airState = landed; + } + }else{ + setDisarmed(); + usleep(100000);//100ms + } +} + +void +OffboardManager::holdZeroPosition() { + moveToRelativeNow(0, 0, 0); +} + +void +OffboardManager::moveto(float target_x, float target_y, float target_z) { + if (use_local_head_target) { + float tx = 0.0; + float ty = 0.0; + tx = target_x * cos(init_yaw) - target_y * sin(init_yaw); + // tx = target_x * cos(init_yaw) + target_y * sin(init_yaw); + ty = target_x * sin(init_yaw) + target_y * cos(init_yaw); + // ty = target_x * sin(init_yaw) - target_y * cos(init_yaw); + target_x = tx; + target_y = ty; + } + sp_pos.header.frame_id = "map"; + // TODO: 测试本处的修改是发送什么坐标系下面的指令 + sp_pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED; + // sp_pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED; + if (using_control_mode == 0) { + sp_pos.header.stamp = ros::Time::now(); + sp_pos.position.x = target_x; + sp_pos.position.y = target_y; + sp_pos.position.z = target_z; + sp_pos.type_mask = 0b110111111000; // 100 111 111 000 xyz + yaw + sp_raw_local_pub.publish(sp_pos); + } else if (using_control_mode == 1) { + sp_pos.header.stamp = ros::Time::now(); + sp_pos.velocity.x = target_x; + sp_pos.velocity.y = target_y; + sp_pos.velocity.z = target_z; + sp_pos.type_mask = 0b110111000111; // 100 111 111 000 xyz + yaw + sp_raw_local_pub.publish(sp_pos); + } + cur_err_dist = getDistanceFrom(target_x, target_y, target_z); +} + +void +OffboardManager::moveto(float target_x, float target_y, float target_z, int i, int total) { + i = i + 1; + float index = (float) (i) / (float) (total); + moveto(cur_pos[0] + index * (target_x - cur_pos[0]), + cur_pos[1] + index * (target_y - cur_pos[1]), + cur_pos[2] + index * (target_z - cur_pos[2]) + ); +} + +void +OffboardManager::moveToRelativeNow(float dx, float dy, float dz) { + moveto(cur_pos[0] + dx,cur_pos[1] + dy,cur_pos[2] + dz); +} + +void +OffboardManager::load_traj() { + readfilepath = pkg_path + readfilepath; + readCSV(readfilepath, traj); + ROS_INFO_STREAM("ID: " << id << " load traj success " << readfilepath); +} + +void +OffboardManager::loadTrajFromVector(vector> &_traj){ + traj.resize(_traj.size()); + traj.assign(_traj.begin(),_traj.end()); + ROS_INFO("Load traj from out vector OK!!!"); +} + +void +OffboardManager::mainLoop(){ + ros::Rate loop_rate(rate); + while (ros::ok()) { + ros::spinOnce(); //获取回调消息 + + if (!init_process_finished||state.flyMode != offboard) { + init_position(loop_rate,1); + holdZeroPosition(); + try_ready_takeoff(); + } + if (state.flyMode != offboard) { + holdZeroPosition(); + try_ready_takeoff(); + } else { + process_run(); + } + loop_rate.sleep(); + } +} + +void +OffboardManager::initState() { + state.currentRunState = run_takeoff; + state.current_run_finished = false; + switch (using_position_methord) { + case 1: + state.positionState = using_vicon; + break; + case 2: + state.positionState = using_gps; + break; + case 3: + state.positionState = using_att; + break; + default: + break; + } +} + +void +OffboardManager::setOffboard() { + mode_cmd.request.custom_mode = "OFFBOARD"; + cmd_request.call(mode_cmd); + ROS_INFO("setting OFFBOARD "); +} + +void +OffboardManager::setMannual() { + mode_cmd.request.custom_mode = "MANUAL"; + cmd_request.call(mode_cmd); + ROS_INFO("setting MANNUAL "); +} + +void +OffboardManager::setPosctl() { + mode_cmd.request.custom_mode = "POSCTL"; + cmd_request.call(mode_cmd); + ROS_INFO("setting POSCTL "); +} + +void +OffboardManager::setAltctl() { + mode_cmd.request.custom_mode = "ALTCTL"; + cmd_request.call(mode_cmd); + ROS_INFO("setting ALTCTL "); +} + +void +OffboardManager::setStabilized() { + mode_cmd.request.custom_mode = "STABILIZED"; + cmd_request.call(mode_cmd); + ROS_INFO("setting STABILIZED "); +} + +void +OffboardManager::setArmed() { + arm_cmd.request.value = true; + arming_client.call(arm_cmd); + ROS_INFO("setting Armed "); +} + +void +OffboardManager::setDisarmed() { + arm_cmd.request.value = false; + arming_client.call(arm_cmd); + ROS_INFO("setting Disarmed "); +} + +void +OffboardManager::record_pose() { + record_pos[0] = cur_pos[0]; + record_pos[1] = cur_pos[1]; + record_pos[2] = cur_pos[2]; +} + +void +OffboardManager::chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg) { + if (msg.landed_state == 1) { + state.airState = landed; + } else { + state.airState = in_air; + } +} + +inline float +OffboardManager::getDistanceFrom(float x, float y, float z) { + return sqrt((cur_pos[0] - x) * (cur_pos[0] - x) + + (cur_pos[1] - y) * (cur_pos[1] - y) + + (cur_pos[2] - z) * (cur_pos[2] - z)); +} + +bool +OffboardManager::swarm_ready() { + bool res = true; + for (int j = 0; j < swarm_num; ++j) { + res = swarm_ready_bool[j] && res; + if(!res) return res; + } + ROS_INFO_STREAM("ID: " << id << " swarm ready: " << res); + for(auto a :ready){ + res = a && res; + if(!res) return res; + } + return res; +} + +void +OffboardManager::init_position(ros::Rate &loop_rate, int average_num) { + ros::spinOnce(); //获取回调消息 + init_pose[0] = 0.0; + init_pose[1] = 0.0; + init_pose[2] = 0.0; + for (int j = 0; j < average_num; ++j) { + if(fabs(cur_pos[2])<0.000001) { + ROS_INFO("cur_pos worng!!::%2.4f",cur_pos[2]); + return; + }; // remove unstable data; + holdZeroPosition(); + init_pose[0] += cur_pos[0]; + init_pose[1] += cur_pos[1]; + init_pose[2] += cur_pos[2]; + init_yaw += cur_angle[2]; + loop_rate.sleep(); //控制帧率 + ros::spinOnce(); //获取回调消息 + } + init_pose[0] /= (float)average_num; + init_pose[1] /= (float)average_num; + init_pose[2] /= (float)average_num; + init_yaw /= (float)average_num; + init_process_finished = true; + ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init position " << init_pose[0] << " " << init_pose[1] << " " << init_pose[2] << " "); + ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init yaw " << init_yaw); + ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init position finished READY TAKEOFF!!!!"); +} + + diff --git a/src/px4_swarm_control/src/OffboardManager/OffboardManager.h b/src/px4_swarm_control/src/OffboardManager/OffboardManager.h new file mode 100644 index 0000000..0879730 --- /dev/null +++ b/src/px4_swarm_control/src/OffboardManager/OffboardManager.h @@ -0,0 +1,275 @@ +// +// Created by sundx on 2020/11/9. +// + +#ifndef SRC_OFFBOARDMANAGER_H +#define SRC_OFFBOARDMANAGER_H + + +#include "ros/ros.h" +#include "serial/serial.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf/transform_datatypes.h" +#include +#include "px4_gcs/swarm_state.h" +#include "Utils/TrajManager.h" +#include "Utils/CSVManager.h" +#include "Utils/RosUtils.h" + +using namespace std; + +#define PI 3.141593 + + +enum air_state { + in_air, + landing, + landed, + takeoffing, + waitting_takeoff +}; + +enum arm_state { + armed, + disarmed +}; + +enum fly_mode { + offboard, + manual, + altctl, + posctl, + stabilized +}; + +enum control_methord { + pos, + vel, + att +}; + +enum position_state { + using_vicon, + using_gps, + using_att, +}; + +enum run_state { + run_takeoff, + run_traj, + run_hold_traj_end, + run_hold, + run_land +}; + +struct current_state { + arm_state armState; + air_state airState; + control_methord controlMethord; + position_state positionState; + fly_mode flyMode; + run_state currentRunState; + run_state nextRunState; + bool current_run_finished; +}; + + +class OffboardManager { +public: + string node_name; + string mavros_id; + string pkg_path; + string local_pose_topic_name; + string publish_pose_topic_name; + + mavros_msgs::PositionTarget sp_pos; + current_state state{}; + tf::Quaternion cur_att; + + ros::Subscriber vision_pose_sub; + ros::Subscriber local_vel_sub; + ros::Subscriber mode_sub; + ros::Subscriber lan_sub; + ros::Subscriber swarm_state_sub; + vector swarm_ready_sub; + vector swarm_ready_bool; + ros::Publisher sp_raw_local_pub; + ros::Publisher current_drone_ready_pub; + ros::ServiceClient cmd_request; + mavros_msgs::SetMode mode_cmd; + mavros_msgs::CommandBool arm_cmd; + mavros_msgs::ExtendedState land_state; + std_msgs::Bool current_drone_ready_msg; + ros::ServiceClient arming_client; + ros::Subscriber local_pose_sub; + + float cur_pos[3] = {0.0, 0.0, 0.0}; + float record_pos[3] = {0.0, 0.0, 0.0}; + float cur_vel[3] = {0.0, 0.0, 0.0}; + float cur_angle[3] = {0.0, 0.0, 0.0}; // roll p + float init_pose[3] = {0.0, 0.0, 0.0}; + float sp_rpyt[4] = {0.0, 0.0, 0.0, 0.0}; + float yaw_rate = 0; + float takeoff_height = 0; + float land_height = 0; + float global_init_pose_x = 0; + float global_init_pose_y = 0; + float land_speed = 0.3; + float rect_x = 1; //矩形x边长 + float rect_y = 1; //矩形y边长 + float traj_scale = 1; + float traj_scale_x = 1; + float traj_scale_y = 1; + float traj_scale_z = 1; + float cur_yaw = 0.0; + float init_yaw = 0.0; + float target_height = 0.0; + float _target[3] = {0.0, 0.0, 0.0}; + float offset_z = 5.0; + float offset_y = 0.0; + float cur_err_dist = 0.0; + + bool isMaster = false; + bool traj_ready = false; + bool swarm_state_changed = false; + bool init_process_finished = false; + bool take_off_finished = false; + bool land_finished = false; + bool record_data = false; + bool fly_traj = true; + bool traj_finished = false; + bool fly_finished = false; + bool using_gazebo = false; + bool use_traj_z = false; + bool use_resample = false; + bool use_takeoff = true; + bool use_land = true; + bool use_local_head_target = false; + + int use_enu = 1;//int 1 use_enu 0 use ned + int re_pub_pos = 0;//int 1 use_enu 0 use ned + int using_position_methord = 0;//int 1 using_vicon 2 using_gps 3 using_att + int using_control_mode = 0; // 0 position 1 vel 2 att + int id = 0; + int swarm_num = 1; + int rate = 30; + int move_stage = 0; + int control_count_time = 0; + int control_time = 60; + int traj_speed = 1; + int traj_speed_control = 1; + int resample_num = 0; + + vector nodes; + vector active; + vector ready; + + OffboardManager() = default; + + OffboardManager(ros::NodeHandle &n); + + void initRosSubPubManager(ros::NodeHandle &n); + + bool readCSV(const string &filename, vector> &traj); + + void updateParams(ros::NodeHandle &nh); + + void chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg); + + void chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg); + + void chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg); + + void chatterCallback_mode(const mavros_msgs::State &msg); + + void takeoff(float height); + + void holdInitPos(float height); + + void land(float target_x, float target_y); + + void land(); + + void holdZeroPosition(); + + void initState(); + + void load_traj(); + + void loadTrajFromVector(vector> &traj); + + void moveToRelativeNow(float dx, float dy, float dz); + + void moveto(float target_x, float target_y, float target_z); + + void moveto(float target_x, float target_y, float target_z, int i, int total); + + inline float getDistanceFrom(float x, float y, float z); + + void fly_traj_count_base(float base_x, float base_y, float base_z, int _control_count); + + void fly_traj_base(float base_x, float base_y, float base_z); + + // 把traj的轨迹点数由原来的size个减小为num个 num应该和size 差不多大 + void resample(int num); + + void fly_rect(); + + void process_run(); + + void try_ready_takeoff(); + + void dronei_ready_cb(const std_msgs::Bool::ConstPtr &msg, int &i); + + void rotate_yaw(float yaw); + + void setDisarmed(); + + void setArmed(); + + void setAltctl(); + + void setStabilized(); + + void setPosctl(); + + void setOffboard(); + + void record_pose(); + + bool swarm_ready(); + + void setMannual(); + + string printState(run_state state); + + template + T readParam(ros::NodeHandle &n, std::string name); + + void init_position(ros::Rate &loop_rate, int average_num); + + void mainLoop(); + +private: + + void swarm_state_cb(const px4_gcs::swarm_state &msg); +}; + + + + + +#endif //SRC_OFFBOARDMANAGER_H diff --git a/src/px4_swarm_control/src/test/test_offboard_manager.cpp b/src/px4_swarm_control/src/test/test_offboard_manager.cpp new file mode 100644 index 0000000..673fd4a --- /dev/null +++ b/src/px4_swarm_control/src/test/test_offboard_manager.cpp @@ -0,0 +1,9 @@ +// +// Created by sundx on 2020/11/9. +// + +#include + +int main(){ + +} -- Gitee