WIP

cmake/bloaty.cmake

@@ -32,7 +32,7 @@
 ############################################################################
 
 find_program(BLOATY_PROGRAM bloaty)
-if(BLOATY_PROGRAM)
+if(BLOATY_PROGRAM AND ((${PX4_PLATFORM} MATCHES "nuttx") OR (${PX4_PLATFORM} MATCHES "posix")))
 
 	set(BLOATY_OPTS --demangle=full --domain=vm -s vm -n 200 -w)
 

launch/offboard_position_control.launch.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2022 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 PX4 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 OWNER 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.
+#
+############################################################################
+
+__author__ = "Jaeyoung Lim"
+__contact__ = "jalim@ethz.ch"
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory('px4_offboard')
+    return LaunchDescription([
+
+        Node(
+            package='px4_offboard',
+            namespace='px4_offboard',
+            executable='visualizer',
+            name='visualizer'
+        ),
+
+        Node(
+            package='px4_offboard',
+            namespace='px4_offboard',
+            executable='offboard_control',
+            name='control'
+        ),
+
+        Node(
+            package='rviz2',
+            namespace='',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', [os.path.join(package_dir, 'visualize.rviz')]]
+        )
+
+    ])

launch/visualize.launch.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2022 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 PX4 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 OWNER 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.
+#
+############################################################################
+
+__author__ = "Jaeyoung Lim"
+__contact__ = "jalim@ethz.ch"
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory('px4_offboard')
+    return LaunchDescription([
+        Node(
+            package='px4_offboard',
+            namespace='px4_offboard',
+            executable='visualizer',
+            name='visualizer'
+        ),
+        Node(
+            package='rviz2',
+            namespace='',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', [os.path.join(package_dir, 'visualize.rviz')]]
+        )
+    ])

package.xml

@@ -10,10 +10,14 @@
   <build_depend>rosidl_default_generators</build_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
+  <depend>builtin_interfaces</depend>
   <depend>rclcpp</depend>
+  <depend>rclpy</depend>
   <depend>std_msgs</depend>
 
+  <exec_depend>ros2launch</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
@@ -23,5 +27,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
+    <build_type>ament_python</build_type>
   </export>
 </package>

platforms/ros2/CMakeLists.txt

@@ -64,6 +64,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 	${ros_msg_files}
 )
 
+ament_export_dependencies(rosidl_default_runtime)
+
 # # mc_rate_control_ros2
 # add_executable(mc_rate_control ${CMAKE_SOURCE_DIR}/src/modules/mc_rate_control_ros2/MulticopterRateControl.cpp)
 # target_link_libraries(mc_rate_control px4_layer work_queue)
@@ -76,3 +78,15 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 #     mc_rate_control
 #   DESTINATION lib/${PROJECT_NAME}
 # )
+
+
+
+# TODO: python node
+
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME})
+# Install Python executables
+install(PROGRAMS
+  scripts/py_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)

platforms/ros2/cmake/init.cmake

@@ -35,6 +35,8 @@ enable_language(CXX)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)

platforms/ros2/nodes/offboard_example/package.xml

@@ -0,0 +1,20 @@
+<?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>px4_offboard</name>
+  <version>0.0.0</version>
+  <description>ROS2 python interface for px4</description>
+  <maintainer email="jalim@ethz.ch">jaeyoung</maintainer>
+  <license>BSD-3</license>
+
+  <depend>px4</depend>
+  <exec_depend>ros2launch</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>

platforms/ros2/nodes/offboard_example/px4_offboard/offboard_control.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+############################################################################
+#
+#   Copyright (C) 2022-2023 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 PX4 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 OWNER 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.
+#
+############################################################################
+
+__author__ = "Jaeyoung Lim"
+__contact__ = "jalim@ethz.ch"
+
+import rclpy
+import numpy as np
+from rclpy.node import Node
+from rclpy.clock import Clock
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
+
+from px4.msg import OffboardControlMode
+from px4.msg import TrajectorySetpoint
+from px4.msg import VehicleStatus
+
+
+class OffboardControl(Node):
+
+    def __init__(self):
+        super().__init__('minimal_publisher')
+        qos_profile = QoSProfile(
+            reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+            durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            depth=1
+        )
+
+        self.status_sub = self.create_subscription(
+            VehicleStatus,
+            '/fmu/out/vehicle_status',
+            self.vehicle_status_callback,
+            qos_profile)
+        self.publisher_offboard_mode = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
+        self.publisher_trajectory = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
+        timer_period = 0.02  # seconds
+        self.timer = self.create_timer(timer_period, self.cmdloop_callback)
+
+        self.nav_state = VehicleStatus.NAVIGATION_STATE_MAX
+        self.dt = timer_period
+        self.theta = 0.0
+        self.radius = 10.0
+        self.omega = 0.5
+
+    def vehicle_status_callback(self, msg):
+        # TODO: handle NED->ENU transformation
+        print("NAV_STATUS: ", msg.nav_state)
+        print("  - offboard status: ", VehicleStatus.NAVIGATION_STATE_OFFBOARD)
+        self.nav_state = msg.nav_state
+
+    def cmdloop_callback(self):
+        # Publish offboard control modes
+        offboard_msg = OffboardControlMode()
+        offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000)
+        offboard_msg.position=True
+        offboard_msg.velocity=False
+        offboard_msg.acceleration=False
+        self.publisher_offboard_mode.publish(offboard_msg)
+        if self.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD:
+
+            trajectory_msg = TrajectorySetpoint()
+            trajectory_msg.position[0] = self.radius * np.cos(self.theta)
+            trajectory_msg.position[1] = self.radius * np.sin(self.theta)
+            trajectory_msg.position[2] = -5.0
+            self.publisher_trajectory.publish(trajectory_msg)
+
+            self.theta = self.theta + self.omega * self.dt
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    offboard_control = OffboardControl()
+
+    rclpy.spin(offboard_control)
+
+    offboard_control.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

platforms/ros2/nodes/offboard_example/px4_offboard/visualizer.py

@@ -0,0 +1,187 @@
+#!/usr/bin/env python3
+############################################################################
+#
+#   Copyright (C) 2022-2023 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 PX4 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 OWNER 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.
+#
+############################################################################
+
+__author__ = "Jaeyoung Lim"
+__contact__ = "jalim@ethz.ch"
+
+from re import M
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from rclpy.clock import Clock
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
+
+from px4.msg import VehicleAttitude
+from px4.msg import VehicleLocalPosition
+from px4.msg import TrajectorySetpoint
+from geometry_msgs.msg import PoseStamped, Point
+from nav_msgs.msg import Path
+from visualization_msgs.msg import Marker
+
+def vector2PoseMsg(frame_id, position, attitude):
+    pose_msg = PoseStamped()
+    # msg.header.stamp = Clock().now().nanoseconds / 1000
+    pose_msg.header.frame_id=frame_id
+    pose_msg.pose.orientation.w = attitude[0]
+    pose_msg.pose.orientation.x = attitude[1]
+    pose_msg.pose.orientation.y = attitude[2]
+    pose_msg.pose.orientation.z = attitude[3]
+    pose_msg.pose.position.x = position[0]
+    pose_msg.pose.position.y = position[1]
+    pose_msg.pose.position.z = position[2]
+    return pose_msg
+
+class PX4Visualizer(Node):
+
+    def __init__(self):
+        super().__init__('px4_visualizer')
+
+        ## Configure subscritpions
+        qos_profile = QoSProfile(
+            reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            depth=1
+        )
+
+        self.attitude_sub = self.create_subscription(
+            VehicleAttitude,
+            '/fmu/out/vehicle_attitude',
+            self.vehicle_attitude_callback,
+            qos_profile)
+        self.local_position_sub = self.create_subscription(
+            VehicleLocalPosition,
+            '/fmu/out/vehicle_local_position',
+            self.vehicle_local_position_callback,
+            qos_profile)
+        self.setpoint_sub = self.create_subscription(
+            TrajectorySetpoint,
+            '/fmu/in/trajectory_setpoint',
+            self.trajectory_setpoint_callback,
+            qos_profile)
+
+        self.vehicle_pose_pub = self.create_publisher(PoseStamped, '/px4_visualizer/vehicle_pose', 10)
+        self.vehicle_vel_pub = self.create_publisher(Marker, '/px4_visualizer/vehicle_velocity', 10)
+        self.vehicle_path_pub = self.create_publisher(Path, '/px4_visualizer/vehicle_path', 10)
+        self.setpoint_path_pub = self.create_publisher(Path, '/px4_visualizer/setpoint_path', 10)
+
+        self.vehicle_attitude = np.array([1.0, 0.0, 0.0, 0.0])
+        self.vehicle_local_position = np.array([0.0, 0.0, 0.0])
+        self.vehicle_local_velocity = np.array([0.0, 0.0, 0.0])
+        self.setpoint_position = np.array([0.0, 0.0, 0.0])
+        self.vehicle_path_msg = Path()
+        self.setpoint_path_msg = Path()
+        timer_period = 0.05  # seconds
+        self.timer = self.create_timer(timer_period, self.cmdloop_callback)
+
+    def vehicle_attitude_callback(self, msg):
+        # TODO: handle NED->ENU transformation
+        self.vehicle_attitude[0] = msg.q[0]
+        self.vehicle_attitude[1] = msg.q[1]
+        self.vehicle_attitude[2] = -msg.q[2]
+        self.vehicle_attitude[3] = -msg.q[3]
+
+    def vehicle_local_position_callback(self, msg):
+        # TODO: handle NED->ENU transformation
+        self.vehicle_local_position[0] = msg.x
+        self.vehicle_local_position[1] = -msg.y
+        self.vehicle_local_position[2] = -msg.z
+        self.vehicle_local_velocity[0] = msg.vx
+        self.vehicle_local_velocity[1] = -msg.vy
+        self.vehicle_local_velocity[2] = -msg.vz
+
+    def trajectory_setpoint_callback(self, msg):
+        self.setpoint_position[0] = msg.position[0]
+        self.setpoint_position[1] = -msg.position[1]
+        self.setpoint_position[2] = -msg.position[2]
+
+    def create_arrow_marker(self, id, tail, vector):
+        msg = Marker()
+        msg.action = Marker.ADD
+        msg.header.frame_id = 'map'
+        # msg.header.stamp = Clock().now().nanoseconds / 1000
+        msg.ns = 'arrow'
+        msg.id = id
+        msg.type = Marker.ARROW
+        msg.scale.x = 0.1
+        msg.scale.y = 0.2
+        msg.scale.z = 0.0
+        msg.color.r = 0.5
+        msg.color.g = 0.5
+        msg.color.b = 0.0
+        msg.color.a = 1.0
+        dt = 0.3
+        tail_point = Point()
+        tail_point.x = tail[0]
+        tail_point.y = tail[1]
+        tail_point.z = tail[2]
+        head_point = Point()
+        head_point.x = tail[0] + dt * vector[0]
+        head_point.y = tail[1] + dt * vector[1]
+        head_point.z = tail[2] + dt * vector[2]
+        msg.points = [tail_point, head_point]
+        return msg
+
+    def cmdloop_callback(self):
+        vehicle_pose_msg = vector2PoseMsg('map', self.vehicle_local_position, self.vehicle_attitude)
+        self.vehicle_pose_pub.publish(vehicle_pose_msg)
+
+        # Publish time history of the vehicle path
+        self.vehicle_path_msg.header = vehicle_pose_msg.header
+        self.vehicle_path_msg.poses.append(vehicle_pose_msg)
+        self.vehicle_path_pub.publish(self.vehicle_path_msg)
+
+        # Publish time history of the vehicle path
+        setpoint_pose_msg = vector2PoseMsg('map', self.setpoint_position, self.vehicle_attitude)
+        self.setpoint_path_msg.header = setpoint_pose_msg.header
+        self.setpoint_path_msg.poses.append(setpoint_pose_msg)
+        self.setpoint_path_pub.publish(self.setpoint_path_msg)
+
+        # Publish arrow markers for velocity
+        velocity_msg = self.create_arrow_marker(1, self.vehicle_local_position, self.vehicle_local_velocity)
+        self.vehicle_vel_pub.publish(velocity_msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    px4_visualizer = PX4Visualizer()
+
+    rclpy.spin(px4_visualizer)
+
+    px4_visualizer.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

platforms/ros2/nodes/offboard_example/resource/visualize.rviz

@@ -0,0 +1,213 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Pose1
+        - /Marker1
+        - /Marker1/Topic1
+        - /Path1
+        - /Path2
+      Splitter Ratio: 0.5
+    Tree Height: 907
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Axes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /px4_visualizer/vehicle_pose
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        arrow: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /px4_visualizer/vehicle_velocity
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /px4_visualizer/vehicle_path
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 0; 255
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /px4_visualizer/setpoint_path
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 24.51659393310547
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.795397937297821
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.6553981304168701
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1136
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000028e00000416fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000416000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039c0000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 924
+  X: 72
+  Y: 27

platforms/ros2/nodes/offboard_example/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/px4_offboard
+[install]
+install-scripts=$base/lib/px4_offboard

platforms/ros2/nodes/offboard_example/setup.py

@@ -0,0 +1,33 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'px4_offboard'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + 'visualize.rviz']),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*')),
+        (os.path.join('share', package_name), glob('resource/*rviz'))
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='jaeyoung',
+    maintainer_email='jalim@ethz.ch',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+                'offboard_control = px4_offboard.offboard_control:main',
+                'visualizer = px4_offboard.visualizer:main',
+        ],
+    },
+)

platforms/ros2/nodes/px4/CMakeLists.txt

@@ -0,0 +1,6 @@
+
+
+include(ExternalProject)
+
+# build px4_sitl_default?
+

platforms/ros2/nodes/px4_firmware_manager/CMakeLists.txt

@@ -0,0 +1,20 @@
+
+
+# upload script
+
+# build px4_fmu-v5x
+#  node checks version and uploads
+
+include(ExternalProject)
+
+ExternalProject_Add(px4_firmware_build
+	DOWNLOAD_COMMAND rsync -a --safe-links --delete --exclude=build --exclude=*.o --exclude=*.a --exclude=.config ${PX4_SOURCE_DIR}/ ${PX4_BINARY_DIR}/external/Source/px4io_firmware_build/
+	UPDATE_COMMAND   rsync -a --safe-links --delete --exclude=build --exclude=*.o --exclude=*.a --exclude=.config ${PX4_SOURCE_DIR}/ ${PX4_BINARY_DIR}/external/Source/px4io_firmware_build/
+	DEPENDS git_nuttx git_nuttx_apps
+	SOURCE_DIR ${PX4_BINARY_DIR}/external/Source/px4_firmware_build/
+	CMAKE_ARGS -DCONFIG=px4_fmu-v5x_default
+	INSTALL_COMMAND ""
+	USES_TERMINAL_BUILD true
+	EXCLUDE_FROM_ALL true
+	BUILD_ALWAYS true
+)

src/modules/microdds_client/dds_topics.yaml

@@ -6,81 +6,81 @@
 publications:
 
   - topic: /fmu/out/collision_constraints
-    type: px4_msgs::msg::CollisionConstraints
+    type: px4::msg::CollisionConstraints
 
   - topic: /fmu/out/failsafe_flags
-    type: px4_msgs::msg::FailsafeFlags
+    type: px4::msg::FailsafeFlags
 
   - topic: /fmu/out/sensor_combined
-    type: px4_msgs::msg::SensorCombined
+    type: px4::msg::SensorCombined
 
   - topic: /fmu/out/timesync_status
-    type: px4_msgs::msg::TimesyncStatus
+    type: px4::msg::TimesyncStatus
 
   # - topic: /fmu/out/vehicle_angular_velocity
-  #   type: px4_msgs::msg::VehicleAngularVelocity
+  #   type: px4::msg::VehicleAngularVelocity
 
   - topic: /fmu/out/vehicle_attitude
-    type: px4_msgs::msg::VehicleAttitude
+    type: px4::msg::VehicleAttitude
 
   - topic: /fmu/out/vehicle_control_mode
-    type: px4_msgs::msg::VehicleControlMode
+    type: px4::msg::VehicleControlMode
 
   - topic: /fmu/out/vehicle_global_position
-    type: px4_msgs::msg::VehicleGlobalPosition
+    type: px4::msg::VehicleGlobalPosition
 
   - topic: /fmu/out/vehicle_gps_position
-    type: px4_msgs::msg::SensorGps
+    type: px4::msg::SensorGps
 
   - topic: /fmu/out/vehicle_local_position
-    type: px4_msgs::msg::VehicleLocalPosition
+    type: px4::msg::VehicleLocalPosition
 
   - topic: /fmu/out/vehicle_odometry
-    type: px4_msgs::msg::VehicleOdometry
+    type: px4::msg::VehicleOdometry
 
   - topic: /fmu/out/vehicle_status
-    type: px4_msgs::msg::VehicleStatus
+    type: px4::msg::VehicleStatus
 
   - topic: /fmu/out/vehicle_trajectory_waypoint_desired
-    type: px4_msgs::msg::VehicleTrajectoryWaypoint
+    type: px4::msg::VehicleTrajectoryWaypoint
 
 subscriptions:
 
   - topic: /fmu/in/offboard_control_mode
-    type: px4_msgs::msg::OffboardControlMode
+    type: px4::msg::OffboardControlMode
 
   - topic: /fmu/in/onboard_computer_status
-    type: px4_msgs::msg::OnboardComputerStatus
+    type: px4::msg::OnboardComputerStatus
 
   - topic: /fmu/in/obstacle_distance
-    type: px4_msgs::msg::ObstacleDistance
+    type: px4::msg::ObstacleDistance
 
   - topic: /fmu/in/sensor_optical_flow
-    type: px4_msgs::msg::SensorOpticalFlow
+    type: px4::msg::SensorOpticalFlow
 
   - topic: /fmu/in/telemetry_status
-    type: px4_msgs::msg::TelemetryStatus
+    type: px4::msg::TelemetryStatus
 
   - topic: /fmu/in/trajectory_setpoint
-    type: px4_msgs::msg::TrajectorySetpoint
+    type: px4::msg::TrajectorySetpoint
 
   - topic: /fmu/in/vehicle_attitude_setpoint
-    type: px4_msgs::msg::VehicleAttitudeSetpoint
+    type: px4::msg::VehicleAttitudeSetpoint
 
   - topic: /fmu/in/vehicle_mocap_odometry
-    type: px4_msgs::msg::VehicleOdometry
+    type: px4::msg::VehicleOdometry
 
   - topic: /fmu/in/vehicle_rates_setpoint
-    type: px4_msgs::msg::VehicleRatesSetpoint
+    type: px4::msg::VehicleRatesSetpoint
 
   - topic: /fmu/in/vehicle_visual_odometry
-    type: px4_msgs::msg::VehicleOdometry
+    type: px4::msg::VehicleOdometry
 
   - topic: /fmu/in/vehicle_command
-    type: px4_msgs::msg::VehicleCommand
+    type: px4::msg::VehicleCommand
 
   - topic: /fmu/in/vehicle_trajectory_bezier
-    type: px4_msgs::msg::VehicleTrajectoryBezier
+    type: px4::msg::VehicleTrajectoryBezier
 
   - topic: /fmu/in/vehicle_trajectory_waypoint
-    type: px4_msgs::msg::VehicleTrajectoryWaypoint
+    type: px4::msg::VehicleTrajectoryWaypoint

src/modules/microdds_client/generate_dds_topics.py

@@ -86,7 +86,7 @@ merged_em_globals = {}
 all_type_includes = []
 
 for p in msg_map['publications']:
-    # eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint
+    # eg TrajectoryWaypoint from px4::msg::TrajectoryWaypoint
     simple_base_type = p['type'].split('::')[-1]
 
     # eg TrajectoryWaypoint -> trajectory_waypoint
@@ -96,7 +96,7 @@ for p in msg_map['publications']:
     # simple_base_type: eg vehicle_status
     p['simple_base_type'] = base_type_name_snake_case
 
-    # dds_type: eg px4_msgs::msg::dds_::VehicleStatus_
+    # dds_type: eg px4::msg::dds_::VehicleStatus_
     p['dds_type'] = p['type'].replace("::msg::", "::msg::dds_::") + "_"
 
     # topic_simple: eg vehicle_status
@@ -106,7 +106,7 @@ for p in msg_map['publications']:
 merged_em_globals['publications'] = msg_map['publications']
 
 for s in msg_map['subscriptions']:
-    # eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint
+    # eg TrajectoryWaypoint from px4::msg::TrajectoryWaypoint
     simple_base_type = s['type'].split('::')[-1]
 
     # eg TrajectoryWaypoint -> trajectory_waypoint
@@ -116,7 +116,7 @@ for s in msg_map['subscriptions']:
     # simple_base_type: eg vehicle_status
     s['simple_base_type'] = base_type_name_snake_case
 
-    # dds_type: eg px4_msgs::msg::dds_::VehicleStatus_
+    # dds_type: eg px4::msg::dds_::VehicleStatus_
     s['dds_type'] = s['type'].replace("::msg::", "::msg::dds_::") + "_"
 
     # topic_simple: eg vehicle_status