Add a new c++ node offboard example

.gitignore

@@ -1,2 +1,10 @@
 *.pyc
 .vscode/*
+
+
+px4_msgs/*
+px4_ros_com/*
+
+build/*
+install/*
+log/*

offboard_cpp/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(offboard_cpp)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(px4_msgs REQUIRED)
+find_package(px4_ros_com REQUIRED)
+
+add_executable(offboard_cpp_node src/offboard_cpp_node.cpp)
+ament_target_dependencies(offboard_cpp_node rclcpp std_msgs px4_msgs px4_ros_com)
+target_include_directories(offboard_cpp_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(offboard_cpp_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+install(TARGETS offboard_cpp_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

offboard_cpp/README.md

@@ -0,0 +1,66 @@
+# offboard_cpp
+This `package` contains a c++ example for offboard control on ROS2 with [PX4](https://px4.io/).The source code is released under a BSD 3-Clause license.
+
+
+## Instructions
+
+### Setup
+Add this `repository` to your RO2 workspace.
+ ```
+git clone https://github.com/Jaeyoung-Lim/px4-offboard.git
+```
+
+Make sure that the newest versions of [px4_msgs](https://github.com/PX4/px4_msgs) and [px4_ros_com](https://github.com/PX4/px4_ros_com) are in the same workspace.
+
+ ```
+git clone --recursive https://github.com/PX4/px4_ros_com.git
+git clone --recursive https://github.com/PX4/px4_msgs.git
+```
+
+In a different folder, outside of your ros2 workspace clone the  [PX4-Autopilot](https://github.com/PX4/PX4-Autopilot). At the time of writing this was commit [f2607335ac](https://github.com/PX4/PX4-Autopilot/commit/f2607335ac80dbd0fe151ba8fde122e91d776fc7).
+
+ ```
+git clone --recursive https://github.com/PX4/PX4-Autopilot.git
+
+```
+
+Finally you need to make sure that you have installed the `micro-ros-agent`. You can use `snap` to install it or [build it from source](https://github.com/micro-ROS/micro_ros_setup#building-micro-ros-agent).
+
+### Build
+1. Go to your ros2 workspace
+2. Source your ros2 installation. For me this was:
+```
+source /opt/ros/galactic/setup.zsh 
+```
+3. Build your workspace, which includes the 4 packages (px4_msgs, px4_ros_com and the two offboard packages from this repo..
+```
+colcon build --symlink-install
+```
+### Run
+1. Open a new terminal and go to your PX4-Autopilot folder. Run:
+```
+make px4_sitl gazebo
+```
+
+2. Open a new terminal and source your ros2 workspace. Then run the micro-ros-agent:
+```
+micro-ros-agent udp4 --port 8888
+```
+
+3. Open a new terminal and source your ros2 workspace. Then run the c++ offboard node:
+```
+ros2 run offboard_cpp offboard_cpp_node
+```
+
+4. The SITL drone should have taken-off.
+
+### Troubleshoot
+* I don't know why but sometimes the offboard node does not manage to change the `NavState` of the SITL drone to offboard (14) and it remains on hold mode (4). To change that use QGC or use your nutt-shell:
+```
+commander mode offboard
+```
+* Once the offboard mode is set the SITL drone must be armed using QGC or the nutt-shell:
+```
+commander arm
+```
+

offboard_cpp/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>offboard_cpp</name>
+  <version>0.0.0</version>
+  <description>Simple PX4 offboard example with ROS2 and C++</description>
+  <maintainer email="[email protected]">claudio</maintainer>
+  <license>BSD 3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>px4_msgs</depend>
+  <depend>px4_ros_com</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

offboard_cpp/src/offboard_cpp_node.cpp

@@ -0,0 +1,207 @@
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/clock.hpp>
+
+#include <px4_msgs/msg/offboard_control_mode.hpp>
+#include <px4_msgs/msg/trajectory_setpoint.hpp>
+#include <px4_msgs/msg/vehicle_status.hpp>
+
+#include <px4_msgs/msg/vehicle_command.hpp>
+#include <px4_msgs/msg/vehicle_control_mode.hpp>
+
+// #include <px4_msgs/msg/timesync.hpp>
+
+using namespace std::chrono;
+using namespace std::chrono_literals;
+
+using std::placeholders::_1;
+
+
+class OffboardControl : public rclcpp::Node
+{
+public:
+	explicit OffboardControl() : Node("offboard_control")
+	{
+    auto qos_sub = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort().durability_volatile();
+	auto qos_pub = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort().transient_local();
+
+
+	// Create subscribers
+	this->vehicle_status_sub_ = this->create_subscription<px4_msgs::msg::VehicleStatus>("/fmu/out/vehicle_status",
+		qos_sub, std::bind(&OffboardControl::vehicle_status_clbk, this, _1)
+	);
+
+	// // get common timestamp
+	// this->timesync_sub_ =
+	// 		this->create_subscription<px4_msgs::msg::Timesync>("fmu/timesync/out", qos,
+	// 			[this](const px4_msgs::msg::Timesync::UniquePtr msg) {
+	// 				timestamp_.store(msg->timestamp);
+	// 			});
+
+	// Create publishers
+	this->offboard_control_mode_publisher_ =
+			this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", qos_pub);
+	this->trajectory_setpoint_publisher_ =
+		this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", qos_pub);
+	this->vehicle_command_publisher_ =
+		this->create_publisher<px4_msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", qos_pub);
+
+	// Set default variables:
+	this->offboard_setpoint_counter_ = 0;
+
+	// timer callback
+	auto timer_callback = [this]() -> void {
+
+			if (this->offboard_setpoint_counter_ == 10) {
+				// Change to Offboard mode after 10 setpoints
+				this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
+
+				// Arm the vehicle
+				this->arm();
+			}
+
+            // offboard_control_mode needs to be paired with trajectory_setpoint
+			this->publish_offboard_control_mode();
+			this->publish_trajectory_setpoint();
+
+           		 // stop the counter after reaching 11
+			if (this->offboard_setpoint_counter_ < 11) {
+				this->offboard_setpoint_counter_++;
+			}
+		};
+
+	// start publisher timer
+	timer_ = this->create_wall_timer(100ms, timer_callback);
+
+	}
+
+	void arm() ;
+	void disarm() ;
+
+
+private:
+
+	// important node variables
+	rclcpp::TimerBase::SharedPtr timer_;  // Timer that makes the node spin
+	uint8_t nav_state;
+	uint8_t arming_state;
+	// std::atomic<uint64_t> timestamp_;   //!< common synced timestamped
+	uint64_t offboard_setpoint_counter_;   //!< counter for the number of setpoints sent
+
+
+
+	// Subscribers
+	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub_;
+		// rclcpp::Subscription<px4_msgs::msg::Timesync>::SharedPtr timesync_sub_;
+
+
+	// publishers
+	rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+	rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
+	rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
+
+	// callbacks
+
+	void vehicle_status_clbk(const px4_msgs::msg::VehicleStatus & msg)
+	{
+		this->arming_state = msg.arming_state;
+		this->nav_state = msg.nav_state;
+		RCLCPP_INFO(this->get_logger(), "ArmingState: %i\nNavState: %i", this->arming_state, this->nav_state);
+	}
+
+	// commands
+	void publish_offboard_control_mode();
+	void publish_trajectory_setpoint() ;
+	void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0) ;
+
+};
+
+/**
+ * @brief Publish vehicle commands
+ * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
+ * @param param1    Command parameter 1
+ * @param param2    Command parameter 2
+ */
+void OffboardControl::publish_vehicle_command(uint16_t command, float param1,
+					      float param2)  {
+	px4_msgs::msg::VehicleCommand msg{};
+	msg.timestamp = int(this->get_clock()->now().nanoseconds() / 1000);
+	msg.param1 = param1;
+	msg.param2 = param2;
+	msg.command = command;
+	msg.target_system = 1;
+	msg.target_component = 1;
+	msg.source_system = 1;
+	msg.source_component = 1;
+	msg.from_external = true;
+
+	vehicle_command_publisher_->publish(msg);
+}
+
+
+
+/**
+ * @brief Send a command to Arm the vehicle
+ */
+void OffboardControl::arm()  {
+	publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
+
+	RCLCPP_INFO(this->get_logger(), "Arm command send");
+}
+
+/**
+ * @brief Send a command to Disarm the vehicle
+ */
+void OffboardControl::disarm()  {
+	publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
+
+	RCLCPP_INFO(this->get_logger(), "Disarm command send");
+}
+
+/**
+ * @brief Publish the offboard control mode.
+ *        For this example, only position and altitude controls are active.
+ */
+void OffboardControl::publish_offboard_control_mode()  {
+	px4_msgs::msg::OffboardControlMode msg{};
+	msg.timestamp = int(this->get_clock()->now().nanoseconds() / 1000);
+	msg.position = true;
+	msg.velocity = false;
+	msg.acceleration = false;
+	msg.attitude = false;
+	msg.body_rate = false;
+
+	offboard_control_mode_publisher_->publish(msg);
+}
+
+
+/**
+ * @brief Publish a trajectory setpoint
+ *        For this example, it sends a trajectory setpoint to make the
+ *        vehicle hover at 5 meters with a yaw angle of 180 degrees.
+ */
+void OffboardControl::publish_trajectory_setpoint()  {
+	px4_msgs::msg::TrajectorySetpoint msg{};
+	msg.timestamp = int(this->get_clock()->now().nanoseconds() / 1000);
+	msg.position = {0.0, 0.0, -5.0};
+	msg.yaw = -3.14; // [-PI:PI]
+
+	trajectory_setpoint_publisher_->publish(msg);
+}
+
+/**
+ * @brief Entry point of Offboard node
+ * 
+ * @param argc 
+ * @param argv 
+ * @return int 
+ */
+int main(int argc, char ** argv)
+{
+  std::cout << "Starting offboard_control node..." << std::endl;
+	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<OffboardControl>());
+
+	rclcpp::shutdown();
+	return 0;
+}