From 335460b6df7b38065400cb86cca9542d525fd81d Mon Sep 17 00:00:00 2001 From: UnHappySquid Date: Fri, 24 Jan 2025 09:21:14 -0500 Subject: [PATCH 1/9] added something to the gitignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 05b3344..ff55b60 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,9 @@ # Created by https://www.toptal.com/developers/gitignore/api/ros2,visualstudiocode,vim,python,c++,cmake,opencv # Edit at https://www.toptal.com/developers/gitignore?templates=ros2,visualstudiocode,vim,python,c++,cmake,opencv +### OctoMap ### +*.bt + ### C++ ### # Prerequisites *.d From afbb3b7fe40400b02c807121633324ddaa393e4f Mon Sep 17 00:00:00 2001 From: UnHappySquid Date: Fri, 24 Jan 2025 18:06:42 -0500 Subject: [PATCH 2/9] Started actually refactoring, QC mode most likely done --- drone_test.sh | 4 +- drone_ws/src/custom_msgs/msg/DroneState.msg | 2 + .../src/px4_controller/src/drone_node.cpp | 380 ++++++++++-------- 3 files changed, 227 insertions(+), 159 deletions(-) diff --git a/drone_test.sh b/drone_test.sh index a5b4238..2f19c8b 100644 --- a/drone_test.sh +++ b/drone_test.sh @@ -4,9 +4,9 @@ ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 2, ta ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 10., e: 10., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" -ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: -200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" -ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" diff --git a/drone_ws/src/custom_msgs/msg/DroneState.msg b/drone_ws/src/custom_msgs/msg/DroneState.msg index 6c681e0..b330ce0 100644 --- a/drone_ws/src/custom_msgs/msg/DroneState.msg +++ b/drone_ws/src/custom_msgs/msg/DroneState.msg @@ -20,6 +20,8 @@ uint32 ACRO=12 uint32 STABILIZED=13 uint32 RATTITUDE=14 +string current_command + uint32 flight_mode bool in_air diff --git a/drone_ws/src/px4_controller/src/drone_node.cpp b/drone_ws/src/px4_controller/src/drone_node.cpp index d1e167e..21ccaf7 100644 --- a/drone_ws/src/px4_controller/src/drone_node.cpp +++ b/drone_ws/src/px4_controller/src/drone_node.cpp @@ -25,9 +25,23 @@ using namespace std::chrono_literals; +template +std::string string_format(const std::string &format, Args... args) { + int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) + + 1; // Extra space for '\0' + if (size_s <= 0) { + throw std::runtime_error("Error during formatting."); + } + auto size = static_cast(size_s); + std::unique_ptr buf(new char[size]); + std::snprintf(buf.get(), size, format.c_str(), args...); + return std::string(buf.get(), + buf.get() + size - 1); // We don't want the '\0' inside +} + class OffboardNode : public rclcpp::Node { public: - OffboardNode() : Node("offboard_controller") { + OffboardNode() : Node("offboard_controller"), qc_tolerance(1.) { // tf2 stuff tf_world_broadcaster = @@ -40,6 +54,8 @@ class OffboardNode : public rclcpp::Node { // MAVSDK stuff mavsdk = std::make_unique(mavsdk::Mavsdk::Configuration{ mavsdk::Mavsdk::ComponentType::GroundStation}); + + // TODO: Modify the connection url when connected to the PixHawk mavsdk::ConnectionResult con_res = mavsdk->add_any_connection("udp://:14540"); @@ -59,8 +75,9 @@ class OffboardNode : public rclcpp::Node { mavsdk_telem = std::make_unique(sys.value()); mavsdk_action = std::make_unique(sys.value()); mavsdk_offboard = std::make_unique(sys.value()); - const mavsdk::Offboard::VelocityNedYaw stay{}; - mavsdk_offboard->set_velocity_ned(stay); + // const mavsdk::Offboard::VelocityNedYaw stay{}; + mavsdk::Offboard::PositionNedYaw stay_pos{}; + mavsdk_offboard->set_position_ned(stay_pos); this->setup_monitoring(); @@ -74,8 +91,8 @@ class OffboardNode : public rclcpp::Node { std::placeholders::_1)); pop_sub = this->create_subscription( - "pop_command", 10, - std::bind(&OffboardNode::dequeue_topic_callback, this, + "cancel_command", 10, + std::bind(&OffboardNode::cancel_command_topic_callback, this, std::placeholders::_1)); this->timer_ = this->create_wall_timer( @@ -88,33 +105,20 @@ class OffboardNode : public rclcpp::Node { private: // This only makes sense in the context of the action queue struct Command { - enum FlightMode { - QC = custom_msgs::msg::DroneState::MC, - FW = custom_msgs::msg::DroneState::FW - }; - - struct GoToTuple { - GoToTuple(double target_n, double target_e, double target_d, - double target_vn, double target_ve, double target_vd) { - n = target_n; - e = target_e; - d = target_d; - vn = target_vn; - ve = target_ve; - vd = target_vd; - } - double n, e, d, vn, ve, vd; - }; - // Returns true if its done, false otherwise + // both perform tick and cancel_command return true if they are done, false + // otherwise virtual bool perform_tick(OffboardNode &on) = 0; - virtual bool is_goto() { return false; } - virtual std::optional get_goto_tuple() { return std::nullopt; } - virtual std::optional get_desired_fm() { return std::nullopt; } + virtual bool cancel_command(OffboardNode &on) { + RCLCPP_INFO(on.get_logger(), "This command is not cancelable"); + return perform_tick(on); + } + virtual std::string get_description() = 0; + virtual ~Command() {} }; struct TakeOff : Command { - TakeOff(double height) { this->target_height = height; } + TakeOff(double height) : target_height(height) {} virtual bool perform_tick(OffboardNode &node) override { if (node.state.in_air && @@ -134,6 +138,10 @@ class OffboardNode : public rclcpp::Node { return false; } + virtual std::string get_description() override { + return string_format("Taking off: target height %lf", target_height); + } + ~TakeOff() {} private: @@ -154,141 +162,189 @@ class OffboardNode : public rclcpp::Node { return false; } - ~Land() {} - }; - - struct GoTo : Command { - GoTo(double n, double e, double d, double vn, double ve, double vd, - FlightMode fm) { - - target_n = n; - target_e = e; - target_d = d; - - target_vn = vn; - target_ve = ve; - target_vd = vd; - - desired_mode = fm; + virtual std::string get_description() override { + return string_format("Landing"); } - virtual bool is_goto() override { return true; } + ~Land() {} + }; - virtual std::optional get_goto_tuple() override { - return std::optional(GoToTuple( - target_n, target_e, target_d, target_vn, target_ve, target_vd)); - }; + struct GoToQC : Command { + GoToQC(double n, double e, double d) + : target_n(n), target_e(e), target_d(d) {} - virtual std::optional get_desired_fm() override { - return std::optional(desired_mode); - } - - // TODO: Finish implementing velocity and yaw control virtual bool perform_tick(OffboardNode &node) override { - if (!start_pos_recorded) { - start_pos_recorded = true; - start_n = node.state.n; - start_e = node.state.e; - start_d = node.state.d; - } - - // switch to offboard if not already - if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { - node.mavsdk_offboard->start(); - } - - // send transition command if need to and if not sent already - if (node.state.vtol_state != desired_mode && !transtion_order_sent) { - mavsdk::Action::Result res; - if (desired_mode == FlightMode::QC) { - res = node.mavsdk_action->transition_to_multicopter(); - } else { - res = node.mavsdk_action->transition_to_fixedwing(); - } - if (res == mavsdk::Action::Result::Success) - transtion_order_sent = true; - return false; - } - double dist_from_target = sqrt(pow((node.state.n - target_n), 2) + pow((node.state.e - target_e), 2) + pow((node.state.d - target_d), 2)); - // if above tolerance and command to go somewhere wasnt sent already - if (((desired_mode == FlightMode::QC && dist_from_target >= 1.) || - (desired_mode == FlightMode::FW && dist_from_target >= 10.)) && - !goto_order_sent) { + if (dist_from_target >= node.qc_tolerance && !goto_command_sent) { mavsdk::Offboard::PositionNedYaw pos; pos.north_m = target_n; pos.east_m = target_e; pos.down_m = target_d; - pos.yaw_deg = NAN; - // If there is a next goto action - if (node.command_deque.size() >= 1 && - node.command_deque[0]->is_goto()) { - - // if next goto action is in FW and we're in QC: setup yaw and - // actually over shoot the point so you dont slow down before trying to transition - if (desired_mode == FlightMode::QC && - node.command_deque[0]->get_desired_fm().value() == - FlightMode::FW) { - - double overshoot_scale = 1.1; - pos.north_m = start_n + (target_n - start_n) * overshoot_scale; - pos.east_m = start_e + (target_e - start_e) * overshoot_scale; - - double next_n, next_e, d_n, d_e; - GoToTuple next_goto = - node.command_deque[0]->get_goto_tuple().value(); - next_n = next_goto.n; - next_e = next_goto.e; - - d_n = next_n - target_n; - d_e = next_e - target_e; - - double angle = atan2(d_e, d_n) * 180. / M_PI; - // atan2 gives -180 to 180 while we want 0 to 360 for yaw control - if (angle < 0) - angle = 360. + angle; - - pos.yaw_deg = angle; - RCLCPP_INFO(node.get_logger(), - "next point is in FW, we're in QC, it is at (%f,%f) " - "from current target, yaw needed is %f", - d_n, d_e, pos.yaw_deg); - } - } - auto res = node.mavsdk_offboard->set_position_ned(pos); - if (res == mavsdk::Offboard::Result::Success) - goto_order_sent = true; + auto result = node.mavsdk_offboard->set_position_ned(pos); + if (result == mavsdk::Offboard::Result::Success) + goto_command_sent = true; } - - // if within tolerance and in the desired mode, you're done ! - if ((((desired_mode == FlightMode::QC && dist_from_target < 1.) || - (desired_mode == FlightMode::FW && dist_from_target < 10.))) && - node.state.vtol_state == desired_mode) { - // if (node.command_deque.size() == ) { - // node.mavsdk_offboard->stop(); - // } - return true; + if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { + node.mavsdk_offboard->start(); + return false; } - return false; + + return dist_from_target < node.qc_tolerance; } - ~GoTo() {} + virtual std::string get_description() override { + return string_format("Going to %lf, %lf, %lf", target_n, target_e, + target_d); + } - double start_n, start_e, start_d; - double target_n, target_e, target_d, target_vn, target_ve, target_vd; - FlightMode desired_mode; + virtual bool cancel_command(OffboardNode &node) override { return true; } - // toggles to not overwhelm the flight controller - bool start_pos_recorded = false; - bool transtion_order_sent = false; - bool goto_order_sent = false; + ~GoToQC() {} + double target_n, target_e, target_d; + bool goto_command_sent = false; }; + // TODO: Do this + struct GoToFW : Command {}; + struct Hold : Command {}; + + // struct GoTo : Command { + // GoTo(double n, double e, double d, double vn, double ve, double vd, + // FlightMode fm) { + // + // target_n = n; + // target_e = e; + // target_d = d; + // + // target_vn = vn; + // target_ve = ve; + // target_vd = vd; + // + // desired_mode = fm; + // } + // + // virtual bool is_goto() override { return true; } + // + // virtual std::optional get_goto_tuple() override { + // return std::optional(GoToTuple( + // target_n, target_e, target_d, target_vn, target_ve, target_vd)); + // }; + // + // virtual std::optional get_desired_fm() override { + // return std::optional(desired_mode); + // } + // + // // TODO: Finish implementing velocity and yaw control + // virtual bool perform_tick(OffboardNode &node) override { + // if (!start_pos_recorded) { + // start_pos_recorded = true; + // start_n = node.state.n; + // start_e = node.state.e; + // start_d = node.state.d; + // } + // + // // switch to offboard if not already + // if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { + // node.mavsdk_offboard->start(); + // } + // + // // send transition command if need to and if not sent already + // if (node.state.vtol_state != desired_mode && !transtion_order_sent) { + // mavsdk::Action::Result res; + // if (desired_mode == FlightMode::QC) { + // res = node.mavsdk_action->transition_to_multicopter(); + // } else { + // res = node.mavsdk_action->transition_to_fixedwing(); + // } + // if (res == mavsdk::Action::Result::Success) + // transtion_order_sent = true; + // return false; + // } + // + // double dist_from_target = sqrt(pow((node.state.n - target_n), 2) + + // pow((node.state.e - target_e), 2) + + // pow((node.state.d - target_d), 2)); + // + // // if above tolerance and command to go somewhere wasnt sent already + // if (((desired_mode == FlightMode::QC && dist_from_target >= 1.) || + // (desired_mode == FlightMode::FW && dist_from_target >= 10.)) && + // !goto_order_sent) { + // mavsdk::Offboard::PositionNedYaw pos; + // pos.north_m = target_n; + // pos.east_m = target_e; + // pos.down_m = target_d; + // + // pos.yaw_deg = NAN; + // // If there is a next goto action + // if (node.command_deque.size() >= 1 && + // node.command_deque[0]->is_goto()) { + // + // // if next goto action is in FW and we're in QC: setup yaw and + // // actually over shoot the point so you dont slow down before + // trying to transition if (desired_mode == FlightMode::QC && + // node.command_deque[0]->get_desired_fm().value() == + // FlightMode::FW) { + // + // double overshoot_scale = 1.1; + // pos.north_m = start_n + (target_n - start_n) * overshoot_scale; + // pos.east_m = start_e + (target_e - start_e) * overshoot_scale; + // + // double next_n, next_e, d_n, d_e; + // GoToTuple next_goto = + // node.command_deque[0]->get_goto_tuple().value(); + // next_n = next_goto.n; + // next_e = next_goto.e; + // + // d_n = next_n - target_n; + // d_e = next_e - target_e; + // + // double angle = atan2(d_e, d_n) * 180. / M_PI; + // // atan2 gives -180 to 180 while we want 0 to 360 for yaw control + // if (angle < 0) + // angle = 360. + angle; + // + // pos.yaw_deg = angle; + // RCLCPP_INFO(node.get_logger(), + // "next point is in FW, we're in QC, it is at (%f,%f) " + // "from current target, yaw needed is %f", + // d_n, d_e, pos.yaw_deg); + // } + // } + // + // auto res = node.mavsdk_offboard->set_position_ned(pos); + // if (res == mavsdk::Offboard::Result::Success) + // goto_order_sent = true; + // } + // + // // if within tolerance and in the desired mode, you're done ! + // if ((((desired_mode == FlightMode::QC && dist_from_target < 1.) || + // (desired_mode == FlightMode::FW && dist_from_target < 10.))) && + // node.state.vtol_state == desired_mode) { + // // if (node.command_deque.size() == ) { + // // node.mavsdk_offboard->stop(); + // // } + // return true; + // } + // return false; + // } + // + // ~GoTo() {} + // + // double start_n, start_e, start_d; + // double target_n, target_e, target_d, target_vn, target_ve, target_vd; + // FlightMode desired_mode; + // + // // toggles to not overwhelm the flight controller + // bool start_pos_recorded = false; + // bool transtion_order_sent = false; + // bool goto_order_sent = false; + // }; + // TODO: DO REST OF COMMAND CLASSES void make_world_frame() { @@ -436,6 +492,11 @@ class OffboardNode : public rclcpp::Node { state.header.stamp = this->get_clock()->now(); transform.header.stamp = this->get_clock()->now(); state.command_queue_size = command_deque.size(); + if (command_deque.size() == 0) { + state.current_command = "None"; + } else { + state.current_command = command_deque[0]->get_description(); + } this->tf_drone_broadcaster->sendTransform(transform); this->monitoring_pub->publish(state); } @@ -445,18 +506,19 @@ class OffboardNode : public rclcpp::Node { std::unique_ptr com; switch (action.action_type) { case custom_msgs::msg::Action::HOLD: + // TODO: As in this and break; case custom_msgs::msg::Action::GOTO_ACTION: - OffboardNode::GoTo::FlightMode fm; if (action.vtol_config == custom_msgs::msg::Action::IN_FW) { - fm = OffboardNode::GoTo::FlightMode::FW; + // TODO: this + RCLCPP_INFO(this->get_logger(), "NOT IMPLEMENTED YET"); + return; } if (action.vtol_config == custom_msgs::msg::Action::IN_QC) { - fm = OffboardNode::GoTo::FlightMode::QC; + com = std::make_unique(action.n, action.e, + action.d); } - com = std::make_unique( - action.n, action.e, action.d, action.vn, action.ve, action.vd, fm); break; case custom_msgs::msg::Action::LAND: com = std::make_unique(); @@ -468,35 +530,38 @@ class OffboardNode : public rclcpp::Node { command_deque.push_back(std::move(com)); } - void dequeue_topic_callback(const std_msgs::msg::Empty &em) { + void cancel_command_topic_callback(const std_msgs::msg::Empty &em) { if (command_deque.size() > 0) - command_deque.pop_front(); + want_to_cancel = true; } void command_queue_callback() { // RCLCPP_INFO(this->get_logger(), "Command queue has size %d", // command_deque.size()); if (command_deque.size() == 0) { - // This is incase we havent reset things because we poped a command before - // it was done or something like that if (state.flight_mode == custom_msgs::msg::DroneState::OFFBOARD) { mavsdk_offboard->stop(); } - if (state.flight_mode != custom_msgs::msg::DroneState::HOLD) { - mavsdk_action->hold(); - } + // This may make it impossible for the Remote controller to take over, + // above should suffice if (state.flight_mode != + // custom_msgs::msg::DroneState::HOLD && state.flight_mode != + // custom_msgs::msg::DroneState::MANUAL) { + // mavsdk_action->hold(); + // } return; } - auto head = std::move(command_deque.front()); - command_deque.pop_front(); - - bool is_done = head->perform_tick(*this); + bool is_done = (want_to_cancel) ? command_deque[0]->cancel_command(*this) + : command_deque[0]->perform_tick(*this); - if (!is_done) { - command_deque.push_front(std::move(head)); + if (is_done) { + want_to_cancel = false; + command_deque.pop_front(); } } + // flight parameters + double qc_tolerance; + // ROS2 related stuff rclcpp::Publisher::SharedPtr monitoring_pub; rclcpp::Subscription::SharedPtr enqueue_sub; @@ -520,6 +585,7 @@ class OffboardNode : public rclcpp::Node { // Command queue std::deque> command_deque = {}; + bool want_to_cancel = false; }; int main(int argc, char *argv[]) { From a2cce11f828d775369c8d269b95d1597e0d01368 Mon Sep 17 00:00:00 2001 From: Black-bird14 Date: Mon, 3 Feb 2025 23:03:53 -0500 Subject: [PATCH 3/9] Finally compiles successfully --- drone_ws/src/mapping_and_planning/CMakeLists.txt | 9 +++++++-- drone_ws/src/mapping_and_planning/package.xml | 1 + drone_ws/src/mapping_and_planning/src/mapping.cpp | 2 +- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/drone_ws/src/mapping_and_planning/CMakeLists.txt b/drone_ws/src/mapping_and_planning/CMakeLists.txt index e705491..8c47947 100644 --- a/drone_ws/src/mapping_and_planning/CMakeLists.txt +++ b/drone_ws/src/mapping_and_planning/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(std_msgs REQUIRED) find_package (Eigen3 3.3 REQUIRED NO_MODULE) find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(mapping_and_planning REQUIRED) +find_package(mapping_and_planning REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Obstacle.msg" @@ -23,8 +23,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} # further dependencies manually. # find_package( REQUIRED) -add_executable(map_n_plan src/mapping.cpp) +add_executable(map_n_plan src/mapping.cpp) +# Fix double namespace issue +set(_generated_files_dir "${CMAKE_BINARY_DIR}/rosidl_generator_cpp") +target_include_directories(map_n_plan PUBLIC + ${_generated_files_dir} +) # set(ENV{octomap_DIR} /home/unhappysquid/octomap/) include_directories(${OCTOMAP_INCLUDE_DIRS}) link_directories(${OCTOMAP_LIBRARY_DIRS}) diff --git a/drone_ws/src/mapping_and_planning/package.xml b/drone_ws/src/mapping_and_planning/package.xml index 905fe52..ca31b1e 100644 --- a/drone_ws/src/mapping_and_planning/package.xml +++ b/drone_ws/src/mapping_and_planning/package.xml @@ -15,6 +15,7 @@ octomap eigen geometry_msgs + rosidl_default_generators rosidl_default_runtime rosidl_interface_packages diff --git a/drone_ws/src/mapping_and_planning/src/mapping.cpp b/drone_ws/src/mapping_and_planning/src/mapping.cpp index 2880b2d..cc99e7d 100644 --- a/drone_ws/src/mapping_and_planning/src/mapping.cpp +++ b/drone_ws/src/mapping_and_planning/src/mapping.cpp @@ -14,7 +14,7 @@ Planner::Planner() bounds_sub_ = this->create_subscription( "mission_boundary", 10, bind(&Planner::boundaryCallback, this, _1)); - obs_sub_ = this->create_subscription( + obs_sub_ = this->create_subscription( "detection", 10, std::bind(&Planner::obstacleCallback, this, _1)); cout << this->drone_frame_ << endl; From 73e40c8795cf302f116b6a1ac7c5f30a791f96c5 Mon Sep 17 00:00:00 2001 From: UnHappySquid Date: Mon, 10 Feb 2025 12:09:19 -0500 Subject: [PATCH 4/9] FW command mutations working, just need to implement behaviour for each phase now. --- drone_test.sh | 6 +- .../src/px4_controller/src/drone_node.cpp | 236 +++++++----------- 2 files changed, 98 insertions(+), 144 deletions(-) diff --git a/drone_test.sh b/drone_test.sh index 2f19c8b..d489c9f 100644 --- a/drone_test.sh +++ b/drone_test.sh @@ -6,7 +6,11 @@ ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: -200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" -ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" diff --git a/drone_ws/src/px4_controller/src/drone_node.cpp b/drone_ws/src/px4_controller/src/drone_node.cpp index 21ccaf7..c859f92 100644 --- a/drone_ws/src/px4_controller/src/drone_node.cpp +++ b/drone_ws/src/px4_controller/src/drone_node.cpp @@ -114,6 +114,8 @@ class OffboardNode : public rclcpp::Node { } virtual std::string get_description() = 0; + virtual void mutate(Command *new_command) {} + virtual bool is_fw() { return false; } virtual ~Command() {} }; @@ -198,7 +200,7 @@ class OffboardNode : public rclcpp::Node { } virtual std::string get_description() override { - return string_format("Going to %lf, %lf, %lf", target_n, target_e, + return string_format("Going to %lf, %lf, %lf in QC", target_n, target_e, target_d); } @@ -209,143 +211,93 @@ class OffboardNode : public rclcpp::Node { bool goto_command_sent = false; }; - // TODO: Do this - struct GoToFW : Command {}; + struct GoToFW : Command { + + GoToFW(double n, double e, double d) + : target_n(n), target_e(e), target_d(d) {} + + // TODO: Do this + virtual bool perform_tick(OffboardNode &node) override { + // IF PREV NOT FW: + // FIX HEADING + // TRANSITION + // START HEADING THERE + // IF NEXT COMMAND IS ALSO FW, RETURN TRUE WHEN REACHED AND PASS THAT INFO + // INTO NEXT COMMAND ELSE, PRE-UNTRANSITION THEN RETURN TRUE WHEN REACHED + RCLCPP_INFO(node.get_logger(), "FW TYPE: %s", get_type_str().c_str()); + return true; + } + + // TODO: Fix cancel command for FW + virtual bool cancel_command(OffboardNode &node) override { return true; } + + virtual std::string get_description() override { + return string_format("Going to %lf, %lf, %lf in FW", target_n, target_e, + target_d); + } + virtual void mutate(Command *new_command) override { + if (new_command->is_fw()) { + static_cast(new_command)->type = END; + switch (type) { + case SOLE: + type = START; + break; + case START: + break; + case MIDDLE: + break; + case END: + type = MIDDLE; + break; + } + } + } + + virtual bool is_fw() override { return true; } + + private: + enum TypeFW { SOLE = 1, START = 2, MIDDLE = 3, END = 4 }; + std::string get_type_str() { + switch (type) { + case SOLE: + return "SOLE"; + case START: + return "START"; + case MIDDLE: + return "MIDDLE"; + case END: + return "END"; + } + } + TypeFW type = TypeFW::SOLE; + double target_n, target_e, target_d; + }; + struct Hold : Command {}; - // struct GoTo : Command { - // GoTo(double n, double e, double d, double vn, double ve, double vd, - // FlightMode fm) { - // - // target_n = n; - // target_e = e; - // target_d = d; - // - // target_vn = vn; - // target_ve = ve; - // target_vd = vd; - // - // desired_mode = fm; - // } - // - // virtual bool is_goto() override { return true; } - // - // virtual std::optional get_goto_tuple() override { - // return std::optional(GoToTuple( - // target_n, target_e, target_d, target_vn, target_ve, target_vd)); - // }; - // - // virtual std::optional get_desired_fm() override { - // return std::optional(desired_mode); - // } - // - // // TODO: Finish implementing velocity and yaw control - // virtual bool perform_tick(OffboardNode &node) override { - // if (!start_pos_recorded) { - // start_pos_recorded = true; - // start_n = node.state.n; - // start_e = node.state.e; - // start_d = node.state.d; - // } - // - // // switch to offboard if not already - // if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { - // node.mavsdk_offboard->start(); - // } - // - // // send transition command if need to and if not sent already - // if (node.state.vtol_state != desired_mode && !transtion_order_sent) { - // mavsdk::Action::Result res; - // if (desired_mode == FlightMode::QC) { - // res = node.mavsdk_action->transition_to_multicopter(); - // } else { - // res = node.mavsdk_action->transition_to_fixedwing(); - // } - // if (res == mavsdk::Action::Result::Success) - // transtion_order_sent = true; - // return false; - // } - // - // double dist_from_target = sqrt(pow((node.state.n - target_n), 2) + - // pow((node.state.e - target_e), 2) + - // pow((node.state.d - target_d), 2)); - // - // // if above tolerance and command to go somewhere wasnt sent already - // if (((desired_mode == FlightMode::QC && dist_from_target >= 1.) || - // (desired_mode == FlightMode::FW && dist_from_target >= 10.)) && - // !goto_order_sent) { - // mavsdk::Offboard::PositionNedYaw pos; - // pos.north_m = target_n; - // pos.east_m = target_e; - // pos.down_m = target_d; - // - // pos.yaw_deg = NAN; - // // If there is a next goto action - // if (node.command_deque.size() >= 1 && - // node.command_deque[0]->is_goto()) { - // - // // if next goto action is in FW and we're in QC: setup yaw and - // // actually over shoot the point so you dont slow down before - // trying to transition if (desired_mode == FlightMode::QC && - // node.command_deque[0]->get_desired_fm().value() == - // FlightMode::FW) { - // - // double overshoot_scale = 1.1; - // pos.north_m = start_n + (target_n - start_n) * overshoot_scale; - // pos.east_m = start_e + (target_e - start_e) * overshoot_scale; - // - // double next_n, next_e, d_n, d_e; - // GoToTuple next_goto = - // node.command_deque[0]->get_goto_tuple().value(); - // next_n = next_goto.n; - // next_e = next_goto.e; - // - // d_n = next_n - target_n; - // d_e = next_e - target_e; - // - // double angle = atan2(d_e, d_n) * 180. / M_PI; - // // atan2 gives -180 to 180 while we want 0 to 360 for yaw control - // if (angle < 0) - // angle = 360. + angle; - // - // pos.yaw_deg = angle; - // RCLCPP_INFO(node.get_logger(), - // "next point is in FW, we're in QC, it is at (%f,%f) " - // "from current target, yaw needed is %f", - // d_n, d_e, pos.yaw_deg); - // } - // } - // - // auto res = node.mavsdk_offboard->set_position_ned(pos); - // if (res == mavsdk::Offboard::Result::Success) - // goto_order_sent = true; - // } - // - // // if within tolerance and in the desired mode, you're done ! - // if ((((desired_mode == FlightMode::QC && dist_from_target < 1.) || - // (desired_mode == FlightMode::FW && dist_from_target < 10.))) && - // node.state.vtol_state == desired_mode) { - // // if (node.command_deque.size() == ) { - // // node.mavsdk_offboard->stop(); - // // } - // return true; - // } - // return false; - // } - // - // ~GoTo() {} - // - // double start_n, start_e, start_d; - // double target_n, target_e, target_d, target_vn, target_ve, target_vd; - // FlightMode desired_mode; - // - // // toggles to not overwhelm the flight controller - // bool start_pos_recorded = false; - // bool transtion_order_sent = false; - // bool goto_order_sent = false; - // }; - - // TODO: DO REST OF COMMAND CLASSES + struct CommandQueue { + + CommandQueue() {} + + int size() { return command_queue.size(); } + + Command *operator[](std::size_t idx) { return command_queue[idx].get(); } + + void push_back(std::unique_ptr c) { + command_queue.push_back(std::move(c)); + } + + void enqueue_command(std::unique_ptr c) { + if (size() > 0) + command_queue[size() - 1]->mutate(c.get()); + push_back(std::move(c)); + } + + void pop_front() { command_queue.pop_front(); } + + private: + std::deque> command_queue = {}; + }; void make_world_frame() { geometry_msgs::msg::TransformStamped t; @@ -511,11 +463,9 @@ class OffboardNode : public rclcpp::Node { break; case custom_msgs::msg::Action::GOTO_ACTION: if (action.vtol_config == custom_msgs::msg::Action::IN_FW) { - // TODO: this - RCLCPP_INFO(this->get_logger(), "NOT IMPLEMENTED YET"); - return; - } - if (action.vtol_config == custom_msgs::msg::Action::IN_QC) { + com = std::make_unique(action.n, action.e, + action.d); + } else if (action.vtol_config == custom_msgs::msg::Action::IN_QC) { com = std::make_unique(action.n, action.e, action.d); } @@ -527,7 +477,7 @@ class OffboardNode : public rclcpp::Node { com = std::make_unique(action.takeoff_height); break; } - command_deque.push_back(std::move(com)); + command_deque.enqueue_command(std::move(com)); } void cancel_command_topic_callback(const std_msgs::msg::Empty &em) { @@ -584,7 +534,7 @@ class OffboardNode : public rclcpp::Node { std::unique_ptr mavsdk_offboard; // Command queue - std::deque> command_deque = {}; + CommandQueue command_deque = {}; bool want_to_cancel = false; }; From 6d68d82dfc8bd23b0b61c19fa8ab658746a4ac4c Mon Sep 17 00:00:00 2001 From: UnHappySquid Date: Tue, 11 Feb 2025 22:49:49 -0500 Subject: [PATCH 5/9] Added documentation, euler angles for attitude in drone state, and almost done with FW commands logic --- drone_ws/src/custom_msgs/msg/DroneState.msg | 5 + .../src/px4_controller/src/drone_node.cpp | 360 ++++++++++++++---- 2 files changed, 300 insertions(+), 65 deletions(-) diff --git a/drone_ws/src/custom_msgs/msg/DroneState.msg b/drone_ws/src/custom_msgs/msg/DroneState.msg index b330ce0..b69e358 100644 --- a/drone_ws/src/custom_msgs/msg/DroneState.msg +++ b/drone_ws/src/custom_msgs/msg/DroneState.msg @@ -58,6 +58,11 @@ float64 y float64 z float64 w +# this is also the attitude, as roll, pitch and yaw +float64 roll_deg +float64 pitch_deg +float64 yaw_deg + # this is the angular velocity (refer to)[https://mariogc.com/post/angular-velocity-quaternions/] float64 roll_rad_s float64 pitch_rad_s diff --git a/drone_ws/src/px4_controller/src/drone_node.cpp b/drone_ws/src/px4_controller/src/drone_node.cpp index c859f92..b6b0870 100644 --- a/drone_ws/src/px4_controller/src/drone_node.cpp +++ b/drone_ws/src/px4_controller/src/drone_node.cpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include #include @@ -25,6 +27,7 @@ using namespace std::chrono_literals; +// UTILITY FUNCTIONS template std::string string_format(const std::string &format, Args... args) { int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) + @@ -39,14 +42,20 @@ std::string string_format(const std::string &format, Args... args) { buf.get() + size - 1); // We don't want the '\0' inside } +double smallestAngle(double currentAngle, double targetAngle) { + double diff = fmod((targetAngle - currentAngle), 360.); + return diff <= 180. ? diff : -(360. - diff); +} + class OffboardNode : public rclcpp::Node { public: + // TODO: Add parameters as parameters OffboardNode() : Node("offboard_controller"), qc_tolerance(1.) { // tf2 stuff tf_world_broadcaster = std::make_shared(this); - this->make_world_frame(); + this->setup_world_frame(); tf_drone_broadcaster = std::make_unique(*this); @@ -90,10 +99,11 @@ class OffboardNode : public rclcpp::Node { std::bind(&OffboardNode::enqueue_topic_callback, this, std::placeholders::_1)); - pop_sub = this->create_subscription( - "cancel_command", 10, - std::bind(&OffboardNode::cancel_command_topic_callback, this, - std::placeholders::_1)); + // TODO: DECOMMISSIONED FOR NOW, COME BACK TO THIS LATER + // pop_sub = this->create_subscription( + // "cancel_command", 10, + // std::bind(&OffboardNode::cancel_command_topic_callback, this, + // std::placeholders::_1)); this->timer_ = this->create_wall_timer( 100ms, std::bind(&OffboardNode::monitoring_callback, this)); @@ -103,22 +113,68 @@ class OffboardNode : public rclcpp::Node { } private: - // This only makes sense in the context of the action queue + /* Objects that represent a command. This defines a common interface and has + * utility functions + * */ struct Command { // both perform tick and cancel_command return true if they are done, false // otherwise + + /* Performs one tick of the command. This is for the command to be + * non-blocking + * */ virtual bool perform_tick(OffboardNode &on) = 0; + /* This is also one tick for the same reason as above, but is supposed to + * handel cancelations properly. FOR NOW, DO NOT DO THIS (OUT OF SCOPE) + * */ virtual bool cancel_command(OffboardNode &on) { RCLCPP_INFO(on.get_logger(), "This command is not cancelable"); return perform_tick(on); } + /* Returns a string that describes the command. For debugging purposes. + * */ virtual std::string get_description() = 0; + /* Function that transforms the this command to adapt to the new_command + * This is really only relevant for FW commands as they need to change + * behavour if the new_command is also FW (ex: need to stay in FW and not + * transition after the goal is reached) + * */ virtual void mutate(Command *new_command) {} - virtual bool is_fw() { return false; } virtual ~Command() {} + + protected: + /* Sends a way point command, over 2 ticks if not in offboard mode, over 1 + * tick otherwise. returns true when sending is done (not when reached + * destination) If you don't want control over a parameter (like yaw), set + * it to NAN + * */ + bool send_offboard_goto(OffboardNode &node, double north, double east, + double down, double yaw) { + mavsdk::Offboard::PositionNedYaw pos; + pos.north_m = north; + pos.east_m = east; + pos.down_m = down; + pos.yaw_deg = yaw; + + auto result = node.mavsdk_offboard->set_position_ned(pos); + if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { + node.mavsdk_offboard->start(); + return false; + } + return true; + } + + double distance_from_target(OffboardNode &node, double target_n, + double target_e, double target_d) { + return sqrt(pow((node.state.n - target_n), 2) + + pow((node.state.e - target_e), 2) + + pow((node.state.d - target_d), 2)); + } }; + /* Command responsible for taking off to a certain height. + * */ struct TakeOff : Command { TakeOff(double height) : target_height(height) {} @@ -150,6 +206,8 @@ class OffboardNode : public rclcpp::Node { double target_height; }; + /* Command responsible for landing. + * */ struct Land : Command { Land() {} @@ -171,29 +229,18 @@ class OffboardNode : public rclcpp::Node { ~Land() {} }; + /* Command responsible for going to a destination in QC mode. + * */ struct GoToQC : Command { GoToQC(double n, double e, double d) : target_n(n), target_e(e), target_d(d) {} virtual bool perform_tick(OffboardNode &node) override { - double dist_from_target = sqrt(pow((node.state.n - target_n), 2) + - pow((node.state.e - target_e), 2) + - pow((node.state.d - target_d), 2)); - + double dist_from_target = + distance_from_target(node, target_n, target_e, target_d); if (dist_from_target >= node.qc_tolerance && !goto_command_sent) { - mavsdk::Offboard::PositionNedYaw pos; - pos.north_m = target_n; - pos.east_m = target_e; - pos.down_m = target_d; - pos.yaw_deg = NAN; - - auto result = node.mavsdk_offboard->set_position_ned(pos); - if (result == mavsdk::Offboard::Result::Success) - goto_command_sent = true; - } - if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { - node.mavsdk_offboard->start(); - return false; + goto_command_sent = + send_offboard_goto(node, target_n, target_e, target_d, NAN); } return dist_from_target < node.qc_tolerance; @@ -211,70 +258,231 @@ class OffboardNode : public rclcpp::Node { bool goto_command_sent = false; }; + /* Command responsible for going to a destination in FW mode. This needs to + * adapt it's behabiour w.r.t. the next command. All new commands start as + * SOLE (alone). They then transition to the appropriate type of FW Command if + * need to. + * */ struct GoToFW : Command { GoToFW(double n, double e, double d) : target_n(n), target_e(e), target_d(d) {} - // TODO: Do this virtual bool perform_tick(OffboardNode &node) override { - // IF PREV NOT FW: - // FIX HEADING - // TRANSITION - // START HEADING THERE - // IF NEXT COMMAND IS ALSO FW, RETURN TRUE WHEN REACHED AND PASS THAT INFO - // INTO NEXT COMMAND ELSE, PRE-UNTRANSITION THEN RETURN TRUE WHEN REACHED - RCLCPP_INFO(node.get_logger(), "FW TYPE: %s", get_type_str().c_str()); - return true; + bool res; + switch (fw_command_type) { + case TypeFW::SOLE: + res = sole_flight(node); + break; + case TypeFW::STARTING_FW: + res = starting_fw_flight(node); + break; + case TypeFW::MIDDLE_FW: + res = middle_fw_flight(node); + break; + case TypeFW::END_FW: + res = end_fw_flight(node); + break; + } + return res; } - // TODO: Fix cancel command for FW - virtual bool cancel_command(OffboardNode &node) override { return true; } - virtual std::string get_description() override { return string_format("Going to %lf, %lf, %lf in FW", target_n, target_e, target_d); } + virtual void mutate(Command *new_command) override { - if (new_command->is_fw()) { - static_cast(new_command)->type = END; - switch (type) { - case SOLE: - type = START; + + GoToFW *command_ptr = dynamic_cast(new_command); + if (command_ptr) { + command_ptr->adapt(); + switch (fw_command_type) { + case TypeFW::SOLE: + fw_command_type = TypeFW::STARTING_FW; break; - case START: + case TypeFW::STARTING_FW: + std::cout << "THIS COMMAD IS STARTING_FW AND AT THE END OF THE " + "QUEUE, IMPOSSIBLE!! EXITING..." + << std::endl; + exit(2); break; - case MIDDLE: + case TypeFW::MIDDLE_FW: + std::cout << "THIS COMMAD IS MIDDLE_FW AND AT THE END OF THE " + "QUEUE, IMPOSSIBLE!! EXITING..." + << std::endl; + exit(3); break; - case END: - type = MIDDLE; + case TypeFW::END_FW: + fw_command_type = TypeFW::MIDDLE_FW; break; } } } - virtual bool is_fw() override { return true; } - private: - enum TypeFW { SOLE = 1, START = 2, MIDDLE = 3, END = 4 }; + /* Describes the different types of FW commands in a command. Very much like + * POS Tagging START_FW: Start of a sequence of FW commands, responsible for + * starting the sequence (fix heading and transition) and reaching the + * destination. MIDDLE_FW: Middle of a sequence of FW commands, responsible + * for stopping when destination is reached. END_FW: End of a sequence of FW + * commands, responsible for untransitionning slightly before reaching the + * destination, then stop when destination is reached. SOLE: Unique FW + * command, responsible for both parts of STARTING_FW behaviour and END_FW + * behaviour at the start and end respectively. + * */ + enum class TypeFW { SOLE, STARTING_FW, MIDDLE_FW, END_FW }; std::string get_type_str() { - switch (type) { - case SOLE: + switch (fw_command_type) { + case TypeFW::SOLE: return "SOLE"; - case START: + case TypeFW::STARTING_FW: return "START"; - case MIDDLE: + case TypeFW::MIDDLE_FW: return "MIDDLE"; - case END: + case TypeFW::END_FW: return "END"; } } - TypeFW type = TypeFW::SOLE; + /* Make this command the end of a FW sequence, only called if previous + * command is FW and gets mutated + * */ + void adapt() { fw_command_type = TypeFW::END_FW; } + + /* Behaviour needs to once again change w.r.t. what's been done already + * A task should only be complete once its done with its STOP stage. + * */ + enum class LifetimeStage { BEGIN, ONGOING, STOP }; + + /* A function which alligns the heading of the drone with the target + * destination returns true when at the appropriate heading only. + * */ + bool fix_heading(OffboardNode &node) { + double desired_heading = + std::atan2(target_e - node.state.e, target_n - node.state.n) * 180. / + M_PI; + desired_heading = + (desired_heading >= 0) ? desired_heading : 360. + desired_heading; + double current_heading = node.state.yaw_deg; + if (smallestAngle(desired_heading, current_heading) < 10.0) { + return true; + } + + if (!yaw_fixing_command_sent) { + yaw_fixing_command_sent = + send_offboard_goto(node, NAN, NAN, NAN, desired_heading); + } + + return false; + } + + /* A function which sends the waypoint and transition commands. Returns true + * when those commands are successfully sent + * */ + bool send_waypoint_transition(OffboardNode &node) { + if (!goto_command_sent) { + goto_command_sent = + send_offboard_goto(node, target_n, target_e, target_d, NAN); + } + if (!transition_command_sent) { + auto res = node.mavsdk_action->transition_to_fixedwing(); + transition_command_sent = (res == mavsdk::Action::Result::Success); + } + return goto_command_sent && transition_command_sent; + } + + /* A function which sends a untransition command. Returns true when the + * command is successful. + * */ + bool send_untransition(OffboardNode &node) { + return node.mavsdk_action->transition_to_multicopter() == + mavsdk::Action::Result::Success; + } + + bool is_within_from_target(OffboardNode &node, double meters) { + return distance_from_target(node, target_n, target_e, target_d) < meters; + } + + bool sole_flight(OffboardNode &node) { + // FIX HEADING + // ONCE FIXED SET WAYPOINT THEN TRANSITION + // THEN PRE-UNTRANSITION BEFORE REACING END + // BOOL -> RETURN TRUE WHEN IN QC AND REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + break; + case LifetimeStage::ONGOING: + break; + case LifetimeStage::STOP: + // if () return true; + break; + } + return false; + } + + // TODO: FINISH DOING THIS + bool starting_fw_flight(OffboardNode &node) { + // FIX HEADING + // ONCE FIXED SET WAYPOINT THEN TRANSITION + // BOOL -> RETURN TRUE WHEN REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + break; + case LifetimeStage::ONGOING: + break; + case LifetimeStage::STOP: + // if () return true; + break; + } + return false; + } + + bool middle_fw_flight(OffboardNode &node) { + // SET WAYPOINT + // BOOL -> RETURN TRUE WHEN REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + break; + case LifetimeStage::ONGOING: + break; + case LifetimeStage::STOP: + // if () return true; + break; + } + return false; + } + + bool end_fw_flight(OffboardNode &node) { + // SET WAYPOINT + // THEN PRE-UNTRANSITION BEFORE REACING END + // BOOL -> RETURN TRUE WHEN IN QC AND REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + break; + case LifetimeStage::ONGOING: + break; + case LifetimeStage::STOP: + // if () return true; + break; + } + return false; + } + + TypeFW fw_command_type = TypeFW::SOLE; + LifetimeStage current_stage = LifetimeStage::BEGIN; double target_n, target_e, target_d; + + // FLAGS + bool yaw_fixing_command_sent = false; + bool goto_command_sent = false; + bool transition_command_sent = false; }; + // TODO: IMPLEMENT HOLD struct Hold : Command {}; + /* A queue composed object, needed to automatically call mutate on append. + * */ struct CommandQueue { CommandQueue() {} @@ -299,7 +507,9 @@ class OffboardNode : public rclcpp::Node { std::deque> command_queue = {}; }; - void make_world_frame() { + /* Sets up the world_frame object (used by RViz). + * */ + void setup_world_frame() { geometry_msgs::msg::TransformStamped t; t.header.stamp = this->get_clock()->now(); t.header.frame_id = "world"; @@ -317,6 +527,9 @@ class OffboardNode : public rclcpp::Node { this->tf_world_broadcaster->sendTransform(t); } + /* Sets up the various mavsdk callbacks necessary to update the state and + * world_frame. + * */ void setup_monitoring() { state.header.frame_id = "world"; @@ -381,6 +594,13 @@ class OffboardNode : public rclcpp::Node { transform.transform.rotation.w = quat.w; }); + mavsdk_telem->subscribe_attitude_euler( + [this](mavsdk::Telemetry::EulerAngle angles) { + state.roll_deg = angles.roll_deg; + state.pitch_deg = angles.pitch_deg; + state.yaw_deg = angles.yaw_deg; + }); + mavsdk_telem->subscribe_attitude_angular_velocity_body( [this](mavsdk::Telemetry::AngularVelocityBody angular_vel) { state.roll_rad_s = angular_vel.roll_rad_s; @@ -440,6 +660,8 @@ class OffboardNode : public rclcpp::Node { }); } + /* Callback to publish the state and world_frame + * */ void monitoring_callback() { state.header.stamp = this->get_clock()->now(); transform.header.stamp = this->get_clock()->now(); @@ -453,6 +675,8 @@ class OffboardNode : public rclcpp::Node { this->monitoring_pub->publish(state); } + /* Callback to enqueue a new topic in the topic queue. + * */ // TODO: DO THIS void enqueue_topic_callback(const custom_msgs::msg::Action &action) { std::unique_ptr com; @@ -476,18 +700,23 @@ class OffboardNode : public rclcpp::Node { case custom_msgs::msg::Action::TAKEOFF_ACTION: com = std::make_unique(action.takeoff_height); break; + default: + std::cout << "ACTION TYPE INVALID!! EXITING..." << std::endl; + exit(1); + break; } command_deque.enqueue_command(std::move(com)); } - void cancel_command_topic_callback(const std_msgs::msg::Empty &em) { - if (command_deque.size() > 0) - want_to_cancel = true; - } + // TODO: DECOMMISIONED, COME BACK TO THIS LATER + // void cancel_command_topic_callback(const std_msgs::msg::Empty &em) { + // if (command_deque.size() > 0) + // want_to_cancel = true; + // } + /* Responsible for running one tick of the top command. + * */ void command_queue_callback() { - // RCLCPP_INFO(this->get_logger(), "Command queue has size %d", - // command_deque.size()); if (command_deque.size() == 0) { if (state.flight_mode == custom_msgs::msg::DroneState::OFFBOARD) { mavsdk_offboard->stop(); @@ -500,11 +729,12 @@ class OffboardNode : public rclcpp::Node { // } return; } - bool is_done = (want_to_cancel) ? command_deque[0]->cancel_command(*this) - : command_deque[0]->perform_tick(*this); + // bool is_done = (want_to_cancel) ? command_deque[0]->cancel_command(*this) + // : command_deque[0]->perform_tick(*this); + bool is_done = command_deque[0]->perform_tick(*this); if (is_done) { - want_to_cancel = false; + // want_to_cancel = false; command_deque.pop_front(); } } @@ -535,7 +765,7 @@ class OffboardNode : public rclcpp::Node { // Command queue CommandQueue command_deque = {}; - bool want_to_cancel = false; + // bool want_to_cancel = false; }; int main(int argc, char *argv[]) { From eeb1e85821bfb0f60ca048104a1bf5511c6e3eea Mon Sep 17 00:00:00 2001 From: UnHappySquid Date: Wed, 12 Feb 2025 17:37:27 -0500 Subject: [PATCH 6/9] FW Command of SOLE type seems to work, need more testing but for now moving on --- drone_test.sh | 6 +-- .../src/px4_controller/src/drone_node.cpp | 41 +++++++++++++++---- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/drone_test.sh b/drone_test.sh index d489c9f..f09542f 100644 --- a/drone_test.sh +++ b/drone_test.sh @@ -8,9 +8,9 @@ ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" -ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" -ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" -ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +# ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +# ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +# ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" diff --git a/drone_ws/src/px4_controller/src/drone_node.cpp b/drone_ws/src/px4_controller/src/drone_node.cpp index b6b0870..a97fbe4 100644 --- a/drone_ws/src/px4_controller/src/drone_node.cpp +++ b/drone_ws/src/px4_controller/src/drone_node.cpp @@ -42,9 +42,12 @@ std::string string_format(const std::string &format, Args... args) { buf.get() + size - 1); // We don't want the '\0' inside } +/* Assumes both current angle and target angle are positive + * */ double smallestAngle(double currentAngle, double targetAngle) { - double diff = fmod((targetAngle - currentAngle), 360.); - return diff <= 180. ? diff : -(360. - diff); + double bigger = std::max(currentAngle, targetAngle); + double smaller = std::min(currentAngle, targetAngle); + return std::min(bigger - smaller, smaller + 360. - bigger); } class OffboardNode : public rclcpp::Node { @@ -288,8 +291,9 @@ class OffboardNode : public rclcpp::Node { } virtual std::string get_description() override { - return string_format("Going to %lf, %lf, %lf in FW", target_n, target_e, - target_d); + return string_format( + "Going to %lf, %lf, %lf in FW (FW Type: %s | FW Stage %s)", + target_n, target_e, target_d, get_type_str().c_str(), get_lifetime_str().c_str()); } virtual void mutate(Command *new_command) override { @@ -353,6 +357,16 @@ class OffboardNode : public rclcpp::Node { * A task should only be complete once its done with its STOP stage. * */ enum class LifetimeStage { BEGIN, ONGOING, STOP }; + std::string get_lifetime_str() { + switch (current_stage) { + case LifetimeStage::BEGIN: + return "BEGIN"; + case LifetimeStage::ONGOING: + return "ONGOING"; + case LifetimeStage::STOP: + return "STOP"; + } + } /* A function which alligns the heading of the drone with the target * destination returns true when at the appropriate heading only. @@ -364,13 +378,14 @@ class OffboardNode : public rclcpp::Node { desired_heading = (desired_heading >= 0) ? desired_heading : 360. + desired_heading; double current_heading = node.state.yaw_deg; - if (smallestAngle(desired_heading, current_heading) < 10.0) { + std::cout << "Desired : " << desired_heading << ", Current : " << current_heading << std::endl; + if (smallestAngle(desired_heading, current_heading) < 5.0) { return true; } if (!yaw_fixing_command_sent) { yaw_fixing_command_sent = - send_offboard_goto(node, NAN, NAN, NAN, desired_heading); + send_offboard_goto(node, node.state.n, node.state.e, node.state.d, desired_heading); } return false; @@ -410,14 +425,23 @@ class OffboardNode : public rclcpp::Node { // BOOL -> RETURN TRUE WHEN IN QC AND REACHED switch (current_stage) { case LifetimeStage::BEGIN: + if (fix_heading(node)) + current_stage = LifetimeStage::ONGOING; + return false; break; case LifetimeStage::ONGOING: + if (send_waypoint_transition(node)) + current_stage = LifetimeStage::STOP; + return false; break; case LifetimeStage::STOP: - // if () return true; + if (is_within_from_target(node, 50.) && !untransition_command_sent) + untransition_command_sent = send_untransition(node); + if (is_within_from_target(node, 5.) && untransition_command_sent) + return true; + return false; break; } - return false; } // TODO: FINISH DOING THIS @@ -476,6 +500,7 @@ class OffboardNode : public rclcpp::Node { bool yaw_fixing_command_sent = false; bool goto_command_sent = false; bool transition_command_sent = false; + bool untransition_command_sent = false; }; // TODO: IMPLEMENT HOLD From a9818a6bda8a40bd20e823b7ec46592bd714bb00 Mon Sep 17 00:00:00 2001 From: UnHappySquid Date: Wed, 12 Feb 2025 18:00:38 -0500 Subject: [PATCH 7/9] DRONE CONTROLLER COMPLETE ????? --- drone_test.sh | 8 ++- .../src/px4_controller/src/drone_node.cpp | 57 ++++++++++++++----- 2 files changed, 49 insertions(+), 16 deletions(-) diff --git a/drone_test.sh b/drone_test.sh index f09542f..06ad2ad 100644 --- a/drone_test.sh +++ b/drone_test.sh @@ -8,9 +8,11 @@ ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" -# ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" -# ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" -# ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 100., e: 100., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 10., e: 10., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" diff --git a/drone_ws/src/px4_controller/src/drone_node.cpp b/drone_ws/src/px4_controller/src/drone_node.cpp index a97fbe4..ec6bb26 100644 --- a/drone_ws/src/px4_controller/src/drone_node.cpp +++ b/drone_ws/src/px4_controller/src/drone_node.cpp @@ -42,7 +42,7 @@ std::string string_format(const std::string &format, Args... args) { buf.get() + size - 1); // We don't want the '\0' inside } -/* Assumes both current angle and target angle are positive +/* Returns the smallest angle between 2 angles, in degrees. * */ double smallestAngle(double currentAngle, double targetAngle) { double bigger = std::max(currentAngle, targetAngle); @@ -292,8 +292,9 @@ class OffboardNode : public rclcpp::Node { virtual std::string get_description() override { return string_format( - "Going to %lf, %lf, %lf in FW (FW Type: %s | FW Stage %s)", - target_n, target_e, target_d, get_type_str().c_str(), get_lifetime_str().c_str()); + "Going to %lf, %lf, %lf in FW (FW Type: %s | FW Stage %s)", target_n, + target_e, target_d, get_type_str().c_str(), + get_lifetime_str().c_str()); } virtual void mutate(Command *new_command) override { @@ -378,27 +379,33 @@ class OffboardNode : public rclcpp::Node { desired_heading = (desired_heading >= 0) ? desired_heading : 360. + desired_heading; double current_heading = node.state.yaw_deg; - std::cout << "Desired : " << desired_heading << ", Current : " << current_heading << std::endl; + std::cout << "Desired : " << desired_heading + << ", Current : " << current_heading << std::endl; if (smallestAngle(desired_heading, current_heading) < 5.0) { return true; } if (!yaw_fixing_command_sent) { - yaw_fixing_command_sent = - send_offboard_goto(node, node.state.n, node.state.e, node.state.d, desired_heading); + yaw_fixing_command_sent = send_offboard_goto( + node, node.state.n, node.state.e, node.state.d, desired_heading); } return false; } - /* A function which sends the waypoint and transition commands. Returns true - * when those commands are successfully sent - * */ - bool send_waypoint_transition(OffboardNode &node) { + bool send_waypoint(OffboardNode &node) { if (!goto_command_sent) { goto_command_sent = send_offboard_goto(node, target_n, target_e, target_d, NAN); } + return goto_command_sent; + } + + /* A function which sends the waypoint and transition commands. Returns true + * when those commands are successfully sent + * */ + bool send_waypoint_transition(OffboardNode &node) { + goto_command_sent = send_waypoint(node); if (!transition_command_sent) { auto res = node.mavsdk_action->transition_to_fixedwing(); transition_command_sent = (res == mavsdk::Action::Result::Success); @@ -444,21 +451,28 @@ class OffboardNode : public rclcpp::Node { } } - // TODO: FINISH DOING THIS bool starting_fw_flight(OffboardNode &node) { // FIX HEADING // ONCE FIXED SET WAYPOINT THEN TRANSITION // BOOL -> RETURN TRUE WHEN REACHED switch (current_stage) { case LifetimeStage::BEGIN: + if (fix_heading(node)) + current_stage = LifetimeStage::ONGOING; + return false; break; case LifetimeStage::ONGOING: + if (send_waypoint_transition(node)) + current_stage = LifetimeStage::STOP; + return false; break; case LifetimeStage::STOP: // if () return true; + if (is_within_from_target(node, 5.)) + return true; + return false; break; } - return false; } bool middle_fw_flight(OffboardNode &node) { @@ -466,11 +480,19 @@ class OffboardNode : public rclcpp::Node { // BOOL -> RETURN TRUE WHEN REACHED switch (current_stage) { case LifetimeStage::BEGIN: + current_stage = LifetimeStage::ONGOING; + return false; break; case LifetimeStage::ONGOING: + if (send_waypoint(node)) + current_stage = LifetimeStage::STOP; + return false; break; case LifetimeStage::STOP: // if () return true; + if (is_within_from_target(node, 5.)) + return true; + return false; break; } return false; @@ -482,11 +504,20 @@ class OffboardNode : public rclcpp::Node { // BOOL -> RETURN TRUE WHEN IN QC AND REACHED switch (current_stage) { case LifetimeStage::BEGIN: + current_stage = LifetimeStage::ONGOING; + return false; break; case LifetimeStage::ONGOING: + if (send_waypoint(node)) + current_stage = LifetimeStage::STOP; + return false; break; case LifetimeStage::STOP: - // if () return true; + if (is_within_from_target(node, 50.) && !untransition_command_sent) + untransition_command_sent = send_untransition(node); + if (is_within_from_target(node, 5.) && untransition_command_sent) + return true; + return false; break; } return false; From e98685c6301e36deb95ec409fb8cfb5b3a171943 Mon Sep 17 00:00:00 2001 From: UnHappySquid Date: Wed, 12 Feb 2025 23:15:55 -0500 Subject: [PATCH 8/9] Getting altitude loss on untransition, needs some tweking still :( --- drone_test.sh | 4 ++++ drone_test2.sh | 19 ++++++++++++++++++ .../src/px4_controller/src/drone_node.cpp | 20 +++++++++++++++---- 3 files changed, 39 insertions(+), 4 deletions(-) create mode 100644 drone_test2.sh diff --git a/drone_test.sh b/drone_test.sh index 06ad2ad..12ae905 100644 --- a/drone_test.sh +++ b/drone_test.sh @@ -16,5 +16,9 @@ ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" + ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 3}" diff --git a/drone_test2.sh b/drone_test2.sh new file mode 100644 index 0000000..ae18df9 --- /dev/null +++ b/drone_test2.sh @@ -0,0 +1,19 @@ +source drone_ws/install/setup.bash + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 2, takeoff_height: 10.}" + + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 100., e: 100., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 10., e: 10., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 3}" + diff --git a/drone_ws/src/px4_controller/src/drone_node.cpp b/drone_ws/src/px4_controller/src/drone_node.cpp index ec6bb26..08a7349 100644 --- a/drone_ws/src/px4_controller/src/drone_node.cpp +++ b/drone_ws/src/px4_controller/src/drone_node.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -406,9 +407,14 @@ class OffboardNode : public rclcpp::Node { * */ bool send_waypoint_transition(OffboardNode &node) { goto_command_sent = send_waypoint(node); - if (!transition_command_sent) { + double current_speed = + sqrt(pow(node.state.vn, 2) + pow(node.state.ve, 2)); + if (current_speed >= 5. && goto_command_sent && + !transition_command_sent) { auto res = node.mavsdk_action->transition_to_fixedwing(); - transition_command_sent = (res == mavsdk::Action::Result::Success); + transition_command_sent = + (res == mavsdk::Action::Result::Success) && + node.state.vtol_state == custom_msgs::msg::DroneState::FW; } return goto_command_sent && transition_command_sent; } @@ -442,8 +448,11 @@ class OffboardNode : public rclcpp::Node { return false; break; case LifetimeStage::STOP: - if (is_within_from_target(node, 50.) && !untransition_command_sent) + if (is_within_from_target(node, 50.) && !untransition_command_sent) { untransition_command_sent = send_untransition(node); + goto_command_sent = false; + goto_command_sent = send_waypoint(node); + } if (is_within_from_target(node, 5.) && untransition_command_sent) return true; return false; @@ -513,8 +522,11 @@ class OffboardNode : public rclcpp::Node { return false; break; case LifetimeStage::STOP: - if (is_within_from_target(node, 50.) && !untransition_command_sent) + if (is_within_from_target(node, 50.) && !untransition_command_sent) { untransition_command_sent = send_untransition(node); + goto_command_sent = false; + goto_command_sent = send_waypoint(node); + } if (is_within_from_target(node, 5.) && untransition_command_sent) return true; return false; From 9d0ed0954be5c96e7df9b7eed4d7208d739e9e04 Mon Sep 17 00:00:00 2001 From: Black-bird14 Date: Mon, 19 May 2025 20:21:22 -0400 Subject: [PATCH 9/9] New path planner outline --- .../src/mapping_and_planning/CMakeLists.txt | 63 ------ drone_ws/src/mapping_and_planning/LICENSE | 202 ------------------ drone_ws/src/mapping_and_planning/README.md | 13 -- .../include/mapping_and_planning/mapping.hpp | 43 ---- .../src/mapping_and_planning/msg/Obstacle.msg | 6 - .../msg/ObstacleArray.msg | 1 - drone_ws/src/mapping_and_planning/package.xml | 26 --- .../src/mapping_and_planning/src/mapping.cpp | 86 -------- drone_ws/src/planner/LICENSE | 17 ++ drone_ws/src/planner/package.xml | 18 ++ drone_ws/src/planner/planner/__init__.py | 0 drone_ws/src/planner/planner/path_planner.py | 108 ++++++++++ drone_ws/src/planner/resource/planner | 0 drone_ws/src/planner/setup.cfg | 4 + drone_ws/src/planner/setup.py | 25 +++ drone_ws/src/planner/test/test_copyright.py | 25 +++ drone_ws/src/planner/test/test_flake8.py | 25 +++ drone_ws/src/planner/test/test_pep257.py | 23 ++ 18 files changed, 245 insertions(+), 440 deletions(-) delete mode 100644 drone_ws/src/mapping_and_planning/CMakeLists.txt delete mode 100644 drone_ws/src/mapping_and_planning/LICENSE delete mode 100644 drone_ws/src/mapping_and_planning/README.md delete mode 100644 drone_ws/src/mapping_and_planning/include/mapping_and_planning/mapping.hpp delete mode 100644 drone_ws/src/mapping_and_planning/msg/Obstacle.msg delete mode 100644 drone_ws/src/mapping_and_planning/msg/ObstacleArray.msg delete mode 100644 drone_ws/src/mapping_and_planning/package.xml delete mode 100644 drone_ws/src/mapping_and_planning/src/mapping.cpp create mode 100644 drone_ws/src/planner/LICENSE create mode 100644 drone_ws/src/planner/package.xml create mode 100644 drone_ws/src/planner/planner/__init__.py create mode 100644 drone_ws/src/planner/planner/path_planner.py create mode 100644 drone_ws/src/planner/resource/planner create mode 100644 drone_ws/src/planner/setup.cfg create mode 100644 drone_ws/src/planner/setup.py create mode 100644 drone_ws/src/planner/test/test_copyright.py create mode 100644 drone_ws/src/planner/test/test_flake8.py create mode 100644 drone_ws/src/planner/test/test_pep257.py diff --git a/drone_ws/src/mapping_and_planning/CMakeLists.txt b/drone_ws/src/mapping_and_planning/CMakeLists.txt deleted file mode 100644 index 8c47947..0000000 --- a/drone_ws/src/mapping_and_planning/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mapping_and_planning) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(octomap REQUIRED) -find_package(std_msgs REQUIRED) -find_package (Eigen3 3.3 REQUIRED NO_MODULE) -find_package(rosidl_default_generators REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(mapping_and_planning REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Obstacle.msg" - "msg/ObstacleArray.msg" - ) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - - -add_executable(map_n_plan src/mapping.cpp) -# Fix double namespace issue -set(_generated_files_dir "${CMAKE_BINARY_DIR}/rosidl_generator_cpp") -target_include_directories(map_n_plan PUBLIC - ${_generated_files_dir} -) -# set(ENV{octomap_DIR} /home/unhappysquid/octomap/) -include_directories(${OCTOMAP_INCLUDE_DIRS}) -link_directories(${OCTOMAP_LIBRARY_DIRS}) -link_libraries(${OCTOMAP_LIBRARIES}) - - -target_link_libraries(map_n_plan octomap Eigen3::Eigen) -ament_target_dependencies(map_n_plan rclcpp std_msgs octomap geometry_msgs mapping_and_planning) - -target_include_directories(map_n_plan PUBLIC - $ - $) - -target_compile_features(map_n_plan PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - -install(TARGETS map_n_plan - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/drone_ws/src/mapping_and_planning/LICENSE b/drone_ws/src/mapping_and_planning/LICENSE deleted file mode 100644 index d645695..0000000 --- a/drone_ws/src/mapping_and_planning/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/drone_ws/src/mapping_and_planning/README.md b/drone_ws/src/mapping_and_planning/README.md deleted file mode 100644 index 206c1b6..0000000 --- a/drone_ws/src/mapping_and_planning/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# Mapping and planning package -Contains nodes relevant for mapping and planning -## Dependency -Refer to the `README.md` under the workspace directory (`../..`) - - - - - - - - - diff --git a/drone_ws/src/mapping_and_planning/include/mapping_and_planning/mapping.hpp b/drone_ws/src/mapping_and_planning/include/mapping_and_planning/mapping.hpp deleted file mode 100644 index e96a620..0000000 --- a/drone_ws/src/mapping_and_planning/include/mapping_and_planning/mapping.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef MAPPING_HPP -#define MAPPING_HPP - -#include -#include -#include -#include -#include -#include -#include - -#include "Eigen/Dense" -#include "octomap/OcTree.h" -#include "octomap/octomap.h" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" -#include "geometry_msgs/msg/polygon.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "mapping_and_planning/msg/obstacle.hpp" -#include "mapping_and_planning/msg/obstacle_array.hpp" - -using namespace std::chrono_literals; -using std::placeholders::_1; -using namespace std; -using namespace Eigen; - -class Planner : public rclcpp::Node{ -public: - Planner(); - octomap::OcTree build_octomap(std::vector boundary, - std::vector detections); -private: - void boundaryCallback(const geometry_msgs::msg::Polygon::SharedPtr msg); - void obstacleCallback(const mapping_and_planning::msg::ObstacleArray::SharedPtr obs); - vector detections_; - vector boundaries_; - MatrixXd drone_frame_; - double octomap_granularity_; - rclcpp::Subscription::SharedPtr bounds_sub_; - rclcpp::Subscription::SharedPtr obs_sub_; -}; - -#endif // MAPPING_HPP \ No newline at end of file diff --git a/drone_ws/src/mapping_and_planning/msg/Obstacle.msg b/drone_ws/src/mapping_and_planning/msg/Obstacle.msg deleted file mode 100644 index b234162..0000000 --- a/drone_ws/src/mapping_and_planning/msg/Obstacle.msg +++ /dev/null @@ -1,6 +0,0 @@ -#The detected obtsacles will be sent in terms of lon (deg), lat(deg), distance and radius -float64 lon -float64 lat -float64 dist -float64 radius - diff --git a/drone_ws/src/mapping_and_planning/msg/ObstacleArray.msg b/drone_ws/src/mapping_and_planning/msg/ObstacleArray.msg deleted file mode 100644 index 542c379..0000000 --- a/drone_ws/src/mapping_and_planning/msg/ObstacleArray.msg +++ /dev/null @@ -1 +0,0 @@ -mapping_and_planning/Obstacle[] obstacles \ No newline at end of file diff --git a/drone_ws/src/mapping_and_planning/package.xml b/drone_ws/src/mapping_and_planning/package.xml deleted file mode 100644 index ca31b1e..0000000 --- a/drone_ws/src/mapping_and_planning/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - mapping_and_planning - 0.0.0 - TODO: Package description - unhappysquid - Apache-2.0 - - ament_cmake - - ament_lint_auto - ament_lint_common - - octomap - eigen - geometry_msgs - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/drone_ws/src/mapping_and_planning/src/mapping.cpp b/drone_ws/src/mapping_and_planning/src/mapping.cpp deleted file mode 100644 index cc99e7d..0000000 --- a/drone_ws/src/mapping_and_planning/src/mapping.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "mapping_and_planning/mapping.hpp" - - - -Planner::Planner() - : Node("planner"), drone_frame_(4, 4), octomap_granularity_(3.) { - this->drone_frame_(3, 3) = 1.; - Quaterniond quat(1, 0, 0, 0); - auto rotation = quat.matrix(); - Vector3d translation(10, 20, 30); - this->drone_frame_.block<3, 3>(0, 0) = rotation; - this->drone_frame_.block<3, 1>(0, 3) = translation; - - bounds_sub_ = this->create_subscription( - "mission_boundary", 10, bind(&Planner::boundaryCallback, this, _1)); - - obs_sub_ = this->create_subscription( - "detection", 10, std::bind(&Planner::obstacleCallback, this, _1)); - cout << this->drone_frame_ << endl; - - // Example boundary and detection, refer to the mapping docs for how the map looks like - vector boundary{{-10, 30}, {20, 30}, {20, 10}, - {30, 10}, {30, 0}, {-30, 0}, - {-30, 10}, {-10, 10}}; - - // First detection should be right above the drone, 5 meters away, with radius 2 - // Second on should be to the right of the drone, 5 meters away, with a radius 10. - vector detections{{0.0, M_PI, 5., 2.}, {M_PI, 2*M_PI, 20., 10.}}; - - - octomap::OcTree tree = build_octomap(boundary, detections); - tree.writeBinary("simple_tree.bt"); - cout << "wrote octomap file simple_tree.bt" << endl << endl; - cout << "now you can use octovis to visualize: octovis simple_tree.bt" - << endl; - } - - void Planner::boundaryCallback(const geometry_msgs::msg::Polygon::SharedPtr msg) - { - RCLCPP_INFO(this->get_logger(), "Received a polygon with %ld vertices.", msg->points.size()); - boundaries_.resize(msg->points.size()); - for (size_t i = 0; i < msg->points.size(); ++i) - { - const auto &point = msg->points[i]; - boundaries_[i] = Vector2d(point.x, point.y); - RCLCPP_INFO(this->get_logger(), "Vertex %ld: [x=%.2f, y=%.2f, z=%.2f]", i, point.x, point.y, point.z); - } - } - - void Planner::obstacleCallback(const mapping_and_planning::msg::ObstacleArray::SharedPtr obs){ - RCLCPP_INFO(this->get_logger(), "Received detections."); - detections_.resize(obs->obstacles.size()); - detections_ = obs->obstacles; - } - - octomap::OcTree Planner::build_octomap(std::vector boundary, - std::vector detections) { - octomap::OcTree tree(this->octomap_granularity_); - - // TODO: Fill your code here - // Build the tree, refer to - // https://github.com/OctoMap/octomap/blob/devel/octomap/src/simple_example.cpp - // for an example of how to use octomap The drone's frame of reference is in - // the this->drone_frame matrix (homogenous coords) std::vector is C++'s - // class for an ArrayList, Eigen::Vector2d and Eigen::Vector4d are eigen's - // implementation of linear algebra vectors - // Avoid the keyword `new` or any form of heap utilization, just use the - // stack! If you do use `new`, always remember to use `delete` when - // appropriate to free the memory. - cout << "Boundary :" << endl; - for (auto vec : boundary) { - cout << vec << endl; - } - cout << "Detections :" << endl; - for (auto vec : detections) { - cout << vec << endl; - } - return tree; - } - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/drone_ws/src/planner/LICENSE b/drone_ws/src/planner/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/drone_ws/src/planner/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/drone_ws/src/planner/package.xml b/drone_ws/src/planner/package.xml new file mode 100644 index 0000000..39f367b --- /dev/null +++ b/drone_ws/src/planner/package.xml @@ -0,0 +1,18 @@ + + + + planner + 0.0.0 + TODO: Package description + axia + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/drone_ws/src/planner/planner/__init__.py b/drone_ws/src/planner/planner/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/drone_ws/src/planner/planner/path_planner.py b/drone_ws/src/planner/planner/path_planner.py new file mode 100644 index 0000000..87fd2af --- /dev/null +++ b/drone_ws/src/planner/planner/path_planner.py @@ -0,0 +1,108 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped +from builtin_interfaces.msg import Time +from typing import List, Tuple +from shapely.geometry import Polygon, Point, LineString +import networkx as nx + +class path_planner(Node){ + def __init__(self): + super().__init__('path_planner') + self.boundary_xy: List[Tuple[float, float]] | None = None + self.waypoints_xy: List[Tuple[float, float]] | None = None + self.publisher_ = self.create_publisher(Path, 'planning', 10)#TODO: CHANGE STRING TO WHATEVER i AM MEANT TO OUTPUT + self.boundary_sub = self.create_subscription( + Path,#TODO: SAME HERE + 'mission_boundaries', + self.boundaries_callback, + 10) + self.waypoints_sub = self.create_subscription( + Path, 'mission_waypoints', self.waypoints_callback, 10) + + def boundaries_callback(self, boundaries: Path)-> None: + """Save boundary vertices""" + self.boundary_xy = [(p.pose.position.x, p.pose.position.y) for p in boundaries.poses] + self.get_logger().info(f'Received mission boundaries') + self.plan() + + def waypoints_callback(self, waypoints: Path)-> None: + """Save way‑points""" + self.waypoints_xy = [(p.pose.position.x, p.pose.position.y) for p in waypoints.poses] + self.get_logger().info(f'Received {len(self.waypoints_xy)} way‑points') + self.plan() + + def plan(self)-> None: + if self.boundary_xy is None or self.waypoints_xy is None: + return # haven’t got both yet + try: + path_xy = self.build_graph( + self.boundary_xy, + self.waypoints_xy, + start=(0.0, 0.0) # drone’s take‑off point in mission_origin frame TODO: potentially change that + ) + except Exception as e: + self.get_logger().error(f'Planner failed: {e}') + return + + self.publish_path(path_xy) + + def build_graph(self, + boundary: List[Tuple[float, float]], + waypoints: List[Tuple[float, float]], + start: Tuple[float, float] + )-> List[Tuple[float, float]]: + + """ Build 2D Visibility Graph """ + #TODO: CREATE BOUNDARY POLYGON (SELF.BOUNDARY) WITH SHAPELY + #TODO: ENSURE EACH WP INSIDE POLYGON + #TODO: ASSEMBLE NODE SET + #TODO: BUILD VISIBILITY EDGES WITH SHAPELY + #TODO: BUILD GRAPH WITH NETWORKX + + def publish_path(self, full_path: List[Tuple[float, float]]) -> None: + """ + Convert a list of (x, y) tuples into nav_msgs/Path and publish it. + • Z is fixed to 0.0 + • Orientation is the identity quaternion (w = 1) + """ + path_msg = Path() + path_msg.header.frame_id = 'map' # <- change to your global frame + path_msg.header.stamp = self.get_clock().now().to_msg() + + for (x, y) in full_path: + pose = PoseStamped() + pose.header = path_msg.header # same frame & stamp + + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = 0.0 + + # Identity quaternion (no rotation): (x, y, z, w) = (0, 0, 0, 1) + pose.pose.orientation.w = 1.0 + + path_msg.poses.append(pose) + + self.path_pub.publish(path_msg) + self.get_logger().info(f'Published path with {len(path_msg.poses)} poses') +} + + +def main(args=None): + rclpy.init(args=args) + + node = VisibilityGraphPlanner() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/drone_ws/src/planner/resource/planner b/drone_ws/src/planner/resource/planner new file mode 100644 index 0000000..e69de29 diff --git a/drone_ws/src/planner/setup.cfg b/drone_ws/src/planner/setup.cfg new file mode 100644 index 0000000..2697782 --- /dev/null +++ b/drone_ws/src/planner/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/planner +[install] +install_scripts=$base/lib/planner diff --git a/drone_ws/src/planner/setup.py b/drone_ws/src/planner/setup.py new file mode 100644 index 0000000..9edf8ad --- /dev/null +++ b/drone_ws/src/planner/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'planner' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='axia', + maintainer_email='alexianatenin.ans@gmail.com', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/drone_ws/src/planner/test/test_copyright.py b/drone_ws/src/planner/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/drone_ws/src/planner/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/drone_ws/src/planner/test/test_flake8.py b/drone_ws/src/planner/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/drone_ws/src/planner/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/drone_ws/src/planner/test/test_pep257.py b/drone_ws/src/planner/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/drone_ws/src/planner/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'