From 55ec3d7f896ae06e840cf03d187ad16250b4b1a5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 15:42:15 +0200 Subject: [PATCH 01/84] merge with main --- 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 1d5ecf96c8395050f9d5f99768f8319e35e8867a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 15:43:57 +0200 Subject: [PATCH 02/84] add drone services in dependencies of heartbeat target --- src/px4_connection/CMakeLists.txt | 1 + src/px4_msgs | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index 638d46f7..7a16c5c1 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -30,6 +30,7 @@ ament_target_dependencies( rclcpp px4_ros_com px4_msgs + drone_services ) add_executable(px4_controller src/px4_controller.cpp) 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 8a94168414db86501ad6b82db7040017d49015ac Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 16:23:01 +0200 Subject: [PATCH 03/84] try to build --- src/px4_connection/src/heartbeat.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 3f1c5119..75d8ed96 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -69,14 +69,14 @@ private: const std::shared_ptr request, const std::shared_ptr response) { - if (request->control < 0 || request->control > CONTROL_POSITION_POS) - { - response->status = 1; - } else { - this->control_mode = request->control - RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode) - response->status = 0; - } + // if (request->control < 0 || request->control > CONTROL_POSITION_POS) + // { + // response->status = 1; + // } else { + // this->control_mode = request->control + // RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode) + // response->status = 0; + // } } }; From b3d170a3db07435f4496437ca1f2af9f12fa4e36 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 16:25:02 +0200 Subject: [PATCH 04/84] add include for drone services --- src/px4_connection/src/heartbeat.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 75d8ed96..e8bccfa1 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -7,6 +7,7 @@ #include #include "rclcpp/rclcpp.hpp" #include +#include #define CONTROL_ACTUATOR_POS 0 #define CONTROL_BODY_RATE_POS 1 From 3665d0ddd7a31a63c57e2f91eb5dca632c51cb3e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 16:27:10 +0200 Subject: [PATCH 05/84] add bind and fix typo --- src/px4_connection/src/heartbeat.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index e8bccfa1..6fb9240a 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -24,9 +24,9 @@ public: HeartBeat() : Node("heartbeat") { // disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind()) - // create a publisher on the offboard control mode topic - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind(&Heartbeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + // 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(); @@ -66,7 +66,7 @@ private: } void handle_vehicle_control_change( - const std::shared_ptr request_header, + const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) { From 436051db5e98d86ad1bfb079b8a4d9b1f0a84e8a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 16:28:01 +0200 Subject: [PATCH 06/84] typo --- src/px4_connection/src/heartbeat.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 6fb9240a..c6306143 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -24,7 +24,7 @@ public: HeartBeat() : Node("heartbeat") { // disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind(&Heartbeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // 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 @@ -70,14 +70,14 @@ private: const std::shared_ptr request, const std::shared_ptr response) { - // if (request->control < 0 || request->control > CONTROL_POSITION_POS) - // { - // response->status = 1; - // } else { - // this->control_mode = request->control - // RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode) - // response->status = 0; - // } + if (request->control < 0 || request->control > CONTROL_POSITION_POS) + { + response->status = 1; + } else { + this->control_mode = request->control + RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode) + response->status = 0; + } } }; From 567604033bfc06f78b646bc00383f9ffe79721da Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 16:28:46 +0200 Subject: [PATCH 07/84] ; --- 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 c6306143..716e36cd 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -74,8 +74,8 @@ private: { response->status = 1; } else { - this->control_mode = request->control - RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode) + this->control_mode = request->control; + RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode); response->status = 0; } From 89a37a8f9cf071d6459e437e66367d42a78c53c3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 16:40:01 +0200 Subject: [PATCH 08/84] add print of service --- src/px4_connection/src/heartbeat.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 716e36cd..d989b02c 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -24,7 +24,8 @@ public: HeartBeat() : Node("heartbeat") { // disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + vehicle_control_mode_service_ = this->create_service("/drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + RCLCPP_INFO(this->get_logger(), "initialized service /drone/set_vehicle_control"); // 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 From 5424cfb3f49cf2ac0ef731c62656f4510721c452 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 11 May 2023 14:02:24 +0200 Subject: [PATCH 09/84] add drone control mode message definition --- src/drone_services/CMakeLists.txt | 1 + src/drone_services/msg/DroneControlMode.msg | 1 + src/test_controls/test_controls/test_controller.py | 4 +++- 3 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 src/drone_services/msg/DroneControlMode.msg diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 00b7c0fa..60d7a10b 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetVelocity.srv" "srv/TakePicture.srv" "srv/SetVehicleControl.srv" + "msg/DroneControlMode.msg" ) if(BUILD_TESTING) diff --git a/src/drone_services/msg/DroneControlMode.msg b/src/drone_services/msg/DroneControlMode.msg new file mode 100644 index 00000000..68758456 --- /dev/null +++ b/src/drone_services/msg/DroneControlMode.msg @@ -0,0 +1 @@ +int32 mode \ No newline at end of file diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 312780bb..8a6d7bda 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -5,6 +5,7 @@ from sshkeyboard import listen_keyboard_manual import asyncio from drone_services.srv import SetAttitude +from drone_services.srv import SetVehicleControl class TestController(Node): @@ -13,7 +14,8 @@ class TestController(Node): 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.get_logger().info('set attitude service not available, waiting again...') + self.vehicle_control_cli = self.create_client(SetVehicleControl, 'drone/set_vehicle_control') 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\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") From 9785136a4849fb2543f9018847adfea3f61c90ee Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 12:16:14 +0200 Subject: [PATCH 10/84] defines and logging control mode --- src/drone_services/msg/DroneControlMode.msg | 6 ++- src/px4_connection/src/heartbeat.cpp | 55 +++++++++++++++++++-- 2 files changed, 55 insertions(+), 6 deletions(-) diff --git a/src/drone_services/msg/DroneControlMode.msg b/src/drone_services/msg/DroneControlMode.msg index 68758456..92669b9e 100644 --- a/src/drone_services/msg/DroneControlMode.msg +++ b/src/drone_services/msg/DroneControlMode.msg @@ -1 +1,5 @@ -int32 mode \ No newline at end of file +# control mode of the drone +# 1: Attitude +# 2: Velocity +# 3: Position +int8 control_mode \ No newline at end of file diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index d989b02c..09718930 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -8,6 +8,11 @@ #include "rclcpp/rclcpp.hpp" #include #include +#include + +#define CONTROL_MODE_ATTITUDE 1 +#define CONTROL_MODE_VELOCITY 2 +#define CONTROL_MODE_POSITION 3 #define CONTROL_ACTUATOR_POS 0 #define CONTROL_BODY_RATE_POS 1 @@ -23,9 +28,10 @@ class HeartBeat : public rclcpp::Node public: HeartBeat() : Node("heartbeat") { - // disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); vehicle_control_mode_service_ = this->create_service("/drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); RCLCPP_INFO(this->get_logger(), "initialized service /drone/set_vehicle_control"); + // create a publisher on the drone control mode topic + drone_control_mode_publisher_ = this->create_publisher("/drone/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 @@ -37,14 +43,17 @@ public: private: // publisher for offboard control mode messages rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + // publisher for the control mode of the drone + rclcpp::Publisher::SharedPtr drone_control_mode_publisher_; // timer for sending the heartbeat rclcpp::TimerBase::SharedPtr timer_; + // service to change vehicle_mode + rclcpp::Service::SharedPtr vehicle_control_mode_service_; + // start time of node in seconds double start_time; // which level of control is needed, only attitude control by default int control_mode = 1 << CONTROL_ATTITUDE_POS; - // service to change vehicle_mode - rclcpp::Service::SharedPtr vehicle_control_mode_service_; /** * @brief Publish offboard control mode messages as a heartbeat. * Only the attitude is enabled, because that is how the drone will be controlled. @@ -66,6 +75,40 @@ private: offboard_control_mode_publisher_->publish(msg); } +/** + * @brief Publish the current control mode of the drone onto the 'drone/control_mode' topic + * + */ + void publish_new_control_mode() + { + auto msg = drone_services::msg::DroneControlMode(); + if ((this->control_mode >> CONTROL_ATTITUDE_POS) & 1) + { + msg.control_mode = CONTROL_MODE_ATTITUDE; + RCLCPP_INFO(this->get_logger(), "set control mode to attitude"); + } + else if ((this->control_mode >> CONTROL_VELOCITY_POS) & 1) + { + msg.control_mode = CONTROL_MODE_VELOCITY; + RCLCPP_INFO(this->get_logger(), "set control mode to velocity"); + } + else if ((this->control_mode >> CONTROL_POSITION_POS) & 1) + { + msg.control_mode = CONTROL_MODE_POSITION; + RLCPP_INFO(this->get_logger(), "set control mode to position"); + } + RCLCPP_INFO(this->get_logger(), "publishing new control mode %d", msg.control_mode); + drone_control_mode_publisher_->publish(msg); + } + + /** + * @brief Handle a request to change the vehicle control mode + * + * @param request_header the header of the service request + * @param request the request that was sent to the service + * @param response the reponse that the service sends back to the client. + */ + void handle_vehicle_control_change( const std::shared_ptr request_header, const std::shared_ptr request, @@ -74,12 +117,14 @@ private: if (request->control < 0 || request->control > CONTROL_POSITION_POS) { response->status = 1; - } else { + } + else + { this->control_mode = request->control; RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode); + publish_new_control_mode(); response->status = 0; } - } }; From 9faab231e79f9f05ddc7d81e108d7c16f5a79df6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 12:19:03 +0200 Subject: [PATCH 11/84] change checking of set bit --- src/px4_connection/src/heartbeat.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 09718930..2a99a67c 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -82,17 +82,17 @@ private: void publish_new_control_mode() { auto msg = drone_services::msg::DroneControlMode(); - if ((this->control_mode >> CONTROL_ATTITUDE_POS) & 1) + if (this->control_mode & (1 << CONTROL_ATTITUDE_POS)) { msg.control_mode = CONTROL_MODE_ATTITUDE; RCLCPP_INFO(this->get_logger(), "set control mode to attitude"); } - else if ((this->control_mode >> CONTROL_VELOCITY_POS) & 1) + else if (this->control_mode & (1 << CONTROL_VELOCITY_POS)) { msg.control_mode = CONTROL_MODE_VELOCITY; RCLCPP_INFO(this->get_logger(), "set control mode to velocity"); } - else if ((this->control_mode >> CONTROL_POSITION_POS) & 1) + else if (this->control_mode & (1 << CONTROL_POSITION_POS)) { msg.control_mode = CONTROL_MODE_POSITION; RLCPP_INFO(this->get_logger(), "set control mode to position"); From b20a75dc92d7149375cb4b622b2441464a0a60cf Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 12:22:47 +0200 Subject: [PATCH 12/84] typo --- 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 2a99a67c..13480867 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -95,7 +95,7 @@ private: else if (this->control_mode & (1 << CONTROL_POSITION_POS)) { msg.control_mode = CONTROL_MODE_POSITION; - RLCPP_INFO(this->get_logger(), "set control mode to position"); + RCLCPP_INFO(this->get_logger(), "set control mode to position"); } RCLCPP_INFO(this->get_logger(), "publishing new control mode %d", msg.control_mode); drone_control_mode_publisher_->publish(msg); From 10191e122bde87fdb7e41b2ce6d23514d9d32ac4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 12:29:31 +0200 Subject: [PATCH 13/84] change checking of control_mode --- 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 13480867..0ed09f55 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -114,7 +114,7 @@ private: const std::shared_ptr request, const std::shared_ptr response) { - if (request->control < 0 || request->control > CONTROL_POSITION_POS) + if (request->control < 0 || request->control > (1 << CONTROL_POSITION_POS)) { response->status = 1; } From 6bc357b6f8f2f93f6835a26d2344e75650da5757 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 12:42:13 +0200 Subject: [PATCH 14/84] add control modes h file --- src/drone_services/srv/SetVehicleControl.srv | 4 ++++ src/px4_connection/include/drone_control_modes.h | 15 +++++++++++++++ src/px4_connection/src/heartbeat.cpp | 11 ++--------- src/px4_connection/src/px4_controller.cpp | 2 ++ 4 files changed, 23 insertions(+), 9 deletions(-) create mode 100644 src/px4_connection/include/drone_control_modes.h diff --git a/src/drone_services/srv/SetVehicleControl.srv b/src/drone_services/srv/SetVehicleControl.srv index c0dd2411..d96ce97a 100644 --- a/src/drone_services/srv/SetVehicleControl.srv +++ b/src/drone_services/srv/SetVehicleControl.srv @@ -6,6 +6,10 @@ # bit 4: velocity # bit 5: position +# used control modes: +# 4: attitude and thrust +# 16: velocity +# 32: position int32 control # control bitmask --- int8 status # status of operation \ No newline at end of file diff --git a/src/px4_connection/include/drone_control_modes.h b/src/px4_connection/include/drone_control_modes.h new file mode 100644 index 00000000..d1324b90 --- /dev/null +++ b/src/px4_connection/include/drone_control_modes.h @@ -0,0 +1,15 @@ +#ifndef DRONE_CONTROL_MODES_H +#define DRONE_CONTROL_MODES_H + +#define CONTROL_MODE_ATTITUDE 1 +#define CONTROL_MODE_VELOCITY 2 +#define CONTROL_MODE_POSITION 3 + +#define CONTROL_ACTUATOR_POS 0 +#define CONTROL_BODY_RATE_POS 1 +#define CONTROL_ATTITUDE_POS 2 +#define CONTROL_ACCELERATION_POS 3 +#define CONTROL_VELOCITY_POS 4 +#define CONTROL_POSITION_POS 5 + +#endif \ No newline at end of file diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 0ed09f55..9c4bdd04 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -10,16 +10,9 @@ #include #include -#define CONTROL_MODE_ATTITUDE 1 -#define CONTROL_MODE_VELOCITY 2 -#define CONTROL_MODE_POSITION 3 +#include "drone_control_modes.h" + -#define CONTROL_ACTUATOR_POS 0 -#define CONTROL_BODY_RATE_POS 1 -#define CONTROL_ATTITUDE_POS 2 -#define CONTROL_ACCELERATION_POS 3 -#define CONTROL_VELOCITY_POS 4 -#define CONTROL_POSITION_POS 5 using namespace std::chrono_literals; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9f28763c..250c0f60 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -20,6 +20,8 @@ #include +#include "drone_control_modes.h" + #define D_SPEED(x) -x - 9.81 using namespace std::chrono_literals; From 3da7ac030611a776b653bd7f2f94290b701024a5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 12:52:33 +0200 Subject: [PATCH 15/84] change service response to success boolean --- .vscode/c_cpp_properties.json | 3 ++- src/drone_services/srv/SetAttitude.srv | 2 +- src/drone_services/srv/SetVehicleControl.srv | 2 +- src/drone_services/srv/SetVelocity.srv | 2 +- src/px4_connection/src/heartbeat.cpp | 4 ++-- src/px4_connection/src/px4_controller.cpp | 18 +++++++++++++++--- 6 files changed, 22 insertions(+), 9 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6743b427..b8851067 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -12,7 +12,8 @@ "/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/**", - "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**" + "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**", + "/home/ubuntu/ros2_ws/src/px4_connection/include/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv index 52957c0c..9a8125d7 100644 --- a/src/drone_services/srv/SetAttitude.srv +++ b/src/drone_services/srv/SetAttitude.srv @@ -6,4 +6,4 @@ float32 roll #thrust between -1 and 1 float32 thrust --- -int8 status +bool success diff --git a/src/drone_services/srv/SetVehicleControl.srv b/src/drone_services/srv/SetVehicleControl.srv index d96ce97a..6ae9ec4c 100644 --- a/src/drone_services/srv/SetVehicleControl.srv +++ b/src/drone_services/srv/SetVehicleControl.srv @@ -12,4 +12,4 @@ # 32: position int32 control # control bitmask --- -int8 status # status of operation \ No newline at end of file +bool success # if operation succeeded \ No newline at end of file diff --git a/src/drone_services/srv/SetVelocity.srv b/src/drone_services/srv/SetVelocity.srv index 0d85cba9..6602ab3e 100644 --- a/src/drone_services/srv/SetVelocity.srv +++ b/src/drone_services/srv/SetVelocity.srv @@ -6,4 +6,4 @@ float32 z_speed #angle is in degrees float32 angle --- -int8 status +bool success diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 9c4bdd04..4e5795c5 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -109,14 +109,14 @@ private: { if (request->control < 0 || request->control > (1 << CONTROL_POSITION_POS)) { - response->status = 1; + response->success = false; } else { this->control_mode = request->control; RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode); publish_new_control_mode(); - response->status = 0; + response->success = true; } } }; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 250c0f60..f4b561fc 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -42,7 +43,8 @@ public: // 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)); - + control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_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)); @@ -58,10 +60,13 @@ private: rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; rclcpp::Subscription::SharedPtr vehicle_attitude_subscription_; + rclcpp::Subscription::SharedPtr control_mode_subscription_; rclcpp::Service::SharedPtr set_attitude_service_; rclcpp::Service::SharedPtr disarm_service_; + + // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -83,6 +88,8 @@ private: float base_q[4] = {0, 0, 0, 0}; int base_q_amount = 0; + char current_control_mode = CONTROL_MODE_ATTITUDE; + // result quaternion std::array q = {0, 0, 0, 0}; @@ -117,7 +124,7 @@ 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); new_setpoint = true; - response->status = 0; + response->success = true; } else { @@ -125,7 +132,7 @@ private: last_setpoint[0] = 0; last_setpoint[1] = 0; last_setpoint[2] = 0; - response->status = 1; + response->success = false; } } @@ -286,6 +293,11 @@ private: } } + void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg) + { + + } + /** * @brief Publish vehicle commands * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) From 55e7af159ff30aebe114459ea351ecde2c3839d0 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 13:56:49 +0200 Subject: [PATCH 16/84] add sending different setpoints --- src/drone_services/srv/SetAttitude.srv | 2 + src/drone_services/srv/SetVehicleControl.srv | 1 + .../include/drone_control_modes.h | 5 + src/px4_connection/src/px4_controller.cpp | 175 +++++++++++------- 4 files changed, 111 insertions(+), 72 deletions(-) diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv index 9a8125d7..03a30ec9 100644 --- a/src/drone_services/srv/SetAttitude.srv +++ b/src/drone_services/srv/SetAttitude.srv @@ -1,3 +1,5 @@ +#service to set attitude setpoints for manual attitude control + #all angles are in degrees float32 yaw float32 pitch diff --git a/src/drone_services/srv/SetVehicleControl.srv b/src/drone_services/srv/SetVehicleControl.srv index 6ae9ec4c..3ab33ea3 100644 --- a/src/drone_services/srv/SetVehicleControl.srv +++ b/src/drone_services/srv/SetVehicleControl.srv @@ -1,3 +1,4 @@ +# service to set control mode for heartbeat messages # bitmask for control: # bit 0: actuator # bit 1: body rate diff --git a/src/px4_connection/include/drone_control_modes.h b/src/px4_connection/include/drone_control_modes.h index d1324b90..16594db6 100644 --- a/src/px4_connection/include/drone_control_modes.h +++ b/src/px4_connection/include/drone_control_modes.h @@ -5,6 +5,9 @@ #define CONTROL_MODE_VELOCITY 2 #define CONTROL_MODE_POSITION 3 +#define CONTROL_MODE_MIN CONTROL_MODE_ATTITUDE +#define CONTROL_MODE_MAX CONTROL_MODE_POSITION + #define CONTROL_ACTUATOR_POS 0 #define CONTROL_BODY_RATE_POS 1 #define CONTROL_ATTITUDE_POS 2 @@ -12,4 +15,6 @@ #define CONTROL_VELOCITY_POS 4 #define CONTROL_POSITION_POS 5 + + #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 f4b561fc..f9456fcd 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -1,7 +1,7 @@ /** * @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. + * @brief Controller node to contol the PX4 using attitude or trajectory setpoints. * It subscribes to the /drone/set_attitude topic to receive control commands */ @@ -23,7 +23,8 @@ #include "drone_control_modes.h" -#define D_SPEED(x) -x - 9.81 +#define DEFAULT_YAW_SPEED 0.6 // default turning speed in radians per second +#define D_SPEED(x) -x - 9.81 // a speed up of x m/s has to be ajusted for the earths gravity using namespace std::chrono_literals; @@ -44,10 +45,10 @@ public: vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1)); control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_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)); + 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)); + arm_service = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_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)); @@ -64,15 +65,14 @@ private: rclcpp::Service::SharedPtr set_attitude_service_; rclcpp::Service::SharedPtr disarm_service_; - - + rclcpp::Service::SharedPtr arm_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; double start_time_; bool has_sent_status = false; - bool flying = false; + bool flying = false; // if user has taken over control bool armed = false; bool has_swithed = false; int setpoint_count = 0; @@ -82,13 +82,16 @@ private: 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; - float last_thrust = 0; + float last_angle = 0; // angle in radians (-PI .. PI) + float last_thrust = -0.1; // default 10% thrust for when the drone gets armed + + float velocity[3] = {0, 0, 0}; + float position[3] = {0, 0, 0}; float base_q[4] = {0, 0, 0, 0}; int base_q_amount = 0; - char current_control_mode = CONTROL_MODE_ATTITUDE; + char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control // result quaternion std::array q = {0, 0, 0, 0}; @@ -153,70 +156,58 @@ private: if (armed) { armed = false; + flying = false; publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); } } - void send_trajectory_setpoint() + /** + * @brief arms the drone when a call to the arm service is made + * + * @param request_header the header for the service request + * @param request the request (empty) + * @param response the response (empty) + */ + void handle_arm_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) { + RCLCPP_INFO(this->get_logger(), "Got arm request..."); - auto msg = px4_msgs::msg::TrajectorySetpoint(); - if (this->get_clock()->now().seconds() - start_time_ < 10) + if (!armed) { - msg.acceleration[0] = 0; - msg.acceleration[1] = 0; - msg.acceleration[2] = D_SPEED(10); - msg.yawspeed = 0; - msg.yaw = -3.14; + 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; } else { - if (!has_swithed) - { - RCLCPP_INFO(this->get_logger(), "waiting for service input..."); - has_swithed = true; - } - - msg.acceleration[0] = last_setpoint[0]; - msg.acceleration[1] = last_setpoint[1]; - msg.acceleration[2] = D_SPEED(last_setpoint[2]); - msg.yawspeed = 0.5; + RCLCPP_ERROR(this->get_logger(), "Arm request received but the drone is already armed!"); } - - msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; - - trajectory_setpoint_publisher->publish(msg); } - void send_attitude_setpoint() + void send_idle_attitude_setpoint() + { + send_attitude_setpoint(false); + } + + void send_attitude_setpoint(bool new_setpoint) { // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); - // 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; 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 - // { - // 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 msg.q_d[0] = q.at(0); msg.q_d[1] = base_q[1] + q.at(1); @@ -237,33 +228,62 @@ private: vehicle_setpoint_publisher_->publish(msg); } + void send_velocity_setpoint() + { + auto msg = px4_msgs::msg::TrajectorySetpoint(); + + msg.velocity[0] = velocity[0]; + msg.velocity[1] = velocity[1]; + msg.velocity[2] = D_SPEED(velocity[2]); + + publish_trajectory_setpoint(msg); + } + + void send_position_setpoint() + { + auto msg = px4_msgs::msg::TrajectorySetpoint(); + + for (int i = 0; i < 3; i++) + { + msg.position[i] = position[i]; + } + + publish_trajectory_setpoint(msg); + } + + void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg) + { + msg.yaw = last_angle; + + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + trajectory_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++; - - ready_to_fly = setpoint_count == 20; - // after 20 setpoints, send arm command to make drone actually fly - if (ready_to_fly) + if (!flying) { - // 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; - armed = true; + send_idle_attitude_setpoint(); + } + else + { + if (current_control_mode == CONTROL_MODE_ATTITUDE) + { + send_attitude_setpoint(new_setpoint); + } + else if (current_control_mode == CONTROL_MODE_VELOCITY) + { + send_velocity_setpoint(); + } + else if (current_control_mode == CONTROL_MODE_POSITION) + { + send_position_setpoint(); + } } - - send_attitude_setpoint(); } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) @@ -282,7 +302,9 @@ private: 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 { + } + else + { base_q[1] = msg->q[1]; base_q[2] = msg->q[2]; base_q[3] = msg->q[3]; @@ -295,7 +317,16 @@ private: void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg) { - + if (msg->control_mode >= CONTROL_MODE_MIN && msg->control_mode <= CONTROL_MODE_MAX) + { + current_control_mode = msg->control_mode; + flying = true; // user has taken over control + RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Received invalid control mode %d", msg->control_mode); + } } /** From 77262e42e533a70c295e206db1c0c6c2f0a89355 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 13:57:54 +0200 Subject: [PATCH 17/84] setting default yaw speed --- 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 f9456fcd..292502b2 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -254,7 +254,7 @@ private: void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg) { msg.yaw = last_angle; - + msg.yawspeed = DEFAULT_YAW_SPEED; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; trajectory_setpoint_publisher->publish(msg); } From ceb1c8222bb2bff3ede3b91449f525ef9c9ba305 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 14:03:05 +0200 Subject: [PATCH 18/84] fix include name --- 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 292502b2..b431a2f8 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include From 503d8f80a99e883f888f46e1ac61909540f283e7 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 14:05:38 +0200 Subject: [PATCH 19/84] fix name of arm service variable --- 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 b431a2f8..843fc045 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -48,7 +48,7 @@ public: 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)); - arm_service = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_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 cdc8a9fed2b0edc38f577f42509502ed694b5a84 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 14:20:37 +0200 Subject: [PATCH 20/84] add extra log --- 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 843fc045..fcf3ea21 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -320,13 +320,14 @@ private: if (msg->control_mode >= CONTROL_MODE_MIN && msg->control_mode <= CONTROL_MODE_MAX) { current_control_mode = msg->control_mode; + RCLCPP_INFO(this->get_logger(), "Got valid control mode"); flying = true; // user has taken over control - RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode); } else { RCLCPP_ERROR(this->get_logger(), "Received invalid control mode %d", msg->control_mode); } + RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode); } /** From e4e9d60eadb6c038ef98c7705349ea9a11e93d45 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 14:54:22 +0200 Subject: [PATCH 21/84] add more logs --- src/px4_connection/src/px4_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fcf3ea21..b10ddccc 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -273,14 +273,17 @@ private: { if (current_control_mode == CONTROL_MODE_ATTITUDE) { + RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); send_attitude_setpoint(new_setpoint); } else if (current_control_mode == CONTROL_MODE_VELOCITY) { + RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); send_velocity_setpoint(); } else if (current_control_mode == CONTROL_MODE_POSITION) { + RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); send_position_setpoint(); } } From 94c71e4aa0951dbf82932506084f029032347f3b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 14:59:01 +0200 Subject: [PATCH 22/84] log --- 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 b10ddccc..319ee9c3 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -267,6 +267,7 @@ private: { if (!flying) { + RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint"); send_idle_attitude_setpoint(); } else From 3ba698cc5c152f29a15d04240108c25827c91aaa Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 15:02:13 +0200 Subject: [PATCH 23/84] log variables --- src/px4_connection/src/px4_controller.cpp | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 319ee9c3..801d5af0 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -83,7 +83,7 @@ private: float last_setpoint[3] = {0, 0, 0}; float last_angle = 0; // angle in radians (-PI .. PI) - float last_thrust = -0.1; // default 10% thrust for when the drone gets armed + float last_thrust = 0; // default 10% thrust for when the drone gets armed float velocity[3] = {0, 0, 0}; float position[3] = {0, 0, 0}; @@ -183,6 +183,7 @@ private: this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); RCLCPP_INFO(this->get_logger(), "Arm command sent"); + this->last_thrust = -0.1; // spin motors at 10% thrust armed = true; } else @@ -191,12 +192,7 @@ private: } } - void send_idle_attitude_setpoint() - { - send_attitude_setpoint(false); - } - - void send_attitude_setpoint(bool new_setpoint) + void send_attitude_setpoint() { // set message to enable attitude @@ -214,12 +210,6 @@ private: msg.q_d[2] = base_q[2] + q.at(2); msg.q_d[3] = base_q[3] + q.at(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; msg.fw_control_yaw_wheel = false; @@ -265,10 +255,11 @@ private: */ void send_setpoint() { + RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!flying) { RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint"); - send_idle_attitude_setpoint(); + send_attitude_setpoint(); } else { From eae3620848d6c05afa72ab6e15beb2879d8f2b4b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 15:06:05 +0200 Subject: [PATCH 24/84] remove new_setpoint bool --- 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 801d5af0..e9f257f1 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -266,7 +266,7 @@ private: if (current_control_mode == CONTROL_MODE_ATTITUDE) { RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); - send_attitude_setpoint(new_setpoint); + send_attitude_setpoint(); } else if (current_control_mode == CONTROL_MODE_VELOCITY) { From 3b41f4f81d108538fa483b5a9e745393cc8c8c6f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 15:13:08 +0200 Subject: [PATCH 25/84] add test arming back after 20 setpoints --- src/px4_connection/src/px4_controller.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e9f257f1..39d25c57 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -82,7 +82,7 @@ private: 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; // angle in radians (-PI .. PI) + float last_angle = 0; // angle in radians (-PI .. PI) float last_thrust = 0; // default 10% thrust for when the drone gets armed float velocity[3] = {0, 0, 0}; @@ -255,6 +255,20 @@ private: */ void send_setpoint() { + + setpoint_count++; + if (setpoint_count == 20) + { + 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"); + this->last_thrust = -0.1; // spin motors at 10% thrust + armed = true; + } + RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!flying) { From 46d32066a294bed56741cb1cf06d3196a5ecfeb4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 15:46:22 +0200 Subject: [PATCH 26/84] bruh --- src/px4_connection/src/heartbeat.cpp | 9 ++++++++- src/px4_connection/src/px4_controller.cpp | 3 +++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 4e5795c5..eb662cf3 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -56,13 +56,20 @@ private: { // set message to enable attitude based on control mode variable auto msg = px4_msgs::msg::OffboardControlMode(); + /* msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false; msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false; msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false; msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false; msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false; msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false; - + */ + 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); diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 39d25c57..98bcc1a0 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -269,6 +269,8 @@ private: armed = true; } + send_attitude_setpoint(); + /* RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!flying) { @@ -293,6 +295,7 @@ private: send_position_setpoint(); } } + */ } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) From 2c4f6017595ebbffbefa79434bed53c59fba3a74 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 16:18:39 +0200 Subject: [PATCH 27/84] add trajectory setpoint handling --- src/drone_services/CMakeLists.txt | 1 + src/drone_services/srv/SetTrajectory.srv | 7 ++++ src/px4_connection/src/px4_controller.cpp | 45 +++++++++++++++++++++-- 3 files changed, 49 insertions(+), 4 deletions(-) create mode 100644 src/drone_services/srv/SetTrajectory.srv diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 60d7a10b..f840cd5e 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/SetTrajectory.srv" "srv/SetVelocity.srv" "srv/TakePicture.srv" "srv/SetVehicleControl.srv" diff --git a/src/drone_services/srv/SetTrajectory.srv b/src/drone_services/srv/SetTrajectory.srv new file mode 100644 index 00000000..17058ee1 --- /dev/null +++ b/src/drone_services/srv/SetTrajectory.srv @@ -0,0 +1,7 @@ +# control mode velocity or position, refer to drone_services/msg/DroneControlMode.msg +int8 control_mode +# x, y, z values +float32[3] values +float32 yaw +--- +bool success \ 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 98bcc1a0..61c4db95 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -46,7 +47,8 @@ public: vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1)); control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_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)); + set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + set_trajectory_service_ = this->create_service("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_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)); arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // create timer to send vehicle attitude setpoints every second @@ -64,6 +66,7 @@ private: rclcpp::Subscription::SharedPtr control_mode_subscription_; rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr set_trajectory_service_; rclcpp::Service::SharedPtr disarm_service_; rclcpp::Service::SharedPtr arm_service_; @@ -96,7 +99,7 @@ private: // result quaternion std::array q = {0, 0, 0, 0}; - void set_setpoint( + void handle_attitude_setpoint( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -139,6 +142,38 @@ private: } } + void handle_trajectory_setpoint( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + if (request->control_mode != CONTROL_MODE_VELOCITY || request->control_mode != CONTROL_MODE_POSITION) + { + RCLCPP_INFO(this->get_logger(), "Got invalid control mode: %d", request->control_mode); + response->success = false; + } + else + { + if (request->control_mode == CONTROL_MODE_VELOCITY) + { + for (int i = 0; i < 3; i++) + { + velocity[i] = request->values[i]; + } + } + else if (request->control_mode == CONTROL_MODE_POSITION) + { + for (int i = 0; i < 3; i++) + { + position[i] = request->values[i]; + } + } + + last_angle = request->yaw; + response->success = true; + } + } + /** * @brief disarms the drone when a call to the disarm service is made * @@ -256,6 +291,7 @@ private: void send_setpoint() { + /* setpoint_count++; if (setpoint_count == 20) { @@ -270,7 +306,8 @@ private: } send_attitude_setpoint(); - /* + */ + RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!flying) { @@ -295,7 +332,7 @@ private: send_position_setpoint(); } } - */ + } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) From 5b4c6c149c2b7b28551fb8485872869d8bca817b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 16:19:27 +0200 Subject: [PATCH 28/84] change back control mode --- src/px4_connection/src/heartbeat.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index eb662cf3..330a85ed 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -54,22 +54,23 @@ private: */ void send_heartbeat() { - // set message to enable attitude based on control mode variable auto msg = px4_msgs::msg::OffboardControlMode(); - /* + msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false; msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false; msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false; msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false; msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false; msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false; - */ + + /* 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); From ff9b01cc79435f95211d77ce3bca8b9a1a219fd2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:25:02 +0200 Subject: [PATCH 29/84] add changing control modes in test controller --- .../test_controls/test_controller.py | 215 ++++++++++++++---- 1 file changed, 166 insertions(+), 49 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 8a6d7bda..bb2b4162 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -5,6 +5,7 @@ from sshkeyboard import listen_keyboard_manual import asyncio from drone_services.srv import SetAttitude +from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl @@ -15,23 +16,59 @@ class TestController(Node): self.cli = self.create_client(SetAttitude, 'drone/set_attitude') while not self.cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set attitude service not available, waiting again...') - self.vehicle_control_cli = self.create_client(SetVehicleControl, 'drone/set_vehicle_control') - self.req = SetAttitude.Request() + self.vehicle_control_cli = self.create_client( + SetVehicleControl, '/drone/set_vehicle_control') + while not self.vehicle_control_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('set vehicle control service not available, waiting again...') + self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory') + while not self.traj_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('set trajectory service not available, waiting again...') + + self.attitude_req = SetAttitude.Request() + self.vehicle_control_req = SetVehicleControl.Request() + self.traj_req = SetTrajectory.Request() - self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") + self.get_logger().info("Controls:1 - Attitude control\n2 - Velocity control\n3 - Position control\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\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 send_control_mode(self): + self.vehicle_control_req.control = self.control_mode + self.future = self.vehicle_control_cli.call_async(self.vehicle_control_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message vehicle control msg on service') + return self.future.result() - def send_request(self, yaw, pitch, roll, thrust): - self.req.yaw = yaw - self.req.pitch = pitch - self.req.roll = roll - self.req.thrust = thrust + def send_attitude_request(self, yaw, pitch, roll, thrust): + self.attitude_req.yaw = yaw + self.attitude_req.pitch = pitch + self.attitude_req.roll = roll + self.attitude_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) + self.future = self.cli.call_async(self.attitude_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message on service') + return self.future.result() + + def send_velocity_request(self, x, y, z, angle): + self.traj_req.control_mode = 2 + self.traj_req.yaw = angle + self.traj_req.values = [x, y, z] + self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) + self.future = self.traj_cli.call_async(self.traj_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message on service') + return self.future.result() + + def send_position_request(self, x, y, z, angle): + self.traj_req.control_mode = 3 + self.traj_req.yaw = angle + self.traj_req.values = [x, y, z] + self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) + self.future = self.traj_cli.call_async(self.traj_req) rclpy.spin_until_future_complete(self, self.future) self.get_logger().info('publishing message on service') return self.future.result() @@ -40,71 +77,151 @@ class TestController(Node): # self.get_logger().info('released ' + str(key)) pass + def move_up(self): + pass + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.05) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=1.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) + + def move_right(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=1.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=1.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=1.0, y=0.0, z=0.0, angle=0.0) + + def move_down(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.05) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + + def move_left(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=-1.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=-1.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=-1.0, y=0.0, z=0.0, angle=0.0) + + def rotate_right(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=1.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=1.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=1.0) + + def rotate_left(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=-1.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=-1.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=-1.0) + + def move_forward(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=-1.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=1.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=1.0, z=0.0, angle=0.0) + + def move_backward(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=1.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=-1.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=-1.0, z=0.0, angle=0.0) + + def stop(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=0.0) + + def move_up_little(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.01) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.5, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) + + def move_down_little(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.01) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=-0.5, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + def on_press(self, key): try: # self.get_logger().info('pressed ' + char) if key == 'w': self.get_logger().info('forward') - self.send_request(pitch=-1.0, yaw=0.0, - roll=0.0, thrust=0.0) + self.move_forward() if key == 's': self.get_logger().info('backward') - self.send_request(pitch=1.0, yaw=0.0, - roll=0.0, thrust=0.0) + self.move_backward() if key == 'a': self.get_logger().info('left') - self.send_request(pitch=0.0, yaw=0.0, - roll=-1.0, thrust=0.0) + self.move_left() if key == 'd': self.get_logger().info('right') - self.send_request(pitch=0.0, yaw=0.0, - roll=1.0, thrust=0.0) + self.move_right() if key == 'q': self.get_logger().info('rotate left') - self.send_request(pitch=0.0, yaw=-1.0, - roll=0.0, thrust=0.0) + self.rotate_left() if key == 'e': self.get_logger().info('rotate right') - self.send_request(pitch=0.0, yaw=1.0, - roll=0.0, thrust=0.0) + self.rotate_right() if key == 'z': self.get_logger().info('down') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.05) + self.move_down() if key == 'space': self.get_logger().info('up') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=0.05) + self.move_up() if key == 'v': self.get_logger().info('down a little') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.01) + self.move_down_little() if key == 'f': self.get_logger().info('up a little') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=0.01) + self.move_up_little() 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 - # 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.stop() + if key == '1': + self.get_logger().info('attitude control') + self.control_mode = 1 + if key == '2': + self.get_logger().info('velocity control') + self.control_mode = 2 + if key == '1': + self.get_logger().info('position control') + self.control_mode = 3 except Exception as e: self.get_logger().error(str(e)) From a670aa8c1ac24ca0a1865459a415c06a3578fbed Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:30:00 +0200 Subject: [PATCH 30/84] add ability to arm --- src/test_controls/test_controls/test_controller.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index bb2b4162..4ee0ce0e 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,6 +7,7 @@ import asyncio from drone_services.srv import SetAttitude from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl +from std_msgs.msg import Empty class TestController(Node): @@ -28,7 +29,9 @@ class TestController(Node): self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() - self.get_logger().info("Controls:1 - Attitude control\n2 - Velocity control\n3 - Position control\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") + self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) + + self.get_logger().info("Controls:1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") def spin(self): while rclpy.ok(): @@ -222,6 +225,9 @@ class TestController(Node): if key == '1': self.get_logger().info('position control') self.control_mode = 3 + if key == '/': + self.get_logger().info('arming') + self.arm_publisher.publish(Empty()) except Exception as e: self.get_logger().error(str(e)) From f58fb50f269ebb6c6500c6e6448fb383ee28ec5a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:33:02 +0200 Subject: [PATCH 31/84] actually send control mode --- src/test_controls/test_controls/test_controller.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 4ee0ce0e..5f225e50 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -31,7 +31,7 @@ class TestController(Node): self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) - self.get_logger().info("Controls:1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") + self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") def spin(self): while rclpy.ok(): @@ -219,12 +219,15 @@ class TestController(Node): if key == '1': self.get_logger().info('attitude control') self.control_mode = 1 + self.send_control_mode() if key == '2': self.get_logger().info('velocity control') self.control_mode = 2 - if key == '1': + self.send_control_mode() + if key == '3': self.get_logger().info('position control') self.control_mode = 3 + self.send_control_mode() if key == '/': self.get_logger().info('arming') self.arm_publisher.publish(Empty()) From e245e143caacf7bff3b370d86d566508d2b2fdf8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:34:36 +0200 Subject: [PATCH 32/84] correct control modes --- 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 5f225e50..f30e8a3b 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -218,15 +218,15 @@ class TestController(Node): self.stop() if key == '1': self.get_logger().info('attitude control') - self.control_mode = 1 + self.control_mode = 4 self.send_control_mode() if key == '2': self.get_logger().info('velocity control') - self.control_mode = 2 + self.control_mode = 16 self.send_control_mode() if key == '3': self.get_logger().info('position control') - self.control_mode = 3 + self.control_mode = 32 self.send_control_mode() if key == '/': self.get_logger().info('arming') From fa16f6e304399145c644fc875d5c0d7af14a263b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:40:45 +0200 Subject: [PATCH 33/84] add arm service instead of message --- .../test_controls/test_controller.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index f30e8a3b..f47f734f 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,7 +7,7 @@ import asyncio from drone_services.srv import SetAttitude from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl -from std_msgs.msg import Empty +from std_srvs.srv import Empty class TestController(Node): @@ -24,10 +24,14 @@ class TestController(Node): self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory') while not self.traj_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set trajectory service not available, waiting again...') + self.arm_cli = self.create_client(Empty, '/drone/arm') + while not self.arm_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('arm service not available, waiting again...') self.attitude_req = SetAttitude.Request() self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() + self.arm_req = Empty.Request() self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) @@ -37,7 +41,12 @@ class TestController(Node): while rclpy.ok(): asyncio.run(listen_keyboard_manual(on_press=self.on_press)) rclpy.spin_once(self, timeout_sec=0.1) - + def send_arm(self): + self.future = self.arm_cli.call_async(self.arm_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message on service') + return self.future.result() + def send_control_mode(self): self.vehicle_control_req.control = self.control_mode self.future = self.vehicle_control_cli.call_async(self.vehicle_control_req) @@ -218,15 +227,15 @@ class TestController(Node): self.stop() if key == '1': self.get_logger().info('attitude control') - self.control_mode = 4 + self.control_mode = 4 #bitmask of 100 self.send_control_mode() if key == '2': self.get_logger().info('velocity control') - self.control_mode = 16 + self.control_mode = 16 #bitmask of 10000 self.send_control_mode() if key == '3': self.get_logger().info('position control') - self.control_mode = 32 + self.control_mode = 32 #bitmask of 100000 self.send_control_mode() if key == '/': self.get_logger().info('arming') From 5654e440885557a36610662552087f0f5150764d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:42:02 +0200 Subject: [PATCH 34/84] huh --- src/test_controls/test_controls/test_controller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index f47f734f..a6d629ee 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -31,7 +31,7 @@ class TestController(Node): self.attitude_req = SetAttitude.Request() self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() - self.arm_req = Empty.Request() + # self.arm_req = Empty.Request() self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) @@ -41,6 +41,7 @@ class TestController(Node): while rclpy.ok(): asyncio.run(listen_keyboard_manual(on_press=self.on_press)) rclpy.spin_once(self, timeout_sec=0.1) + def send_arm(self): self.future = self.arm_cli.call_async(self.arm_req) rclpy.spin_until_future_complete(self, self.future) From eb599f764166d69432005ac1a4462924bb8ad405 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:42:54 +0200 Subject: [PATCH 35/84] test --- src/test_controls/test_controls/test_controller.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index a6d629ee..e9d3b90f 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -24,14 +24,10 @@ class TestController(Node): self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory') while not self.traj_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set trajectory service not available, waiting again...') - self.arm_cli = self.create_client(Empty, '/drone/arm') - while not self.arm_cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('arm service not available, waiting again...') self.attitude_req = SetAttitude.Request() self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() - # self.arm_req = Empty.Request() self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) @@ -41,13 +37,7 @@ class TestController(Node): while rclpy.ok(): asyncio.run(listen_keyboard_manual(on_press=self.on_press)) rclpy.spin_once(self, timeout_sec=0.1) - - def send_arm(self): - self.future = self.arm_cli.call_async(self.arm_req) - rclpy.spin_until_future_complete(self, self.future) - self.get_logger().info('publishing message on service') - return self.future.result() - + def send_control_mode(self): self.vehicle_control_req.control = self.control_mode self.future = self.vehicle_control_cli.call_async(self.vehicle_control_req) From 0c86cdcb5308f01bbe79fac982380d48be62cd13 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:43:55 +0200 Subject: [PATCH 36/84] test --- src/test_controls/test_controls/test_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index e9d3b90f..bae28a52 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,7 +7,7 @@ import asyncio from drone_services.srv import SetAttitude from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl -from std_srvs.srv import Empty +from std_msgs.msg import Empty class TestController(Node): From 4257b90da77f5e2f369cba371bfb8b59d5aeb295 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:45:08 +0200 Subject: [PATCH 37/84] remove arming --- src/test_controls/test_controls/test_controller.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index bae28a52..d416d1b9 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,7 +7,6 @@ import asyncio from drone_services.srv import SetAttitude from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl -from std_msgs.msg import Empty class TestController(Node): @@ -29,8 +28,6 @@ class TestController(Node): self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() - self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) - self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") def spin(self): @@ -230,7 +227,7 @@ class TestController(Node): self.send_control_mode() if key == '/': self.get_logger().info('arming') - self.arm_publisher.publish(Empty()) + except Exception as e: self.get_logger().error(str(e)) From f080d283f6d8baef5ae4bea11b4e5fe1f2809c85 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:49:45 +0200 Subject: [PATCH 38/84] arming? --- .../test_controls/test_controller.py | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index d416d1b9..7e7cdc7f 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,6 +7,7 @@ import asyncio from drone_services.srv import SetAttitude from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl +from std_msgs.msg import Empty class TestController(Node): @@ -23,10 +24,18 @@ class TestController(Node): self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory') while not self.traj_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set trajectory service not available, waiting again...') + self.arm_cli = self.create_client(Empty, '/drone/arm') + while not self.arm_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('arm service not available, waiting again...') + + self.control_mode = 1 self.attitude_req = SetAttitude.Request() self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() + self.arm_req = Empty.Request() + + self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") @@ -34,6 +43,12 @@ class TestController(Node): while rclpy.ok(): asyncio.run(listen_keyboard_manual(on_press=self.on_press)) rclpy.spin_once(self, timeout_sec=0.1) + + def send_arm(self): + self.future = self.arm_cli.call_async(self.arm_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message arm msg on service') + return self.future.result() def send_control_mode(self): self.vehicle_control_req.control = self.control_mode @@ -215,19 +230,19 @@ class TestController(Node): self.stop() if key == '1': self.get_logger().info('attitude control') - self.control_mode = 4 #bitmask of 100 + self.control_mode = 4 self.send_control_mode() if key == '2': self.get_logger().info('velocity control') - self.control_mode = 16 #bitmask of 10000 + self.control_mode = 16 self.send_control_mode() if key == '3': self.get_logger().info('position control') - self.control_mode = 32 #bitmask of 100000 + self.control_mode = 32 self.send_control_mode() if key == '/': self.get_logger().info('arming') - + self.send_arm() except Exception as e: self.get_logger().error(str(e)) From 71a7c30cef72d88b8ce03124d6bcc26f24eb87e9 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:52:20 +0200 Subject: [PATCH 39/84] arming?? --- src/test_controls/test_controls/test_controller.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 7e7cdc7f..0852d893 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -28,14 +28,12 @@ class TestController(Node): while not self.arm_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('arm service not available, waiting again...') + self.get_logger().info('all services available') self.control_mode = 1 self.attitude_req = SetAttitude.Request() self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() - self.arm_req = Empty.Request() - - self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") @@ -45,7 +43,8 @@ class TestController(Node): rclpy.spin_once(self, timeout_sec=0.1) def send_arm(self): - self.future = self.arm_cli.call_async(self.arm_req) + arm_req = Empty() + self.future = self.arm_cli.call_async(arm_req) rclpy.spin_until_future_complete(self, self.future) self.get_logger().info('publishing message arm msg on service') return self.future.result() From 03e9b2f20f8f8584f69f18b71bfc179517e30c78 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 09:55:18 +0200 Subject: [PATCH 40/84] add logs after connecting to services --- src/test_controls/test_controls/test_controller.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 0852d893..b2b53fca 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -17,16 +17,20 @@ class TestController(Node): self.cli = self.create_client(SetAttitude, 'drone/set_attitude') while not self.cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set attitude service not available, waiting again...') + self.get_logger().info('successfully connected to set attitude service') self.vehicle_control_cli = self.create_client( SetVehicleControl, '/drone/set_vehicle_control') while not self.vehicle_control_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set vehicle control service not available, waiting again...') + self.get_logger().info('successfully connected to set vehicle control service') self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory') while not self.traj_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set trajectory service not available, waiting again...') + self.get_logger().info('successfully connected to set trajectory service') self.arm_cli = self.create_client(Empty, '/drone/arm') while not self.arm_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('arm service not available, waiting again...') + self.get_logger().info('successfully connected to arm service') self.get_logger().info('all services available') self.control_mode = 1 From dcd707fb34e960ed2875a5edb3e64d044328d503 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:15:31 +0200 Subject: [PATCH 41/84] add arm and disarm service that return boolean for status and use this in test controller --- src/drone_services/srv/ArmDrone.srv | 2 ++ src/drone_services/srv/DisarmDrone.srv | 2 ++ src/px4_connection/src/px4_controller.cpp | 25 +++++++++++++------ .../test_controls/test_controller.py | 8 +++--- 4 files changed, 25 insertions(+), 12 deletions(-) create mode 100644 src/drone_services/srv/ArmDrone.srv create mode 100644 src/drone_services/srv/DisarmDrone.srv diff --git a/src/drone_services/srv/ArmDrone.srv b/src/drone_services/srv/ArmDrone.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/src/drone_services/srv/ArmDrone.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file diff --git a/src/drone_services/srv/DisarmDrone.srv b/src/drone_services/srv/DisarmDrone.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/src/drone_services/srv/DisarmDrone.srv @@ -0,0 +1,2 @@ +--- +bool success \ 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 61c4db95..101b8408 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include @@ -49,8 +51,8 @@ public: set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); set_trajectory_service_ = this->create_service("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_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)); - arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_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)); + arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_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)); @@ -183,8 +185,8 @@ private: */ void handle_disarm_request( 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) { RCLCPP_INFO(this->get_logger(), "Got disarm request..."); @@ -193,6 +195,12 @@ private: armed = false; flying = false; publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + response->success = true; + } + else + { + RCLCPP_ERROR(this->get_logger(), "Got disarm request but drone was not armed!"); + response->success = false; } } @@ -205,8 +213,8 @@ private: */ void handle_arm_request( 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) { RCLCPP_INFO(this->get_logger(), "Got arm request..."); @@ -220,10 +228,12 @@ private: RCLCPP_INFO(this->get_logger(), "Arm command sent"); this->last_thrust = -0.1; // spin motors at 10% thrust armed = true; + response->success = true; } else { RCLCPP_ERROR(this->get_logger(), "Arm request received but the drone is already armed!"); + response->success = false; } } @@ -307,7 +317,7 @@ private: send_attitude_setpoint(); */ - + RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!flying) { @@ -332,7 +342,6 @@ private: send_position_setpoint(); } } - } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index b2b53fca..484026ff 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,7 +7,7 @@ import asyncio from drone_services.srv import SetAttitude from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl -from std_msgs.msg import Empty +from drone_services.srv import ArmDrone class TestController(Node): @@ -27,7 +27,7 @@ class TestController(Node): while not self.traj_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set trajectory service not available, waiting again...') self.get_logger().info('successfully connected to set trajectory service') - self.arm_cli = self.create_client(Empty, '/drone/arm') + self.arm_cli = self.create_client(ArmDrone, '/drone/arm') while not self.arm_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('arm service not available, waiting again...') self.get_logger().info('successfully connected to arm service') @@ -38,6 +38,7 @@ class TestController(Node): self.attitude_req = SetAttitude.Request() self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() + self.arm_req = ArmDrone.Request() self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") @@ -47,8 +48,7 @@ class TestController(Node): rclpy.spin_once(self, timeout_sec=0.1) def send_arm(self): - arm_req = Empty() - self.future = self.arm_cli.call_async(arm_req) + self.future = self.arm_cli.call_async(self.arm_req) rclpy.spin_until_future_complete(self, self.future) self.get_logger().info('publishing message arm msg on service') return self.future.result() From 1575afc49cccd299d66671e190d6c718ddd78a4c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:16:23 +0200 Subject: [PATCH 42/84] add new services to cmakelists.txt --- src/drone_services/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index f840cd5e..ef491f87 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -26,6 +26,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetVelocity.srv" "srv/TakePicture.srv" "srv/SetVehicleControl.srv" + "srv/ArmDrone.srv" + "srv/DisarmDrone.srv" "msg/DroneControlMode.msg" ) From e240058f4485d98a5f4437b6d13d10843f351566 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:29:17 +0200 Subject: [PATCH 43/84] change types of service --- 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 101b8408..8d1bc093 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -69,8 +69,8 @@ private: rclcpp::Service::SharedPtr set_attitude_service_; rclcpp::Service::SharedPtr set_trajectory_service_; - rclcpp::Service::SharedPtr disarm_service_; - rclcpp::Service::SharedPtr arm_service_; + rclcpp::Service::SharedPtr disarm_service_; + rclcpp::Service::SharedPtr arm_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; From 33872ab99922cdaa96ffe43e2dfe35fae8351b6c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:30:00 +0200 Subject: [PATCH 44/84] 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 8d1bc093..422639f1 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -69,7 +69,7 @@ private: rclcpp::Service::SharedPtr set_attitude_service_; rclcpp::Service::SharedPtr set_trajectory_service_; - rclcpp::Service::SharedPtr disarm_service_; + rclcpp::Service::SharedPtr disarm_service_; rclcpp::Service::SharedPtr arm_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; From d1c8ca537f820d926cab2ceb91596f9f4eaf8b70 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:33:27 +0200 Subject: [PATCH 45/84] change flying bool to user_in_control for clarity --- 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 422639f1..f87222d6 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -77,7 +77,7 @@ private: rclcpp::TimerBase::SharedPtr timer_; double start_time_; bool has_sent_status = false; - bool flying = false; // if user has taken over control + bool user_in_control = false; // if user has taken over control bool armed = false; bool has_swithed = false; int setpoint_count = 0; @@ -193,7 +193,7 @@ private: if (armed) { armed = false; - flying = false; + user_in_control = false; publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); response->success = true; } @@ -318,8 +318,8 @@ private: send_attitude_setpoint(); */ - RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); - if (!flying) + // RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); + if (!user_in_control) { RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint"); send_attitude_setpoint(); @@ -379,7 +379,7 @@ private: { current_control_mode = msg->control_mode; RCLCPP_INFO(this->get_logger(), "Got valid control mode"); - flying = true; // user has taken over control + user_in_control = true; // user has taken over control } else { From 3ee881772c0442eee38eeec4554936967287efb0 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:37:47 +0200 Subject: [PATCH 46/84] edit commands for test controller and remove spamming logs --- src/px4_connection/src/px4_controller.cpp | 8 ++++---- src/test_controls/test_controls/test_controller.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index f87222d6..faddcd73 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -321,24 +321,24 @@ private: // RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!user_in_control) { - RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint"); + // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint"); send_attitude_setpoint(); } else { if (current_control_mode == CONTROL_MODE_ATTITUDE) { - RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); + // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); send_attitude_setpoint(); } else if (current_control_mode == CONTROL_MODE_VELOCITY) { - RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); + // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); send_velocity_setpoint(); } else if (current_control_mode == CONTROL_MODE_POSITION) { - RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); + // RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); send_position_setpoint(); } } diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 484026ff..17f94340 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -40,7 +40,7 @@ class TestController(Node): self.traj_req = SetTrajectory.Request() self.arm_req = ArmDrone.Request() - self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") + self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nCTRL-C - exit") def spin(self): while rclpy.ok(): From 3d74ee2c41341015bf8aedf98e082b7dd68e6d2e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:45:20 +0200 Subject: [PATCH 47/84] add proper checking of control modes --- src/px4_connection/src/px4_controller.cpp | 2 +- .../test_controls/test_controller.py | 59 ++++++++++--------- 2 files changed, 32 insertions(+), 29 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index faddcd73..686d331f 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -151,7 +151,7 @@ private: { if (request->control_mode != CONTROL_MODE_VELOCITY || request->control_mode != CONTROL_MODE_POSITION) { - RCLCPP_INFO(this->get_logger(), "Got invalid control mode: %d", request->control_mode); + RCLCPP_INFO(this->get_logger(), "Got invalid trajectory control mode: %d", request->control_mode); response->success = false; } else diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 17f94340..49d64e21 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -9,6 +9,9 @@ from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl from drone_services.srv import ArmDrone +CONTROL_MODE_ATTITUDE = 4 +CONTROL_MODE_VELOCITY = 16 +CONTROL_MODE_POSITION = 32 class TestController(Node): @@ -68,7 +71,7 @@ class TestController(Node): self.get_logger().info('set request to %f %f %f %f' % (yaw, pitch, roll, thrust)) self.future = self.cli.call_async(self.attitude_req) rclpy.spin_until_future_complete(self, self.future) - self.get_logger().info('publishing message on service') + self.get_logger().info('publishing attitude message on service') return self.future.result() def send_velocity_request(self, x, y, z, angle): @@ -78,7 +81,7 @@ class TestController(Node): self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) self.future = self.traj_cli.call_async(self.traj_req) rclpy.spin_until_future_complete(self, self.future) - self.get_logger().info('publishing message on service') + self.get_logger().info('publishing velocity message on service') return self.future.result() def send_position_request(self, x, y, z, angle): @@ -88,7 +91,7 @@ class TestController(Node): self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) self.future = self.traj_cli.call_async(self.traj_req) rclpy.spin_until_future_complete(self, self.future) - self.get_logger().info('publishing message on service') + self.get_logger().info('publishing position message on service') return self.future.result() def on_release(self, key): @@ -97,100 +100,100 @@ class TestController(Node): def move_up(self): pass - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=0.0, z=1.0, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) def move_right(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=1.0, thrust=0.0) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=1.0, y=0.0, z=0.0, angle=0.0) else: self.send_position_request(x=1.0, y=0.0, z=0.0, angle=0.0) def move_down(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=-0.05) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=0.0, z=-1.0, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) def move_left(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=-1.0, thrust=0.0) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=-1.0, y=0.0, z=0.0, angle=0.0) else: self.send_position_request(x=-1.0, y=0.0, z=0.0, angle=0.0) def rotate_right(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=1.0, roll=0.0, thrust=0.0) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=1.0) else: self.send_position_request(x=0.0, y=0.0, z=0.0, angle=1.0) def rotate_left(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=-1.0, roll=0.0, thrust=0.0) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=-1.0) else: self.send_position_request(x=0.0, y=0.0, z=0.0, angle=-1.0) def move_forward(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=-1.0, yaw=0.0, roll=0.0, thrust=0.0) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=1.0, z=0.0, angle=0.0) else: self.send_position_request(x=0.0, y=1.0, z=0.0, angle=0.0) def move_backward(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=1.0, yaw=0.0, roll=0.0, thrust=0.0) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=-1.0, z=0.0, angle=0.0) else: self.send_position_request(x=0.0, y=-1.0, z=0.0, angle=0.0) def stop(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.0) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=0.0, angle=0.0) def move_up_little(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.01) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=0.0, z=0.5, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) def move_down_little(self): - if (self.control_mode == 1): + if (self.control_mode == CONTROL_MODE_ATTITUDE): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=-0.01) - elif (self.control_mode == 2): + elif (self.control_mode == CONTROL_MODE_VELOCITY): self.send_velocity_request(x=0.0, y=0.0, z=-0.5, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) @@ -233,15 +236,15 @@ class TestController(Node): self.stop() if key == '1': self.get_logger().info('attitude control') - self.control_mode = 4 + self.control_mode = CONTROL_MODE_ATTITUDE self.send_control_mode() if key == '2': self.get_logger().info('velocity control') - self.control_mode = 16 + self.control_mode = CONTROL_MODE_VELOCITY self.send_control_mode() if key == '3': self.get_logger().info('position control') - self.control_mode = 32 + self.control_mode = CONTROL_MODE_POSITION self.send_control_mode() if key == '/': self.get_logger().info('arming') From 8cb032b8d7d272c4b54df27b9435f8d1cd52ec84 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:49:15 +0200 Subject: [PATCH 48/84] fix trajectory setpoint control mode checking --- 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 686d331f..917daf92 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -149,7 +149,7 @@ private: const std::shared_ptr request, const std::shared_ptr response) { - if (request->control_mode != CONTROL_MODE_VELOCITY || request->control_mode != CONTROL_MODE_POSITION) + if (!(request->control_mode == CONTROL_MODE_VELOCITY || request->control_mode == CONTROL_MODE_POSITION)) { RCLCPP_INFO(this->get_logger(), "Got invalid trajectory control mode: %d", request->control_mode); response->success = false; From 25a9aa350947af2a0d94d72b5d67b7b6f90af66b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:51:56 +0200 Subject: [PATCH 49/84] add logging of receiving correct trajectory setpoint message --- src/px4_connection/src/px4_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 917daf92..b01f7803 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -156,12 +156,14 @@ private: } else { + RCLCPP_INFO(this->get_logger(), "Got new trajectory setpoint with control mode: %d", request->control_mode); if (request->control_mode == CONTROL_MODE_VELOCITY) { for (int i = 0; i < 3; i++) { velocity[i] = request->values[i]; } + RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint: %f %f %f", velocity[0], velocity[1], velocity[2]); } else if (request->control_mode == CONTROL_MODE_POSITION) { @@ -169,6 +171,7 @@ private: { position[i] = request->values[i]; } + RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); } last_angle = request->yaw; From 61ee7516420c86ef85c1200a113bafe204da5958 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:54:20 +0200 Subject: [PATCH 50/84] better logging --- src/px4_connection/src/px4_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index b01f7803..5ecb4813 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -161,9 +161,9 @@ private: { for (int i = 0; i < 3; i++) { - velocity[i] = request->values[i]; + velocity[i] += request->values[i]; } - RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint: %f %f %f", velocity[0], velocity[1], velocity[2]); + RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]); } else if (request->control_mode == CONTROL_MODE_POSITION) { @@ -175,6 +175,7 @@ private: } last_angle = request->yaw; + RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle); response->success = true; } } From d50758cb73d938a587d7c2f8e893e30983293a1e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 11:01:25 +0200 Subject: [PATCH 51/84] only send trajectory setpoint once --- src/px4_connection/src/px4_controller.cpp | 26 +++++++++++++++-------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 5ecb4813..6ba66214 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -130,7 +130,6 @@ 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->success = true; } @@ -175,6 +174,7 @@ private: } last_angle = request->yaw; + new_setpoint = true; RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle); response->success = true; } @@ -335,15 +335,23 @@ private: // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); send_attitude_setpoint(); } - else if (current_control_mode == CONTROL_MODE_VELOCITY) + else { - // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); - send_velocity_setpoint(); - } - else if (current_control_mode == CONTROL_MODE_POSITION) - { - // RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); - send_position_setpoint(); + if (!new_setpoint) + { + return; + } + if (current_control_mode == CONTROL_MODE_VELOCITY) + { + // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); + send_velocity_setpoint(); + } + else if (current_control_mode == CONTROL_MODE_POSITION) + { + // RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); + send_position_setpoint(); + } + new_setpoint = false; } } } From 10ae3f3e3b4bb4778a5e4c4114f47b2322e31768 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 15:59:16 +0200 Subject: [PATCH 52/84] set position values to nan when sending velocity setpoint --- src/px4_connection/src/px4_controller.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6ba66214..9e471c5b 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -274,6 +274,10 @@ private: msg.velocity[0] = velocity[0]; msg.velocity[1] = velocity[1]; msg.velocity[2] = D_SPEED(velocity[2]); + for (int i = 0; i < 3; i++) + { + msg.position[i] = NAN; + } publish_trajectory_setpoint(msg); } From 9bca57a915cd5c93ff9f408277b54b32a61a0f76 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 16:03:40 +0200 Subject: [PATCH 53/84] do not use D_SPEED for z velocity --- 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 9e471c5b..e5082311 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -273,7 +273,7 @@ private: msg.velocity[0] = velocity[0]; msg.velocity[1] = velocity[1]; - msg.velocity[2] = D_SPEED(velocity[2]); + msg.velocity[2] = velocity[2]; for (int i = 0; i < 3; i++) { msg.position[i] = NAN; From 06eb59901ee6d8d10fb8f56943d00f18fad0ad0e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 16:07:43 +0200 Subject: [PATCH 54/84] change z velocity to negative --- 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 e5082311..fbcdb0de 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -273,7 +273,7 @@ private: msg.velocity[0] = velocity[0]; msg.velocity[1] = velocity[1]; - msg.velocity[2] = velocity[2]; + msg.velocity[2] = -velocity[2]; for (int i = 0; i < 3; i++) { msg.position[i] = NAN; From b2254dec5752b381adf5f7414b151b1a7975dc19 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 16:57:25 +0200 Subject: [PATCH 55/84] try position with polar coordinates --- src/px4_connection/src/px4_controller.cpp | 76 ++++++++++++++++------- 1 file changed, 55 insertions(+), 21 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fbcdb0de..44d4cc22 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -47,6 +47,7 @@ public: // 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_local_position_subscription_ = this->create_subscription("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1)); control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1)); set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -65,6 +66,7 @@ private: rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; rclcpp::Subscription::SharedPtr vehicle_attitude_subscription_; + rclcpp::Subscription::SharedPtr vehicle_local_position_subscription_; rclcpp::Subscription::SharedPtr control_mode_subscription_; rclcpp::Service::SharedPtr set_attitude_service_; @@ -98,6 +100,24 @@ private: char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control + bool start_trajectory = false; + const float omega = 0.3; // angular speed of the POLAR trajectory + const float K = 2; // [m] gain that regulates the spiral pitch + + const float rho_0 = 2; + const float theta_0 = 0; + const float p0_z = -5.0; + float p0_x = rho_0 * cos(theta_0); + float p0_y = rho_0 * sin(theta_0); + float des_x = p0_x, des_y = p0_y, des_z = p0_z; + float dot_des_x = 0.0, dot_des_y = 0.0; + float gamma = M_PI_4; + + float X; + float Y; + + uint32_t discrete_time_index = 0; + // result quaternion std::array q = {0, 0, 0, 0}; @@ -286,12 +306,12 @@ private: { auto msg = px4_msgs::msg::TrajectorySetpoint(); - for (int i = 0; i < 3; i++) - { - msg.position[i] = position[i]; - } + msg.position = {des_x, des_y, des_z}; + msg.velocity = {dot_des_x, dot_des_y, 0.0}; + msg.yaw = gamma; //-3.14; // [-PI:PI] - publish_trajectory_setpoint(msg); + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + trajectory_setpoint_publisher->publish(msg); } void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg) @@ -308,25 +328,23 @@ private: */ void send_setpoint() { + // the spiral, in polar coordinates (theta, rho), is given by + // theta = theta_0 + omega*t + // rho = rho_0 + K*theta + float theta = theta_0 + omega * 0.1 * discrete_time_index; + float rho = rho_0 + K * theta; - /* - setpoint_count++; - if (setpoint_count == 20) - { - 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); + // from polar to cartesian coordinates + des_x = rho * cos(theta); + des_y = rho * sin(theta); - RCLCPP_INFO(this->get_logger(), "Arm command sent"); - this->last_thrust = -0.1; // spin motors at 10% thrust - armed = true; - } + // velocity computation + float dot_rho = K * omega; + dot_des_x = dot_rho * cos(theta) - rho * sin(theta) * omega; + dot_des_y = dot_rho * sin(theta) + rho * cos(theta) * omega; + // desired heading direction + gamma = atan2(dot_des_y, dot_des_x); - send_attitude_setpoint(); - */ - - // RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!user_in_control) { // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint"); @@ -358,6 +376,10 @@ private: new_setpoint = false; } } + if (start_trajectory) + { + discrete_time_index++; + } } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) @@ -389,6 +411,18 @@ private: } } + void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) + { + X = msg->x; + Y = msg->y; + float Z = msg->z; + if (!start_trj && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z)) + { + start_trj = true; + RCLCPP_INFO(this->get_logger(), "start trajectory"); + } + } + void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg) { if (msg->control_mode >= CONTROL_MODE_MIN && msg->control_mode <= CONTROL_MODE_MAX) From f18b00867de20eec7d973f313ae50607d878ad56 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 17:00:42 +0200 Subject: [PATCH 56/84] add vehicle local position import --- 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 44d4cc22..a1562ecc 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include From 1c0e6ade6dc24817a0a0fd4f6e2fda00fdc16e12 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 17:01:40 +0200 Subject: [PATCH 57/84] 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 a1562ecc..d69f30c7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -417,9 +417,9 @@ private: X = msg->x; Y = msg->y; float Z = msg->z; - if (!start_trj && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z)) + if (!start_trajectory && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z)) { - start_trj = true; + start_trajectory = true; RCLCPP_INFO(this->get_logger(), "start trajectory"); } } From e7e06cf6d77ae83ea3fc4c6b3320f2e853b63fba Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 17:15:58 +0200 Subject: [PATCH 58/84] add z in calculation --- 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 d69f30c7..f2aa3aaa 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -189,7 +189,7 @@ private: { for (int i = 0; i < 3; i++) { - position[i] = request->values[i]; + position[i] += request->values[i]; } RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); } @@ -338,6 +338,7 @@ private: // from polar to cartesian coordinates des_x = rho * cos(theta); des_y = rho * sin(theta); + des_z = position[3]; // the z position can be set to the received height // velocity computation float dot_rho = K * omega; From a38ca838fd67a6db6dd4dcf234587958924ad9e3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 17:19:37 +0200 Subject: [PATCH 59/84] negative height --- 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 f2aa3aaa..88798dcf 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -187,10 +187,10 @@ private: } else if (request->control_mode == CONTROL_MODE_POSITION) { - for (int i = 0; i < 3; i++) - { - position[i] += request->values[i]; - } + position[0] += position[0]; + position[1] += position[1]; + position[2] -= position[2]; // height is negative + RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); } From 6ae6de7b9847abb4b5dd43ab166aac9f1fac4e05 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 17:23:01 +0200 Subject: [PATCH 60/84] typo --- 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 88798dcf..2a7819eb 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -187,10 +187,10 @@ private: } else if (request->control_mode == CONTROL_MODE_POSITION) { - position[0] += position[0]; - position[1] += position[1]; - position[2] -= position[2]; // height is negative - + position[0] += request->values[0]; + position[1] += request->values[1]; + position[2] -= request->values[2]; // height is negative + RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); } From 9c7c90653df35930cefe61a568b027902935df43 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 17:26:58 +0200 Subject: [PATCH 61/84] 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 2a7819eb..5253fb1a 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -338,7 +338,7 @@ private: // from polar to cartesian coordinates des_x = rho * cos(theta); des_y = rho * sin(theta); - des_z = position[3]; // the z position can be set to the received height + des_z = position[2]; // the z position can be set to the received height // velocity computation float dot_rho = K * omega; From c95191af5fffb3b23d63fa90f9ffd98d52f55d99 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 17:30:18 +0200 Subject: [PATCH 62/84] change starting polar coordinates --- 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 5253fb1a..4e4cb08d 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -105,9 +105,9 @@ private: const float omega = 0.3; // angular speed of the POLAR trajectory const float K = 2; // [m] gain that regulates the spiral pitch - const float rho_0 = 2; + const float rho_0 = 0; const float theta_0 = 0; - const float p0_z = -5.0; + const float p0_z = -0.0; float p0_x = rho_0 * cos(theta_0); float p0_y = rho_0 * sin(theta_0); float des_x = p0_x, des_y = p0_y, des_z = p0_z; From 3412fe75c1e048768cf7875a5e71464c0055109b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 18:22:24 +0200 Subject: [PATCH 63/84] comment --- src/px4_connection/src/px4_controller.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4e4cb08d..0b1bfdd1 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -415,6 +415,13 @@ private: void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) { + // set start values to current position + if (!user_in_control) + { + // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf + rho_0 = pow(msg->x,2) + pow(msg->y,2); + theta_0 = atan2(msg->y, msg->x); + } X = msg->x; Y = msg->y; float Z = msg->z; From af85c1fc20aa4b74cd64884954329804d3e4f749 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 18:26:15 +0200 Subject: [PATCH 64/84] make starting polar coordinates not const --- 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 0b1bfdd1..91686c43 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -105,8 +105,8 @@ private: const float omega = 0.3; // angular speed of the POLAR trajectory const float K = 2; // [m] gain that regulates the spiral pitch - const float rho_0 = 0; - const float theta_0 = 0; + float rho_0 = 0; + float theta_0 = 0; const float p0_z = -0.0; float p0_x = rho_0 * cos(theta_0); float p0_y = rho_0 * sin(theta_0); From 4a5a8eb25e2eb675cb16c022173a8f608d3878c5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 18:48:09 +0200 Subject: [PATCH 65/84] add log of des_x,y,z --- 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 91686c43..5fc51d9e 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -307,6 +307,7 @@ private: { auto msg = px4_msgs::msg::TrajectorySetpoint(); + RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z); msg.position = {des_x, des_y, des_z}; msg.velocity = {dot_des_x, dot_des_y, 0.0}; msg.yaw = gamma; //-3.14; // [-PI:PI] From 4fa2387c85d7e3a37a909ba8bc16499d87680c93 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 19:07:39 +0200 Subject: [PATCH 66/84] assign angle to starting angle while not in user control --- 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 5fc51d9e..844f2bfb 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -194,7 +194,7 @@ private: RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); } - last_angle = request->yaw; + last_angle += request->yaw; new_setpoint = true; RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle); response->success = true; @@ -422,6 +422,7 @@ private: // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf rho_0 = pow(msg->x,2) + pow(msg->y,2); theta_0 = atan2(msg->y, msg->x); + last_angle = msg->heading; } X = msg->x; Y = msg->y; From 0935a1b328a06733aea580fc7ead9b392cfc805c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 19:10:50 +0200 Subject: [PATCH 67/84] set gain 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 844f2bfb..6deb1fed 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -103,7 +103,7 @@ private: bool start_trajectory = false; const float omega = 0.3; // angular speed of the POLAR trajectory - const float K = 2; // [m] gain that regulates the spiral pitch + const float K = 0; // [m] gain that regulates the spiral pitch float rho_0 = 0; float theta_0 = 0; From 40ea450b78ae5a116716ac93ea9408186bf3d36d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 16 May 2023 13:32:11 +0200 Subject: [PATCH 68/84] decrease values for moving up and down --- src/test_controls/test_controls/test_controller.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 49d64e21..f5c939fc 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -104,9 +104,9 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=1.0, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=0.5, angle=0.0) else: - self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) + self.send_position_request(x=0.0, y=0.0, z=0.5, angle=0.0) def move_right(self): if (self.control_mode == CONTROL_MODE_ATTITUDE): @@ -122,9 +122,9 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=-0.05) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=-0.5, angle=0.0) else: - self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + self.send_position_request(x=0.0, y=0.0, z=-0.5, angle=0.0) def move_left(self): if (self.control_mode == CONTROL_MODE_ATTITUDE): @@ -185,7 +185,7 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.01) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=0.5, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=0.2, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) @@ -194,7 +194,7 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=-0.01) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=-0.5, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=-0.2, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) From 9b915ee698a9ac4d11f824c935a546dc31aa2675 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 16 May 2023 15:48:58 +0200 Subject: [PATCH 69/84] add saving of local position --- src/px4_connection/src/px4_controller.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6deb1fed..1292b07a 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -117,6 +117,9 @@ private: float X; float Y; + float local_x = 0; + float local_y = 0; + uint32_t discrete_time_index = 0; // result quaternion @@ -308,7 +311,8 @@ private: auto msg = px4_msgs::msg::TrajectorySetpoint(); RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z); - msg.position = {des_x, des_y, des_z}; + RCLCPP_INFO(this->get_logger("local position: %f %f", local_x, local_y)); + msg.position = {local_x, local_y, des_z}; msg.velocity = {dot_des_x, dot_des_y, 0.0}; msg.yaw = gamma; //-3.14; // [-PI:PI] @@ -423,6 +427,9 @@ private: rho_0 = pow(msg->x,2) + pow(msg->y,2); theta_0 = atan2(msg->y, msg->x); last_angle = msg->heading; + + local_x = msg->x; + local_y = msg->y; } X = msg->x; Y = msg->y; From 6467007795e2cfdb8417a13484923644c8cd14ce Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 16 May 2023 15:50:57 +0200 Subject: [PATCH 70/84] move up or down slower --- src/test_controls/test_controls/test_controller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index f5c939fc..47f814aa 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -104,7 +104,7 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=0.5, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=0.1, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=0.5, angle=0.0) @@ -122,7 +122,7 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=-0.05) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=-0.5, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=-0.1, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=-0.5, angle=0.0) @@ -185,7 +185,7 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.01) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=0.2, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=0.05, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) @@ -194,7 +194,7 @@ class TestController(Node): self.send_attitude_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=-0.01) elif (self.control_mode == CONTROL_MODE_VELOCITY): - self.send_velocity_request(x=0.0, y=0.0, z=-0.2, angle=0.0) + self.send_velocity_request(x=0.0, y=0.0, z=-0.05, angle=0.0) else: self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) From 4062d145f9671dc38eebf85db15ad18433f9f5b6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 16 May 2023 15:53:42 +0200 Subject: [PATCH 71/84] 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 1292b07a..8efacfd7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -311,7 +311,7 @@ private: auto msg = px4_msgs::msg::TrajectorySetpoint(); RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z); - RCLCPP_INFO(this->get_logger("local position: %f %f", local_x, local_y)); + RCLCPP_INFO(this->get_logger(),"local position: %f %f", local_x, local_y); msg.position = {local_x, local_y, des_z}; msg.velocity = {dot_des_x, dot_des_y, 0.0}; msg.yaw = gamma; //-3.14; // [-PI:PI] From b3a9b4bf90d4602225636ff1cede5290a54d14ca Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Tue, 16 May 2023 16:00:58 +0200 Subject: [PATCH 72/84] remove attitude spamming --- 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 8efacfd7..4dcc76e3 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -414,7 +414,7 @@ private: 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]); + // RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]); } } From fdc15dbe3744a45ecc2b421ff3f9a72a3c726093 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:05:52 +0200 Subject: [PATCH 73/84] create relais control package --- src/px4_msgs | 2 +- src/relais_control/package.xml | 18 +++++++++++++ src/relais_control/relais_control/__init__.py | 0 src/relais_control/resource/relais_control | 0 src/relais_control/setup.cfg | 4 +++ src/relais_control/setup.py | 25 +++++++++++++++++++ src/relais_control/test/test_copyright.py | 23 +++++++++++++++++ src/relais_control/test/test_flake8.py | 25 +++++++++++++++++++ src/relais_control/test/test_pep257.py | 23 +++++++++++++++++ 9 files changed, 119 insertions(+), 1 deletion(-) create mode 100644 src/relais_control/package.xml create mode 100644 src/relais_control/relais_control/__init__.py create mode 100644 src/relais_control/resource/relais_control create mode 100644 src/relais_control/setup.cfg create mode 100644 src/relais_control/setup.py create mode 100644 src/relais_control/test/test_copyright.py create mode 100644 src/relais_control/test/test_flake8.py create mode 100644 src/relais_control/test/test_pep257.py 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 diff --git a/src/relais_control/package.xml b/src/relais_control/package.xml new file mode 100644 index 00000000..648a5861 --- /dev/null +++ b/src/relais_control/package.xml @@ -0,0 +1,18 @@ + + + + relais_control + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/relais_control/relais_control/__init__.py b/src/relais_control/relais_control/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/relais_control/resource/relais_control b/src/relais_control/resource/relais_control new file mode 100644 index 00000000..e69de29b diff --git a/src/relais_control/setup.cfg b/src/relais_control/setup.cfg new file mode 100644 index 00000000..4d7436a8 --- /dev/null +++ b/src/relais_control/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/relais_control +[install] +install-scripts=$base/lib/relais_control diff --git a/src/relais_control/setup.py b/src/relais_control/setup.py new file mode 100644 index 00000000..858cac94 --- /dev/null +++ b/src/relais_control/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'relais_control' + +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/relais_control/test/test_copyright.py b/src/relais_control/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/src/relais_control/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/relais_control/test/test_flake8.py b/src/relais_control/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/relais_control/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/relais_control/test/test_pep257.py b/src/relais_control/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/relais_control/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' From 7935787cecad3a8fedc7245ed04bbedec0593e8f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:18:44 +0200 Subject: [PATCH 74/84] create relais control node --- src/px4_msgs | 2 +- src/relais_control/package.xml | 7 +-- .../relais_control/relais_controller.py | 48 +++++++++++++++++++ src/relais_control/setup.py | 1 + 4 files changed, 54 insertions(+), 4 deletions(-) create mode 100644 src/relais_control/relais_control/relais_controller.py 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 diff --git a/src/relais_control/package.xml b/src/relais_control/package.xml index 648a5861..88e8691e 100644 --- a/src/relais_control/package.xml +++ b/src/relais_control/package.xml @@ -3,9 +3,10 @@ relais_control 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration + package to control the relais that enables Pixhawk RX and TX communication + ubuntu + Apache License 2.0 + rclpy ament_copyright ament_flake8 diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py new file mode 100644 index 00000000..7b574746 --- /dev/null +++ b/src/relais_control/relais_control/relais_controller.py @@ -0,0 +1,48 @@ +import rclpy +from rclpy.node import Node +try: + import RPi.GPIO as GPIO +except RuntimeError: + print("Error importing RPi.GPIO! This is probably because you need superuser privileges. You can achieve this by using 'sudo' to run your script") + +class RelaisController(Node): + def __init(self): + super().__init__('relais_controller') + self.relais1_pin = 17 + self.relais2_pin = 27 + self.init_gpio() + self.turn_relais_on() + + def init_gpio(self): + GPIO.setwarnings(False) + + self.get_logger().info(GPIO.RPI_INFO) + + GPIO.setmode(GPIO.BCM) + + GPIO.setup(self.relais1_pin, GPIO.OUT) + GPIO.setup(self.relais2_pin, GPIO.OUT) + self.get_logger().info("GPIO initialized") + + def turn_relais_on(self): + GPIO.output(self.relais1_pin, GPIO.HIGH) + GPIO.output(self.relais2_pin, GPIO.HIGH) + self.get_logger().info("Relais turned on") + + +def main(args=None): + rclpy.init(args=args) + + relais_controller = RelaisController() + + relais_controller.spin() + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + relais_controller.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/relais_control/setup.py b/src/relais_control/setup.py index 858cac94..40560f60 100644 --- a/src/relais_control/setup.py +++ b/src/relais_control/setup.py @@ -20,6 +20,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'relais_controller = relais_control.relais_controller:main' ], }, ) From 443e1b876594f804f178f5f2ed75d42e1a6566d3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:20:43 +0200 Subject: [PATCH 75/84] typo in init function --- src/relais_control/relais_control/relais_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py index 7b574746..fba1bd27 100644 --- a/src/relais_control/relais_control/relais_controller.py +++ b/src/relais_control/relais_control/relais_controller.py @@ -6,7 +6,7 @@ except RuntimeError: print("Error importing RPi.GPIO! This is probably because you need superuser privileges. You can achieve this by using 'sudo' to run your script") class RelaisController(Node): - def __init(self): + def __init__(self): super().__init__('relais_controller') self.relais1_pin = 17 self.relais2_pin = 27 From e8f7c1610af091bf90b7dfc03d35ed771d40b5a9 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:21:35 +0200 Subject: [PATCH 76/84] log string --- src/relais_control/relais_control/relais_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py index fba1bd27..9a9cdb8a 100644 --- a/src/relais_control/relais_control/relais_controller.py +++ b/src/relais_control/relais_control/relais_controller.py @@ -16,7 +16,7 @@ class RelaisController(Node): def init_gpio(self): GPIO.setwarnings(False) - self.get_logger().info(GPIO.RPI_INFO) + self.get_logger().info(str(GPIO.RPI_INFO)) GPIO.setmode(GPIO.BCM) From e16e083dd0b89491bfb2e37280ca9c02f543abec Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:31:22 +0200 Subject: [PATCH 77/84] add service for controlling relais --- src/drone_services/CMakeLists.txt | 1 + src/drone_services/srv/ControlRelais.srv | 5 ++++ src/relais_control/package.xml | 26 ++++++++++--------- .../relais_control/relais_controller.py | 24 +++++++++++++++++ 4 files changed, 44 insertions(+), 12 deletions(-) create mode 100644 src/drone_services/srv/ControlRelais.srv diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index ef491f87..130f0f65 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -28,6 +28,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetVehicleControl.srv" "srv/ArmDrone.srv" "srv/DisarmDrone.srv" + "srv/ControlRelais.srv" "msg/DroneControlMode.msg" ) diff --git a/src/drone_services/srv/ControlRelais.srv b/src/drone_services/srv/ControlRelais.srv new file mode 100644 index 00000000..805fcd89 --- /dev/null +++ b/src/drone_services/srv/ControlRelais.srv @@ -0,0 +1,5 @@ +#to turn the relais on or off +bool relais1_on false +bool relais2_on false +--- +int8 bits # relais 1 = bit 0, relais 2 is bit 1 \ No newline at end of file diff --git a/src/relais_control/package.xml b/src/relais_control/package.xml index 88e8691e..806af7fe 100644 --- a/src/relais_control/package.xml +++ b/src/relais_control/package.xml @@ -1,19 +1,21 @@ - relais_control - 0.0.0 - package to control the relais that enables Pixhawk RX and TX communication + relais_control + 0.0.0 + package to control the relais that enables Pixhawk RX and TX communication ubuntu - Apache License 2.0 + Apache License 2.0 rclpy + drone_services - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - ament_python - - + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py index 9a9cdb8a..782a4fd3 100644 --- a/src/relais_control/relais_control/relais_controller.py +++ b/src/relais_control/relais_control/relais_controller.py @@ -1,13 +1,17 @@ import rclpy from rclpy.node import Node + try: import RPi.GPIO as GPIO except RuntimeError: print("Error importing RPi.GPIO! This is probably because you need superuser privileges. You can achieve this by using 'sudo' to run your script") +from drone_services.srv import ControlRelais class RelaisController(Node): def __init__(self): super().__init__('relais_controller') + self.srv = self.create_service(ControlRelais, 'control_relais', self.control_relais_callback) + self.relais1_pin = 17 self.relais2_pin = 27 self.init_gpio() @@ -29,6 +33,26 @@ class RelaisController(Node): GPIO.output(self.relais2_pin, GPIO.HIGH) self.get_logger().info("Relais turned on") + def control_relais_callback(self, request, response): + if request.relais1: + GPIO.output(self.relais1_pin, GPIO.HIGH) + else: + GPIO.output(self.relais1_pin, GPIO.LOW) + if request.relais2: + GPIO.output(self.relais2_pin, GPIO.HIGH) + else: + GPIO.output(self.relais2_pin, GPIO.LOW) + + if GPIO.output(17) == GPIO.LOW: + response.bits = response.bits & 0 + else: + response.bits = response.bits | 1 + + if GPIO.output(27) == GPIO.LOW: + response.bits = response.bits & ~(1 << 1) + else: + response.bits = response.bits | (1 << 1) + return response def main(args=None): rclpy.init(args=args) From 8f537fbc4807df329dbad3752768ea123d207fe4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:35:49 +0200 Subject: [PATCH 78/84] fix spin function --- src/relais_control/relais_control/relais_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py index 782a4fd3..28035cc2 100644 --- a/src/relais_control/relais_control/relais_controller.py +++ b/src/relais_control/relais_control/relais_controller.py @@ -59,7 +59,7 @@ def main(args=None): relais_controller = RelaisController() - relais_controller.spin() + rclpy.spin(relais_controller) # Destroy the node explicitly # (optional - otherwise it will be done automatically From 02b60650ba410a4b02ead4fadf02f48cb48b6253 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:37:44 +0200 Subject: [PATCH 79/84] fix naming of request parameters --- src/relais_control/relais_control/relais_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py index 28035cc2..01bfd1ad 100644 --- a/src/relais_control/relais_control/relais_controller.py +++ b/src/relais_control/relais_control/relais_controller.py @@ -34,11 +34,11 @@ class RelaisController(Node): self.get_logger().info("Relais turned on") def control_relais_callback(self, request, response): - if request.relais1: + if request.relais1_on: GPIO.output(self.relais1_pin, GPIO.HIGH) else: GPIO.output(self.relais1_pin, GPIO.LOW) - if request.relais2: + if request.relais2_on: GPIO.output(self.relais2_pin, GPIO.HIGH) else: GPIO.output(self.relais2_pin, GPIO.LOW) From 19367da170bcb688337fe1b78c6825a8fae98fa5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:38:27 +0200 Subject: [PATCH 80/84] change service name to include drone prefix --- src/relais_control/relais_control/relais_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py index 01bfd1ad..e8dd893b 100644 --- a/src/relais_control/relais_control/relais_controller.py +++ b/src/relais_control/relais_control/relais_controller.py @@ -10,7 +10,7 @@ from drone_services.srv import ControlRelais class RelaisController(Node): def __init__(self): super().__init__('relais_controller') - self.srv = self.create_service(ControlRelais, 'control_relais', self.control_relais_callback) + self.srv = self.create_service(ControlRelais, '/drone/control_relais', self.control_relais_callback) self.relais1_pin = 17 self.relais2_pin = 27 From b3b2a9fe6132553ff6a61da90a2f50b126b448d7 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 11:40:48 +0200 Subject: [PATCH 81/84] change response bits --- .../relais_control/relais_controller.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py index e8dd893b..cd7c427e 100644 --- a/src/relais_control/relais_control/relais_controller.py +++ b/src/relais_control/relais_control/relais_controller.py @@ -36,22 +36,16 @@ class RelaisController(Node): def control_relais_callback(self, request, response): if request.relais1_on: GPIO.output(self.relais1_pin, GPIO.HIGH) + response.bits = response.bits | 1 else: GPIO.output(self.relais1_pin, GPIO.LOW) + response.bits = response.bits & ~(1 << 0) if request.relais2_on: GPIO.output(self.relais2_pin, GPIO.HIGH) + response.bits = response.bits | (1 << 1) else: GPIO.output(self.relais2_pin, GPIO.LOW) - - if GPIO.output(17) == GPIO.LOW: - response.bits = response.bits & 0 - else: - response.bits = response.bits | 1 - - if GPIO.output(27) == GPIO.LOW: response.bits = response.bits & ~(1 << 1) - else: - response.bits = response.bits | (1 << 1) return response def main(args=None): From d1b2f866e0e28fb3f2f74f228d18bb3e11f471e8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 17 May 2023 14:07:50 +0200 Subject: [PATCH 82/84] create drone status package --- src/drone_status/drone_status/__init__.py | 0 src/drone_status/package.xml | 18 ++++++++++++++++ src/drone_status/resource/drone_status | 0 src/drone_status/setup.cfg | 4 ++++ src/drone_status/setup.py | 25 +++++++++++++++++++++++ src/drone_status/test/test_copyright.py | 23 +++++++++++++++++++++ src/drone_status/test/test_flake8.py | 25 +++++++++++++++++++++++ src/drone_status/test/test_pep257.py | 23 +++++++++++++++++++++ src/px4_msgs | 2 +- 9 files changed, 119 insertions(+), 1 deletion(-) create mode 100644 src/drone_status/drone_status/__init__.py create mode 100644 src/drone_status/package.xml create mode 100644 src/drone_status/resource/drone_status create mode 100644 src/drone_status/setup.cfg create mode 100644 src/drone_status/setup.py create mode 100644 src/drone_status/test/test_copyright.py create mode 100644 src/drone_status/test/test_flake8.py create mode 100644 src/drone_status/test/test_pep257.py diff --git a/src/drone_status/drone_status/__init__.py b/src/drone_status/drone_status/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/drone_status/package.xml b/src/drone_status/package.xml new file mode 100644 index 00000000..61241c9d --- /dev/null +++ b/src/drone_status/package.xml @@ -0,0 +1,18 @@ + + + + drone_status + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/drone_status/resource/drone_status b/src/drone_status/resource/drone_status new file mode 100644 index 00000000..e69de29b diff --git a/src/drone_status/setup.cfg b/src/drone_status/setup.cfg new file mode 100644 index 00000000..40ac3e8a --- /dev/null +++ b/src/drone_status/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/drone_status +[install] +install-scripts=$base/lib/drone_status diff --git a/src/drone_status/setup.py b/src/drone_status/setup.py new file mode 100644 index 00000000..8057ea5c --- /dev/null +++ b/src/drone_status/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'drone_status' + +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/drone_status/test/test_copyright.py b/src/drone_status/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/src/drone_status/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/drone_status/test/test_flake8.py b/src/drone_status/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/drone_status/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/drone_status/test/test_pep257.py b/src/drone_status/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/drone_status/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/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 8cbcf17b11170b6e4472abfdd57b60d646ae05ce Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 19 May 2023 17:25:52 +0200 Subject: [PATCH 83/84] add drone status node --- src/drone_services/msg/DroneArmStatus.msg | 1 + src/drone_services/msg/DroneRouteStatus.msg | 2 + src/drone_services/msg/DroneStatus.msg | 5 ++ src/drone_status/drone_status/drone_status.py | 63 +++++++++++++++++++ src/drone_status/package.xml | 29 +++++---- src/px4_msgs | 2 +- 6 files changed, 88 insertions(+), 14 deletions(-) create mode 100644 src/drone_services/msg/DroneArmStatus.msg create mode 100644 src/drone_services/msg/DroneRouteStatus.msg create mode 100644 src/drone_services/msg/DroneStatus.msg create mode 100644 src/drone_status/drone_status/drone_status.py diff --git a/src/drone_services/msg/DroneArmStatus.msg b/src/drone_services/msg/DroneArmStatus.msg new file mode 100644 index 00000000..117ae05f --- /dev/null +++ b/src/drone_services/msg/DroneArmStatus.msg @@ -0,0 +1 @@ +bool armed false \ No newline at end of file diff --git a/src/drone_services/msg/DroneRouteStatus.msg b/src/drone_services/msg/DroneRouteStatus.msg new file mode 100644 index 00000000..bcf2a7c0 --- /dev/null +++ b/src/drone_services/msg/DroneRouteStatus.msg @@ -0,0 +1,2 @@ +int32 current_setpoint_index +float32[5] current_setpoint # x,y,z,angle,take_picture \ No newline at end of file diff --git a/src/drone_services/msg/DroneStatus.msg b/src/drone_services/msg/DroneStatus.msg new file mode 100644 index 00000000..0489be52 --- /dev/null +++ b/src/drone_services/msg/DroneStatus.msg @@ -0,0 +1,5 @@ +float32 battery_percentage +float32 cpu_usage +int32 route_setpoint # -1 if no route +wstring control_mode +bool armed \ No newline at end of file diff --git a/src/drone_status/drone_status/drone_status.py b/src/drone_status/drone_status/drone_status.py new file mode 100644 index 00000000..ab8046af --- /dev/null +++ b/src/drone_status/drone_status/drone_status.py @@ -0,0 +1,63 @@ +import rclpy +from rclpy.node import Node + +from drone_services.msg import DroneStatus +from drone_services.msg import DroneControlMode +from drone_services.msg import DroneArmStatus +from drone_services.msg import DroneRouteStatus +from px4_msgs.msg import BatteryStatus +from px4_msgs.msg import Cpuload + +CONTROL_MODE_ATTITUDE = 1 +CONTROL_MODE_VELOCITY = 2 +CONTROL_MODE_POSITION = 3 + +class DroneStatus(Node): + def __init__(self): + super().__init__('drone_status') + #publish to drone/status topic + self.publisher = self.create_publisher(DroneStatus, '/drone/status', 10) + self.control_mode_subscriber = self.create_subscription(DroneControlMode, '/drone/control_mode', self.control_mode_callback, 10) + self.arm_status_subscriber = self.create_subscription(DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10) + self.route_status_subscriber = self.create_subscription(DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10) + self.battery_status_subscriber = self.create_subscription(BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, 10) + self.cpu_load_subscriber = self.create_subscription(Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, 10) + #publish every 0.5 seconds + self.timer = self.create_timer(0.5, self.publish_status) + self.armed = False + self.control_mode = "attitude" + self.battery_percentage = 100.0 + self.cpu_usage = 0.0 + self.route_setpoint = 0 + + def publish_status(self): + msg = DroneStatus() + msg.armed = self.armed + msg.control_mode = self.control_mode + msg.battery_percentage = self.battery_percentage + msg.cpu_usage = self.cpu_usage + msg.route_setpoint = self.route_setpoint + self.publisher.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.status) + + def control_mode_callback(self,msg): + if msg.control_mode == CONTROL_MODE_ATTITUDE: + self.control_mode = "attitude" + elif msg.control_mode == CONTROL_MODE_VELOCITY: + self.control_mode = "velocity" + elif msg.control_mode == CONTROL_MODE_POSITION: + self.control_mode = "position" + else: + self.control_mode = "unknown" + + def arm_status_callback(self,msg): + self.armed = msg.armed + + def route_status_callback(self,msg): + self.route_setpoint = msg.current_setpoint_index + + def battery_status_callback(self, msg): + self.battery_percentage = msg.remaining * 100.0 + + def cpu_load_callback(self, msg): + self.cpu_usage = msg.load \ No newline at end of file diff --git a/src/drone_status/package.xml b/src/drone_status/package.xml index 61241c9d..96b4dd15 100644 --- a/src/drone_status/package.xml +++ b/src/drone_status/package.xml @@ -1,18 +1,21 @@ - drone_status - 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration + drone_status + 0.0.0 + Package for combining several data points from the drone into a single topic + ubuntu + Apache License 2.0 + rclpy + drone_services + px4_msgs - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest - - ament_python - - + + ament_python + + \ 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 3abaee82507bb9ce83a799d67c00e0a549888a98 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 19 May 2023 17:31:18 +0200 Subject: [PATCH 84/84] make px4_controller publish armed message --- src/px4_connection/src/px4_controller.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 4dcc76e3..fbad0885 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -45,12 +46,14 @@ 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); + arm_status_publisher_ = this->create_publisher("/drone/arm_status", 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_local_position_subscription_ = this->create_subscription("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1)); control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1)); + set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); set_trajectory_service_ = this->create_service("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_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)); @@ -66,6 +69,8 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + rclcpp::Publisher::SharedPtr arm_status_publisher_; + rclcpp::Subscription::SharedPtr vehicle_attitude_subscription_; rclcpp::Subscription::SharedPtr vehicle_local_position_subscription_; rclcpp::Subscription::SharedPtr control_mode_subscription_; @@ -223,6 +228,11 @@ private: armed = false; user_in_control = false; publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + + auto msg = drone_services::msg::DroneArmStatus(); + msg.armed = false; + arm_status_publisher_->publish(msg); + response->success = true; } else @@ -256,6 +266,11 @@ private: RCLCPP_INFO(this->get_logger(), "Arm command sent"); this->last_thrust = -0.1; // spin motors at 10% thrust armed = true; + + auto msg = drone_services::msg::DroneArmStatus(); + msg.armed = true; + arm_status_publisher_->publish(msg); + response->success = true; } else