From a444ce319377c7d677ebc7d457a2770588e30634 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 20 Apr 2023 13:49:43 +0000 Subject: [PATCH 001/188] add setpoints package --- src/send_setpoints/CMakeLists.txt | 37 +++++++++++++++++++++++++++++++ src/send_setpoints/package.xml | 18 +++++++++++++++ 2 files changed, 55 insertions(+) create mode 100644 src/send_setpoints/CMakeLists.txt create mode 100644 src/send_setpoints/package.xml diff --git a/src/send_setpoints/CMakeLists.txt b/src/send_setpoints/CMakeLists.txt new file mode 100644 index 00000000..e421df23 --- /dev/null +++ b/src/send_setpoints/CMakeLists.txt @@ -0,0 +1,37 @@ +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) + +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 new file mode 100644 index 00000000..fe5a7c43 --- /dev/null +++ b/src/send_setpoints/package.xml @@ -0,0 +1,18 @@ + + + + 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 + + From 68cb8effa9b2a0077d6c69fc026839fc0f6815f2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 20 Apr 2023 15:54:47 +0200 Subject: [PATCH 002/188] add px4 includes --- src/send_setpoints/CMakeLists.txt | 7 +++++++ src/send_setpoints/src/send_setpoint.cpp | 24 ++++++++++++++++++++++++ 2 files changed, 31 insertions(+) create mode 100644 src/send_setpoints/src/send_setpoint.cpp diff --git a/src/send_setpoints/CMakeLists.txt b/src/send_setpoints/CMakeLists.txt index e421df23..b42810f4 100644 --- a/src/send_setpoints/CMakeLists.txt +++ b/src/send_setpoints/CMakeLists.txt @@ -23,6 +23,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(px4_ros_com REQUIRED) +add_executable(send_setpoints src/send_setpoints.cpp) +ament_target_dependencies(send_setpoints rclcpp px4_ros_com) + +install(TARGETS + send_setpoints + DESTINATION lib/${PROJECT_NAME} + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp new file mode 100644 index 00000000..1cf68f09 --- /dev/null +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -0,0 +1,24 @@ +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + +class SetpointSender : public rclcpp::Node +{ + public: + SetpointSender() : Node("setpoint_sender") + { + + } +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 263f1c154e043bba76274abc1cdd5442219a76f7 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 20 Apr 2023 15:56:18 +0200 Subject: [PATCH 003/188] comment --- src/send_setpoints/src/send_setpoint.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp index 1cf68f09..0e837fa6 100644 --- a/src/send_setpoints/src/send_setpoint.cpp +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -1,3 +1,12 @@ +/* + +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 "rclcpp/rclcpp.hpp" #include From 615d026eb60dfaf1110fe65e9c47b7e68fdec5dd Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 20 Apr 2023 15:58:01 +0200 Subject: [PATCH 004/188] fix typo --- src/send_setpoints/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/send_setpoints/CMakeLists.txt b/src/send_setpoints/CMakeLists.txt index b42810f4..fbe77080 100644 --- a/src/send_setpoints/CMakeLists.txt +++ b/src/send_setpoints/CMakeLists.txt @@ -29,6 +29,7 @@ ament_target_dependencies(send_setpoints rclcpp px4_ros_com) install(TARGETS send_setpoints DESTINATION lib/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) From be267f2aff4d1843d93b58ca54d604df7070c311 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 20 Apr 2023 15:59:16 +0200 Subject: [PATCH 005/188] fix typo --- src/send_setpoints/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/send_setpoints/CMakeLists.txt b/src/send_setpoints/CMakeLists.txt index fbe77080..79d4b524 100644 --- a/src/send_setpoints/CMakeLists.txt +++ b/src/send_setpoints/CMakeLists.txt @@ -23,7 +23,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(px4_ros_com REQUIRED) -add_executable(send_setpoints src/send_setpoints.cpp) +add_executable(send_setpoint src/send_setpoint.cpp) ament_target_dependencies(send_setpoints rclcpp px4_ros_com) install(TARGETS From a1b778b74b1cedda6f1d7770255bb1dcf74a0433 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 20 Apr 2023 16:00:09 +0200 Subject: [PATCH 006/188] fix typo --- src/send_setpoints/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/send_setpoints/CMakeLists.txt b/src/send_setpoints/CMakeLists.txt index 79d4b524..42a1fe8c 100644 --- a/src/send_setpoints/CMakeLists.txt +++ b/src/send_setpoints/CMakeLists.txt @@ -24,10 +24,10 @@ find_package(rclcpp REQUIRED) find_package(px4_ros_com REQUIRED) add_executable(send_setpoint src/send_setpoint.cpp) -ament_target_dependencies(send_setpoints rclcpp px4_ros_com) +ament_target_dependencies(send_setpoint rclcpp px4_ros_com) install(TARGETS - send_setpoints + send_setpoint DESTINATION lib/${PROJECT_NAME} ) From 91354beb0b05152c5779ab2637c75149d401f886 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 24 Apr 2023 09:18:37 +0000 Subject: [PATCH 007/188] stuff --- src/send_setpoints/CMakeLists.txt | 3 ++- src/send_setpoints/src/send_setpoint.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/send_setpoints/CMakeLists.txt b/src/send_setpoints/CMakeLists.txt index 42a1fe8c..df05c262 100644 --- a/src/send_setpoints/CMakeLists.txt +++ b/src/send_setpoints/CMakeLists.txt @@ -22,9 +22,10 @@ find_package(ament_cmake REQUIRED) # 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) +ament_target_dependencies(send_setpoint rclcpp px4_ros_com px4_msgs) install(TARGETS send_setpoint diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp index 0e837fa6..3a85d8a9 100644 --- a/src/send_setpoints/src/send_setpoint.cpp +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -22,12 +22,12 @@ class SetpointSender : public rclcpp::Node { } -} +}; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); +// rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From 5c4d47b590883bb381416cf1a8aa29e2af0ee4b0 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 11:53:49 +0000 Subject: [PATCH 008/188] add message includes for sending offboard control mode messages --- .vscode/c_cpp_properties.json | 3 ++- src/send_setpoints/src/send_setpoint.cpp | 4 +--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 2ce1dddf..6743b427 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -11,7 +11,8 @@ "/usr/include/**", "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/rtls_driver/include/**", "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/serial_communication/include/**", - "/mnt/Homework/Avans/AFSTUDEERSTAGE/terabee_api/include/**" + "/mnt/Homework/Avans/AFSTUDEERSTAGE/terabee_api/include/**", + "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp index 3a85d8a9..113c9346 100644 --- a/src/send_setpoints/src/send_setpoint.cpp +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -10,10 +10,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include "rclcpp/rclcpp.hpp" #include -#include +#include #include -#include -#include class SetpointSender : public rclcpp::Node { From c748bc5da807963389ba998624a586b5bd4c7fee Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 11:58:21 +0000 Subject: [PATCH 009/188] add timer and publisher --- src/send_setpoints/src/send_setpoint.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp index 113c9346..c3f7e382 100644 --- a/src/send_setpoints/src/send_setpoint.cpp +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -15,17 +15,26 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- class SetpointSender : public rclcpp::Node { - public: +public: SetpointSender() : Node("setpoint_sender") { - + offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); } + +private: + + void send_heartbeat() + { + + } + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + rclcpp::TimerBase::SharedPtr timer_; }; int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); -// rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + // rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } From f53385ae0a11d0d30bf86972a65a67d667866d01 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 14:08:42 +0200 Subject: [PATCH 010/188] add sending offboardcontrolmode message --- src/px4_msgs | 2 +- src/px4_ros_com | 2 +- src/send_setpoints/src/send_setpoint.cpp | 28 +++++++++++++++++++++--- 3 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/px4_msgs b/src/px4_msgs index 4db0a3f1..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit 4db0a3f14ea81b9de7511d738f8ad9bd8ae5b3ad +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 diff --git a/src/px4_ros_com b/src/px4_ros_com index 0bcf68bc..1562ff30 160000 --- a/src/px4_ros_com +++ b/src/px4_ros_com @@ -1 +1 @@ -Subproject commit 0bcf68bcb635199adcd134e8932932054e863c0d +Subproject commit 1562ff30d56b7ba26e4d2436724490f900cc2375 diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp index c3f7e382..6f96478d 100644 --- a/src/send_setpoints/src/send_setpoint.cpp +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -7,25 +7,47 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- */ +#include + #include "rclcpp/rclcpp.hpp" #include #include #include +using namespace std::chrono_literals; + class SetpointSender : public rclcpp::Node { public: SetpointSender() : Node("setpoint_sender") { - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + // 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)); } 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() { - + 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; + + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + offboard_control_mode_publisher_->publish(msg); + RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!"); } rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -34,7 +56,7 @@ private: int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - // rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } From 5ea8d5674f0377c7f872729986f7b231f0ef6d26 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 14:24:56 +0200 Subject: [PATCH 011/188] finish sending offboard control and check if 5 seconds elapsed --- src/send_setpoints/src/send_setpoint.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp index 6f96478d..93ecff47 100644 --- a/src/send_setpoints/src/send_setpoint.cpp +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -26,6 +26,7 @@ public: 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: @@ -37,6 +38,7 @@ private: */ void send_heartbeat() { + // set message to enable attitude auto msg = px4_msgs::msg::OffboardControlMode(); msg.position = false; msg.velocity = false; @@ -45,12 +47,23 @@ private: 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[]) From 9ace6a70b399b3a9c16833e3e2b298c83dcf3fcf Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 13:52:47 +0000 Subject: [PATCH 012/188] add heartbeat package --- px4_connection/CMakeLists.txt | 45 ++++++++++++++++++++++++++++++++ px4_connection/package.xml | 20 ++++++++++++++ px4_connection/src/heartbeat.cpp | 10 +++++++ src/px4_msgs | 2 +- src/px4_ros_com | 2 +- 5 files changed, 77 insertions(+), 2 deletions(-) create mode 100644 px4_connection/CMakeLists.txt create mode 100644 px4_connection/package.xml create mode 100644 px4_connection/src/heartbeat.cpp diff --git a/px4_connection/CMakeLists.txt b/px4_connection/CMakeLists.txt new file mode 100644 index 00000000..874483cc --- /dev/null +++ b/px4_connection/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_connection) + +# 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) +find_package(rclcpp REQUIRED) + +add_executable(heartbeat src/heartbeat.cpp) +target_include_directories(heartbeat PUBLIC + $ + $) +ament_target_dependencies( + heartbeat + "rclcpp" +) + +install(TARGETS heartbeat + 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/px4_connection/package.xml b/px4_connection/package.xml new file mode 100644 index 00000000..4a804039 --- /dev/null +++ b/px4_connection/package.xml @@ -0,0 +1,20 @@ + + + + px4_connection + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/px4_connection/src/heartbeat.cpp b/px4_connection/src/heartbeat.cpp new file mode 100644 index 00000000..5d36d006 --- /dev/null +++ b/px4_connection/src/heartbeat.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world px4_connection package\n"); + return 0; +} 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 From 010559ea9bc7bf13f020e8ba561f02ae634b7d73 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 13:56:48 +0000 Subject: [PATCH 013/188] put px4_connection package in src folder --- {px4_connection => src/px4_connection}/CMakeLists.txt | 0 {px4_connection => src/px4_connection}/package.xml | 0 {px4_connection => src/px4_connection}/src/heartbeat.cpp | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename {px4_connection => src/px4_connection}/CMakeLists.txt (100%) rename {px4_connection => src/px4_connection}/package.xml (100%) rename {px4_connection => src/px4_connection}/src/heartbeat.cpp (100%) diff --git a/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt similarity index 100% rename from px4_connection/CMakeLists.txt rename to src/px4_connection/CMakeLists.txt diff --git a/px4_connection/package.xml b/src/px4_connection/package.xml similarity index 100% rename from px4_connection/package.xml rename to src/px4_connection/package.xml diff --git a/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp similarity index 100% rename from px4_connection/src/heartbeat.cpp rename to src/px4_connection/src/heartbeat.cpp From 8b1790f763b89976fa80af5f336dc66340c96220 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:15:03 +0200 Subject: [PATCH 014/188] change CMakeLists.txt of heartbeat to include packages for ros --- src/px4_connection/CMakeLists.txt | 6 +- src/px4_connection/package.xml | 4 +- src/px4_connection/src/heartbeat.cpp | 76 ++++++++++++++++++++++-- src/px4_msgs | 2 +- src/px4_ros_com | 2 +- src/send_setpoints/src/send_setpoint.cpp | 1 - 6 files changed, 79 insertions(+), 12 deletions(-) diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index 874483cc..8153e79c 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -18,6 +18,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(px4_ros_com REQUIRED) +find_package(px4_msgs REQUIRED) add_executable(heartbeat src/heartbeat.cpp) target_include_directories(heartbeat PUBLIC @@ -25,7 +27,9 @@ target_include_directories(heartbeat PUBLIC $) ament_target_dependencies( heartbeat - "rclcpp" + rclcpp + px4_ros_com + px4_msgs ) install(TARGETS heartbeat diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml index 4a804039..e0eaf074 100644 --- a/src/px4_connection/package.xml +++ b/src/px4_connection/package.xml @@ -3,9 +3,9 @@ px4_connection 0.0.0 - TODO: Package description + Package to communicate with PX4 through the XRCE-DDS bridge ubuntu - TODO: License declaration + Apache License 2.0 ament_cmake diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 5d36d006..172c5359 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -1,10 +1,74 @@ -#include +/* -int main(int argc, char ** argv) +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 HeartBeat : public rclcpp::Node { - (void) argc; - (void) argv; +public: + HeartBeat() : 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(&HeartBeat::send_heartbeat, this)); + start_time = this->get_clock()->now().seconds(); + } - printf("hello world px4_connection package\n"); - return 0; +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; } diff --git a/src/px4_msgs b/src/px4_msgs index 4db0a3f1..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit 4db0a3f14ea81b9de7511d738f8ad9bd8ae5b3ad +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 diff --git a/src/px4_ros_com b/src/px4_ros_com index 0bcf68bc..1562ff30 160000 --- a/src/px4_ros_com +++ b/src/px4_ros_com @@ -1 +1 @@ -Subproject commit 0bcf68bcb635199adcd134e8932932054e863c0d +Subproject commit 1562ff30d56b7ba26e4d2436724490f900cc2375 diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp index 93ecff47..c1a10371 100644 --- a/src/send_setpoints/src/send_setpoint.cpp +++ b/src/send_setpoints/src/send_setpoint.cpp @@ -12,7 +12,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include "rclcpp/rclcpp.hpp" #include -#include #include using namespace std::chrono_literals; 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 015/188] 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; -} From b5daa4a77a11681e9a7f91abc5a6f9d34dc94472 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 14:32:19 +0000 Subject: [PATCH 016/188] add px4 controller cmakelists --- src/px4_connection/CMakeLists.txt | 9 ++- src/px4_connection/src/px4_controller.cpp | 74 +++++++++++++++++++++++ 2 files changed, 82 insertions(+), 1 deletion(-) create mode 100644 src/px4_connection/src/px4_controller.cpp diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index 8153e79c..6d581bd9 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -22,9 +22,16 @@ find_package(px4_ros_com REQUIRED) find_package(px4_msgs REQUIRED) add_executable(heartbeat src/heartbeat.cpp) +add_executable(px4_controller src/px4_controller.cpp) + target_include_directories(heartbeat PUBLIC $ $) + +target_include_directories(px4_controller PUBLIC + $ + $) + ament_target_dependencies( heartbeat rclcpp @@ -32,7 +39,7 @@ ament_target_dependencies( px4_msgs ) -install(TARGETS heartbeat +install(TARGETS heartbeat px4_controller DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp new file mode 100644 index 00000000..172c5359 --- /dev/null +++ b/src/px4_connection/src/px4_controller.cpp @@ -0,0 +1,74 @@ +/* + +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 HeartBeat : public rclcpp::Node +{ +public: + HeartBeat() : 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(&HeartBeat::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; +} From 5b6c097bd9ffa257fa68d1b11d790e1be70f5ede Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:34:05 +0200 Subject: [PATCH 017/188] update CMakeLists.txt to be similar to object detection --- src/px4_connection/CMakeLists.txt | 19 +++++++++++++------ src/px4_msgs | 2 +- src/px4_ros_com | 2 +- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index 6d581bd9..df58848f 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -22,7 +22,20 @@ find_package(px4_ros_com REQUIRED) find_package(px4_msgs REQUIRED) add_executable(heartbeat src/heartbeat.cpp) +ament_target_dependencies( + heartbeat + rclcpp + px4_ros_com + px4_msgs +) + add_executable(px4_controller src/px4_controller.cpp) +ament_target_dependencies( + px4_controller + rclcpp + px4_ros_com + px4_msgs +) target_include_directories(heartbeat PUBLIC $ @@ -32,12 +45,6 @@ target_include_directories(px4_controller PUBLIC $ $) -ament_target_dependencies( - heartbeat - rclcpp - px4_ros_com - px4_msgs -) install(TARGETS heartbeat px4_controller DESTINATION lib/${PROJECT_NAME}) diff --git a/src/px4_msgs b/src/px4_msgs index 4db0a3f1..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit 4db0a3f14ea81b9de7511d738f8ad9bd8ae5b3ad +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 diff --git a/src/px4_ros_com b/src/px4_ros_com index 0bcf68bc..1562ff30 160000 --- a/src/px4_ros_com +++ b/src/px4_ros_com @@ -1 +1 @@ -Subproject commit 0bcf68bcb635199adcd134e8932932054e863c0d +Subproject commit 1562ff30d56b7ba26e4d2436724490f900cc2375 From 43f383897966743241c5b815839c710a63de0bdb Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:39:15 +0200 Subject: [PATCH 018/188] add vehicle attitude setpoint publisher --- src/px4_connection/src/px4_controller.cpp | 38 +++++++---------------- 1 file changed, 11 insertions(+), 27 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 172c5359..a6a00fa7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -11,21 +11,20 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include "rclcpp/rclcpp.hpp" -#include +#include #include using namespace std::chrono_literals; -class HeartBeat : public rclcpp::Node +class PX4Controller : public rclcpp::Node { public: - HeartBeat() : Node("setpoint_sender") + PX4Controller() : Node("px4_controller") { - // 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(&HeartBeat::send_heartbeat, this)); - start_time = this->get_clock()->now().seconds(); + // create a publisher on the vehicle attitude setpoint topic + offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); + // create timer to send vehicle attitude setpoints every second + timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); } private: @@ -35,40 +34,25 @@ private: * Only the attitude is enabled, because that is how the drone will be controlled. * */ - void send_heartbeat() + void send_setpoint() { // 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; + auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); // 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::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::spin(std::make_shared()); rclcpp::shutdown(); return 0; } From a12d1836febc6747485da30d206f91deb8aca9fb Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:45:01 +0200 Subject: [PATCH 019/188] add filling message with test values --- src/px4_connection/src/px4_controller.cpp | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index a6a00fa7..cec78c4c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -40,10 +40,29 @@ private: auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); // get timestamp and publish message + + //https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg + msg.roll_body = 1; + msg.pitch_body = 1; + msg.yaw_body = 1; + + msg.yaw_sp_move_rate = 1; + + for (int i = 0; i < 4; i++) + { + msg.q_d[i] = 1; + } + + msg.thrust_body[0] = 0; + msg.thrust_body[1] = 0; + msg.thrust_body[2] = -10; // negative throttle amount + msg.reset_integral = false; + msg.fw_control_yaw_wheel = false; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + publisher_->publish(msg); - - + RCLCPP_INFO(this->get_logger(), "published message"); } rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; From 4540a24d85daf938548bb2df3d29a0969ba45658 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:46:28 +0200 Subject: [PATCH 020/188] fix name --- src/px4_connection/src/px4_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index cec78c4c..250ba29a 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -22,7 +22,7 @@ public: PX4Controller() : Node("px4_controller") { // create a publisher on the vehicle attitude setpoint topic - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); + vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); } @@ -60,11 +60,11 @@ private: msg.fw_control_yaw_wheel = false; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; - publisher_->publish(msg); + vehicle_setpoint_publisher_->publish(msg); RCLCPP_INFO(this->get_logger(), "published message"); } - rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::TimerBase::SharedPtr timer_; }; From 455c4956852fe1c063c8a27da0f5e25f6d0bf988 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:53:40 +0200 Subject: [PATCH 021/188] add setting to offboard mode and arming --- src/px4_connection/src/px4_controller.cpp | 49 +++++++++++++++++++---- 1 file changed, 41 insertions(+), 8 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 250ba29a..64ed0358 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -13,6 +13,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include +#include +#include using namespace std::chrono_literals; @@ -23,17 +25,29 @@ public: { // create a publisher on the vehicle attitude setpoint topic vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); + vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); + + // set to offboard mode + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); + RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); + // arm the drone + publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); + + RCLCPP_INFO(this->get_logger(), "Arm command sent"); + // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); } private: + rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; + rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + rclcpp::TimerBase::SharedPtr timer_; -/** - * @brief Publish offboard control mode messages as a heartbeat. - * Only the attitude is enabled, because that is how the drone will be controlled. - * - */ + /** + * @brief Only the attitude is enabled, because that is how the drone will be controlled. + * + */ void send_setpoint() { // set message to enable attitude @@ -41,7 +55,7 @@ private: // get timestamp and publish message - //https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg + // https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg msg.roll_body = 1; msg.pitch_body = 1; msg.yaw_body = 1; @@ -64,8 +78,27 @@ private: RCLCPP_INFO(this->get_logger(), "published message"); } - rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; - rclcpp::TimerBase::SharedPtr timer_; + + /** + * @brief Publish vehicle commands + * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) + * @param param1 Command parameter 1 + * @param param2 Command parameter 2 + */ + void publish_vehicle_command(uint16_t command, float param1, float param2) + { + px4_msgs::msg::VehicleCommand msg{}; + msg.param1 = param1; + msg.param2 = param2; + msg.command = command; + msg.target_system = 1; + msg.target_component = 1; + msg.source_system = 1; + msg.source_component = 1; + msg.from_external = true; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + vehicle_command_publisher_->publish(msg); + } }; int main(int argc, char *argv[]) From fd2ace29ced2a85d4901a4ba84ba249508b4d7a2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:54:57 +0200 Subject: [PATCH 022/188] typo --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 64ed0358..8e0a80f4 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -25,13 +25,13 @@ public: { // create a publisher on the vehicle attitude setpoint topic vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); - vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); + vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); // set to offboard mode this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); // arm the drone - publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); RCLCPP_INFO(this->get_logger(), "Arm command sent"); From 78ac1963df64b175a780607d825ced28d31280ab Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:55:55 +0200 Subject: [PATCH 023/188] typo v2 --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 8e0a80f4..e31c6be5 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -25,7 +25,7 @@ public: { // create a publisher on the vehicle attitude setpoint topic vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); - vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); + vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); // set to offboard mode this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); From 4ed6dde80e1a1505615971f699a11cae4ee3e44f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 16:58:10 +0200 Subject: [PATCH 024/188] typo v3 --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e31c6be5..c64bae08 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -31,7 +31,7 @@ public: this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); // arm the drone - publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0,0); RCLCPP_INFO(this->get_logger(), "Arm command sent"); From 5496e400cf0556816a7bb770d1b455c065693798 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 25 Apr 2023 15:45:14 +0000 Subject: [PATCH 025/188] it finally works --- src/px4_connection/src/px4_controller.cpp | 2 +- src/px4_msgs | 2 +- src/px4_ros_com | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index c64bae08..bfc74062 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -69,7 +69,7 @@ private: msg.thrust_body[0] = 0; msg.thrust_body[1] = 0; - msg.thrust_body[2] = -10; // negative throttle amount + msg.thrust_body[2] = -1; // negative throttle amount msg.reset_integral = false; msg.fw_control_yaw_wheel = false; 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 From 6bdb10dd5c8f5fa4c8080b3605acd2bce6119fa7 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 12:45:27 +0200 Subject: [PATCH 026/188] change vehicle attitude setpoint to trajectory setpoint for velocity --- src/px4_connection/src/heartbeat.cpp | 4 +-- src/px4_connection/src/px4_controller.cpp | 30 +++++++++-------------- 2 files changed, 14 insertions(+), 20 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 172c5359..587e3f37 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -40,9 +40,9 @@ private: // set message to enable attitude auto msg = px4_msgs::msg::OffboardControlMode(); msg.position = false; - msg.velocity = false; + msg.velocity = true; msg.acceleration = false; - msg.attitude = true; + msg.attitude = false; msg.body_rate = false; msg.actuator = false; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index c64bae08..5a5a5196 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -8,10 +8,12 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- */ #include +#include #include "rclcpp/rclcpp.hpp" #include +#include #include #include #include @@ -26,6 +28,7 @@ public: // create a publisher on the vehicle attitude setpoint topic vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); + trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint",10); // set to offboard mode this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); @@ -41,6 +44,7 @@ public: private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; + rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -51,30 +55,20 @@ private: void send_setpoint() { // set message to enable attitude - auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); + auto msg = px4_msgs::msg::TrajectorySetpoint(); // get timestamp and publish message - // https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg - msg.roll_body = 1; - msg.pitch_body = 1; - msg.yaw_body = 1; + // https://github.com/PX4/px4_msgs/blob/main/msg/TrajectorySetpoint.msg + msg.velocity[0] = 0; // north + msg.velocity[1] = 0; // east + msg.velocity[2] = 1.0; // down (1 m/s -> TODO test if this accounts for 9.81 m/s earth gravity) - msg.yaw_sp_move_rate = 1; - - for (int i = 0; i < 4; i++) - { - msg.q_d[i] = 1; - } - - msg.thrust_body[0] = 0; - msg.thrust_body[1] = 0; - msg.thrust_body[2] = -10; // negative throttle amount - msg.reset_integral = false; - msg.fw_control_yaw_wheel = false; + msg.yaw = (0f * M_PI) / 180f; // 0 degrees rotation of yaw + msg.yawspeed = 0; // 0 rotation speed msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; - vehicle_setpoint_publisher_->publish(msg); + trajectory_setpoint_publisher->publish(msg); RCLCPP_INFO(this->get_logger(), "published message"); } From e3466a1077fc76b3aab7a538fcad12ed89931bdf Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 13:39:18 +0200 Subject: [PATCH 027/188] change float f to .0 --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 5a5a5196..9657f983 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -64,7 +64,7 @@ private: msg.velocity[1] = 0; // east msg.velocity[2] = 1.0; // down (1 m/s -> TODO test if this accounts for 9.81 m/s earth gravity) - msg.yaw = (0f * M_PI) / 180f; // 0 degrees rotation of yaw + msg.yaw = (0.0 * M_PI) / 180.0; // 0 degrees rotation of yaw msg.yawspeed = 0; // 0 rotation speed msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; From 388963511a94b8b909760f2f845b2cff4763402f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 14:47:36 +0200 Subject: [PATCH 028/188] test with 0 values --- src/px4_connection/src/px4_controller.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9657f983..454c3ede 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -55,17 +55,16 @@ private: void send_setpoint() { // set message to enable attitude - auto msg = px4_msgs::msg::TrajectorySetpoint(); + auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); - // get timestamp and publish message + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = -0.1; // down, 10% thrust up - // https://github.com/PX4/px4_msgs/blob/main/msg/TrajectorySetpoint.msg - msg.velocity[0] = 0; // north - msg.velocity[1] = 0; // east - msg.velocity[2] = 1.0; // down (1 m/s -> TODO test if this accounts for 9.81 m/s earth gravity) - - msg.yaw = (0.0 * M_PI) / 180.0; // 0 degrees rotation of yaw - msg.yawspeed = 0; // 0 rotation speed + msg.q_d[0] = 0; + msg.q_d[1] = 0; + msg.q_d[2] = 0; + msg.q_d[3] = 0; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; trajectory_setpoint_publisher->publish(msg); From bd8cec85169275534a42be96f06f1f2b185d1c6c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 14:51:14 +0200 Subject: [PATCH 029/188] typo --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 454c3ede..e76ecb2d 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -67,7 +67,7 @@ private: msg.q_d[3] = 0; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; - trajectory_setpoint_publisher->publish(msg); + vehicle_setpoint_publisher_->publish(msg); RCLCPP_INFO(this->get_logger(), "published message"); } From d24f91512fd9facb7ac269c7b07c4cbcbb68959d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 15:12:08 +0200 Subject: [PATCH 030/188] change quaternion values --- src/px4_connection/src/px4_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e76ecb2d..43ab653f 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -62,9 +62,9 @@ private: msg.thrust_body[2] = -0.1; // down, 10% thrust up msg.q_d[0] = 0; - msg.q_d[1] = 0; - msg.q_d[2] = 0; - msg.q_d[3] = 0; + msg.q_d[1] = 0.1; + msg.q_d[2] = 0.1; + msg.q_d[3] = 0.1; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; vehicle_setpoint_publisher_->publish(msg); From 1775cae4438f114faad1f22d398129d91574e2d1 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 15:16:13 +0200 Subject: [PATCH 031/188] remove timesync includes --- src/px4_connection/src/heartbeat.cpp | 2 +- src/px4_connection/src/px4_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 587e3f37..e755c222 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -12,7 +12,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include "rclcpp/rclcpp.hpp" #include -#include +// #include using namespace std::chrono_literals; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 43ab653f..fe329c4c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -14,7 +14,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include -#include +// #include #include #include From 7fa1cd58161e09713eb769c15f448521fba03ad3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 15:19:04 +0200 Subject: [PATCH 032/188] 100% thrust --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fe329c4c..135ba9ea 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -59,7 +59,7 @@ private: msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -0.1; // down, 10% thrust up + msg.thrust_body[2] = -1; // down, 100% thrust up msg.q_d[0] = 0; msg.q_d[1] = 0.1; From abd59314610f8b20e42c91514ab8da4298919e2a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 15:53:44 +0200 Subject: [PATCH 033/188] add calculating quaternion --- src/px4_connection/src/px4_controller.cpp | 67 ++++++++++++++++++++--- 1 file changed, 58 insertions(+), 9 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 135ba9ea..c5f2be83 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -28,13 +28,13 @@ public: // create a publisher on the vehicle attitude setpoint topic vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); - trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint",10); + trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // set to offboard mode this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); // arm the drone - this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0,0); + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); RCLCPP_INFO(this->get_logger(), "Arm command sent"); @@ -57,14 +57,17 @@ private: // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -1; // down, 100% thrust up + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = 1; // down, 100% thrust up - msg.q_d[0] = 0; - msg.q_d[1] = 0.1; - msg.q_d[2] = 0.1; - msg.q_d[3] = 0.1; + auto array = std::make_unique(4); + calculate_quaternion(array, 0, degrees_to_radians(20), 0); + + msg.q_d[0] = array[0]; + msg.q_d[1] = array[1]; + msg.q_d[2] = array[2]; + msg.q_d[3] = array[3]; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; vehicle_setpoint_publisher_->publish(msg); @@ -92,6 +95,52 @@ private: msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; vehicle_command_publisher_->publish(msg); } + + + /** + * @brief calculates a quaternion based on the given input values + * + * @param ptr pointer to result quaternion. Must be of size 4. + * @param heading desired heading (yaw) in radians. + * @param attitude desired attitude (pitch) in radians. + * @param bank desired bank (roll) in radians. + * @return -1 if the size of the quaternion was not correct, 1 if success. + */ + static char calculate_quaternion(std::unique_ptr ptr, float heading, float attitude, float bank) + { + if (sizeof(ptr.get()) / sizeof(float) != 4) + { + return -1; + } + + float c1 = cos(heading / 2); + float c2 = cos(attitude / 2); + float c3 = cos(bank / 2); + float s1 = sin(heading / 2); + float s2 = sin(attitude / 2); + float s3 = sin(bank / 2); + + float c1c2 = c1 * c2; + float s1s2 = s1 * s2; + + ptr[0] = c1c2 * c3 - s1s2 * s3; // w + ptr[1] = c1c2 * s3 + s1s2 * c3; // x + ptr[2] = s1 * c2 * c3 + c1 * s2 * s3; // y + ptr[3] = c1 * s2 * c3 - s1 * c2 * s3; // z + + return 1; + } + + /** + * @brief converts degrees to radians + * + * @param deg angle in degrees + * @return float new angle in radians + */ + static float degrees_to_radians(float deg) + { + return deg * (M_PI / 180.0); + } }; int main(int argc, char *argv[]) From 29931565e20220c95f3efaaee9272bd5d1ef7b1b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 16:40:00 +0200 Subject: [PATCH 034/188] change ptr to std::array --- src/px4_connection/src/px4_controller.cpp | 21 +++++++-------------- 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index c5f2be83..a8afd2b0 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -61,7 +61,7 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 1; // down, 100% thrust up - auto array = std::make_unique(4); + std::array array = {0, 0, 0, 0}; calculate_quaternion(array, 0, degrees_to_radians(20), 0); msg.q_d[0] = array[0]; @@ -100,18 +100,13 @@ private: /** * @brief calculates a quaternion based on the given input values * - * @param ptr pointer to result quaternion. Must be of size 4. + * @param ptr array of result quaternion. Must be of size 4. * @param heading desired heading (yaw) in radians. * @param attitude desired attitude (pitch) in radians. * @param bank desired bank (roll) in radians. - * @return -1 if the size of the quaternion was not correct, 1 if success. */ - static char calculate_quaternion(std::unique_ptr ptr, float heading, float attitude, float bank) + static void calculate_quaternion(std::array ptr, float heading, float attitude, float bank) { - if (sizeof(ptr.get()) / sizeof(float) != 4) - { - return -1; - } float c1 = cos(heading / 2); float c2 = cos(attitude / 2); @@ -123,12 +118,10 @@ private: float c1c2 = c1 * c2; float s1s2 = s1 * s2; - ptr[0] = c1c2 * c3 - s1s2 * s3; // w - ptr[1] = c1c2 * s3 + s1s2 * c3; // x - ptr[2] = s1 * c2 * c3 + c1 * s2 * s3; // y - ptr[3] = c1 * s2 * c3 - s1 * c2 * s3; // z - - return 1; + ptr.at(0) = c1c2 * c3 - s1s2 * s3; // w + ptr.at(1) = c1c2 * s3 + s1s2 * c3; // x + ptr.at(2) = s1 * c2 * c3 + c1 * s2 * s3; // y + ptr.at(3) = c1 * s2 * c3 - s1 * c2 * s3; // z } /** From c44dcb7f894c933444baccd3d9e373641a9fe09c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 16:40:12 +0200 Subject: [PATCH 035/188] change ptr to std::array --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index a8afd2b0..33e3b0b6 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -61,7 +61,7 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 1; // down, 100% thrust up - std::array array = {0, 0, 0, 0}; + std::array array = {0, 0, 0, 0}; calculate_quaternion(array, 0, degrees_to_radians(20), 0); msg.q_d[0] = array[0]; From b4494a726d2e22e3a9e585b435f825d064905250 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 16:47:54 +0200 Subject: [PATCH 036/188] change heartbeat to attitude --- src/px4_connection/src/heartbeat.cpp | 4 ++-- src/px4_ros_com | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index e755c222..9aeda743 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -40,9 +40,9 @@ private: // set message to enable attitude auto msg = px4_msgs::msg::OffboardControlMode(); msg.position = false; - msg.velocity = true; + msg.velocity = false; msg.acceleration = false; - msg.attitude = false; + msg.attitude = true; msg.body_rate = false; msg.actuator = false; 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 From 8acd7f2c73e50370cc9f2951ebe40be1b8952461 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 16:50:28 +0200 Subject: [PATCH 037/188] add print of quaternion --- src/px4_connection/src/px4_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 33e3b0b6..035a5b60 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -64,6 +64,12 @@ private: std::array array = {0, 0, 0, 0}; calculate_quaternion(array, 0, degrees_to_radians(20), 0); + std::string elements = ""; + for (float f : array) + { + elements += std::to_string(f) + " "; + } + RCLCPP_INFO(this->get_logger(), "q_d: %s", elements); msg.q_d[0] = array[0]; msg.q_d[1] = array[1]; msg.q_d[2] = array[2]; From 52132684d2e86247e4266163b1e8de186e46c44a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:25:26 +0200 Subject: [PATCH 038/188] test move up --- src/px4_connection/src/px4_controller.cpp | 39 ++++++++++++++--------- 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 035a5b60..4100e687 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -40,6 +40,8 @@ public: // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); + + start_time_ = this->get_clock()->now().seconds(); } private: @@ -47,6 +49,7 @@ private: rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; rclcpp::TimerBase::SharedPtr timer_; + double start_time_; /** * @brief Only the attitude is enabled, because that is how the drone will be controlled. @@ -56,20 +59,27 @@ private: { // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); - - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = 1; // down, 100% thrust up - - std::array array = {0, 0, 0, 0}; - calculate_quaternion(array, 0, degrees_to_radians(20), 0); - - std::string elements = ""; - for (float f : array) + if (this->get_clock()->now().seconds() - start_time_ < 5) { - elements += std::to_string(f) + " "; + // move up? + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = 0.5; // down, 50% thrust up + + std::array array = {0, 0, 0, 0}; + calculate_quaternion(array, degrees_to_radians(40), 0, 0); } - RCLCPP_INFO(this->get_logger(), "q_d: %s", elements); + else + { + // no thrust + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = 0; // down + + std::array array = {0, 0, 0, 0}; + calculate_quaternion(array, degrees_to_radians(270), 0, 0); + } + msg.q_d[0] = array[0]; msg.q_d[1] = array[1]; msg.q_d[2] = array[2]; @@ -102,10 +112,9 @@ private: vehicle_command_publisher_->publish(msg); } - /** * @brief calculates a quaternion based on the given input values - * + * * @param ptr array of result quaternion. Must be of size 4. * @param heading desired heading (yaw) in radians. * @param attitude desired attitude (pitch) in radians. @@ -132,7 +141,7 @@ private: /** * @brief converts degrees to radians - * + * * @param deg angle in degrees * @return float new angle in radians */ From c8a61cac401d879b503f1ae0326c1549c6606480 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:29:35 +0200 Subject: [PATCH 039/188] change name of quaternion array --- src/px4_connection/src/px4_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4100e687..5fd83dcf 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -76,14 +76,14 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 0; // down - std::array array = {0, 0, 0, 0}; - calculate_quaternion(array, degrees_to_radians(270), 0, 0); + std::array q = {0, 0, 0, 0}; + calculate_quaternion(q, degrees_to_radians(270), 0, 0); } - msg.q_d[0] = array[0]; - msg.q_d[1] = array[1]; - msg.q_d[2] = array[2]; - msg.q_d[3] = array[3]; + msg.q_d[0] = q[0]; + msg.q_d[1] = q[1]; + msg.q_d[2] = q[2]; + msg.q_d[3] = q[3]; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; vehicle_setpoint_publisher_->publish(msg); From b8f572d86f7d30c3df6c722c12c4856c483d71fb Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:31:16 +0200 Subject: [PATCH 040/188] fix --- src/px4_connection/src/px4_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 5fd83dcf..4617cdfd 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -59,6 +59,8 @@ private: { // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); + std::array q = {0, 0, 0, 0}; + if (this->get_clock()->now().seconds() - start_time_ < 5) { // move up? @@ -66,7 +68,6 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 0.5; // down, 50% thrust up - std::array array = {0, 0, 0, 0}; calculate_quaternion(array, degrees_to_radians(40), 0, 0); } else @@ -76,14 +77,13 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 0; // down - std::array q = {0, 0, 0, 0}; calculate_quaternion(q, degrees_to_radians(270), 0, 0); } - msg.q_d[0] = q[0]; - msg.q_d[1] = q[1]; - msg.q_d[2] = q[2]; - msg.q_d[3] = q[3]; + msg.q_d[0] = q.at(0); + msg.q_d[1] = q.at(1); + msg.q_d[2] = q.at(2); + msg.q_d[3] = q.at(3); msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; vehicle_setpoint_publisher_->publish(msg); From 7a2f19d311bbe9328e73b4fe52cda53281c8eae5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:37:07 +0200 Subject: [PATCH 041/188] add comment --- src/px4_connection/src/px4_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4617cdfd..1f1a06c5 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -59,6 +59,7 @@ private: { // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); + // result quaternion std::array q = {0, 0, 0, 0}; if (this->get_clock()->now().seconds() - start_time_ < 5) @@ -68,7 +69,7 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 0.5; // down, 50% thrust up - calculate_quaternion(array, degrees_to_radians(40), 0, 0); + calculate_quaternion(q, degrees_to_radians(40), 0, 0); } else { From 5a7a54fc535ea5562e4c2ba059833838fd328fc4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:44:22 +0200 Subject: [PATCH 042/188] change 0.5 thrust to 0.8 --- src/px4_connection/src/heartbeat.cpp | 11 ++++++----- src/px4_connection/src/px4_controller.cpp | 11 ++++++++--- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 9aeda743..e662fbdf 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -26,6 +26,7 @@ public: // create timer to send heartbeat messages (offboard control) every 100ms timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this)); start_time = this->get_clock()->now().seconds(); + RCLCPP_INFO(this->get_logger(), "done initializing after %d seconds. Sending heartbeat...", start_time); } private: @@ -49,13 +50,13 @@ private: // 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!"); + // 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!"); - } + // if (this->get_clock()->now().seconds() - start_time > 5) + // { + // RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!"); + // } diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 1f1a06c5..78a5fbde 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -42,6 +42,7 @@ public: timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); start_time_ = this->get_clock()->now().seconds(); + RCLCPP_INFO(this->get_logger(), "finished initializing after %d seconds.", start_time_); } private: @@ -50,6 +51,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_command_publisher_; rclcpp::TimerBase::SharedPtr timer_; double start_time_; + bool has_sent_status = false; /** * @brief Only the attitude is enabled, because that is how the drone will be controlled. @@ -67,12 +69,17 @@ private: // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = 0.5; // down, 50% thrust up + msg.thrust_body[2] = 0.8; // down, 80% thrust up calculate_quaternion(q, degrees_to_radians(40), 0, 0); } else { + if (!has_sent_status) + { + has_sent_status = true; + RCLCPP_INFO(this->get_logger(), "changing down thrust to 0"); + } // no thrust msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east @@ -88,8 +95,6 @@ private: msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; vehicle_setpoint_publisher_->publish(msg); - - RCLCPP_INFO(this->get_logger(), "published message"); } /** From f883d826ec834d1ab48ae61f35202575428b9789 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:50:15 +0200 Subject: [PATCH 043/188] change to 100% thrust --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 78a5fbde..ee243962 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -69,7 +69,7 @@ private: // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = 0.8; // down, 80% thrust up + msg.thrust_body[2] = 1; // down, 100% thrust up calculate_quaternion(q, degrees_to_radians(40), 0, 0); } From c9f546cb0ca5f025c22285bdbbd66561d45c657e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:53:46 +0200 Subject: [PATCH 044/188] try different values --- src/px4_connection/src/heartbeat.cpp | 2 +- src/px4_connection/src/px4_controller.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index e662fbdf..52f6f87d 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -26,7 +26,7 @@ public: // create timer to send heartbeat messages (offboard control) every 100ms timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this)); start_time = this->get_clock()->now().seconds(); - RCLCPP_INFO(this->get_logger(), "done initializing after %d seconds. Sending heartbeat...", start_time); + RCLCPP_INFO(this->get_logger(), "done initializing at %d seconds. Sending heartbeat...", start_time); } private: diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index ee243962..fd34008c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -42,7 +42,7 @@ public: timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); start_time_ = this->get_clock()->now().seconds(); - RCLCPP_INFO(this->get_logger(), "finished initializing after %d seconds.", start_time_); + RCLCPP_INFO(this->get_logger(), "finished initializing at %d seconds.", start_time_); } private: @@ -64,14 +64,14 @@ private: // result quaternion std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 5) + if (this->get_clock()->now().seconds() - start_time_ < 10) { // move up? msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east + msg.thrust_body[1] = 0.1; // east msg.thrust_body[2] = 1; // down, 100% thrust up - calculate_quaternion(q, degrees_to_radians(40), 0, 0); + calculate_quaternion(q, degrees_to_radians(10), 0, 0); } else { @@ -85,7 +85,7 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 0; // down - calculate_quaternion(q, degrees_to_radians(270), 0, 0); + calculate_quaternion(q, degrees_to_radians(10), 0, 0); } msg.q_d[0] = q.at(0); From 9464dbf5ac7ef8da5a78549426a23786ad2240f7 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 17:57:32 +0200 Subject: [PATCH 045/188] try different values --- src/px4_connection/src/px4_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fd34008c..2c266225 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -68,10 +68,10 @@ private: { // move up? msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0.1; // east + msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 1; // down, 100% thrust up - calculate_quaternion(q, degrees_to_radians(10), 0, 0); + calculate_quaternion(q, 0, degrees_to_radians(10), 0); } else { @@ -85,7 +85,7 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 0; // down - calculate_quaternion(q, degrees_to_radians(10), 0, 0); + calculate_quaternion(q, 0, degrees_to_radians(10), 0); } msg.q_d[0] = q.at(0); From 7545a8a2a857746013faf2284da44cff441dcb5f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:03:14 +0200 Subject: [PATCH 046/188] add disarm --- src/px4_connection/src/px4_controller.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 2c266225..48ebef1b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -37,6 +37,7 @@ public: this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); RCLCPP_INFO(this->get_logger(), "Arm command sent"); + armed = true; // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -52,6 +53,7 @@ private: rclcpp::TimerBase::SharedPtr timer_; double start_time_; bool has_sent_status = false; + bool armed = false; /** * @brief Only the attitude is enabled, because that is how the drone will be controlled. @@ -72,6 +74,14 @@ private: msg.thrust_body[2] = 1; // down, 100% thrust up calculate_quaternion(q, 0, degrees_to_radians(10), 0); + } else if (this->get_clock()->now().seconds() - start_time > 20) + { + if (armed) + { + publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); + armed = false; + RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds"); + } } else { From 5b935af8945d9717f39e0b7578d14ed6bd0b088c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:03:43 +0200 Subject: [PATCH 047/188] change attitude value --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 48ebef1b..5d79fd4a 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -73,7 +73,7 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 1; // down, 100% thrust up - calculate_quaternion(q, 0, degrees_to_radians(10), 0); + calculate_quaternion(q, 0, -3.14, 0); } else if (this->get_clock()->now().seconds() - start_time > 20) { if (armed) From b03951faabf75fe6e627cd30657c20175198f0d1 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:07:04 +0200 Subject: [PATCH 048/188] oops --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 5d79fd4a..fb4441e8 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -74,11 +74,11 @@ private: msg.thrust_body[2] = 1; // down, 100% thrust up calculate_quaternion(q, 0, -3.14, 0); - } else if (this->get_clock()->now().seconds() - start_time > 20) + } else if (this->get_clock()->now().seconds() - start_time_ > 20) { if (armed) { - publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); armed = false; RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds"); } From 806317a04c859f277387e8745eee73c048e0aa67 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:07:42 +0200 Subject: [PATCH 049/188] oops v2 --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fb4441e8..2f346423 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -78,7 +78,7 @@ private: { if (armed) { - publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0,0); armed = false; RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds"); } From a3b0b761e732bd9bee89c4e9216f47dabe201e46 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:22:41 +0200 Subject: [PATCH 050/188] made px4controller also send heartbeat --- src/px4_connection/src/px4_controller.cpp | 77 ++++++++++++++++------- 1 file changed, 55 insertions(+), 22 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 2f346423..7247e52e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,6 +17,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- // #include #include #include +#include using namespace std::chrono_literals; @@ -29,18 +30,10 @@ public: vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); - - // set to offboard mode - this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); - RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); - // arm the drone - this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); - - RCLCPP_INFO(this->get_logger(), "Arm command sent"); - armed = true; + offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); // create timer to send vehicle attitude setpoints every second - timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); + timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); start_time_ = this->get_clock()->now().seconds(); RCLCPP_INFO(this->get_logger(), "finished initializing at %d seconds.", start_time_); @@ -50,10 +43,13 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + rclcpp::TimerBase::SharedPtr timer_; double start_time_; bool has_sent_status = false; - bool armed = false; + bool flying = false; + int setpoint_count = 0; /** * @brief Only the attitude is enabled, because that is how the drone will be controlled. @@ -61,6 +57,23 @@ private: */ void send_setpoint() { + + if (setpoint_count < 11) + setpoint_count++; + + if (setpoint_count == 10) + { + // switch to offboard mode and arm + + // set to offboard mode + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); + RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); + // arm the drone + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); + + RCLCPP_INFO(this->get_logger(), "Arm command sent"); + flying = true; + } // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); // result quaternion @@ -69,17 +82,18 @@ private: if (this->get_clock()->now().seconds() - start_time_ < 10) { // move up? - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 1; // down, 100% thrust up - calculate_quaternion(q, 0, -3.14, 0); - } else if (this->get_clock()->now().seconds() - start_time_ > 20) + calculate_quaternion(q, 0, 1, 1); + } + else if (this->get_clock()->now().seconds() - start_time_ > 20) { - if (armed) + if (flying) { - publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0,0); - armed = false; + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + flying = false; RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds"); } } @@ -88,12 +102,12 @@ private: if (!has_sent_status) { has_sent_status = true; - RCLCPP_INFO(this->get_logger(), "changing down thrust to 0"); + RCLCPP_INFO(this->get_logger(), "changing down thrust to 0.5 and east to 1"); } // no thrust - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = 0; // down + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 1; // east + msg.thrust_body[2] = 0.5; // down calculate_quaternion(q, 0, degrees_to_radians(10), 0); } @@ -104,9 +118,28 @@ private: msg.q_d[3] = q.at(3); msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + // send heartbeat together with attitude setpoint + send_heartbeat(); vehicle_setpoint_publisher_->publish(msg); } + 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!"); + } + /** * @brief Publish vehicle commands * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) From 1224735954f117299c71517b390a1f66251a6030 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:25:22 +0200 Subject: [PATCH 051/188] waited longer before arming --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 7247e52e..d082f6d7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -58,10 +58,10 @@ private: void send_setpoint() { - if (setpoint_count < 11) + if (setpoint_count < 21) setpoint_count++; - if (setpoint_count == 10) + if (setpoint_count == 20) { // switch to offboard mode and arm From f767ee2583d1560f8b93f89d3f679fce98d9905d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:29:20 +0200 Subject: [PATCH 052/188] try different values --- src/px4_connection/src/px4_controller.cpp | 33 ++++++++++++----------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index d082f6d7..42d8c4e4 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -79,16 +79,17 @@ private: // result quaternion std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 10) + if (this->get_clock()->now().seconds() - start_time_ < 20) { // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east msg.thrust_body[2] = 1; // down, 100% thrust up - calculate_quaternion(q, 0, 1, 1); + calculate_quaternion(q, 0, degrees_to_radians(20), 0); } - else if (this->get_clock()->now().seconds() - start_time_ > 20) + + else { if (flying) { @@ -97,20 +98,20 @@ private: RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds"); } } - else - { - if (!has_sent_status) - { - has_sent_status = true; - RCLCPP_INFO(this->get_logger(), "changing down thrust to 0.5 and east to 1"); - } - // no thrust - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 1; // east - msg.thrust_body[2] = 0.5; // down + // else + // { + // if (!has_sent_status) + // { + // has_sent_status = true; + // RCLCPP_INFO(this->get_logger(), "changing down thrust to 0.5 and east to 1"); + // } + // // no thrust + // msg.thrust_body[0] = 0; // north + // msg.thrust_body[1] = 1; // east + // msg.thrust_body[2] = 0.5; // down - calculate_quaternion(q, 0, degrees_to_radians(10), 0); - } + // calculate_quaternion(q, 0, degrees_to_radians(10), 0); + // } msg.q_d[0] = q.at(0); msg.q_d[1] = q.at(1); From 4b063ce9b2b6bf052f3143425db6c6f1e51b9687 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:36:23 +0200 Subject: [PATCH 053/188] try with increasing thrust --- src/px4_connection/src/px4_controller.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 42d8c4e4..e68770b7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -50,6 +50,7 @@ private: bool has_sent_status = false; bool flying = false; int setpoint_count = 0; + float thrust = 0; /** * @brief Only the attitude is enabled, because that is how the drone will be controlled. @@ -57,9 +58,12 @@ private: */ void send_setpoint() { - - if (setpoint_count < 21) - setpoint_count++; + + setpoint_count++; + + if (setpoint_count % 20 == 0 && thrust <= 1) { + thrust += 0.1; + } if (setpoint_count == 20) { @@ -79,12 +83,12 @@ private: // result quaternion std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 20) + if (this->get_clock()->now().seconds() - start_time_ < 30) { // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = 1; // down, 100% thrust up + msg.thrust_body[2] = thrust; // down, 100% thrust up calculate_quaternion(q, 0, degrees_to_radians(20), 0); } From 10c0b5ae5f86b0463b7ddfbd2f6007c5e0d4773c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:33:20 +0200 Subject: [PATCH 054/188] increase thrust --- src/px4_connection/src/px4_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e68770b7..4b250244 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -63,6 +63,7 @@ private: if (setpoint_count % 20 == 0 && thrust <= 1) { thrust += 0.1; + RCLCPP_INFO(this->get_logger(), "increasing thrust"); } if (setpoint_count == 20) From b775f8015c5df07ca795cb26d0b20b5bab63747c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:36:01 +0200 Subject: [PATCH 055/188] try diff values --- src/px4_connection/src/px4_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4b250244..534eacbe 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -50,7 +50,7 @@ private: bool has_sent_status = false; bool flying = false; int setpoint_count = 0; - float thrust = 0; + float thrust = 0.5; /** * @brief Only the attitude is enabled, because that is how the drone will be controlled. @@ -87,11 +87,11 @@ private: if (this->get_clock()->now().seconds() - start_time_ < 30) { // move up? - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east + msg.thrust_body[0] = thrust; // north + msg.thrust_body[1] = thrust; // east msg.thrust_body[2] = thrust; // down, 100% thrust up - calculate_quaternion(q, 0, degrees_to_radians(20), 0); + calculate_quaternion(q, 0.1, degrees_to_radians(20), 0.1); } else From a037b7f00e65181884d123e7828fc0756644817e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:40:28 +0200 Subject: [PATCH 056/188] try diff values --- src/px4_connection/src/px4_controller.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 534eacbe..efccc49d 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -61,10 +61,10 @@ private: setpoint_count++; - if (setpoint_count % 20 == 0 && thrust <= 1) { - thrust += 0.1; - RCLCPP_INFO(this->get_logger(), "increasing thrust"); - } + // if (setpoint_count % 20 == 0 && thrust <= 1) { + // thrust += 0.1; + // RCLCPP_INFO(this->get_logger(), "increasing thrust"); + // } if (setpoint_count == 20) { @@ -87,11 +87,11 @@ private: if (this->get_clock()->now().seconds() - start_time_ < 30) { // move up? - msg.thrust_body[0] = thrust; // north - msg.thrust_body[1] = thrust; // east - msg.thrust_body[2] = thrust; // down, 100% thrust up + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = 1; // down, 100% thrust up - calculate_quaternion(q, 0.1, degrees_to_radians(20), 0.1); + calculate_quaternion(q, 0, degrees_to_radians(20), 0); } else From 52f237fcb5fd71493bef38c7bc300a4cb0e6ad1b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:45:08 +0200 Subject: [PATCH 057/188] add extra parameters --- src/px4_connection/src/px4_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index efccc49d..e0453b33 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -100,7 +100,7 @@ private: { publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); flying = false; - RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds"); + RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds"); } } // else @@ -123,6 +123,10 @@ private: msg.q_d[2] = q.at(2); msg.q_d[3] = q.at(3); + msg.yaw_sp_move_rate = 0; + msg.reset_integral = false; + msg.fw_control_yaw_wheel = false; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; // send heartbeat together with attitude setpoint send_heartbeat(); From 1515206e2ecd476d46d2d58c2607f798f15eced6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:46:41 +0200 Subject: [PATCH 058/188] set throttle to negative --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e0453b33..5fde4572 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -89,7 +89,7 @@ private: // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = 1; // down, 100% thrust up + msg.thrust_body[2] = -1; // down, 100% thrust up calculate_quaternion(q, 0, degrees_to_radians(20), 0); } @@ -126,7 +126,7 @@ private: msg.yaw_sp_move_rate = 0; msg.reset_integral = false; msg.fw_control_yaw_wheel = false; - + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; // send heartbeat together with attitude setpoint send_heartbeat(); From 37b5f14f72b62c4e766362e65557d2abf7095808 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:48:07 +0200 Subject: [PATCH 059/188] change throttle --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 5fde4572..8fbe6adc 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -89,7 +89,7 @@ private: // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -1; // down, 100% thrust up + msg.thrust_body[2] = -0.6; // down, 100% thrust up calculate_quaternion(q, 0, degrees_to_radians(20), 0); } From c4e9e3bb10ce3edc0b76b7345e6ac9410761749c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:52:02 +0200 Subject: [PATCH 060/188] remove heartbeat from px4 controller --- src/px4_connection/src/px4_controller.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 8fbe6adc..e2207913 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,7 +17,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- // #include #include #include -#include +// #include using namespace std::chrono_literals; @@ -30,7 +30,7 @@ public: vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -43,7 +43,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; - rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; double start_time_; @@ -61,10 +61,10 @@ private: setpoint_count++; - // if (setpoint_count % 20 == 0 && thrust <= 1) { - // thrust += 0.1; - // RCLCPP_INFO(this->get_logger(), "increasing thrust"); - // } + if (setpoint_count % 20 == 0 && thrust <= 1) { + thrust += 0.1; + RCLCPP_INFO(this->get_logger(), "increasing thrust to %d", thrust); + } if (setpoint_count == 20) { @@ -89,9 +89,9 @@ private: // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -0.6; // down, 100% thrust up + msg.thrust_body[2] = -thrust; // down, 100% thrust up - calculate_quaternion(q, 0, degrees_to_radians(20), 0); + calculate_quaternion(q, 0, 0, 0); } else @@ -129,7 +129,7 @@ private: msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; // send heartbeat together with attitude setpoint - send_heartbeat(); + // send_heartbeat(); vehicle_setpoint_publisher_->publish(msg); } From 0c543614a954760e85afb5e6ce21ab78a6401150 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:52:38 +0200 Subject: [PATCH 061/188] oops --- src/px4_connection/src/px4_controller.cpp | 30 +++++++++++------------ 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e2207913..37fb4a32 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -133,22 +133,22 @@ private: vehicle_setpoint_publisher_->publish(msg); } - 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; + // 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!"); - } + // // 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!"); + // } /** * @brief Publish vehicle commands From 799ef4237e222789d1c5f30698e034c6a20db6db Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:57:35 +0200 Subject: [PATCH 062/188] try adding hover? --- src/px4_connection/src/px4_controller.cpp | 52 ++++++++--------------- 1 file changed, 17 insertions(+), 35 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 37fb4a32..54613ae7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -49,6 +49,7 @@ private: double start_time_; bool has_sent_status = false; bool flying = false; + bool has_swithed = false; int setpoint_count = 0; float thrust = 0.5; @@ -84,7 +85,7 @@ private: // result quaternion std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 30) + if (this->get_clock()->now().seconds() - start_time_ < 15) { // move up? msg.thrust_body[0] = 0; // north @@ -94,7 +95,21 @@ private: calculate_quaternion(q, 0, 0, 0); } - else + else if (this->get_clock()->now().seconds() - start_time_ < 30) + { + if (!has_swithed) + { + RCLCPP_INFO(this->get_logger(),"changing to 0.5 thrust"); + has_swithed = true; + } + + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = -0.5; // down, 100% thrust up + + calculate_quaternion(q, 0, 0, 0); + } + else { if (flying) { @@ -103,20 +118,6 @@ private: RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds"); } } - // else - // { - // if (!has_sent_status) - // { - // has_sent_status = true; - // RCLCPP_INFO(this->get_logger(), "changing down thrust to 0.5 and east to 1"); - // } - // // no thrust - // msg.thrust_body[0] = 0; // north - // msg.thrust_body[1] = 1; // east - // msg.thrust_body[2] = 0.5; // down - - // calculate_quaternion(q, 0, degrees_to_radians(10), 0); - // } msg.q_d[0] = q.at(0); msg.q_d[1] = q.at(1); @@ -128,28 +129,9 @@ private: msg.fw_control_yaw_wheel = false; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; - // send heartbeat together with attitude setpoint - // send_heartbeat(); vehicle_setpoint_publisher_->publish(msg); } - // 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!"); - // } - /** * @brief Publish vehicle commands * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) From c0d327165ce4283725bfef8e557bd71160052268 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 20:59:38 +0200 Subject: [PATCH 063/188] change formatting numbers to f in strings --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 54613ae7..35f99a83 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -36,7 +36,7 @@ public: timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); start_time_ = this->get_clock()->now().seconds(); - RCLCPP_INFO(this->get_logger(), "finished initializing at %d seconds.", start_time_); + RCLCPP_INFO(this->get_logger(), "finished initializing at %f seconds.", start_time_); } private: @@ -64,7 +64,7 @@ private: if (setpoint_count % 20 == 0 && thrust <= 1) { thrust += 0.1; - RCLCPP_INFO(this->get_logger(), "increasing thrust to %d", thrust); + RCLCPP_INFO(this->get_logger(), "increasing thrust to %f", thrust); } if (setpoint_count == 20) From 19d1987484f107a20126df80b2436058434cac09 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 21:01:12 +0200 Subject: [PATCH 064/188] try adding hover? --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 35f99a83..c8d59d25 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -85,12 +85,12 @@ private: // result quaternion std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 15) + if (this->get_clock()->now().seconds() - start_time_ < 10) { // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -thrust; // down, 100% thrust up + msg.thrust_body[2] = -0.8; // down, 100% thrust up calculate_quaternion(q, 0, 0, 0); } From 3fe7b603744bda7bc68850103d0c87084814b9b7 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 10:19:45 +0200 Subject: [PATCH 065/188] add try sending trajectory setpoints --- src/px4_connection/src/heartbeat.cpp | 4 +- src/px4_connection/src/px4_controller.cpp | 78 +++++++++++++++-------- 2 files changed, 53 insertions(+), 29 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 52f6f87d..7f6459d2 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -41,9 +41,9 @@ private: // set message to enable attitude auto msg = px4_msgs::msg::OffboardControlMode(); msg.position = false; - msg.velocity = false; + msg.velocity = true; msg.acceleration = false; - msg.attitude = true; + msg.attitude = false; msg.body_rate = false; msg.actuator = false; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index c8d59d25..64249a30 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -52,34 +52,31 @@ private: bool has_swithed = false; int setpoint_count = 0; float thrust = 0.5; + bool ready_to_fly = setpoint_count == 20; - /** - * @brief Only the attitude is enabled, because that is how the drone will be controlled. - * - */ - void send_setpoint() + void send_trajectory_setpoint() { + auto msg = px4_msgs::msg::TrajectorySetpoint(); - setpoint_count++; + msg.velocity[0] = 2; + msg.velocity[1] = 0; + msg.velocity[2] = 0.5; - if (setpoint_count % 20 == 0 && thrust <= 1) { + msg.yaw = -3.14; + msg.yawspeed = 0; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + + trajectory_setpoint_publisher->publush(msg); + } + + void send_attitude_setpoint() + { + if (setpoint_count % 20 == 0 && thrust <= 1) + { thrust += 0.1; RCLCPP_INFO(this->get_logger(), "increasing thrust to %f", thrust); } - if (setpoint_count == 20) - { - // switch to offboard mode and arm - - // set to offboard mode - this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); - RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); - // arm the drone - this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); - - RCLCPP_INFO(this->get_logger(), "Arm command sent"); - flying = true; - } // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); // result quaternion @@ -88,8 +85,8 @@ private: if (this->get_clock()->now().seconds() - start_time_ < 10) { // move up? - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east msg.thrust_body[2] = -0.8; // down, 100% thrust up calculate_quaternion(q, 0, 0, 0); @@ -97,19 +94,19 @@ private: else if (this->get_clock()->now().seconds() - start_time_ < 30) { - if (!has_swithed) + if (!has_swithed) { - RCLCPP_INFO(this->get_logger(),"changing to 0.5 thrust"); + RCLCPP_INFO(this->get_logger(), "changing to 0.5 thrust"); has_swithed = true; } - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east msg.thrust_body[2] = -0.5; // down, 100% thrust up calculate_quaternion(q, 0, 0, 0); } - else + else { if (flying) { @@ -132,6 +129,33 @@ private: vehicle_setpoint_publisher_->publish(msg); } + /** + * @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed. + * + */ + void send_setpoint() + { + // increase amount of setpoints sent + setpoint_count++; + + // after 20 setpoints, send arm command to make drone actually fly + if (ready_to_fly) + { + // switch to offboard mode and arm + + // set to offboard mode + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); + RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); + // arm the drone + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); + + RCLCPP_INFO(this->get_logger(), "Arm command sent"); + flying = true; + } + + send_trajectory_setpoint(); + } + /** * @brief Publish vehicle commands * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) From 23a59a449c32d4b66228c3e365f03a832aa88d6c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 10:21:03 +0200 Subject: [PATCH 066/188] typo --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 64249a30..44a37af2 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -66,7 +66,7 @@ private: msg.yawspeed = 0; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; - trajectory_setpoint_publisher->publush(msg); + trajectory_setpoint_publisher->publish(msg); } void send_attitude_setpoint() From 4449c6cc1fac984abf78602fd0f33e307707ca96 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 10:51:03 +0200 Subject: [PATCH 067/188] change checking of ready to fly --- src/px4_connection/src/px4_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 44a37af2..97052d71 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -52,7 +52,7 @@ private: bool has_swithed = false; int setpoint_count = 0; float thrust = 0.5; - bool ready_to_fly = setpoint_count == 20; + bool ready_to_fly = false; void send_trajectory_setpoint() { @@ -138,6 +138,7 @@ private: // increase amount of setpoints sent setpoint_count++; + ready_to_fly = setpoint_count == 20; // after 20 setpoints, send arm command to make drone actually fly if (ready_to_fly) { From ae18d7834ac78e8968e91e2a3abb27c339eeeec8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 10:55:01 +0200 Subject: [PATCH 068/188] try up velocity of 1 --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 97052d71..1ed4e6ff 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -60,7 +60,7 @@ private: msg.velocity[0] = 2; msg.velocity[1] = 0; - msg.velocity[2] = 0.5; + msg.velocity[2] = 1; msg.yaw = -3.14; msg.yawspeed = 0; From 6b33ded940196c7fd9fd00a73e2c5d49a5c2de72 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 10:56:43 +0200 Subject: [PATCH 069/188] add compensation for earth gravity pull --- src/px4_connection/src/px4_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 1ed4e6ff..4632cb9e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,6 +19,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include // #include +#define D_SPEED(x) x + 9.81 + using namespace std::chrono_literals; class PX4Controller : public rclcpp::Node @@ -60,7 +62,7 @@ private: msg.velocity[0] = 2; msg.velocity[1] = 0; - msg.velocity[2] = 1; + msg.velocity[2] = D_SPEED(9.81); msg.yaw = -3.14; msg.yawspeed = 0; From 60033323d4a887e299860b5a2a3b29fe55900e9c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:01:24 +0200 Subject: [PATCH 070/188] change speeds --- src/px4_connection/src/px4_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4632cb9e..db10e22f 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -60,9 +60,9 @@ private: { auto msg = px4_msgs::msg::TrajectorySetpoint(); - msg.velocity[0] = 2; - msg.velocity[1] = 0; - msg.velocity[2] = D_SPEED(9.81); + msg.velocity[0] = 5; + msg.velocity[1] = 5; + msg.velocity[2] = D_SPEED(10); msg.yaw = -3.14; msg.yawspeed = 0; From 088af872f9c6b309934504ee11a43f5569d61fbd Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:02:50 +0200 Subject: [PATCH 071/188] try negative down speed --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index db10e22f..839853e4 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,7 +19,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include // #include -#define D_SPEED(x) x + 9.81 +#define D_SPEED(x) -x - 9.81 using namespace std::chrono_literals; @@ -62,7 +62,7 @@ private: msg.velocity[0] = 5; msg.velocity[1] = 5; - msg.velocity[2] = D_SPEED(10); + msg.velocity[2] = D_SPEED(1); msg.yaw = -3.14; msg.yawspeed = 0; From 33d173d1e09dc014cf53742f709f356f5909a31f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:07:27 +0200 Subject: [PATCH 072/188] try to hover --- src/px4_connection/src/px4_controller.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 839853e4..652f95c6 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -60,12 +60,22 @@ private: { auto msg = px4_msgs::msg::TrajectorySetpoint(); - msg.velocity[0] = 5; - msg.velocity[1] = 5; - msg.velocity[2] = D_SPEED(1); + if (setpoint_count < 30) + { + msg.velocity[0] = 0; + msg.velocity[1] = 0; + msg.velocity[2] = D_SPEED(1); + msg.yawspeed = 0; - msg.yaw = -3.14; - msg.yawspeed = 0; + } else { + //try to hover + msg.velocity[0] = 0; + msg.velocity[1] = 0; + msg.velocity[2] = 0; + msg.yawspeed = 1; + + } + msg.yaw = -3.14; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; trajectory_setpoint_publisher->publish(msg); From 346e41f4758c44225b08f4090901f3ede2af3f03 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:08:39 +0200 Subject: [PATCH 073/188] change values --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 652f95c6..1c1d3af8 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -64,7 +64,7 @@ private: { msg.velocity[0] = 0; msg.velocity[1] = 0; - msg.velocity[2] = D_SPEED(1); + msg.velocity[2] = D_SPEED(10); msg.yawspeed = 0; } else { From 048a1b4929624fd0305aebb33b57ec2a1efa9733 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:18:35 +0200 Subject: [PATCH 074/188] change values --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 1c1d3af8..425a8e7e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -63,7 +63,7 @@ private: if (setpoint_count < 30) { msg.velocity[0] = 0; - msg.velocity[1] = 0; + msg.velocity[1] = 1; msg.velocity[2] = D_SPEED(10); msg.yawspeed = 0; From 9c3dcb463d4799654af57584bf72643e07d67cf6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:23:25 +0200 Subject: [PATCH 075/188] make dat boi schmoov --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 425a8e7e..a9158d00 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -62,7 +62,7 @@ private: if (setpoint_count < 30) { - msg.velocity[0] = 0; + msg.velocity[0] = 5; msg.velocity[1] = 1; msg.velocity[2] = D_SPEED(10); msg.yawspeed = 0; From 4eb876df0cc7be119cdf4a6b742d1693bb831167 Mon Sep 17 00:00:00 2001 From: ubuntu Date: Mon, 1 May 2023 11:27:38 +0200 Subject: [PATCH 076/188] add run script --- get_build_run_controller.sh | 7 +++++++ src/px4_msgs | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) create mode 100755 get_build_run_controller.sh diff --git a/get_build_run_controller.sh b/get_build_run_controller.sh new file mode 100755 index 00000000..64ff7e9d --- /dev/null +++ b/get_build_run_controller.sh @@ -0,0 +1,7 @@ +#!/bin/bash +echo "updating git..." +./fgit.sh +echo "building package..." +colcon build --packages-select px4_connection +echo "launching controller..." +ros2 run px4_connection px4_controller diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 From 6348e5371f8731e1f8f5f2bae7f0db918489381a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:30:06 +0200 Subject: [PATCH 077/188] add launch file --- .../launch/px4_controller_heardbeat_launch.py | 15 +++++++++++++++ src/px4_msgs | 2 +- 2 files changed, 16 insertions(+), 1 deletion(-) create mode 100644 src/px4_connection/launch/px4_controller_heardbeat_launch.py diff --git a/src/px4_connection/launch/px4_controller_heardbeat_launch.py b/src/px4_connection/launch/px4_controller_heardbeat_launch.py new file mode 100644 index 00000000..d0b733ca --- /dev/null +++ b/src/px4_connection/launch/px4_controller_heardbeat_launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="px4_connection", + executable="heartbeat" + ), + Node( + package="px4_connection", + executable="px4_controller" + ) + + ]) diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 From e0c9c2601d1dbdc0a8a97ceb4aafc799aa18b07a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 11:38:16 +0200 Subject: [PATCH 078/188] add launch file to cmakelists --- src/px4_connection/CMakeLists.txt | 55 +++++++++++++++++-------------- 1 file changed, 30 insertions(+), 25 deletions(-) diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index df58848f..c9cf9937 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -3,16 +3,16 @@ project(px4_connection) # Default to C99 if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) + set(CMAKE_C_STANDARD 99) endif() # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies @@ -23,41 +23,46 @@ find_package(px4_msgs REQUIRED) add_executable(heartbeat src/heartbeat.cpp) ament_target_dependencies( - heartbeat - rclcpp - px4_ros_com - px4_msgs + heartbeat + rclcpp + px4_ros_com + px4_msgs ) add_executable(px4_controller src/px4_controller.cpp) ament_target_dependencies( - px4_controller - rclcpp - px4_ros_com - px4_msgs + px4_controller + rclcpp + px4_ros_com + px4_msgs ) target_include_directories(heartbeat PUBLIC - $ - $) + $ + $) target_include_directories(px4_controller PUBLIC - $ - $) - + $ + $) install(TARGETS heartbeat px4_controller - DESTINATION lib/${PROJECT_NAME}) + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY launch + DESTINATION share/${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() + 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() From a9c63cc235e44cc75ee021fb84ef794cad2d9e9a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:35:08 +0200 Subject: [PATCH 079/188] change value --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index a9158d00..67d4b7e7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -63,8 +63,8 @@ private: if (setpoint_count < 30) { msg.velocity[0] = 5; - msg.velocity[1] = 1; - msg.velocity[2] = D_SPEED(10); + msg.velocity[1] = 5; + msg.velocity[2] = -20; msg.yawspeed = 0; } else { From a739fb51c3e15a79197937b0daf997017650b039 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:36:38 +0200 Subject: [PATCH 080/188] change value --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 67d4b7e7..78b386c1 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -64,7 +64,7 @@ private: { msg.velocity[0] = 5; msg.velocity[1] = 5; - msg.velocity[2] = -20; + msg.velocity[2] = 20; msg.yawspeed = 0; } else { From ac759ace1313425deb56011d989500df63ce5e28 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:38:25 +0200 Subject: [PATCH 081/188] change to not check for 30 --- src/px4_connection/src/px4_controller.cpp | 28 +++++++++++------------ 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 78b386c1..64e96508 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -59,23 +59,23 @@ private: void send_trajectory_setpoint() { auto msg = px4_msgs::msg::TrajectorySetpoint(); + msg.velocity[0] = 5; + msg.velocity[1] = 5; + msg.velocity[2] = -15; + msg.yawspeed = 0; - if (setpoint_count < 30) - { - msg.velocity[0] = 5; - msg.velocity[1] = 5; - msg.velocity[2] = 20; - msg.yawspeed = 0; + // if (setpoint_count < 30) + // { - } else { - //try to hover - msg.velocity[0] = 0; - msg.velocity[1] = 0; - msg.velocity[2] = 0; - msg.yawspeed = 1; + // } else { + // //try to hover + // msg.velocity[0] = 0; + // msg.velocity[1] = 0; + // msg.velocity[2] = 0; + // msg.yawspeed = 1; - } - msg.yaw = -3.14; + // } + msg.yaw = -3.14; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; trajectory_setpoint_publisher->publish(msg); From 04cad041b68034e59d31b24630b8b90385670b1d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:39:12 +0200 Subject: [PATCH 082/188] change value --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 64e96508..ebfd82f5 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -59,8 +59,8 @@ private: void send_trajectory_setpoint() { auto msg = px4_msgs::msg::TrajectorySetpoint(); - msg.velocity[0] = 5; - msg.velocity[1] = 5; + msg.velocity[0] = 0; + msg.velocity[1] = 0; msg.velocity[2] = -15; msg.yawspeed = 0; From 3f2ffd6dc7e3b91d01ccba02dbfa2110d59052f5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:43:02 +0200 Subject: [PATCH 083/188] add hover after 20 s --- src/px4_connection/src/px4_controller.cpp | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index ebfd82f5..3908dcfa 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -58,11 +58,25 @@ private: void send_trajectory_setpoint() { + auto msg = px4_msgs::msg::TrajectorySetpoint(); - msg.velocity[0] = 0; - msg.velocity[1] = 0; - msg.velocity[2] = -15; - msg.yawspeed = 0; + if (start_time_ - this->get_clock().now().seconds() < 20) + { + msg.velocity[0] = 0; + msg.velocity[1] = 0; + msg.velocity[2] = -15; + msg.yawspeed = 0; + } else { + if (!has_swithed) + { + RCLCPP_INFO(this->get_logger(), "switching to 0 vel"); + has_swithed = true; + } + msg.velocity[0] = 0; + msg.velocity[1] = 0; + msg.velocity[2] = 0; + msg.yawspeed = 0; + } // if (setpoint_count < 30) // { From 7b0520c9202c7e8b5e3e269dbcb9e83b9ed90579 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:43:41 +0200 Subject: [PATCH 084/188] use pointer method get --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 3908dcfa..b7ba3fa6 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -60,7 +60,7 @@ private: { auto msg = px4_msgs::msg::TrajectorySetpoint(); - if (start_time_ - this->get_clock().now().seconds() < 20) + if (start_time_ - this->get_clock()->now().seconds() < 20) { msg.velocity[0] = 0; msg.velocity[1] = 0; From 500356f4e94f811eebafe4e4afd53e697462e27b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:45:34 +0200 Subject: [PATCH 085/188] fix bug --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index b7ba3fa6..7240728c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -60,7 +60,7 @@ private: { auto msg = px4_msgs::msg::TrajectorySetpoint(); - if (start_time_ - this->get_clock()->now().seconds() < 20) + if (this->get_clock()->now().seconds() - start_time_ < 20) { msg.velocity[0] = 0; msg.velocity[1] = 0; From 568197a95aa741bcd7263adbe2fff3925abc10db Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:47:31 +0200 Subject: [PATCH 086/188] change to use d_speed --- src/px4_connection/src/px4_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 7240728c..b09d2a0a 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -60,11 +60,11 @@ private: { auto msg = px4_msgs::msg::TrajectorySetpoint(); - if (this->get_clock()->now().seconds() - start_time_ < 20) + if (this->get_clock()->now().seconds() - start_time_ < 10) { msg.velocity[0] = 0; msg.velocity[1] = 0; - msg.velocity[2] = -15; + msg.velocity[2] = D_SPEED(10); msg.yawspeed = 0; } else { if (!has_swithed) @@ -74,7 +74,7 @@ private: } msg.velocity[0] = 0; msg.velocity[1] = 0; - msg.velocity[2] = 0; + msg.velocity[2] = D_SPEED(0); msg.yawspeed = 0; } From eeb67733d3f33f15204999a429e5a6638ae59fd2 Mon Sep 17 00:00:00 2001 From: ubuntu Date: Mon, 1 May 2023 13:49:31 +0200 Subject: [PATCH 087/188] create drone_controls package --- get_build_run_controller.sh => get_build.sh | 4 +-- src/drone_controls/CMakeLists.txt | 33 +++++++++++++++++++++ src/drone_controls/package.xml | 20 +++++++++++++ src/px4_msgs | 2 +- 4 files changed, 56 insertions(+), 3 deletions(-) rename get_build_run_controller.sh => get_build.sh (62%) create mode 100644 src/drone_controls/CMakeLists.txt create mode 100644 src/drone_controls/package.xml diff --git a/get_build_run_controller.sh b/get_build.sh similarity index 62% rename from get_build_run_controller.sh rename to get_build.sh index 64ff7e9d..4686bb99 100755 --- a/get_build_run_controller.sh +++ b/get_build.sh @@ -3,5 +3,5 @@ echo "updating git..." ./fgit.sh echo "building package..." colcon build --packages-select px4_connection -echo "launching controller..." -ros2 run px4_connection px4_controller +#echo "launching controller..." +#ros2 run px4_connection px4_controller diff --git a/src/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt new file mode 100644 index 00000000..cc182294 --- /dev/null +++ b/src/drone_controls/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(drone_controls) + +# 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) +find_package(rclcpp REQUIRED) + +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/drone_controls/package.xml b/src/drone_controls/package.xml new file mode 100644 index 00000000..5d264637 --- /dev/null +++ b/src/drone_controls/package.xml @@ -0,0 +1,20 @@ + + + + drone_controls + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 From 65cd11ca11afcc50e49d1749a357532e14310deb Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 13:59:48 +0200 Subject: [PATCH 088/188] add changing yawspeed --- src/px4_connection/src/px4_controller.cpp | 4 ++-- src/px4_msgs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index b09d2a0a..2ae4a8b3 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -72,10 +72,10 @@ private: RCLCPP_INFO(this->get_logger(), "switching to 0 vel"); has_swithed = true; } - msg.velocity[0] = 0; + msg.velocity[0] = 1; msg.velocity[1] = 0; msg.velocity[2] = D_SPEED(0); - msg.yawspeed = 0; + msg.yawspeed = 1; } // if (setpoint_count < 30) diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 From c7dd7a25a038cfc24461b173d63dd6544b90c08f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:02:23 +0200 Subject: [PATCH 089/188] try changing yawspeed --- src/px4_connection/src/px4_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 2ae4a8b3..6d9c4bf3 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -66,6 +66,7 @@ private: msg.velocity[1] = 0; msg.velocity[2] = D_SPEED(10); msg.yawspeed = 0; + msg.yaw = -3.14; } else { if (!has_swithed) { @@ -76,6 +77,7 @@ private: msg.velocity[1] = 0; msg.velocity[2] = D_SPEED(0); msg.yawspeed = 1; + msg.yaw = 3.14; } // if (setpoint_count < 30) @@ -89,7 +91,7 @@ private: // msg.yawspeed = 1; // } - msg.yaw = -3.14; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; trajectory_setpoint_publisher->publish(msg); From 3e740de48e883031e75fd2b3e8b3005f4ead8274 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:07:09 +0200 Subject: [PATCH 090/188] add circling --- src/px4_connection/src/px4_controller.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6d9c4bf3..7411ca22 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -55,6 +55,7 @@ private: int setpoint_count = 0; float thrust = 0.5; bool ready_to_fly = false; + float cur_yaw = 0; void send_trajectory_setpoint() { @@ -73,11 +74,13 @@ private: RCLCPP_INFO(this->get_logger(), "switching to 0 vel"); has_swithed = true; } - msg.velocity[0] = 1; + + cur_yaw += 1 % 360; + msg.velocity[0] = 0; msg.velocity[1] = 0; msg.velocity[2] = D_SPEED(0); - msg.yawspeed = 1; - msg.yaw = 3.14; + msg.yawspeed = 0.5; + msg.yaw = degrees_to_radians(cur_yaw); } // if (setpoint_count < 30) From d82b1925a4c4857ccc2d02ef1c05fd88e010ac23 Mon Sep 17 00:00:00 2001 From: ubuntu Date: Mon, 1 May 2023 14:17:59 +0200 Subject: [PATCH 091/188] add srv file --- src/px4_connection/src/px4_controller.cpp | 5 ++--- src/px4_connection/srv/SetAttitude.srv | 9 +++++++++ src/px4_msgs | 2 +- 3 files changed, 12 insertions(+), 4 deletions(-) create mode 100644 src/px4_connection/srv/SetAttitude.srv diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 7411ca22..002e6ff6 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -75,12 +75,11 @@ private: has_swithed = true; } - cur_yaw += 1 % 360; - msg.velocity[0] = 0; + msg.velocity[0] = 5; msg.velocity[1] = 0; msg.velocity[2] = D_SPEED(0); msg.yawspeed = 0.5; - msg.yaw = degrees_to_radians(cur_yaw); + msg.yaw = degrees_to_radians(80); } // if (setpoint_count < 30) diff --git a/src/px4_connection/srv/SetAttitude.srv b/src/px4_connection/srv/SetAttitude.srv new file mode 100644 index 00000000..46896943 --- /dev/null +++ b/src/px4_connection/srv/SetAttitude.srv @@ -0,0 +1,9 @@ +#all speeds are in meters/second +float32 x_speed +float32 y_speed +float32 z_speed + +#angle is in degrees +float32 angle +--- +int status diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 From fb34e50b384e225093fe71ce268af4467359621e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:28:00 +0200 Subject: [PATCH 092/188] add service definition --- src/px4_connection/CMakeLists.txt | 5 +++++ src/px4_connection/package.xml | 31 ++++++++++++++++++------------- src/px4_msgs | 2 +- 3 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index c9cf9937..5dc275e0 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -20,6 +20,11 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(px4_ros_com REQUIRED) find_package(px4_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SetAttitude.srv" +) add_executable(heartbeat src/heartbeat.cpp) ament_target_dependencies( diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml index e0eaf074..2e73b9e9 100644 --- a/src/px4_connection/package.xml +++ b/src/px4_connection/package.xml @@ -1,20 +1,25 @@ - px4_connection - 0.0.0 - Package to communicate with PX4 through the XRCE-DDS bridge - ubuntu - Apache License 2.0 + px4_connection + 0.0.0 + Package to communicate with PX4 through the XRCE-DDS bridge + ubuntu + Apache License 2.0 - ament_cmake + ament_cmake + rosidl_default_generators - rclcpp + rosidl_default_runtime + rosidl_interface_packages - ament_lint_auto - ament_lint_common - - ament_cmake - - + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 From db2b25e60d50b9e875f78a1241be61ed8f37c14a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:29:11 +0200 Subject: [PATCH 093/188] change int to int8 --- src/px4_connection/srv/SetAttitude.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/srv/SetAttitude.srv b/src/px4_connection/srv/SetAttitude.srv index 46896943..0d85cba9 100644 --- a/src/px4_connection/srv/SetAttitude.srv +++ b/src/px4_connection/srv/SetAttitude.srv @@ -6,4 +6,4 @@ float32 z_speed #angle is in degrees float32 angle --- -int status +int8 status From e7582c5760f350dbfeb806c8af6de8928c09a6b2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:32:39 +0200 Subject: [PATCH 094/188] add srv include --- src/px4_connection/src/px4_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 002e6ff6..6474241e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,6 +17,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- // #include #include #include + +#include // #include #define D_SPEED(x) -x - 9.81 From 5996ac225a86b54a868ba146b5c8ecff489b1d5a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:34:07 +0200 Subject: [PATCH 095/188] add include own package --- src/px4_connection/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index 5dc275e0..d24cc36e 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(px4_ros_com REQUIRED) find_package(px4_msgs REQUIRED) +find_package(px4_connection REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} @@ -40,6 +41,7 @@ ament_target_dependencies( rclcpp px4_ros_com px4_msgs + px4_connection ) target_include_directories(heartbeat PUBLIC From 6129216d80b20356eee2d9b6dffa05ef0ef984a7 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:41:28 +0200 Subject: [PATCH 096/188] add message code hpp file --- src/px4_connection/CMakeLists.txt | 1 + src/px4_connection/include/attitude_msg_code.hpp | 8 ++++++++ src/px4_connection/src/px4_controller.cpp | 1 + 3 files changed, 10 insertions(+) create mode 100644 src/px4_connection/include/attitude_msg_code.hpp diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index d24cc36e..c66f40d3 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetAttitude.srv" ) +include_directories(include) add_executable(heartbeat src/heartbeat.cpp) ament_target_dependencies( heartbeat diff --git a/src/px4_connection/include/attitude_msg_code.hpp b/src/px4_connection/include/attitude_msg_code.hpp new file mode 100644 index 00000000..2a54b920 --- /dev/null +++ b/src/px4_connection/include/attitude_msg_code.hpp @@ -0,0 +1,8 @@ +#ifndef ATTITUDE_MSG_CODE_HPP +#define ATTITUDE_MSG_CODE_HPP + +#define ATTITUDE_STATUS_OK 0 +#define ATTITUDE_STATUS_ERROR 1 + + +#endif \ No newline at end of file diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6474241e..9c0e8359 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -11,6 +11,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include "rclcpp/rclcpp.hpp" +#include "attitude_msg_code.hpp" #include #include From b9b8c99f20f2ee05eb26556715c2244c7833193f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:46:06 +0200 Subject: [PATCH 097/188] add server function --- src/px4_connection/src/px4_controller.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9c0e8359..116ee6fc 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -37,6 +37,8 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + set_attitude_service = this->create_service("set_attitude", &set_setpoint); + // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -48,6 +50,8 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + rclcpp::Service::SharedPtr set_attitude_service_; + // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -60,6 +64,11 @@ private: bool ready_to_fly = false; float cur_yaw = 0; + void set_setpoint(const px4_connection::srv::SetAttitude::Request request, px4_connection::srv::SetAttitude::Response response) + { + + } + void send_trajectory_setpoint() { From 8f807bdfa344a8e6854ee20cc6fc70dbd7c0f078 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:47:22 +0200 Subject: [PATCH 098/188] try setting callback with bind --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 116ee6fc..a0dd064e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -37,7 +37,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service = this->create_service("set_attitude", &set_setpoint); + set_attitude_service = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); From 1c995253bdbd4631392e1809d801142dc686d009 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:48:08 +0200 Subject: [PATCH 099/188] oops --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index a0dd064e..bb1ee8a6 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -37,7 +37,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); From 1ff1218359cdcf6ed075995809235d7e75a14c92 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:50:41 +0200 Subject: [PATCH 100/188] try something else --- src/px4_connection/src/px4_controller.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index bb1ee8a6..3b144ee4 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -26,6 +26,10 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- using namespace std::chrono_literals; +void set_setpoint(const px4_connection::srv::SetAttitude::Request request, px4_connection::srv::SetAttitude::Response response) +{ +} + class PX4Controller : public rclcpp::Node { public: @@ -37,7 +41,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); + set_attitude_service_ = this->create_service("set_attitude", &set_setpoint); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -64,11 +68,6 @@ private: bool ready_to_fly = false; float cur_yaw = 0; - void set_setpoint(const px4_connection::srv::SetAttitude::Request request, px4_connection::srv::SetAttitude::Response response) - { - - } - void send_trajectory_setpoint() { @@ -80,7 +79,9 @@ private: msg.velocity[2] = D_SPEED(10); msg.yawspeed = 0; msg.yaw = -3.14; - } else { + } + else + { if (!has_swithed) { RCLCPP_INFO(this->get_logger(), "switching to 0 vel"); @@ -105,7 +106,7 @@ private: // msg.yawspeed = 1; // } - + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; trajectory_setpoint_publisher->publish(msg); From f4819381a5fadf27307678621519c88e3983c8e1 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:16:45 +0200 Subject: [PATCH 101/188] stupid --- src/px4_connection/src/px4_controller.cpp | 25 ++++++++--------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 3b144ee4..c6e1445b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -26,10 +26,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- using namespace std::chrono_literals; -void set_setpoint(const px4_connection::srv::SetAttitude::Request request, px4_connection::srv::SetAttitude::Response response) -{ -} - class PX4Controller : public rclcpp::Node { public: @@ -95,18 +91,6 @@ private: msg.yaw = degrees_to_radians(80); } - // if (setpoint_count < 30) - // { - - // } else { - // //try to hover - // msg.velocity[0] = 0; - // msg.velocity[1] = 0; - // msg.velocity[2] = 0; - // msg.yawspeed = 1; - - // } - msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; trajectory_setpoint_publisher->publish(msg); @@ -260,10 +244,17 @@ private: } }; +void set_setpoint(const px4_connection::srv::SetAttitude::Request request, px4_connection::srv::SetAttitude::Response response) +{ +} + int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + + rclcpp::Node node = PX4Controller(); + node.create_service("set_attitude", &set_setpoint); + rclcpp::spin(node); rclcpp::shutdown(); return 0; } From 9abee2b9656c9f4144a2280431428555fa8427c8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:18:06 +0200 Subject: [PATCH 102/188] add shared ptr --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index c6e1445b..a5bd4348 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -244,7 +244,7 @@ private: } }; -void set_setpoint(const px4_connection::srv::SetAttitude::Request request, px4_connection::srv::SetAttitude::Response response) +void set_setpoint(const std::shared_ptr request, std::shared_ptr response) { } From de2af0cf7c8fe1d1731c9500555c543186866e9b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:27:15 +0200 Subject: [PATCH 103/188] try according to tutorial --- src/px4_connection/src/px4_controller.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index a5bd4348..02532700 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -37,7 +37,12 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", &set_setpoint); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); + + // service_ptr_ = this->create_service( + // "test_service", + // std::bind(&ServiceNode::service_callback, this, _1, _2, _3) + // ); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -64,6 +69,14 @@ private: bool ready_to_fly = false; float cur_yaw = 0; + void set_setpoint( + // const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + + } + void send_trajectory_setpoint() { @@ -244,16 +257,11 @@ private: } }; -void set_setpoint(const std::shared_ptr request, std::shared_ptr response) -{ -} - int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - rclcpp::Node node = PX4Controller(); - node.create_service("set_attitude", &set_setpoint); + rclcpp::Node::SharedPtr node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; From 8f7e4961072818d8e6c65588dc331a19efa90e03 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:28:58 +0200 Subject: [PATCH 104/188] add placeholders --- src/px4_connection/src/px4_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 02532700..8fb6813c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -25,6 +25,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #define D_SPEED(x) -x - 9.81 using namespace std::chrono_literals; +using namespace std::placeholders; class PX4Controller : public rclcpp::Node { @@ -37,7 +38,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, _1, _2, _3)); // service_ptr_ = this->create_service( // "test_service", From d74b7db6b394caa3b7ee7bf2592711977abbacf9 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:29:57 +0200 Subject: [PATCH 105/188] add request header? --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 8fb6813c..30fc3b5f 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -71,7 +71,7 @@ private: float cur_yaw = 0; void set_setpoint( - // const std::shared_ptr request_header, + const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) { From 22126929a7ba5f3b97717ce4f3b6d62f81b0b76e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:30:30 +0200 Subject: [PATCH 106/188] remove placeholders --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 30fc3b5f..1b8f8a25 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -38,7 +38,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, _1, _2, _3)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); // service_ptr_ = this->create_service( // "test_service", From 4846ee5052f1c9f349e272ff99f510e1ec56a920 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:31:02 +0200 Subject: [PATCH 107/188] add placeholders back --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 1b8f8a25..30fc3b5f 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -38,7 +38,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, _1, _2, _3)); // service_ptr_ = this->create_service( // "test_service", From 0b98dfbf02b691143ce4b636f2ece5b73dc5cabc Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:41:51 +0200 Subject: [PATCH 108/188] add processing of service request --- src/px4_connection/src/px4_controller.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 30fc3b5f..89170207 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -70,12 +70,21 @@ private: bool ready_to_fly = false; float cur_yaw = 0; + float last_setpoint[3] = {0,0,0}; + float last_angle = 0; + void set_setpoint( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) { + last_setpoint[0] = request.x_speed; + last_setpoint[1] = request.y_speed; + last_setpoint[2] = request.x_speed; + last_angle = degrees_to_radians(request.angle); + + response.status = ATTITUDE_STATUS_OK; } void send_trajectory_setpoint() @@ -98,11 +107,11 @@ private: has_swithed = true; } - msg.velocity[0] = 5; - msg.velocity[1] = 0; - msg.velocity[2] = D_SPEED(0); + msg.velocity[0] = last_setpoint[0]; + msg.velocity[1] = last_setpoint[1]; + msg.velocity[2] = D_SPEED(last_setpoint[2]); msg.yawspeed = 0.5; - msg.yaw = degrees_to_radians(80); + msg.yaw = last_angle; } msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; From baad3abae2c1536189735ad2d23c91d1ace97bbc Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:42:29 +0200 Subject: [PATCH 109/188] use pointers --- src/px4_connection/src/px4_controller.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 89170207..705aa995 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -78,13 +78,13 @@ private: const std::shared_ptr request, const std::shared_ptr response) { - last_setpoint[0] = request.x_speed; - last_setpoint[1] = request.y_speed; - last_setpoint[2] = request.x_speed; + last_setpoint[0] = request->x_speed; + last_setpoint[1] = request->y_speed; + last_setpoint[2] = request->x_speed; - last_angle = degrees_to_radians(request.angle); + last_angle = degrees_to_radians(request->angle); - response.status = ATTITUDE_STATUS_OK; + response->status = ATTITUDE_STATUS_OK; } void send_trajectory_setpoint() From a24c145968cb1be8c0dc01216b6729fb44799904 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:48:06 +0200 Subject: [PATCH 110/188] remove attitude message codes --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 705aa995..450a4ac9 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -11,7 +11,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include "rclcpp/rclcpp.hpp" -#include "attitude_msg_code.hpp" +// #include "attitude_msg_code.hpp" #include #include @@ -84,7 +84,7 @@ private: last_angle = degrees_to_radians(request->angle); - response->status = ATTITUDE_STATUS_OK; + response->status = 0; } void send_trajectory_setpoint() From 8be8a6d1f323da1ffcf601fc5135bdf63bd6a933 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:51:01 +0200 Subject: [PATCH 111/188] bull --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 450a4ac9..31b34f11 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -38,7 +38,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, _1, _2, _3)); + // set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, _1, _2, _3)); // service_ptr_ = this->create_service( // "test_service", From a919c5b7f70e05ac29f7a8db7dffd02004c7fddc Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:52:38 +0200 Subject: [PATCH 112/188] add test --- src/px4_connection/src/px4_controller.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 31b34f11..f7e2cdae 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -30,6 +30,15 @@ using namespace std::placeholders; class PX4Controller : public rclcpp::Node { public: + + void yeet( + const std::shared_ptr request, + const std::shared_ptr response) + { + + + } + PX4Controller() : Node("px4_controller") { // create a publisher on the vehicle attitude setpoint topic @@ -38,6 +47,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + set_attitude_service_ = this->create_service("set_attitude", &yeet); // set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, _1, _2, _3)); // service_ptr_ = this->create_service( From 8efa23948d6dfb37f7922d34c96693f4ae725b0a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:53:39 +0200 Subject: [PATCH 113/188] no placeholders? --- src/px4_connection/src/px4_controller.cpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index f7e2cdae..98d51a3e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -30,15 +30,6 @@ using namespace std::placeholders; class PX4Controller : public rclcpp::Node { public: - - void yeet( - const std::shared_ptr request, - const std::shared_ptr response) - { - - - } - PX4Controller() : Node("px4_controller") { // create a publisher on the vehicle attitude setpoint topic @@ -47,8 +38,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", &yeet); - // set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, _1, _2, _3)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); // service_ptr_ = this->create_service( // "test_service", From ea4cfb17ec62ef1dcfbc4871b27e45036ac8c2e0 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:55:15 +0200 Subject: [PATCH 114/188] test --- src/px4_connection/src/px4_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 98d51a3e..e2848852 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -25,7 +25,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #define D_SPEED(x) -x - 9.81 using namespace std::chrono_literals; -using namespace std::placeholders; class PX4Controller : public rclcpp::Node { @@ -38,7 +37,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1)); // service_ptr_ = this->create_service( // "test_service", From c2193594879296d25c1172ed0fba7d4c961cd4fa Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 15:58:15 +0200 Subject: [PATCH 115/188] test --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e2848852..e614531b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -37,7 +37,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); // service_ptr_ = this->create_service( // "test_service", From bb3942c301deb225b33386c6ced60416f20b381f Mon Sep 17 00:00:00 2001 From: ubuntu Date: Mon, 1 May 2023 16:02:19 +0200 Subject: [PATCH 116/188] add services package --- src/drone_services/CMakeLists.txt | 33 +++++++++++++++++++++++++++++++ src/drone_services/package.xml | 20 +++++++++++++++++++ src/px4_msgs | 2 +- 3 files changed, 54 insertions(+), 1 deletion(-) create mode 100644 src/drone_services/CMakeLists.txt create mode 100644 src/drone_services/package.xml diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt new file mode 100644 index 00000000..b89e2577 --- /dev/null +++ b/src/drone_services/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(drone_services) + +# 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) +find_package(rclcpp REQUIRED) + +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/drone_services/package.xml b/src/drone_services/package.xml new file mode 100644 index 00000000..099494dd --- /dev/null +++ b/src/drone_services/package.xml @@ -0,0 +1,20 @@ + + + + drone_services + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 From 0f0c84cf0c0c1cd909a61c565367a8ad0479550e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 16:06:43 +0200 Subject: [PATCH 117/188] add separate package for service --- src/drone_services/CMakeLists.txt | 5 ++++ src/drone_services/package.xml | 29 ++++++++++--------- .../srv/SetAttitude.srv | 0 src/px4_connection/CMakeLists.txt | 9 ++---- src/px4_connection/package.xml | 5 ---- src/px4_msgs | 2 +- 6 files changed, 24 insertions(+), 26 deletions(-) rename src/{px4_connection => drone_services}/srv/SetAttitude.srv (100%) diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index b89e2577..48b1bf31 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -18,6 +18,11 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SetAttitude.srv" +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/drone_services/package.xml b/src/drone_services/package.xml index 099494dd..af4dc178 100644 --- a/src/drone_services/package.xml +++ b/src/drone_services/package.xml @@ -1,20 +1,23 @@ - drone_services - 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration + drone_services + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration - ament_cmake + ament_cmake - rclcpp + rclcpp + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages - ament_lint_auto - ament_lint_common + ament_lint_auto + ament_lint_common - - ament_cmake - - + + ament_cmake + + \ No newline at end of file diff --git a/src/px4_connection/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv similarity index 100% rename from src/px4_connection/srv/SetAttitude.srv rename to src/drone_services/srv/SetAttitude.srv diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index c66f40d3..96e592bc 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -20,12 +20,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(px4_ros_com REQUIRED) find_package(px4_msgs REQUIRED) -find_package(px4_connection REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/SetAttitude.srv" -) +find_package(drone_services REQUIRED) include_directories(include) add_executable(heartbeat src/heartbeat.cpp) @@ -42,7 +37,7 @@ ament_target_dependencies( rclcpp px4_ros_com px4_msgs - px4_connection + drone_services ) target_include_directories(heartbeat PUBLIC diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml index 2e73b9e9..458ea4e3 100644 --- a/src/px4_connection/package.xml +++ b/src/px4_connection/package.xml @@ -8,11 +8,6 @@ Apache License 2.0 ament_cmake - rosidl_default_generators - - rosidl_default_runtime - rosidl_interface_packages - rclcpp diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 From 64464d06a5ed4fef1af1efd577b1ea95a3f0ddfe Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 16:07:49 +0200 Subject: [PATCH 118/188] remove srv from px4controller --- src/px4_connection/src/px4_controller.cpp | 28 +++++++++++------------ 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e614531b..9ff112ca 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,7 +19,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include -#include + // #include #define D_SPEED(x) -x - 9.81 @@ -37,7 +37,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); + // set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); // service_ptr_ = this->create_service( // "test_service", @@ -55,7 +55,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; - rclcpp::Service::SharedPtr set_attitude_service_; + // rclcpp::Service::SharedPtr set_attitude_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; @@ -72,19 +72,19 @@ private: float last_setpoint[3] = {0,0,0}; float last_angle = 0; - void set_setpoint( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) - { - last_setpoint[0] = request->x_speed; - last_setpoint[1] = request->y_speed; - last_setpoint[2] = request->x_speed; + // void set_setpoint( + // const std::shared_ptr request_header, + // const std::shared_ptr request, + // const std::shared_ptr response) + // { + // last_setpoint[0] = request->x_speed; + // last_setpoint[1] = request->y_speed; + // last_setpoint[2] = request->x_speed; - last_angle = degrees_to_radians(request->angle); + // last_angle = degrees_to_radians(request->angle); - response->status = 0; - } + // response->status = 0; + // } void send_trajectory_setpoint() { From a9b91d7a497f925031bd1eeaa868a865c830067b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 16:11:24 +0200 Subject: [PATCH 119/188] add drone services to package.xml --- src/px4_connection/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml index 458ea4e3..57fb022d 100644 --- a/src/px4_connection/package.xml +++ b/src/px4_connection/package.xml @@ -10,6 +10,9 @@ ament_cmake rclcpp + px4_ros_com + px4_msgs + drone_services ament_lint_auto ament_lint_common From 2513e56631a32c5e55e8af377c689c26c38c5d2e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 16:13:55 +0200 Subject: [PATCH 120/188] add include srv --- src/px4_connection/src/px4_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9ff112ca..9dcd0749 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,6 +19,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include +#include + // #include From 767c6589004379c0cd4648099e4657b278c9f65c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 16:15:49 +0200 Subject: [PATCH 121/188] add service back --- src/px4_connection/src/px4_controller.cpp | 31 ++++++++++------------- 1 file changed, 13 insertions(+), 18 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9dcd0749..cd011ccc 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -39,12 +39,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - // set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); - - // service_ptr_ = this->create_service( - // "test_service", - // std::bind(&ServiceNode::service_callback, this, _1, _2, _3) - // ); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -57,7 +52,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; - // rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr set_attitude_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; @@ -74,19 +69,19 @@ private: float last_setpoint[3] = {0,0,0}; float last_angle = 0; - // void set_setpoint( - // const std::shared_ptr request_header, - // const std::shared_ptr request, - // const std::shared_ptr response) - // { - // last_setpoint[0] = request->x_speed; - // last_setpoint[1] = request->y_speed; - // last_setpoint[2] = request->x_speed; + void set_setpoint( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + last_setpoint[0] = request->x_speed; + last_setpoint[1] = request->y_speed; + last_setpoint[2] = request->x_speed; - // last_angle = degrees_to_radians(request->angle); + last_angle = degrees_to_radians(request->angle); - // response->status = 0; - // } + response->status = 0; + } void send_trajectory_setpoint() { From d149b72c245eeb316b3cd5346cad9f583c304075 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 16:17:39 +0200 Subject: [PATCH 122/188] change printout --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index cd011ccc..fa810c06 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -99,7 +99,7 @@ private: { if (!has_swithed) { - RCLCPP_INFO(this->get_logger(), "switching to 0 vel"); + RCLCPP_INFO(this->get_logger(), "waiting for service input..."); has_swithed = true; } From 4249f2b58963d7b7f54f3808686f92c5b8198803 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 11:05:51 +0200 Subject: [PATCH 123/188] add setAttitude service --- src/drone_services/CMakeLists.txt | 1 + src/drone_services/srv/SetAttitude.srv | 12 ++++++------ src/drone_services/srv/SetVelocity.srv | 9 +++++++++ 3 files changed, 16 insertions(+), 6 deletions(-) create mode 100644 src/drone_services/srv/SetVelocity.srv diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 48b1bf31..d63e092b 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetAttitude.srv" + "srv/SetVelocity.srv" ) if(BUILD_TESTING) diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv index 0d85cba9..52957c0c 100644 --- a/src/drone_services/srv/SetAttitude.srv +++ b/src/drone_services/srv/SetAttitude.srv @@ -1,9 +1,9 @@ -#all speeds are in meters/second -float32 x_speed -float32 y_speed -float32 z_speed +#all angles are in degrees +float32 yaw +float32 pitch +float32 roll -#angle is in degrees -float32 angle +#thrust between -1 and 1 +float32 thrust --- int8 status diff --git a/src/drone_services/srv/SetVelocity.srv b/src/drone_services/srv/SetVelocity.srv new file mode 100644 index 00000000..0d85cba9 --- /dev/null +++ b/src/drone_services/srv/SetVelocity.srv @@ -0,0 +1,9 @@ +#all speeds are in meters/second +float32 x_speed +float32 y_speed +float32 z_speed + +#angle is in degrees +float32 angle +--- +int8 status From 0b5d4c2c4f668351b06787b28243b198f0ad454e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 11:08:53 +0200 Subject: [PATCH 124/188] change px4 controller to setvelocity service --- src/px4_connection/src/px4_controller.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fa810c06..0b6b7c07 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,7 +19,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include -#include +#include // #include @@ -39,7 +39,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -52,7 +52,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; - rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr set_attitude_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; @@ -71,8 +71,8 @@ private: void set_setpoint( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { last_setpoint[0] = request->x_speed; last_setpoint[1] = request->y_speed; From f23415f60dee372520a147b6ca6088f91e562e74 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 11:24:06 +0200 Subject: [PATCH 125/188] test --- src/px4_connection/src/heartbeat.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 7f6459d2..4ad9e9fc 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -22,7 +22,7 @@ public: HeartBeat() : 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); + // 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(&HeartBeat::send_heartbeat, this)); start_time = this->get_clock()->now().seconds(); From 716c80d9054797609b96e6b10d5844e21cef0661 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 11:27:05 +0200 Subject: [PATCH 126/188] add back offboard control mode publisher --- src/px4_connection/src/heartbeat.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 4ad9e9fc..7f6459d2 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -22,7 +22,7 @@ public: HeartBeat() : 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); + 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(&HeartBeat::send_heartbeat, this)); start_time = this->get_clock()->now().seconds(); From ce89e730fa98a30e8f88914d8f12ef7c5a239884 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 11:40:15 +0200 Subject: [PATCH 127/188] change to attitude setpoints --- src/px4_connection/src/heartbeat.cpp | 4 ++-- src/px4_connection/src/px4_controller.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 7f6459d2..52f6f87d 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -41,9 +41,9 @@ private: // set message to enable attitude auto msg = px4_msgs::msg::OffboardControlMode(); msg.position = false; - msg.velocity = true; + msg.velocity = false; msg.acceleration = false; - msg.attitude = false; + msg.attitude = true; msg.body_rate = false; msg.actuator = false; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 0b6b7c07..efa818ab 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -200,7 +200,7 @@ private: flying = true; } - send_trajectory_setpoint(); + send_attitude_setpoint(); } /** From 517240a46329661b2f025f6b7801b1bfaecd640e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 11:43:35 +0200 Subject: [PATCH 128/188] change to gradually increase thrust --- src/px4_connection/src/px4_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index efa818ab..63cf066b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -62,7 +62,7 @@ private: bool flying = false; bool has_swithed = false; int setpoint_count = 0; - float thrust = 0.5; + float thrust = 0.0; bool ready_to_fly = false; float cur_yaw = 0; @@ -119,7 +119,7 @@ private: { if (setpoint_count % 20 == 0 && thrust <= 1) { - thrust += 0.1; + thrust += 0.05; RCLCPP_INFO(this->get_logger(), "increasing thrust to %f", thrust); } @@ -133,7 +133,7 @@ private: // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -0.8; // down, 100% thrust up + msg.thrust_body[2] = -thrust; // down, 100% thrust up calculate_quaternion(q, 0, 0, 0); } From 1e521ea890a19ec5f51e4b83c4d425fa1bbc7395 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 11:54:57 +0200 Subject: [PATCH 129/188] add service control --- src/px4_connection/src/px4_controller.cpp | 47 ++++++++--------------- 1 file changed, 17 insertions(+), 30 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 63cf066b..83e3fd9b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,7 +19,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include -#include +#include // #include @@ -39,7 +39,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -67,18 +67,20 @@ private: float cur_yaw = 0; float last_setpoint[3] = {0,0,0}; - float last_angle = 0; + float last_thrust = 0; void set_setpoint( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { - last_setpoint[0] = request->x_speed; - last_setpoint[1] = request->y_speed; - last_setpoint[2] = request->x_speed; + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); + last_setpoint[2] = degrees_to_radians(request->roll); - last_angle = degrees_to_radians(request->angle); + last_thrust = request->thrust; + + RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2] last_thrust); response->status = 0; } @@ -117,40 +119,23 @@ private: void send_attitude_setpoint() { - if (setpoint_count % 20 == 0 && thrust <= 1) - { - thrust += 0.05; - RCLCPP_INFO(this->get_logger(), "increasing thrust to %f", thrust); - } // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); // result quaternion std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 10) + if (this->get_clock()->now().seconds() - start_time_ < 30) { // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -thrust; // down, 100% thrust up + msg.thrust_body[2] = -last_thrust; - calculate_quaternion(q, 0, 0, 0); - } + RCLCPP_INFO(this->get_logger(), "Thrust: %f", msg.thrust_body[2]); - else if (this->get_clock()->now().seconds() - start_time_ < 30) - { - if (!has_swithed) - { - RCLCPP_INFO(this->get_logger(), "changing to 0.5 thrust"); - has_swithed = true; - } + calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -0.5; // down, 100% thrust up - - calculate_quaternion(q, 0, 0, 0); } else { @@ -162,6 +147,8 @@ private: } } + + // set quaternion msg.q_d[0] = q.at(0); msg.q_d[1] = q.at(1); msg.q_d[2] = q.at(2); From e25abc3973daadb0074f0b3e9fd76160e9a86f5a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 12:00:57 +0200 Subject: [PATCH 130/188] fix bug --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 83e3fd9b..2ebb904e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -52,7 +52,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; - rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr set_attitude_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; From b0587139033bb3f6b77a5cc55fe26ba82692b871 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 12:01:36 +0200 Subject: [PATCH 131/188] fix bug v2 --- src/px4_connection/src/px4_controller.cpp | 3 +-- src/px4_msgs | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 83e3fd9b..789b4b14 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -80,7 +80,7 @@ private: last_thrust = request->thrust; - RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2] last_thrust); + RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_thrust); response->status = 0; } @@ -109,7 +109,6 @@ private: msg.velocity[1] = last_setpoint[1]; msg.velocity[2] = D_SPEED(last_setpoint[2]); msg.yawspeed = 0.5; - msg.yaw = last_angle; } msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 From dd474bf8ea44ed8a42697072a3ca91d9752cd44b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 12:11:46 +0200 Subject: [PATCH 132/188] add more log --- src/px4_connection/src/px4_controller.cpp | 2 +- src/px4_msgs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9f6893fc..cccfd5ff 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -131,7 +131,7 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = -last_thrust; - RCLCPP_INFO(this->get_logger(), "Thrust: %f", msg.thrust_body[2]); + RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_thrust); calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 From e134bb2b72318b1c81a0108375bad3224e577dbd Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 13:48:41 +0200 Subject: [PATCH 133/188] add python package for testing controls --- src/test_controls/package.xml | 18 +++++++++++++ src/test_controls/resource/test_controls | 0 src/test_controls/setup.cfg | 4 +++ src/test_controls/setup.py | 25 +++++++++++++++++++ src/test_controls/test/test_copyright.py | 23 +++++++++++++++++ src/test_controls/test/test_flake8.py | 25 +++++++++++++++++++ src/test_controls/test/test_pep257.py | 23 +++++++++++++++++ src/test_controls/test_controls/__init__.py | 0 .../test_controls/test_controller.py | 0 9 files changed, 118 insertions(+) create mode 100644 src/test_controls/package.xml create mode 100644 src/test_controls/resource/test_controls create mode 100644 src/test_controls/setup.cfg create mode 100644 src/test_controls/setup.py create mode 100644 src/test_controls/test/test_copyright.py create mode 100644 src/test_controls/test/test_flake8.py create mode 100644 src/test_controls/test/test_pep257.py create mode 100644 src/test_controls/test_controls/__init__.py create mode 100644 src/test_controls/test_controls/test_controller.py diff --git a/src/test_controls/package.xml b/src/test_controls/package.xml new file mode 100644 index 00000000..06506d70 --- /dev/null +++ b/src/test_controls/package.xml @@ -0,0 +1,18 @@ + + + + test_controls + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/test_controls/resource/test_controls b/src/test_controls/resource/test_controls new file mode 100644 index 00000000..e69de29b diff --git a/src/test_controls/setup.cfg b/src/test_controls/setup.cfg new file mode 100644 index 00000000..5dc0dac5 --- /dev/null +++ b/src/test_controls/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/test_controls +[install] +install-scripts=$base/lib/test_controls diff --git a/src/test_controls/setup.py b/src/test_controls/setup.py new file mode 100644 index 00000000..65d5a88b --- /dev/null +++ b/src/test_controls/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'test_controls' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ubuntu', + maintainer_email='semmer99@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/test_controls/test/test_copyright.py b/src/test_controls/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/src/test_controls/test/test_copyright.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_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/test_controls/test/test_flake8.py b/src/test_controls/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/test_controls/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/src/test_controls/test/test_pep257.py b/src/test_controls/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/test_controls/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' diff --git a/src/test_controls/test_controls/__init__.py b/src/test_controls/test_controls/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py new file mode 100644 index 00000000..e69de29b From 9852edb8b665a0c72725c8411c7f82d823252861 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 13:48:52 +0200 Subject: [PATCH 134/188] add python package for testing controls --- src/px4_msgs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_msgs b/src/px4_msgs index b64ef047..ffc3a4cd 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 +Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 From b971c7989e337fdb3aef507f6c22b5841cefcbcf Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 13:55:06 +0200 Subject: [PATCH 135/188] change to acceleration setpoints --- src/px4_connection/src/heartbeat.cpp | 4 +- src/px4_connection/src/px4_controller.cpp | 49 ++++++++++++----------- 2 files changed, 27 insertions(+), 26 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 52f6f87d..6b7b9198 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -42,8 +42,8 @@ private: auto msg = px4_msgs::msg::OffboardControlMode(); msg.position = false; msg.velocity = false; - msg.acceleration = false; - msg.attitude = true; + msg.acceleration = true; + msg.attitude = false; msg.body_rate = false; msg.actuator = false; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index cccfd5ff..1c6af829 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -39,7 +39,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -52,7 +52,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; - rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr set_attitude_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; @@ -67,20 +67,21 @@ private: float cur_yaw = 0; float last_setpoint[3] = {0,0,0}; + float last_angle = 0; float last_thrust = 0; void set_setpoint( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { - last_setpoint[0] = degrees_to_radians(request->yaw); - last_setpoint[1] = degrees_to_radians(request->pitch); - last_setpoint[2] = degrees_to_radians(request->roll); + last_setpoint[0] = request->x_speed; + last_setpoint[1] = request->y_speed; + last_setpoint[2] = request->z_speed; - last_thrust = request->thrust; + last_angle = request->angle; - RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_thrust); + RCLCPP_INFO(this->get_logger(), "New setpoint: x:%f y:%f z:%f angle:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_angle); response->status = 0; } @@ -91,9 +92,9 @@ private: auto msg = px4_msgs::msg::TrajectorySetpoint(); if (this->get_clock()->now().seconds() - start_time_ < 10) { - msg.velocity[0] = 0; - msg.velocity[1] = 0; - msg.velocity[2] = D_SPEED(10); + msg.acceleration[0] = 0; + msg.acceleration[1] = 0; + msg.acceleration[2] = D_SPEED(10); msg.yawspeed = 0; msg.yaw = -3.14; } @@ -105,9 +106,9 @@ private: has_swithed = true; } - msg.velocity[0] = last_setpoint[0]; - msg.velocity[1] = last_setpoint[1]; - msg.velocity[2] = D_SPEED(last_setpoint[2]); + msg.acceleration[0] = last_setpoint[0]; + msg.acceleration[1] = last_setpoint[1]; + msg.acceleration[2] = D_SPEED(last_setpoint[2]); msg.yawspeed = 0.5; } @@ -136,15 +137,15 @@ private: calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); } - else - { - if (flying) - { - publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); - flying = false; - RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds"); - } - } + // else + // { + // if (flying) + // { + // publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + // flying = false; + // RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds"); + // } + // } // set quaternion From dab4077ceff4316ac2f2845877d7e67126ff2794 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 13:55:19 +0200 Subject: [PATCH 136/188] change to acceleration setpoints --- src/px4_msgs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 From 7d2624e7170a0a40754a49761d7fceea779404a5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 14:08:24 +0200 Subject: [PATCH 137/188] add import for velocity service --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 1c6af829..eed4cbc8 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,7 +19,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include -#include +#include // #include From 5b04a69e78dfd6c9587cd0e24c3ee61c05c5ff6e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 14:26:27 +0200 Subject: [PATCH 138/188] change to trajectorysetpoint --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index eed4cbc8..e5d88b3b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -187,7 +187,7 @@ private: flying = true; } - send_attitude_setpoint(); + send_trajectory_setpoint(); } /** From 38079342fff4d3f0e3f33667474ea1edad60703a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 15:40:54 +0200 Subject: [PATCH 139/188] change to attitude --- src/px4_connection/src/px4_controller.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e5d88b3b..104a4e73 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -19,7 +19,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include -#include +#include // #include @@ -39,7 +39,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -52,7 +52,7 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; - rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr set_attitude_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; @@ -72,16 +72,16 @@ private: void set_setpoint( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { - last_setpoint[0] = request->x_speed; - last_setpoint[1] = request->y_speed; - last_setpoint[2] = request->z_speed; + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); + last_setpoint[2] = degrees_to_radians(request->roll); - last_angle = request->angle; + last_thrust = request->thrust; - RCLCPP_INFO(this->get_logger(), "New setpoint: x:%f y:%f z:%f angle:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_angle); + RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); response->status = 0; } From b0f261848cadf8f576afe573cc3e60076184899d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 16:10:01 +0200 Subject: [PATCH 140/188] change to attitude setpoint --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 104a4e73..f9669fcf 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -187,7 +187,7 @@ private: flying = true; } - send_trajectory_setpoint(); + send_attitude_setpoint(); } /** From 0a18a68f5c429c2cff78f146084718c226538fe5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 16:24:24 +0200 Subject: [PATCH 141/188] change to attitude heartbeat --- src/px4_connection/src/heartbeat.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 6b7b9198..52f6f87d 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -42,8 +42,8 @@ private: auto msg = px4_msgs::msg::OffboardControlMode(); msg.position = false; msg.velocity = false; - msg.acceleration = true; - msg.attitude = false; + msg.acceleration = false; + msg.attitude = true; msg.body_rate = false; msg.actuator = false; From 09b6c6110e8c36943a0b139633975730affd3f08 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 2 May 2023 16:39:13 +0200 Subject: [PATCH 142/188] change to use _body msg params --- src/px4_connection/src/px4_controller.cpp | 32 +++++++++++------------ 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index f9669fcf..87d50d9d 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -21,7 +21,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include - // #include #define D_SPEED(x) -x - 9.81 @@ -39,7 +38,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3)); + set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -66,9 +65,11 @@ private: bool ready_to_fly = false; float cur_yaw = 0; - float last_setpoint[3] = {0,0,0}; + float last_setpoint[3] = {0, 0, 0}; float last_angle = 0; float last_thrust = 0; + // result quaternion + std::array q = {0, 0, 0, 0}; void set_setpoint( const std::shared_ptr request_header, @@ -122,21 +123,21 @@ private: // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); - // result quaternion - std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 30) - { - // move up? - msg.thrust_body[0] = 0; // north - msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = -last_thrust; + // if (this->get_clock()->now().seconds() - start_time_ < 30) + // { + msg.yaw_body = last_setpoint[0]; + msg.pitch_body = last_setpoint[1]; + msg.roll_body = last_setpoint[2]; + // move up? + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = -last_thrust; - RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f",last_setpoint[0],last_setpoint[1],last_setpoint[2], last_thrust); + RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); - calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); - - } + calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); + // } // else // { // if (flying) @@ -147,7 +148,6 @@ private: // } // } - // set quaternion msg.q_d[0] = q.at(0); msg.q_d[1] = q.at(1); From 796c74f318f1ba5bf6729aafb4ab2cedb5f85492 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 11:48:25 +0200 Subject: [PATCH 143/188] add disarm service --- src/px4_connection/CMakeLists.txt | 2 ++ src/px4_connection/package.xml | 1 + src/px4_connection/src/px4_controller.cpp | 22 ++++++++++++++++++++-- 3 files changed, 23 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index 96e592bc..638d46f7 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(rclcpp REQUIRED) find_package(px4_ros_com REQUIRED) find_package(px4_msgs REQUIRED) find_package(drone_services REQUIRED) +find_package(std_srvs REQUIRED) include_directories(include) add_executable(heartbeat src/heartbeat.cpp) @@ -38,6 +39,7 @@ ament_target_dependencies( px4_ros_com px4_msgs drone_services + std_srvs ) target_include_directories(heartbeat PUBLIC diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml index 57fb022d..604e89b2 100644 --- a/src/px4_connection/package.xml +++ b/src/px4_connection/package.xml @@ -13,6 +13,7 @@ px4_ros_com px4_msgs drone_services + std_srvs ament_lint_auto ament_lint_common diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 87d50d9d..40663c6a 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -15,12 +15,13 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include -// #include #include #include #include +#include + // #include #define D_SPEED(x) -x - 9.81 @@ -38,7 +39,8 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + set_attitude_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); @@ -52,6 +54,7 @@ private: rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr disarm_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; @@ -87,6 +90,21 @@ private: response->status = 0; } +/** + * @brief disarms the drone when a call to the disarm service is made + * + * @param request_header the header for the service request + * @param request the request (empty) + * @param response the response (empty) + */ + void handle_disarm_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + } + void send_trajectory_setpoint() { From d084827f6785e1693559dcda3b77177dcb5bb575 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 11:54:41 +0200 Subject: [PATCH 144/188] add empty service --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 40663c6a..153a5960 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -40,7 +40,7 @@ public: // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - set_attitude_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); From ade2b38a58e6c32a227f4a1f792591080b70df96 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 12:05:03 +0200 Subject: [PATCH 145/188] add extra checks for when armed or not armed --- src/px4_connection/src/px4_controller.cpp | 45 ++++++++++++++++------- 1 file changed, 31 insertions(+), 14 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 153a5960..859123b7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -62,6 +62,7 @@ private: double start_time_; bool has_sent_status = false; bool flying = false; + bool armed = false; bool has_swithed = false; int setpoint_count = 0; float thrust = 0.0; @@ -71,6 +72,7 @@ private: float last_setpoint[3] = {0, 0, 0}; float last_angle = 0; float last_thrust = 0; + // result quaternion std::array q = {0, 0, 0, 0}; @@ -79,30 +81,44 @@ private: const std::shared_ptr request, const std::shared_ptr response) { - last_setpoint[0] = degrees_to_radians(request->yaw); - last_setpoint[1] = degrees_to_radians(request->pitch); - last_setpoint[2] = degrees_to_radians(request->roll); + if (armed) + { + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); + last_setpoint[2] = degrees_to_radians(request->roll); - last_thrust = request->thrust; + last_thrust = request->thrust; - RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); + RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); - response->status = 0; + response->status = 0; + } else + { + last_thrust = 0; + last_setpoint = {0, 0, 0}; + response->status = 1; + } } -/** - * @brief disarms the drone when a call to the disarm service is made - * - * @param request_header the header for the service request - * @param request the request (empty) - * @param response the response (empty) - */ + /** + * @brief disarms the drone when a call to the disarm service is made + * + * @param request_header the header for the service request + * @param request the request (empty) + * @param response the response (empty) + */ void handle_disarm_request( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) { - publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + RCLCPP_INFO(this->get_logger(), "Got disarm request..."); + + if (armed) + { + armed = false; + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + } } void send_trajectory_setpoint() @@ -203,6 +219,7 @@ private: RCLCPP_INFO(this->get_logger(), "Arm command sent"); flying = true; + armed = true; } send_attitude_setpoint(); From 1ea831d1c4f9c569ed274cbb9d0d9cb61c1eed8b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 12:06:10 +0200 Subject: [PATCH 146/188] add extra checks for when armed or not armed --- src/px4_connection/src/px4_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 859123b7..c7acd315 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -95,7 +95,9 @@ private: } else { last_thrust = 0; - last_setpoint = {0, 0, 0}; + last_setpoint[0] = 0; + last_setpoint[1] = 0; + last_setpoint[2] = 0; response->status = 1; } } From 66880f710ddf93d42f74c945ed951d97f8aeab46 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 12:23:48 +0200 Subject: [PATCH 147/188] add print of q --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index c7acd315..976646b4 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -170,9 +170,9 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = -last_thrust; - RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); + RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", q.at(0), q.at(1), q.at(2), last_thrust); // } // else // { From 0a56a1fbb95502b42ff82ec19cf912e7cf53240c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 12:30:04 +0200 Subject: [PATCH 148/188] change passing q array by reference --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 976646b4..4ceb09b0 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -256,7 +256,7 @@ private: * @param attitude desired attitude (pitch) in radians. * @param bank desired bank (roll) in radians. */ - static void calculate_quaternion(std::array ptr, float heading, float attitude, float bank) + static void calculate_quaternion(std::array &ptr, float heading, float attitude, float bank) { float c1 = cos(heading / 2); From 5b779a9ae3ded891ea760cb753c6a538c544050d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 13:49:36 +0200 Subject: [PATCH 149/188] add keyboard listener --- src/test_controls/package.xml | 3 + .../test_controls/test_controller.py | 75 +++++++++++++++++++ 2 files changed, 78 insertions(+) diff --git a/src/test_controls/package.xml b/src/test_controls/package.xml index 06506d70..5369227f 100644 --- a/src/test_controls/package.xml +++ b/src/test_controls/package.xml @@ -7,6 +7,9 @@ ubuntu TODO: License declaration + rclpy + drone_services + ament_copyright ament_flake8 ament_pep257 diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index e69de29b..96a3a266 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -0,0 +1,75 @@ +import rclpy +from rclpy.node import Node + +from pynput.keyboard import KeyCode +from pynput import keyboard + +from drone_services.srv import SetAttitude + + +class TestController(Node): + + def __init__(self): + super().__init__('test_controller') + self.cli = self.create_client(SetAttitude, 'drone/set_attitude') + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = SetAttitude.Request() + + def spin(self): + with keyboard.Listener(on_press=self.on_press, on_release=self.on_release) as listener: + while rclpy.ok() and listener.running: + rclpy.spin_once(self, timeout_sec=0.1) + + def send_request(self, yaw, pitch, roll, thrust): + self.req.yaw = yaw + self.req.pitch = pitch + self.req.roll = roll + self.req.thrust = thrust + self.get_logger().info('set request to %f %f %f %f' % (yaw, pitch, roll, thrust)) + self.future = self.cli.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message on service') + return self.future.result() + + def on_press(self, key): + try: + char = getattr(key, 'char', None) + if isinstance(char, str): + self.logger.info('pressed ' + char) + self.pub_glyph.publish(self.pub_glyph.msg_type(data=char)) + else: + try: + # known keys like spacebar, ctrl + name = key.name + vk = key.value.vk + except AttributeError: + # unknown keys like headphones skip song button + name = 'UNKNOWN' + vk = key.vk + self.logger.info('pressed {} ({})'.format(name, vk)) + except Exception as e: + self.logger.error(str(e)) + raise + + if key == keyboard.Key.esc: + self.logger.info('stopping listener') + raise keyboard.Listener.StopException + + +def main(args=None): + rclpy.init(args=args) + + test_controller = TestController() + + test_controller.spin() + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + test_controller.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 8a017bd7b9be34c9beb5f9103bf962032991ab80 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 13:54:34 +0200 Subject: [PATCH 150/188] add entry point --- src/test_controls/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test_controls/setup.py b/src/test_controls/setup.py index 65d5a88b..a5591b80 100644 --- a/src/test_controls/setup.py +++ b/src/test_controls/setup.py @@ -20,6 +20,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'test_controller = test_controls.test_controller:main' ], }, ) From e5ae3dedfb0c1c5d06c957a7fe8184f243457a19 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:02:01 +0200 Subject: [PATCH 151/188] add package description and licence --- src/test_controls/package.xml | 4 ++-- src/test_controls/setup.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/test_controls/package.xml b/src/test_controls/package.xml index 5369227f..9904e9d7 100644 --- a/src/test_controls/package.xml +++ b/src/test_controls/package.xml @@ -3,9 +3,9 @@ test_controls 0.0.0 - TODO: Package description + test controls ubuntu - TODO: License declaration + Apache License 2.0 rclpy drone_services diff --git a/src/test_controls/setup.py b/src/test_controls/setup.py index a5591b80..a521d2fb 100644 --- a/src/test_controls/setup.py +++ b/src/test_controls/setup.py @@ -15,8 +15,8 @@ setup( zip_safe=True, maintainer='ubuntu', maintainer_email='semmer99@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + description='test controls', + license='Apache License 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ From d23a16358b5298ddf5a0bae824bff848986350e4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:03:11 +0200 Subject: [PATCH 152/188] test keys --- src/test_controls/test_controls/test_controller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 96a3a266..2ed34194 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -11,9 +11,9 @@ class TestController(Node): def __init__(self): super().__init__('test_controller') - self.cli = self.create_client(SetAttitude, 'drone/set_attitude') - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + # self.cli = self.create_client(SetAttitude, 'drone/set_attitude') + # while not self.cli.wait_for_service(timeout_sec=1.0): + # self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() def spin(self): From cfa773231eb4030cbf451b941d68f05cfb67579c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:06:05 +0200 Subject: [PATCH 153/188] add on_release --- src/test_controls/test_controls/test_controller.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 2ed34194..29ed4bfc 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -32,6 +32,9 @@ class TestController(Node): self.get_logger().info('publishing message on service') return self.future.result() + def on_release(self, key): + self.get_logger().info('released ' + str(key)) + def on_press(self, key): try: char = getattr(key, 'char', None) From 1b48c1d4b76cb4997b49d27c067147c46c204f15 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:09:54 +0200 Subject: [PATCH 154/188] change every self.logger to self.get_logger --- src/test_controls/test_controls/test_controller.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 29ed4bfc..cb68fdf3 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -34,13 +34,12 @@ class TestController(Node): def on_release(self, key): self.get_logger().info('released ' + str(key)) - + def on_press(self, key): try: char = getattr(key, 'char', None) if isinstance(char, str): - self.logger.info('pressed ' + char) - self.pub_glyph.publish(self.pub_glyph.msg_type(data=char)) + self.get_logger().info('pressed ' + char) else: try: # known keys like spacebar, ctrl @@ -50,13 +49,13 @@ class TestController(Node): # unknown keys like headphones skip song button name = 'UNKNOWN' vk = key.vk - self.logger.info('pressed {} ({})'.format(name, vk)) + self.get_logger().info('pressed {} ({})'.format(name, vk)) except Exception as e: - self.logger.error(str(e)) + self.get_logger().error(str(e)) raise if key == keyboard.Key.esc: - self.logger.info('stopping listener') + self.get_logger().info('stopping listener') raise keyboard.Listener.StopException From 4fd44cec46671287f68c2b5fb832b15b7442ab35 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:12:09 +0200 Subject: [PATCH 155/188] add controls info --- src/test_controls/test_controls/test_controller.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index cb68fdf3..1ac7fbb4 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -11,11 +11,13 @@ class TestController(Node): def __init__(self): super().__init__('test_controller') - # self.cli = self.create_client(SetAttitude, 'drone/set_attitude') - # while not self.cli.wait_for_service(timeout_sec=1.0): - # self.get_logger().info('service not available, waiting again...') + self.cli = self.create_client(SetAttitude, 'drone/set_attitude') + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() + self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") + def spin(self): with keyboard.Listener(on_press=self.on_press, on_release=self.on_release) as listener: while rclpy.ok() and listener.running: From 80fa83a42ff1457feadc216d9b3be523e2cdedc9 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:12:40 +0200 Subject: [PATCH 156/188] remove waiting on service for testing --- src/test_controls/test_controls/test_controller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 1ac7fbb4..e5afef07 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -11,9 +11,9 @@ class TestController(Node): def __init__(self): super().__init__('test_controller') - self.cli = self.create_client(SetAttitude, 'drone/set_attitude') - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + # self.cli = self.create_client(SetAttitude, 'drone/set_attitude') + # while not self.cli.wait_for_service(timeout_sec=1.0): + # self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") From bb426cf1dcd8aa1057e50ccc755b3fba8a02cc5b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:18:02 +0200 Subject: [PATCH 157/188] add printing directions --- .../test_controls/test_controller.py | 20 +++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index e5afef07..a9af75d5 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -1,7 +1,7 @@ import rclpy from rclpy.node import Node -from pynput.keyboard import KeyCode +from pynput.keyboard import Key from pynput import keyboard from drone_services.srv import SetAttitude @@ -41,7 +41,19 @@ class TestController(Node): try: char = getattr(key, 'char', None) if isinstance(char, str): - self.get_logger().info('pressed ' + char) + # self.get_logger().info('pressed ' + char) + if char == 'w': + self.get_logger().info('forward') + if char == 's': + self.get_logger().info('backward') + if char == 'a': + self.get_logger().info('left') + if char == 'd': + self.get_logger().info('right') + if char == 'q': + self.get_logger().info('rotate left') + if char == 'e': + self.get_logger().info('rotate right') else: try: # known keys like spacebar, ctrl @@ -52,6 +64,10 @@ class TestController(Node): name = 'UNKNOWN' vk = key.vk self.get_logger().info('pressed {} ({})'.format(name, vk)) + if vk == Key.space: + self.get_logger().info('up') + if vk == Key.shift: + self.get_logger().info('down') except Exception as e: self.get_logger().error(str(e)) raise From 85b56aef7b4c830ee7c4220ddc5aae120260e794 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:19:28 +0200 Subject: [PATCH 158/188] change vk to ints --- src/test_controls/test_controls/test_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index a9af75d5..ef2eb1d3 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -64,9 +64,9 @@ class TestController(Node): name = 'UNKNOWN' vk = key.vk self.get_logger().info('pressed {} ({})'.format(name, vk)) - if vk == Key.space: + if vk == 32: self.get_logger().info('up') - if vk == Key.shift: + if vk == 65505: self.get_logger().info('down') except Exception as e: self.get_logger().error(str(e)) From 779a5c998758d7c22614f4d418d6ea3397871563 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:20:21 +0200 Subject: [PATCH 159/188] remove prints of keys --- src/test_controls/test_controls/test_controller.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index ef2eb1d3..360e355b 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -35,7 +35,8 @@ class TestController(Node): return self.future.result() def on_release(self, key): - self.get_logger().info('released ' + str(key)) + # self.get_logger().info('released ' + str(key)) + pass def on_press(self, key): try: @@ -63,7 +64,7 @@ class TestController(Node): # unknown keys like headphones skip song button name = 'UNKNOWN' vk = key.vk - self.get_logger().info('pressed {} ({})'.format(name, vk)) + # self.get_logger().info('pressed {} ({})'.format(name, vk)) if vk == 32: self.get_logger().info('up') if vk == 65505: From 6db6c89bd3525a80fab8e79281d77dc5fb347582 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:23:42 +0200 Subject: [PATCH 160/188] add increasing/decreasing yaw pitch roll instead of setting --- src/px4_connection/src/px4_controller.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4ceb09b0..6d6b005c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,12 +83,19 @@ private: { if (armed) { - last_setpoint[0] = degrees_to_radians(request->yaw); - last_setpoint[1] = degrees_to_radians(request->pitch); - last_setpoint[2] = degrees_to_radians(request->roll); - + if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) + { + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); + last_setpoint[2] = degrees_to_radians(request->roll); + } else { + last_setpoint[0] += degrees_to_radians(request->yaw); + last_setpoint[1] += degrees_to_radians(request->pitch); + last_setpoint[2] += degrees_to_radians(request->roll); + } last_thrust = request->thrust; + RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); response->status = 0; From fbef2636822f09662f444acc188a0590498edd32 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:35:44 +0200 Subject: [PATCH 161/188] remove yaw pitch and roll body --- src/px4_connection/src/px4_controller.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6d6b005c..6e5d9cd9 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,16 +83,16 @@ private: { if (armed) { - if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) - { last_setpoint[0] = degrees_to_radians(request->yaw); last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); - } else { - last_setpoint[0] += degrees_to_radians(request->yaw); - last_setpoint[1] += degrees_to_radians(request->pitch); - last_setpoint[2] += degrees_to_radians(request->roll); - } + // if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) + // { + // } else { + // last_setpoint[0] += degrees_to_radians(request->yaw); + // last_setpoint[1] += degrees_to_radians(request->pitch); + // last_setpoint[2] += degrees_to_radians(request->roll); + // } last_thrust = request->thrust; @@ -169,9 +169,9 @@ private: // if (this->get_clock()->now().seconds() - start_time_ < 30) // { - msg.yaw_body = last_setpoint[0]; - msg.pitch_body = last_setpoint[1]; - msg.roll_body = last_setpoint[2]; + // msg.yaw_body = last_setpoint[0]; + // msg.pitch_body = last_setpoint[1]; + // msg.roll_body = last_setpoint[2]; // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east From a23286f28247c242ce0d1ca96e9f44e15507b0dd Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:38:22 +0200 Subject: [PATCH 162/188] change service to be correct --- src/drone_services/srv/SetAttitude.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv index 52957c0c..8f8b64e4 100644 --- a/src/drone_services/srv/SetAttitude.srv +++ b/src/drone_services/srv/SetAttitude.srv @@ -1,6 +1,6 @@ #all angles are in degrees -float32 yaw float32 pitch +float32 yaw float32 roll #thrust between -1 and 1 From bb26e74f544498d720dd9892543263ef67f77ce6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 14:58:00 +0200 Subject: [PATCH 163/188] change position of parameters --- src/drone_services/srv/SetAttitude.srv | 2 +- src/px4_connection/src/px4_controller.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv index 8f8b64e4..52957c0c 100644 --- a/src/drone_services/srv/SetAttitude.srv +++ b/src/drone_services/srv/SetAttitude.srv @@ -1,6 +1,6 @@ #all angles are in degrees -float32 pitch float32 yaw +float32 pitch float32 roll #thrust between -1 and 1 diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6e5d9cd9..2a75f434 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,8 +83,8 @@ private: { if (armed) { - last_setpoint[0] = degrees_to_radians(request->yaw); - last_setpoint[1] = degrees_to_radians(request->pitch); + last_setpoint[1] = degrees_to_radians(request->yaw); + last_setpoint[0] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); // if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) // { From 72b252a970682a020e607321c0b0c8503f7acbd6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 15:01:51 +0200 Subject: [PATCH 164/188] change pitch and yaw --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 2a75f434..6e5d9cd9 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,8 +83,8 @@ private: { if (armed) { - last_setpoint[1] = degrees_to_radians(request->yaw); - last_setpoint[0] = degrees_to_radians(request->pitch); + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); // if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) // { From 2c3123039e990ce4a81dd5a48c8a8e63b0b75bfa Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 15:08:58 +0200 Subject: [PATCH 165/188] change pitch and yaw --- src/px4_connection/src/px4_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6e5d9cd9..2a75f434 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,8 +83,8 @@ private: { if (armed) { - last_setpoint[0] = degrees_to_radians(request->yaw); - last_setpoint[1] = degrees_to_radians(request->pitch); + last_setpoint[1] = degrees_to_radians(request->yaw); + last_setpoint[0] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); // if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) // { From db1322e2de1125a49486aea429a6d0292e477172 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 15:21:49 +0200 Subject: [PATCH 166/188] add incrementing/decrementing values --- src/px4_connection/src/px4_controller.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 2a75f434..58a6ba0d 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,16 +83,16 @@ private: { if (armed) { - last_setpoint[1] = degrees_to_radians(request->yaw); - last_setpoint[0] = degrees_to_radians(request->pitch); + if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) + { + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); - // if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) - // { - // } else { - // last_setpoint[0] += degrees_to_radians(request->yaw); - // last_setpoint[1] += degrees_to_radians(request->pitch); - // last_setpoint[2] += degrees_to_radians(request->roll); - // } + } else { + last_setpoint[0] += degrees_to_radians(request->yaw); + last_setpoint[1] += degrees_to_radians(request->pitch); + last_setpoint[2] += degrees_to_radians(request->roll); + } last_thrust = request->thrust; From adac3eb0226e784e1b9bac8b2babf48aed9f4c94 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 15:34:57 +0200 Subject: [PATCH 167/188] change setting thrust and angles --- src/px4_connection/src/px4_controller.cpp | 10 +++++++--- .../test_controls/test_controller.py | 15 ++++++++++++--- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 58a6ba0d..8f7d011e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,18 +83,22 @@ private: { if (armed) { - if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) + if (request->yaw == 0 && request->pitch = 0 && request->roll = 0 && request->thrust = 0) { last_setpoint[0] = degrees_to_radians(request->yaw); last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); + last_thrust = request->thrust; } else { last_setpoint[0] += degrees_to_radians(request->yaw); last_setpoint[1] += degrees_to_radians(request->pitch); last_setpoint[2] += degrees_to_radians(request->roll); + last_thrust += request->thrust; + if (last_thrust > 1) + last_thrust = 1; + else if (last_thrust < 0) + last_thrust = 0; } - last_thrust = request->thrust; - RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 360e355b..2a42f14f 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -11,9 +11,9 @@ class TestController(Node): def __init__(self): super().__init__('test_controller') - # self.cli = self.create_client(SetAttitude, 'drone/set_attitude') - # while not self.cli.wait_for_service(timeout_sec=1.0): - # self.get_logger().info('service not available, waiting again...') + self.cli = self.create_client(SetAttitude, 'drone/set_attitude') + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") @@ -45,16 +45,22 @@ class TestController(Node): # self.get_logger().info('pressed ' + char) if char == 'w': self.get_logger().info('forward') + self.send_request(pitch=-10, yaw=0,roll=0, thrust=0) if char == 's': self.get_logger().info('backward') + self.send_request(pitch=10, yaw=0,roll=0, thrust=0) if char == 'a': self.get_logger().info('left') + self.send_request(pitch=0, yaw=0,roll=-10, thrust=0) if char == 'd': self.get_logger().info('right') + self.send_request(pitch=0, yaw=0,roll=10, thrust=0) if char == 'q': self.get_logger().info('rotate left') + self.send_request(pitch=0, yaw=-10,roll=0, thrust=0) if char == 'e': self.get_logger().info('rotate right') + self.send_request(pitch=0, yaw=10,roll=0, thrust=0) else: try: # known keys like spacebar, ctrl @@ -67,8 +73,11 @@ class TestController(Node): # self.get_logger().info('pressed {} ({})'.format(name, vk)) if vk == 32: self.get_logger().info('up') + self.send_request(pitch=0, yaw=0,roll=0, thrust=0.1) if vk == 65505: self.get_logger().info('down') + self.send_request(pitch=0, yaw=0,roll=0, thrust=-0.1) + except Exception as e: self.get_logger().error(str(e)) raise From c88e82d42deab5d26461657131b984560d99f344 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 15:36:10 +0200 Subject: [PATCH 168/188] bug --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 8f7d011e..02272e3d 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,7 +83,7 @@ private: { if (armed) { - if (request->yaw == 0 && request->pitch = 0 && request->roll = 0 && request->thrust = 0) + if (request->yaw == 0 && request->pitch == 0 && request->roll == 0 && request->thrust == 0) { last_setpoint[0] = degrees_to_radians(request->yaw); last_setpoint[1] = degrees_to_radians(request->pitch); From 741fa5b096d7a5650fb38646fa86b69576ba94fb Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 15:38:45 +0200 Subject: [PATCH 169/188] change sending to floats --- .../test_controls/test_controller.py | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 2a42f14f..9a0baab1 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -45,22 +45,28 @@ class TestController(Node): # self.get_logger().info('pressed ' + char) if char == 'w': self.get_logger().info('forward') - self.send_request(pitch=-10, yaw=0,roll=0, thrust=0) + self.send_request(pitch=-10.0, yaw=0.0, + roll=0.0, thrust=0.0) if char == 's': self.get_logger().info('backward') - self.send_request(pitch=10, yaw=0,roll=0, thrust=0) + self.send_request(pitch=10.0, yaw=0.0, + roll=0.0, thrust=0.0) if char == 'a': self.get_logger().info('left') - self.send_request(pitch=0, yaw=0,roll=-10, thrust=0) + self.send_request(pitch=0.0, yaw=0.0, + roll=-10.0, thrust=0.0) if char == 'd': self.get_logger().info('right') - self.send_request(pitch=0, yaw=0,roll=10, thrust=0) + self.send_request(pitch=0.0, yaw=0.0, + roll=10.0, thrust=0.0) if char == 'q': self.get_logger().info('rotate left') - self.send_request(pitch=0, yaw=-10,roll=0, thrust=0) + self.send_request(pitch=0.0, yaw=-10.0, + roll=0.0, thrust=0.0) if char == 'e': self.get_logger().info('rotate right') - self.send_request(pitch=0, yaw=10,roll=0, thrust=0) + self.send_request(pitch=0.0, yaw=10.0, + roll=0.0, thrust=0.0) else: try: # known keys like spacebar, ctrl @@ -73,10 +79,11 @@ class TestController(Node): # self.get_logger().info('pressed {} ({})'.format(name, vk)) if vk == 32: self.get_logger().info('up') - self.send_request(pitch=0, yaw=0,roll=0, thrust=0.1) + self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.1) if vk == 65505: self.get_logger().info('down') - self.send_request(pitch=0, yaw=0,roll=0, thrust=-0.1) + self.send_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.1) except Exception as e: self.get_logger().error(str(e)) From 5f90c53128bdca6721a76f9472417bfd38474e31 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 15:45:18 +0200 Subject: [PATCH 170/188] add extra print in service receive: --- src/px4_connection/src/px4_controller.cpp | 3 ++- src/test_controls/test_controls/test_controller.py | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 02272e3d..af69df99 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -100,6 +100,7 @@ private: last_thrust = 0; } + RCLCPP_INFO(this->get_logger(), "got values: yaw:%f pitch:%f roll:%f thrust:%f", request->yaw, request->pitch, request->roll, request->thrust); RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); response->status = 0; @@ -183,7 +184,7 @@ private: calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); - RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", q.at(0), q.at(1), q.at(2), last_thrust); + // RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", q.at(0), q.at(1), q.at(2), last_thrust); // } // else // { diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 9a0baab1..e2ef88f9 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -79,11 +79,11 @@ class TestController(Node): # self.get_logger().info('pressed {} ({})'.format(name, vk)) if vk == 32: self.get_logger().info('up') - self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.1) + self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) if vk == 65505: self.get_logger().info('down') self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.1) + roll=0.0, thrust=-0.05) except Exception as e: self.get_logger().error(str(e)) From 2b287c561df3b42a0a582ae8b55005d637c3879d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 16:33:09 +0200 Subject: [PATCH 171/188] add subscription to vehicle attitude --- src/px4_connection/src/px4_controller.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index af69df99..aa2ac8b6 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,6 +17,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include #include #include +#include #include @@ -33,12 +34,18 @@ class PX4Controller : public rclcpp::Node public: PX4Controller() : Node("px4_controller") { + + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); + // create a publisher on the vehicle attitude setpoint topic vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this)); + set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -53,6 +60,8 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + rclcpp::Subscription::SharedPtr vehicle_attitude_subscription_; + rclcpp::Service::SharedPtr set_attitude_service_; rclcpp::Service::SharedPtr disarm_service_; @@ -239,6 +248,11 @@ private: send_attitude_setpoint(); } + void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) + { + + } + /** * @brief Publish vehicle commands * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) From 78d5221633cc278ac5a5a6a8f1d1e4fa1680c91a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 16:40:59 +0200 Subject: [PATCH 172/188] add base_q amount --- src/px4_connection/src/px4_controller.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index aa2ac8b6..9165520c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -82,6 +82,9 @@ private: float last_angle = 0; float last_thrust = 0; + float base_q[4] = {0, 0, 0, 0}; + int base_q_amount = 0; + // result quaternion std::array q = {0, 0, 0, 0}; @@ -250,7 +253,14 @@ private: void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) { - + if (!ready_to_fly) + { + base_q_amount++; + base_q[0] = (base_q[0] + msg->q[0])/base_q_amount; + base_q[1] = (base_q[1] + msg->q[1])/base_q_amount; + base_q[2] = (base_q[2] + msg->q[2])/base_q_amount; + base_q[3] = (base_q[3] + msg->q[3])/base_q_amount; + } } /** From be2ef48f0e70df20de44e18aea6f8c65dce823de Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 16:49:39 +0200 Subject: [PATCH 173/188] add _1 placeholder to subscription bind --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9165520c..a7c24168 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -44,7 +44,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this)); + vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this,std::placeholders::_1)); set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); From 96edc69227d5279964242a8b89abcb06e249f738 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 11:23:03 +0200 Subject: [PATCH 174/188] use sshkeyboard library --- src/test_controls/test_controls/test_controller.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index e2ef88f9..79194ec0 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -1,8 +1,8 @@ import rclpy from rclpy.node import Node -from pynput.keyboard import Key -from pynput import keyboard +from sshkeyboard import listen_keyboard_manual +import asyncio from drone_services.srv import SetAttitude @@ -12,15 +12,15 @@ class TestController(Node): def __init__(self): super().__init__('test_controller') self.cli = self.create_client(SetAttitude, 'drone/set_attitude') - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + # while not self.cli.wait_for_service(timeout_sec=1.0): + # self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") - def spin(self): - with keyboard.Listener(on_press=self.on_press, on_release=self.on_release) as listener: - while rclpy.ok() and listener.running: + def spin(self): + while rclpy.ok(): + asyncio.run(listen_keyboard_manual(on_press=self.on_press)) rclpy.spin_once(self, timeout_sec=0.1) def send_request(self, yaw, pitch, roll, thrust): From 7133a11e9267cc7f10547a2f9e81b8d8a0e2fc6f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 11:49:06 +0200 Subject: [PATCH 175/188] change char to key --- .../test_controls/test_controller.py | 92 +++++++++---------- 1 file changed, 43 insertions(+), 49 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 79194ec0..67dabc1d 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -40,58 +40,52 @@ class TestController(Node): def on_press(self, key): try: - char = getattr(key, 'char', None) - if isinstance(char, str): - # self.get_logger().info('pressed ' + char) - if char == 'w': - self.get_logger().info('forward') - self.send_request(pitch=-10.0, yaw=0.0, - roll=0.0, thrust=0.0) - if char == 's': - self.get_logger().info('backward') - self.send_request(pitch=10.0, yaw=0.0, - roll=0.0, thrust=0.0) - if char == 'a': - self.get_logger().info('left') - self.send_request(pitch=0.0, yaw=0.0, - roll=-10.0, thrust=0.0) - if char == 'd': - self.get_logger().info('right') - self.send_request(pitch=0.0, yaw=0.0, - roll=10.0, thrust=0.0) - if char == 'q': - self.get_logger().info('rotate left') - self.send_request(pitch=0.0, yaw=-10.0, - roll=0.0, thrust=0.0) - if char == 'e': - self.get_logger().info('rotate right') - self.send_request(pitch=0.0, yaw=10.0, - roll=0.0, thrust=0.0) - else: - try: - # known keys like spacebar, ctrl - name = key.name - vk = key.value.vk - except AttributeError: - # unknown keys like headphones skip song button - name = 'UNKNOWN' - vk = key.vk - # self.get_logger().info('pressed {} ({})'.format(name, vk)) - if vk == 32: - self.get_logger().info('up') - self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) - if vk == 65505: - self.get_logger().info('down') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.05) + # self.get_logger().info('pressed ' + char) + if key == 'w': + self.get_logger().info('forward') + self.send_request(pitch=-10.0, yaw=0.0, + roll=0.0, thrust=0.0) + if key == 's': + self.get_logger().info('backward') + self.send_request(pitch=10.0, yaw=0.0, + roll=0.0, thrust=0.0) + if key == 'a': + self.get_logger().info('left') + self.send_request(pitch=0.0, yaw=0.0, + roll=-10.0, thrust=0.0) + if key == 'd': + self.get_logger().info('right') + self.send_request(pitch=0.0, yaw=0.0, + roll=10.0, thrust=0.0) + if key == 'q': + self.get_logger().info('rotate left') + self.send_request(pitch=0.0, yaw=-10.0, + roll=0.0, thrust=0.0) + if key == 'e': + self.get_logger().info('rotate right') + self.send_request(pitch=0.0, yaw=10.0, + roll=0.0, thrust=0.0) + # else: + # try: + # # known keys like spacebar, ctrl + # name = key.name + # vk = key.value.vk + # except AttributeError: + # # unknown keys like headphones skip song button + # name = 'UNKNOWN' + # vk = key.vk + # # self.get_logger().info('pressed {} ({})'.format(name, vk)) + # if vk == 32: + # self.get_logger().info('up') + # self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) + # if vk == 65505: + # self.get_logger().info('down') + # self.send_request(pitch=0.0, yaw=0.0, + # roll=0.0, thrust=-0.05) except Exception as e: self.get_logger().error(str(e)) - raise - - if key == keyboard.Key.esc: - self.get_logger().info('stopping listener') - raise keyboard.Listener.StopException + raise e def main(args=None): From 2485647c72553eae34907485d6f16457a27068bc Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 11:51:59 +0200 Subject: [PATCH 176/188] =?UTF-8?q?add=20checking=20for=20up=20or=20down?= =?UTF-8?q?=20with=20sshkeyboard=C2=A8v?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../test_controls/test_controller.py | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 67dabc1d..a762ab00 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -12,16 +12,16 @@ class TestController(Node): def __init__(self): super().__init__('test_controller') self.cli = self.create_client(SetAttitude, 'drone/set_attitude') - # while not self.cli.wait_for_service(timeout_sec=1.0): - # self.get_logger().info('service not available, waiting again...') + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") - def spin(self): - while rclpy.ok(): - asyncio.run(listen_keyboard_manual(on_press=self.on_press)) - rclpy.spin_once(self, timeout_sec=0.1) + def spin(self): + while rclpy.ok(): + asyncio.run(listen_keyboard_manual(on_press=self.on_press)) + rclpy.spin_once(self, timeout_sec=0.1) def send_request(self, yaw, pitch, roll, thrust): self.req.yaw = yaw @@ -44,27 +44,35 @@ class TestController(Node): if key == 'w': self.get_logger().info('forward') self.send_request(pitch=-10.0, yaw=0.0, - roll=0.0, thrust=0.0) + roll=0.0, thrust=0.0) if key == 's': self.get_logger().info('backward') self.send_request(pitch=10.0, yaw=0.0, - roll=0.0, thrust=0.0) + roll=0.0, thrust=0.0) if key == 'a': self.get_logger().info('left') self.send_request(pitch=0.0, yaw=0.0, - roll=-10.0, thrust=0.0) + roll=-10.0, thrust=0.0) if key == 'd': self.get_logger().info('right') self.send_request(pitch=0.0, yaw=0.0, - roll=10.0, thrust=0.0) + roll=10.0, thrust=0.0) if key == 'q': self.get_logger().info('rotate left') self.send_request(pitch=0.0, yaw=-10.0, - roll=0.0, thrust=0.0) + roll=0.0, thrust=0.0) if key == 'e': self.get_logger().info('rotate right') self.send_request(pitch=0.0, yaw=10.0, - roll=0.0, thrust=0.0) + roll=0.0, thrust=0.0) + if key == 'z': + self.get_logger().info('down') + self.send_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.05) + if key == 'space': + self.get_logger().info('down') + self.send_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.05) # else: # try: # # known keys like spacebar, ctrl From 5ab0a39ee303f9e363c48d120da375a444b98b43 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 11:53:40 +0200 Subject: [PATCH 177/188] change up to 0.05 --- src/test_controls/test_controls/test_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index a762ab00..89cd5432 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -70,9 +70,9 @@ class TestController(Node): self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=-0.05) if key == 'space': - self.get_logger().info('down') + self.get_logger().info('up') self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.05) + roll=0.0, thrust=0.05) # else: # try: # # known keys like spacebar, ctrl From 97a72d5a4a78a463be1d8426522925670115c6c3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 14:25:29 +0200 Subject: [PATCH 178/188] add calculation of base_q into message setpoint --- src/px4_connection/src/px4_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index a7c24168..18ed5689 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -209,10 +209,10 @@ private: // } // set quaternion - msg.q_d[0] = q.at(0); - msg.q_d[1] = q.at(1); - msg.q_d[2] = q.at(2); - msg.q_d[3] = q.at(3); + msg.q_d[0] = base_q[0] + q.at(0); + msg.q_d[1] = base_q[1] + q.at(1); + msg.q_d[2] = base_q[2] + q.at(2); + msg.q_d[3] = base_q[3] + q.at(3); msg.yaw_sp_move_rate = 0; msg.reset_integral = false; From e9e6e033d1083617725b0e85a8a9b4b5095ba1aa Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 14:27:30 +0200 Subject: [PATCH 179/188] add printing of base_q --- src/px4_connection/src/px4_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 18ed5689..876f3e14 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -260,6 +260,8 @@ private: base_q[1] = (base_q[1] + msg->q[1])/base_q_amount; base_q[2] = (base_q[2] + msg->q[2])/base_q_amount; base_q[3] = (base_q[3] + msg->q[3])/base_q_amount; + + RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]); } } From 3baad7c7bd23e1bf36274b92d3f3987b4f20072d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 14:43:39 +0200 Subject: [PATCH 180/188] set proper getting average of base_q --- src/px4_connection/src/px4_controller.cpp | 32 ++++++++++++++--------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 876f3e14..60971e8b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -36,7 +36,7 @@ public: { rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); // create a publisher on the vehicle attitude setpoint topic vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); @@ -44,7 +44,7 @@ public: trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); - vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this,std::placeholders::_1)); + vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1)); set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -101,7 +101,9 @@ private: last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); last_thrust = request->thrust; - } else { + } + else + { last_setpoint[0] += degrees_to_radians(request->yaw); last_setpoint[1] += degrees_to_radians(request->pitch); last_setpoint[2] += degrees_to_radians(request->roll); @@ -116,7 +118,8 @@ private: RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); response->status = 0; - } else + } + else { last_thrust = 0; last_setpoint[0] = 0; @@ -194,7 +197,6 @@ private: msg.thrust_body[1] = 0; // east msg.thrust_body[2] = -last_thrust; - calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); // RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", q.at(0), q.at(1), q.at(2), last_thrust); // } @@ -209,7 +211,7 @@ private: // } // set quaternion - msg.q_d[0] = base_q[0] + q.at(0); + msg.q_d[0] = q.at(0); msg.q_d[1] = base_q[1] + q.at(1); msg.q_d[2] = base_q[2] + q.at(2); msg.q_d[3] = base_q[3] + q.at(3); @@ -253,13 +255,19 @@ private: void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) { - if (!ready_to_fly) + if (!armed) { - base_q_amount++; - base_q[0] = (base_q[0] + msg->q[0])/base_q_amount; - base_q[1] = (base_q[1] + msg->q[1])/base_q_amount; - base_q[2] = (base_q[2] + msg->q[2])/base_q_amount; - base_q[3] = (base_q[3] + msg->q[3])/base_q_amount; + if (base_q_amount > 2) + { + base_q[1] = (base_q[1] + msg->q[1]) / 2; + base_q[2] = (base_q[2] + msg->q[2]) / 2; + base_q[3] = (base_q[3] + msg->q[3]) / 2; + } else { + base_q[1] = msg->q[1]; + base_q[2] = msg->q[2]; + base_q[3] = msg->q[3]; + base_q_amount++; + } RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]); } From 5bd2885ad8b91879cd58a7e18e3f81d25c651d08 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 14:49:07 +0200 Subject: [PATCH 181/188] add print of q_d --- src/px4_connection/src/px4_controller.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 60971e8b..e003c191 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -216,6 +216,8 @@ private: msg.q_d[2] = base_q[2] + q.at(2); msg.q_d[3] = base_q[3] + q.at(3); + RCLCPP_INFO(this->get_logger(), "q_d: [%f, %f, %f, %f]", msg.q_d[0], msg.q_d[1], msg.q_d[2], msg.q_d[3]); + msg.yaw_sp_move_rate = 0; msg.reset_integral = false; msg.fw_control_yaw_wheel = false; @@ -255,6 +257,13 @@ private: void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) { + /* + average q: + - 0.7313786745071411 + - 0.005042835604399443 + - 0.0002370707516092807 + - 0.6819528937339783 + */ if (!armed) { if (base_q_amount > 2) From 4f4c148b69f79e67cc298e50054221623fe8176d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 14:53:05 +0200 Subject: [PATCH 182/188] add printing q only if new setpoint has been received --- src/px4_connection/src/px4_controller.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e003c191..12a6f645 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -77,6 +77,7 @@ private: float thrust = 0.0; bool ready_to_fly = false; float cur_yaw = 0; + bool new_setpoint = false; // for printing new q_d when a new setpoint has been received float last_setpoint[3] = {0, 0, 0}; float last_angle = 0; @@ -116,6 +117,7 @@ private: RCLCPP_INFO(this->get_logger(), "got values: yaw:%f pitch:%f roll:%f thrust:%f", request->yaw, request->pitch, request->roll, request->thrust); RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); + new_setpoint = true; response->status = 0; } @@ -216,7 +218,11 @@ private: msg.q_d[2] = base_q[2] + q.at(2); msg.q_d[3] = base_q[3] + q.at(3); - RCLCPP_INFO(this->get_logger(), "q_d: [%f, %f, %f, %f]", msg.q_d[0], msg.q_d[1], msg.q_d[2], msg.q_d[3]); + if (new_setpoint) + { + RCLCPP_INFO(this->get_logger(), "q_d: [%f, %f, %f, %f]", msg.q_d[0], msg.q_d[1], msg.q_d[2], msg.q_d[3]); + new_setpoint = false; + } msg.yaw_sp_move_rate = 0; msg.reset_integral = false; From b93a616f58cab778d7329b694b9d392e44b87ea5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 14:55:39 +0200 Subject: [PATCH 183/188] add emergency stop --- src/px4_connection/src/px4_controller.cpp | 1 + src/test_controls/test_controls/test_controller.py | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 12a6f645..8c8e1d1f 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -102,6 +102,7 @@ private: last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); last_thrust = request->thrust; + RCLCPP_INFO(this->get_logger(), "STOPPING MOTORS"); } else { diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 89cd5432..9ed15c26 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -16,7 +16,7 @@ class TestController(Node): self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() - self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") + self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nN - emergency stop\nEsc - exit") def spin(self): while rclpy.ok(): @@ -73,6 +73,10 @@ class TestController(Node): self.get_logger().info('up') self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) + if key == 'n': + self.get_logger().info('stop') + self.send_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.0) # else: # try: # # known keys like spacebar, ctrl From 615190ca0683a74d549a3244997b7c6829648da6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 5 May 2023 11:52:29 +0200 Subject: [PATCH 184/188] add comments to heartbeat --- src/px4_connection/src/heartbeat.cpp | 28 ++++++---------------------- 1 file changed, 6 insertions(+), 22 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 52f6f87d..23cb0511 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -1,18 +1,8 @@ -/* - -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; @@ -30,6 +20,12 @@ public: } private: + // publisher for offboard control mode messages + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + // timer for sending the heartbeat + rclcpp::TimerBase::SharedPtr timer_; + // start time of node in seconds + double start_time; /** * @brief Publish offboard control mode messages as a heartbeat. @@ -50,20 +46,8 @@ private: // 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[]) From 07f0202d5111ac7b498cfe25465a26964e2aa6cf Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 5 May 2023 12:49:18 +0200 Subject: [PATCH 185/188] remove unnecessary pipe operator from height --- src/height/src/height_reader.cpp | 12 ------------ src/px4_connection/src/heartbeat.cpp | 2 +- 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index d8f70b06..92a652fa 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -13,18 +13,6 @@ using namespace std::chrono_literals; using terabee::DistanceData; using terabee::ITerarangerEvoMini; -std::ostream &operator<<(std::ostream &os, const DistanceData &d) -{ - os << "["; - for (size_t i = 0; i < d.distance.size(); i++) - { - os << d.distance[i] << ", "; - } - os << "\b\b" - << " ]"; - return os; -} - class HeightReader : public rclcpp::Node { public: diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 23cb0511..fafd2c2c 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -9,7 +9,7 @@ using namespace std::chrono_literals; class HeartBeat : public rclcpp::Node { public: - HeartBeat() : Node("setpoint_sender") + HeartBeat() : Node("heartbeat") { // create a publisher on the offboard control mode topic offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); From 6a29220bc9a21edacf1ed898a756f84639dbad9f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 5 May 2023 12:57:48 +0200 Subject: [PATCH 186/188] add comments --- src/height/src/height_reader.cpp | 122 ++++++++++++---------- src/px4_connection/src/heartbeat.cpp | 17 +-- src/px4_connection/src/px4_controller.cpp | 17 ++- 3 files changed, 86 insertions(+), 70 deletions(-) diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 92a652fa..1df63c2d 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -1,5 +1,10 @@ +/** + * @file height_reader.cpp + * @author Sem van der Hoeven (sem.hoeven@gmail.com) + * @brief Uses the Terabee API to read the Terabee Evo Mini + * height sensor data and publishes it on a /drone/height topic + */ #include -#include #include "rclcpp/rclcpp.hpp" #include "height/msg/height_data.hpp" @@ -16,70 +21,81 @@ using terabee::ITerarangerEvoMini; class HeightReader : public rclcpp::Node { public: - HeightReader() : rclcpp::Node("height_reader") - { - rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - descriptor.description = "serial port for the USB port of the height sensor"; - - this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); - - factory = terabee::ITerarangerFactory::getFactory(); - evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); - - if (!evo_mini) + HeightReader() : rclcpp::Node("height_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); - return; + rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + descriptor.description = "serial port for the USB port of the height sensor"; + + this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); + + factory = terabee::ITerarangerFactory::getFactory(); + evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); + + if (!evo_mini) + { + RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); + return; + } + + evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); + + if (!evo_mini->initialize()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); + + timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); + publisher_ = this->create_publisher("drone/height", 10); } - evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); - - if (!evo_mini->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); - return; - } - - RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); - - timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); - publisher_ = this->create_publisher("drone/height", 10); - } - private: - void read_height() - { - auto msg = height::msg::HeightData(); - - float min = 255; - terabee::DistanceData distance_data = evo_mini->getDistance(); - for (size_t i = 0; i < distance_data.size(); i++) + /** + * @brief reads the height value from the Terabee Evo Mini sensor and publishes a + * + */ + void read_height() { - msg.heights[i] = distance_data.distance[i]; - if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) - { - min = distance_data.distance[i]; - } + auto msg = height::msg::HeightData(); + + // high initial minimal value + float min = 255; + terabee::DistanceData distance_data = evo_mini->getDistance(); + // add readings and calculate mimimum value + for (size_t i = 0; i < distance_data.size(); i++) + { + msg.heights[i] = distance_data.distance[i]; + if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) + { + min = distance_data.distance[i]; + } + } + // add minimum value and publish message + msg.min_height = min; + publisher_->publish(msg); + + RCLCPP_INFO(this->get_logger(), "publishing message with min distance %f", msg.min_height); } - msg.min_height = min; - publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(),"publishing message with min distance %f",msg.min_height); - } + // timer for publishing height readings + rclcpp::TimerBase::SharedPtr timer_; + // publisher for height readings + rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - - std::unique_ptr factory; - std::unique_ptr evo_mini; + // factory for height sensor + std::unique_ptr factory; + // height sensor object pointer + std::unique_ptr evo_mini; }; int main(int argc, char **argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); - return 0; + return 0; } diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index fafd2c2c..dd733c46 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -1,3 +1,9 @@ +/** + * @file heartbeat.cpp + * @author Sem van der Hoeven (sem.hoeven@gmail.com) + * @brief Heartbeat node that keeps the connection between the flight computer + * and PX4 flight controller alive by sending OffboardControl messages + */ #include #include "rclcpp/rclcpp.hpp" @@ -27,11 +33,11 @@ private: // start time of node in seconds double start_time; -/** - * @brief Publish offboard control mode messages as a heartbeat. - * Only the attitude is enabled, because that is how the drone will be controlled. - * - */ + /** + * @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 @@ -46,7 +52,6 @@ private: // get timestamp and publish message msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; offboard_control_mode_publisher_->publish(msg); - } }; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 8c8e1d1f..9f28763c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -1,17 +1,14 @@ -/* - -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 - -*/ +/** + * @file px4_controller.cpp + * @author Sem van der Hoeven (sem.hoeven@gmail.com) + * @brief Controller node to contol the PX4 using attitude or trajectory setpoints. + * It subscribes to the /drone/set_attitude topic to receive control commands + */ #include #include #include "rclcpp/rclcpp.hpp" -// #include "attitude_msg_code.hpp" #include #include @@ -23,8 +20,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include -// #include - #define D_SPEED(x) -x - 9.81 using namespace std::chrono_literals; From 0a1ef8af3d2674c7a4314f176a23daa3adc3c5ad Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 5 May 2023 16:19:24 +0200 Subject: [PATCH 187/188] add comments and code to lidar node --- src/object_detection/src/lidar_reader.cpp | 138 +++++++++------------- src/px4_connection/src/heartbeat.cpp | 2 - 2 files changed, 57 insertions(+), 83 deletions(-) diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index eaa98bd8..df21a116 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -18,104 +18,80 @@ using terabee::TowerDistanceData; using namespace std::chrono_literals; -/* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ - -std::ostream &operator<<(std::ostream &os, const TowerDistanceData &d) -{ - os << "["; - for (size_t i = 0; i < d.distance.size(); i++) - { - os << i << " " << d.distance[i] << (d.mask[i] ? " , " : " , "); - } - os << "\b\b" - << " ]"; - return os; -} - -std::ostream &operator<<(std::ostream &os, const ImuData &d) -{ - os << "["; - for (size_t i = 0; i < d.data.size(); i++) - { - os << d.data[i] << ", "; - } - os << "\b\b" - << " ]"; - return os; -} - class LidarReader : public rclcpp::Node { public: - LidarReader() - : Node("lidar_reader") - { - - rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - descriptor.description = "serial port for the USB port of the LIDAR"; - - this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); - - // publisher_ = this->create_publisher("drone/object_detection", 10); - timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); - - ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); - - factory = terabee::ITerarangerFactory::getFactory(); - tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string()); - - if (!tower) + LidarReader() + : Node("lidar_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo"); - return; - } + // get serial port of sensor through parameter + rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + descriptor.description = "serial port for the USB port of the LIDAR"; + this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); - tower->setImuMode(mode); + // create publisher and bind timer to publish function + publisher_ = this->create_publisher("drone/object_detection", 10); + timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); - if (!tower->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo"); - return; + // set lidar to measure including IMU + ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); + + factory = terabee::ITerarangerFactory::getFactory(); + tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string()); + + if (!tower) + { + RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo"); + return; + } + + tower->setImuMode(mode); + + if (!tower->initialize()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo"); + return; + } } - } private: - void read_lidar_data() - { - //TODO publish message with all data from lidar - // std::cout << "Distance = " << tower->getDistance() << std::endl; - // std::cout << "IMU = " << tower->getImuData() << std::endl; + void read_lidar_data() + { + auto msg = object_detection::msg::LidarReading(); - // auto msg = object_detection::msg::LidarReading(); + // read distance data from all sensors + msg.sensor_1 = tower->getDistance().distance.at(0); + msg.sensor_2 = tower->getDistance().distance.at(2); + msg.sensor_3 = tower->getDistance().distance.at(4); + msg.sensor_4 = tower->getDistance().distance.at(6); - // msg.sensor_1 = tower->getDistance().distance.at(0); - // msg.sensor_2 = tower->getDistance().distance.at(2); - // msg.sensor_3 = tower->getDistance().distance.at(4); - // msg.sensor_4 = tower->getDistance().distance.at(6); + // read data from built-in IMU + ImuData imu_data = tower->getImuData(); - // ImuData imu_data = tower->getImuData(); + for (size_t i = 0; i < imu_data.data.size(); i++) + { + msg.imu_data.push_back(imu_data.data[i]); + } - // for (size_t i = 0; i < imu_data.data.size(); i++) - // { - // msg.imu_data.push_back(imu_data.data[i]); - // } - // // publisher_->publish(msg); - // RCLCPP_INFO(this->get_logger(), "Publishing message"); - } + // publish message + publisher_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Publishing message"); + } - // rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; + // publisher for lidar data + rclcpp::Publisher::SharedPtr publisher_; + // timer for publishing readings + rclcpp::TimerBase::SharedPtr timer_; - // terabee tower evo variables - std::unique_ptr tower; - std::unique_ptr factory; + // terabee tower evo variables + std::unique_ptr tower; + std::unique_ptr factory; }; int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index dd733c46..0376e8dd 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -5,9 +5,7 @@ * and PX4 flight controller alive by sending OffboardControl messages */ #include - #include "rclcpp/rclcpp.hpp" - #include using namespace std::chrono_literals; From 44b3f135a9e76f34271ee5c36c3032b2d4f051da Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 5 May 2023 16:28:27 +0200 Subject: [PATCH 188/188] add comments and improve code for object_detection package --- src/object_detection/src/lidar_reader.cpp | 33 +++--- src/object_detection/src/multiflex_reader.cpp | 112 ++++++++++-------- 2 files changed, 79 insertions(+), 66 deletions(-) diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index df21a116..265d35a2 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -33,21 +33,19 @@ public: publisher_ = this->create_publisher("drone/object_detection", 10); timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); - // set lidar to measure including IMU - ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); - factory = terabee::ITerarangerFactory::getFactory(); tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string()); - if (!tower) + if (!tower) // check if the object could be created { RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo"); return; } - tower->setImuMode(mode); + // set lidar to measure including IMU + tower->setImuMode(ITerarangerTowerEvo::QuaternionLinearAcc); - if (!tower->initialize()) + if (!tower->initialize()) // check if communication with the sensor works { RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo"); return; @@ -55,6 +53,19 @@ public: } private: + // publisher for lidar data + rclcpp::Publisher::SharedPtr publisher_; + // timer for publishing readings + rclcpp::TimerBase::SharedPtr timer_; + + // terabee tower evo variables + std::unique_ptr tower; + std::unique_ptr factory; + + /** + * @brief Reads the data from the LIDAR distance modules and the IMU, and publishes it onto the drone/object_detection topic + * + */ void read_lidar_data() { auto msg = object_detection::msg::LidarReading(); @@ -67,7 +78,6 @@ private: // read data from built-in IMU ImuData imu_data = tower->getImuData(); - for (size_t i = 0; i < imu_data.data.size(); i++) { msg.imu_data.push_back(imu_data.data[i]); @@ -77,15 +87,6 @@ private: publisher_->publish(msg); RCLCPP_INFO(this->get_logger(), "Publishing message"); } - - // publisher for lidar data - rclcpp::Publisher::SharedPtr publisher_; - // timer for publishing readings - rclcpp::TimerBase::SharedPtr timer_; - - // terabee tower evo variables - std::unique_ptr tower; - std::unique_ptr factory; }; int main(int argc, char *argv[]) diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp index b92b3db6..f2e92272 100644 --- a/src/object_detection/src/multiflex_reader.cpp +++ b/src/object_detection/src/multiflex_reader.cpp @@ -18,64 +18,76 @@ using namespace std::chrono_literals; class MultiflexReader : public rclcpp::Node { public: - MultiflexReader() - : Node("multiflex_reader") - { - rcl_interfaces::msg::ParameterDescriptor serial_port_descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - serial_port_descriptor.description = "Serial port of the USB port that the multiflex PCB is connected to."; - this->declare_parameter("multiflex_serial_port", "/dev/ttyACM0", serial_port_descriptor); - - factory = terabee::ITerarangerFactory::getFactory(); - multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string()); - - if (!multiflex) + MultiflexReader() + : Node("multiflex_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex"); - return; + // get serial port of sensor through parameter + rcl_interfaces::msg::ParameterDescriptor serial_port_descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + serial_port_descriptor.description = "Serial port of the USB port that the multiflex PCB is connected to."; + this->declare_parameter("multiflex_serial_port", "/dev/ttyACM0", serial_port_descriptor); + + // create the sensor object + factory = terabee::ITerarangerFactory::getFactory(); + multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string()); + + if (!multiflex) // check if the object can be created + { + RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex"); + return; + } + + if (!multiflex->initialize()) // check if communication with the sensor works + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex"); + return; + } + + if (!multiflex->configureNumberOfSensors(0x3f)) // check if all 6 distance sensors work + { + RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Multiflex initialized"); + + publisher_ = this->create_publisher("drone/object_detection", 10); + timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this)); } - if (!multiflex->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex"); - return; - } - - if (!multiflex->configureNumberOfSensors(0x3f)) - { - RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!"); - return; - } - - RCLCPP_INFO(this->get_logger(), "Multiflex initialized"); - - - publisher_ = this->create_publisher("drone/object_detection", 10); - timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this)); - } - private: - void read_multiflex_data() - { - terabee::DistanceData data = multiflex->getDistance(); - auto msg = object_detection::msg::MultiflexReading(); - for (size_t i = 0; i < data.size(); i++) + // factory and object for multiflex sensor + std::unique_ptr factory; + std::unique_ptr multiflex; + + // timer and publisher for publishing message onto topic + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + /** + * @brief Reads distance data from the sensor and publishes it onto the drone/object_detection topic + * + */ + void read_multiflex_data() { - msg.distance_data[i] = data.distance[i]; + // get distance reading + terabee::DistanceData data = multiflex->getDistance(); + + // populate message with readings + auto msg = object_detection::msg::MultiflexReading(); + for (size_t i = 0; i < data.size(); i++) + { + msg.distance_data[i] = data.distance[i]; + } + + // publish message + publisher_->publish(msg); } - publisher_->publish(msg); - } - - std::unique_ptr factory; - std::unique_ptr multiflex; - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; }; int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; }