Add python examples for offboard control and sensor combined listener

launch/offboard_control.launch.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+
+################################################################################
+#
+# Copyright (c) 2018, PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors
+# may be used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+################################################################################
+
+"""
+Example to launch a sensor_combined listener node.
+
+.. seealso::
+    https://index.ros.org/doc/ros2/Launch-system/
+"""
+
+from launch import LaunchDescription
+import launch_ros.actions
+import os
+
+
+def generate_launch_description():
+    if os.environ['ROS_DISTRO'] != "galactic" and os.environ['ROS_DISTRO'] != "rolling":
+        return LaunchDescription([
+            launch_ros.actions.Node(
+                package='px4_ros_com', node_executable='offboard_control', output='screen'),
+        ])
+    else:
+        return LaunchDescription([
+            launch_ros.actions.Node(
+                package='px4_ros_com', executable='offboard_control', output='screen'),
+        ])

scripts/offboard_control_py.py

@@ -0,0 +1,153 @@
+# /****************************************************************************
+#  *
+#  * Copyright 2020 PX4 Development Team. All rights reserved.
+#  *
+#  * Redistribution and use in source and binary forms, with or without
+#  * modification, are permitted provided that the following conditions are met:
+#  *
+#  * 1. Redistributions of source code must retain the above copyright notice, this
+#  * list of conditions and the following disclaimer.
+#  *
+#  * 2. Redistributions in binary form must reproduce the above copyright notice,
+#  * this list of conditions and the following disclaimer in the documentation
+#  * and/or other materials provided with the distribution.
+#  *
+#  * 3. Neither the name of the copyright holder nor the names of its contributors
+#  * may be used to endorse or promote products derived from this software without
+#  * specific prior written permission.
+#  *
+#  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+#  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+#  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+#  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+#  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+#  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+#  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+#  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+#  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+#  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#  * POSSIBILITY OF SUCH DAMAGE.
+#  *
+#  ****************************************************************************/
+
+# /**
+#  * @brief Offboard control example
+#  * @file offboard_control_py.py
+#  * @addtogroup examples
+# * @ author < hanghaotian@gmail.com >
+# * @ author Haotian Hang
+
+#  * The TrajectorySetpoint message and the OFFBOARD mode in general are under an ongoing update.
+#  * Please refer to PR: https://github.com/PX4/PX4-Autopilot/pull/16739 for more info.
+#  * As per PR: https://github.com/PX4/PX4-Autopilot/pull/17094, the format
+#  * of the TrajectorySetpoint message shall change.
+#  */
+
+import rclpy
+from rclpy.node import Node
+from px4_msgs.msg import OffboardControlMode
+from px4_msgs.msg import TrajectorySetpoint
+from px4_msgs.msg import Timesync
+from px4_msgs.msg import VehicleCommand
+from px4_msgs.msg import VehicleControlMode
+import sys
+
+
+class OffboardControl(Node):
+    def __init__(self):
+        super().__init__('offboard_control')
+        # initialize parameters
+        self.offboard_setpoint_counter_ = 0  # counter for the number of setpoints sent
+        self.timestamp_ = 0  # in python because of GIL,  the basic operations are already atomic
+        self.offboard_control_mode_publisher_ = self.create_publisher(
+            OffboardControlMode, "fmu/offboard_control_mode/in", 10)
+        self.trajectory_setpoint_publisher_ = self.create_publisher(
+            TrajectorySetpoint, "fmu/trajectory_setpoint/in", 10)
+        self.vehicle_command_publisher_ = self.create_publisher(
+            VehicleCommand, "fmu/vehicle_command/in", 10)
+        self.timesync_sub_ = self.create_subscription(
+            Timesync, "fmu/timesync/out", self.timesync_callback, 10)
+        timer_period = 0.1  # seconds
+        self.timer_ = self.create_timer(
+            timer_period, self.timer_callback)
+
+    def timesync_callback(self, msg):
+        self.timestamp_ = msg.timestamp
+
+    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, float(1), float(6))
+            self.arm()
+        # offboard_control_mode needs to be paired with trajectory_setpoint
+        self.publish_offboard_control_mode()
+        self.publish_trajectory_setpoint()
+        if (self.offboard_setpoint_counter_ < 11):
+            self.offboard_setpoint_counter_ += 1
+
+    # @brief  Send a command to 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")
+
+    # @brief  Send a command to 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")
+
+    # @brief Publish the offboard control mode. For this example, only position and altitude controls are active.
+    def publish_offboard_control_mode(self):
+        msg = OffboardControlMode()
+        msg.timestamp = self.timestamp_
+        msg.position = True
+        msg.velocity = False
+        msg.acceleration = False
+        msg.attitude = False
+        msg.body_rate = False
+        # self.get_logger().info("offboard control mode publisher send")
+        self.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.
+    def publish_trajectory_setpoint(self):
+        msg = TrajectorySetpoint()
+        msg.timestamp = self.timestamp_
+        msg.x = 0.0
+        msg.y = 0.0
+        msg.z = -5.0
+        msg.yaw = -3.14  # [-PI:PI]
+        # self.get_logger().info("trajectory setpoint send")
+        self.trajectory_setpoint_publisher_.publish(msg)
+
+    #  @ 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
+
+    def publish_vehicle_command(self, command, param1, param2=0.0):
+        msg = VehicleCommand()
+        msg.timestamp = self.timestamp_
+        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
+        self.vehicle_command_publisher_.publish(msg)
+
+
+def main(argc, argv):
+    print("Starting offboard control node...")
+    rclpy.init()
+    offboard_control = OffboardControl()
+    rclpy.spin(offboard_control)
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main(len(sys.argv), sys.argv)

scripts/sensor_combined_listener_py.py

@@ -0,0 +1,90 @@
+# /****************************************************************************
+# *
+# * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL(eProsima).
+# *           2018 PX4 Pro Development Team. All rights reserved.
+# *
+# * Redistribution and use in source and binary forms, with or without
+# * modification, are permitted provided that the following conditions are met:
+#     *
+#     * 1. Redistributions of source code must retain the above copyright notice, this
+#     * list of conditions and the following disclaimer.
+#     *
+#     * 2. Redistributions in binary form must reproduce the above copyright notice,
+#     * this list of conditions and the following disclaimer in the documentation
+#     * and / or other materials provided with the distribution.
+#     *
+#     * 3. Neither the name of the copyright holder nor the names of its contributors
+#     * may be used to endorse or promote products derived from this software without
+#     * specific prior written permission.
+#     *
+#     * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+#     * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+#     * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+#     * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+#     * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+#     * CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+#                             * SUBSTITUTE GOODS OR SERVICES
+#                             LOSS OF USE, DATA, OR PROFITS
+#                             OR BUSINESS
+#                             * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+#     * CONTRACT, STRICT LIABILITY, OR TORT(INCLUDING NEGLIGENCE OR OTHERWISE)
+#     * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#     * POSSIBILITY OF SUCH DAMAGE.
+#     *
+#     ****************************************************************************/
+
+# /**
+# * @ brief Sensor Combined uORB topic listener example
+# * @ file sensor_combined_listener_py.py
+# * @ addtogroup examples
+# * @ author < hanghaotian@gmail.com >
+# * @ author Haotian Hang
+# */
+
+
+# /**
+# * @ brief Sensor Combined uORB topic data callback
+# */
+
+import rclpy
+from rclpy.node import Node
+from px4_msgs.msg import SensorCombined
+
+
+class SensorCombinedListener(Node):
+    def __init__(self):
+        super().__init__('sensor_combined_listener')
+        self.subscription = self.create_subscription(
+            SensorCombined,
+            "fmu/sensor_combined/out",
+            self.listener_callback,
+            10)
+        self.subscription
+
+    def listener_callback(self, msg):
+        print("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n")
+        print("RECEIVED SENSOR COMBINED DATA")
+        print("=============================")
+        print("ts: ", msg.timestamp)
+        print("gyro_rad[0]: ", msg.gyro_rad[0])
+        print("gyro_rad[1]: ", msg.gyro_rad[1])
+        print("gyro_rad[2]: ", msg.gyro_rad[2])
+        print("accelerometer_timestamp_relative: ",
+              msg.accelerometer_timestamp_relative)
+        print("accelerometer_m_s2[0]: ", msg.accelerometer_m_s2[0])
+        print("accelerometer_m_s2[1]: ", msg.accelerometer_m_s2[1])
+        print("accelerometer_m_s2[2]: ", msg.accelerometer_m_s2[2])
+        print("accelerometer_integral_dt: ", msg.accelerometer_integral_dt)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    print("Starting sensor_combined listener node...")
+    sensor_combined_listener = SensorCombinedListener()
+    rclpy.spin(sensor_combined_listener)
+    sensor_combined_listener.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()