From 15e530067e226d9f80d0abdabf72f75485471c91 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 14:24:28 +0000 Subject: [PATCH] remove send setpoints --- src/px4_msgs | 2 +- src/px4_ros_com | 2 +- src/send_setpoints/CMakeLists.txt | 46 --------------- src/send_setpoints/package.xml | 18 ------ src/send_setpoints/src/send_setpoint.cpp | 74 ------------------------ 5 files changed, 2 insertions(+), 140 deletions(-) delete mode 100644 src/send_setpoints/CMakeLists.txt delete mode 100644 src/send_setpoints/package.xml delete mode 100644 src/send_setpoints/src/send_setpoint.cpp diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..4db0a3f1 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit 4db0a3f14ea81b9de7511d738f8ad9bd8ae5b3ad diff --git a/src/px4_ros_com b/src/px4_ros_com index 1562ff30..0bcf68bc 160000 --- a/src/px4_ros_com +++ b/src/px4_ros_com @@ -1 +1 @@ -Subproject commit 1562ff30d56b7ba26e4d2436724490f900cc2375 +Subproject commit 0bcf68bcb635199adcd134e8932932054e863c0d diff --git a/src/send_setpoints/CMakeLists.txt b/src/send_setpoints/CMakeLists.txt deleted file mode 100644 index df05c262..00000000 --- a/src/send_setpoints/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(send_setpoints) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -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) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -find_package(rclcpp REQUIRED) -find_package(px4_ros_com REQUIRED) -find_package(px4_msgs REQUIRED) - -add_executable(send_setpoint src/send_setpoint.cpp) -ament_target_dependencies(send_setpoint rclcpp px4_ros_com px4_msgs) - -install(TARGETS - send_setpoint - DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/send_setpoints/package.xml b/src/send_setpoints/package.xml deleted file mode 100644 index fe5a7c43..00000000 --- a/src/send_setpoints/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - send_setpoints - 0.0.0 - package to send attitude setpoints to pixhawk so it can be armed - ubuntu - Apache License 2.0 - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp deleted file mode 100644 index c1a10371..00000000 --- a/src/send_setpoints/src/send_setpoint.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/* - -We need to send attitude setpoints to be able to arm the drone: -https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET -We need attitude setpoints because we don't have a GPS: -https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-manual-control-lost/31565/9 - -*/ - -#include - -#include "rclcpp/rclcpp.hpp" - -#include -#include - -using namespace std::chrono_literals; - -class SetpointSender : public rclcpp::Node -{ -public: - SetpointSender() : Node("setpoint_sender") - { - // create a publisher on the offboard control mode topic - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - // create timer to send heartbeat messages (offboard control) every 100ms - timer_ = this->create_wall_timer(100ms, std::bind(&SetpointSender::send_heartbeat, this)); - start_time = this->get_clock()->now().seconds(); - } - -private: - -/** - * @brief Publish offboard control mode messages as a heartbeat. - * Only the attitude is enabled, because that is how the drone will be controlled. - * - */ - void send_heartbeat() - { - // set message to enable attitude - auto msg = px4_msgs::msg::OffboardControlMode(); - msg.position = false; - msg.velocity = false; - msg.acceleration = false; - msg.attitude = true; - msg.body_rate = false; - msg.actuator = false; - - // get timestamp and publish message - msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; - offboard_control_mode_publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!"); - - // check if 5 seconds have elapsed - if (this->get_clock()->now().seconds() - start_time > 5) - { - RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!"); - } - - - - } - rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - double start_time; -}; - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -}