Update Pyx4

- Add GPS sample
- Add README.md
- Doc for conver QoS C++ to Python

README.md

@@ -1,2 +1,109 @@
-# pyx4
-Python implementation of PX4
+<img src="./docs/imgs/pyx4.png" width="100%">
+
+# Pyx4
+
+Python ROS2 interface with PX4 through a Fast-RTPS bridge.
+
+## Requirements
+
+You need to install the required packages for PX4, ROS, and XRCE-DDS before running the Pyx4 examples, as instructed in this [guide](https://docs.px4.io/main/en/ros/ros2_comm.html).
+
+<div align='center'>
+  <img src="./docs/imgs/architecture_xrce-dds_ros2.svg" width="90%">
+</div>
+
+## Getting Started
+
+Setting up `Pyx4`:
+
+```bash
+cd dev_ws/src
+git clone https://github.com/kirinslab/pyx4.git
+cd dev_ws
+colcon build --packages-select pyx4
+source install/setup.bash
+```
+
+## Sample
+
+Run the example to read information about `GPS Position`:
+
+```bash
+ros2 run pyx4 pyx4_gps
+```
+
+Output:
+
+```
+RECEIVED VEHICLE GPS POSITION DATA
+==================================
+[Lat] :  473977440
+[lon] :  85455944
+[Alt]:  487624
+[Heading] :  nan
+RECEIVED VEHICLE GPS POSITION DATA
+==================================
+[Lat] :  473977472
+[lon] :  85455960
+[Alt]:  487939
+[Heading] :  nan
+```
+
+## QoS for PX4 Python
+
+PX4 QoS settings for publishers are incompatible with the default QoS settings for ROS 2 subscribers. So if ROS 2 code needs to subscribe to a uORB topic, it will need to use compatible QoS settings.
+
+PX4 uses the following QoS settings for `Sensor Data` subscribers:
+
+```C++
+rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
+auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
+```
+
+PX4 defines `qos_profile` as `rmw_qos_profile_sensor_data`. You can look up this profile in [QoS Profiles](https://github.com/ros2/rmw/blob/foxy/rmw/include/rmw/qos_profiles.h) as follows:
+
+```C++
+static const rmw_qos_profile_t rmw_qos_profile_sensor_data =
+{
+  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  5,
+  RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+  RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false
+}
+```
+
+Looking up the values of `rmw_qos_profile_sensor_data` corresponding to the classes defined in the [QoS rclpy](https://docs.ros2.org/foxy/api/rclpy/api/qos.html#) library. We get the following results:
+
+```python
+qos = QoSProfile(
+            history=HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            depth=5,
+            reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+            durability=DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
+        )
+```
+
+With QoS defined in `Python`, we can use it to read the packets that `PX4` publishes using `ROS2` and `Python`.
+
+## Author
+
+```
+[email protected]
+```
+
+## Reference
+
+- [PX4 Documentation](https://docs.px4.io/main/en/ros/ros2_comm.html)
+- [PX4 ROS COM](https://github.com/PX4/px4_ros_com)
+- [About Quality of Service Setting](https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html)
+- [QoS Profiles](https://github.com/ros2/rmw/blob/foxy/rmw/include/rmw/qos_profiles.h)
+- [QoS rclpy](https://docs.ros2.org/foxy/api/rclpy/api/qos.html#)
+
+## License
+
+Copyright (c) Kirinslab. All rights reserved. Licensed under the [GPT-3](LICENSE) license.

package.xml

@@ -0,0 +1,21 @@
+<?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>pyx4</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">rtr</maintainer>
+  <license>TODO: License declaration</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>px4_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

pyx4/offboard_control.py

@@ -0,0 +1,119 @@
+"""
+Python implementation of Offboard Control
+
+"""
+
+
+import rclpy
+from rclpy.node import Node
+from rclpy.clock import Clock
+
+from px4_msgs.msg import OffboardControlMode
+from px4_msgs.msg import TrajectorySetpoint
+from px4_msgs.msg import VehicleCommand
+from px4_msgs.msg import VehicleControlMode
+
+
+class OffboardControl(Node):
+
+    def __init__(self):
+        super().__init__('OffboardControl')
+        self.offboard_control_mode_publisher_ = self.create_publisher(OffboardControlMode,
+                                                                        "/fmu/in/offboard_control_mode", 10)
+        self.trajectory_setpoint_publisher_ = self.create_publisher(TrajectorySetpoint,
+                                                                    "/fmu/in/trajectory_setpoint", 10)
+        self.vehicle_command_publisher_ = self.create_publisher(VehicleCommand, "/fmu/in/vehicle_command", 10)
+
+        self.offboard_setpoint_counter_ = 0
+
+        timer_period = 0.1  # 100 milliseconds
+        self.timer_ = self.create_timer(timer_period, self.timer_callback)
+
+    def timer_callback(self):
+        if (self.offboard_setpoint_counter_ == 10):
+            # Change to Offboard mode after 10 setpoints
+            self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1., 6.)
+            # Arm the vehicle
+            self.arm()
+
+        # Offboard_control_mode needs to be paired with trajectory_setpoint
+        self.publish_offboard_control_mode()
+        self.publish_trajectory_setpoint()
+
+        # stop the counter after reaching 11
+        if (self.offboard_setpoint_counter_ < 11):
+            self.offboard_setpoint_counter_ += 1
+
+    # Arm the vehicle
+    def arm(self):
+        self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
+        self.get_logger().info("Arm command send")
+
+    # Disarm the vehicle
+    def disarm(self):
+        self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0)
+        self.get_logger().info("Disarm command send")
+
+    '''
+	Publish the offboard control mode.
+	For this example, only position and altitude controls are active.
+    '''
+
+    def publish_offboard_control_mode(self):
+        msg = OffboardControlMode()
+        msg.position = True
+        msg.velocity = False
+        msg.acceleration = False
+        msg.attitude = False
+        msg.body_rate = False
+        msg.timestamp = int(Clock().now().nanoseconds / 1000) # time in microseconds
+        self.offboard_control_mode_publisher_.publish(msg)
+
+    '''
+	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.
+    '''
+
+    def publish_trajectory_setpoint(self):
+        msg = TrajectorySetpoint()
+        # msg.timestamp = self.timestamp_
+        msg.position = [0.0, 0.0, -5.0] 
+        msg.yaw = -3.14  # [-PI:PI]
+        msg.timestamp = int(Clock().now().nanoseconds / 1000) # time in microseconds
+        self.trajectory_setpoint_publisher_.publish(msg)
+
+    '''
+    Publish vehicle commands
+        command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
+        param1    Command parameter 1 as defined by MAVLink uint16 VEHICLE_CMD enum
+        param2    Command parameter 2 as defined by MAVLink uint16 VEHICLE_CMD enum
+    '''
+    def publish_vehicle_command(self, command, param1=0.0, param2=0.0):
+        msg = VehicleCommand()
+        msg.param1 = param1
+        msg.param2 = param2
+        msg.command = command  # command ID
+        msg.target_system = 1  # system which should execute the command
+        msg.target_component = 1  # component which should execute the command, 0 for all components
+        msg.source_system = 1  # system sending the command
+        msg.source_component = 1  # component sending the command
+        msg.from_external = True
+        msg.timestamp = int(Clock().now().nanoseconds / 1000) # time in microseconds
+        self.vehicle_command_publisher_.publish(msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    print("Starting offboard control node...\n")
+    offboard_control = OffboardControl()
+    rclpy.spin(offboard_control)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    offboard_control.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

pyx4/pyx4_gps.py

@@ -0,0 +1,55 @@
+"""
+Python implementation of PX4
+
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.clock import Clock
+
+from px4_msgs.msg import SensorGps
+from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
+
+class Pyx4_GPS(Node):
+
+    def __init__(self):
+        super().__init__('pyx4_gps')
+
+        qos = QoSProfile(
+            history=HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            depth=5,
+            reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+            durability=DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE
+        )
+
+        self.gps_subscription = self.create_subscription(
+            SensorGps, 
+            "/fmu/out/vehicle_gps_position", 
+            self.gps_callback, 
+            qos)
+        self.gps_subscription
+
+    def gps_callback(self, msg):
+        print("RECEIVED VEHICLE GPS POSITION DATA")
+        print("==================================")
+        print("[Lat] : ", msg.lat)
+        print("[lon] : ", msg.lon)
+        print("[Alt]: ", msg.alt)
+        print("[Heading] : ", msg.heading)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    print("Starting infor node...\n")
+    infor = Pyx4_GPS()
+    rclpy.spin(infor)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    infor.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

samples/vehicle_gps_position_listener.cpp

@@ -0,0 +1,60 @@
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/sensor_gps.hpp>
+
+/**
+ * @brief Vehicle GPS position uORB topic data callback
+ */
+class VehicleGpsPositionListener : public rclcpp::Node
+{
+public:
+	explicit VehicleGpsPositionListener() : Node("vehicle_global_position_listener")
+	{
+		rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
+		auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
+
+		subscription_ = this->create_subscription<px4_msgs::msg::SensorGps>("/fmu/out/vehicle_gps_position", qos,
+		[this](const px4_msgs::msg::SensorGps::UniquePtr msg) {
+			std::cout << "\n\n\n\n\n\n\n\n\n\n";
+			std::cout << "RECEIVED VEHICLE GPS POSITION DATA"   << std::endl;
+			std::cout << "=================================="   << std::endl;
+			std::cout << "ts: "      << msg->timestamp    << std::endl;
+			std::cout << "lat: " << msg->lat  << std::endl;
+			std::cout << "lon: " << msg->lon << std::endl;
+			std::cout << "alt: " << msg->alt  << std::endl;
+			std::cout << "alt_ellipsoid: " << msg->alt_ellipsoid << std::endl;
+			std::cout << "s_variance_m_s: " << msg->s_variance_m_s << std::endl;
+			std::cout << "c_variance_rad: " << msg->c_variance_rad << std::endl;
+			std::cout << "fix_type: " << msg->fix_type << std::endl;
+			std::cout << "eph: " << msg->eph << std::endl;
+			std::cout << "epv: " << msg->epv << std::endl;
+			std::cout << "hdop: " << msg->hdop << std::endl;
+			std::cout << "vdop: " << msg->vdop << std::endl;
+			std::cout << "noise_per_ms: " << msg->noise_per_ms << std::endl;
+			std::cout << "vel_m_s: " << msg->vel_m_s << std::endl;
+			std::cout << "vel_n_m_s: " << msg->vel_n_m_s << std::endl;
+			std::cout << "vel_e_m_s: " << msg->vel_e_m_s << std::endl;
+			std::cout << "vel_d_m_s: " << msg->vel_d_m_s << std::endl;
+			std::cout << "cog_rad: " << msg->cog_rad << std::endl;
+			std::cout << "vel_ned_valid: " << msg->vel_ned_valid << std::endl;
+			std::cout << "timestamp_time_relative: " << msg->timestamp_time_relative << std::endl;
+			std::cout << "time_utc_usec: " << msg->time_utc_usec << std::endl;
+			std::cout << "satellites_used: " << msg->satellites_used << std::endl;
+			std::cout << "heading: " << msg->heading << std::endl;
+			std::cout << "heading_offset: " << msg->heading_offset << std::endl;
+		});
+	}
+
+private:
+	rclcpp::Subscription<px4_msgs::msg::SensorGps>::SharedPtr subscription_;
+};
+
+int main(int argc, char *argv[])
+{
+	std::cout << "Starting vehicle_global_position listener node..." << std::endl;
+	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<VehicleGpsPositionListener>());
+
+	rclcpp::shutdown();
+	return 0;
+}