From 8b1790f763b89976fa80af5f336dc66340c96220 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Tue, 25 Apr 2023 16:15:03 +0200
Subject: [PATCH] change CMakeLists.txt of heartbeat to include packages for
 ros

---
 src/px4_connection/CMakeLists.txt        |  6 +-
 src/px4_connection/package.xml           |  4 +-
 src/px4_connection/src/heartbeat.cpp     | 76 ++++++++++++++++++++++--
 src/px4_msgs                             |  2 +-
 src/px4_ros_com                          |  2 +-
 src/send_setpoints/src/send_setpoint.cpp |  1 -
 6 files changed, 79 insertions(+), 12 deletions(-)

diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt
index 874483cc..8153e79c 100644
--- a/src/px4_connection/CMakeLists.txt
+++ b/src/px4_connection/CMakeLists.txt
@@ -18,6 +18,8 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(px4_ros_com REQUIRED)
+find_package(px4_msgs REQUIRED)
 
 add_executable(heartbeat src/heartbeat.cpp)
 target_include_directories(heartbeat PUBLIC
@@ -25,7 +27,9 @@ target_include_directories(heartbeat PUBLIC
   $<INSTALL_INTERFACE:include>)
 ament_target_dependencies(
   heartbeat
-  "rclcpp"
+  rclcpp
+  px4_ros_com
+  px4_msgs
 )
 
 install(TARGETS heartbeat
diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml
index 4a804039..e0eaf074 100644
--- a/src/px4_connection/package.xml
+++ b/src/px4_connection/package.xml
@@ -3,9 +3,9 @@
 <package format="3">
   <name>px4_connection</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Package to communicate with PX4 through the XRCE-DDS bridge</description>
   <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp
index 5d36d006..172c5359 100644
--- a/src/px4_connection/src/heartbeat.cpp
+++ b/src/px4_connection/src/heartbeat.cpp
@@ -1,10 +1,74 @@
-#include <cstdio>
+/*
 
-int main(int argc, char ** argv)
+We need to send attitude setpoints to be able to arm the drone:
+https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET
+We need attitude setpoints because we don't have a GPS:
+https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-manual-control-lost/31565/9
+
+*/
+
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <px4_msgs/msg/offboard_control_mode.hpp>
+#include <px4_msgs/msg/timesync.hpp>
+
+using namespace std::chrono_literals;
+
+class HeartBeat : public rclcpp::Node
 {
-  (void) argc;
-  (void) argv;
+public:
+    HeartBeat() : Node("setpoint_sender")
+    {
+        // create a publisher on the offboard control mode topic
+        offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
+        // create timer to send heartbeat messages (offboard control) every 100ms
+        timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this));
+        start_time = this->get_clock()->now().seconds();
+    }
 
-  printf("hello world px4_connection package\n");
-  return 0;
+private:
+
+/**
+ * @brief Publish offboard control mode messages as a heartbeat.
+ *        Only the attitude is enabled, because that is how the drone will be controlled.
+ * 
+ */
+    void send_heartbeat()
+    {
+        // set message to enable attitude
+        auto msg = px4_msgs::msg::OffboardControlMode();
+        msg.position = false;
+        msg.velocity = false;
+        msg.acceleration = false;
+        msg.attitude = true;
+        msg.body_rate = false;
+        msg.actuator = false;
+
+        // get timestamp and publish message
+        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
+        offboard_control_mode_publisher_->publish(msg);
+        RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
+
+        // check if 5 seconds have elapsed
+        if (this->get_clock()->now().seconds() - start_time > 5)
+        {
+            RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!");
+        }
+
+
+        
+    }
+    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    double start_time;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<HeartBeat>());
+    rclcpp::shutdown();
+    return 0;
 }
diff --git a/src/px4_msgs b/src/px4_msgs
index 4db0a3f1..b64ef047 160000
--- a/src/px4_msgs
+++ b/src/px4_msgs
@@ -1 +1 @@
-Subproject commit 4db0a3f14ea81b9de7511d738f8ad9bd8ae5b3ad
+Subproject commit b64ef0475c1d44605688f4770899fe453d532be4
diff --git a/src/px4_ros_com b/src/px4_ros_com
index 0bcf68bc..1562ff30 160000
--- a/src/px4_ros_com
+++ b/src/px4_ros_com
@@ -1 +1 @@
-Subproject commit 0bcf68bcb635199adcd134e8932932054e863c0d
+Subproject commit 1562ff30d56b7ba26e4d2436724490f900cc2375
diff --git a/src/send_setpoints/src/send_setpoint.cpp b/src/send_setpoints/src/send_setpoint.cpp
index 93ecff47..c1a10371 100644
--- a/src/send_setpoints/src/send_setpoint.cpp
+++ b/src/send_setpoints/src/send_setpoint.cpp
@@ -12,7 +12,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
 #include "rclcpp/rclcpp.hpp"
 
 #include <px4_msgs/msg/offboard_control_mode.hpp>
-#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
 #include <px4_msgs/msg/timesync.hpp>
 
 using namespace std::chrono_literals;