├── .gitignore ├── CITATION ├── LICENSE ├── README.md ├── doc └── ROS2_PX4_Offboard_Tutorial.md ├── launch ├── offboard_hardware_position_control.launch.py ├── offboard_position_control.launch.py └── visualize.launch.py ├── package.xml ├── px4_offboard ├── __init__.py ├── offboard_control.py └── visualizer.py ├── resource ├── px4_offboard └── visualize.rviz ├── setup.cfg ├── setup.py └── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .vscode/* 3 | -------------------------------------------------------------------------------- /CITATION: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software in an academic context, please cite it as below." 3 | authors: 4 | - family-names: "Lim" 5 | given-names: "Jaeyoung" 6 | orcid: "https://orcid.org/0000-0002-5695-4511" 7 | title: "px4-offboard" 8 | version: v1.0 9 | date-released: 2023-04-04 10 | url: "https://github.com/Jaeyoung-Lim/px4-offboard" 11 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, JaeyoungLim 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # px4-offboard 2 | This `repository` contains a python examples for offboard control on ROS2 with [PX4](https://px4.io/) 3 | 4 | The `px4_offboard` package contains the following nodes 5 | - `offboard_control.py`: Example of offboard position control using position setpoints 6 | - `visualizer.py`: Used for visualizing vehicle states in Rviz 7 | 8 | The source code is released under a BSD 3-Clause license. 9 | 10 | - **Author**: Jaeyoung Lim 11 | - **Affiliation**: Autonomous Systems Lab, ETH Zurich 12 | 13 | ## Setup 14 | Add the repository to the ros2 workspace 15 | ``` 16 | git clone https://github.com/Jaeyoung-Lim/px4-offboard.git 17 | ``` 18 | 19 | If you are running this on a companion computer to PX4, you will need to build the package on the companion computer directly. 20 | 21 | ## Running 22 | 23 | ### Software in the Loop 24 | You will make use of 3 different terminals to run the offboard demo. 25 | 26 | On the first terminal, run a SITL instance from the PX4 Autopilot firmware. 27 | ``` 28 | make px4_sitl gz_x500 29 | ``` 30 | 31 | On the second terminal terminal, run the micro-ros-agent which will perform the mapping between Micro XRCE-DDS and RTPS. So that ROS2 Nodes are able to communicate with the PX4 micrortps_client. 32 | ``` 33 | MicroXRCEAgent udp4 -p 8888 34 | ``` 35 | 36 | In order to run the offboard position control example, open a third terminal and run the the node. 37 | This runs two ros nodes, which publishes offboard position control setpoints and the visualizer. 38 | ``` 39 | ros2 launch px4_offboard offboard_position_control.launch.py 40 | ``` 41 | ![offboard](https://user-images.githubusercontent.com/5248102/194742116-64b93fcb-ec99-478d-9f4f-f32f7f06e9fd.gif) 42 | 43 | In order to just run the visualizer, 44 | ``` 45 | ros2 launch px4_offboard visualize.launch.py 46 | ``` 47 | ### Hardware 48 | 49 | This section is intended for running the offboard control node on a companion computer, such as a Raspberry Pi or Nvidia Jetson/Xavier. You will either need an SSH connection to run this node, or have a shell script to run the nodes on start up. 50 | 51 | If you are running this through a UART connection into the USB port, start the micro-ros agent with the following command 52 | 53 | ``` 54 | micro-ros-agent serial --dev /dev/ttyUSB0 -b 921600 -v 55 | ``` 56 | If you are using a UART connection which goes into the pinouts on the board, start the micro-ros agent with the following comand 57 | ``` 58 | micro-ros-agent serial --dev /dev/ttyTHS1 -b 921600 -V 59 | ``` 60 | 61 | To run the offboard position control example, run the node on the companion computer 62 | ``` 63 | ros2 launch px4_offboard offboard_hardware_position_control.launch.py 64 | ``` 65 | 66 | ### Visualization parameters 67 | 68 | The following section describes ROS2 parameters that alter behavior of visualization tool. 69 | 70 | 71 | #### Automatic path clearing between consequent simulation runs 72 | 73 | Running visualization node separately from the simulation can be desired to iteratively test how variety of PX4 (or custom offboard programs) parameters adjustments influence the system behavior. 74 | 75 | In such case RVIZ2 is left opened and in separate shell simulation is repeadetly restarted. This process causes paths from consequent runs to overlap with incorrect path continuity where end of previous path is connected to the beginning of new path from new run. To prevent that and clear old path automatically on start of new simulation, set `path_clearing_timeout` to positive float value which corresponds to timeout seconds after which, upon starting new simulation, old path is removed. 76 | 77 | As an example, setting the parameter to `1.0` means that one second of delay between position updates through ROS2 topic will schedule path clearing right when next position update comes in (effectively upon next simulation run). 78 | 79 | 80 | To enable automatic path clearing without closing visualization node set the param to positive floating point value: 81 | ``` 82 | ros2 param set /px4_offboard/visualizer path_clearing_timeout 1.0 83 | ``` 84 | 85 | To disable this feature set timeout to any negative floating point value (the feature is disabled by default): 86 | ``` 87 | ros2 param set /px4_offboard/visualizer path_clearing_timeout -1.0 88 | ``` 89 | -------------------------------------------------------------------------------- /doc/ROS2_PX4_Offboard_Tutorial.md: -------------------------------------------------------------------------------- 1 | 2 | ## Overview 3 | 4 | This tutorial explains how to interface ROS2 with PX4 (SITL) using DDS. 5 | 6 | ### Prerequisites 7 | 8 | * [ROS2 Installed](https://docs.px4.io/main/en/ros/ros2_comm.html#install-ros-2), and setup for your operating system (e.g. [Linux Ubuntu](https://docs.px4.io/main/en/dev_setup/dev_env_linux_ubuntu.html)) with Gazebo 9 | * [FastDDS Installed](https://docs.px4.io/v1.13/en/dev_setup/fast-dds-installation.html#fast-dds-installation) 10 | * [PX4-Autopilot downloaded](https://docs.px4.io/main/en/dev_setup/building_px4.html) 11 | * [QGroundControl installed](https://docs.qgroundcontrol.com/master/en/getting_started/download_and_install.html) 12 | * Ubuntu 22.04 13 | * ROS2 Humble 14 | * Python 3.10 15 | 16 | ## Install PX4 Offboard and dependencies (one time setup) 17 | 18 | ### Install the px4-offboard example from Jaeyoung-Lim 19 | 20 | ``` 21 | cd ~ 22 | git clone https://github.com/Jaeyoung-Lim/px4-offboard.git 23 | ``` 24 | 25 | ### Install PX4 msg 26 | 27 | The `px4-offboard` example requires `px4_msgs` definitions: 28 | 29 | ``` 30 | mkdir -p ~/px4_ros_com_ws/src && cd ~/px4_ros_com_ws/src 31 | git clone https://github.com/PX4/px4_msgs.git 32 | ``` 33 | 34 | Build: 35 | 36 | ``` 37 | colcon build 38 | ``` 39 | 40 | This should build. You may see some warnings interspered with the output. As long as there are no __*errors*__ you should be OK.. 41 | 42 | ## Install the micro_ros_agent (one time setup) 43 | Follow these instructions to build the micro_ros_setup: [Building micro_ros_setup](https://github.com/micro-ROS/micro_ros_setup#building) 44 | ``` 45 | source /opt/ros/$ROS_DISTRO/setup.bash 46 | 47 | mkdir microros_ws && cd microros_ws 48 | 49 | git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git src/micro_ros_setup 50 | 51 | rosdep update && rosdep install --from-paths src --ignore-src -y 52 | 53 | colcon build 54 | 55 | source install/local_setup.bash 56 | ``` 57 | 58 | Follow these instructions to build the micro_ros_agent: [Building micro-ROS-Agent](https://github.com/micro-ROS/micro_ros_setup#building-micro-ros-agent) 59 | ``` 60 | ros2 run micro_ros_setup create_agent_ws.sh 61 | ros2 run micro_ros_setup build_agent.sh 62 | source install/local_setup.sh 63 | ``` 64 | 65 | Try running the agent (assuming the agent is installed at `~/microros_ws`): 66 | 67 | ``` 68 | cd ~/microros_ws 69 | source install/local_setup.sh 70 | ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 71 | ``` 72 | 73 | It should respond with: 74 | 75 | ``` 76 | [1670101284.566587] info | UDPv4AgentLinux.cpp | init | running... | port: 8888 77 | [1670101284.566822] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 4 78 | ``` 79 | 80 | ## Run the Demo 81 | 82 | ### Overview 83 | 84 | You will need 4 terminal windows ([gnome terminator](https://gnome-terminator.org/) is a great for this!), one for each of the following components: 85 | 86 | * micro-ros-agent 87 | * Gazebo 88 | * px4-offboard example 89 | * QGroundControl 90 | 91 | In each terminal window, we will first source any workspace settings required for a particular component and also set the `ROS_DOMAIN_ID` and `PYTHONOPTIMIZE` in each window 92 | 93 | ### Start the micro-ros-agent 94 | 95 | In the terminal you designated as the `micro-ros-agent` terminal: 96 | 97 | ``` 98 | cd ~/microros_ws 99 | export ROS_DOMAIN_ID=0 100 | export PYTHONOPTIMIZE=1 101 | ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 ROS_DOMAIN_ID=0 102 | ``` 103 | 104 | This should start with a few initial messages: 105 | 106 | ``` 107 | [1680365404.306615] info | UDPv4AgentLinux.cpp | init | running... | port: 8888 108 | [1680365404.306837] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 4 109 | ``` 110 | 111 | ### Start Gazebo 112 | 113 | In the terminal you designated for gazebo: 114 | 115 | ``` 116 | cd ~/PX4-Autopilot 117 | export ROS_DOMAIN_ID=0 118 | export PYTHONOPTIMIZE=1 119 | make px4_sitl gazebo 120 | ``` 121 | 122 | Gazebo should start and you will see a big PX4 ascii art banner in the gazebo terminal and the GUI will launch. 123 | 124 | Back in the `micro-ros-agent` terminal, you should see the Micro ROS Agent start to receive DDS messages: 125 | 126 | 127 | ``` 128 | [1680365951.442844] info | ProxyClient.cpp | create_topic | topic created | client_key: 0xAAAABBBB, topic_id: 0x0C7(2), participant_id: 0x001(1) 129 | [1680365951.442905] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0xAAAABBBB, publisher_id: 0x0C7(3), participant_id: 0x001(1) 130 | [1680365951.443306] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0xAAAABBBB, datawriter_id: 0x0C7(5), publisher_id: 0x0C7(3) 131 | [1680365956.248671] info | ProxyClient.cpp | create_topic | topic created | client_key: 0xAAAABBBB, topic_id: 0x0CE(2), participant_id: 0x001(1) 132 | [1680365956.248727] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0xAAAABBBB, publisher_id: 0x0CE(3), participant_id: 0x001(1) 133 | [1680365956.249012] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0xAAAABBBB, datawriter_id: 0x0CE(5), publisher_id: 0x0CE(3) 134 | ``` 135 | 136 | Leave the agent running for the rest of the tutorial 137 | 138 | If you scroll up in the Gazebo terminal window, you should see logs indicating it set up the microdds_client: 139 | 140 | ``` 141 | INFO [microdds_client] synchronized with time offset 1680366210257897us 142 | INFO [microdds_client] successfully created rt/fmu/out/failsafe_flags data writer, topic id: 79 143 | INFO [microdds_client] successfully created rt/fmu/out/sensor_combined data writer, topic id: 162 144 | INFO [microdds_client] successfully created rt/fmu/out/timesync_status data writer, topic id: 182 145 | INFO [microdds_client] successfully created rt/fmu/out/vehicle_control_mode data writer, topic id: 205 146 | INFO [microdds_client] successfully created rt/fmu/out/vehicle_status data writer, topic id: 222 147 | INFO [microdds_client] successfully created rt/fmu/out/vehicle_gps_position data writer, topic id: 208 148 | INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550 149 | INFO [microdds_client] successfully created rt/fmu/out/vehicle_local_position data writer, topic id: 212 150 | INFO [microdds_client] successfully created rt/fmu/out/vehicle_odometry data writer, topic id: 217 151 | ``` 152 | 153 | ### Start QGround Controller and Take Off 154 | 155 | In the terminal you designated as the `QGroundControl` terminal, launch the app: 156 | 157 | ``` 158 | cd /dir/where/qgroundcontroller/is/installed 159 | ./QGroundControl.AppImage 160 | ``` 161 | 162 | Click `Takeoff` from left hand menum, then slide to confirm 163 | 164 | The simulated drone should takeoff and climb to an altitude of 10m (~32ft) 165 | 166 | 167 | ### Start the px4-offboard example 168 | 169 | #### Check ROS Messages 170 | 171 | Before we start the example, lets check ROS2 topics. 172 | 173 | In the window you have designated for the `px4-offboard` example: 174 | 175 | 176 | ``` 177 | cd ~/px4-offboard 178 | source ../px4_ros_com_ws/install/setup.bash 179 | export ROS_DOMAIN_ID=0 180 | export PYTHONOPTIMIZE=1 181 | ros2 topic list 182 | ``` 183 | 184 | You should see a list of topics that match the ones sent from Gazebo.: 185 | 186 | ``` 187 | /fmu/in/obstacle_distance 188 | /fmu/in/offboard_control_mode 189 | /fmu/in/onboard_computer_status 190 | /fmu/in/sensor_optical_flow 191 | /fmu/in/telemetry_status 192 | /fmu/in/trajectory_setpoint 193 | /fmu/in/vehicle_attitude_setpoint 194 | /fmu/in/vehicle_command 195 | /fmu/in/vehicle_mocap_odometry 196 | /fmu/in/vehicle_rates_setpoint 197 | /fmu/in/vehicle_trajectory_bezier 198 | /fmu/in/vehicle_trajectory_waypoint 199 | /fmu/in/vehicle_visual_odometry 200 | /fmu/out/failsafe_flags 201 | /fmu/out/sensor_combined 202 | /fmu/out/timesync_status 203 | /fmu/out/vehicle_attitude 204 | /fmu/out/vehicle_control_mode 205 | /fmu/out/vehicle_global_position 206 | /fmu/out/vehicle_gps_position 207 | /fmu/out/vehicle_local_position 208 | /fmu/out/vehicle_odometry 209 | /fmu/out/vehicle_status 210 | /parameter_events 211 | /rosout 212 | ``` 213 | 214 | If you do not see the topics: 215 | 216 | * Check that `ROS_DOMAIN_ID=0` in all the terminals 217 | 218 | Let's echo a topic: 219 | 220 | ``` 221 | ros2 topic echo /fmu/out/vehicle_odometry 222 | ``` 223 | 224 | The terminal should echo some rapidly updating details about the simulated drone. If you look at position, you can see it matches the height of our drone: 225 | 226 | ``` 227 | timestamp: 1680367557988752 228 | timestamp_sample: 1680367557988752 229 | pose_frame: 1 230 | position: 231 | - 0.023736324161291122 232 | - -0.007955201901495457 233 | - -9.922133445739746 234 | q: 235 | - 0.6887969374656677 236 | - 0.002538114320486784 237 | - -0.007746106944978237 238 | - 0.7249085307121277 239 | velocity_frame: 1 240 | velocity: 241 | - -0.007310825865715742 242 | - -0.032901208847761154 243 | - 0.010859847068786621 244 | angular_velocity: 245 | - -0.0012153536081314087 246 | - -0.01004723273217678 247 | - -0.0018821069970726967 248 | position_variance: 249 | - 0.12015653401613235 250 | - 0.1201564222574234 251 | - 0.11353325098752975 252 | orientation_variance: 253 | - 0.00042706530075520277 254 | - 0.00021347538859117776 255 | - 0.000316519319312647 256 | velocity_variance: 257 | - 0.014987492933869362 258 | - 0.01498822309076786 259 | - 0.014155800454318523 260 | reset_counter: 7 261 | quality: 0 262 | ``` 263 | 264 | *In this case the `-9.922133445739746` value indicates the drone in the gazebo sim is in the air.* 265 | 266 | Now that we verfied the DDS-ROS subscription communication link, we can start the demo 267 | 268 | CTRL-C to stop the topic echo and then: 269 | 270 | ``` 271 | source ../px4_ros_com_ws/install/setup.bash 272 | source install/setup.bash 273 | ``` 274 | 275 | The first source reensures the dependencies are loaded for the demo. 276 | The second is for the demo itself. 277 | 278 | Now launch the demo: 279 | 280 | ``` 281 | ros2 launch px4_offboard offboard_position_control.launch.py 282 | ``` 283 | 284 | If things work, the demo should immediately launch an RViz window with the 3D axis indicator (red, green blue color) at the top of the window above the grid. This indicates the drone's position in the gazebo sim/ 285 | 286 | The terminal should show: 287 | 288 | ``` 289 | [INFO] [launch]: All log files can be found below /home/analyst/.ros/log/2023-04-01-16-50-55-835808-casa-32775 290 | [INFO] [launch]: Default logging verbosity is set to INFO 291 | [INFO] [visualizer-1]: process started with pid [32776] 292 | [INFO] [offboard_control-2]: process started with pid [32778] 293 | [INFO] [rviz2-3]: process started with pid [32780] 294 | [rviz2-3] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway. 295 | [offboard_control-2] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT is deprecated. Use ReliabilityPolicy.BEST_EFFORT instead. 296 | [offboard_control-2] warnings.warn( 297 | [offboard_control-2] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead. 298 | [offboard_control-2] warnings.warn( 299 | [offboard_control-2] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST is deprecated. Use HistoryPolicy.KEEP_LAST instead. 300 | [offboard_control-2] warnings.warn( 301 | [rviz2-3] [INFO] [1680367856.767339081] [rviz2]: Stereo is NOT SUPPORTED 302 | [rviz2-3] [INFO] [1680367856.767961910] [rviz2]: OpenGl version: 4.5 (GLSL 4.5) 303 | [visualizer-1] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT is deprecated. Use ReliabilityPolicy.BEST_EFFORT instead. 304 | [visualizer-1] warnings.warn( 305 | [visualizer-1] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST is deprecated. Use HistoryPolicy.KEEP_LAST instead. 306 | [visualizer-1] warnings.warn( 307 | [rviz2-3] [INFO] [1680367856.847242023] [rviz2]: Stereo is NOT SUPPORTED 308 | ``` 309 | 310 | Now head back to QGroundControl and enable offboard control. Click the current mode "HOLD" in upper left, then in the menu, select "Offboard": 311 | 312 | After a 1-2 sec pause, the demo should take control and you should see the 3d indicator in Rviz drawing circles. 313 | -------------------------------------------------------------------------------- /launch/offboard_hardware_position_control.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ############################################################################ 3 | # 4 | # Copyright (C) 2022 PX4 Development Team. All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # 2. Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in 14 | # the documentation and/or other materials provided with the 15 | # distribution. 16 | # 3. Neither the name PX4 nor the names of its contributors may be 17 | # used to endorse or promote products derived from this software 18 | # without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ############################################################################ 34 | 35 | __author__ = "Jaeyoung Lim" 36 | __contact__ = "jalim@ethz.ch" 37 | 38 | from launch import LaunchDescription 39 | from launch_ros.actions import Node 40 | from ament_index_python.packages import get_package_share_directory 41 | import os 42 | 43 | 44 | def generate_launch_description(): 45 | package_dir = get_package_share_directory('px4_offboard') 46 | return LaunchDescription([ 47 | Node( 48 | package='px4_offboard', 49 | namespace='px4_offboard', 50 | executable='offboard_control', 51 | name='control', 52 | parameters= [{'radius': 2.0},{'altitude': 3.0},{'omega': 0.5}] 53 | ) 54 | ]) 55 | -------------------------------------------------------------------------------- /launch/offboard_position_control.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ############################################################################ 3 | # 4 | # Copyright (C) 2022 PX4 Development Team. All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # 2. Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in 14 | # the documentation and/or other materials provided with the 15 | # distribution. 16 | # 3. Neither the name PX4 nor the names of its contributors may be 17 | # used to endorse or promote products derived from this software 18 | # without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ############################################################################ 34 | 35 | __author__ = "Jaeyoung Lim" 36 | __contact__ = "jalim@ethz.ch" 37 | 38 | from launch import LaunchDescription 39 | from launch_ros.actions import Node 40 | from ament_index_python.packages import get_package_share_directory 41 | import os 42 | 43 | 44 | def generate_launch_description(): 45 | package_dir = get_package_share_directory('px4_offboard') 46 | return LaunchDescription([ 47 | Node( 48 | package='px4_offboard', 49 | namespace='px4_offboard', 50 | executable='visualizer', 51 | name='visualizer' 52 | ), 53 | Node( 54 | package='px4_offboard', 55 | namespace='px4_offboard', 56 | executable='offboard_control', 57 | name='control', 58 | parameters= [{'radius': 10.0},{'altitude': 5.0},{'omega': 0.5}] 59 | ), 60 | Node( 61 | package='rviz2', 62 | namespace='', 63 | executable='rviz2', 64 | name='rviz2', 65 | arguments=['-d', [os.path.join(package_dir, 'visualize.rviz')]] 66 | ) 67 | ]) 68 | -------------------------------------------------------------------------------- /launch/visualize.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ############################################################################ 3 | # 4 | # Copyright (C) 2022 PX4 Development Team. All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # 2. Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in 14 | # the documentation and/or other materials provided with the 15 | # distribution. 16 | # 3. Neither the name PX4 nor the names of its contributors may be 17 | # used to endorse or promote products derived from this software 18 | # without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ############################################################################ 34 | 35 | __author__ = "Jaeyoung Lim" 36 | __contact__ = "jalim@ethz.ch" 37 | 38 | from launch import LaunchDescription 39 | from launch_ros.actions import Node 40 | from ament_index_python.packages import get_package_share_directory 41 | import os 42 | 43 | 44 | def generate_launch_description(): 45 | package_dir = get_package_share_directory('px4_offboard') 46 | return LaunchDescription([ 47 | Node( 48 | package='px4_offboard', 49 | namespace='px4_offboard', 50 | executable='visualizer', 51 | name='visualizer' 52 | ), 53 | Node( 54 | package='rviz2', 55 | namespace='', 56 | executable='rviz2', 57 | name='rviz2', 58 | arguments=['-d', [os.path.join(package_dir, 'visualize.rviz')]] 59 | ) 60 | ]) 61 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | px4_offboard 5 | 0.0.0 6 | ROS2 python interface for px4 7 | jaeyoung 8 | BSD-3 9 | 10 | px4_msgs 11 | ros2launch 12 | ament_copyright 13 | ament_flake8 14 | ament_pep257 15 | python3-pytest 16 | 17 | 18 | ament_python 19 | 20 | 21 | -------------------------------------------------------------------------------- /px4_offboard/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaeyoung-Lim/px4-offboard/821f091699ceffa5224b0a4a02de13d6e960aed1/px4_offboard/__init__.py -------------------------------------------------------------------------------- /px4_offboard/offboard_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ############################################################################ 3 | # 4 | # Copyright (C) 2022 PX4 Development Team. All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # 2. Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in 14 | # the documentation and/or other materials provided with the 15 | # distribution. 16 | # 3. Neither the name PX4 nor the names of its contributors may be 17 | # used to endorse or promote products derived from this software 18 | # without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ############################################################################ 34 | 35 | __author__ = "Jaeyoung Lim" 36 | __contact__ = "jalim@ethz.ch" 37 | 38 | import rclpy 39 | import numpy as np 40 | from rclpy.node import Node 41 | from rclpy.clock import Clock 42 | from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy 43 | 44 | from px4_msgs.msg import OffboardControlMode 45 | from px4_msgs.msg import TrajectorySetpoint 46 | from px4_msgs.msg import VehicleStatus 47 | 48 | 49 | class OffboardControl(Node): 50 | 51 | def __init__(self): 52 | super().__init__('minimal_publisher') 53 | qos_profile = QoSProfile( 54 | reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, 55 | durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 56 | history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, 57 | depth=1 58 | ) 59 | 60 | self.status_sub = self.create_subscription( 61 | VehicleStatus, 62 | '/fmu/out/vehicle_status_v1', 63 | self.vehicle_status_callback, 64 | qos_profile) 65 | self.publisher_offboard_mode = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile) 66 | self.publisher_trajectory = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile) 67 | timer_period = 0.02 # seconds 68 | self.timer = self.create_timer(timer_period, self.cmdloop_callback) 69 | self.dt = timer_period 70 | self.declare_parameter('radius', 10.0) 71 | self.declare_parameter('omega', 5.0) 72 | self.declare_parameter('altitude', 5.0) 73 | self.nav_state = VehicleStatus.NAVIGATION_STATE_MAX 74 | self.arming_state = VehicleStatus.ARMING_STATE_DISARMED 75 | # Note: no parameter callbacks are used to prevent sudden inflight changes of radii and omega 76 | # which would result in large discontinuities in setpoints 77 | self.theta = 0.0 78 | self.radius = self.get_parameter('radius').value 79 | self.omega = self.get_parameter('omega').value 80 | self.altitude = self.get_parameter('altitude').value 81 | 82 | def vehicle_status_callback(self, msg): 83 | # TODO: handle NED->ENU transformation 84 | print("NAV_STATUS: ", msg.nav_state) 85 | print(" - offboard status: ", VehicleStatus.NAVIGATION_STATE_OFFBOARD) 86 | self.nav_state = msg.nav_state 87 | self.arming_state = msg.arming_state 88 | 89 | def cmdloop_callback(self): 90 | # Publish offboard control modes 91 | offboard_msg = OffboardControlMode() 92 | offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000) 93 | offboard_msg.position=True 94 | offboard_msg.velocity=False 95 | offboard_msg.acceleration=False 96 | self.publisher_offboard_mode.publish(offboard_msg) 97 | if (self.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD and self.arming_state == VehicleStatus.ARMING_STATE_ARMED): 98 | 99 | trajectory_msg = TrajectorySetpoint() 100 | trajectory_msg.position[0] = self.radius * np.cos(self.theta) 101 | trajectory_msg.position[1] = self.radius * np.sin(self.theta) 102 | trajectory_msg.position[2] = -self.altitude 103 | self.publisher_trajectory.publish(trajectory_msg) 104 | 105 | self.theta = self.theta + self.omega * self.dt 106 | 107 | 108 | def main(args=None): 109 | rclpy.init(args=args) 110 | 111 | offboard_control = OffboardControl() 112 | 113 | rclpy.spin(offboard_control) 114 | 115 | offboard_control.destroy_node() 116 | rclpy.shutdown() 117 | 118 | 119 | if __name__ == '__main__': 120 | main() 121 | -------------------------------------------------------------------------------- /px4_offboard/visualizer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ############################################################################ 3 | # 4 | # Copyright (C) 2022 PX4 Development Team. All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # 2. Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in 14 | # the documentation and/or other materials provided with the 15 | # distribution. 16 | # 3. Neither the name PX4 nor the names of its contributors may be 17 | # used to endorse or promote products derived from this software 18 | # without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ############################################################################ 34 | 35 | __author__ = "Jaeyoung Lim" 36 | __contact__ = "jalim@ethz.ch" 37 | 38 | from re import M 39 | import numpy as np 40 | 41 | import rclpy 42 | from rclpy.node import Node 43 | from rclpy.clock import Clock 44 | from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy 45 | 46 | from px4_msgs.msg import VehicleAttitude 47 | from px4_msgs.msg import VehicleLocalPosition 48 | from px4_msgs.msg import TrajectorySetpoint 49 | from geometry_msgs.msg import PoseStamped, Point 50 | from nav_msgs.msg import Path 51 | from visualization_msgs.msg import Marker 52 | 53 | 54 | def vector2PoseMsg(frame_id, position, attitude): 55 | pose_msg = PoseStamped() 56 | # msg.header.stamp = Clock().now().nanoseconds / 1000 57 | pose_msg.header.frame_id = frame_id 58 | pose_msg.pose.orientation.w = attitude[0] 59 | pose_msg.pose.orientation.x = attitude[1] 60 | pose_msg.pose.orientation.y = attitude[2] 61 | pose_msg.pose.orientation.z = attitude[3] 62 | pose_msg.pose.position.x = position[0] 63 | pose_msg.pose.position.y = position[1] 64 | pose_msg.pose.position.z = position[2] 65 | return pose_msg 66 | 67 | 68 | class PX4Visualizer(Node): 69 | def __init__(self): 70 | super().__init__("visualizer") 71 | 72 | # Configure subscritpions 73 | qos_profile = QoSProfile( 74 | reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, 75 | history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, 76 | depth=1, 77 | ) 78 | 79 | self.attitude_sub = self.create_subscription( 80 | VehicleAttitude, 81 | "/fmu/out/vehicle_attitude", 82 | self.vehicle_attitude_callback, 83 | qos_profile, 84 | ) 85 | self.local_position_sub = self.create_subscription( 86 | VehicleLocalPosition, 87 | "/fmu/out/vehicle_local_position", 88 | self.vehicle_local_position_callback, 89 | qos_profile, 90 | ) 91 | self.setpoint_sub = self.create_subscription( 92 | TrajectorySetpoint, 93 | "/fmu/in/trajectory_setpoint", 94 | self.trajectory_setpoint_callback, 95 | qos_profile, 96 | ) 97 | 98 | self.vehicle_pose_pub = self.create_publisher( 99 | PoseStamped, "/px4_visualizer/vehicle_pose", 10 100 | ) 101 | self.vehicle_vel_pub = self.create_publisher( 102 | Marker, "/px4_visualizer/vehicle_velocity", 10 103 | ) 104 | self.vehicle_path_pub = self.create_publisher( 105 | Path, "/px4_visualizer/vehicle_path", 10 106 | ) 107 | self.setpoint_path_pub = self.create_publisher( 108 | Path, "/px4_visualizer/setpoint_path", 10 109 | ) 110 | 111 | self.vehicle_attitude = np.array([1.0, 0.0, 0.0, 0.0]) 112 | self.vehicle_local_position = np.array([0.0, 0.0, 0.0]) 113 | self.vehicle_local_velocity = np.array([0.0, 0.0, 0.0]) 114 | self.setpoint_position = np.array([0.0, 0.0, 0.0]) 115 | self.vehicle_path_msg = Path() 116 | self.setpoint_path_msg = Path() 117 | 118 | # trail size 119 | self.trail_size = 1000 120 | 121 | # time stamp for the last local position update received on ROS2 topic 122 | self.last_local_pos_update = 0.0 123 | # time after which existing path is cleared upon receiving new 124 | # local position ROS2 message 125 | self.declare_parameter("path_clearing_timeout", -1.0) 126 | 127 | timer_period = 0.05 # seconds 128 | self.timer = self.create_timer(timer_period, self.cmdloop_callback) 129 | 130 | def vehicle_attitude_callback(self, msg): 131 | # TODO: handle NED->ENU transformation 132 | self.vehicle_attitude[0] = msg.q[0] 133 | self.vehicle_attitude[1] = msg.q[1] 134 | self.vehicle_attitude[2] = -msg.q[2] 135 | self.vehicle_attitude[3] = -msg.q[3] 136 | 137 | def vehicle_local_position_callback(self, msg): 138 | path_clearing_timeout = ( 139 | self.get_parameter("path_clearing_timeout") 140 | .get_parameter_value() 141 | .double_value 142 | ) 143 | if path_clearing_timeout >= 0 and ( 144 | (Clock().now().nanoseconds / 1e9 - self.last_local_pos_update) 145 | > path_clearing_timeout 146 | ): 147 | self.vehicle_path_msg.poses.clear() 148 | self.last_local_pos_update = Clock().now().nanoseconds / 1e9 149 | 150 | # TODO: handle NED->ENU transformation 151 | self.vehicle_local_position[0] = msg.x 152 | self.vehicle_local_position[1] = -msg.y 153 | self.vehicle_local_position[2] = -msg.z 154 | self.vehicle_local_velocity[0] = msg.vx 155 | self.vehicle_local_velocity[1] = -msg.vy 156 | self.vehicle_local_velocity[2] = -msg.vz 157 | 158 | def trajectory_setpoint_callback(self, msg): 159 | self.setpoint_position[0] = msg.position[0] 160 | self.setpoint_position[1] = -msg.position[1] 161 | self.setpoint_position[2] = -msg.position[2] 162 | 163 | def create_arrow_marker(self, id, tail, vector): 164 | msg = Marker() 165 | msg.action = Marker.ADD 166 | msg.header.frame_id = "map" 167 | # msg.header.stamp = Clock().now().nanoseconds / 1000 168 | msg.ns = "arrow" 169 | msg.id = id 170 | msg.type = Marker.ARROW 171 | msg.scale.x = 0.1 172 | msg.scale.y = 0.2 173 | msg.scale.z = 0.0 174 | msg.color.r = 0.5 175 | msg.color.g = 0.5 176 | msg.color.b = 0.0 177 | msg.color.a = 1.0 178 | dt = 0.3 179 | tail_point = Point() 180 | tail_point.x = tail[0] 181 | tail_point.y = tail[1] 182 | tail_point.z = tail[2] 183 | head_point = Point() 184 | head_point.x = tail[0] + dt * vector[0] 185 | head_point.y = tail[1] + dt * vector[1] 186 | head_point.z = tail[2] + dt * vector[2] 187 | msg.points = [tail_point, head_point] 188 | return msg 189 | 190 | def append_vehicle_path(self, msg): 191 | self.vehicle_path_msg.poses.append(msg) 192 | if len(self.vehicle_path_msg.poses) > self.trail_size: 193 | del self.vehicle_path_msg.poses[0] 194 | 195 | def append_setpoint_path(self, msg): 196 | self.setpoint_path_msg.poses.append(msg) 197 | if len(self.setpoint_path_msg.poses) > self.trail_size: 198 | del self.setpoint_path_msg.poses[0] 199 | 200 | def cmdloop_callback(self): 201 | vehicle_pose_msg = vector2PoseMsg( 202 | "map", self.vehicle_local_position, self.vehicle_attitude 203 | ) 204 | self.vehicle_pose_pub.publish(vehicle_pose_msg) 205 | 206 | # Publish time history of the vehicle path 207 | self.vehicle_path_msg.header = vehicle_pose_msg.header 208 | self.append_vehicle_path(vehicle_pose_msg) 209 | self.vehicle_path_pub.publish(self.vehicle_path_msg) 210 | 211 | # Publish time history of the vehicle path 212 | setpoint_pose_msg = vector2PoseMsg("map", self.setpoint_position, self.vehicle_attitude) 213 | self.setpoint_path_msg.header = setpoint_pose_msg.header 214 | self.append_setpoint_path(setpoint_pose_msg) 215 | self.setpoint_path_pub.publish(self.setpoint_path_msg) 216 | 217 | # Publish arrow markers for velocity 218 | velocity_msg = self.create_arrow_marker(1, self.vehicle_local_position, self.vehicle_local_velocity) 219 | self.vehicle_vel_pub.publish(velocity_msg) 220 | 221 | 222 | def main(args=None): 223 | rclpy.init(args=args) 224 | 225 | px4_visualizer = PX4Visualizer() 226 | 227 | rclpy.spin(px4_visualizer) 228 | 229 | px4_visualizer.destroy_node() 230 | rclpy.shutdown() 231 | 232 | 233 | if __name__ == "__main__": 234 | main() 235 | -------------------------------------------------------------------------------- /resource/px4_offboard: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaeyoung-Lim/px4-offboard/821f091699ceffa5224b0a4a02de13d6e960aed1/resource/px4_offboard -------------------------------------------------------------------------------- /resource/visualize.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Pose1 10 | - /Marker1 11 | - /Marker1/Topic1 12 | - /Path1 13 | - /Path2 14 | Splitter Ratio: 0.5 15 | Tree Height: 907 16 | - Class: rviz_common/Selection 17 | Name: Selection 18 | - Class: rviz_common/Tool Properties 19 | Expanded: 20 | - /2D Goal Pose1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz_common/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 1 51 | Axes Length: 1 52 | Axes Radius: 0.10000000149011612 53 | Class: rviz_default_plugins/Pose 54 | Color: 255; 25; 0 55 | Enabled: true 56 | Head Length: 0.30000001192092896 57 | Head Radius: 0.10000000149011612 58 | Name: Pose 59 | Shaft Length: 1 60 | Shaft Radius: 0.05000000074505806 61 | Shape: Axes 62 | Topic: 63 | Depth: 5 64 | Durability Policy: Volatile 65 | History Policy: Keep Last 66 | Reliability Policy: Reliable 67 | Value: /px4_visualizer/vehicle_pose 68 | Value: true 69 | - Class: rviz_default_plugins/Marker 70 | Enabled: true 71 | Name: Marker 72 | Namespaces: 73 | arrow: true 74 | Topic: 75 | Depth: 5 76 | Durability Policy: Volatile 77 | History Policy: Keep Last 78 | Reliability Policy: Reliable 79 | Value: /px4_visualizer/vehicle_velocity 80 | Value: true 81 | - Alpha: 1 82 | Buffer Length: 1 83 | Class: rviz_default_plugins/Path 84 | Color: 25; 255; 0 85 | Enabled: true 86 | Head Diameter: 0.30000001192092896 87 | Head Length: 0.20000000298023224 88 | Length: 0.30000001192092896 89 | Line Style: Lines 90 | Line Width: 0.029999999329447746 91 | Name: Path 92 | Offset: 93 | X: 0 94 | Y: 0 95 | Z: 0 96 | Pose Color: 255; 85; 255 97 | Pose Style: None 98 | Radius: 0.029999999329447746 99 | Shaft Diameter: 0.10000000149011612 100 | Shaft Length: 0.10000000149011612 101 | Topic: 102 | Depth: 5 103 | Durability Policy: Volatile 104 | History Policy: Keep Last 105 | Reliability Policy: Reliable 106 | Value: /px4_visualizer/vehicle_path 107 | Value: true 108 | - Alpha: 1 109 | Buffer Length: 1 110 | Class: rviz_default_plugins/Path 111 | Color: 25; 0; 255 112 | Enabled: true 113 | Head Diameter: 0.30000001192092896 114 | Head Length: 0.20000000298023224 115 | Length: 0.30000001192092896 116 | Line Style: Lines 117 | Line Width: 0.029999999329447746 118 | Name: Path 119 | Offset: 120 | X: 0 121 | Y: 0 122 | Z: 0 123 | Pose Color: 255; 85; 255 124 | Pose Style: None 125 | Radius: 0.029999999329447746 126 | Shaft Diameter: 0.10000000149011612 127 | Shaft Length: 0.10000000149011612 128 | Topic: 129 | Depth: 5 130 | Durability Policy: Volatile 131 | History Policy: Keep Last 132 | Reliability Policy: Reliable 133 | Value: /px4_visualizer/setpoint_path 134 | Value: true 135 | Enabled: true 136 | Global Options: 137 | Background Color: 255; 255; 255 138 | Fixed Frame: map 139 | Frame Rate: 30 140 | Name: root 141 | Tools: 142 | - Class: rviz_default_plugins/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz_default_plugins/MoveCamera 145 | - Class: rviz_default_plugins/Select 146 | - Class: rviz_default_plugins/FocusCamera 147 | - Class: rviz_default_plugins/Measure 148 | Line color: 128; 128; 0 149 | - Class: rviz_default_plugins/SetInitialPose 150 | Topic: 151 | Depth: 5 152 | Durability Policy: Volatile 153 | History Policy: Keep Last 154 | Reliability Policy: Reliable 155 | Value: /initialpose 156 | - Class: rviz_default_plugins/SetGoal 157 | Topic: 158 | Depth: 5 159 | Durability Policy: Volatile 160 | History Policy: Keep Last 161 | Reliability Policy: Reliable 162 | Value: /goal_pose 163 | - Class: rviz_default_plugins/PublishPoint 164 | Single click: true 165 | Topic: 166 | Depth: 5 167 | Durability Policy: Volatile 168 | History Policy: Keep Last 169 | Reliability Policy: Reliable 170 | Value: /clicked_point 171 | Transformation: 172 | Current: 173 | Class: rviz_default_plugins/TF 174 | Value: true 175 | Views: 176 | Current: 177 | Class: rviz_default_plugins/Orbit 178 | Distance: 24.51659393310547 179 | Enable Stereo Rendering: 180 | Stereo Eye Separation: 0.05999999865889549 181 | Stereo Focal Distance: 1 182 | Swap Stereo Eyes: false 183 | Value: false 184 | Focal Point: 185 | X: 0 186 | Y: 0 187 | Z: 0 188 | Focal Shape Fixed Size: true 189 | Focal Shape Size: 0.05000000074505806 190 | Invert Z Axis: false 191 | Name: Current View 192 | Near Clip Distance: 0.009999999776482582 193 | Pitch: 0.795397937297821 194 | Target Frame: 195 | Value: Orbit (rviz) 196 | Yaw: 0.6553981304168701 197 | Saved: ~ 198 | Window Geometry: 199 | Displays: 200 | collapsed: true 201 | Height: 1136 202 | Hide Left Dock: true 203 | Hide Right Dock: true 204 | QMainWindow State: 000000ff00000000fd00000004000000000000028e00000416fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000416000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039c0000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 205 | Selection: 206 | collapsed: false 207 | Tool Properties: 208 | collapsed: false 209 | Views: 210 | collapsed: true 211 | Width: 924 212 | X: 72 213 | Y: 27 214 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/px4_offboard 3 | [install] 4 | install_scripts=$base/lib/px4_offboard 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from glob import glob 3 | from setuptools import setup 4 | 5 | package_name = 'px4_offboard' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.0', 10 | packages=[package_name], 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/ament_index/resource_index/packages', 15 | ['resource/' + 'visualize.rviz']), 16 | ('share/' + package_name, ['package.xml']), 17 | (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*')), 18 | (os.path.join('share', package_name), glob('resource/*rviz')) 19 | ], 20 | install_requires=['setuptools'], 21 | zip_safe=True, 22 | maintainer='jaeyoung', 23 | maintainer_email='jalim@ethz.ch', 24 | description='TODO: Package description', 25 | license='TODO: License declaration', 26 | tests_require=['pytest'], 27 | entry_points={ 28 | 'console_scripts': [ 29 | 'offboard_control = px4_offboard.offboard_control:main', 30 | 'visualizer = px4_offboard.visualizer:main', 31 | ], 32 | }, 33 | ) 34 | -------------------------------------------------------------------------------- /test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | --------------------------------------------------------------------------------