diff --git a/.gitignore b/.gitignore index 067c81d3..2bcbb107 100644 --- a/.gitignore +++ b/.gitignore @@ -16,6 +16,9 @@ msg/_*.py build_isolated/ devel_isolated/ +__pycache__/ +.ipynb_checkpoints/ + # Generated by dynamic reconfigure *.cfgc /cfg/cpp/ @@ -55,4 +58,4 @@ frames_*.pdf nucleus_token.txt -docker/extras/px \ No newline at end of file +docker/extras/px diff --git a/.gitmodules b/.gitmodules index 4b1aaa26..19ff9220 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,8 +10,11 @@ url = git@github.com:PX4/px4_msgs.git branch = main [submodule "ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping"] - path = ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping + path = ros_ws/src/robot/autonomy/4_global/a_world_models/vdb_mapping url = https://github.com/fzi-forschungszentrum-informatik/vdb_mapping [submodule "ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping_ros2"] - path = ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping_ros2 + path = ros_ws/src/robot/autonomy/4_global/a_world_models/vdb_mapping_ros2 url = https://github.com/fzi-forschungszentrum-informatik/vdb_mapping_ros2 +[submodule "ros_ws/src/robot/autonomy/1_sensors/zed-ros2-wrapper"] + path = ros_ws/src/robot/autonomy/1_sensors/zed-ros2-wrapper + url = https://github.com/stereolabs/zed-ros2-wrapper.git diff --git a/docker/.bashrc b/docker/.bashrc index 0b4b55bb..4e8b7575 100644 --- a/docker/.bashrc +++ b/docker/.bashrc @@ -141,6 +141,8 @@ function cws(){ echo "Cleaning ROS2 workspace..." set -x rm -rf "$ROS2_WS_DIR"/build/ "$ROS2_WS_DIR"/install/ "$ROS2_WS_DIR"/log/ + export AMENT_PREFIX_PATH="/opt/ros/humble" + export CMAKE_PREFIX_PATH="" { set +x; } 2>/dev/null # set +x w/out it being printed echo "ROS2 workspace has been cleaned." else diff --git a/docs/user_guide/getting_started.md b/docs/user_guide/getting_started.md index 25499464..f8e53637 100644 --- a/docs/user_guide/getting_started.md +++ b/docs/user_guide/getting_started.md @@ -1,4 +1,5 @@ # Getting Started + Welcome to the AirLab Autonomy Stack. By the end of this guide, you will have the autonomy stack running on your machine. ## Requirements @@ -8,32 +9,38 @@ You need at least 25GB free to install the Docker image. Have an NVIDIA GPU >= RTX 3070 to run Isaac Sim locally. ## Setup + ### Clone -``` + +```bash git clone --recursive -j8 git@github.com:castacks/AirStack.git ``` ### Omniverse + Install the Omniverse launcher download from this link: -``` +```bash + wget https://install.launcher.omniverse.nvidia.com/installers/omniverse-launcher-linux.AppImage ``` -Follow these instructions to setup Nucleus : https://airlab.slite.com/app/docs/X8dZ8w5S3GP9tw +Follow these instructions to setup Nucleus : [https://airlab.slite.com/app/docs/X8dZ8w5S3GP9tw](https://airlab.slite.com/app/docs/X8dZ8w5S3GP9tw) ### SITL + If you are using the Ascent Spirit drone download the SITL software packages from this link: -https://drive.google.com/file/d/1UxgezaTrHe4WJ28zsVeRhv1VYfOU5VK8/view?usp=drive_link +[https://drive.google.com/file/d/1UxgezaTrHe4WJ28zsVeRhv1VYfOU5VK8/view?usp=drive_link](https://drive.google.com/file/d/1UxgezaTrHe4WJ28zsVeRhv1VYfOU5VK8/view?usp=drive_link) Then unzip the file AscentAeroSystemsSITLPackage.zip in this folder: -``` +```bash cd AirStack/simulation/AscentAeroSystems unzip ~/Downloads/AscentAeroSystemsSITLPackage.zip -d . ``` ### Docker + - Install [Docker Desktop](https://docs.docker.com/desktop/install/ubuntu/). This should come installed with docker compose. - Gain access to NVIDIA NGC Containers by following [these instructions](https://docs.nvidia.com/launchpad/ai/base-command-coe/latest/bc-coe-docker-basics-step-02.html) @@ -89,3 +96,104 @@ ros2 topic pub /controls/mavros/setpoint_position/local geometry_msgs/PoseStampe "{ header: { stamp: { sec: 0, nanosec: 0 }, frame_id: 'base_link' }, \ pose: { position: { x: 10.0, y: 0.0, z: 20.0 }, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } } }" -1 ``` + +## Setup Storage Tools Server Locally + +### Clone and install + +``` bash +git clone https://github.com/castacks/storage_tools_server +cd storage_tools_server +python -m venv venv +. venv/bin/activate +pip install -r requirements.txt +``` + +### Configure + +Edit the `config/config.yaml` file to match your configuration. + +### REQUIRED UPDATES + +- `upload_dir` is the location for uploads. This must be readable and writeable by the user running the Server. +- `volume_root` sets the prefix for all entries in the `volume_map`. This must be readable and writeable by the user running the Server. + +### Set Environment and Run + +- `CONFIG` is the full path to the `config.yaml` in use. By default, the app will use `$PWD/config/config.yaml` +- `PORT` is the same port as define in the optional setup. The default is 8091. + +``` bash +export CONFIG=$PWD/config/config.yaml +export PORT=8091 + +gunicorn -k gevent -w 1 -b "0.0.0.0:${PORT}" --timeout 120 "server.app:app" +``` + +Open a web browser to [http://localhost:8091](http://localhost:8091) (or the PORT you set). The default user is `admin` and the default password is `NodeNodeDevices`. + +### Create an API Key for your robot + +- Log into the Server +- Go to Configure -> Keys +- Enter a name for the device key in the "Add a new key name" field. +- Click "Generate Key" + +## Set up Storage Tools Device on your Robot + +### Install Requirements + +- [Docker Compose](https://docs.docker.com/compose/install/standalone/) + +### Clone Device Repo + +```bash +cd /opt +git clone https://github.com/castacks/storage_tools_device +cd stroage_tools_device +``` + +### Update the config.env + +Update the `config.env` to match your system. + +- Required + - `DATA_DIR`. This is the top level data directory that all of the `watch` dirs share. For example, if you `watch` directories are `/mnt/data/processor_1` and `/mnt/data/processor_2`, set the `DATA_DIR` to `/mnt/data`. +- Optional + - `CONFIG_PORT`. The HTTP port to edit the configuration. Navigate to http://YOUR_DEVICE_IP:CONFIG_PORT to edit the configurations and view online status. The default port is 8811. + +### Build and Run + +This sets up the environment and configures the image to start on boot. + +``` bash +cd /opt/storage_tools_device + +# run in foreground +docker compose --env-file config.env up --build --remove-orphans + +# run in background +docker compose --env-file config.env up --build --remove-orphans -d +``` + +### Configure the Device + +Navigate to http://YOUR_DEVICE_IP:CONFIG_PORT to edit the configurations and view online status. The default port is 8811. + +### Device Configuration + +You must update all *Required* fields. + +- **Project Name**: Name of the project. If this is empty, the server will ask you to fill in the name. +- **Robot Name**: (Required) Name of this robot. This should be unique within the project. +- **API Key Token**: (Required) The API_KEY_TOKEN for this robot. Your admin can provide this to you. If you are running your own upload server, it is set in the Config->Keys page. +- **Watch**: (Required) The list of directories to be watched. Any file with a matching suffix (see Include Suffix) will be uploaded. These all must be in the same subdirectory as `DATA_DIR` from the `config.env` file. +- **Servers**: List of potential servers. If this is empty, the Device will search for servers using ZeroConf. +- **Local Time Zone**: The time zone that the logs were recorded in. +- **Threads**: The number of threads used for uploading from this device to the server. +- **Include Suffix**: The list of included suffixes. Any file in the Watch directory (see above) that matches one of these suffixes will be uploaded to the server. +- **Watch Interval**: How long to wait in seconds before attempting to connect to server again. + +Press [Save] to commit the changes to the device. + +Press [Refresh] to refesh the page with the on device settings. diff --git a/ros_ws/src/airstack_common/include/airstack_common/ros2_helper.hpp b/ros_ws/src/airstack_common/include/airstack_common/ros2_helper.hpp index bfa13dd1..a1131a92 100644 --- a/ros_ws/src/airstack_common/include/airstack_common/ros2_helper.hpp +++ b/ros_ws/src/airstack_common/include/airstack_common/ros2_helper.hpp @@ -16,7 +16,10 @@ namespace airstack { template <> inline int get_param(rclcpp::Node* node, std::string name, int default_value, bool* set){ - node->declare_parameter(name, rclcpp::PARAMETER_INTEGER); + try{ + node->declare_parameter(name, rclcpp::PARAMETER_INTEGER); + } + catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){} rclcpp::Parameter param; bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value)); if(set != NULL) @@ -26,7 +29,10 @@ namespace airstack { template <> inline double get_param(rclcpp::Node* node, std::string name, double default_value, bool* set){ - node->declare_parameter(name, rclcpp::PARAMETER_DOUBLE); + try{ + node->declare_parameter(name, rclcpp::PARAMETER_DOUBLE); + } + catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){} rclcpp::Parameter param; bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value)); if(set != NULL) @@ -36,7 +42,10 @@ namespace airstack { template <> inline std::string get_param(rclcpp::Node* node, std::string name, std::string default_value, bool* set){ - node->declare_parameter(name, rclcpp::PARAMETER_STRING); + try{ + node->declare_parameter(name, rclcpp::PARAMETER_STRING); + } + catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){} rclcpp::Parameter param; bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value)); if(set != NULL) @@ -46,7 +55,10 @@ namespace airstack { template <> inline bool get_param(rclcpp::Node* node, std::string name, bool default_value, bool* set){ - node->declare_parameter(name, rclcpp::PARAMETER_BOOL); + try{ + node->declare_parameter(name, rclcpp::PARAMETER_BOOL); + } + catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){} rclcpp::Parameter param; bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value)); if(set != NULL) diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/COLCON_IGNORE b/ros_ws/src/robot/autonomy/1_sensors/COLCON_IGNORE similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/base_main_class/COLCON_IGNORE rename to ros_ws/src/robot/autonomy/1_sensors/COLCON_IGNORE diff --git a/ros_ws/src/robot/autonomy/1_sensors/zed-ros2-wrapper b/ros_ws/src/robot/autonomy/1_sensors/zed-ros2-wrapper new file mode 160000 index 00000000..baad42b0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/1_sensors/zed-ros2-wrapper @@ -0,0 +1 @@ +Subproject commit baad42b042370732f5e8d3639753c561db7c1e30 diff --git a/ros_ws/src/robot/autonomy/controls/mavros_interface/include/mavros_interface/.gitkeep b/ros_ws/src/robot/autonomy/2_perception/.gitkeep similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mavros_interface/include/mavros_interface/.gitkeep rename to ros_ws/src/robot/autonomy/2_perception/.gitkeep diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/LICENSE b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/LICENSE similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/LICENSE rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/LICENSE diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/README.md b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/README.md similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/README.md rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/README.md diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/config/disparity_expansion_params.yaml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/config/disparity_expansion_params.yaml rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.xml rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.xml diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_pcd.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.xml rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_pcd.xml diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/package.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/package.xml rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_conv.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_conv.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_conv.cpp rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_conv.cpp diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion.cpp rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_pcd.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_pcd.cpp rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/include/disparity_graph/disparity_graph.hpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp similarity index 99% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph/include/disparity_graph/disparity_graph.hpp rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp index ce577172..2f829210 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/include/disparity_graph/disparity_graph.hpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp @@ -115,7 +115,7 @@ class DisparityGraph : rclcpp::Node { std::bind(&DisparityGraph::get_cam_info, this, std::placeholders::_1)); } - virtual ~DisparityGraph(); + //virtual ~DisparityGraph(); void disp_cb(const sensor_msgs::msg::Image::ConstSharedPtr &disp_fg, const sensor_msgs::msg::Image::ConstSharedPtr &disp_bg) { diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/package.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph/package.xml rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/src/disparity_graph.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph/src/disparity_graph.cpp rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt new file mode 100644 index 00000000..c9abf61d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.5) +project(disparity_map_representation) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(airstack_common REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(map_representation_interface REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(disparity_graph REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pcl_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(stereo_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) + + +add_library(disparity_map_representation src/disparity_map_representation.cpp) +target_compile_features(disparity_map_representation PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(disparity_map_representation PUBLIC + $ + $) + +ament_target_dependencies( + disparity_map_representation + airstack_common + airstack_msgs + map_representation_interface + cv_bridge + disparity_graph + geometry_msgs + image_geometry + image_transport + nav_msgs + pcl_msgs + pluginlib + rclcpp + rclpy + sensor_msgs + std_msgs + stereo_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs + ) + +pluginlib_export_plugin_description_file(map_representation_interface disparity_map_representation_plugin.xml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +#target_compile_definitions(disparity_map_representation PRIVATE "disparity_map_representation_INTERFACE_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS disparity_map_representation + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + disparity_map_representation +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/disparity_map_representation_plugin.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/disparity_map_representation_plugin.xml new file mode 100644 index 00000000..1513218b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/disparity_map_representation_plugin.xml @@ -0,0 +1,6 @@ + + + Disparity map representation plugin. + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp similarity index 87% rename from ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp index 18829bc6..60506328 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp @@ -1,6 +1,5 @@ #ifndef _DISPARITY_MAP_REPRESENTATION_ #define _DISPARITY_MAP_REPRESENTATION_ -#include #include #include #include @@ -9,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -18,7 +18,8 @@ #include #include -class DisparityMapRepresentation : public MapRepresentation { +namespace disparity_map_representation { +class DisparityMapRepresentation : public map_representation_interface::MapRepresentation { private: std::unique_ptr disp_graph; @@ -45,5 +46,6 @@ class DisparityMapRepresentation : public MapRepresentation { virtual std::vector > get_values( std::vector > trajectories); }; +} // namespace disparity_map_representation #endif diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/package.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/package.xml similarity index 86% rename from ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/package.xml rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/package.xml index 2f4b6dec..2b50fd34 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/package.xml +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/package.xml @@ -12,7 +12,7 @@ airstack_common airstack_msgs - core_map_representation_interface + map_representation_interface cv_bridge disparity_graph geometry_msgs @@ -32,6 +32,5 @@ ament_cmake - - + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/src/disparity_map_representation.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp similarity index 97% rename from ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/src/disparity_map_representation.cpp rename to ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp index bca03044..1ec59963 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/src/disparity_map_representation.cpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp @@ -1,6 +1,5 @@ #include -#include - +namespace disparity_map_representation { DisparityMapRepresentation::DisparityMapRepresentation() : MapRepresentation(), disp_graph(std::make_unique()) { points.ns = "obstacles"; @@ -256,5 +255,8 @@ void DisparityMapRepresentation::publish_debug() { points.points.clear(); points.colors.clear(); } +} // namespace disparity_map_representation +#include -PLUGINLIB_EXPORT_CLASS(DisparityMapRepresentation, MapRepresentation) +PLUGINLIB_EXPORT_CLASS(disparity_map_representation::DisparityMapRepresentation, + map_representation_interface::MapRepresentation) diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt new file mode 100644 index 00000000..42c0ed7b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.5) +project(map_representation_interface) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) + + +add_library(map_representation_interface src/map_representation_interface.cpp) +target_include_directories(map_representation_interface PUBLIC + $ + $) +target_compile_features(map_representation_interface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + map_representation_interface + # Required dependencies + airstack_msgs + geometry_msgs + nav_msgs + pluginlib + rclcpp + rclpy + sensor_msgs + std_msgs + tf2 + tf2_ros + visualization_msgs +) +ament_export_targets(map_representation_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp geometry_msgs tf2 tf2_ros) + +install(TARGETS map_representation_interface + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS map_representation_interface + EXPORT map_representation_interface + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +ament_export_include_directories( + include + ${geometry_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/include/core_map_representation_interface/map_representation.h b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp similarity index 73% rename from ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/include/core_map_representation_interface/map_representation.h rename to ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp index f9b894cb..d46ff9ff 100644 --- a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/include/core_map_representation_interface/map_representation.h +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp @@ -6,14 +6,16 @@ #include #include +namespace map_representation_interface { + class MapRepresentation : public rclcpp::Node { private: public: /** - Takes in a list of trajectories and outputs a value for each waypoint in each trajectory. - @return A vector of vectors containing values for each waypoint. There is a vector of vectors - for each trajectory. There is a vector of doubles for each waypoint within a trajectory. - */ + Takes in a list of trajectories and outputs a value for each waypoint in each trajectory. + @return A vector of vectors containing values for each waypoint. There is a vector of vectors + for each trajectory. There is a vector of doubles for each waypoint within a trajectory. + */ virtual std::vector > get_values( std::vector > @@ -26,12 +28,12 @@ class MapRepresentation : public rclcpp::Node { /** Clears the map. - */ + */ virtual void clear() { RCLCPP_ERROR(this->get_logger(), "clear CALLED BUT NOT IMPLEMENTED"); } /** Use this function to publish visualizations of the map that might be helpful for debugging. - */ + */ virtual void publish_debug() {} virtual ~MapRepresentation() {} @@ -41,4 +43,6 @@ class MapRepresentation : public rclcpp::Node { MapRepresentation() : Node("map_representation") {} }; +} // namespace map_representation_interface + #endif diff --git a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/package.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/package.xml similarity index 85% rename from ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/package.xml rename to ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/package.xml index 52b15ab6..be4abab7 100644 --- a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/package.xml +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/package.xml @@ -1,9 +1,9 @@ - core_map_representation_interface + map_representation_interface 0.0.0 - The core_map_representation_interface package + The map_representation_interface package john TODO @@ -28,4 +28,4 @@ ament_cmake - + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/src/map_representation_interface.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/src/map_representation_interface.cpp new file mode 100644 index 00000000..9ace4abe --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/src/map_representation_interface.cpp @@ -0,0 +1,3 @@ +#include + +int blank() { return 0; } diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt similarity index 95% rename from ros_ws/src/robot/autonomy/planning/local/droan_local_planner/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt index 35b7ffa9..6cf1cd8b 100644 --- a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(airstack_common REQUIRED) find_package(airstack_msgs REQUIRED) -find_package(core_map_representation_interface REQUIRED) +find_package(map_representation_interface REQUIRED) # find_package(disparity_map_representation REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -29,7 +29,7 @@ ament_target_dependencies( droan_local_planner "airstack_msgs" "airstack_common" - "core_map_representation_interface" + "map_representation_interface" # "disparity_map_representation" "trajectory_controller" "trajectory_library" diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml new file mode 100644 index 00000000..ebfea010 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + obstacle_distance_reward: 1.0 + forward_progress_reward_weight: 0.5 + robot_radius: 0.75 + look_past_distance: 2.0 + + # height parameters + height_mode: 0 + height_above_ground: 1.0 + fixed_height: 1.0 + # 0: use the yaw of the subscribed traj, 1: smoothly vary the yaw in the direction of the subscribed trajectory + yaw_mode: 0 + map_representation: PointCloudMapRepresentation + waypoint_buffer_duration: 30.0 + waypoint_angle_threshold: 30.0 + + # trajectory parameters + # trajectory_library_config: "$(find trajectory_library)/config/acceleration_magnitudes.yaml" + # tf_prefix: robot1 + trajectory_library_config: "/root/AirStack/ros_ws/install/trajectory_library/share/trajectory_library/config/acceleration_magnitudes.yaml" + waypoint_spacing: 0.5 + waypoint_spacing_threshold: 0.5 + + # custom waypoint parameters + custom_waypoint_timeout_factor: 0.3 + custom_waypoint_distance_threshold: 0.5 \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp similarity index 72% rename from ros_ws/src/robot/autonomy/planning/local/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp rename to ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp index bf9b7069..f32961d0 100644 --- a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp @@ -1,10 +1,9 @@ #pragma once -#include - #include #include #include +#include #include #include // #include @@ -27,17 +26,17 @@ class DroanLocalPlanner : public rclcpp::Node { private: std::unique_ptr traj_lib; - std::string map_representation; + std::string map_representation_class_string; bool is_global_plan_received; - airstack_msgs::msg::TrajectoryXYZVYaw global_plan; + airstack_msgs::msg::TrajectoryXYZVYaw global_plan_msg; double global_plan_trajectory_distance; bool is_look_ahead_received, is_tracking_point_received; airstack_msgs::msg::Odometry look_ahead_odom, tracking_point_odom; std::vector static_trajectories; - double waypoint_spacing, obstacle_check_radius, obstacle_penalty_weight, - forward_progress_penalty_weight; + double waypoint_spacing, obstacle_check_radius, obstacle_distance_reward, + forward_progress_reward_weight; double robot_radius; int obstacle_check_points; @@ -65,9 +64,7 @@ class DroanLocalPlanner : public rclcpp::Node { GoalMode goal_mode; double custom_waypoint_timeout_factor, custom_waypoint_distance_threshold; - // MapRepresentationDeprecated* map; - // MapRepresentation* pc_map; - std::shared_ptr pc_map; + std::shared_ptr map_representation; rclcpp::Subscription::SharedPtr global_plan_sub; rclcpp::Subscription::SharedPtr waypoint_sub; @@ -141,11 +138,11 @@ class DroanLocalPlanner : public rclcpp::Node { // init parameters this->declare_parameter("waypoint_spacing", 0.5); waypoint_spacing = this->get_parameter("waypoint_spacing").as_double(); - this->declare_parameter("obstacle_penalty_weight", 1.); - obstacle_penalty_weight = this->get_parameter("obstacle_penalty_weight").as_double(); - this->declare_parameter("forward_progress_penalty_weight", 0.5); - forward_progress_penalty_weight = - this->get_parameter("forward_progress_penalty_weight").as_double(); + this->declare_parameter("obstacle_distance_reward", 1.); + obstacle_distance_reward = this->get_parameter("obstacle_distance_reward").as_double(); + this->declare_parameter("forward_progress_reward_weight", 0.5); + forward_progress_reward_weight = + this->get_parameter("forward_progress_reward_weight").as_double(); this->declare_parameter("robot_radius", 0.75); robot_radius = this->get_parameter("robot_radius").as_double(); this->declare_parameter("look_past_distance", 0.0); @@ -159,7 +156,7 @@ class DroanLocalPlanner : public rclcpp::Node { this->declare_parameter("yaw_mode", 0); yaw_mode = this->get_parameter("yaw_mode").as_int(); this->declare_parameter("map_representation", std::string("PointCloudMapRepresentation")); - map_representation = this->get_parameter("map_representation").as_string(); + map_representation_class_string = this->get_parameter("map_representation").as_string(); this->declare_parameter("waypoint_buffer_duration", 30.); waypoint_buffer_duration = this->get_parameter("waypoint_buffer_duration").as_double(); this->declare_parameter("waypoint_spacing_threshold", 0.5); @@ -177,36 +174,47 @@ class DroanLocalPlanner : public rclcpp::Node { this->declare_parameter("trajectory_library_config", std::string("")); traj_lib = std::make_unique( this->get_parameter("trajectory_library_config").as_string(), this); + + pluginlib::ClassLoader + map_representation_loader("map_representation_interface", + "map_representation_interface::MapRepresentation"); + try { + map_representation = + map_representation_loader.createSharedInstance(map_representation_class_string); + } catch (pluginlib::PluginlibException& ex) { + RCLCPP_INFO(this->get_logger(), + "The MapRepresentation plugin failed to load. Error: %s", ex.what()); + } } virtual ~DroanLocalPlanner() {} - virtual bool execute() { - update_waypoint_mode(); - - if (!is_global_plan_received) return true; - - Trajectory gp(this, global_plan); - - // set the height of the global plan - if (height_mode == FIXED_HEIGHT) { - gp.set_fixed_height(fixed_height); - } else if (height_mode == RANGE_SENSOR_HEIGHT) { + /** + * Sets the global plan height based on the height mode + */ + bool set_global_plan_height_inplace(Trajectory& global_plan) { + // fixed height mode + if (this->height_mode == FIXED_HEIGHT) { + global_plan.set_fixed_height(fixed_height); + } + // set between the range sensors + else if (this->height_mode == RANGE_SENSOR_HEIGHT) { if (!got_range_up || !got_range_down) return true; try { tf2::Stamped transform_up, transform_down; geometry_msgs::msg::TransformStamped tf_up_msg, tf_down_msg; - tf_buffer.canTransform(gp.get_frame_id(), range_up.header.frame_id, + tf_buffer.canTransform(global_plan.get_frame_id(), range_up.header.frame_id, range_up.header.stamp, rclcpp::Duration::from_seconds(0.1)); - tf_up_msg = tf_buffer.lookupTransform(gp.get_frame_id(), range_up.header.frame_id, - range_up.header.stamp); + tf_up_msg = tf_buffer.lookupTransform( + global_plan.get_frame_id(), range_up.header.frame_id, range_up.header.stamp); tf2::fromMsg(tf_up_msg, transform_up); - tf_buffer.canTransform(gp.get_frame_id(), range_down.header.frame_id, + tf_buffer.canTransform(global_plan.get_frame_id(), range_down.header.frame_id, range_down.header.stamp, rclcpp::Duration::from_seconds(0.1)); - tf_down_msg = tf_buffer.lookupTransform( - gp.get_frame_id(), range_down.header.frame_id, range_down.header.stamp); + tf_down_msg = + tf_buffer.lookupTransform(global_plan.get_frame_id(), + range_down.header.frame_id, range_down.header.stamp); tf2::fromMsg(tf_down_msg, transform_down); tf2::Vector3 range_up_gp_frame = transform_up * tf2::Vector3(range_up.range, 0, 0); @@ -219,19 +227,28 @@ class DroanLocalPlanner : public rclcpp::Node { z_setpoint = range_down_gp_frame.z() + height_above_ground; } - gp.set_fixed_height(z_setpoint); + global_plan.set_fixed_height(z_setpoint); } catch (tf2::TransformException& ex) { RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); return true; } } + } + + virtual bool execute() { + update_waypoint_mode(); + + if (!is_global_plan_received) return true; + + Trajectory global_plan(this, global_plan_msg); + set_global_plan_height_inplace(global_plan); // set the height of the global plan // transform the look ahead point to the global plan frame tf2::Vector3 look_ahead_position = tflib::to_tf(look_ahead_odom.pose.position); - bool success = - tflib::to_frame(&tf_buffer, look_ahead_position, look_ahead_odom.header.frame_id, - gp.get_frame_id(), look_ahead_odom.header.stamp, &look_ahead_position); + bool success = tflib::to_frame(&tf_buffer, look_ahead_position, + look_ahead_odom.header.frame_id, global_plan.get_frame_id(), + look_ahead_odom.header.stamp, &look_ahead_position); if (!success) { RCLCPP_ERROR_STREAM(this->get_logger(), "Couldn't transform from lookahead frame to global plan frame"); @@ -240,18 +257,19 @@ class DroanLocalPlanner : public rclcpp::Node { // increment how far along the global plan we are double trajectory_distance; - bool valid = - gp.get_trajectory_distance_at_closest_point(look_ahead_position, &trajectory_distance); + bool valid = global_plan.get_trajectory_distance_at_closest_point(look_ahead_position, + &trajectory_distance); if (valid) { global_plan_trajectory_distance += trajectory_distance; - gp = gp.get_subtrajectory_distance(global_plan_trajectory_distance, - global_plan_trajectory_distance + 10.0); + global_plan = global_plan.get_subtrajectory_distance( + global_plan_trajectory_distance, global_plan_trajectory_distance + 10.0); } else { RCLCPP_INFO(this->get_logger(), "invalid"); } // publish the segment of the global plan currently being used, for visualization - visualization_msgs::msg::MarkerArray global_markers = gp.get_markers(this->now(), 0, 0, 1); + visualization_msgs::msg::MarkerArray global_markers = + global_plan.get_markers(this->now(), 0, 0, 1); global_plan_vis_pub->publish(global_markers); // get the dynamic trajectories @@ -260,14 +278,15 @@ class DroanLocalPlanner : public rclcpp::Node { // pick the best trajectory Trajectory best_traj; - bool all_in_collision = this->get_best_trajectory(dynamic_trajectories, gp, &best_traj); + bool all_in_collision = + this->get_best_trajectory(dynamic_trajectories, global_plan, best_traj); // publish the trajectory if (!all_in_collision) { - airstack_msgs::msg::TrajectoryXYZVYaw path = best_traj.get_TrajectoryXYZVYaw(); + airstack_msgs::msg::TrajectoryXYZVYaw best_traj_msg = best_traj.get_TrajectoryXYZVYaw(); // set yaw - if (yaw_mode == SMOOTH_YAW && path.waypoints.size() > 0) { + if (yaw_mode == SMOOTH_YAW && best_traj.waypoint_count() > 0) { bool found_initial_heading = false; double initial_heading = 0; try { @@ -291,14 +310,15 @@ class DroanLocalPlanner : public rclcpp::Node { } if (found_initial_heading) { - path.waypoints[0].yaw = initial_heading; + best_traj_msg.waypoints[0].yaw = initial_heading; double alpha = 0.1; - double sin_yaw_prev = sin(path.waypoints[0].yaw); - double cos_yaw_prev = cos(path.waypoints[0].yaw); + double sin_yaw_prev = sin(best_traj_msg.waypoints[0].yaw); + double cos_yaw_prev = cos(best_traj_msg.waypoints[0].yaw); - for (size_t i = 1; i < path.waypoints.size(); i++) { - airstack_msgs::msg::WaypointXYZVYaw wp_prev = path.waypoints[i - 1]; - airstack_msgs::msg::WaypointXYZVYaw& wp_curr = path.waypoints[i]; + for (size_t i = 1; i < best_traj_msg.waypoints.size(); i++) { + airstack_msgs::msg::WaypointXYZVYaw wp_prev = + best_traj_msg.waypoints[i - 1]; + airstack_msgs::msg::WaypointXYZVYaw& wp_curr = best_traj_msg.waypoints[i]; double yaw = atan2(wp_curr.position.y - wp_prev.position.y, wp_curr.position.x - wp_prev.position.x); @@ -313,31 +333,45 @@ class DroanLocalPlanner : public rclcpp::Node { } } } - path.header.stamp = this->now(); - traj_pub->publish(path); + best_traj_msg.header.stamp = this->now(); + traj_pub->publish(best_traj_msg); } return true; } - bool get_best_trajectory(std::vector trajectories, Trajectory global_plan, - Trajectory* best_traj_ret) { + std::vector> get_trajectory_distances_from_map( + std::vector trajectory_candidates) { + // vector of trajectory candidates in PointStamped-vector form + std::vector> traj_cands_as_point_stamped; + for (size_t i = 0; i < trajectory_candidates.size(); i++) { + traj_cands_as_point_stamped.push_back( + trajectory_candidates[i].get_vector_PointStamped()); + } + // TODO: clearly we should refactor map_representation to accept Trajectory objects + std::vector> trajectory_distances_to_closest_obstacle = + this->map_representation->get_values(traj_cands_as_point_stamped); + return trajectory_distances_to_closest_obstacle; + } + + /** + * Choose the best trajectory based on the cost function. Minimize the cost + */ + bool get_best_trajectory(std::vector trajectory_candidates, Trajectory global_plan, + Trajectory& best_traj_ret) { double min_cost = std::numeric_limits::max(); size_t best_traj_index = 0; bool all_in_collision = true; auto now = this->now(); - visualization_msgs::msg::MarkerArray traj_lib_markers, look_past_markers; + auto trajectory_distances_to_closest_obstacle = + get_trajectory_distances_from_map(trajectory_candidates); - std::vector> trajs_as_point_stamped; - for (size_t i = 0; i < trajectories.size(); i++) { - trajs_as_point_stamped.push_back(trajectories[i].get_vector_PointStamped()); - } - std::vector> values = pc_map->get_values(trajs_as_point_stamped); + visualization_msgs::msg::MarkerArray traj_lib_markers, look_past_markers; - for (size_t i = 0; i < trajectories.size(); ++i) { - Trajectory traj = trajectories[i]; - double average_distance = std::numeric_limits::infinity(); + for (size_t i = 0; i < trajectory_candidates.size(); ++i) { + Trajectory traj = trajectory_candidates[i]; + double avg_distance_from_global_plan = std::numeric_limits::infinity(); double closest_obstacle_distance = std::numeric_limits::infinity(); Trajectory global_plan_in_traj_frame; @@ -349,6 +383,7 @@ class DroanLocalPlanner : public rclcpp::Node { return true; } + // for each waypoint in the trajectory, calculate the distance to the closest obstacle for (size_t j = 0; j < traj.waypoint_count(); j++) { Waypoint wp = traj.get_waypoint(j); @@ -356,25 +391,31 @@ class DroanLocalPlanner : public rclcpp::Node { geometry_msgs::msg::PoseStamped pose; pose.header = odom.header; pose.pose = odom.pose; - closest_obstacle_distance = std::min(closest_obstacle_distance, values[i][j]); + closest_obstacle_distance = + std::min(closest_obstacle_distance, + trajectory_distances_to_closest_obstacle.at(i).at(j)); - Waypoint closest_point(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + // get the closest global plan point to the current trajectory waypoint + Waypoint closest_point_from_global_plan(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); int wp_index; double path_distance; bool valid = global_plan_in_traj_frame.get_closest_point( - wp.position(), &closest_point, &wp_index, &path_distance); - double forward_progress_penalty = -forward_progress_penalty_weight * path_distance; + wp.position(), &closest_point_from_global_plan, &wp_index, &path_distance); + + // reward making progress along the global plan + double forward_progress_reward = -forward_progress_reward_weight * path_distance; if (valid) { - if (!std::isfinite(average_distance)) { - average_distance = 0; + if (!std::isfinite(avg_distance_from_global_plan)) { + avg_distance_from_global_plan = 0; } - average_distance += - closest_point.position().distance(wp.position()) + forward_progress_penalty; + avg_distance_from_global_plan += + closest_point_from_global_plan.position().distance(wp.position()) + + forward_progress_reward; } } + avg_distance_from_global_plan /= traj.waypoint_count(); - average_distance /= traj.waypoint_count(); bool collision = closest_obstacle_distance <= robot_radius; if (!collision) { all_in_collision = false; @@ -385,28 +426,33 @@ class DroanLocalPlanner : public rclcpp::Node { // red for collision traj_markers = traj.get_markers(this->now(), 1, 0, 0, .5); } else { + // green for no collision traj_markers = traj.get_markers(this->now(), 0, 1, 0, .5); } + // if look_past_distance is set, then use that to calculate the cost instead + // wtf this doesn't even make sense if (look_past_distance > 0) { if (traj.waypoint_count() >= 2) { Waypoint curr_wp = traj.get_waypoint(traj.waypoint_count() - 1); Waypoint prev_wp = traj.get_waypoint(traj.waypoint_count() - 2); - tf2::Vector3 segment = curr_wp.position() - prev_wp.position(); - segment.normalize(); + tf2::Vector3 direction = curr_wp.position() - prev_wp.position(); + direction.normalize(); - tf2::Vector3 position = curr_wp.position() + look_past_distance * segment; + tf2::Vector3 look_past_position = + curr_wp.position() + look_past_distance * direction; - Waypoint closest_point(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + Waypoint closest_point_from_global_plan(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); int wp_index; bool valid = global_plan_in_traj_frame.get_closest_point( - position, &closest_point, &wp_index); + look_past_position, &closest_point_from_global_plan, &wp_index); if (!valid) { collision = true; } else { - average_distance = position.distance(closest_point.position()); + avg_distance_from_global_plan = + look_past_position.distance(closest_point_from_global_plan.position()); } visualization_msgs::msg::Marker marker; @@ -417,9 +463,9 @@ class DroanLocalPlanner : public rclcpp::Node { marker.type = visualization_msgs::msg::Marker::SPHERE; marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.position.x = position.x(); - marker.pose.position.y = position.y(); - marker.pose.position.z = position.z(); + marker.pose.position.x = look_past_position.x(); + marker.pose.position.y = look_past_position.y(); + marker.pose.position.z = look_past_position.z(); marker.pose.orientation.w = 1; marker.scale.x = 0.3; marker.scale.y = 0.3; @@ -434,13 +480,15 @@ class DroanLocalPlanner : public rclcpp::Node { traj_lib_markers.markers.insert(traj_lib_markers.markers.end(), traj_markers.markers.begin(), traj_markers.markers.end()); - double cost = - average_distance - obstacle_penalty_weight * - std::min(closest_obstacle_distance, obstacle_check_radius); + // bigger distance from obstacles makes the cost smaller (more negative). cap by the + // obstacle check radius + double cost = avg_distance_from_global_plan - + obstacle_distance_reward * + std::min(closest_obstacle_distance, obstacle_check_radius); if (!collision && cost < min_cost) { min_cost = cost; best_traj_index = i; - *best_traj_ret = traj; + best_traj_ret = traj; } } if (best_traj_index < look_past_markers.markers.size()) { @@ -451,18 +499,19 @@ class DroanLocalPlanner : public rclcpp::Node { } vis_pub->publish(traj_lib_markers); - pc_map->publish_debug(); + map_representation->publish_debug(); look_past_vis_pub->publish(look_past_markers); return all_in_collision; } // subscriber callbacks - void global_plan_callback(const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr global_plan) { + void global_plan_callback( + const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr global_plan_msg) { RCLCPP_INFO_STREAM(this->get_logger(), "GOT GLOBAL PLAN, goal_mode: " << goal_mode); if (goal_mode != TRAJECTORY) return; - this->global_plan = *global_plan; // copies + this->global_plan_msg = *global_plan_msg; // copies is_global_plan_received = true; global_plan_trajectory_distance = 0; } @@ -482,9 +531,9 @@ class DroanLocalPlanner : public rclcpp::Node { } // stitch together the history of waypoints - airstack_msgs::msg::TrajectoryXYZVYaw global_plan; - global_plan.header.frame_id = wp->header.frame_id; - global_plan.header.stamp = wp->header.stamp; + airstack_msgs::msg::TrajectoryXYZVYaw global_plan_msg; + global_plan_msg.header.frame_id = wp->header.frame_id; + global_plan_msg.header.stamp = wp->header.stamp; std::vector backwards_global_plan; @@ -538,7 +587,7 @@ class DroanLocalPlanner : public rclcpp::Node { } } for (size_t i = 0; i < backwards_global_plan.size(); i++) { - global_plan.waypoints.push_back( + global_plan_msg.waypoints.push_back( backwards_global_plan[backwards_global_plan.size() - 1 - i]); } if (is_tracking_point_received) { @@ -563,15 +612,15 @@ class DroanLocalPlanner : public rclcpp::Node { wp2.position.x = wp2_position.x(); wp2.position.y = wp2_position.y(); wp2.position.z = wp2_position.z(); - global_plan.waypoints.push_back(wp1); - global_plan.waypoints.push_back(wp2); + global_plan_msg.waypoints.push_back(wp1); + global_plan_msg.waypoints.push_back(wp2); } catch (tf2::TransformException& ex) { RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); } } global_plan_trajectory_distance = 0; - this->global_plan = global_plan; + this->global_plan_msg = global_plan_msg; this->is_global_plan_received = true; } @@ -589,9 +638,9 @@ class DroanLocalPlanner : public rclcpp::Node { tf2::Vector3 la_position = tflib::to_tf(look_ahead_odom.pose.position); tf2::Vector3 wp_position = transform * tflib::to_tf(wp->pose.position); - airstack_msgs::msg::TrajectoryXYZVYaw global_plan; - global_plan.header.frame_id = look_ahead_odom.header.frame_id; - global_plan.header.stamp = this->now(); + airstack_msgs::msg::TrajectoryXYZVYaw global_plan_msg; + global_plan_msg.header.frame_id = look_ahead_odom.header.frame_id; + global_plan_msg.header.stamp = this->now(); airstack_msgs::msg::WaypointXYZVYaw wp1, wp2; wp1.position.x = la_position.x(); @@ -600,11 +649,11 @@ class DroanLocalPlanner : public rclcpp::Node { wp2.position.x = wp_position.x(); wp2.position.y = wp_position.y(); wp2.position.z = wp_position.z(); - global_plan.waypoints.push_back(wp1); - global_plan.waypoints.push_back(wp2); + global_plan_msg.waypoints.push_back(wp1); + global_plan_msg.waypoints.push_back(wp2); global_plan_trajectory_distance = 0; - this->global_plan = global_plan; + this->global_plan_msg = global_plan_msg; this->is_global_plan_received = true; goal_mode = CUSTOM_WAYPOINT; @@ -613,17 +662,20 @@ class DroanLocalPlanner : public rclcpp::Node { } } + /** + * Check if we should switch from CUSTOM_WAYPOINT to AUTO_WAYPOINT + */ void update_waypoint_mode() { if (goal_mode == CUSTOM_WAYPOINT) { - if (global_plan.waypoints.size() < 2) goal_mode = AUTO_WAYPOINT; + if (global_plan_msg.waypoints.size() < 2) goal_mode = AUTO_WAYPOINT; // check if the time limit for reaching the waypoint has elapsed - double elapsed_time = (this->now() - global_plan.header.stamp).seconds(); + double elapsed_time = (this->now() - global_plan_msg.header.stamp).seconds(); double distance = 0; - for (size_t i = 1; i < global_plan.waypoints.size(); i++) { + for (size_t i = 1; i < global_plan_msg.waypoints.size(); i++) { airstack_msgs::msg::WaypointXYZVYaw prev_wp, curr_wp; - prev_wp = global_plan.waypoints[i - 1]; - curr_wp = global_plan.waypoints[i]; + prev_wp = global_plan_msg.waypoints[i - 1]; + curr_wp = global_plan_msg.waypoints[i]; distance += sqrt(pow(prev_wp.position.x - curr_wp.position.x, 2) + pow(prev_wp.position.y - curr_wp.position.y, 2)); @@ -640,18 +692,18 @@ class DroanLocalPlanner : public rclcpp::Node { if (is_tracking_point_received) { try { tf2::Stamped transform; - tf_buffer.canTransform(tracking_point_odom.header.frame_id, - global_plan.header.frame_id, global_plan.header.stamp, - rclcpp::Duration::from_seconds(0.1)); + tf_buffer.canTransform( + tracking_point_odom.header.frame_id, global_plan_msg.header.frame_id, + global_plan_msg.header.stamp, rclcpp::Duration::from_seconds(0.1)); auto transform_msg = tf_buffer.lookupTransform( - tracking_point_odom.header.frame_id, global_plan.header.frame_id, - global_plan.header.stamp); + tracking_point_odom.header.frame_id, global_plan_msg.header.frame_id, + global_plan_msg.header.stamp); tf2::fromMsg(transform_msg, transform); tf2::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.position); tp_position.setZ(0); tf2::Vector3 wp_position = - transform * tflib::to_tf(global_plan.waypoints.back().position); + transform * tflib::to_tf(global_plan_msg.waypoints.back().position); wp_position.setZ(0); if (tp_position.distance(wp_position) < custom_waypoint_distance_threshold) { @@ -674,12 +726,12 @@ class DroanLocalPlanner : public rclcpp::Node { is_tracking_point_received = true; tracking_point_odom = *odom; } - void range_up_callback(const sensor_msgs::msg::Range::SharedPtr range) { + void range_up_callback(const sensor_msgs::msg::Range::SharedPtr range_msg) { got_range_up = true; - range_up = *range; + range_up = *range_msg; } - void range_down_callback(const sensor_msgs::msg::Range::SharedPtr range) { + void range_down_callback(const sensor_msgs::msg::Range::SharedPtr range_msg) { got_range_down = true; - range_down = *range; + range_down = *range_msg; } }; diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.xml b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.xml new file mode 100644 index 00000000..c1445250 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.xml @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.yaml new file mode 100644 index 00000000..b1a89a75 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.yaml @@ -0,0 +1,16 @@ +launch: +# - arg: +# name: "robot_name" +# default: "robot1" +# - arg: +# name: "tf_prefix" +# default: "$(var robot_name)" +- node: + pkg: "droan_local_planner" + exec: "droan_local_planner" + name: "droan_local_planner" + namespace: "droan_local_planner" + param: + - + from: $(find-pkg-share droan_local_planner)/config/droan.yaml + # allow_substs: true diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/package.xml b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/package.xml similarity index 92% rename from ros_ws/src/robot/autonomy/planning/local/droan_local_planner/package.xml rename to ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/package.xml index 541df373..ef446e80 100644 --- a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/package.xml +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/package.xml @@ -10,7 +10,7 @@ ament_cmake airstack_msgs - core_map_representation_interface + map_representation_interface pluginlib rclcpp std_msgs @@ -25,4 +25,4 @@ ament_cmake - + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/src/droan_local_planner.cpp b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/droan_local_planner/src/droan_local_planner.cpp rename to ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/.gitignore b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/.gitignore similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/.gitignore rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/.gitignore diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/config/acceleration_magnitudes.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/config/acceleration_magnitudes.yaml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/config/acceleration_trajectories.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/config/acceleration_trajectories.yaml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories.yaml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/config/acceleration_trajectories_fast.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories_fast.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/config/acceleration_trajectories_fast.yaml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories_fast.yaml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/config/backup.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/backup.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/config/backup.yaml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/backup.yaml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/config/demo_trajectory_definitions.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/demo_trajectory_definitions.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/config/demo_trajectory_definitions.yaml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/demo_trajectory_definitions.yaml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/config/fixed_trajectories.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/fixed_trajectories.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/config/fixed_trajectories.yaml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/fixed_trajectories.yaml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/config/test.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/test.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/config/test.yaml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/test.yaml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/include/trajectory_library/trajectory_library.hpp b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/include/trajectory_library/trajectory_library.hpp rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/package.xml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/package.xml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/plugin.xml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/plugin.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/plugin.xml rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/plugin.xml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/scripts/fixed_trajectory_generator.py b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/scripts/fixed_trajectory_generator.py rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/scripts/rqt_fixed_trajectory_selector b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/scripts/rqt_fixed_trajectory_selector similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/scripts/rqt_fixed_trajectory_selector rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/scripts/rqt_fixed_trajectory_selector diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/setup.py b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/setup.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/setup.py rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/setup.py diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/fixed_trajectory_generator.py b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/fixed_trajectory_generator.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/src/fixed_trajectory_generator.py rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/fixed_trajectory_generator.py diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/__init__.py b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/__init__.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/__init__.py rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/__init__.py diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/trajectory_library.cpp b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/src/trajectory_library.cpp rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/trajectory_library_generator.py b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library_generator.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_library/src/trajectory_library_generator.py rename to ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library_generator.py diff --git a/ros_ws/src/robot/autonomy/controls/controls_bringup/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/controls_bringup/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/controls_bringup/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/controls_bringup/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/controls_bringup/launch/launch_controls.yaml b/ros_ws/src/robot/autonomy/3_local/c_controls/controls_bringup/launch/launch_controls.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/controls_bringup/launch/launch_controls.yaml rename to ros_ws/src/robot/autonomy/3_local/c_controls/controls_bringup/launch/launch_controls.yaml diff --git a/ros_ws/src/robot/autonomy/controls/controls_bringup/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/controls_bringup/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/controls_bringup/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/controls_bringup/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/.clang-format b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/.clang-format similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/.clang-format rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/.clang-format diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/.github/workflows/build_test.yml b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/.github/workflows/build_test.yml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/.github/workflows/build_test.yml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/.github/workflows/build_test.yml diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/README.md b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/README.md similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/README.md rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/README.md diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_comm/CHANGELOG.rst b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_comm/CHANGELOG.rst similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_comm/CHANGELOG.rst rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_comm/CHANGELOG.rst diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_comm/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_comm/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_comm/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_comm/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_comm/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_comm/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_comm/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_comm/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/CHANGELOG.rst b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/CHANGELOG.rst similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/CHANGELOG.rst rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/CHANGELOG.rst diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/Actuators.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Actuators.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/Actuators.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Actuators.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/AttitudeThrust.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/AttitudeThrust.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/AttitudeThrust.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/AttitudeThrust.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/FilteredSensorData.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/FilteredSensorData.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/FilteredSensorData.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/FilteredSensorData.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/GpsWaypoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/GpsWaypoint.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/GpsWaypoint.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/GpsWaypoint.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/RateThrust.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RateThrust.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/RateThrust.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RateThrust.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/RollPitchYawrateThrust.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RollPitchYawrateThrust.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/RollPitchYawrateThrust.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RollPitchYawrateThrust.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/Status.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Status.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/Status.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Status.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/TorqueThrust.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/TorqueThrust.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/msg/TorqueThrust.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/TorqueThrust.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_msgs/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_msgs/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/CHANGELOG.rst b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CHANGELOG.rst similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/CHANGELOG.rst rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CHANGELOG.rst diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/Point2D.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Point2D.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/Point2D.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Point2D.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PointCloudWithPose.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PointCloudWithPose.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PointCloudWithPose.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PointCloudWithPose.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/Polygon2D.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Polygon2D.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/Polygon2D.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Polygon2D.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolygonWithHoles.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHoles.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolygonWithHoles.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHoles.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolygonWithHolesStamped.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHolesStamped.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolygonWithHolesStamped.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHolesStamped.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment4D.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment4D.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment4D.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment4D.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory4D.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory4D.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory4D.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory4D.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/srv/ChangeNameService.srv b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/ChangeNameService.srv similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/srv/ChangeNameService.srv rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/ChangeNameService.srv diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/srv/PlannerService.srv b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PlannerService.srv similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/srv/PlannerService.srv rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PlannerService.srv diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/srv/PolygonService.srv b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PolygonService.srv similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_planning_msgs/srv/PolygonService.srv rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PolygonService.srv diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/msg/StartStopTask.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/msg/StartStopTask.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/msg/StartStopTask.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/msg/StartStopTask.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/srv/RunTaskService.srv b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/srv/RunTaskService.srv similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_state_machine_msgs/srv/RunTaskService.srv rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/srv/RunTaskService.srv diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/msg/CpuInfo.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/CpuInfo.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/msg/CpuInfo.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/CpuInfo.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/msg/ProcessInfo.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/ProcessInfo.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/msg/ProcessInfo.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/ProcessInfo.msg diff --git a/ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mav_comm/mav_system_msgs/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/mavros_interface/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mavros_interface/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/perception/.gitkeep b/ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/include/mavros_interface/.gitkeep similarity index 100% rename from ros_ws/src/robot/autonomy/perception/.gitkeep rename to ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/include/mavros_interface/.gitkeep diff --git a/ros_ws/src/robot/autonomy/controls/mavros_interface/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mavros_interface/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/mavros_interface/plugins.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/plugins.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mavros_interface/plugins.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/plugins.xml diff --git a/ros_ws/src/robot/autonomy/controls/mavros_interface/scripts/position_setpoint_pub.py b/ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/scripts/position_setpoint_pub.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mavros_interface/scripts/position_setpoint_pub.py rename to ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/scripts/position_setpoint_pub.py diff --git a/ros_ws/src/robot/autonomy/controls/mavros_interface/src/mavros_interface.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/src/mavros_interface.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/mavros_interface/src/mavros_interface.cpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/mavros_interface/src/mavros_interface.cpp diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.github/dependabot.yml b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.github/dependabot.yml new file mode 100644 index 00000000..c7fc735e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.github/dependabot.yml @@ -0,0 +1,6 @@ +version: 2 +updates: + - package-ecosystem: github-actions + directory: "/" + schedule: + interval: "daily" diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.github/workflows/build.yml b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.github/workflows/build.yml new file mode 100644 index 00000000..0a467bba --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.github/workflows/build.yml @@ -0,0 +1,40 @@ +name: Build package + +# CI runs over all branches that do not contain 'ros1' in the name +on: + push: + schedule: + - cron: '0 0 * * *' + +defaults: + run: + shell: bash + +jobs: + focal: + name: "Build on Ubuntu Focal" + runs-on: ubuntu-20.04 + steps: + - uses: actions/checkout@v4 + - uses: ros-tooling/setup-ros@v0.6 + with: + required-ros-distributions: foxy + - uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: px4_msgs + target-ros2-distro: foxy + jammy: + name: "Build on Ubuntu Jammy" + runs-on: ubuntu-22.04 + strategy: + matrix: + ros2_distro: [humble, rolling] + steps: + - uses: actions/checkout@v4 + - uses: ros-tooling/setup-ros@v0.6 + with: + required-ros-distributions: ${{ matrix.ros2_distro }} + - uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: px4_msgs + target-ros2-distro: ${{ matrix.ros2_distro }} \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.gitignore b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.gitignore new file mode 100644 index 00000000..b89e1984 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/.gitignore @@ -0,0 +1,2 @@ +build/ +msg/idl.cc diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/CMakeLists.txt new file mode 100644 index 00000000..0fc6edc2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5) + +project(px4_msgs) + +list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# ############################################################################## +# Generate ROS messages, ROS2 interfaces and IDL files # +# ############################################################################## + +# get all msg files +set(MSGS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/msg") +file(GLOB PX4_MSGS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${MSGS_DIR}/*.msg") + +# get all srv files +set(SRVS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/srv") +file(GLOB PX4_SRVS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${SRVS_DIR}/*.srv") + +# Generate introspection typesupport for C and C++ and IDL files +rosidl_generate_interfaces(${PROJECT_NAME} + ${PX4_MSGS} + ${PX4_SRVS} + DEPENDENCIES builtin_interfaces + ADD_LINTER_TESTS +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING.md b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING.md new file mode 100644 index 00000000..5df008e5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING.md @@ -0,0 +1,7 @@ +# Contributing + +*Do not* commit changes directly to this repository that change the message definitions. All the message definitions are directly generated from the [uORB msg definitions](https://github.com/PX4/Firmware/tree/master/msg) on the [PX4 Firmware repository](https://github.com/PX4/Firmware). Any fixes or improvements one finds suitable to apply to the message definitions should be directly done on the uORB message files. The deployment of these are taken care by a Jenkins CI/CD stage. + +### Contributing to the PX4 Firmware repository (or to this repository, not including message definitions) + +Follow the [`Contributing` guide](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md) from the PX4 Firmware repo. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/LICENSE b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/LICENSE new file mode 100644 index 00000000..31f2eb48 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2019, 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: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/README.md b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/README.md new file mode 100644 index 00000000..5cbfa157 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/README.md @@ -0,0 +1,49 @@ +# px4_msgs + +[![GitHub license](https://img.shields.io/github/license/PX4/px4_msgs.svg)](https://github.com/PX4/px4_msg/blob/master/LICENSE) [![Build package](https://github.com/PX4/px4_msgs/workflows/Build%20package/badge.svg)](https://github.com/PX4/px4_msgs/actions) + +[![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode) + +ROS 2 message definitions for the [PX4 Autopilot](https://px4.io/) project. + +Building this package generates all the required interfaces to interface ROS 2 nodes with the PX4 internals. + +## Supported versions and compatibility + +Depending on the PX4 and ROS versions you want to use, you need to checkout the appropriate branch of this package: + +| PX4 | ROS 2 | Ubuntu | branch | +|----------------|---------|--------------|-------------------------------------------------------------------| +| [v1.13](https://github.com/PX4/px4_msgs/tree/release/1.13) | Foxy | Ubuntu 20.04 | [release/1.13](https://github.com/PX4/px4_msgs/tree/release/1.13) | +| [v1.14](https://github.com/PX4/px4_msgs/tree/release/1.14) | Foxy | Ubuntu 20.04 | [release/1.14](https://github.com/PX4/px4_msgs/tree/release/1.14) | +| [v1.14](https://github.com/PX4/px4_msgs/tree/release/1.14) | Humble | Ubuntu 22.04 | [release/1.14](https://github.com/PX4/px4_msgs/tree/release/1.14) | +| [v1.14](https://github.com/PX4/px4_msgs/tree/release/1.14) | Rolling | Ubuntu 22.04 | [release/1.14](https://github.com/PX4/px4_msgs/tree/release/1.14) | +| [main](https://github.com/PX4/px4_msgs/tree/main) | Foxy | Ubuntu 22.04 | [main](https://github.com/PX4/px4_msgs) | +| [main](https://github.com/PX4/px4_msgs/tree/main) | Humble | Ubuntu 22.04 | [main](https://github.com/PX4/px4_msgs) | +| [main](https://github.com/PX4/px4_msgs/tree/main) | Rolling | Ubuntu 22.04 | [main](https://github.com/PX4/px4_msgs) | + +### Messages Sync from PX4 + +When PX4 message definitions in the `main` branch of [PX4 Autopilot](https://github.com/PX4/PX4-Autopilot) change, a [CI/CD pipeline](https://github.com/PX4/PX4-Autopilot/blob/main/.github/workflows/metadata.yml#L119) automatically copies and pushes updated ROS message definitions to this repository. This ensures that this repository `main` branch and the PX4-Autopilot `main` branch are always up to date. +However, if you are using a custom PX4 version and you modified existing messages or created new one, then you have to manually synchronize them in this repository: +### Manual Message Sync + +- Checkout the correct branch associated to the PX4 version from which you detached you custom version. +- Delete all `*.msg` files in `msg/` and copy all `*.msg` files from `PX4-Autopilot/msg/` in it. Assuming that this repository and the PX4-Autopilot repository are placed in your home folder, you can run: + ```sh + rm -f ~/px4_msgs/msg/*.msg + cp ~/PX4-Autopilot/msg/*.msg ~/px4_msgs/msg/ + ``` + +## Install, build and usage + +Check [Using colcon to build packages](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html#build-a-package) to understand how this can be built inside a workspace. Check the [PX4 ROS 2 User Guide](https://docs.px4.io/main/en/ros/ros2_comm.html) section on the PX4 documentation for further details on how this integrates PX4 and how to exchange messages with the autopilot. + +## Bug tracking and feature requests + +Use the [Issues](https://github.com/PX4/px4_msgs/issues) section to create a new issue. Report your issue or feature request [here](https://github.com/PX4/px4_msgs/issues/new). + +## Questions and troubleshooting + +Reach the PX4 development team on the [PX4 Discord Server](https://discord.gg/dronecode). + diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActionRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActionRequest.msg new file mode 100644 index 00000000..888814e0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActionRequest.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 action # what action is requested +uint8 ACTION_DISARM = 0 +uint8 ACTION_ARM = 1 +uint8 ACTION_TOGGLE_ARMING = 2 +uint8 ACTION_UNKILL = 3 +uint8 ACTION_KILL = 4 +uint8 ACTION_SWITCH_MODE = 5 +uint8 ACTION_VTOL_TRANSITION_TO_MULTICOPTER = 6 +uint8 ACTION_VTOL_TRANSITION_TO_FIXEDWING = 7 + +uint8 source # how the request was triggered +uint8 SOURCE_RC_STICK_GESTURE = 0 +uint8 SOURCE_RC_SWITCH = 1 +uint8 SOURCE_RC_BUTTON = 2 +uint8 SOURCE_RC_MODE_SLOT = 3 + +uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to vehicle_status_s::NAVIGATION_STATE_* diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorArmed.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorArmed.msg new file mode 100644 index 00000000..6867adf2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorArmed.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +bool armed # Set to true if system is armed +bool prearmed # Set to true if the actuator safety is disabled but motors are not armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool manual_lockdown # Set to true if manual throttle kill switch is engaged +bool force_failsafe # Set to true if the actuators are forced to the failsafe position +bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorControlsStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorControlsStatus.msg new file mode 100644 index 00000000..c89f669e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorControlsStatus.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32[3] control_power + +# TOPICS actuator_controls_status_0 actuator_controls_status_1 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorMotors.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorMotors.msg new file mode 100644 index 00000000..e74566f1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorMotors.msg @@ -0,0 +1,12 @@ +# Motor control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint16 reversible_flags # bitset which motors are configured to be reversible + +uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 + +uint8 NUM_CONTROLS = 12 +float32[12] control # range: [-1, 1], where 1 means maximum positive thrust, + # -1 maximum negative (if not supported by the output, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorOutputs.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorOutputs.msg new file mode 100644 index 00000000..3209e54e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorOutputs.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint8 NUM_ACTUATOR_OUTPUTS = 16 +uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking +uint32 noutputs # valid outputs +float32[16] output # output data, in natural output units + +# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1]) +# TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServos.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServos.msg new file mode 100644 index 00000000..2c7900e8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServos.msg @@ -0,0 +1,8 @@ +# Servo control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint8 NUM_CONTROLS = 8 +float32[8] control # range: [-1, 1], where 1 means maximum positive position, + # -1 maximum negative, + # and NaN maps to disarmed diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServosTrim.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServosTrim.msg new file mode 100644 index 00000000..30953e7a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServosTrim.msg @@ -0,0 +1,5 @@ +# Servo trims, added as offset to servo outputs +uint64 timestamp # time since system start (microseconds) + +uint8 NUM_CONTROLS = 8 +float32[8] trim # range: [-1, 1] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorTest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorTest.msg new file mode 100644 index 00000000..221258f9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorTest.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +# Topic to test individual actuator output functions + +uint8 ACTION_RELEASE_CONTROL = 0 # exit test mode for the given function +uint8 ACTION_DO_CONTROL = 1 # enable actuator test mode + +uint8 FUNCTION_MOTOR1 = 101 +uint8 MAX_NUM_MOTORS = 12 +uint8 FUNCTION_SERVO1 = 201 +uint8 MAX_NUM_SERVOS = 8 + +uint8 action # one of ACTION_* +uint16 function # actuator output function +float32 value # range: [-1, 1], where 1 means maximum positive output, + # 0 to center servos or minimum motor thrust, + # -1 maximum negative (if not supported by the motors, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) +uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out) + +uint8 ORB_QUEUE_LENGTH = 16 # >= MAX_NUM_MOTORS to support code in esc_calibration diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AdcReport.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AdcReport.msg new file mode 100644 index 00000000..1ae72b6d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AdcReport.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles +int16[12] channel_id # ADC channel IDs, negative for non-existent, TODO: should be kept same as array index +int32[12] raw_data # ADC channel raw value, accept negative value, valid if channel ID is positive +uint32 resolution # ADC channel resolution +float32 v_ref # ADC channel voltage reference, use to calculate LSB voltage(lsb=scale/resolution) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Airspeed.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Airspeed.msg new file mode 100644 index 00000000..aaed7b72 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Airspeed.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +float32 indicated_airspeed_m_s # indicated airspeed in m/s + +float32 true_airspeed_m_s # true filtered airspeed in m/s + +float32 air_temperature_celsius # air temperature in degrees Celsius, -1000 if unknown + +float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedValidated.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedValidated.msg new file mode 100644 index 00000000..06731cc4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedValidated.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid +float32 calibrated_airspeed_m_s # calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid +float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid + +float32 calibrated_ground_minus_wind_m_s # CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid +float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid + +bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid. + +int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedWind.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedWind.msg new file mode 100644 index 00000000..6ca513a3 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedWind.msg @@ -0,0 +1,26 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated + +float32 tas_innov # True airspeed innovation +float32 tas_innov_var # True airspeed innovation variance + +float32 tas_scale_raw # Estimated true airspeed scale factor (not validated) +float32 tas_scale_raw_var # True airspeed scale factor variance + +float32 tas_scale_validated # Estimated true airspeed scale factor after validation + +float32 beta_innov # Sideslip measurement innovation +float32 beta_innov_var # Sideslip measurement innovation variance + +uint8 source # source of wind estimate + +uint8 SOURCE_AS_BETA_ONLY = 0 # wind estimate only based on synthetic sideslip fusion +uint8 SOURCE_AS_SENSOR_1 = 1 # combined synthetic sideslip and airspeed fusion (data from first airspeed sensor) +uint8 SOURCE_AS_SENSOR_2 = 2 # combined synthetic sideslip and airspeed fusion (data from second airspeed sensor) +uint8 SOURCE_AS_SENSOR_3 = 3 # combined synthetic sideslip and airspeed fusion (data from third airspeed sensor) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckReply.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckReply.msg new file mode 100644 index 00000000..589ad1b1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckReply.msg @@ -0,0 +1,33 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 request_id +uint8 registration_id + +uint8 HEALTH_COMPONENT_INDEX_NONE = 0 +uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19 + +uint8 health_component_index # HEALTH_COMPONENT_INDEX_* +bool health_component_is_present +bool health_component_warning +bool health_component_error + +bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run + +uint8 num_events + +Event[5] events + +# Mode requirements +bool mode_req_angular_velocity +bool mode_req_attitude +bool mode_req_local_alt +bool mode_req_local_position +bool mode_req_local_position_relaxed +bool mode_req_global_position +bool mode_req_mission +bool mode_req_home_position +bool mode_req_prevent_arming +bool mode_req_manual_control + + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckRequest.msg new file mode 100644 index 00000000..69e7e85f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckRequest.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +# broadcast message to request all registered arming checks to be reported + +uint8 request_id diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AutotuneAttitudeControlStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AutotuneAttitudeControlStatus.msg new file mode 100644 index 00000000..021a8c34 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/AutotuneAttitudeControlStatus.msg @@ -0,0 +1,35 @@ +uint64 timestamp # time since system start (microseconds) + +float32[5] coeff # coefficients of the identified discrete-time model +float32[5] coeff_var # coefficients' variance of the identified discrete-time model +float32 fitness # fitness of the parameter estimate +float32 innov +float32 dt_model + +float32 kc +float32 ki +float32 kd +float32 kff +float32 att_p + +float32[3] rate_sp + +float32 u_filt +float32 y_filt + +uint8 STATE_IDLE = 0 +uint8 STATE_INIT = 1 +uint8 STATE_ROLL = 2 +uint8 STATE_ROLL_PAUSE = 3 +uint8 STATE_PITCH = 4 +uint8 STATE_PITCH_PAUSE = 5 +uint8 STATE_YAW = 6 +uint8 STATE_YAW_PAUSE = 7 +uint8 STATE_VERIFICATION = 8 +uint8 STATE_APPLY = 9 +uint8 STATE_TEST = 10 +uint8 STATE_COMPLETE = 11 +uint8 STATE_FAIL = 12 +uint8 STATE_WAIT_FOR_DISARM = 13 + +uint8 state diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/BatteryStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/BatteryStatus.msg new file mode 100644 index 00000000..66fcffa0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/BatteryStatus.msg @@ -0,0 +1,78 @@ +uint64 timestamp # time since system start (microseconds) +bool connected # Whether or not a battery is connected, based on a voltage threshold +float32 voltage_v # Battery voltage in volts, 0 if unknown +float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown +float32 current_a # Battery current in amperes, -1 if unknown +float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown +float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown +float32 discharged_mah # Discharged amount in mAh, -1 if unknown +float32 remaining # From 1 to 0, -1 if unknown +float32 scale # Power scaling factor, >= 1, or -1 if unknown +float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown +float32 temperature # temperature of the battery. NaN if unknown +uint8 cell_count # Number of cells, 0 if unknown + +uint8 BATTERY_SOURCE_POWER_MODULE = 0 +uint8 BATTERY_SOURCE_EXTERNAL = 1 +uint8 BATTERY_SOURCE_ESCS = 2 +uint8 source # Battery source +uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 +uint16 capacity # actual capacity of the battery +uint16 cycle_count # number of discharge cycles the battery has experienced +uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min +uint16 serial_number # serial number of the battery pack +uint16 manufacture_date # manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512 +uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity, 0-100%. +uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100% +uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed. +uint16 interface_error # interface error counter + +float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown +float32 max_cell_voltage_delta # Max difference between individual cell voltages + +bool is_powering_off # Power off event imminent indication, false if unknown +bool is_required # Set if the battery is explicitly required before arming + + +uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active +uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage +uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately +uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required +uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely +uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. +uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging + +uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged +uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes +uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed +uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current +uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature +uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault +uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one +uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware +uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system +uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem +uint8 BATTERY_WARNING_OVER_TEMPERATURE = 10 # Over-temperature +uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element! + +uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication. +uint32 custom_faults # Bitmask indicating smart battery internal manufacturer faults, those are not user actionable. +uint8 warning # Current battery warning +uint8 mode # Battery mode. Note, the normal operation mode + +uint8 BATTERY_MODE_UNKNOWN = 0 # Battery does not support a mode, or if it does, is operational +uint8 BATTERY_MODE_AUTO_DISCHARGING = 1 # Battery is auto discharging (towards storage level) +uint8 BATTERY_MODE_HOT_SWAP = 2 # Battery in hot-swap mode +uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're fully migrated to events interface we can just comment this)! + + +uint8 MAX_INSTANCES = 4 + +float32 average_power # The average power of the current discharge +float32 available_energy # The predicted charge or energy remaining in the battery +float32 full_charge_capacity_wh # The compensated battery capacity +float32 remaining_capacity_wh # The compensated battery capacity remaining +float32 design_capacity # The design capacity of the battery +uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes +uint16 over_discharge_count # Number of battery overdischarge +float32 nominal_voltage # Nominal voltage of the battery pack diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Buffer128.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Buffer128.msg new file mode 100644 index 00000000..342aa83d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Buffer128.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 len # length of data +uint32 MAX_BUFLEN = 128 + +uint8[128] data # data + +# TOPICS voxl2_io_data + diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ButtonEvent.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ButtonEvent.msg new file mode 100644 index 00000000..bbca356a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ButtonEvent.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +bool triggered # Set to true if the event is triggered + +# TOPICS button_event safety_button + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraCapture.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraCapture.msg new file mode 100644 index 00000000..141bb2eb --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraCapture.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_utc # Capture time in UTC / GPS time +uint32 seq # Image sequence number +float64 lat # Latitude in degrees (WGS84) +float64 lon # Longitude in degrees (WGS84) +float32 alt # Altitude (AMSL) +float32 ground_distance # Altitude above ground (meters) +float32[4] q # Attitude of the camera relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude +int8 result # 1 for success, 0 for failure, -1 if camera does not provide feedback diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraStatus.msg new file mode 100644 index 00000000..c83be897 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraStatus.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 active_sys_id # mavlink system id of the currently active camera +uint8 active_comp_id # mavlink component id of currently active camera diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraTrigger.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraTrigger.msg new file mode 100644 index 00000000..abfdac6d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CameraTrigger.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_utc # UTC timestamp + +uint32 seq # Image sequence number +bool feedback # Trigger feedback from camera + +uint32 ORB_QUEUE_LENGTH = 2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CanInterfaceStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CanInterfaceStatus.msg new file mode 100644 index 00000000..4129c8d5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CanInterfaceStatus.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint8 interface + +uint64 io_errors +uint64 frames_tx +uint64 frames_rx diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CellularStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CellularStatus.msg new file mode 100644 index 00000000..5a1c8bac --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CellularStatus.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 CELLULAR_STATUS_FLAG_UNKNOWN=0 # State unknown or not reportable +uint8 CELLULAR_STATUS_FLAG_FAILED=1 # velocity setpoint +uint8 CELLULAR_STATUS_FLAG_INITIALIZING=2 # Modem is being initialized +uint8 CELLULAR_STATUS_FLAG_LOCKED=3 # Modem is locked +uint8 CELLULAR_STATUS_FLAG_DISABLED=4 # Modem is not enabled and is powered down +uint8 CELLULAR_STATUS_FLAG_DISABLING=5 # Modem is currently transitioning to the CELLULAR_STATUS_FLAG_DISABLED state +uint8 CELLULAR_STATUS_FLAG_ENABLING=6 # Modem is currently transitioning to the CELLULAR_STATUS_FLAG_ENABLED state +uint8 CELLULAR_STATUS_FLAG_ENABLED=7 # Modem is enabled and powered on but not registered with a network provider and not available for data connections +uint8 CELLULAR_STATUS_FLAG_SEARCHING=8 # Modem is searching for a network provider to register +uint8 CELLULAR_STATUS_FLAG_REGISTERED=9 # Modem is registered with a network provider, and data connections and messaging may be available for use +uint8 CELLULAR_STATUS_FLAG_DISCONNECTING=10 # Modem is disconnecting and deactivating the last active packet data bearer. This state will not be entered if more than one packet data bearer is active and one of the active bearers is deactivated +uint8 CELLULAR_STATUS_FLAG_CONNECTING=11 # Modem is activating and connecting the first packet data bearer. Subsequent bearer activations when another bearer is already active do not cause this state to be entered +uint8 CELLULAR_STATUS_FLAG_CONNECTED=12 # One or more packet data bearers is active and connected + +uint8 CELLULAR_NETWORK_FAILED_REASON_NONE=0 # No error +uint8 CELLULAR_NETWORK_FAILED_REASON_UNKNOWN=1 # Error state is unknown +uint8 CELLULAR_NETWORK_FAILED_REASON_SIM_MISSING=2 # SIM is required for the modem but missing +uint8 CELLULAR_NETWORK_FAILED_REASON_SIM_ERROR=3 # SIM is available, but not usable for connection + +uint16 status # Status bitmap 1: Roaming is active +uint8 failure_reason #Failure reason when status in in CELLUAR_STATUS_FAILED +uint8 type # Cellular network radio type 0: none 1: gsm 2: cdma 3: wcdma 4: lte +uint8 quality # Cellular network RSSI/RSRP in dBm, absolute value +uint16 mcc # Mobile country code. If unknown, set to: UINT16_MAX +uint16 mnc # Mobile network code. If unknown, set to: UINT16_MAX +uint16 lac # Location area code. If unknown, set to: 0 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CollisionConstraints.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CollisionConstraints.msg new file mode 100644 index 00000000..40f67e29 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CollisionConstraints.msg @@ -0,0 +1,7 @@ +# Local setpoint constraints in NED frame +# setting something to NaN means that no limit is provided + +uint64 timestamp # time since system start (microseconds) + +float32[2] original_setpoint # velocities demanded +float32[2] adapted_setpoint # velocities allowed diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CollisionReport.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CollisionReport.msg new file mode 100644 index 00000000..1ad7ce72 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/CollisionReport.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint8 src +uint32 id +uint8 action +uint8 threat_level +float32 time_to_minimum_delta +float32 altitude_minimum_delta +float32 horizontal_minimum_delta diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ConfigOverrides.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ConfigOverrides.msg new file mode 100644 index 00000000..09b87253 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ConfigOverrides.msg @@ -0,0 +1,18 @@ +# Configurable overrides by (external) modes or mode executors +uint64 timestamp # time since system start (microseconds) + +bool disable_auto_disarm # Prevent the drone from automatically disarming after landing (if configured) + +bool defer_failsafes # Defer all failsafes that can be deferred (until the flag is cleared) +int16 defer_failsafes_timeout_s # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout + + +int8 SOURCE_TYPE_MODE = 0 +int8 SOURCE_TYPE_MODE_EXECUTOR = 1 +int8 source_type + +uint8 source_id # ID depending on source_type + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS config_overrides config_overrides_request diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ControlAllocatorStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ControlAllocatorStatus.msg new file mode 100644 index 00000000..2d7b0883 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ControlAllocatorStatus.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +bool torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved. + # Computed as: unallocated_torque = torque_setpoint - allocated_torque + +bool thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved. + # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust + +int8 ACTUATOR_SATURATION_OK = 0 # The actuator is not saturated +int8 ACTUATOR_SATURATION_UPPER_DYN = 1 # The actuator is saturated (with a value <= the desired value) because it cannot increase its value faster +int8 ACTUATOR_SATURATION_UPPER = 2 # The actuator is saturated (with a value <= the desired value) because it has reached its maximum value +int8 ACTUATOR_SATURATION_LOWER_DYN = -1 # The actuator is saturated (with a value >= the desired value) because it cannot decrease its value faster +int8 ACTUATOR_SATURATION_LOWER = -2 # The actuator is saturated (with a value >= the desired value) because it has reached its minimum value + +int8[16] actuator_saturation # Indicates actuator saturation status. + # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved. + # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value. + +uint16 handled_motor_failure_mask # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Cpuload.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Cpuload.msg new file mode 100644 index 00000000..efc2c4df --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Cpuload.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +float32 load # processor load from 0 to 1 +float32 ram_usage # RAM usage from 0 to 1 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DatamanRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DatamanRequest.msg new file mode 100644 index 00000000..f819771a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DatamanRequest.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data +uint32 data_length \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DatamanResponse.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DatamanResponse.msg new file mode 100644 index 00000000..ebf752db --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DatamanResponse.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data + +uint8 STATUS_SUCCESS = 0 +uint8 STATUS_FAILURE_ID_ERR = 1 +uint8 STATUS_FAILURE_NO_DATA = 2 +uint8 STATUS_FAILURE_READ_FAILED = 3 +uint8 STATUS_FAILURE_WRITE_FAILED = 4 +uint8 STATUS_FAILURE_CLEAR_FAILED = 5 +uint8 status diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugArray.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugArray.msg new file mode 100644 index 00000000..4763a0f5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugArray.msg @@ -0,0 +1,5 @@ +uint8 ARRAY_SIZE = 58 +uint64 timestamp # time since system start (microseconds) +uint16 id # unique ID of debug array, used to discriminate between arrays +char[10] name # name of the debug array (max. 10 characters) +float32[58] data # data \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugKeyValue.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugKeyValue.msg new file mode 100644 index 00000000..6815811e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugKeyValue.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +char[10] key # max. 10 characters as key / name +float32 value # the value to send as debug output diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugValue.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugValue.msg new file mode 100644 index 00000000..8be13124 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugValue.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +int8 ind # index of debug variable +float32 value # the value to send as debug output diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugVect.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugVect.msg new file mode 100644 index 00000000..9c22e1da --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DebugVect.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +char[10] name # max. 10 characters as key / name +float32 x # x value +float32 y # y value +float32 z # z value diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialDriveSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialDriveSetpoint.msg new file mode 100644 index 00000000..f7e4c584 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialDriveSetpoint.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 speed # [m/s] collective roll-off speed in body x-axis +bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward +float32 yaw_rate # [rad/s] yaw rate +bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward + +# TOPICS differential_drive_setpoint differential_drive_control_output diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialPressure.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialPressure.msg new file mode 100644 index 00000000..0cdf1e4c --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialPressure.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 differential_pressure_pa # differential pressure reading in Pascals (may be negative) + +float32 temperature # Temperature provided by sensor in degrees Celsius, NAN if unknown + +uint32 error_count # Number of errors detected by driver diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DistanceSensor.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DistanceSensor.msg new file mode 100644 index 00000000..dd08e4b5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/DistanceSensor.msg @@ -0,0 +1,42 @@ +# DISTANCE_SENSOR message data + +uint64 timestamp # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 min_distance # Minimum distance the sensor can measure (in m) +float32 max_distance # Maximum distance the sensor can measure (in m) +float32 current_distance # Current distance reading (in m) +float32 variance # Measurement variance (in m^2), 0 for unknown / invalid readings +int8 signal_quality # Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. + +uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 +uint8 MAV_DISTANCE_SENSOR_RADAR = 3 + +float32 h_fov # Sensor horizontal field of view (rad) +float32 v_fov # Sensor vertical field of view (rad) +float32[4] q # Quaterion sensor orientation with respect to the vehicle body frame to specify the orientation ROTATION_CUSTOM + +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + +uint8 ROTATION_YAW_0 = 0 # MAV_SENSOR_ROTATION_NONE +uint8 ROTATION_YAW_45 = 1 # MAV_SENSOR_ROTATION_YAW_45 +uint8 ROTATION_YAW_90 = 2 # MAV_SENSOR_ROTATION_YAW_90 +uint8 ROTATION_YAW_135 = 3 # MAV_SENSOR_ROTATION_YAW_135 +uint8 ROTATION_YAW_180 = 4 # MAV_SENSOR_ROTATION_YAW_180 +uint8 ROTATION_YAW_225 = 5 # MAV_SENSOR_ROTATION_YAW_225 +uint8 ROTATION_YAW_270 = 6 # MAV_SENSOR_ROTATION_YAW_270 +uint8 ROTATION_YAW_315 = 7 # MAV_SENSOR_ROTATION_YAW_315 + +uint8 ROTATION_FORWARD_FACING = 0 # MAV_SENSOR_ROTATION_NONE +uint8 ROTATION_RIGHT_FACING = 2 # MAV_SENSOR_ROTATION_YAW_90 +uint8 ROTATION_BACKWARD_FACING = 4 # MAV_SENSOR_ROTATION_YAW_180 +uint8 ROTATION_LEFT_FACING = 6 # MAV_SENSOR_ROTATION_YAW_270 + +uint8 ROTATION_UPWARD_FACING = 24 # MAV_SENSOR_ROTATION_PITCH_90 +uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270 + +uint8 ROTATION_CUSTOM = 100 # MAV_SENSOR_ROTATION_CUSTOM diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Ekf2Timestamps.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Ekf2Timestamps.msg new file mode 100644 index 00000000..ae3ac067 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Ekf2Timestamps.msg @@ -0,0 +1,23 @@ +# this message contains the (relative) timestamps of the sensor inputs used by EKF2. +# It can be used for reproducible replay. + +# the timestamp field is the ekf2 reference time and matches the timestamp of +# the sensor_combined topic. + +uint64 timestamp # time since system start (microseconds) + +int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative timestamps + # is set to this value, it means the associated sensor values did not update + +# timestamps are relative to the main timestamp and are in 0.1 ms (timestamp + +# *_timestamp_rel = absolute timestamp). For int16, this allows a maximum +# difference of +-3.2s to the sensor_combined topic. + +int16 airspeed_timestamp_rel +int16 distance_sensor_timestamp_rel +int16 optical_flow_timestamp_rel +int16 vehicle_air_data_timestamp_rel +int16 vehicle_magnetometer_timestamp_rel +int16 visual_odometry_timestamp_rel + +# Note: this is a high-rate logged topic, so it needs to be as small as possible diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EscReport.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EscReport.msg new file mode 100644 index 00000000..9a75c3d7 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EscReport.msg @@ -0,0 +1,27 @@ +uint64 timestamp # time since system start (microseconds) +uint32 esc_errorcount # Number of reported errors by ESC - if supported +int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported +float32 esc_voltage # Voltage measured from current ESC [V] - if supported +float32 esc_current # Current measured from current ESC [A] - if supported +float32 esc_temperature # Temperature measured from current ESC [degC] - if supported +uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) +uint8 esc_cmdcount # Counter of number of commands + +uint8 esc_state # State of ESC - depend on Vendor + +uint8 actuator_function # actuator output function (one of Motor1...MotorN) + +uint16 failures # Bitmask to indicate the internal ESC faults +int8 esc_power # Applied power 0-100 in % (negative values reserved) + +uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0) +uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1) +uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2) +uint8 FAILURE_OVER_RPM = 3 # (1 << 3) +uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries) +uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5) +uint8 FAILURE_GENERIC = 6 # (1 << 6) +uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7) +uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8) +uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9) +uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element! diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EscStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EscStatus.msg new file mode 100644 index 00000000..e5e220ce --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EscStatus.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) +uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs + +uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC +uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC +uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM +uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C +uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus +uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot + +uint16 counter # incremented by the writing thread everytime new data is stored + +uint8 esc_count # number of connected ESCs +uint8 esc_connectiontype # how ESCs connected to the system + +uint8 esc_online_flags # Bitmask indicating which ESC is online/offline +# esc_online_flags bit 0 : Set to 1 if ESC0 is online +# esc_online_flags bit 1 : Set to 1 if ESC1 is online +# esc_online_flags bit 2 : Set to 1 if ESC2 is online +# esc_online_flags bit 3 : Set to 1 if ESC3 is online +# esc_online_flags bit 4 : Set to 1 if ESC4 is online +# esc_online_flags bit 5 : Set to 1 if ESC5 is online +# esc_online_flags bit 6 : Set to 1 if ESC6 is online +# esc_online_flags bit 7 : Set to 1 if ESC7 is online + +uint8 esc_armed_flags # Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set. + +EscReport[8] esc diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource1d.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource1d.msg new file mode 100644 index 00000000..7bd8ea76 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource1d.msg @@ -0,0 +1,27 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64 time_last_fuse + +float32 observation +float32 observation_variance + +float32 innovation +float32 innovation_filtered + +float32 innovation_variance + +float32 test_ratio # normalized innovation squared +float32 test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused + +# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt +# TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip +# TOPICS estimator_aid_src_fake_hgt +# TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource2d.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource2d.msg new file mode 100644 index 00000000..14e3ac3f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource2d.msg @@ -0,0 +1,26 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64 time_last_fuse + +float32[2] observation +float32[2] observation_variance + +float32[2] innovation +float32[2] innovation_filtered + +float32[2] innovation_variance + +float32[2] test_ratio # normalized innovation squared +float32[2] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused + +# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position +# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow +# TOPICS estimator_aid_src_drag diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource3d.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource3d.msg new file mode 100644 index 00000000..b89add28 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource3d.msg @@ -0,0 +1,24 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64 time_last_fuse + +float32[3] observation +float32[3] observation_variance + +float32[3] innovation +float32[3] innovation_filtered + +float32[3] innovation_variance + +float32[3] test_ratio # normalized innovation squared +float32[3] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused + +# TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias.msg new file mode 100644 index 00000000..bb65e47b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles +float32 bias # estimated barometric altitude bias (m) +float32 bias_var # estimated barometric altitude bias variance (m^2) + +float32 innov # innovation of the last measurement fusion (m) +float32 innov_var # innovation variance of the last measurement fusion (m^2) +float32 innov_test_ratio # normalized innovation squared test ratio + +# TOPICS estimator_baro_bias estimator_gnss_hgt_bias diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias3d.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias3d.msg new file mode 100644 index 00000000..16b29372 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias3d.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[3] bias # estimated barometric altitude bias (m) +float32[3] bias_var # estimated barometric altitude bias variance (m^2) + +float32[3] innov # innovation of the last measurement fusion (m) +float32[3] innov_var # innovation variance of the last measurement fusion (m^2) +float32[3] innov_test_ratio # normalized innovation squared test ratio + +# TOPICS estimator_bias3d +# TOPICS estimator_ev_pos_bias diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorEventFlags.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorEventFlags.msg new file mode 100644 index 00000000..1a47e676 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorEventFlags.msg @@ -0,0 +1,37 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +# information events +uint32 information_event_changes # number of information event changes +bool gps_checks_passed # 0 - true when gps quality checks are passing passed +bool reset_vel_to_gps # 1 - true when the velocity states are reset to the gps measurement +bool reset_vel_to_flow # 2 - true when the velocity states are reset using the optical flow measurement +bool reset_vel_to_vision # 3 - true when the velocity states are reset to the vision system measurement +bool reset_vel_to_zero # 4 - true when the velocity states are reset to zero +bool reset_pos_to_last_known # 5 - true when the position states are reset to the last known position +bool reset_pos_to_gps # 6 - true when the position states are reset to the gps measurement +bool reset_pos_to_vision # 7 - true when the position states are reset to the vision system measurement +bool starting_gps_fusion # 8 - true when the filter starts using gps measurements to correct the state estimates +bool starting_vision_pos_fusion # 9 - true when the filter starts using vision system position measurements to correct the state estimates +bool starting_vision_vel_fusion # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates +bool starting_vision_yaw_fusion # 11 - true when the filter starts using vision system yaw measurements to correct the state estimates +bool yaw_aligned_to_imu_gps # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data +bool reset_hgt_to_baro # 13 - true when the vertical position state is reset to the baro measurement +bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement +bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement +bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement + +# warning events +uint32 warning_event_changes # number of warning event changes +bool gps_quality_poor # 0 - true when the gps is failing quality checks +bool gps_fusion_timout # 1 - true when the gps data has not been used to correct the state estimates for a significant time period +bool gps_data_stopped # 2 - true when the gps data has stopped for a significant time period +bool gps_data_stopped_using_alternate # 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation +bool height_sensor_timeout # 4 - true when the height sensor has not been used to correct the state estimates for a significant time period +bool stopping_navigation # 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation +bool invalid_accel_bias_cov_reset # 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements +bool bad_yaw_using_gps_course # 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course +bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data +bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period +bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data +bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorGpsStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorGpsStatus.msg new file mode 100644 index 00000000..2d2462ee --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorGpsStatus.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool checks_passed + +bool check_fail_gps_fix # 0 : insufficient fix type (no 3D solution) +bool check_fail_min_sat_count # 1 : minimum required sat count fail +bool check_fail_max_pdop # 2 : maximum allowed PDOP fail +bool check_fail_max_horz_err # 3 : maximum allowed horizontal position error fail +bool check_fail_max_vert_err # 4 : maximum allowed vertical position error fail +bool check_fail_max_spd_err # 5 : maximum allowed speed error fail +bool check_fail_max_horz_drift # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle +bool check_fail_max_vert_drift # 7 : maximum allowed vertical position drift fail - requires stationary vehicle +bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle +bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail + +float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s) +float32 position_drift_rate_vertical_m_s # Vertical position rate magnitude (m/s) +float32 filtered_horizontal_speed_m_s # Filtered horizontal velocity magnitude (m/s) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorInnovations.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorInnovations.msg new file mode 100644 index 00000000..11cc6a58 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorInnovations.msg @@ -0,0 +1,39 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +# GPS +float32[2] gps_hvel # horizontal GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32 gps_vvel # vertical GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32[2] gps_hpos # horizontal GPS position innovation (m) and innovation variance (m**2) +float32 gps_vpos # vertical GPS position innovation (m) and innovation variance (m**2) + +# External Vision +float32[2] ev_hvel # horizontal external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32 ev_vvel # vertical external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32[2] ev_hpos # horizontal external vision position innovation (m) and innovation variance (m**2) +float32 ev_vpos # vertical external vision position innovation (m) and innovation variance (m**2) + +# Height sensors +float32 rng_vpos # range sensor height innovation (m) and innovation variance (m**2) +float32 baro_vpos # barometer height innovation (m) and innovation variance (m**2) + +# Auxiliary velocity +float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) + +# Optical flow +float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2) + +# Various +float32 heading # heading innovation (rad) and innovation variance (rad**2) +float32[3] mag_field # earth magnetic field innovation (Gauss) and innovation variance (Gauss**2) +float32[3] gravity # gravity innovation from accelerometerr vector (m/s**2) +float32[2] drag # drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2) +float32 airspeed # airspeed innovation (m/sec) and innovation variance ((m/sec)**2) +float32 beta # synthetic sideslip innovation (rad) and innovation variance (rad**2) +float32 hagl # height of ground innovation (m) and innovation variance (m**2) +float32 hagl_rate # height of ground rate innovation (m/s) and innovation variance ((m/s)**2) + +# The innovation test ratios are scalar values. In case the field is a vector, +# the test ratio will be put in the first component of the vector. + +# TOPICS estimator_innovations estimator_innovation_variances estimator_innovation_test_ratios diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSelectorStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSelectorStatus.msg new file mode 100644 index 00000000..52f80859 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSelectorStatus.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 primary_instance + +uint8 instances_available + +uint32 instance_changed_count +uint64 last_instance_change + +uint32 accel_device_id +uint32 baro_device_id +uint32 gyro_device_id +uint32 mag_device_id + +float32[9] combined_test_ratio +float32[9] relative_test_ratio +bool[9] healthy + +float32[4] accumulated_gyro_error +float32[4] accumulated_accel_error +bool gyro_fault_detected +bool accel_fault_detected diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSensorBias.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSensorBias.msg new file mode 100644 index 00000000..f42e1aa8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSensorBias.msg @@ -0,0 +1,30 @@ +# +# Sensor readings and in-run biases in SI-unit form. Sensor readings are compensated for static offsets, +# scale errors, in-run bias and thermal drift (if thermal compensation is enabled and available). +# + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +# In-run bias estimates (subtract from uncorrected data) + +uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles +float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) +float32 gyro_bias_limit # magnitude of maximum gyroscope in-run bias in body frame (rad/s) +float32[3] gyro_bias_variance +bool gyro_bias_valid +bool gyro_bias_stable # true when the gyro bias estimate is stable enough to use for calibration + +uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles +float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) +float32 accel_bias_limit # magnitude of maximum accelerometer in-run bias in body frame (m/s^2) +float32[3] accel_bias_variance +bool accel_bias_valid +bool accel_bias_stable # true when the accel bias estimate is stable enough to use for calibration + +uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles +float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) +float32 mag_bias_limit # magnitude of maximum magnetometer in-run bias in body frame (Gauss) +float32[3] mag_bias_variance +bool mag_bias_valid +bool mag_bias_stable # true when the mag bias estimate is stable enough to use for calibration diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStates.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStates.msg new file mode 100644 index 00000000..885246d8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStates.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[25] states # Internal filter states +uint8 n_states # Number of states effectively used + +float32[24] covariances # Diagonal Elements of Covariance Matrix diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatus.msg new file mode 100644 index 00000000..ac13b59e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatus.msg @@ -0,0 +1,121 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) + +uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see definition below +# bits are true when corresponding test has failed +uint8 GPS_CHECK_FAIL_GPS_FIX = 0 # 0 : insufficient fix type (no 3D solution) +uint8 GPS_CHECK_FAIL_MIN_SAT_COUNT = 1 # 1 : minimum required sat count fail +uint8 GPS_CHECK_FAIL_MAX_PDOP = 2 # 2 : maximum allowed PDOP fail +uint8 GPS_CHECK_FAIL_MAX_HORZ_ERR = 3 # 3 : maximum allowed horizontal position error fail +uint8 GPS_CHECK_FAIL_MAX_VERT_ERR = 4 # 4 : maximum allowed vertical position error fail +uint8 GPS_CHECK_FAIL_MAX_SPD_ERR = 5 # 5 : maximum allowed speed error fail +uint8 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6 # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle +uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position drift fail - requires stationary vehicle +uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle +uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail + +uint64 control_mode_flags # Bitmask to indicate EKF logic state +uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete +uint8 CS_YAW_ALIGN = 1 # 1 - true if the filter yaw alignment is complete +uint8 CS_GPS = 2 # 2 - true if GPS measurements are being fused +uint8 CS_OPT_FLOW = 3 # 3 - true if optical flow measurements are being fused +uint8 CS_MAG_HDG = 4 # 4 - true if a simple magnetic yaw heading is being fused +uint8 CS_MAG_3D = 5 # 5 - true if 3-axis magnetometer measurement are being fused +uint8 CS_MAG_DEC = 6 # 6 - true if synthetic magnetic declination measurements are being fused +uint8 CS_IN_AIR = 7 # 7 - true when thought to be airborne +uint8 CS_WIND = 8 # 8 - true when wind velocity is being estimated +uint8 CS_BARO_HGT = 9 # 9 - true when baro height is being fused as a primary height reference +uint8 CS_RNG_HGT = 10 # 10 - true when range finder height is being fused as a primary height reference +uint8 CS_GPS_HGT = 11 # 11 - true when GPS height is being fused as a primary height reference +uint8 CS_EV_POS = 12 # 12 - true when local position data from external vision is being fused +uint8 CS_EV_YAW = 13 # 13 - true when yaw data from external vision measurements is being fused +uint8 CS_EV_HGT = 14 # 14 - true when height data from external vision measurements is being fused +uint8 CS_BETA = 15 # 15 - true when synthetic sideslip measurements are being fused +uint8 CS_MAG_FIELD = 16 # 16 - true when only the magnetic field states are updated by the magnetometer +uint8 CS_FIXED_WING = 17 # 17 - true when thought to be operating as a fixed wing vehicle with constrained sideslip +uint8 CS_MAG_FAULT = 18 # 18 - true when the magnetometer has been declared faulty and is no longer being used +uint8 CS_ASPD = 19 # 19 - true when airspeed measurements are being fused +uint8 CS_GND_EFFECT = 20 # 20 - true when when protection from ground effect induced static pressure rise is active +uint8 CS_RNG_STUCK = 21 # 21 - true when a stuck range finder sensor has been detected +uint8 CS_GPS_YAW = 22 # 22 - true when yaw (not ground course) data from a GPS receiver is being fused +uint8 CS_MAG_ALIGNED = 23 # 23 - true when the in-flight mag field alignment has been completed +uint8 CS_EV_VEL = 24 # 24 - true when local frame velocity data fusion from external vision measurements is intended +uint8 CS_SYNTHETIC_MAG_Z = 25 # 25 - true when we are using a synthesized measurement for the magnetometer Z component +uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest +uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used +uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used + +uint32 filter_fault_flags # Bitmask to indicate EKF internal faults +# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +# 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +# 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +# 3 - true if the fusion of the magnetic heading has encountered a numerical error +# 4 - true if the fusion of the magnetic declination has encountered a numerical error +# 5 - true if fusion of the airspeed has encountered a numerical error +# 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +# 7 - true if fusion of the optical flow X axis has encountered a numerical error +# 8 - true if fusion of the optical flow Y axis has encountered a numerical error +# 9 - true if fusion of the North velocity has encountered a numerical error +# 10 - true if fusion of the East velocity has encountered a numerical error +# 11 - true if fusion of the Down velocity has encountered a numerical error +# 12 - true if fusion of the North position has encountered a numerical error +# 13 - true if fusion of the East position has encountered a numerical error +# 14 - true if fusion of the Down position has encountered a numerical error +# 15 - true if bad delta velocity bias estimates have been detected +# 16 - true if bad vertical accelerometer data has been detected +# 17 - true if delta velocity data contains clipping (asymmetric railing) + +float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m) +float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m) + +float32 mag_test_ratio # low-pass filtered ratio of the largest magnetometer innovation component to the innovation test limit +float32 vel_test_ratio # low-pass filtered ratio of the largest velocity innovation component to the innovation test limit +float32 pos_test_ratio # low-pass filtered ratio of the largest horizontal position innovation component to the innovation test limit +float32 hgt_test_ratio # low-pass filtered ratio of the vertical position innovation to the innovation test limit +float32 tas_test_ratio # low-pass filtered ratio of the true airspeed innovation to the innovation test limit +float32 hagl_test_ratio # low-pass filtered ratio of the height above ground innovation to the innovation test limit +float32 beta_test_ratio # low-pass filtered ratio of the synthetic sideslip innovation to the innovation test limit + +uint16 solution_status_flags # Bitmask indicating which filter kinematic state outputs are valid for flight control use. +# 0 - True if the attitude estimate is good +# 1 - True if the horizontal velocity estimate is good +# 2 - True if the vertical velocity estimate is good +# 3 - True if the horizontal position (relative) estimate is good +# 4 - True if the horizontal position (absolute) estimate is good +# 5 - True if the vertical position (absolute) estimate is good +# 6 - True if the vertical position (above ground) estimate is good +# 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) +# 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate +# 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate +# 10 - True if the EKF has detected a GPS glitch +# 11 - True if the EKF has detected bad accelerometer data + +uint8 reset_count_vel_ne # number of horizontal position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_vel_d # number of vertical velocity reset events (allow to wrap if count exceeds 255) +uint8 reset_count_pos_ne # number of horizontal position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_pod_d # number of vertical position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_quat # number of quaternion reset events (allow to wrap if count exceeds 255) + +float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time + +bool pre_flt_fail_innov_heading +bool pre_flt_fail_innov_vel_horiz +bool pre_flt_fail_innov_vel_vert +bool pre_flt_fail_innov_height +bool pre_flt_fail_mag_field_disturbed + +uint32 accel_device_id +uint32 gyro_device_id +uint32 baro_device_id +uint32 mag_device_id + +# legacy local position estimator (LPE) flags +uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) +uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) + +float32 mag_inclination_deg +float32 mag_inclination_ref_deg +float32 mag_strength_gs +float32 mag_strength_ref_gs diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatusFlags.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatusFlags.msg new file mode 100644 index 00000000..c6e0504f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatusFlags.msg @@ -0,0 +1,74 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + + +# filter control status +uint32 control_status_changes # number of filter control status (cs) changes +bool cs_tilt_align # 0 - true if the filter tilt alignment is complete +bool cs_yaw_align # 1 - true if the filter yaw alignment is complete +bool cs_gps # 2 - true if GPS measurement fusion is intended +bool cs_opt_flow # 3 - true if optical flow measurements fusion is intended +bool cs_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended +bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fusion is intended +bool cs_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended +bool cs_in_air # 7 - true when the vehicle is airborne +bool cs_wind # 8 - true when wind velocity is being estimated +bool cs_baro_hgt # 9 - true when baro height is being fused as a primary height reference +bool cs_rng_hgt # 10 - true when range finder height is being fused as a primary height reference +bool cs_gps_hgt # 11 - true when GPS height is being fused as a primary height reference +bool cs_ev_pos # 12 - true when local position data fusion from external vision is intended +bool cs_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended +bool cs_ev_hgt # 14 - true when height data from external vision measurements is being fused +bool cs_fuse_beta # 15 - true when synthetic sideslip measurements are being fused +bool cs_mag_field_disturbed # 16 - true when the mag field does not match the expected strength +bool cs_fixed_wing # 17 - true when the vehicle is operating as a fixed wing vehicle +bool cs_mag_fault # 18 - true when the magnetometer has been declared faulty and is no longer being used +bool cs_fuse_aspd # 19 - true when airspeed measurements are being fused +bool cs_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active +bool cs_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough +bool cs_gps_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed +bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended +bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component +bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest +bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used +bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used +bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift +bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements +bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing +bool cs_fake_pos # 32 - true when fake position measurements are being fused +bool cs_fake_hgt # 33 - true when fake height measurements are being fused +bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused +bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended +bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used +bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter +bool cs_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended + +# fault status +uint32 fault_status_changes # number of filter fault status (fs) changes +bool fs_bad_mag_x # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +bool fs_bad_mag_y # 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +bool fs_bad_mag_z # 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +bool fs_bad_hdg # 3 - true if the fusion of the heading angle has encountered a numerical error +bool fs_bad_mag_decl # 4 - true if the fusion of the magnetic declination has encountered a numerical error +bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encountered a numerical error +bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error +bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error +bool fs_bad_acc_bias # 9 - true if bad delta velocity bias estimates have been detected +bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected +bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing) + + +# innovation test failures +uint32 innovation_fault_status_changes # number of innovation fault status (reject) changes +bool reject_hor_vel # 0 - true if horizontal velocity observations have been rejected +bool reject_ver_vel # 1 - true if vertical velocity observations have been rejected +bool reject_hor_pos # 2 - true if horizontal position observations have been rejected +bool reject_ver_pos # 3 - true if vertical position observations have been rejected +bool reject_yaw # 7 - true if the yaw observation has been rejected +bool reject_airspeed # 8 - true if the airspeed observation has been rejected +bool reject_sideslip # 9 - true if the synthetic sideslip observation has been rejected +bool reject_hagl # 10 - true if the height above ground observation has been rejected +bool reject_optflow_x # 11 - true if the X optical flow observation has been rejected +bool reject_optflow_y # 12 - true if the Y optical flow observation has been rejected diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Event.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Event.msg new file mode 100644 index 00000000..df1dd4a9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Event.msg @@ -0,0 +1,10 @@ +# Events interface +uint64 timestamp # time since system start (microseconds) + +uint32 id # Event ID +uint16 event_sequence # Event sequence number +uint8[25] arguments # (optional) arguments, depend on event id + +uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external + +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FailsafeFlags.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FailsafeFlags.msg new file mode 100644 index 00000000..44945afa --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FailsafeFlags.msg @@ -0,0 +1,59 @@ +# Input flags for the failsafe state machine set by the arming & health checks. +# +# Flags must be named such that false == no failure (e.g. _invalid, _unhealthy, _lost) +# The flag comments are used as label for the failsafe state machine simulation + +uint64 timestamp # time since system start (microseconds) + +# Per-mode requirements +uint32 mode_req_angular_velocity +uint32 mode_req_attitude +uint32 mode_req_local_alt +uint32 mode_req_local_position +uint32 mode_req_local_position_relaxed +uint32 mode_req_global_position +uint32 mode_req_mission +uint32 mode_req_offboard_signal +uint32 mode_req_home_position +uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded +uint32 mode_req_prevent_arming # if set, cannot arm while in this mode +uint32 mode_req_manual_control +uint32 mode_req_other # other requirements, not covered above (for external modes) + + +# Mode requirements +bool angular_velocity_invalid # Angular velocity invalid +bool attitude_invalid # Attitude invalid +bool local_altitude_invalid # Local altitude invalid +bool local_position_invalid # Local position estimate invalid +bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow) +bool local_velocity_invalid # Local velocity estimate invalid +bool global_position_invalid # Global position estimate invalid +bool auto_mission_missing # No mission available +bool offboard_control_signal_lost # Offboard signal lost +bool home_position_invalid # No home position available + +# Control links +bool manual_control_signal_lost # Manual control (RC) signal lost +bool gcs_connection_lost # GCS connection lost + +# Battery +uint8 battery_warning # Battery warning level +bool battery_low_remaining_time # Low battery based on remaining flight time +bool battery_unhealthy # Battery unhealthy + +# Other +bool geofence_breached # Geofence breached (one or multiple) +bool mission_failure # Mission failure +bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) +bool wind_limit_exceeded # Wind limit exceeded +bool flight_time_limit_exceeded # Maximum flight time exceeded +bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid + +# Failure detector +bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) +bool fd_esc_arming_failure # ESC failed to arm +bool fd_imbalanced_prop # Imbalanced propeller detected +bool fd_motor_failure # Motor failure + + diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FailureDetectorStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FailureDetectorStatus.msg new file mode 100644 index 00000000..923ceb36 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FailureDetectorStatus.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) + +# FailureDetector status +bool fd_roll +bool fd_pitch +bool fd_alt +bool fd_ext +bool fd_arm_escs +bool fd_battery +bool fd_imbalanced_prop +bool fd_motor + +float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed) +uint16 motor_failure_mask # Bit-mask with motor indices, indicating critical motor failures diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FigureEightStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FigureEightStatus.msg new file mode 100644 index 00000000..e14d8f0d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FigureEightStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +float32 major_radius # Major axis radius of the figure eight [m]. Positive values orbit clockwise, negative values orbit counter-clockwise. +float32 minor_radius # Minor axis radius of the figure eight [m]. +float32 orientation # Orientation of the major axis of the figure eight [rad]. +uint8 frame # The coordinate system of the fields: x, y, z. +int32 x # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. +int32 y # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7. +float32 z # Altitude of center point. Coordinate system depends on frame field. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FlightPhaseEstimation.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FlightPhaseEstimation.msg new file mode 100644 index 00000000..e05b3291 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FlightPhaseEstimation.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 flight_phase # Estimate of current flight phase + +uint8 FLIGHT_PHASE_UNKNOWN = 0 # vehicle flight phase is unknown +uint8 FLIGHT_PHASE_LEVEL = 1 # Vehicle is in level flight +uint8 FLIGHT_PHASE_DESCEND = 2 # vehicle is in descend +uint8 FLIGHT_PHASE_CLIMB = 3 # vehicle is climbing diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTarget.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTarget.msg new file mode 100644 index 00000000..e88c2460 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTarget.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +float64 lat # target position (deg * 1e7) +float64 lon # target position (deg * 1e7) +float32 alt # target position + +float32 vy # target vel in y +float32 vx # target vel in x +float32 vz # target vel in z + +uint8 est_cap # target reporting capabilities diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetEstimator.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetEstimator.msg new file mode 100644 index 00000000..9d3df9f6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetEstimator.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) +uint64 last_filter_reset_timestamp # time of last filter reset (microseconds) + +bool valid # True if estimator states are okay to be used +bool stale # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate. + +float64 lat_est # Estimated target latitude +float64 lon_est # Estimated target longitude +float32 alt_est # Estimated target altitude + +float32[3] pos_est # Estimated target NED position (m) +float32[3] vel_est # Estimated target NED velocity (m/s) +float32[3] acc_est # Estimated target NED acceleration (m^2/s) + +uint64 prediction_count +uint64 fusion_count diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetStatus.msg new file mode 100644 index 00000000..713a7dcd --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # [microseconds] time since system start + +float32 tracked_target_course # [rad] Tracked target course in NED local frame (North is course zero) +float32 follow_angle # [rad] Current follow angle setting + +float32 orbit_angle_setpoint # [rad] Current orbit angle setpoint from the smooth trajectory generator +float32 angular_rate_setpoint # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle + +float32[3] desired_position_raw # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places + +bool in_emergency_ascent # [bool] True when doing emergency ascent (when distance to ground is below safety altitude) +float32 gimbal_pitch # [rad] Gimbal pitch commanded to track target in the center of the frame diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FuelTankStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FuelTankStatus.msg new file mode 100644 index 00000000..22d21e4a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/FuelTankStatus.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +float32 maximum_fuel_capacity # maximum fuel capacity. Must always be provided, either from the driver or a parameter +float32 consumed_fuel # consumed fuel, NaN if not measured. Should not be inferred from the max fuel capacity +float32 fuel_consumption_rate # fuel consumption rate, NaN if not measured + +uint8 percent_remaining # percentage of remaining fuel, UINT8_MAX if not provided +float32 remaining_fuel # remaining fuel, NaN if not measured. Should not be inferred from the max fuel capacity + +uint8 fuel_tank_id # identifier for the fuel tank. Must match ID of other messages for same fuel system. 0 by default when only a single tank exists + +uint32 fuel_type # type of fuel based on MAV_FUEL_TYPE enum. Set to MAV_FUEL_TYPE_UNKNOWN if unknown or it does not fit the provided types +uint8 MAV_FUEL_TYPE_UNKNOWN = 0 # fuel type not specified. Fuel levels are normalized (i.e., maximum is 1, and other levels are relative to 1). +uint8 MAV_FUEL_TYPE_LIQUID = 1 # represents generic liquid fuels, such as gasoline or diesel. Fuel levels are measured in millilitres (ml), and flow rates in millilitres per second (ml/s). +uint8 MAV_FUEL_TYPE_GAS = 2 # represents a gas fuel, such as hydrogen, methane, or propane. Fuel levels are in kilo-Pascal (kPa), and flow rates are in milliliters per second (ml/s). + +float32 temperature # fuel temperature in Kelvin, NaN if not measured diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeneratorStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeneratorStatus.msg new file mode 100644 index 00000000..7ba9b402 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeneratorStatus.msg @@ -0,0 +1,44 @@ +uint64 timestamp # time since system start (microseconds) + + +uint64 STATUS_FLAG_OFF = 1 # Generator is off. +uint64 STATUS_FLAG_READY = 2 # Generator is ready to start generating power. +uint64 STATUS_FLAG_GENERATING = 4 # Generator is generating power. +uint64 STATUS_FLAG_CHARGING = 8 # Generator is charging the batteries (generating enough power to charge and provide the load). +uint64 STATUS_FLAG_REDUCED_POWER = 16 # Generator is operating at a reduced maximum power. +uint64 STATUS_FLAG_MAXPOWER = 32 # Generator is providing the maximum output. +uint64 STATUS_FLAG_OVERTEMP_WARNING = 64 # Generator is near the maximum operating temperature, cooling is insufficient. +uint64 STATUS_FLAG_OVERTEMP_FAULT = 128 # Generator hit the maximum operating temperature and shutdown. +uint64 STATUS_FLAG_ELECTRONICS_OVERTEMP_WARNING = 256 # Power electronics are near the maximum operating temperature, cooling is insufficient. +uint64 STATUS_FLAG_ELECTRONICS_OVERTEMP_FAULT = 512 # Power electronics hit the maximum operating temperature and shutdown. +uint64 STATUS_FLAG_ELECTRONICS_FAULT = 1024 # Power electronics experienced a fault and shutdown. +uint64 STATUS_FLAG_POWERSOURCE_FAULT = 2048 # The power source supplying the generator failed e.g. mechanical generator stopped, tether is no longer providing power, solar cell is in shade, hydrogen reaction no longer happening. +uint64 STATUS_FLAG_COMMUNICATION_WARNING = 4096 # Generator controller having communication problems. +uint64 STATUS_FLAG_COOLING_WARNING = 8192 # Power electronic or generator cooling system error. +uint64 STATUS_FLAG_POWER_RAIL_FAULT = 16384 # Generator controller power rail experienced a fault. +uint64 STATUS_FLAG_OVERCURRENT_FAULT = 32768 # Generator controller exceeded the overcurrent threshold and shutdown to prevent damage. +uint64 STATUS_FLAG_BATTERY_OVERCHARGE_CURRENT_FAULT = 65536 # Generator controller detected a high current going into the batteries and shutdown to prevent battery damage. | +uint64 STATUS_FLAG_OVERVOLTAGE_FAULT = 131072 # Generator controller exceeded it's overvoltage threshold and shutdown to prevent it exceeding the voltage rating. +uint64 STATUS_FLAG_BATTERY_UNDERVOLT_FAULT = 262144 # Batteries are under voltage (generator will not start). +uint64 STATUS_FLAG_START_INHIBITED = 524288 # Generator start is inhibited by e.g. a safety switch. +uint64 STATUS_FLAG_MAINTENANCE_REQUIRED = 1048576 # Generator requires maintenance. +uint64 STATUS_FLAG_WARMING_UP = 2097152 # Generator is not ready to generate yet. +uint64 STATUS_FLAG_IDLE = 4194304 # Generator is idle. + +uint64 status # Status flags + + +float32 battery_current # [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. +float32 load_current # [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided +float32 power_generated # [W] The power being generated. NaN: field not provided +float32 bus_voltage # [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. +float32 bat_current_setpoint # [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + +uint32 runtime # [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + +int32 time_until_maintenance # [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + +uint16 generator_speed # [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + +int16 rectifier_temperature # [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. +int16 generator_temperature # [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceResult.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceResult.msg new file mode 100644 index 00000000..7782d1d6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceResult.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination +uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND + +bool geofence_max_dist_triggered # true the check for max distance from Home is triggered +bool geofence_max_alt_triggered # true the check for max altitude above Home is triggered +bool geofence_custom_fence_triggered # true the check for custom inclusion/exclusion geofence(s) is triggered + +uint8 geofence_action # action to take when the geofence is breached diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceStatus.msg new file mode 100644 index 00000000..d32b9010 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 geofence_id # loaded geofence id +uint8 status # Current geofence status + +uint8 GF_STATUS_LOADING = 0 +uint8 GF_STATUS_READY = 1 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalControls.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalControls.msg new file mode 100644 index 00000000..3e1c5a9d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalControls.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 + +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[3] control diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceAttitudeStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceAttitudeStatus.msg new file mode 100644 index 00000000..0be66bab --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceAttitudeStatus.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component +uint16 device_flags + +uint16 DEVICE_FLAGS_RETRACT = 1 +uint16 DEVICE_FLAGS_NEUTRAL = 2 +uint16 DEVICE_FLAGS_ROLL_LOCK = 4 +uint16 DEVICE_FLAGS_PITCH_LOCK = 8 +uint16 DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z + +uint32 failure_flags + +bool received_from_mavlink diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceInformation.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceInformation.msg new file mode 100644 index 00000000..8f7a4164 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceInformation.msg @@ -0,0 +1,36 @@ +uint64 timestamp # time since system start (microseconds) + +uint8[32] vendor_name +uint8[32] model_name +uint8[32] custom_name +uint32 firmware_version +uint32 hardware_version +uint64 uid + +uint16 cap_flags + +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 + +uint16 custom_cap_flags + +float32 roll_min # [rad] +float32 roll_max # [rad] + +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] + +uint8 gimbal_device_compid diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceSetAttitude.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceSetAttitude.msg new file mode 100644 index 00000000..f224a234 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceSetAttitude.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component + +uint16 flags +uint32 GIMBAL_DEVICE_FLAGS_RETRACT = 1 +uint32 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerInformation.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerInformation.msg new file mode 100644 index 00000000..28db68a4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerInformation.msg @@ -0,0 +1,29 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 cap_flags + +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 + +uint8 gimbal_device_id + +float32 roll_min # [rad] +float32 roll_max # [rad] + +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetAttitude.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetAttitude.msg new file mode 100644 index 00000000..d88acca8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetAttitude.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 + +uint32 flags +uint8 gimbal_device_id + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetManualControl.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetManualControl.msg new file mode 100644 index 00000000..4061438f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetManualControl.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 + +uint32 flags +uint8 gimbal_device_id + +float32 pitch # unitless -1..1, can be NAN +float32 yaw # unitless -1..1, can be NAN +float32 pitch_rate # unitless -1..1, can be NAN +float32 yaw_rate # unitless -1..1, can be NAN diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerStatus.msg new file mode 100644 index 00000000..002e5c90 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 flags +uint8 gimbal_device_id +uint8 primary_control_sysid +uint8 primary_control_compid +uint8 secondary_control_sysid +uint8 secondary_control_compid diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GotoSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GotoSetpoint.msg new file mode 100644 index 00000000..5fe3ab8a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GotoSetpoint.msg @@ -0,0 +1,24 @@ +# Position and (optional) heading setpoints with corresponding speed constraints +# Setpoints are intended as inputs to position and heading smoothers, respectively +# Setpoints do not need to be kinematically consistent +# Optional heading setpoints may be specified as controlled by the respective flag +# Unset optional setpoints are not controlled +# Unset optional constraints default to vehicle specifications + +uint64 timestamp # time since system start (microseconds) + +# setpoints +float32[3] position # [m] NED local world frame + +bool flag_control_heading # true if heading is to be controlled +float32 heading # (optional) [rad] [-pi,pi] from North + +# constraints +bool flag_set_max_horizontal_speed # true if setting a non-default horizontal speed limit +float32 max_horizontal_speed # (optional) [m/s] maximum speed (absolute) in the NE-plane + +bool flag_set_max_vertical_speed # true if setting a non-default vertical speed limit +float32 max_vertical_speed # (optional) [m/s] maximum speed (absolute) in the D-axis + +bool flag_set_max_heading_rate # true if setting a non-default heading rate limit +float32 max_heading_rate # (optional) [rad/s] maximum heading rate (absolute) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioConfig.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioConfig.msg new file mode 100644 index 00000000..0ff393ec --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioConfig.msg @@ -0,0 +1,28 @@ +# GPIO configuration + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 mask # Pin mask +uint32 state # Initial pin output state + +# Configuration Mask +# Bit 0-3: Direction: 0=Input, 1=Output +# Bit 4-7: Input Config: 0=Floating, 1=PullUp, 2=PullDown +# Bit 8-12: Output Config: 0=PushPull, 1=OpenDrain +# Bit 13-31: Reserved +uint32 INPUT = 0 # 0x0000 +uint32 OUTPUT = 1 # 0x0001 +uint32 PULLUP = 16 # 0x0010 +uint32 PULLDOWN = 32 # 0x0020 +uint32 OPENDRAIN = 256 # 0x0100 + +uint32 INPUT_FLOATING = 0 # 0x0000 +uint32 INPUT_PULLUP = 16 # 0x0010 +uint32 INPUT_PULLDOWN = 32 # 0x0020 + +uint32 OUTPUT_PUSHPULL = 0 # 0x0000 +uint32 OUTPUT_OPENDRAIN = 256 # 0x0100 +uint32 OUTPUT_OPENDRAIN_PULLUP = 272 # 0x0110 + +uint32 config diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioIn.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioIn.msg new file mode 100644 index 00000000..0482a218 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioIn.msg @@ -0,0 +1,6 @@ +# GPIO mask and state + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 state # pin state mask diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioOut.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioOut.msg new file mode 100644 index 00000000..3865bbf2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioOut.msg @@ -0,0 +1,7 @@ +# GPIO mask and state + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 mask # pin mask +uint32 state # pin state mask diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioRequest.msg new file mode 100644 index 00000000..3328b001 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpioRequest.msg @@ -0,0 +1,4 @@ +# Request GPIO mask to be read + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpsDump.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpsDump.msg new file mode 100644 index 00000000..2477bcfa --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpsDump.msg @@ -0,0 +1,10 @@ +# This message is used to dump the raw gps communication to the log. + +uint64 timestamp # time since system start (microseconds) + +uint8 instance # Instance of GNSS receiver +uint8 len # length of data, MSB bit set = message to the gps device, + # clear = message from the device +uint8[79] data # data to write to the log + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpsInjectData.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpsInjectData.msg new file mode 100644 index 00000000..516d5cb5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/GpsInjectData.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint16 len # length of data +uint8 flags # LSB: 1=fragmented +uint8[300] data # data to write to GPS device (RTCM message) + +uint8 ORB_QUEUE_LENGTH = 8 + +uint8 MAX_INSTANCES = 2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Gripper.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Gripper.msg new file mode 100644 index 00000000..4f1445cb --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Gripper.msg @@ -0,0 +1,7 @@ +## Used to command an actuation in the gripper, which is mapped to a specific output in the control allocation module + +uint64 timestamp + +int8 command # Commanded state for the gripper +int8 COMMAND_GRAB = 0 +int8 COMMAND_RELEASE = 1 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HealthReport.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HealthReport.msg new file mode 100644 index 00000000..18951805 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HealthReport.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 can_arm_mode_flags # bitfield for each flight mode (NAVIGATION_STATE_*) if arming is possible +uint64 can_run_mode_flags # bitfield for each flight mode if it can run + +uint64 health_is_present_flags # flags for each health_component_t +uint64 health_warning_flags +uint64 health_error_flags +# A component is required but missing, if present==0 and error==1 + +uint64 arming_check_warning_flags +uint64 arming_check_error_flags diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HeaterStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HeaterStatus.msg new file mode 100644 index 00000000..44207a39 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HeaterStatus.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 device_id + +bool heater_on +bool temperature_target_met + +float32 temperature_sensor +float32 temperature_target + +uint32 controller_period_usec +uint32 controller_time_on_usec + +float32 proportional_value +float32 integrator_value +float32 feed_forward_value + +uint8 MODE_GPIO = 1 +uint8 MODE_PX4IO = 2 +uint8 mode diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HomePosition.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HomePosition.msg new file mode 100644 index 00000000..e6a51728 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HomePosition.msg @@ -0,0 +1,21 @@ +# GPS home position in WGS84 coordinates. + +uint64 timestamp # time since system start (microseconds) + +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude in meters (AMSL) + +float32 x # X coordinate in meters +float32 y # Y coordinate in meters +float32 z # Z coordinate in meters + +float32 yaw # Yaw angle in radians + +bool valid_alt # true when the altitude has been set +bool valid_hpos # true when the latitude and longitude have been set +bool valid_lpos # true when the local position (xyz) has been set + +bool manual_home # true when home position was set manually + +uint32 update_count # update counter of the home position diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HoverThrustEstimate.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HoverThrustEstimate.msg new file mode 100644 index 00000000..a38d9042 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/HoverThrustEstimate.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time of corresponding sensor data last used for this estimate + +float32 hover_thrust # estimated hover thrust [0.1, 0.9] +float32 hover_thrust_var # estimated hover thrust variance + +float32 accel_innov # innovation of the last acceleration fusion +float32 accel_innov_var # innovation variance of the last acceleration fusion +float32 accel_innov_test_ratio # normalized innovation squared test ratio + +float32 accel_noise_var # vertical acceleration noise variance estimated form innovation residual + +bool valid diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/InputRc.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/InputRc.msg new file mode 100644 index 00000000..db4b3de2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/InputRc.msg @@ -0,0 +1,40 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 RC_INPUT_SOURCE_UNKNOWN = 0 +uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1 +uint8 RC_INPUT_SOURCE_PX4IO_PPM = 2 +uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 +uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 +uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 +uint8 RC_INPUT_SOURCE_MAVLINK = 6 +uint8 RC_INPUT_SOURCE_QURT = 7 +uint8 RC_INPUT_SOURCE_PX4FMU_SPEKTRUM = 8 +uint8 RC_INPUT_SOURCE_PX4FMU_SBUS = 9 +uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10 +uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11 +uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12 +uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13 +uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14 +uint8 RC_INPUT_SOURCE_PX4FMU_GHST = 15 + +uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. + +uint64 timestamp_last_signal # last valid reception time + +uint8 channel_count # number of channels actually being seen + +int8 RSSI_MAX = 100 +int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception + +bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. +bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usually means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. + +uint16 rc_lost_frame_count # Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike functionality. +uint16 rc_total_frame_count # Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike functionality. +uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM systems + +uint8 input_source # Input source +uint16[18] values # measured pulse widths for each of the supported channels + +int8 link_quality # link quality. Percentage 0-100%. -1 = invalid +float32 rssi_dbm # Actual rssi in units of dBm. NaN = invalid \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/InternalCombustionEngineStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/InternalCombustionEngineStatus.msg new file mode 100644 index 00000000..301eb92a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/InternalCombustionEngineStatus.msg @@ -0,0 +1,64 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 STATE_STOPPED = 0 # The engine is not running. This is the default state. +uint8 STATE_STARTING = 1 # The engine is starting. This is a transient state. +uint8 STATE_RUNNING = 2 # The engine is running normally. +uint8 STATE_FAULT = 3 # The engine can no longer function. +uint8 state + +uint32 FLAG_GENERAL_ERROR = 1 # General error. + +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED = 2 # Error of the crankshaft sensor. This flag is optional. +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR = 4 + +uint32 FLAG_TEMPERATURE_SUPPORTED = 8 # Temperature levels. These flags are optional +uint32 FLAG_TEMPERATURE_BELOW_NOMINAL = 16 # Under-temperature warning +uint32 FLAG_TEMPERATURE_ABOVE_NOMINAL = 32 # Over-temperature warning +uint32 FLAG_TEMPERATURE_OVERHEATING = 64 # Critical overheating +uint32 FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL = 128 # Exhaust gas over-temperature warning + +uint32 FLAG_FUEL_PRESSURE_SUPPORTED = 256 # Fuel pressure. These flags are optional +uint32 FLAG_FUEL_PRESSURE_BELOW_NOMINAL = 512 # Under-pressure warning +uint32 FLAG_FUEL_PRESSURE_ABOVE_NOMINAL = 1024 # Over-pressure warning + +uint32 FLAG_DETONATION_SUPPORTED = 2048 # Detonation warning. This flag is optional. +uint32 FLAG_DETONATION_OBSERVED = 4096 # Detonation condition observed warning + +uint32 FLAG_MISFIRE_SUPPORTED = 8192 # Misfire warning. This flag is optional. +uint32 FLAG_MISFIRE_OBSERVED = 16384 # Misfire condition observed warning + +uint32 FLAG_OIL_PRESSURE_SUPPORTED = 32768 # Oil pressure. These flags are optional +uint32 FLAG_OIL_PRESSURE_BELOW_NOMINAL = 65536 # Under-pressure warning +uint32 FLAG_OIL_PRESSURE_ABOVE_NOMINAL = 131072 # Over-pressure warning + +uint32 FLAG_DEBRIS_SUPPORTED = 262144 # Debris warning. This flag is optional +uint32 FLAG_DEBRIS_DETECTED = 524288 # Detection of debris warning +uint32 flags + +uint8 engine_load_percent # Engine load estimate, percent, [0, 127] +uint32 engine_speed_rpm # Engine speed, revolutions per minute +float32 spark_dwell_time_ms # Spark dwell time, millisecond +float32 atmospheric_pressure_kpa # Atmospheric (barometric) pressure, kilopascal +float32 intake_manifold_pressure_kpa # Engine intake manifold pressure, kilopascal +float32 intake_manifold_temperature # Engine intake manifold temperature, kelvin +float32 coolant_temperature # Engine coolant temperature, kelvin +float32 oil_pressure # Oil pressure, kilopascal +float32 oil_temperature # Oil temperature, kelvin +float32 fuel_pressure # Fuel pressure, kilopascal +float32 fuel_consumption_rate_cm3pm # Instant fuel consumption estimate, (centimeter^3)/minute +float32 estimated_consumed_fuel_volume_cm3 # Estimate of the consumed fuel since the start of the engine, centimeter^3 +uint8 throttle_position_percent # Throttle position, percent +uint8 ecu_index # The index of the publishing ECU + + +uint8 SPARK_PLUG_SINGLE = 0 +uint8 SPARK_PLUG_FIRST_ACTIVE = 1 +uint8 SPARK_PLUG_SECOND_ACTIVE = 2 +uint8 SPARK_PLUG_BOTH_ACTIVE = 3 +uint8 spark_plug_usage # Spark plug activity report. + +float32 ignition_timing_deg # Cylinder ignition timing, angular degrees of the crankshaft +float32 injection_time_ms # Fuel injection time, millisecond +float32 cylinder_head_temperature # Cylinder head temperature (CHT), kelvin +float32 exhaust_gas_temperature # Exhaust gas temperature (EGT), kelvin +float32 lambda_coefficient # Estimated lambda coefficient, dimensionless ratio diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/IridiumsbdStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/IridiumsbdStatus.msg new file mode 100644 index 00000000..436654e4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/IridiumsbdStatus.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 last_at_ok_timestamp # timestamp of the last "OK" received after the "AT" command +uint16 tx_buf_write_index # current size of the tx buffer +uint16 rx_buf_read_index # the rx buffer is parsed up to that index +uint16 rx_buf_end_index # current size of the rx buffer +uint16 failed_sbd_sessions # number of failed sbd sessions +uint16 successful_sbd_sessions # number of successful sbd sessions +uint16 num_tx_buf_reset # number of times the tx buffer was reset +uint8 signal_quality # current signal quality, 0 is no signal, 5 the best +uint8 state # current state of the driver, see the satcom_state of IridiumSBD.h for the definition +bool ring_pending # indicates if a ring call is pending +bool tx_buf_write_pending # indicates if a tx buffer write is pending +bool tx_session_pending # indicates if a tx session is pending +bool rx_read_pending # indicates if a rx read is pending +bool rx_session_pending # indicates if a rx session is pending diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/IrlockReport.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/IrlockReport.msg new file mode 100644 index 00000000..9f23cbf3 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/IrlockReport.msg @@ -0,0 +1,11 @@ +# IRLOCK_REPORT message data + +uint64 timestamp # time since system start (microseconds) + +uint16 signature + +# When looking along the optical axis of the camera, x points right, y points down, and z points along the optical axis. +float32 pos_x # tan(theta), where theta is the angle between the target and the camera center of projection in camera x-axis +float32 pos_y # tan(theta), where theta is the angle between the target and the camera center of projection in camera y-axis +float32 size_x #/** size of target along camera x-axis in units of tan(theta) **/ +float32 size_y #/** size of target along camera y-axis in units of tan(theta) **/ diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingGear.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingGear.msg new file mode 100644 index 00000000..5ef9ee52 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingGear.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +int8 GEAR_UP = 1 # landing gear up +int8 GEAR_DOWN = -1 # landing gear down +int8 GEAR_KEEP = 0 # keep the current state + +int8 landing_gear diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingGearWheel.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingGearWheel.msg new file mode 100644 index 00000000..2ff99fcc --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingGearWheel.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_wheel_setpoint # negative is turning left, positive turning right [-1, 1] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetInnovations.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetInnovations.msg new file mode 100644 index 00000000..5dd892c5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetInnovations.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +# Innovation of landing target position estimator +float32 innov_x +float32 innov_y + +# Innovation covariance of landing target position estimator +float32 innov_cov_x +float32 innov_cov_y diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetPose.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetPose.msg new file mode 100644 index 00000000..875920f1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetPose.msg @@ -0,0 +1,26 @@ +# Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames + +uint64 timestamp # time since system start (microseconds) + +bool is_static # Flag indicating whether the landing target is static or moving with respect to the ground + +bool rel_pos_valid # Flag showing whether relative position is valid +bool rel_vel_valid # Flag showing whether relative velocity is valid + +float32 x_rel # X/north position of target, relative to vehicle (navigation frame) [meters] +float32 y_rel # Y/east position of target, relative to vehicle (navigation frame) [meters] +float32 z_rel # Z/down position of target, relative to vehicle (navigation frame) [meters] + +float32 vx_rel # X/north velocity of target, relative to vehicle (navigation frame) [meters/second] +float32 vy_rel # Y/east velocity of target, relative to vehicle (navigation frame) [meters/second] + +float32 cov_x_rel # X/north position variance [meters^2] +float32 cov_y_rel # Y/east position variance [meters^2] + +float32 cov_vx_rel # X/north velocity variance [(meters/second)^2] +float32 cov_vy_rel # Y/east velocity variance [(meters/second)^2] + +bool abs_pos_valid # Flag showing whether absolute position is valid +float32 x_abs # X/north position of target, relative to origin (navigation frame) [meters] +float32 y_abs # Y/east position of target, relative to origin (navigation frame) [meters] +float32 z_abs # Z/down position of target, relative to origin (navigation frame) [meters] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LaunchDetectionStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LaunchDetectionStatus.msg new file mode 100644 index 00000000..6917f4bc --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LaunchDetectionStatus.msg @@ -0,0 +1,9 @@ +# Status of the launch detection state machine (fixed-wing only) + +uint64 timestamp # time since system start (microseconds) + +uint8 STATE_WAITING_FOR_LAUNCH = 0 # waiting for launch +uint8 STATE_LAUNCH_DETECTED_DISABLED_MOTOR = 1 # launch detected, but keep motor(s) disabled (e.g. because it can't spin freely while on catapult) +uint8 STATE_FLYING = 2 # launch detected, use normal takeoff/flying configuration + +uint8 launch_detection_state diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LedControl.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LedControl.msg new file mode 100644 index 00000000..4be5cc1c --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LedControl.msg @@ -0,0 +1,37 @@ +# LED control: control a single or multiple LED's. +# These are the externally visible LED's, not the board LED's + +uint64 timestamp # time since system start (microseconds) + +# colors +uint8 COLOR_OFF = 0 # this is only used in the drivers +uint8 COLOR_RED = 1 +uint8 COLOR_GREEN = 2 +uint8 COLOR_BLUE = 3 +uint8 COLOR_YELLOW = 4 +uint8 COLOR_PURPLE = 5 +uint8 COLOR_AMBER = 6 +uint8 COLOR_CYAN = 7 +uint8 COLOR_WHITE = 8 + +# LED modes definitions +uint8 MODE_OFF = 0 # turn LED off +uint8 MODE_ON = 1 # turn LED on +uint8 MODE_DISABLED = 2 # disable this priority (switch to lower priority setting) +uint8 MODE_BLINK_SLOW = 3 +uint8 MODE_BLINK_NORMAL = 4 +uint8 MODE_BLINK_FAST = 5 +uint8 MODE_BREATHE = 6 # continuously increase & decrease brightness (solid color if driver does not support it) +uint8 MODE_FLASH = 7 # two fast blinks (on/off) with timing as in MODE_BLINK_FAST and then off for a while + +uint8 MAX_PRIORITY = 2 # maximum priority (minimum is 0) + + +uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all +uint8 color # see COLOR_* +uint8 mode # see MODE_* +uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite + # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20 +uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY) + +uint8 ORB_QUEUE_LENGTH = 8 # needs to match BOARD_MAX_LEDS diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LogMessage.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LogMessage.msg new file mode 100644 index 00000000..afb690b1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LogMessage.msg @@ -0,0 +1,8 @@ +# A logging message, output with PX4_WARN, PX4_ERR, PX4_INFO + +uint64 timestamp # time since system start (microseconds) + +uint8 severity # log level (same as in the linux kernel, starting with 0) +char[127] text + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LoggerStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LoggerStatus.msg new file mode 100644 index 00000000..c67c8895 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/LoggerStatus.msg @@ -0,0 +1,23 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 LOGGER_TYPE_FULL = 0 # Normal, full size log +uint8 LOGGER_TYPE_MISSION = 1 # reduced mission log (e.g. for geotagging) +uint8 type + +uint8 BACKEND_FILE = 1 +uint8 BACKEND_MAVLINK = 2 +uint8 BACKEND_ALL = 3 +uint8 backend + +bool is_logging + +float32 total_written_kb # total written to log in kiloBytes +float32 write_rate_kb_s # write rate in kiloBytes/s + +uint32 dropouts # number of failed buffer writes due to buffer overflow +uint32 message_gaps # messages misssed + +uint32 buffer_used_bytes # current buffer fill in Bytes +uint32 buffer_size_bytes # total buffer size in Bytes + +uint8 num_messages diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MagWorkerData.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MagWorkerData.msg new file mode 100644 index 00000000..09626e8a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MagWorkerData.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint8 MAX_MAGS = 4 + +uint32 done_count +uint32 calibration_points_perside +uint64 calibration_interval_perside_us +uint32[4] calibration_counter_total +bool[4] side_data_collected +float32[4] x +float32[4] y +float32[4] z diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MagnetometerBiasEstimate.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MagnetometerBiasEstimate.msg new file mode 100644 index 00000000..3c0c1136 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MagnetometerBiasEstimate.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32[4] bias_x # estimated X-bias of all the sensors +float32[4] bias_y # estimated Y-bias of all the sensors +float32[4] bias_z # estimated Z-bias of all the sensors + +bool[4] valid # true if the estimator has converged +bool[4] stable diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSetpoint.msg new file mode 100644 index 00000000..95fa6222 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSetpoint.msg @@ -0,0 +1,46 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool valid + +uint8 SOURCE_UNKNOWN = 0 +uint8 SOURCE_RC = 1 # radio control (input_rc) +uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 +uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 +uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 +uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 3 +uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 +uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 + +uint8 data_source + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. + +# Stick positions [-1,1] +# on a common RC mode 1/2/3/4 remote/joystick the stick deflection: -1 is down/left, 1 is up/right +# Note: QGC sends throttle/z in range [0,1000] - [0,1]. The MAVLink input conversion [0,1] to [-1,1] is at the moment kept backwards compatible. +# Positive values are generally used for: +float32 roll # move right, positive roll rotation, right side down +float32 pitch # move forward, negative pitch rotation, nose down +float32 yaw # positive yaw rotation, clockwise when seen top down +float32 throttle # move up, positive thrust, -1 is minimum available 0% or -100% +1 is 100% thrust + +float32 flaps # position of flaps switch/knob/lever [-1, 1] + +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +float32 aux5 +float32 aux6 + +bool sticks_moving + +uint16 buttons # From uint16 buttons field of Mavlink manual_control message + +# TOPICS manual_control_setpoint manual_control_input +# DEPRECATED: float32 x +# DEPRECATED: float32 y +# DEPRECATED: float32 z +# DEPRECATED: float32 r diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSwitches.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSwitches.msg new file mode 100644 index 00000000..4d1cbab2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSwitches.msg @@ -0,0 +1,34 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 SWITCH_POS_NONE = 0 # switch is not mapped +uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) +uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) +uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) + +uint8 MODE_SLOT_NONE = 0 # no mode slot assigned +uint8 MODE_SLOT_1 = 1 # mode slot 1 selected +uint8 MODE_SLOT_2 = 2 # mode slot 2 selected +uint8 MODE_SLOT_3 = 3 # mode slot 3 selected +uint8 MODE_SLOT_4 = 4 # mode slot 4 selected +uint8 MODE_SLOT_5 = 5 # mode slot 5 selected +uint8 MODE_SLOT_6 = 6 # mode slot 6 selected +uint8 MODE_SLOT_NUM = 6 # number of slots + +uint8 mode_slot # the slot a specific model selector is in + +uint8 arm_switch # arm/disarm switch: _DISARMED_, ARMED +uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 kill_switch # throttle kill: _NORMAL_, KILL +uint8 gear_switch # landing gear switch: _DOWN_, UP +uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT + +uint8 photo_switch # Photo trigger switch +uint8 video_switch # Photo trigger switch + +uint8 engage_main_motor_switch # Engage the main motor (for helicopters) + +uint32 switch_changes # number of switch changes diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkLog.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkLog.msg new file mode 100644 index 00000000..8f52ec7d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkLog.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +char[127] text +uint8 severity # log level (same as in the linux kernel, starting with 0) + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkTunnel.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkTunnel.msg new file mode 100644 index 00000000..16934a95 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkTunnel.msg @@ -0,0 +1,20 @@ +# MAV_TUNNEL_PAYLOAD_TYPE enum + +uint8 MAV_TUNNEL_PAYLOAD_TYPE_UNKNOWN = 0 # Encoding of payload unknown +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller + +uint64 timestamp # Time since system start (microseconds) +uint16 payload_type # A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. +uint8 target_system # System ID (can be 0 for broadcast, but this is discouraged) +uint8 target_component # Component ID (can be 0 for broadcast, but this is discouraged) +uint8 payload_length # Length of the data transported in payload +uint8[128] payload # Data itself diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatRequest.msg new file mode 100644 index 00000000..6ceb66d0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatRequest.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +# Request to PX4 to get the hash of a message, to check for message compatibility + +uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes. + +uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp + +char[50] topic_name # E.g. /fmu/in/vehicle_command diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatResponse.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatResponse.msg new file mode 100644 index 00000000..41ee9627 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatResponse.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +# Response from PX4 with the format of a message + +uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp + +char[50] topic_name # E.g. /fmu/in/vehicle_command + +bool success +uint32 message_hash # hash over all message fields + diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Mission.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Mission.msg new file mode 100644 index 00000000..a923193d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Mission.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) +uint8 mission_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 fence_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 safepoint_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 + +uint16 count # count of the missions stored in the dataman +int32 current_seq # default -1, start at the one changed latest + +int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise +int32 land_index # Index of the land item, -1 otherwise + +uint32 mission_id # indicates updates to the mission, reload from dataman if changed +uint32 geofence_id # indicates updates to the geofence, reload from dataman if changed +uint32 safe_points_id # indicates updates to the safe points, reload from dataman if changed diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MissionResult.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MissionResult.msg new file mode 100644 index 00000000..f70326be --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MissionResult.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 mission_id # Id for the mission for which the result was generated +uint32 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility) +uint32 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility) + +int32 seq_reached # Sequence of the mission item which has been reached, default -1 +uint16 seq_current # Sequence of the current mission item +uint16 seq_total # Total number of mission items + +bool valid # true if mission is valid +bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings +bool finished # true if mission has been completed +bool failure # true if the mission cannot continue or be completed for some reason + +bool item_do_jump_changed # true if the number of do jumps remaining has changed +uint16 item_changed_index # indicate which item has changed +uint16 item_do_jump_remaining # set to the number of do jumps remaining for that item + +uint8 execution_mode # indicates the mode in which the mission is executed diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ModeCompleted.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ModeCompleted.msg new file mode 100644 index 00000000..bacff4a9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ModeCompleted.msg @@ -0,0 +1,15 @@ +# Mode completion result, published by an active mode. +# The possible values of nav_state are defined in the VehicleStatus msg. +# Note that this is not always published (e.g. when a user switches modes or on +# failsafe activation) +uint64 timestamp # time since system start (microseconds) + + +uint8 RESULT_SUCCESS = 0 +# [1-99]: reserved +uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error) + +uint8 result # One of RESULT_* + +uint8 nav_state # Source mode (values in VehicleStatus) + diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MountOrientation.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MountOrientation.msg new file mode 100644 index 00000000..7ae54a39 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/MountOrientation.msg @@ -0,0 +1,2 @@ +uint64 timestamp # time since system start (microseconds) +float32[3] attitude_euler_angle # Attitude/direction of the mount as euler angles in rad diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NavigatorMissionItem.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NavigatorMissionItem.msg new file mode 100644 index 00000000..64af762f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NavigatorMissionItem.msg @@ -0,0 +1,25 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified + +uint16 sequence_current # Sequence of the current mission item + +uint16 nav_cmd + +float32 latitude +float32 longitude + +float32 time_inside # time that the MAV should stay inside the radius before advancing in seconds +float32 acceptance_radius # default radius in which the mission is accepted as reached in meters +float32 loiter_radius # loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise +float32 yaw # in radians NED -PI..+PI, NAN means don't change yaw +float32 altitude # altitude in meters (AMSL) + +uint8 frame # mission frame +uint8 origin # mission item origin (onboard or mavlink) + +bool loiter_exit_xtrack # exit xtrack location: 0 for center of loiter wp, 1 for exit location +bool force_heading # heading needs to be reached +bool altitude_is_relative # true if altitude is relative from start point +bool autocontinue # true if next waypoint should follow after this one +bool vtol_back_transition # part of the vtol back transition sequence diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NormalizedUnsignedSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NormalizedUnsignedSetpoint.msg new file mode 100644 index 00000000..10193b8c --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NormalizedUnsignedSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_setpoint # [0, 1] + +# TOPICS flaps_setpoint spoilers_setpoint diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NpfgStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NpfgStatus.msg new file mode 100644 index 00000000..132c1f7f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/NpfgStatus.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 wind_est_valid # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid) +float32 lat_accel # resultant lateral acceleration reference [m/s^2] +float32 lat_accel_ff # lateral acceleration demand only for maintaining curvature [m/s^2] +float32 bearing_feas # bearing feasibility [0,1] +float32 bearing_feas_on_track # on-track bearing feasibility [0,1] +float32 signed_track_error # signed track error [m] +float32 track_error_bound # track error bound [m] +float32 airspeed_ref # (true) airspeed reference [m/s] +float32 bearing # bearing angle [rad] +float32 heading_ref # heading angle reference [rad] +float32 min_ground_speed_ref # minimum forward ground speed reference [m/s] +float32 adapted_period # adapted period (if auto-tuning enabled) [s] +float32 p_gain # controller proportional gain [rad/s] +float32 time_const # controller time constant [s] +float32 can_run_factor # estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ObstacleDistance.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ObstacleDistance.msg new file mode 100644 index 00000000..e3c4963a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ObstacleDistance.msg @@ -0,0 +1,24 @@ +# Obstacle distances in front of the sensor. +uint64 timestamp # time since system start (microseconds) + +uint8 frame #Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is North aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. +uint8 MAV_FRAME_GLOBAL = 0 +uint8 MAV_FRAME_LOCAL_NED = 1 +uint8 MAV_FRAME_BODY_FRD = 12 + +uint8 sensor_type # Type from MAV_DISTANCE_SENSOR enum. +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 +uint8 MAV_DISTANCE_SENSOR_RADAR = 3 + +uint16[72] distances # Distance of obstacles around the UAV with index 0 corresponding to local North. A value of 0 means that the obstacle is right in front of the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + +float32 increment # Angular width in degrees of each array element. + +uint16 min_distance # Minimum distance the sensor can measure in centimeters. +uint16 max_distance # Maximum distance the sensor can measure in centimeters. + +float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right. + +# TOPICS obstacle_distance obstacle_distance_fused diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OffboardControlMode.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OffboardControlMode.msg new file mode 100644 index 00000000..885164a6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OffboardControlMode.msg @@ -0,0 +1,11 @@ +# Off-board control mode + +uint64 timestamp # time since system start (microseconds) + +bool position +bool velocity +bool acceleration +bool attitude +bool body_rate +bool thrust_and_torque +bool direct_actuator diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OnboardComputerStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OnboardComputerStatus.msg new file mode 100644 index 00000000..736932bc --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OnboardComputerStatus.msg @@ -0,0 +1,23 @@ +# ONBOARD_COMPUTER_STATUS message data +uint64 timestamp # [us] time since system start (microseconds) +uint32 uptime # [ms] time since system boot of the companion (milliseconds) + +uint8 type # type of onboard computer 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + +uint8[8] cpu_cores # CPU usage on the component in percent +uint8[10] cpu_combined # Combined CPU usage as the last 10 slices of 100 MS +uint8[4] gpu_cores # GPU usage on the component in percent +uint8[10] gpu_combined # Combined GPU usage as the last 10 slices of 100 MS +int8 temperature_board # [degC] Temperature of the board +int8[8] temperature_core # [degC] Temperature of the CPU core +int16[4] fan_speed # [rpm] Fan speeds +uint32 ram_usage # [MB] Amount of used RAM on the component system +uint32 ram_total # [MB] Total amount of RAM on the component system +uint32[4] storage_type # Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable) +uint32[4] storage_usage # [MB] Amount of used storage space on the component system +uint32[4] storage_total # [MB] Total amount of storage space on the component system +uint32[6] link_type # [Kb/s] Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary +uint32[6] link_tx_rate # [Kb/s] Network traffic from the component system +uint32[6] link_rx_rate # [Kb/s] Network traffic to the component system +uint32[6] link_tx_max # [Kb/s] Network capacity from the component system +uint32[6] link_rx_max # [Kb/s] Network capacity to the component system diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTest.msg new file mode 100644 index 00000000..bbbca412 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTest.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +# TOPICS orb_test orb_multitest diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestLarge.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestLarge.msg new file mode 100644 index 00000000..48d6a427 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestLarge.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +uint8[512] junk diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestMedium.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestMedium.msg new file mode 100644 index 00000000..43109d49 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestMedium.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +uint8[64] junk + +uint8 ORB_QUEUE_LENGTH = 16 + +# TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbitStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbitStatus.msg new file mode 100644 index 00000000..a04265db --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/OrbitStatus.msg @@ -0,0 +1,14 @@ +# ORBIT_YAW_BEHAVIOUR +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 +uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 +uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 + +uint64 timestamp # time since system start (microseconds) +float32 radius # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m] +uint8 frame # The coordinate system of the fields: x, y, z. +float64 x # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. +float64 y # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7. +float32 z # Altitude of center point. Coordinate system depends on frame field. +uint8 yaw_behaviour diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterResetRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterResetRequest.msg new file mode 100644 index 00000000..db08edb3 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterResetRequest.msg @@ -0,0 +1,8 @@ +# ParameterResetRequest : Used by the primary to reset one or all parameter value(s) on the remote + +uint64 timestamp +uint16 parameter_index + +bool reset_all # If this is true then ignore parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetUsedRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetUsedRequest.msg new file mode 100644 index 00000000..ca97f9e9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetUsedRequest.msg @@ -0,0 +1,6 @@ +# ParameterSetUsedRequest : Used by a remote to update the used flag for a parameter on the primary + +uint64 timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 64 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueRequest.msg new file mode 100644 index 00000000..eaf650f2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueRequest.msg @@ -0,0 +1,11 @@ +# ParameterSetValueRequest : Used by a remote or primary to update the value for a parameter at the other end + +uint64 timestamp +uint16 parameter_index + +int32 int_value # Optional value for an integer parameter +float32 float_value # Optional value for a float parameter + +uint8 ORB_QUEUE_LENGTH = 32 + +# TOPICS parameter_set_value_request parameter_remote_set_value_request parameter_primary_set_value_request diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueResponse.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueResponse.msg new file mode 100644 index 00000000..09f8e308 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueResponse.msg @@ -0,0 +1,9 @@ +# ParameterSetValueResponse : Response to a set value request by either primary or secondary + +uint64 timestamp +uint64 request_timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterUpdate.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterUpdate.msg new file mode 100644 index 00000000..bfb49937 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/ParameterUpdate.msg @@ -0,0 +1,14 @@ +# This message is used to notify the system about one or more parameter changes + +uint64 timestamp # time since system start (microseconds) + +uint32 instance # Instance count - constantly incrementing + +uint32 get_count +uint32 set_count +uint32 find_count +uint32 export_count + +uint16 active +uint16 changed +uint16 custom_default diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Ping.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Ping.msg new file mode 100644 index 00000000..498a3c73 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Ping.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint64 ping_time # Timestamp of the ping packet +uint32 ping_sequence # Sequence number of the ping packet +uint32 dropped_packets # Number of dropped ping packets +float32 rtt_ms # Round trip time (in ms) +uint8 system_id # System ID of the remote system +uint8 component_id # Component ID of the remote system diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerLandingStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerLandingStatus.msg new file mode 100644 index 00000000..249529b4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerLandingStatus.msg @@ -0,0 +1,16 @@ +uint64 timestamp # [us] time since system start +float32 lateral_touchdown_offset # [m] lateral touchdown position offset manually commanded during landing +bool flaring # true if the aircraft is flaring + +# abort status is: +# 0 if not aborted +# >0 if aborted, with the singular abort criterion which triggered the landing abort enumerated by the following abort reasons +uint8 abort_status + +# abort reasons +# after the manual operator abort, corresponds to individual bits of param FW_LND_ABORT +uint8 NOT_ABORTED = 0 +uint8 ABORTED_BY_OPERATOR = 1 +uint8 TERRAIN_NOT_FOUND = 2 # FW_LND_ABORT (1 << 0) +uint8 TERRAIN_TIMEOUT = 3 # FW_LND_ABORT (1 << 1) +uint8 UNKNOWN_ABORT_CRITERION = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerStatus.msg new file mode 100644 index 00000000..7237351f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +float32 nav_roll # Roll setpoint [rad] +float32 nav_pitch # Pitch setpoint [rad] +float32 nav_bearing # Bearing angle[rad] +float32 target_bearing # Bearing angle from aircraft to current target [rad] +float32 xtrack_error # Signed track error [m] +float32 wp_dist # Distance to active (next) waypoint [m] +float32 acceptance_radius # Current horizontal acceptance radius [m] +float32 yaw_acceptance # Yaw acceptance error[rad] +float32 altitude_acceptance # Current vertical acceptance error [m] +uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpoint.msg new file mode 100644 index 00000000..035d3520 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpoint.msg @@ -0,0 +1,37 @@ +# this file is only used in the position_setpoint triple as a dependency + +uint64 timestamp # time since system start (microseconds) + +uint8 SETPOINT_TYPE_POSITION=0 # position setpoint +uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint +uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) + +uint8 LOITER_TYPE_ORBIT=0 # Circular pattern +uint8 LOITER_TYPE_FIGUREEIGHT=1 # Pattern resembling an 8 + +bool valid # true if setpoint is valid +uint8 type # setpoint type to adjust behavior of position controller + +float32 vx # local velocity setpoint in m/s in NED +float32 vy # local velocity setpoint in m/s in NED +float32 vz # local velocity setpoint in m/s in NED + +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m +float32 yaw # yaw (only in hover), in rad [-PI..PI), NaN = leave to flight task + +float32 loiter_radius # loiter major axis radius in m +float32 loiter_minor_radius # loiter minor axis radius (used for non-circular loiter shapes) in m +bool loiter_direction_counter_clockwise # loiter direction is clockwise by default and can be changed using this field +float32 loiter_orientation # Orientation of the major axis with respect to true north in rad [-pi,pi) +uint8 loiter_pattern # loitern pattern to follow + +float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation + +float32 cruising_speed # the generally desired cruising speed (not a hard constraint) +bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only) +float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint), only has an effect for rover diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpointTriplet.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpointTriplet.msg new file mode 100644 index 00000000..6f9ac4d2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpointTriplet.msg @@ -0,0 +1,8 @@ +# Global position setpoint triplet in WGS84 coordinates. +# This are the three next waypoints (or just the next two or one). + +uint64 timestamp # time since system start (microseconds) + +PositionSetpoint previous +PositionSetpoint current +PositionSetpoint next diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PowerButtonState.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PowerButtonState.msg new file mode 100644 index 00000000..15180666 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PowerButtonState.msg @@ -0,0 +1,10 @@ +# power button state notification message + +uint64 timestamp # time since system start (microseconds) + +uint8 PWR_BUTTON_STATE_IDEL = 0 # Button went up without meeting shutdown button down time (delete event) +uint8 PWR_BUTTON_STATE_DOWN = 1 # Button went Down +uint8 PWR_BUTTON_STATE_UP = 2 # Button went Up +uint8 PWR_BUTTON_STATE_REQUEST_SHUTDOWN = 3 # Button went Up after meeting shutdown button down time + +uint8 event # one of PWR_BUTTON_STATE_* diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PowerMonitor.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PowerMonitor.msg new file mode 100644 index 00000000..a64585fa --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PowerMonitor.msg @@ -0,0 +1,15 @@ +# power monitor message + +uint64 timestamp # Time since system start (microseconds) + +float32 voltage_v # Voltage in volts, 0 if unknown +float32 current_a # Current in amperes, -1 if unknown +float32 power_w # power in watts, -1 if unknown +int16 rconf +int16 rsv +int16 rbv +int16 rp +int16 rc +int16 rcal +int16 me +int16 al diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PpsCapture.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PpsCapture.msg new file mode 100644 index 00000000..c6fa2cb9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PpsCapture.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) at PPS capture event +uint64 rtc_timestamp # Corrected GPS UTC timestamp at PPS capture event +uint8 pps_rate_exceeded_counter # Increments when PPS dt < 50ms diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PwmInput.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PwmInput.msg new file mode 100644 index 00000000..fcc7dbe4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/PwmInput.msg @@ -0,0 +1,4 @@ +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) +uint32 pulse_width # Pulse width, timer counts +uint32 period # Period, timer counts diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Px4ioStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Px4ioStatus.msg new file mode 100644 index 00000000..295c8fba --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Px4ioStatus.msg @@ -0,0 +1,43 @@ +uint64 timestamp # time since system start (microseconds) + +uint16 free_memory_bytes + +float32 voltage_v # Servo rail voltage in volts +float32 rssi_v # RSSI pin voltage in volts + +# PX4IO status flags (PX4IO_P_STATUS_FLAGS) +bool status_arm_sync +bool status_failsafe +bool status_fmu_initialized +bool status_fmu_ok +bool status_init_ok +bool status_outputs_armed +bool status_raw_pwm +bool status_rc_ok +bool status_rc_dsm +bool status_rc_ppm +bool status_rc_sbus +bool status_rc_st24 +bool status_rc_sumd +bool status_safety_button_event # px4io safety button was pressed for longer than 1 second + +# PX4IO alarms (PX4IO_P_STATUS_ALARMS) +bool alarm_pwm_error +bool alarm_rc_lost + +# PX4IO arming (PX4IO_P_SETUP_ARMING) +bool arming_failsafe_custom +bool arming_fmu_armed +bool arming_fmu_prearmed +bool arming_force_failsafe +bool arming_io_arm_ok +bool arming_lockdown +bool arming_termination_failsafe + +uint16[8] pwm +uint16[8] pwm_disarmed +uint16[8] pwm_failsafe + +uint16[8] pwm_rate_hz + +uint16[18] raw_inputs diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/QshellReq.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/QshellReq.msg new file mode 100644 index 00000000..d472e8dd --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/QshellReq.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +char[100] cmd +uint32 MAX_STRLEN = 100 +uint32 strlen +uint32 request_sequence diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/QshellRetval.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/QshellRetval.msg new file mode 100644 index 00000000..d42a7715 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/QshellRetval.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +int32 return_value +uint32 return_sequence diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RadioStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RadioStatus.msg new file mode 100644 index 00000000..c57c82b5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RadioStatus.msg @@ -0,0 +1,13 @@ + +uint64 timestamp # time since system start (microseconds) + +uint8 rssi # local signal strength +uint8 remote_rssi # remote signal strength + +uint8 txbuf # how full the tx buffer is as a percentage +uint8 noise # background noise level + +uint8 remote_noise # remote background noise level +uint16 rxerrors # receive errors + +uint16 fix # count of error corrected packets diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RateCtrlStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RateCtrlStatus.msg new file mode 100644 index 00000000..3f564469 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RateCtrlStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +# rate controller integrator status +float32 rollspeed_integ +float32 pitchspeed_integ +float32 yawspeed_integ +float32 wheel_rate_integ # FW only and optional diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RcChannels.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RcChannels.msg new file mode 100644 index 00000000..546755f6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RcChannels.msg @@ -0,0 +1,40 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 FUNCTION_THROTTLE = 0 +uint8 FUNCTION_ROLL = 1 +uint8 FUNCTION_PITCH = 2 +uint8 FUNCTION_YAW = 3 +uint8 FUNCTION_RETURN = 4 +uint8 FUNCTION_LOITER = 5 +uint8 FUNCTION_OFFBOARD = 6 +uint8 FUNCTION_FLAPS = 7 +uint8 FUNCTION_AUX_1 = 8 +uint8 FUNCTION_AUX_2 = 9 +uint8 FUNCTION_AUX_3 = 10 +uint8 FUNCTION_AUX_4 = 11 +uint8 FUNCTION_AUX_5 = 12 +uint8 FUNCTION_AUX_6 = 13 +uint8 FUNCTION_PARAM_1 = 14 +uint8 FUNCTION_PARAM_2 = 15 +uint8 FUNCTION_PARAM_3_5 = 16 +uint8 FUNCTION_KILLSWITCH = 17 +uint8 FUNCTION_TRANSITION = 18 +uint8 FUNCTION_GEAR = 19 +uint8 FUNCTION_ARMSWITCH = 20 +uint8 FUNCTION_FLTBTN_SLOT_1 = 21 +uint8 FUNCTION_FLTBTN_SLOT_2 = 22 +uint8 FUNCTION_FLTBTN_SLOT_3 = 23 +uint8 FUNCTION_FLTBTN_SLOT_4 = 24 +uint8 FUNCTION_FLTBTN_SLOT_5 = 25 +uint8 FUNCTION_FLTBTN_SLOT_6 = 26 +uint8 FUNCTION_ENGAGE_MAIN_MOTOR = 27 + +uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6 + +uint64 timestamp_last_valid # Timestamp of last valid RC signal +float32[18] channels # Scaled to -1..1 (throttle: 0..1) +uint8 channel_count # Number of valid channels +int8[28] function # Functions mapping +uint8 rssi # Receive signal strength index +bool signal_lost # Control signal lost, should be checked together with topic timeout +uint32 frame_drop_count # Number of dropped frames diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RcParameterMap.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RcParameterMap.msg new file mode 100644 index 00000000..2a5df3e7 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RcParameterMap.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) +uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +bool[3] valid #true for RC-Param channels which are mapped to a param +int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used +char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated +float32[3] scale # scale to map the RC input [-1, 1] to a parameter value +float32[3] value0 # initial value around which the parameter value is changed +float32[3] value_min # minimal parameter value +float32[3] value_max # minimal parameter value diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentReply.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentReply.msg new file mode 100644 index 00000000..7cd7eef0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentReply.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 request_id # ID from the request +char[25] name # name from the request + +uint16 px4_ros2_api_version + +bool success +int8 arming_check_id # arming check registration ID (-1 if invalid) +int8 mode_id # assigned mode ID (-1 if invalid) +int8 mode_executor_id # assigned mode executor ID (-1 if invalid) + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentRequest.msg new file mode 100644 index 00000000..46ab0cb0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentRequest.msg @@ -0,0 +1,21 @@ +# Request to register an external component +uint64 timestamp # time since system start (microseconds) + +uint64 request_id # ID, set this to a random value +char[25] name # either the requested mode name, or component name + +uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change. + +uint16 px4_ros2_api_version # Set to LATEST_PX4_ROS2_API_VERSION + +# Components to be registered +bool register_arming_check +bool register_mode # registering a mode also requires arming_check to be set +bool register_mode_executor # registering an executor also requires a mode to be registered (which is the owned mode by the executor) + +bool enable_replace_internal_mode # set to true if an internal mode should be replaced +uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_* +bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor) + + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RoverAckermannGuidanceStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RoverAckermannGuidanceStatus.msg new file mode 100644 index 00000000..3c34b63c --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RoverAckermannGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 actual_speed # [m/s] Rover ground speed +float32 desired_speed # [m/s] Rover desired ground speed +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error # [deg] Heading error of the pure pursuit controller +float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions +float32 crosstrack_error # [m] Shortest distance from the vehicle to the path + +# TOPICS rover_ackermann_guidance_status diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Rpm.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Rpm.msg new file mode 100644 index 00000000..baab7c6a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Rpm.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32 indicated_frequency_rpm # indicated rotor Frequency in Revolution per minute +float32 estimated_accurancy_rpm # estimated accuracy in Revolution per minute diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RtlStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RtlStatus.msg new file mode 100644 index 00000000..f25b2224 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RtlStatus.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 safe_points_id # unique ID of active set of safe_point_items +bool is_evaluation_pending # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading). + +bool has_vtol_approach # flag if approaches are defined for current RTL_TYPE parameter setting + +uint8 rtl_type # Type of RTL chosen +uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode + +uint8 RTL_STATUS_TYPE_NONE=0 # pending if evaluation can't pe performed currently e.g. when it is still loading the safe points +uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # chosen to directly go to a safe point or home position +uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # going straight to the beginning of the mission landing +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # Following the mission from start index to mission landing. Start index is current WP if in Mission mode, and closest WP otherwise. +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # Following the mission in reverse from start index to the beginning of the mission. Start index is previous WP if in Mission mode, and closest WP otherwise. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RtlTimeEstimate.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RtlTimeEstimate.msg new file mode 100644 index 00000000..ee46888d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/RtlTimeEstimate.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +bool valid # Flag indicating whether the time estiamtes are valid +float32 time_estimate # [s] Estimated time for RTL +float32 safe_time_estimate # [s] Same as time_estimate, but with safety factor and safety margin included (factor*t + margin) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SatelliteInfo.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SatelliteInfo.msg new file mode 100644 index 00000000..2980ffcb --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SatelliteInfo.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint8 SAT_INFO_MAX_SATELLITES = 20 + +uint8 count # Number of satellites visible to the receiver +uint8[20] svid # Space vehicle ID [1..255], see scheme below +uint8[20] used # 0: Satellite not used, 1: used for navigation +uint8[20] elevation # Elevation (0: right on top of receiver, 90: on the horizon) of satellite +uint8[20] azimuth # Direction of satellite, 0: 0 deg, 255: 360 deg. +uint8[20] snr # dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. +uint8[20] prn # Satellite PRN code assignment, (psuedorandom number SBAS, valid codes are 120-144) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccel.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccel.msg new file mode 100644 index 00000000..e47d813a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccel.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 x # acceleration in the FRD board frame X-axis in m/s^2 +float32 y # acceleration in the FRD board frame Y-axis in m/s^2 +float32 z # acceleration in the FRD board frame Z-axis in m/s^2 + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8[3] clip_counter # clip count per axis in the sample period + +uint8 samples # number of raw samples that went into this message + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccelFifo.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccelFifo.msg new file mode 100644 index 00000000..1eae5dd1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccelFifo.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 dt # delta time between samples (microseconds) +float32 scale + +uint8 samples # number of valid samples + +int16[32] x # acceleration in the FRD board frame X-axis in m/s^2 +int16[32] y # acceleration in the FRD board frame Y-axis in m/s^2 +int16[32] z # acceleration in the FRD board frame Z-axis in m/s^2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAirflow.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAirflow.msg new file mode 100644 index 00000000..dd55ad0b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorAirflow.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles +float32 speed # the speed being reported by the wind / airflow sensor +float32 direction # the direction bein report by the wind / airflow sensor +uint8 status # Status code from the sensor diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorBaro.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorBaro.msg new file mode 100644 index 00000000..7c4154e1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorBaro.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 pressure # static pressure measurement in Pascals + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorCombined.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorCombined.msg new file mode 100644 index 00000000..837c7a1f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorCombined.msg @@ -0,0 +1,25 @@ +# Sensor readings in SI-unit form. +# These fields are scaled and offset-compensated where possible and do not +# change with board revisions and sensor updates. + +uint64 timestamp # time since system start (microseconds) + +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid + +# gyro timstamp is equal to the timestamp of the message +float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period +uint32 gyro_integral_dt # gyro measurement sampling period in microseconds + +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp +float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period +uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds + +uint8 CLIPPING_X = 1 +uint8 CLIPPING_Y = 2 +uint8 CLIPPING_Z = 4 + +uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame +uint8 gyro_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame + +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorCorrection.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorCorrection.msg new file mode 100644 index 00000000..bfbc8e2e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorCorrection.msg @@ -0,0 +1,41 @@ +# +# Sensor corrections in SI-unit form for the voted sensor +# + +uint64 timestamp # time since system start (microseconds) + +# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] accel_device_ids +float32[4] accel_temperature +float32[3] accel_offset_0 # accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] accel_offset_1 # accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] accel_offset_2 # accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] accel_offset_3 # accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s + +# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] gyro_device_ids +float32[4] gyro_temperature +float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_3 # gyro 3 XYZ offsets in the sensor frame in rad/s + +# Corrections for magnetometer measurement outputs where corrected_mag = raw_mag * mag_scale + mag_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] mag_device_ids +float32[4] mag_temperature +float32[3] mag_offset_0 # magnetometer 0 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_1 # magnetometer 1 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_2 # magnetometer 2 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_3 # magnetometer 3 offsets in the FRD board frame XYZ-axis in m/s^s + +# Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] baro_device_ids +float32[4] baro_temperature +float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in Pascals +float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in Pascals +float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in Pascals +float32 baro_offset_3 # barometric pressure 3 offsets in the sensor frame in Pascals diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGnssRelative.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGnssRelative.msg new file mode 100644 index 00000000..6d87a344 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGnssRelative.msg @@ -0,0 +1,30 @@ +# GNSS relative positioning information in NED frame. The NED frame is defined as the local topological system at the reference station. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint16 reference_station_id # Reference Station ID + +float32[3] position # GPS NED relative position vector (m) +float32[3] position_accuracy # Accuracy of relative position (m) + +float32 heading # Heading of the relative position vector (radians) +float32 heading_accuracy # Accuracy of heading of the relative position vector (radians) + +float32 position_length # Length of the position vector (m) +float32 accuracy_length # Accuracy of the position length (m) + +bool gnss_fix_ok # GNSS valid fix (i.e within DOP & accuracy masks) +bool differential_solution # differential corrections were applied +bool relative_position_valid +bool carrier_solution_floating # carrier phase range solution with floating ambiguities +bool carrier_solution_fixed # carrier phase range solution with fixed ambiguities +bool moving_base_mode # if the receiver is operating in moving base mode +bool reference_position_miss # extrapolated reference position was used to compute moving base solution this epoch +bool reference_observations_miss # extrapolated reference observations were used to compute moving base solution this epoch +bool heading_valid +bool relative_position_normalized # the components of the relative position vector (including the high-precision parts) are normalized diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGps.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGps.msg new file mode 100644 index 00000000..ce2bfad4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGps.msg @@ -0,0 +1,72 @@ +# GPS position in WGS84 coordinates. +# the field 'timestamp' is for the position & velocity (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float64 latitude_deg # Latitude in degrees, allows centimeter level RTK precision +float64 longitude_deg # Longitude in degrees, allows centimeter level RTK precision +float64 altitude_msl_m # Altitude above MSL, meters +float64 altitude_ellipsoid_m # Altitude above Ellipsoid, meters + +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +float32 c_variance_rad # GPS course accuracy estimate, (radians) +uint8 FIX_TYPE_NONE = 1 # Value 0 is also valid to represent no fix. +uint8 FIX_TYPE_2D = 2 +uint8 FIX_TYPE_3D = 3 +uint8 FIX_TYPE_RTCM_CODE_DIFFERENTIAL = 4 +uint8 FIX_TYPE_RTK_FLOAT = 5 +uint8 FIX_TYPE_RTK_FIXED = 6 +uint8 FIX_TYPE_EXTRAPOLATED = 8 +uint8 fix_type # Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) + +float32 hdop # Horizontal dilution of precision +float32 vdop # Vertical dilution of precision + +int32 noise_per_ms # GPS noise per millisecond +uint16 automatic_gain_control # Automatic gain control monitor + +uint8 JAMMING_STATE_UNKNOWN = 0 +uint8 JAMMING_STATE_OK = 1 +uint8 JAMMING_STATE_WARNING = 2 +uint8 JAMMING_STATE_CRITICAL = 3 +uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical +int32 jamming_indicator # indicates jamming is occurring + +uint8 SPOOFING_STATE_UNKNOWN = 0 +uint8 SPOOFING_STATE_NONE = 1 +uint8 SPOOFING_STATE_INDICATED = 2 +uint8 SPOOFING_STATE_MULTIPLE = 3 +uint8 spoofing_state # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical + +float32 vel_m_s # GPS ground speed, (metres/sec) +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) +float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) +bool vel_ned_valid # True if NED velocity is valid + +int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint8 satellites_used # Number of satellites used + +float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) +float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) +float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) + +float32 rtcm_injection_rate # RTCM message injection rate Hz +uint8 selected_rtcm_instance # uorb instance that is being used for RTCM corrections + +bool rtcm_crc_failed # RTCM message CRC failure detected + +uint8 RTCM_MSG_USED_UNKNOWN = 0 +uint8 RTCM_MSG_USED_NOT_USED = 1 +uint8 RTCM_MSG_USED_USED = 2 +uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver + +# TOPICS sensor_gps vehicle_gps_position diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyro.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyro.msg new file mode 100644 index 00000000..b906127b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyro.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 x # angular velocity in the FRD board frame X-axis in rad/s +float32 y # angular velocity in the FRD board frame Y-axis in rad/s +float32 z # angular velocity in the FRD board frame Z-axis in rad/s + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8[3] clip_counter # clip count per axis in the sample period + +uint8 samples # number of raw samples that went into this message + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFft.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFft.msg new file mode 100644 index 00000000..ed84d0a0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFft.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 sensor_sample_rate_hz +float32 resolution_hz + +float32[3] peak_frequencies_x # x axis peak frequencies +float32[3] peak_frequencies_y # y axis peak frequencies +float32[3] peak_frequencies_z # z axis peak frequencies + +float32[3] peak_snr_x # x axis peak SNR +float32[3] peak_snr_y # y axis peak SNR +float32[3] peak_snr_z # z axis peak SNR diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFifo.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFifo.msg new file mode 100644 index 00000000..2e77ef07 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFifo.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 dt # delta time between samples (microseconds) +float32 scale + +uint8 samples # number of valid samples + +int16[32] x # angular velocity in the FRD board frame X-axis in rad/s +int16[32] y # angular velocity in the FRD board frame Y-axis in rad/s +int16[32] z # angular velocity in the FRD board frame Z-axis in rad/s + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorHygrometer.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorHygrometer.msg new file mode 100644 index 00000000..490a7402 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorHygrometer.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 temperature # Temperature provided by sensor (Celsius) + +float32 humidity # Humidity provided by sensor diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorMag.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorMag.msg new file mode 100644 index 00000000..1b5ba487 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorMag.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 x # magnetic field in the FRD board frame X-axis in Gauss +float32 y # magnetic field in the FRD board frame Y-axis in Gauss +float32 z # magnetic field in the FRD board frame Z-axis in Gauss + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorOpticalFlow.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorOpticalFlow.msg new file mode 100644 index 00000000..ce7e8bf0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorOpticalFlow.msg @@ -0,0 +1,30 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data. +bool delta_angle_available + +float32 distance_m # (meters) Distance to the center of the flow field +bool distance_available + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # quality, 0: bad quality, 255: maximum quality + +uint32 error_count + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably + +uint8 MODE_UNKNOWN = 0 +uint8 MODE_BRIGHT = 1 +uint8 MODE_LOWLIGHT = 2 +uint8 MODE_SUPER_LOWLIGHT = 3 + +uint8 mode diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorPreflightMag.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorPreflightMag.msg new file mode 100644 index 00000000..2b5333c4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorPreflightMag.msg @@ -0,0 +1,7 @@ +# +# Pre-flight sensor check metrics. +# The topic will not be updated when the vehicle is armed +# +uint64 timestamp # time since system start (microseconds) + +float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorSelection.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorSelection.msg new file mode 100644 index 00000000..799ccf18 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorSelection.msg @@ -0,0 +1,7 @@ +# +# Sensor ID's for the voted sensors output on the sensor_combined topic. +# Will be updated on startup of the sensor module and when sensor selection changes +# +uint64 timestamp # time since system start (microseconds) +uint32 accel_device_id # unique device ID for the selected accelerometers +uint32 gyro_device_id # unique device ID for the selected rate gyros diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorUwb.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorUwb.msg new file mode 100644 index 00000000..ae889a8b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorUwb.msg @@ -0,0 +1,34 @@ +# UWB distance contains the distance information measured by an ultra-wideband positioning system, +# such as Pozyx or NXP Rddrone. + +uint64 timestamp # time since system start (microseconds) + +uint32 sessionid # UWB SessionID +uint32 time_offset # Time between Ranging Rounds in ms +uint32 counter # Number of Ranges since last Start of Ranging +uint16 mac # MAC adress of Initiator (controller) + +uint16 mac_dest # MAC adress of Responder (Controlee) +uint16 status # status feedback # +uint8 nlos # None line of site condition y/n +float32 distance # distance in m to the UWB receiver + + +#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees +float32 aoa_azimuth_dev # Angle of arrival of first incomming RX msg +float32 aoa_elevation_dev # Angle of arrival of first incomming RX msg +float32 aoa_azimuth_resp # Angle of arrival of first incomming RX msg at the responder +float32 aoa_elevation_resp # Angle of arrival of first incomming RX msg at the responder + +# Figure of merit for the angle measurements +uint8 aoa_azimuth_fom # AOA Azimuth FOM +uint8 aoa_elevation_fom # AOA Elevation FOM +uint8 aoa_dest_azimuth_fom # AOA Azimuth FOM +uint8 aoa_dest_elevation_fom # AOA Elevation FOM + +# Initiator physical configuration +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + # Standard configuration is Antennas facing down and azimuth aligened in forward direction +float32 offset_x # UWB initiator offset in X axis (NED drone frame) +float32 offset_y # UWB initiator offset in Y axis (NED drone frame) +float32 offset_z # UWB initiator offset in Z axis (NED drone frame) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatus.msg new file mode 100644 index 00000000..c16bf1c6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatus.msg @@ -0,0 +1,15 @@ +# +# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated. +# +uint64 timestamp # time since system start (microseconds) + +uint32 device_id_primary # current primary device id for reference + +uint32[4] device_ids +float32[4] inconsistency # magnitude of difference between sensor instance and mean +bool[4] healthy # sensor healthy +uint8[4] priority +bool[4] enabled +bool[4] external + +# TOPICS sensors_status_baro sensors_status_mag diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatusImu.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatusImu.msg new file mode 100644 index 00000000..cfad3419 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatusImu.msg @@ -0,0 +1,18 @@ +# +# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated. +# +uint64 timestamp # time since system start (microseconds) + +uint32 accel_device_id_primary # current primary accel device id for reference + +uint32[4] accel_device_ids +float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in m/s^2. +bool[4] accel_healthy +uint8[4] accel_priority + +uint32 gyro_device_id_primary # current primary gyro device id for reference + +uint32[4] gyro_device_ids +float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s). +bool[4] gyro_healthy +uint8[4] gyro_priority diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SystemPower.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SystemPower.msg new file mode 100644 index 00000000..21b35ba1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/SystemPower.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) +float32 voltage5v_v # peripheral 5V rail voltage +float32[4] sensors3v3 # Sensors 3V3 rail voltage +uint8 sensors3v3_valid # Sensors 3V3 rail voltage was read (bitfield). +uint8 usb_connected # USB is connected when 1 +uint8 brick_valid # brick bits power is good when bit 1 +uint8 usb_valid # USB is valid when 1 +uint8 servo_valid # servo power is good when 1 +uint8 periph_5v_oc # peripheral overcurrent when 1 +uint8 hipower_5v_oc # high power peripheral overcurrent when 1 +uint8 comp_5v_valid # 5V to companion valid +uint8 can1_gps1_5v_valid # 5V for CAN1/GPS1 valid + +uint8 BRICK1_VALID_SHIFTS=0 +uint8 BRICK1_VALID_MASK=1 +uint8 BRICK2_VALID_SHIFTS=1 +uint8 BRICK2_VALID_MASK=2 +uint8 BRICK3_VALID_SHIFTS=2 +uint8 BRICK3_VALID_MASK=4 +uint8 BRICK4_VALID_SHIFTS=3 +uint8 BRICK4_VALID_MASK=8 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TakeoffStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TakeoffStatus.msg new file mode 100644 index 00000000..4cc49d50 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TakeoffStatus.msg @@ -0,0 +1,14 @@ +# Status of the takeoff state machine currently just available for multicopters + +uint64 timestamp # time since system start (microseconds) + +uint8 TAKEOFF_STATE_UNINITIALIZED = 0 +uint8 TAKEOFF_STATE_DISARMED = 1 +uint8 TAKEOFF_STATE_SPOOLUP = 2 +uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 3 +uint8 TAKEOFF_STATE_RAMPUP = 4 +uint8 TAKEOFF_STATE_FLIGHT = 5 + +uint8 takeoff_state + +float32 tilt_limit # limited tilt feasibility during takeoff, contains maximum tilt otherwise diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TaskStackInfo.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TaskStackInfo.msg new file mode 100644 index 00000000..bb69bf1a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TaskStackInfo.msg @@ -0,0 +1,8 @@ +# stack information for a single running process + +uint64 timestamp # time since system start (microseconds) + +uint16 stack_free +char[24] task_name + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TecsStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TecsStatus.msg new file mode 100644 index 00000000..ae6835dc --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TecsStatus.msg @@ -0,0 +1,29 @@ +uint64 timestamp # time since system start (microseconds) + +float32 altitude_sp # Altitude setpoint AMSL [m] +float32 altitude_reference # Altitude setpoint reference AMSL [m] +float32 height_rate_reference # Height rate setpoint reference [m/s] +float32 height_rate_direct # Direct height rate setpoint from velocity reference generator [m/s] +float32 height_rate_setpoint # Height rate setpoint [m/s] +float32 height_rate # Height rate [m/s] +float32 equivalent_airspeed_sp # Equivalent airspeed setpoint [m/s] +float32 true_airspeed_sp # True airspeed setpoint [m/s] +float32 true_airspeed_filtered # True airspeed filtered [m/s] +float32 true_airspeed_derivative_sp # True airspeed derivative setpoint [m/s^2] +float32 true_airspeed_derivative # True airspeed derivative [m/s^2] +float32 true_airspeed_derivative_raw # True airspeed derivative raw [m/s^2] + +float32 total_energy_rate_sp # Total energy rate setpoint [m^2/s^3] +float32 total_energy_rate # Total energy rate estimate [m^2/s^3] + +float32 total_energy_balance_rate_sp # Energy balance rate setpoint [m^2/s^3] +float32 total_energy_balance_rate # Energy balance rate estimate [m^2/s^3] + +float32 throttle_integ # Throttle integrator value [-] +float32 pitch_integ # Pitch integrator value [rad] + +float32 throttle_sp # Current throttle setpoint [-] +float32 pitch_sp_rad # Current pitch setpoint [rad] +float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions + +float32 underspeed_ratio # 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TelemetryStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TelemetryStatus.msg new file mode 100644 index 00000000..48d85f93 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TelemetryStatus.msg @@ -0,0 +1,63 @@ +uint8 LINK_TYPE_GENERIC = 0 +uint8 LINK_TYPE_UBIQUITY_BULLET = 1 +uint8 LINK_TYPE_WIRE = 2 +uint8 LINK_TYPE_USB = 3 +uint8 LINK_TYPE_IRIDIUM = 4 + +uint64 timestamp # time since system start (microseconds) + +uint8 type # type of the radio hardware (LINK_TYPE_*) + +uint8 mode + +bool flow_control +bool forwarding +bool mavlink_v2 +bool ftp + +uint8 streams + +float32 data_rate # configured maximum data rate (Bytes/s) + +float32 rate_multiplier + +float32 tx_rate_avg # transmit rate average (Bytes/s) +float32 tx_error_rate_avg # transmit error rate average (Bytes/s) +uint32 tx_message_count # total message sent count +uint32 tx_buffer_overruns # number of TX buffer overruns + +float32 rx_rate_avg # transmit rate average (Bytes/s) +uint32 rx_message_count # count of total messages received +uint32 rx_message_lost_count +uint32 rx_buffer_overruns # number of RX buffer overruns +uint32 rx_parse_errors # number of parse errors +uint32 rx_packet_drop_count # number of packet drops +float32 rx_message_lost_rate + + +uint64 HEARTBEAT_TIMEOUT_US = 2500000 # Heartbeat timeout (tolerate missing 1 + jitter) + +# Heartbeats per type +bool heartbeat_type_antenna_tracker # MAV_TYPE_ANTENNA_TRACKER +bool heartbeat_type_gcs # MAV_TYPE_GCS +bool heartbeat_type_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER +bool heartbeat_type_gimbal # MAV_TYPE_GIMBAL +bool heartbeat_type_adsb # MAV_TYPE_ADSB +bool heartbeat_type_camera # MAV_TYPE_CAMERA +bool heartbeat_type_parachute # MAV_TYPE_PARACHUTE +bool heartbeat_type_open_drone_id # MAV_TYPE_ODID + +# Heartbeats per component +bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO +bool heartbeat_component_log # MAV_COMP_ID_LOG +bool heartbeat_component_osd # MAV_COMP_ID_OSD +bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE +bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY +bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER +bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE +bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE + +# Misc component health +bool avoidance_system_healthy +bool open_drone_id_system_healthy +bool parachute_system_healthy diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TiltrotorExtraControls.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TiltrotorExtraControls.msg new file mode 100644 index 00000000..20af316c --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TiltrotorExtraControls.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32 collective_tilt_normalized_setpoint # Collective tilt angle of motors of tiltrotor, 0: vertical, 1: horizontal [0, 1] +float32 collective_thrust_normalized_setpoint # Collective thrust setpoint [0, 1] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TimesyncStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TimesyncStatus.msg new file mode 100644 index 00000000..71e84e85 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TimesyncStatus.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 SOURCE_PROTOCOL_UNKNOWN = 0 +uint8 SOURCE_PROTOCOL_MAVLINK = 1 +uint8 SOURCE_PROTOCOL_DDS = 2 +uint8 source_protocol # timesync source + +uint64 remote_timestamp # remote system timestamp (microseconds) +int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) +int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) +uint32 round_trip_time # round trip time of this timesync packet (microseconds) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryBezier.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryBezier.msg new file mode 100644 index 00000000..e3d9d4e0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryBezier.msg @@ -0,0 +1,8 @@ +# Bezier Trajectory description. See also Mavlink TRAJECTORY msg +# The topic trajectory_bezier describe each waypoint defined in vehicle_trajectory_bezier + +uint64 timestamp # time since system start (microseconds) + +float32[3] position # local position x,y,z (metres) +float32 yaw # yaw angle (rad) +float32 delta # time it should take to get to this waypoint, if this is the final waypoint (seconds) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectorySetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectorySetpoint.msg new file mode 100644 index 00000000..4a88c867 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectorySetpoint.msg @@ -0,0 +1,15 @@ +# Trajectory setpoint in NED frame +# Input to PID position controller. +# Needs to be kinematically consistent and feasible for smooth flight. +# setting a value to NaN means the state should not be controlled + +uint64 timestamp # time since system start (microseconds) + +# NED local world frame +float32[3] position # in meters +float32[3] velocity # in meters/second +float32[3] acceleration # in meters/second^2 +float32[3] jerk # in meters/second^3 (for logging only) + +float32 yaw # euler angle of desired attitude in radians -PI..+PI +float32 yawspeed # angular velocity around NED frame z-axis in radians/second diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryWaypoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryWaypoint.msg new file mode 100644 index 00000000..6ea9bae4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryWaypoint.msg @@ -0,0 +1,13 @@ +# Waypoint Trajectory description. See also Mavlink TRAJECTORY msg +# The topic trajectory_waypoint describe each waypoint defined in vehicle_trajectory_waypoint + +uint64 timestamp # time since system start (microseconds) + +float32[3] position +float32[3] velocity +float32[3] acceleration +float32 yaw +float32 yaw_speed + +bool point_valid +uint8 type diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TransponderReport.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TransponderReport.msg new file mode 100644 index 00000000..d5171cf3 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TransponderReport.msg @@ -0,0 +1,50 @@ +uint64 timestamp # time since system start (microseconds) +uint32 icao_address # ICAO address +float64 lat # Latitude, expressed as degrees +float64 lon # Longitude, expressed as degrees +uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum +float32 altitude # Altitude(ASL) in meters +float32 heading # Course over ground in radians, -pi to +pi, 0 is north +float32 hor_velocity # The horizontal velocity in m/s +float32 ver_velocity # The vertical velocity in m/s, positive is up +char[9] callsign # The callsign, 8+null +uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum +uint8 tslc # Time since last communication in seconds +uint16 flags # Flags to indicate various statuses including valid data fields +uint16 squawk # Squawk code +uint8[18] uas_id # Unique UAS ID + +# ADSB flags +uint16 PX4_ADSB_FLAGS_VALID_COORDS = 1 +uint16 PX4_ADSB_FLAGS_VALID_ALTITUDE = 2 +uint16 PX4_ADSB_FLAGS_VALID_HEADING = 4 +uint16 PX4_ADSB_FLAGS_VALID_VELOCITY = 8 +uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16 +uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32 +uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256 + +#ADSB Emitter Data: +#from mavlink/v2.0/common/common.h +uint16 ADSB_EMITTER_TYPE_NO_INFO=0 +uint16 ADSB_EMITTER_TYPE_LIGHT=1 +uint16 ADSB_EMITTER_TYPE_SMALL=2 +uint16 ADSB_EMITTER_TYPE_LARGE=3 +uint16 ADSB_EMITTER_TYPE_HIGH_VORTEX_LARGE=4 +uint16 ADSB_EMITTER_TYPE_HEAVY=5 +uint16 ADSB_EMITTER_TYPE_HIGHLY_MANUV=6 +uint16 ADSB_EMITTER_TYPE_ROTOCRAFT=7 +uint16 ADSB_EMITTER_TYPE_UNASSIGNED=8 +uint16 ADSB_EMITTER_TYPE_GLIDER=9 +uint16 ADSB_EMITTER_TYPE_LIGHTER_AIR=10 +uint16 ADSB_EMITTER_TYPE_PARACHUTE=11 +uint16 ADSB_EMITTER_TYPE_ULTRA_LIGHT=12 +uint16 ADSB_EMITTER_TYPE_UNASSIGNED2=13 +uint16 ADSB_EMITTER_TYPE_UAV=14 +uint16 ADSB_EMITTER_TYPE_SPACE=15 +uint16 ADSB_EMITTER_TYPE_UNASSGINED3=16 +uint16 ADSB_EMITTER_TYPE_EMERGENCY_SURFACE=17 +uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18 +uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19 +uint16 ADSB_EMITTER_TYPE_ENUM_END=20 + +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TuneControl.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TuneControl.msg new file mode 100644 index 00000000..96d70af1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/TuneControl.msg @@ -0,0 +1,39 @@ +# This message is used to control the tunes, when the tune_id is set to CUSTOM +# then the frequency, duration are used otherwise those values are ignored. + +uint64 timestamp # time since system start (microseconds) + +uint8 TUNE_ID_STOP = 0 +uint8 TUNE_ID_STARTUP = 1 +uint8 TUNE_ID_ERROR = 2 +uint8 TUNE_ID_NOTIFY_POSITIVE = 3 +uint8 TUNE_ID_NOTIFY_NEUTRAL = 4 +uint8 TUNE_ID_NOTIFY_NEGATIVE = 5 +uint8 TUNE_ID_ARMING_WARNING = 6 +uint8 TUNE_ID_BATTERY_WARNING_SLOW = 7 +uint8 TUNE_ID_BATTERY_WARNING_FAST = 8 +uint8 TUNE_ID_GPS_WARNING = 9 +uint8 TUNE_ID_ARMING_FAILURE = 10 +uint8 TUNE_ID_PARACHUTE_RELEASE = 11 +uint8 TUNE_ID_SINGLE_BEEP = 12 +uint8 TUNE_ID_HOME_SET = 13 +uint8 TUNE_ID_SD_INIT = 14 +uint8 TUNE_ID_SD_ERROR = 15 +uint8 TUNE_ID_PROG_PX4IO = 16 +uint8 TUNE_ID_PROG_PX4IO_OK = 17 +uint8 TUNE_ID_PROG_PX4IO_ERR = 18 +uint8 TUNE_ID_POWER_OFF = 19 +uint8 NUMBER_OF_TUNES = 20 + +uint8 tune_id # tune_id corresponding to TuneID::* from the tune_defaults.h in the tunes library +bool tune_override # if true the tune which is playing will be stopped and the new started +uint16 frequency # in Hz +uint32 duration # in us +uint32 silence # in us +uint8 volume # value between 0-100 if supported by backend + +uint8 VOLUME_LEVEL_MIN = 0 +uint8 VOLUME_LEVEL_DEFAULT = 20 +uint8 VOLUME_LEVEL_MAX = 100 + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterRequest.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterRequest.msg new file mode 100644 index 00000000..3cfec15f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterRequest.msg @@ -0,0 +1,23 @@ +# UAVCAN-MAVLink parameter bridge request type +uint64 timestamp # time since system start (microseconds) + +uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20 # MAVLINK_MSG_ID_PARAM_REQUEST_READ +uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21 # MAVLINK_MSG_ID_PARAM_REQUEST_LIST +uint8 MESSAGE_TYPE_PARAM_SET = 23 # MAVLINK_MSG_ID_PARAM_SET +uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET + +uint8 NODE_ID_ALL = 0 # MAV_COMP_ID_ALL +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID + +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # -1 if the param_id field should be used as identifier + +uint8 PARAM_TYPE_UINT8 = 1 # MAV_PARAM_TYPE_UINT8 +uint8 PARAM_TYPE_INT64 = 8 # MAV_PARAM_TYPE_INT64 +uint8 PARAM_TYPE_REAL32 = 9 # MAV_PARAM_TYPE_REAL32 +uint8 param_type # MAVLink parameter type + +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterValue.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterValue.msg new file mode 100644 index 00000000..8eff663a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterValue.msg @@ -0,0 +1,9 @@ +# UAVCAN-MAVLink parameter bridge response type +uint64 timestamp # time since system start (microseconds) +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # parameter index, if known +uint16 param_count # number of parameters exposed by the node +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UlogStream.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UlogStream.msg new file mode 100644 index 00000000..d206b4a4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UlogStream.msg @@ -0,0 +1,19 @@ +# Message to stream ULog data from the logger. Corresponds to the LOGGING_DATA +# mavlink message + +uint64 timestamp # time since system start (microseconds) + +# flags bitmasks +uint8 FLAGS_NEED_ACK = 1 # if set, this message requires to be acked. + # Acked messages are published synchronous: a + # publisher waits for an ack before sending the + # next message + +uint8 length # length of data +uint8 first_message_offset # offset into data where first message starts. This + # can be used for recovery, when a previous message got lost +uint16 msg_sequence # allows determine drops +uint8 flags # see FLAGS_* +uint8[249] data # ulog data + +uint8 ORB_QUEUE_LENGTH = 16 # TODO: we might be able to reduce this if mavlink polled on the topic diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UlogStreamAck.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UlogStreamAck.msg new file mode 100644 index 00000000..e3747fff --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UlogStreamAck.msg @@ -0,0 +1,8 @@ +# Ack a previously sent ulog_stream message that had +# the NEED_ACK flag set + +uint64 timestamp # time since system start (microseconds) +int32 ACK_TIMEOUT = 50 # timeout waiting for an ack until we retry to send the message [ms] +int32 ACK_MAX_TRIES = 50 # maximum amount of tries to (re-)send a message, each time waiting ACK_TIMEOUT ms + +uint16 msg_sequence diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UnregisterExtComponent.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UnregisterExtComponent.msg new file mode 100644 index 00000000..2ad78d4b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/UnregisterExtComponent.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +char[25] name # either the mode name, or component name + +int8 arming_check_id # arming check registration ID (-1 if not registered) +int8 mode_id # assigned mode ID (-1 if not registered) +int8 mode_executor_id # assigned mode executor ID (-1 if not registered) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAcceleration.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAcceleration.msg new file mode 100644 index 00000000..7d555d7f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAcceleration.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] xyz # Bias corrected acceleration (including gravity) in the FRD body frame XYZ-axis in m/s^2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAirData.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAirData.msg new file mode 100644 index 00000000..59ca5e5c --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAirData.msg @@ -0,0 +1,15 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 baro_device_id # unique device ID for the selected barometer + +float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. +float32 baro_temp_celcius # Temperature in degrees Celsius +float32 baro_pressure_pa # Absolute pressure in Pascals + +float32 rho # air density +float32 eas2tas # equivalent airspeed to true airspeed conversion factor + +uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularAccelerationSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularAccelerationSetpoint.msg new file mode 100644 index 00000000..94da11b8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularAccelerationSetpoint.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2 diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularVelocity.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularVelocity.msg new file mode 100644 index 00000000..db3767c0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularVelocity.msg @@ -0,0 +1,9 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s + +float32[3] xyz_derivative # angular acceleration about the FRD body frame XYZ-axis in rad/s^2 + +# TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitude.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitude.msg new file mode 100644 index 00000000..99e6f25c --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitude.msg @@ -0,0 +1,13 @@ +# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use +# The quaternion uses the Hamilton convention, and the order is q(w, x, y, z) + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame +float32[4] delta_q_reset # Amount by which quaternion has changed during last reset +uint8 quat_reset_counter # Quaternion reset counter + +# TOPICS vehicle_attitude vehicle_attitude_groundtruth external_ins_attitude +# TOPICS estimator_attitude diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitudeSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitudeSetpoint.msg new file mode 100644 index 00000000..f52025d2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitudeSetpoint.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +float32 roll_body # body angle in NED frame (can be NaN for FW) +float32 pitch_body # body angle in NED frame (can be NaN for FW) +float32 yaw_body # body angle in NED frame (can be NaN for FW) + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control + +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body FRD frame [-1,1] + +bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) + +bool fw_control_yaw_wheel # control heading with steering wheel (used for auto takeoff on runway) + +# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommand.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommand.msg new file mode 100644 index 00000000..b147bef0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommand.msg @@ -0,0 +1,187 @@ +# Vehicle Command uORB message. Used for commanding a mission / action / etc. +# Follows the MAVLink COMMAND_INT / COMMAND_LONG definition + +uint64 timestamp # time since system start (microseconds) + +uint16 VEHICLE_CMD_CUSTOM_0 = 0 # test command +uint16 VEHICLE_CMD_CUSTOM_1 = 1 # test command +uint16 VEHICLE_CMD_CUSTOM_2 = 2 # test command +uint16 VEHICLE_CMD_NAV_WAYPOINT = 16 # Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_UNLIM = 17 # Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_TURNS = 18 # Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_TIME = 19 # Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_PRECLAND = 23 # Attempt a precision landing +uint16 VEHICLE_CMD_DO_ORBIT = 34 # Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z | +uint16 VEHICLE_CMD_DO_FIGUREEIGHT = 35 # Start flying on the outline of a figure eight defined by the parameters. |Major Radius [m] |Minor Radius [m] |Velocity [m/s] |Orientation |Latitude/X |Longitude/Y |Altitude/Z | +uint16 VEHICLE_CMD_NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| +uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| +uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84 # Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_VTOL_LAND = 85 # Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_DELAY = 112 # Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| +uint16 VEHICLE_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_YAW = 115 # Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_GATE = 4501 # Wait until passing a threshold |2D coord mode: 0: Orthogonal to planned route | Altitude mode: 0: Ignore altitude| Empty| Empty| Lat| Lon| Alt| +uint16 VEHICLE_CMD_DO_SET_MODE = 176 # Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_RELAY = 181 # Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cycles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CHANGE_ALTITUDE = 186 # Set the vehicle to Loiter mode and change the altitude to specified value |Altitude| Frame of new altitude | Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187 # Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index| +uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| +uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonomous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPOSITION = 192 # Reposition to specific WGS84 GPS position. |Ground speed [m/s] |Bitmask |Loiter radius [m] for planes |Yaw [deg] |Latitude |Longitude |Altitude | +uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 +uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint| +uint16 VEHICLE_CMD_DO_SET_ROI_NONE = 197 # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_ROI = 201 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| +uint16 VEHICLE_CMD_DO_DIGICAM_CONTROL=203 +uint16 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204 # Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| +uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set TRIG_DIST for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOTOR_TEST=209 # motor test command |Instance (1, ...)| throttle type| throttle| timeout [s]| Motor count | Test order| Empty| +uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GRIPPER = 211 # Command to operate a gripper +uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION +uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration +uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| +uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started +uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| +uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| +uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty| +uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVLink mode |MAV_STANDARD_MODE| +uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal + +uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| +uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function| +uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command|configuration|Empty|Empty|Empty|output function| +uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm +uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401 # Instructs a target system to run pre-arm checks. +uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes +uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message +uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) +uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom +uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532 +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control +uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence. +uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system +uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500 # Start a video capture. +uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture. +uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data +uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data +uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # control starting/stopping transmitting data over the high latency link +uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition +uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization +uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan +uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment +uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. +uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. + +uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning + +# PX4 vehicle commands (beyond 16 bit mavlink commands) +uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) +uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude| +uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty| + +uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization | +uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | +uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | +uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | +uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt | +uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5 # + +uint8 VEHICLE_ROI_NONE = 0 # No region of interest | +uint8 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION | +uint8 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION | +uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location | +uint8 VEHICLE_ROI_TARGET = 4 # Point toward target +uint8 VEHICLE_ROI_ENUM_END = 5 + +uint8 PARACHUTE_ACTION_DISABLE = 0 +uint8 PARACHUTE_ACTION_ENABLE = 1 +uint8 PARACHUTE_ACTION_RELEASE = 2 + +uint8 FAILURE_UNIT_SENSOR_GYRO = 0 +uint8 FAILURE_UNIT_SENSOR_ACCEL = 1 +uint8 FAILURE_UNIT_SENSOR_MAG = 2 +uint8 FAILURE_UNIT_SENSOR_BARO = 3 +uint8 FAILURE_UNIT_SENSOR_GPS = 4 +uint8 FAILURE_UNIT_SENSOR_OPTICAL_FLOW = 5 +uint8 FAILURE_UNIT_SENSOR_VIO = 6 +uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7 +uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8 +uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100 +uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101 +uint8 FAILURE_UNIT_SYSTEM_SERVO = 102 +uint8 FAILURE_UNIT_SYSTEM_AVOIDANCE = 103 +uint8 FAILURE_UNIT_SYSTEM_RC_SIGNAL = 104 +uint8 FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL = 105 + +uint8 FAILURE_TYPE_OK = 0 +uint8 FAILURE_TYPE_OFF = 1 +uint8 FAILURE_TYPE_STUCK = 2 +uint8 FAILURE_TYPE_GARBAGE = 3 +uint8 FAILURE_TYPE_WRONG = 4 +uint8 FAILURE_TYPE_SLOW = 5 +uint8 FAILURE_TYPE_DELAYED = 6 +uint8 FAILURE_TYPE_INTERMITTENT = 7 + +# used as param1 in DO_CHANGE_SPEED command +uint8 SPEED_TYPE_AIRSPEED = 0 +uint8 SPEED_TYPE_GROUNDSPEED = 1 +uint8 SPEED_TYPE_CLIMB_SPEED = 2 +uint8 SPEED_TYPE_DESCEND_SPEED = 3 + +# used as param1 in ARM_DISARM command +int8 ARMING_ACTION_DISARM = 0 +int8 ARMING_ACTION_ARM = 1 + +# param2 in VEHICLE_CMD_DO_GRIPPER +uint8 GRIPPER_ACTION_RELEASE = 0 +uint8 GRIPPER_ACTION_GRAB = 1 + +uint8 ORB_QUEUE_LENGTH = 8 + +float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param2 # Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param3 # Parameter 3, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param4 # Parameter 4, as defined by MAVLink uint16 VEHICLE_CMD enum. +float64 param5 # Parameter 5, as defined by MAVLink uint16 VEHICLE_CMD enum. +float64 param6 # Parameter 6, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param7 # Parameter 7, as defined by MAVLink uint16 VEHICLE_CMD enum. +uint32 command # Command ID +uint8 target_system # System which should execute the command +uint8 target_component # Component which should execute the command, 0 for all components +uint8 source_system # System sending the command +uint16 source_component # Component / mode executor sending the command +uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) +bool from_external + +uint16 COMPONENT_MODE_EXECUTOR_START = 1000 + +# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommandAck.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommandAck.msg new file mode 100644 index 00000000..6f54fa46 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommandAck.msg @@ -0,0 +1,33 @@ +# Vehicle Command Ackonwledgement uORB message. +# Used for acknowledging the vehicle command being received. +# Follows the MAVLink COMMAND_ACK message definition + +uint64 timestamp # time since system start (microseconds) + +# Result cases. This follows the MAVLink MAV_RESULT enum definition +uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | +uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | +uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | +uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED | +uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed | +uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed | +uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command Canceled + +# Arming denied specific cases +uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0 +uint16 ARM_AUTH_DENIED_REASON_NONE = 1 +uint16 ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2 +uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 +uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 +uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 + +uint8 ORB_QUEUE_LENGTH = 4 + +uint32 command # Command that is being acknowledged +uint8 result # Command result +uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS +int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. +uint8 target_system +uint16 target_component # Target component / mode executor + +bool from_external # Indicates if the command came from an external source diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleConstraints.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleConstraints.msg new file mode 100644 index 00000000..aa3a491b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleConstraints.msg @@ -0,0 +1,9 @@ +# Local setpoint constraints in NED frame +# setting something to NaN means that no limit is provided + +uint64 timestamp # time since system start (microseconds) + +float32 speed_up # in meters/sec +float32 speed_down # in meters/sec + +bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleControlMode.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleControlMode.msg new file mode 100644 index 00000000..9b33f9b8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleControlMode.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) +bool flag_armed # synonym for actuator_armed.armed + +bool flag_multicopter_position_control_enabled + +bool flag_control_manual_enabled # true if manual input is mixed in +bool flag_control_auto_enabled # true if onboard autopilot should act +bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_position_enabled # true if position is controlled +bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled +bool flag_control_altitude_enabled # true if altitude is controlled +bool flag_control_climb_rate_enabled # true if climb rate is controlled +bool flag_control_acceleration_enabled # true if acceleration is controlled +bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_rates_enabled # true if rates are stabilized +bool flag_control_allocation_enabled # true if control allocation is enabled +bool flag_control_termination_enabled # true if flighttermination is enabled + +# TODO: use dedicated topic for external requests +uint8 source_id # Mode ID (nav_state) + +# TOPICS vehicle_control_mode config_control_setpoints diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleGlobalPosition.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleGlobalPosition.msg new file mode 100644 index 00000000..c7d9ee78 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleGlobalPosition.msg @@ -0,0 +1,30 @@ +# Fused global position in WGS84. +# This struct contains global position estimation. It is not the raw GPS +# measurement (@see vehicle_gps_position). This topic is usually published by the position +# estimator, which will take more sources of information into account than just GPS, +# e.g. control inputs of the vehicle in a Kalman-filter implementation. +# + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) +float32 alt_ellipsoid # Altitude above ellipsoid, (meters) + +float32 delta_alt # Reset delta for altitude +uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates +uint8 alt_reset_counter # Counter for reset events on altitude + +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) + +float32 terrain_alt # Terrain altitude WGS84, (metres) +bool terrain_alt_valid # Terrain altitude estimate is valid + +bool dead_reckoning # True if this position is estimated through dead-reckoning + +# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position +# TOPICS estimator_global_position +# TOPICS aux_global_position diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImu.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImu.msg new file mode 100644 index 00000000..a71bb7a0 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImu.msg @@ -0,0 +1,23 @@ +# IMU readings in SI-unit form. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 accel_device_id # Accelerometer unique device ID for the sensor that does not change between power cycles +uint32 gyro_device_id # Gyroscope unique device ID for the sensor that does not change between power cycles + +float32[3] delta_angle # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt) +float32[3] delta_velocity # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt) + +uint16 delta_angle_dt # integration period in microseconds +uint16 delta_velocity_dt # integration period in microseconds + +uint8 CLIPPING_X = 1 +uint8 CLIPPING_Y = 2 +uint8 CLIPPING_Z = 4 + +uint8 delta_angle_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame +uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame + +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImuStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImuStatus.msg new file mode 100644 index 00000000..78fb4470 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImuStatus.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles +uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles + +uint32[3] accel_clipping # total clipping per axis +uint32[3] gyro_clipping # total clipping per axis + +uint32 accel_error_count +uint32 gyro_error_count + +float32 accel_rate_hz +float32 gyro_rate_hz + +float32 accel_raw_rate_hz # full raw sensor sample rate (Hz) +float32 gyro_raw_rate_hz # full raw sensor sample rate (Hz) + +float32 accel_vibration_metric # high frequency vibration level in the accelerometer data (m/s/s) +float32 gyro_vibration_metric # high frequency vibration level in the gyro data (rad/s) +float32 delta_angle_coning_metric # average IMU delta angle coning correction (rad^2) + +float32[3] mean_accel # average accelerometer readings since last publication +float32[3] mean_gyro # average gyroscope readings since last publication +float32[3] var_accel # accelerometer variance since last publication +float32[3] var_gyro # gyroscope variance since last publication + +float32 temperature_accel +float32 temperature_gyro diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLandDetected.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLandDetected.msg new file mode 100644 index 00000000..fc0ca4a6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLandDetected.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) + +bool freefall # true if vehicle is currently in free-fall +bool ground_contact # true if vehicle has ground contact but is not landed (1. stage) +bool maybe_landed # true if the vehicle might have landed (2. stage) +bool landed # true if vehicle is currently landed on the ground (3. stage) + +bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing). +bool in_descend + +bool has_low_throttle + +bool vertical_movement +bool horizontal_movement +bool rotational_movement + +bool close_to_ground_or_skipped_check + +bool at_rest diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPosition.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPosition.msg new file mode 100644 index 00000000..c1a0dffe --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPosition.msg @@ -0,0 +1,79 @@ +# Fused local position in NED. +# The coordinate system origin is the vehicle position at the time when the EKF2-module was started. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vx and vy are valid +bool v_z_valid # true if vz is valid + +# Position in local NED frame +float32 x # North position in NED earth-fixed frame, (metres) +float32 y # East position in NED earth-fixed frame, (metres) +float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres) + +# Position reset delta +float32[2] delta_xy # Amount of lateral shift of position estimate in latest reset (in x and y) [m] +uint8 xy_reset_counter # Index of latest lateral position estimate reset +float32 delta_z # Amount of vertical shift of position estimate in latest reset [m] +uint8 z_reset_counter # Index of latest vertical position estimate reset + +# Velocity in NED frame +float32 vx # North velocity in NED earth-fixed frame, (metres/sec) +float32 vy # East velocity in NED earth-fixed frame, (metres/sec) +float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) +float32 z_deriv # Down position time derivative in NED earth-fixed frame, (metres/sec) + +# Velocity reset delta +float32[2] delta_vxy # Amount of lateral shift of velocity estimate in latest reset (in x and y) [m/s] +uint8 vxy_reset_counter # Index of latest vertical velocity estimate reset +float32 delta_vz # Amount of vertical shift of velocity estimate in latest reset [m/s] +uint8 vz_reset_counter # Index of latest vertical velocity estimate reset + +# Acceleration in NED frame +float32 ax # North velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2) + +float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 heading_var +float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only +float32 delta_heading # Heading delta caused by latest heading reset [rad] +uint8 heading_reset_counter # Index of latest heading reset +bool heading_good_for_control + +float32 tilt_var + +# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame +bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon) +bool z_global # true if z has a valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set since system start, (microseconds) +float64 ref_lat # Reference point latitude, (degrees) +float64 ref_lon # Reference point longitude, (degrees) +float32 ref_alt # Reference altitude AMSL, (metres) + +# Distance to surface +float32 dist_bottom # Distance from from bottom surface to ground, (metres) +bool dist_bottom_valid # true if distance to bottom surface is valid +uint8 dist_bottom_sensor_bitfield # bitfield indicating what type of sensor is used to estimate dist_bottom +uint8 DIST_BOTTOM_SENSOR_NONE = 0 +uint8 DIST_BOTTOM_SENSOR_RANGE = 1 # (1 << 0) a range sensor is used to estimate dist_bottom field +uint8 DIST_BOTTOM_SENSOR_FLOW = 2 # (1 << 1) a flow sensor is used to estimate dist_bottom field (mostly fixed-wing use case) + +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) +float32 evh # Standard deviation of horizontal velocity error, (metres/sec) +float32 evv # Standard deviation of vertical velocity error, (metres/sec) + +bool dead_reckoning # True if this position is estimated through dead-reckoning + +# estimator specified vehicle limits +float32 vxy_max # maximum horizontal speed - set to 0 when limiting not required (meters/sec) +float32 vz_max # maximum vertical speed - set to 0 when limiting not required (meters/sec) +float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters) +float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters) + +# TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position +# TOPICS estimator_local_position diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPositionSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPositionSetpoint.msg new file mode 100644 index 00000000..0093d52d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPositionSetpoint.msg @@ -0,0 +1,19 @@ +# Local position setpoint in NED frame +# Telemetry of PID position controller to monitor tracking. +# NaN means the state was not controlled + +uint64 timestamp # time since system start (microseconds) + +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED + +float32 vx # in meters/sec +float32 vy # in meters/sec +float32 vz # in meters/sec + +float32[3] acceleration # in meters/sec^2 +float32[3] thrust # normalized thrust vector in NED + +float32 yaw # in radians NED -PI..+PI +float32 yawspeed # in radians/sec diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleMagnetometer.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleMagnetometer.msg new file mode 100644 index 00000000..f2249c78 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleMagnetometer.msg @@ -0,0 +1,10 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the selected magnetometer + +float32[3] magnetometer_ga # Magnetic field in the FRD body frame XYZ-axis in Gauss + +uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOdometry.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOdometry.msg new file mode 100644 index 00000000..fbdd1920 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOdometry.msg @@ -0,0 +1,31 @@ +# Vehicle odometry data. Fits ROS REP 147 for aerial vehicles +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint8 POSE_FRAME_UNKNOWN = 0 +uint8 POSE_FRAME_NED = 1 # NED earth-fixed frame +uint8 POSE_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference +uint8 pose_frame # Position and orientation frame of reference + +float32[3] position # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown +float32[4] q # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown + +uint8 VELOCITY_FRAME_UNKNOWN = 0 +uint8 VELOCITY_FRAME_NED = 1 # NED earth-fixed frame +uint8 VELOCITY_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference +uint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frame +uint8 velocity_frame # Reference frame of the velocity data + +float32[3] velocity # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown + +float32[3] angular_velocity # Angular velocity in body-fixed frame (rad/s). NaN if invalid/unknown + +float32[3] position_variance +float32[3] orientation_variance +float32[3] velocity_variance + +uint8 reset_counter +int8 quality + +# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry +# TOPICS estimator_odometry diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlow.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlow.msg new file mode 100644 index 00000000..13bdb57b --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlow.msg @@ -0,0 +1,21 @@ +# Optical flow in XYZ body frame in SI units. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable) + +float32 distance_m # (meters) Distance to the center of the flow field (NAN if unavailable) + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlowVel.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlowVel.msg new file mode 100644 index 00000000..947131da --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlowVel.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) +float32[2] vel_ne # same as vel_body but in local frame (m/s) + +float32[2] flow_rate_uncompensated # integrated optical flow measurement (rad/s) +float32[2] flow_rate_compensated # integrated optical flow measurement compensated for angular motion (rad/s) + +float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) + +float32[3] gyro_bias +float32[3] ref_gyro + +# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRatesSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRatesSetpoint.msg new file mode 100644 index 00000000..35a06c35 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRatesSetpoint.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +# body angular rates in FRD frame +float32 roll # [rad/s] roll rate setpoint +float32 pitch # [rad/s] pitch rate setpoint +float32 yaw # [rad/s] yaw rate setpoint + +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] + +bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRoi.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRoi.msg new file mode 100644 index 00000000..2948f157 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRoi.msg @@ -0,0 +1,21 @@ +# Vehicle Region Of Interest (ROI) + +uint64 timestamp # time since system start (microseconds) + +uint8 ROI_NONE = 0 # No region of interest +uint8 ROI_WPNEXT = 1 # Point toward next MISSION with optional offset +uint8 ROI_WPINDEX = 2 # Point toward given MISSION +uint8 ROI_LOCATION = 3 # Point toward fixed location +uint8 ROI_TARGET = 4 # Point toward target +uint8 ROI_ENUM_END = 5 + +uint8 mode # ROI mode (see above) + +float64 lat # Latitude to point to +float64 lon # Longitude to point to +float32 alt # Altitude to point to + +# additional angle offsets to next waypoint (only used with ROI_WPNEXT) +float32 roll_offset # angle offset in rad +float32 pitch_offset # angle offset in rad +float32 yaw_offset # angle offset in rad diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleStatus.msg new file mode 100644 index 00000000..4c711b97 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleStatus.msg @@ -0,0 +1,138 @@ +# Encodes the system state of the vehicle published by commander + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 +uint8 ARM_DISARM_REASON_RC_STICK = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6 +uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 +uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 +uint8 ARM_DISARM_REASON_LOCKDOWN = 10 +uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 +uint8 ARM_DISARM_REASON_SHUTDOWN = 12 +uint8 ARM_DISARM_REASON_UNIT_TEST = 13 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_FREE4 = 8 +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_AIRSHIP = 4 + +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* + +# Link loss +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleThrustSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleThrustSetpoint.msg new file mode 100644 index 00000000..444ee500 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleThrustSetpoint.msg @@ -0,0 +1,8 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # thrust setpoint along X, Y, Z body axis [-1, 1] + +# TOPICS vehicle_thrust_setpoint +# TOPICS vehicle_thrust_setpoint_virtual_fw vehicle_thrust_setpoint_virtual_mc diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTorqueSetpoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTorqueSetpoint.msg new file mode 100644 index 00000000..c20519b1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTorqueSetpoint.msg @@ -0,0 +1,8 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # torque setpoint about X, Y, Z body axis (normalized) + +# TOPICS vehicle_torque_setpoint +# TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryBezier.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryBezier.msg new file mode 100644 index 00000000..d4bf99b4 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryBezier.msg @@ -0,0 +1,18 @@ +# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg +# The topic vehicle_trajectory_bezier is used to send a smooth flight path from the +# companion computer / avoidance module to the position controller. + +uint64 timestamp # time since system start (microseconds) + +uint8 POINT_0 = 0 +uint8 POINT_1 = 1 +uint8 POINT_2 = 2 +uint8 POINT_3 = 3 +uint8 POINT_4 = 4 + +uint8 NUMBER_POINTS = 5 + +TrajectoryBezier[5] control_points +uint8 bezier_order + +# TOPICS vehicle_trajectory_bezier diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryWaypoint.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryWaypoint.msg new file mode 100644 index 00000000..6bff1cec --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryWaypoint.msg @@ -0,0 +1,21 @@ +# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg +# The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module. +# The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller. + +uint64 timestamp # time since system start (microseconds) + +uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 + +uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum. + +uint8 POINT_0 = 0 +uint8 POINT_1 = 1 +uint8 POINT_2 = 2 +uint8 POINT_3 = 3 +uint8 POINT_4 = 4 + +uint8 NUMBER_POINTS = 5 + +TrajectoryWaypoint[5] waypoints + +# TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VelocityLimits.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VelocityLimits.msg new file mode 100644 index 00000000..9ab5115a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VelocityLimits.msg @@ -0,0 +1,8 @@ +# Velocity and yaw rate limits for a multicopter position slow mode only + +uint64 timestamp # time since system start (microseconds) + +# absolute speeds, NAN means use default limit +float32 horizontal_velocity # [m/s] +float32 vertical_velocity # [m/s] +float32 yaw_rate # [rad/s] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VtolVehicleStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VtolVehicleStatus.msg new file mode 100644 index 00000000..61a82467 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/VtolVehicleStatus.msg @@ -0,0 +1,12 @@ +# VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE +uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 +uint8 VEHICLE_VTOL_STATE_MC = 3 +uint8 VEHICLE_VTOL_STATE_FW = 4 + +uint64 timestamp # time since system start (microseconds) + +uint8 vehicle_vtol_state # current state of the vtol, see VEHICLE_VTOL_STATE + +bool fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/WheelEncoders.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/WheelEncoders.msg new file mode 100644 index 00000000..a4f3955d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/WheelEncoders.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +# Two wheels: 0 right, 1 left +float32[2] wheel_speed # [rad/s] +float32[2] wheel_angle # [rad] diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Wind.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Wind.msg new file mode 100644 index 00000000..ff8b6f45 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/Wind.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated + +float32 tas_innov # True airspeed innovation +float32 tas_innov_var # True airspeed innovation variance + +float32 beta_innov # Sideslip measurement innovation +float32 beta_innov_var # Sideslip measurement innovation variance + +# TOPICS wind estimator_wind diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/YawEstimatorStatus.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/YawEstimatorStatus.msg new file mode 100644 index 00000000..36091e26 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/msg/YawEstimatorStatus.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 yaw_composite # composite yaw from GSF (rad) +float32 yaw_variance # composite yaw variance from GSF (rad^2) +bool yaw_composite_valid + +float32[5] yaw # yaw estimate for each model in the filter bank (rad) +float32[5] innov_vn # North velocity innovation for each model in the filter bank (m/s) +float32[5] innov_ve # East velocity innovation for each model in the filter bank (m/s) +float32[5] weight # weighting for each model in the filter bank diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/package.xml new file mode 100644 index 00000000..65291fb6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/package.xml @@ -0,0 +1,26 @@ + + + + px4_msgs + 2.0.1 + Package with the ROS-equivalent of PX4 uORB msgs + Nuno Marques + Nuno Marques + BSD 3-Clause + + ament_cmake + rosidl_default_generators + + builtin_interfaces + ros_environment + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/srv/VehicleCommand.srv b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/srv/VehicleCommand.srv new file mode 100644 index 00000000..134e2a81 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_msgs/srv/VehicleCommand.srv @@ -0,0 +1,3 @@ +VehicleCommand request +--- +VehicleCommandAck reply diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.github/dependabot.yml b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.github/dependabot.yml new file mode 100644 index 00000000..c7fc735e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.github/dependabot.yml @@ -0,0 +1,6 @@ +version: 2 +updates: + - package-ecosystem: github-actions + directory: "/" + schedule: + interval: "daily" diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.github/workflows/build.yml b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.github/workflows/build.yml new file mode 100644 index 00000000..ed4afbf1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.github/workflows/build.yml @@ -0,0 +1,48 @@ +name: Build and Test package + +# CI runs over all branches +on: + push: + branches: + - 'main' + +defaults: + run: + shell: bash + +jobs: + build_and_test: + name: "ROS 2 ${{ matrix.ros2_distro }}" + runs-on: ubuntu-20.04 + container: px4io/px4-dev-ros2-${{ matrix.ros2_distro }}:2021-05-31 + strategy: + matrix: + ros2_distro: [foxy, galactic, humble, rolling] + steps: + - uses: actions/checkout@v4 + - name: Configure workspace + run: | + unset ROS_DISTRO + mkdir -p ~/colcon_ws/src + cd ~/colcon_ws + ln -s ${GITHUB_WORKSPACE} src/px4_ros_com + git clone https://github.com/PX4/px4_msgs.git -b main src/px4_msgs + - name: Build package + run: | + cd ~/colcon_ws/src/px4_ros_com/scripts + ./build_ros2_workspace.bash --verbose -ros_distro ${{ matrix.ros2_distro }} --ros_path /opt/ros/${{ matrix.ros2_distro }}/setup.bash + # - name: Build PX4 Firmware + # run: | + # git clone https://github.com/PX4/Firmware.git ~/PX4/Firmware + # cd ~/PX4/Firmware + # DONT_RUN=1 make px4_sitl_rtps gazebo + # - name: SITL integration test - data output + # run: | + # source ~/colcon_ws/install/setup.bash + # cd ~/colcon_ws/src/px4_ros_com/test + # python3 pipeline_io_test.py -f ~/PX4/Firmware/ -p debug_vect -t fcu_output + # - name: SITL integration test - data input + # run: | + # source ~/colcon_ws/install/setup.bash + # cd ~/colcon_ws/src/px4_ros_com/test + # python3 pipeline_io_test.py -f ~/PX4/Firmware/ -s sensor_combined -t fcu_input diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.gitignore b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.gitignore new file mode 100644 index 00000000..86d46f8d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.gitignore @@ -0,0 +1,2 @@ +build/ +scripts/__pycache__ diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.vscode/settings.json b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.vscode/settings.json new file mode 100644 index 00000000..0f4d0a0f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "chrono": "cpp", + "ostream": "cpp" + } +} \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/CMakeLists.txt new file mode 100644 index 00000000..dc37f55d --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/CMakeLists.txt @@ -0,0 +1,128 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_ros_com) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(rclpy REQUIRED) + +################# +# Setup targets # +################# + +include_directories(include) + +# Add frame_transforms lib +add_library(frame_transforms SHARED src/lib/frame_transforms.cpp) +ament_target_dependencies(frame_transforms Eigen3 geometry_msgs sensor_msgs) +target_include_directories(frame_transforms PUBLIC + $ + $ +) + + +# examples/listeners/sensor_combined_listener +add_executable(sensor_combined_listener src/examples/listeners/sensor_combined_listener.cpp) +ament_target_dependencies(sensor_combined_listener rclcpp px4_msgs) +install(TARGETS sensor_combined_listener DESTINATION lib/${PROJECT_NAME}) + +# examples/listeners/vehicle_gps_position_listener +add_executable(vehicle_gps_position_listener src/examples/listeners/vehicle_gps_position_listener.cpp) +ament_target_dependencies(vehicle_gps_position_listener rclcpp px4_msgs) +install(TARGETS vehicle_gps_position_listener DESTINATION lib/${PROJECT_NAME}) + +# examples/advertisers/debug_vect_advertiser +add_executable(debug_vect_advertiser src/examples/advertisers/debug_vect_advertiser.cpp) +ament_target_dependencies(debug_vect_advertiser rclcpp px4_msgs) +install(TARGETS debug_vect_advertiser DESTINATION lib/${PROJECT_NAME}) + +# examples/offboard/offboard_control +add_executable(offboard_control src/examples/offboard/offboard_control.cpp) +ament_target_dependencies(offboard_control rclcpp px4_msgs) +install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME}) + +# examples/offboard/offboard_control_srv +add_executable(offboard_control_srv src/examples/offboard/offboard_control_srv.cpp) +ament_target_dependencies(offboard_control_srv rclcpp px4_msgs) +install(TARGETS offboard_control_srv DESTINATION lib/${PROJECT_NAME}) + + +############ +# Install ## +############ + +# Export information to downstream packages +ament_export_dependencies(ament_cmake rclcpp rosidl_default_runtime eigen3_cmake_module Eigen3 px4_msgs geometry_msgs sensor_msgs) + +ament_export_targets(export_frame_transforms HAS_LIBRARY_TARGET) + +ament_export_include_directories(include) +ament_export_libraries(frame_transforms) + +# Install header files +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +install(TARGETS frame_transforms + EXPORT export_frame_transforms + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# Install launch files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) + +# Install tests +install(DIRECTORY test DESTINATION share/${PROJECT_NAME}/) + + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +########### +# Python ## +########### + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + src/examples/offboard_py/offboard_control.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/LICENSE b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/LICENSE new file mode 100644 index 00000000..7664bbb9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2018-2019, 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: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/README.md b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/README.md new file mode 100644 index 00000000..25ae9714 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/README.md @@ -0,0 +1,21 @@ +# PX4-ROS2 bridge + +[![GitHub license](https://img.shields.io/github/license/PX4/px4_ros_com.svg)](https://github.com/PX4/px4_ros_com/blob/master/LICENSE) [![GitHub (pre-)release](https://img.shields.io/github/release-pre/PX4/px4_ros_com.svg)](https://github.com/PX4/px4_ros_com/releases/tag/beta) [![DOI](https://zenodo.org/badge/142936318.svg)](https://zenodo.org/badge/latestdoi/142936318) [![Build and Test package](https://github.com/PX4/px4_ros_com/workflows/Build%20and%20Test%20package/badge.svg?branch=master)](https://github.com/PX4/px4_ros_com/actions) + +[![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode) + +This package provides example nodes for exchanging data and commands between ROS2 and PX4. +It also provides a [library](./include/px4_ros_com/frame_transforms.h) to ease the conversion between ROS2 and PX4 frame conventions. +It has a straight dependency on the [`px4_msgs`](https://github.com/PX4/px4_msgs) package. + +## Install, build and usage + +Check the [uXRCE-DDS](https://docs.px4.io/main/en/middleware/uxrce_dds.html) and the [ROS2 Interface](https://docs.px4.io/main/en/ros/ros2_comm.html) sections on the PX4 Devguide for details on how to install the required dependencies, build the package and use it. + +## Bug tracking and feature requests + +Use the [Issues](https://github.com/PX4/px4_ros_com/issues) section to create a new issue. Report your issue or feature request [here](https://github.com/PX4/px4_ros_com/issues/new). + +## Questions and troubleshooting + +Reach the PX4 development team on the [PX4 Discord Server](https://discord.gg/dronecode). diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/include/px4_ros_com/frame_transforms.h b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/include/px4_ros_com/frame_transforms.h new file mode 100644 index 00000000..14ad7bee --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/include/px4_ros_com/frame_transforms.h @@ -0,0 +1,448 @@ +/**************************************************************************** + * + * 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 Functions to use on frame transforms + * @file frame_transform.h + * @addtogroup lib + * @author Nuno Marques + * + * Adapted from MAVROS frame_tf.h + */ + +#ifndef FRAME_TRANSFORMS_H +#define FRAME_TRANSFORMS_H + +#include +#include +#include + +// for Covariance types +#include +#include +#include +#include +#include + +namespace px4_ros_com +{ +namespace frame_transforms +{ + +//! Type matching rosmsg for 3x3 covariance matrix +using Covariance3d = sensor_msgs::msg::Imu::_angular_velocity_covariance_type; + +//! Type matching rosmsg for 6x6 covariance matrix +using Covariance6d = geometry_msgs::msg::PoseWithCovariance::_covariance_type; + +//! Type matching rosmsg for 9x9 covariance matrix +using Covariance9d = std::array; + +//! Eigen::Map for Covariance3d +using EigenMapCovariance3d = Eigen::Map>; +using EigenMapConstCovariance3d = Eigen::Map>; + +//! Eigen::Map for Covariance6d +using EigenMapCovariance6d = Eigen::Map>; +using EigenMapConstCovariance6d = Eigen::Map>; + +//! Eigen::Map for Covariance9d +using EigenMapCovariance9d = Eigen::Map>; +using EigenMapConstCovariance9d = Eigen::Map>; + +/** + * @brief Orientation transform options when applying rotations to data + */ +enum class StaticTF { + NED_TO_ENU, //!< change from expressed WRT NED frame to WRT ENU frame + ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame + AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame + BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft + ECEF_TO_ENU, //!< change from expressed WRT ECEF frame to WRT ENU frame + ENU_TO_ECEF //!< change from expressed WRT ENU frame to WRT ECEF frame +}; + +// Utils to ease conversions +namespace utils +{ + +// Quaternion +namespace quaternion +{ + +/** + * @brief Convert euler angles to quaternion. + */ +Eigen::Quaterniond quaternion_from_euler(const Eigen::Vector3d &euler); + +/** + * @brief Convert euler angles to quaternion. + * + * @return quaternion, same as @a tf::quaternionFromeuler() but in Eigen format. + */ +Eigen::Quaterniond quaternion_from_euler(const double roll, const double pitch, const double yaw); + +/** + * @brief Convert quaternion to euler angles + * @return Eigen::Quaterniond + */ +Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond &q); + +/** + * @brief Convert quaternion to euler angles + */ +void quaternion_to_euler(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw); + +/** + * @brief Store Quaternion to float[4] + * Eigen::Quaterniond xyzw internal order to PX4 quaternion wxyz. + */ +void eigen_quat_to_array(const Eigen::Quaterniond &q, std::array &qarray); + +/** + * @brief Convert float[4] quaternion to Eigen Quaternion + */ +Eigen::Quaterniond array_to_eigen_quat(const std::array &q); + +/** + * @brief Get Yaw angle from quaternion + */ +double quaternion_get_yaw(const Eigen::Quaterniond &q); + +} // namespace quaternion + +// Data types +namespace types +{ + +/** + * @brief Convert covariance matrix to float[n] + */ +template void covariance_to_array(const T &cov, std::array &covmsg); + +/** + * @brief Convert upper right triangular of a covariance matrix to float[n] array + */ +template +void covariance_urt_to_array(const T &covmap, std::array &covmsg); + +/** + * @brief Convert float[n] array (upper right triangular of a covariance matrix) + * to Eigen::MatrixXd full covariance matrix + */ +template +void array_urt_to_covariance_matrix(const std::array &covmsg, T &covmat); + +} // namespace types +} // namespace utils + +/** + * @brief Static quaternion needed for rotating between ENU and NED frames + * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) + * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) + */ +static const auto NED_ENU_Q = utils::quaternion::quaternion_from_euler(M_PI, 0.0, M_PI_2); + +/** + * @brief Static quaternion needed for rotating between aircraft and base_link frames + * +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft) + * Fto Forward, Left, Up (base_link) frames. + */ +static const auto AIRCRAFT_BASELINK_Q = utils::quaternion::quaternion_from_euler(M_PI, 0.0, 0.0); + +/** + * @brief Static vector needed for rotating between ENU and NED frames + * +PI rotation around X (North) axis follwed by +PI/2 rotation about Z (Down) + * gives the ENU frame. Similarly, a +PI rotation about X (East) followed by + * a +PI/2 roation about Z (Up) gives the NED frame. + */ +static const Eigen::Affine3d NED_ENU_AFFINE(NED_ENU_Q); + +/** + * @brief Static vector needed for rotating between aircraft and base_link frames + * +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft) + * Fto Forward, Left, Up (base_link) frames. + */ +static const Eigen::Affine3d AIRCRAFT_BASELINK_AFFINE(AIRCRAFT_BASELINK_Q); + +/** + * @brief 3-D matrices to fill 6-D rotation matrix applied to change covariance matrices coordinate frames + */ +static const auto NED_ENU_R = NED_ENU_Q.normalized().toRotationMatrix(); +static const auto AIRCRAFT_BASELINK_R = AIRCRAFT_BASELINK_Q.normalized().toRotationMatrix(); + +/** + * @brief Use reflections instead of rotations for NED <-> ENU transformation + * to avoid NaN/Inf floating point pollution across different axes + * since in NED <-> ENU the axes are perfectly aligned. + */ +static const Eigen::PermutationMatrix<3> NED_ENU_REFLECTION_XY(Eigen::Vector3i(1, 0, 2)); +static const Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z(1, 1, -1); + +/** + * @brief Auxiliar matrices to Covariance transforms + */ +using Matrix6d = Eigen::Matrix; +using Matrix9d = Eigen::Matrix; + +/** + * @brief Transform representation of orientation from 1 frame to another. + * (e.g. transfrom orientation from representing from base_link -> NED to + * representing base_link -> ENU) + */ +Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform); + +/** + * @brief Transform data expressed in one frame to another. + */ +Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q); + +/** + * @brief Transform 3x3 convariance expressed in one frame to another. + */ +Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q); + +/** + * @brief Transform 6x6 convariance expressed in one frame to another. + */ +Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q); + +/** + * @brief Transform 9x9 convariance expressed in one frame to another. + */ +Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q); + +/** + * @brief Transform data expressed in one frame to another. + */ +Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform); + +/** + * @brief Transform 3d convariance expressed in one frame to another. + */ +Covariance3d transform_static_frame(const Covariance3d &cov, const StaticTF transform); + +/** + * @brief Transform 6d convariance expressed in one frame to another. + */ +Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF transform); + +/** + * @brief Transform 9d convariance expressed in one frame to another + */ +Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform); + +/** + * @brief Transform data expressed in one frame to another frame with additional + * map origin parameter. + */ +Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, + const StaticTF transform); + +/** + * @brief Transform from orientation represented WRT NED frame to orientation + * represented WRT ENU frame + */ +template inline T ned_to_enu_orientation(const T &in) +{ + return transform_orientation(in, StaticTF::NED_TO_ENU); +} + +/** + * @brief Transform from orientation represented WRT ENU frame to orientation + * represented WRT NED frame + */ +template inline T enu_to_ned_orientation(const T &in) +{ + return transform_orientation(in, StaticTF::ENU_TO_NED); +} + +/** + * @brief Transform from orientation represented WRT aircraft body frame to + * orientation represented WRT base_link body frame + */ +template inline T aircraft_to_baselink_orientation(const T &in) +{ + return transform_orientation(in, StaticTF::AIRCRAFT_TO_BASELINK); +} + +/** + * @brief Transform from orientation represented WRT base_link body frame to + * orientation represented WRT aircraft body frame + */ +template inline T baselink_to_aircraft_orientation(const T &in) +{ + return transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT); +} + +/** + * @brief Transform from orientation represented in PX4 format to ROS format + * PX4 format is aircraft to NED + * ROS format is baselink to ENU + * + * Two steps conversion: + * 1. aircraft to NED is converted to aircraft to ENU (NED_to_ENU conversion) + * 2. aircraft to ENU is converted to baselink to ENU (baselink_to_aircraft conversion) + */ +template inline T px4_to_ros_orientation(const T &in) +{ + return baselink_to_aircraft_orientation(ned_to_enu_orientation(in)); +} + +/** + * @brief Transform from orientation represented in ROS format to PX4 format + * PX4 format is aircraft to NED + * ROS format is baselink to ENU + * + * Two steps conversion: + * 1. baselink to ENU is converted to baselink to NED (ENU_to_NED conversion) + * 2. baselink to NED is converted to aircraft to NED (aircraft_to_baselink conversion) + */ +template inline T ros_to_px4_orientation(const T &in) +{ + return aircraft_to_baselink_orientation(enu_to_ned_orientation(in)); +} + +/** + * @brief Transform data expressed in NED to ENU local frame. + */ +template inline T ned_to_enu_local_frame(const T &in) +{ + return transform_static_frame(in, StaticTF::NED_TO_ENU); +} + +/** + * @brief Transform data expressed in ENU to NED frame. + */ +template inline T enu_to_ned_local_frame(const T &in) +{ + return transform_static_frame(in, StaticTF::ENU_TO_NED); +} + +/** + * @brief Transform data expressed in aircraft body frame to base_link body frame. + */ +template inline T aircraft_to_baselink_body_frame(const T &in) +{ + return transform_static_frame(in, StaticTF::AIRCRAFT_TO_BASELINK); +} + +/** + * @brief Transform data expressed in base_link body frame to aircraft body frame. + */ +template inline T baselink_to_aircraft_body_frame(const T &in) +{ + return transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT); +} + +/** + * @brief Transform data expressed in ECEF frame to ENU frame. + * + * @param in local ECEF coordinates [m] + * @param map_origin geodetic origin [lla] + * @returns local ENU coordinates [m]. + */ +template inline T ecef_to_enu_local_frame(const T &in, const T &map_origin) +{ + return transform_static_frame(in, map_origin, StaticTF::ECEF_TO_ENU); +} + +/** + * @brief Transform data expressed in ENU frame to ECEF frame. + * + * @param in local ENU coordinates [m] + * @param map_origin geodetic origin [lla] + * @returns local ECEF coordinates [m]. + */ +template inline T enu_to_ecef_local_frame(const T &in, const T &map_origin) +{ + return transform_static_frame(in, map_origin, StaticTF::ENU_TO_ECEF); +} + +/** + * @brief Transform data expressed in aircraft frame to NED frame. + * Assumes quaternion represents rotation from aircraft frame to NED frame. + */ +template inline T aircraft_to_ned_frame(const T &in, const Eigen::Quaterniond &q) +{ + return transform_frame(in, q); +} + +/** + * @brief Transform data expressed in NED to aircraft frame. + * Assumes quaternion represents rotation from NED to aircraft frame. + */ +template inline T ned_to_aircraft_frame(const T &in, const Eigen::Quaterniond &q) +{ + return transform_frame(in, q); +} + +/** + * @brief Transform data expressed in aircraft frame to ENU frame. + * Assumes quaternion represents rotation from aircraft frame to ENU frame. + */ +template inline T aircraft_to_enu_frame(const T &in, const Eigen::Quaterniond &q) +{ + return transform_frame(in, q); +} + +/** + * @brief Transform data expressed in ENU to aircraft frame. + * Assumes quaternion represents rotation from ENU to aircraft frame. + */ +template inline T enu_to_aircraft_frame(const T &in, const Eigen::Quaterniond &q) +{ + return transform_frame(in, q); +} + +/** + * @brief Transform data expressed in baselink to ENU frame. + * Assumes quaternion represents rotation from basel_link to ENU frame. + */ +template inline T baselink_to_enu_frame(const T &in, const Eigen::Quaterniond &q) +{ + return transform_frame(in, q); +} + +/** + * @brief Transform data expressed in ENU to base_link frame. + * Assumes quaternion represents rotation from ENU to base_link frame. + */ +template inline T enu_to_baselink_frame(const T &in, const Eigen::Quaterniond &q) +{ + return transform_frame(in, q); +} + +} // namespace frame_transforms +} // namespace px4_ros_com + +#endif // FRAME_TRANSFORMS_H diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/launch/offboard_control_launch.yaml b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/launch/offboard_control_launch.yaml new file mode 100644 index 00000000..3c568582 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/launch/offboard_control_launch.yaml @@ -0,0 +1,7 @@ +launch: + +- node: + pkg: "px4_ros_com" + exec: "offboard_control" + name: "offboard_control" + output: "screen" \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/launch/sensor_combined_listener.launch.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/launch/sensor_combined_listener.launch.py new file mode 100644 index 00000000..16cfaf4e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/launch/sensor_combined_listener.launch.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python + +################################################################################ +# +# Copyright (c) 2018-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 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. +""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess + +def generate_launch_description(): + + micro_ros_agent = ExecuteProcess( + cmd=[[ + 'micro-ros-agent udp4 --port 8888 -v ' + ]], + shell=True + ) + + sensor_combined_listener_node = Node( + package='px4_ros_com', + executable='sensor_combined_listener', + output='screen', + shell=True, + ) + + return LaunchDescription([ + #micro_ros_agent, + sensor_combined_listener_node + ]) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/package.xml new file mode 100644 index 00000000..529d1ef2 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/package.xml @@ -0,0 +1,43 @@ + + + + px4_ros_com + 0.1.0 + Package to interface ROS 2 with PX4 + + Beniamino Pozzan + Beniamino Pozzan + Nuno Marques + + BSD 3-Clause + + ament_cmake + eigen3_cmake_module + + eigen + ros_environment + + builtin_interfaces + rclcpp + + px4_msgs + geometry_msgs + sensor_msgs + + launch + launch_testing + launch_testing_ros + + rosidl_default_runtime + ros2launch + + eigen3_cmake_module + eigen + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/px4_ros_com/__init__.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py rename to ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/px4_ros_com/__init__.py diff --git a/ros_ws/src/robot/autonomy/planning/global/.gitkeep b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/px4_ros_com/module_to_import.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/.gitkeep rename to ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/px4_ros_com/module_to_import.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/__init__.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py rename to ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/__init__.py diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/build_all.bash b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/build_all.bash new file mode 100755 index 00000000..8e74e6d6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/build_all.bash @@ -0,0 +1,165 @@ +#!/bin/bash +set -e + +# parse help argument +if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then + echo -e "Usage: build_all.bash [option...] \t This script builds px4_ros_com workspaces for ROS 2 and ROS (1)" >&2 + echo + echo -e "\t--ros2_distro \t\t Set ROS 2 distro name (dashing|eloquent|foxy|galactic|humble|rolling). If not set, the script will set the ROS_DISTRO env variable based on the Ubuntu codename" + echo -e "\t--ros2_path \t\t Set ROS 2 environment setup.bash location. Useful for source installs. If not set, the script sources the environment in /opt/ros/$ROS_DISTRO/" + echo -e "\t--verbose \t\t Add more verbosity to the console output" + echo + exit 0 +fi + +SCRIPT_DIR=$0 +if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then + SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0")) +fi + +# parse the arguments +while [ $# -gt 0 ]; do + if [[ $1 == *"--"* ]]; then + v="${1/--/}" + if [ ! -z $2 ]; then + declare $v="$2" + else + declare $v=1 + fi + fi + shift +done + +unset ROS_DISTRO +# One can pass the ROS_DISTRO's using the '--ros2_distro' args +if [ -z $ros2_distro]; then + # set the ROS_DISTRO variables automatically based on the Ubuntu codename and + # ROS install directory + # The distros are order by priority (according to being LTS vs non-LTS) + case "$(lsb_release -s -c)" in "bionic") + + if [ -d "/opt/ros/foxy" ]; then + export ROS2_DISTRO="foxy" + elif [ -d "/opt/ros/galactic" ]; then + export ROS2_DISTRO="galactic" + elif [ -d "/opt/ros/humble" ]; then + export ROS2_DISTRO="humble" + + else + if [ -z $ros2_path ]; then + echo "- No ROS 2 distro installed or not installed in the default directory." + echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros2_path' arg! (ex: ~/ros_src/eloquent/install). Otherwise, please install ROS 2 Dashing following https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Binary/" + exit 1 + else + # source the ROS2 environment (from arg) + source $ros2_path + export ROS2_DISTRO="$(rosversion -d)" + fi + fi + ;; + "focal") + + if [ -d "/opt/ros/foxy" ]; then + export ROS2_DISTRO="foxy" + elif [ -d "/opt/ros/galactic" ]; then + ROS_DISTRO="galactic" + elif [ -d "/opt/ros/rolling" ]; then + ROS_DISTRO="rolling" + else + if [ -z $ros2_path ]; then + echo "- No ROS 2 distro installed or not installed in the default directory." + echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros2_path' arg! (ex: ~/ros_src/foxy/install). Otherwise, please install ROS 2 Foxy following https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Binary/" + exit 1 + else + # source the ROS2 environment (from arg) + source $ros2_path + export ROS2_DISTRO="$(rosversion -d)" + fi + fi + ;; + "jammy") + + if [ -d "/opt/ros/humble" ]; then + ROS2_DISTRO="humble" + elif [ -d "/opt/ros/rolling" ]; then + ROS2_DISTRO="rolling" + else + if [ -z $ros2_path ]; then + echo "- No ROS 2 distro installed or not installed in the default directory." + echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros2_path' arg! (ex: ~/ros_src/humble/install). Otherwise, please install ROS 2 Humble following https://docs.ros.org/en/humble/Installation.html" + exit 1 + else + # source the ROS2 environment (from arg) + source $ros2_path + export ROS2_DISTRO="$(rosversion -d)" + fi + fi + ;; + *) + echo "Unsupported version of Ubuntu detected." + exit 1 + ;; + esac +else + + if [ -z $ros2_path ]; then + echo "- Warning: You set a ROS 2 manually to be used." + echo " This assumes you want to use another ROS 2 version installed on your system. Please set the install location with '--ros2_path' arg! (ex: --ros_path ~/ros_src/eloquent/install/setup.bash)" + exit 1 + else + export ROS2_DISTRO="$ros2_distro" + fi +fi + +# setup the required path variables +export ROS2_REPO_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd) +export ROS2_WS_SRC_DIR=$(cd "$(dirname "$ROS2_REPO_DIR")" && pwd) +export ROS2_WS_DIR=$(cd "$(dirname "$ROS2_WS_SRC_DIR")" && pwd) + +# Check if gnome-terminal exists +if [ -x "$(command -v gnome-terminal)" ]; then + SHELL_TERM="gnome-terminal --tab -- /bin/bash -c" + echo $SHELL_TERM +# Check if xterm exists +elif [ -x "$(command -v xterm)" ]; then + SHELL_TERM="xterm -e" + echo $SHELL_TERM +fi + +printf "\n************* Building ROS2 workspace *************\n\n" + +# source the ROS2 environment +if [ -z $ros2_path ]; then + source /opt/ros/$ROS2_DISTRO/setup.bash +else + source $ros2_path +fi + +# build px4_ros_com package +[ ! -v $verbose ] && colcon_output=$(echo "--event-handlers console_direct+") +cd $ROS2_WS_DIR && colcon build $colcon_output + +$SHELL_TERM \ + ''' + # source the ROS2 workspace + unset ROS_DISTRO && source $ROS2_WS_DIR/install/setup.bash + + # source the ROS2 workspace environment so to have it ready to use + unset ROS_DISTRO && source $ROS2_WS_DIR/install/setup.bash + + exec /bin/bash + ''' & + +# source the ROS2 workspace environment so to have it ready to use +source $ROS2_WS_DIR/install/setup.bash + +printf "\nROS2 workspace ready...\n\n" + +$SHELL_TERM \ + ''' + # source the ROS2 workspace environment so to have it ready to use + source $ROS2_WS_DIR/install/setup.bash + + printf "To start the XRCE-DDS agent, use \"micro-ros-agent udp4 --port 2019\"\n\n" + exec /bin/bash + ''' & diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/build_ros2_workspace.bash b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/build_ros2_workspace.bash new file mode 100755 index 00000000..62f44b96 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/build_ros2_workspace.bash @@ -0,0 +1,120 @@ +#!/bin/bash +set -e + +# parse help argument +if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then + echo -e "Usage: build_ros2_workspace.bash [option...] \t This script builds px4_ros_com workspace for ROS 2" >&2 + echo + echo -e "\t--ros_distro \t\t Set ROS 2 distro name (dashing|eloquent|foxy|galactic|humble|rolling). If not set, the script will set the ROS_DISTRO env variable based on the Ubuntu codename" + echo -e "\t--ros_path \t\t Set ROS 2 environment setup.bash location. Useful for source installs. If not set, the script sources the environment in /opt/ros/$ROS_DISTRO" + echo -e "\t--verbose \t\t Add more verbosity to the console output" + echo + exit 0 +fi + +SCRIPT_DIR=$0 +if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then + SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0")) +fi + +# parse the arguments +while [ $# -gt 0 ]; do + if [[ $1 == *"--"* ]]; then + v="${1/--/}" + if [ ! -z $2 ]; then + declare $v="$2" + else + declare $v=1 + fi + fi + shift +done + +# One can pass the ROS_DISTRO using the '--ros_distro' arg +unset ROS_DISTRO +if [ -z $ros_distro ]; then + # set the ROS_DISTRO variables automatically based on the Ubuntu codename and + # ROS 2 install directory + # The distros are order by priority (according to being LTS vs non-LTS) + case "$(lsb_release -s -c)" in + "bionic") + if [ -d "/opt/ros/dashing" ]; then + ROS_DISTRO="dashing" + elif [ -d "/opt/ros/eloquent" ]; then + ROS_DISTRO="eloquent" + else + if [ -z $ros_path ]; then + echo "- No ROS 2 distro installed or not installed in the default directory." + echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros_path' arg! (ex: ~/ros_src/eloquent/install). Otherwise, please install ROS 2 Dashing following https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Binary/" + exit 1 + else + # source the ROS2 environment (from arg) + source $ros_path + fi + fi + ;; + "focal") + if [ -d "/opt/ros/foxy" ]; then + ROS_DISTRO="foxy" + elif [ -d "/opt/ros/galactic" ]; then + ROS_DISTRO="galactic" + elif [ -d "/opt/ros/rolling" ]; then + ROS_DISTRO="rolling" + else + if [ -z $ros_path ]; then + echo "- No ROS 2 distro installed or not installed in the default directory." + echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros_path' arg! (ex: ~/ros_src/foxy/install). Otherwise, please install ROS 2 Foxy following https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Binary/" + exit 1 + else + # source the ROS2 environment (from arg) + source $ros_path + fi + fi + ;; + "jammy") + if [ -d "/opt/ros/humble" ]; then + ROS_DISTRO="humble" + elif [ -d "/opt/ros/rolling" ]; then + ROS_DISTRO="rolling" + else + if [ -z $ros_path ]; then + echo "- No ROS 2 distro installed or not installed in the default directory." + echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros_path' arg! (ex: ~/ros_src/foxy/install). Otherwise, please install ROS 2 Humble following https://docs.ros.org/en/humble/Installation.html" + exit 1 + else + # source the ROS2 environment (from arg) + source $ros_path + fi + fi + ;; + *) + echo "Unsupported version of Ubuntu detected." + exit 1 + ;; + esac + # source the ROS2 environment + source /opt/ros/$ROS_DISTRO/setup.bash +else + if [ -z $ros_path ]; then + echo "- Warning: You set a ROS 2 manually to be used." + echo " This assumes you want to use another ROS 2 version installed on your system. Please set the install location with '--ros_path' arg! (ex: --ros_path ~/ros_src/eloquent/install/setup.bash)" + exit 1 + else + # source the ROS 2 environment (from arg) + source $ros_path + fi +fi + +# setup the required path variables +ROS_REPO_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd) +ROS_WS_SRC_DIR=$(cd "$(dirname "$ROS_REPO_DIR")" && pwd) +ROS_WS_DIR=$(cd "$(dirname "$ROS_WS_SRC_DIR")" && pwd) + +# build px4_ros_com package +[ ! -v $verbose ] && colcon_output=$(echo "--event-handlers console_direct+") +cd $ROS_WS_DIR && colcon build --cmake-args -DCMAKE_BUILD_TYPE=RELWITHDEBINFO --symlink-install $colcon_output + +# source the ROS2 workspace environment so to have it ready to use +source $ROS_WS_DIR/install/setup.bash + +printf "\nROS2 workspace ready...\n\n" diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/setup_system.bash b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/setup_system.bash new file mode 100755 index 00000000..da772d04 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/scripts/setup_system.bash @@ -0,0 +1,98 @@ +#!/bin/bash +set -e + +# parse help argument +if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then + echo -e "Usage: setup_system.bash [option...] \t This script setups the system with the required dependencies." >&2 + echo + echo -e "\t--ros2_distro \t Set ROS2 distro name (dashing|eloquent|foxy|galactic|rolling). If not set, the script will set the ROS_DISTRO env variable based on the Ubuntu codename" + echo -e "\t--clean \t If set, issues 'apt-get autoremove', 'apt-get autoclean' and deletes temp files." + echo + exit 0 +fi + +# parse the arguments +while [ $# -gt 0 ]; do + if [[ $1 == *"--"* ]]; then + v="${1/--/}" + if [ ! -z $2 ]; then + declare $v="$2" + else + declare $v=1 + fi + fi + shift +done + +# One can pass the ROS_DISTRO's using the '--ros2_distro' args +unset ROS_DISTRO +if [ -z $ros2_distro ]; then + # set the ROS_DISTRO variables automatically based on the Ubuntu codename. + # the following distros are the recommended as they are LTS + case "$(lsb_release -s -c)" in + "bionic") + ROS2_DISTRO="dashing" + ;; + "focal") + ROS2_DISTRO="foxy" + ;; + *) + echo "Unsupported version of Ubuntu detected." + exit 1 + ;; + esac +else + ROS2_DISTRO="$ros2_distro" +fi + +# Install ROS2 dependencies +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \ + sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + +echo "Updating package lists ..." +sudo apt-get -qq update +sudo apt-get -qq dist-upgrade +echo "Installing ROS2 $ROS2_DISTRO and some dependencies..." + +sudo apt-get install -y \ + dirmngr \ + gnupg2 \ + python3-colcon-common-extensions \ + python3-dev \ + ros-$ROS2_DISTRO-desktop \ + ros-$ROS2_DISTRO-eigen3-cmake-module \ + ros-$ROS2_DISTRO-launch-testing-ament-cmake \ + ros-$ROS2_DISTRO-rosidl-generator-dds-idl + +# Install Python3 packages needed for testing +curl https://bootstrap.pypa.io/get-pip.py | python3 && + python3 -m pip install --upgrade pip \ + setuptools \ + argcomplete \ + flake8 \ + flake8-blind-except \ + flake8-builtins \ + flake8-class-newline \ + flake8-comprehensions \ + flake8-deprecated \ + flake8-docstrings \ + flake8-import-order \ + flake8-quotes \ + pytest \ + pytest-cov \ + pytest-repeat \ + pytest-runner \ + pytest-rerunfailures + +# Install Python3 packages for uORB topic generation +python3 -c "import em" || python3 -m pip install --user empy +python3 -c "import genmsg.template_tools" || python3 -m pip install --user pyros-genmsg +python3 -c "import packaging" || python3 -m pip install --user packaging + +# Clean residuals +if [ -o clean ]; then + sudo apt-get -y autoremove && + sudo apt-get clean autoclean && + sudo rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/* +fi diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/advertisers/debug_vect_advertiser.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/advertisers/debug_vect_advertiser.cpp new file mode 100644 index 00000000..dc8f3afb --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/advertisers/debug_vect_advertiser.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright 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. + * + ****************************************************************************/ + +/** + * @brief Debug Vect uORB topic adverstiser example + * @file debug_vect_advertiser.cpp + * @addtogroup examples + * @author Nuno Marques + */ + +#include +#include +#include + +using namespace std::chrono_literals; + +class DebugVectAdvertiser : public rclcpp::Node +{ +public: + DebugVectAdvertiser() : Node("debug_vect_advertiser") + { + + publisher_ = this->create_publisher("/fmu/in/debug_vect", 10); + + auto timer_callback = [this]()->void { + auto debug_vect = px4_msgs::msg::DebugVect(); + debug_vect.timestamp = std::chrono::time_point_cast(std::chrono::steady_clock::now()).time_since_epoch().count(); + std::string name = "test"; + std::copy(name.begin(), name.end(), debug_vect.name.begin()); + debug_vect.x = 1.0; + debug_vect.y = 2.0; + debug_vect.z = 3.0; + RCLCPP_INFO(this->get_logger(), "\033[97m Publishing debug_vect: time: %lu x: %f y: %f z: %f \033[0m", + debug_vect.timestamp, debug_vect.x, debug_vect.y, debug_vect.z); + + this->publisher_->publish(debug_vect); + }; + + timer_ = this->create_wall_timer(500ms, timer_callback); + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +int main(int argc, char *argv[]) +{ + std::cout << "Starting debug_vect advertiser node..." << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + return 0; +} diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/listeners/sensor_combined_listener.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/listeners/sensor_combined_listener.cpp new file mode 100644 index 00000000..86bc6f6f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/listeners/sensor_combined_listener.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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.cpp + * @addtogroup examples + * @author Nuno Marques + * @author Vicente Monge + */ + +#include +#include + +/** + * @brief Sensor Combined uORB topic data callback + */ +class SensorCombinedListener : public rclcpp::Node +{ +public: + explicit SensorCombinedListener() : Node("sensor_combined_listener") + { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); + + subscription_ = this->create_subscription("/fmu/out/sensor_combined", qos, + [this](const px4_msgs::msg::SensorCombined::UniquePtr msg) { + std::cout << "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n"; + std::cout << "RECEIVED SENSOR COMBINED DATA" << std::endl; + std::cout << "=============================" << std::endl; + std::cout << "ts: " << msg->timestamp << std::endl; + std::cout << "gyro_rad[0]: " << msg->gyro_rad[0] << std::endl; + std::cout << "gyro_rad[1]: " << msg->gyro_rad[1] << std::endl; + std::cout << "gyro_rad[2]: " << msg->gyro_rad[2] << std::endl; + std::cout << "gyro_integral_dt: " << msg->gyro_integral_dt << std::endl; + std::cout << "accelerometer_timestamp_relative: " << msg->accelerometer_timestamp_relative << std::endl; + std::cout << "accelerometer_m_s2[0]: " << msg->accelerometer_m_s2[0] << std::endl; + std::cout << "accelerometer_m_s2[1]: " << msg->accelerometer_m_s2[1] << std::endl; + std::cout << "accelerometer_m_s2[2]: " << msg->accelerometer_m_s2[2] << std::endl; + std::cout << "accelerometer_integral_dt: " << msg->accelerometer_integral_dt << std::endl; + }); + } + +private: + rclcpp::Subscription::SharedPtr subscription_; + +}; + +int main(int argc, char *argv[]) +{ + std::cout << "Starting sensor_combined listener node..." << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + return 0; +} diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/listeners/vehicle_gps_position_listener.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/listeners/vehicle_gps_position_listener.cpp new file mode 100644 index 00000000..98971b80 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/listeners/vehicle_gps_position_listener.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright 2019 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 Vehicle GPS position uORB topic listener example + * @file vehicle_global_position_listener.cpp + * @addtogroup examples + * @author Nuno Marques + */ + +#include +#include + +/** + * @brief Vehicle GPS position uORB topic data callback + */ +class VehicleGpsPositionListener : public rclcpp::Node +{ +public: + explicit VehicleGpsPositionListener() : Node("vehicle_global_position_listener") + { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); + + subscription_ = this->create_subscription("/fmu/out/vehicle_gps_position", qos, + [this](const px4_msgs::msg::SensorGps::UniquePtr msg) { + std::cout << "\n\n\n\n\n\n\n\n\n\n"; + std::cout << "RECEIVED VEHICLE GPS POSITION DATA" << std::endl; + std::cout << "==================================" << std::endl; + std::cout << "ts: " << msg->timestamp << std::endl; + std::cout << "lat: " << msg->latitude_deg << std::endl; + std::cout << "lon: " << msg->longitude_deg << std::endl; + std::cout << "alt: " << msg->altitude_msl_m << std::endl; + std::cout << "alt_ellipsoid: " << msg->altitude_ellipsoid_m << std::endl; + std::cout << "s_variance_m_s: " << msg->s_variance_m_s << std::endl; + std::cout << "c_variance_rad: " << msg->c_variance_rad << std::endl; + std::cout << "fix_type: " << msg->fix_type << std::endl; + std::cout << "eph: " << msg->eph << std::endl; + std::cout << "epv: " << msg->epv << std::endl; + std::cout << "hdop: " << msg->hdop << std::endl; + std::cout << "vdop: " << msg->vdop << std::endl; + std::cout << "noise_per_ms: " << msg->noise_per_ms << std::endl; + std::cout << "vel_m_s: " << msg->vel_m_s << std::endl; + std::cout << "vel_n_m_s: " << msg->vel_n_m_s << std::endl; + std::cout << "vel_e_m_s: " << msg->vel_e_m_s << std::endl; + std::cout << "vel_d_m_s: " << msg->vel_d_m_s << std::endl; + std::cout << "cog_rad: " << msg->cog_rad << std::endl; + std::cout << "vel_ned_valid: " << msg->vel_ned_valid << std::endl; + std::cout << "timestamp_time_relative: " << msg->timestamp_time_relative << std::endl; + std::cout << "time_utc_usec: " << msg->time_utc_usec << std::endl; + std::cout << "satellites_used: " << msg->satellites_used << std::endl; + std::cout << "heading: " << msg->heading << std::endl; + std::cout << "heading_offset: " << msg->heading_offset << std::endl; + }); + } + +private: + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char *argv[]) +{ + std::cout << "Starting vehicle_global_position listener node..." << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + return 0; +} diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard/offboard_control.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard/offboard_control.cpp new file mode 100644 index 00000000..bab8ee4f --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard/offboard_control.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * 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.cpp + * @addtogroup examples + * @author Mickey Cowden + * @author Nuno Marques + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std::chrono; +using namespace std::chrono_literals; +using namespace px4_msgs::msg; + +class OffboardControl : public rclcpp::Node +{ +public: + OffboardControl() : Node("offboard_control") + { + + offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + trajectory_setpoint_publisher_ = this->create_publisher("/fmu/in/trajectory_setpoint", 10); + vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); + + offboard_setpoint_counter_ = 0; + + auto timer_callback = [this]() -> void { + + if (offboard_setpoint_counter_ == 10) { + // Change to Offboard mode after 10 setpoints + this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); + + // Arm the vehicle + this->arm(); + } + + // offboard_control_mode needs to be paired with trajectory_setpoint + publish_offboard_control_mode(); + publish_trajectory_setpoint(); + + // stop the counter after reaching 11 + if (offboard_setpoint_counter_ < 11) { + offboard_setpoint_counter_++; + } + }; + timer_ = this->create_wall_timer(100ms, timer_callback); + } + + void arm(); + void disarm(); + +private: + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher_; + rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + + std::atomic timestamp_; //!< common synced timestamped + + uint64_t offboard_setpoint_counter_; //!< counter for the number of setpoints sent + + void publish_offboard_control_mode(); + void publish_trajectory_setpoint(); + void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0); +}; + +/** + * @brief Send a command to Arm the vehicle + */ +void OffboardControl::arm() +{ + publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); + + RCLCPP_INFO(this->get_logger(), "Arm command send"); +} + +/** + * @brief Send a command to Disarm the vehicle + */ +void OffboardControl::disarm() +{ + publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); + + RCLCPP_INFO(this->get_logger(), "Disarm command send"); +} + +/** + * @brief Publish the offboard control mode. + * For this example, only position and altitude controls are active. + */ +void OffboardControl::publish_offboard_control_mode() +{ + OffboardControlMode msg{}; + msg.position = true; + msg.velocity = false; + msg.acceleration = false; + msg.attitude = false; + msg.body_rate = false; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + offboard_control_mode_publisher_->publish(msg); +} + +/** + * @brief Publish a trajectory setpoint + * For this example, it sends a trajectory setpoint to make the + * vehicle hover at 5 meters with a yaw angle of 180 degrees. + */ +void OffboardControl::publish_trajectory_setpoint() +{ + TrajectorySetpoint msg{}; + msg.position = {0.0, 0.0, -5.0}; + msg.yaw = -3.14; // [-PI:PI] + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + 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 + */ +void OffboardControl::publish_vehicle_command(uint16_t command, float param1, float param2) +{ + VehicleCommand msg{}; + 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; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + vehicle_command_publisher_->publish(msg); +} + +int main(int argc, char *argv[]) +{ + std::cout << "Starting offboard control node..." << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + return 0; +} diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard/offboard_control_srv.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard/offboard_control_srv.cpp new file mode 100644 index 00000000..a7f9c405 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard/offboard_control_srv.cpp @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright 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 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.cpp + * @addtogroup examples * + * @author Beniamino Pozzan + * @author Mickey Cowden + * @author Nuno Marques + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std::chrono; +using namespace std::chrono_literals; +using namespace px4_msgs::msg; + +class OffboardControl : public rclcpp::Node +{ +public: + OffboardControl(std::string px4_namespace) : + Node("offboard_control_srv"), + state_{State::init}, + service_result_{0}, + service_done_{false}, + offboard_control_mode_publisher_{this->create_publisher(px4_namespace+"in/offboard_control_mode", 10)}, + trajectory_setpoint_publisher_{this->create_publisher(px4_namespace+"in/trajectory_setpoint", 10)}, + vehicle_command_client_{this->create_client(px4_namespace+"vehicle_command")} + { + RCLCPP_INFO(this->get_logger(), "Starting Offboard Control example with PX4 services"); + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for " << px4_namespace << "vehicle_command service"); + while (!vehicle_command_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + + timer_ = this->create_wall_timer(100ms, std::bind(&OffboardControl::timer_callback, this)); + } + + void switch_to_offboard_mode(); + void arm(); + void disarm(); + +private: + enum class State{ + init, + offboard_requested, + wait_for_stable_offboard_mode, + arm_requested, + armed + } state_; + uint8_t service_result_; + bool service_done_; + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher_; + rclcpp::Client::SharedPtr vehicle_command_client_; + + + void publish_offboard_control_mode(); + void publish_trajectory_setpoint(); + void request_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0); + void response_callback(rclcpp::Client::SharedFuture future); + void timer_callback(void); +}; + +/** + * @brief Send a command to switch to offboard mode + */ +void OffboardControl::switch_to_offboard_mode(){ + RCLCPP_INFO(this->get_logger(), "requesting switch to Offboard mode"); + request_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); +} + +/** + * @brief Send a command to Arm the vehicle + */ +void OffboardControl::arm() +{ + RCLCPP_INFO(this->get_logger(), "requesting arm"); + request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); +} + +/** + * @brief Send a command to Disarm the vehicle + */ +void OffboardControl::disarm() +{ + RCLCPP_INFO(this->get_logger(), "requesting disarm"); + request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); +} + +/** + * @brief Publish the offboard control mode. + * For this example, only position and altitude controls are active. + */ +void OffboardControl::publish_offboard_control_mode() +{ + OffboardControlMode msg{}; + msg.position = true; + msg.velocity = false; + msg.acceleration = false; + msg.attitude = false; + msg.body_rate = false; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + offboard_control_mode_publisher_->publish(msg); +} + +/** + * @brief Publish a trajectory setpoint + * For this example, it sends a trajectory setpoint to make the + * vehicle hover at 5 meters with a yaw angle of 180 degrees. + */ +void OffboardControl::publish_trajectory_setpoint() +{ + TrajectorySetpoint msg{}; + msg.position = {0.0, 0.0, -5.0}; + msg.yaw = -3.14; // [-PI:PI] + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + 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 + */ +void OffboardControl::request_vehicle_command(uint16_t command, float param1, float param2) +{ + auto request = std::make_shared(); + + VehicleCommand msg{}; + 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; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + request->request = msg; + + service_done_ = false; + auto result = vehicle_command_client_->async_send_request(request, std::bind(&OffboardControl::response_callback, this, + std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Command send"); +} + +void OffboardControl::timer_callback(void){ + static uint8_t num_of_steps = 0; + + // offboard_control_mode needs to be paired with trajectory_setpoint + publish_offboard_control_mode(); + publish_trajectory_setpoint(); + + switch (state_) + { + case State::init : + switch_to_offboard_mode(); + state_ = State::offboard_requested; + break; + case State::offboard_requested : + if(service_done_){ + if (service_result_==0){ + RCLCPP_INFO(this->get_logger(), "Entered offboard mode"); + state_ = State::wait_for_stable_offboard_mode; + } + else{ + RCLCPP_ERROR(this->get_logger(), "Failed to enter offboard mode, exiting"); + rclcpp::shutdown(); + } + } + break; + case State::wait_for_stable_offboard_mode : + if (++num_of_steps>10){ + arm(); + state_ = State::arm_requested; + } + break; + case State::arm_requested : + if(service_done_){ + if (service_result_==0){ + RCLCPP_INFO(this->get_logger(), "vehicle is armed"); + state_ = State::armed; + } + else{ + RCLCPP_ERROR(this->get_logger(), "Failed to arm, exiting"); + rclcpp::shutdown(); + } + } + break; + default: + break; + } +} + +void OffboardControl::response_callback( + rclcpp::Client::SharedFuture future) { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) { + auto reply = future.get()->reply; + service_result_ = reply.result; + switch (service_result_) + { + case reply.VEHICLE_CMD_RESULT_ACCEPTED: + RCLCPP_INFO(this->get_logger(), "command accepted"); + break; + case reply.VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + RCLCPP_WARN(this->get_logger(), "command temporarily rejected"); + break; + case reply.VEHICLE_CMD_RESULT_DENIED: + RCLCPP_WARN(this->get_logger(), "command denied"); + break; + case reply.VEHICLE_CMD_RESULT_UNSUPPORTED: + RCLCPP_WARN(this->get_logger(), "command unsupported"); + break; + case reply.VEHICLE_CMD_RESULT_FAILED: + RCLCPP_WARN(this->get_logger(), "command failed"); + break; + case reply.VEHICLE_CMD_RESULT_IN_PROGRESS: + RCLCPP_WARN(this->get_logger(), "command in progress"); + break; + case reply.VEHICLE_CMD_RESULT_CANCELLED: + RCLCPP_WARN(this->get_logger(), "command cancelled"); + break; + default: + RCLCPP_WARN(this->get_logger(), "command reply unknown"); + break; + } + service_done_ = true; + } else { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + +int main(int argc, char *argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("/fmu/")); + + rclcpp::shutdown(); + return 0; +} diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard_py/offboard_control.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard_py/offboard_control.py new file mode 100755 index 00000000..7d59f536 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/src/examples/offboard_py/offboard_control.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleLocalPosition, VehicleStatus + + +class OffboardControl(Node): + """Node for controlling a vehicle in offboard mode.""" + + def __init__(self) -> None: + super().__init__('offboard_control_takeoff_and_land') + + # Configure QoS profile for publishing and subscribing + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1 + ) + + # Create publishers + self.offboard_control_mode_publisher = self.create_publisher( + OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile) + self.trajectory_setpoint_publisher = self.create_publisher( + TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile) + self.vehicle_command_publisher = self.create_publisher( + VehicleCommand, '/fmu/in/vehicle_command', qos_profile) + + # Create subscribers + self.vehicle_local_position_subscriber = self.create_subscription( + VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, qos_profile) + self.vehicle_status_subscriber = self.create_subscription( + VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile) + + # Initialize variables + self.offboard_setpoint_counter = 0 + self.vehicle_local_position = VehicleLocalPosition() + self.vehicle_status = VehicleStatus() + self.takeoff_height = -5.0 + + # Create a timer to publish control commands + self.timer = self.create_timer(0.1, self.timer_callback) + + def vehicle_local_position_callback(self, vehicle_local_position): + """Callback function for vehicle_local_position topic subscriber.""" + self.vehicle_local_position = vehicle_local_position + + def vehicle_status_callback(self, vehicle_status): + """Callback function for vehicle_status topic subscriber.""" + self.vehicle_status = vehicle_status + + def arm(self): + """Send an arm command to the vehicle.""" + self.publish_vehicle_command( + VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0) + self.get_logger().info('Arm command sent') + + def disarm(self): + """Send a disarm command to the vehicle.""" + self.publish_vehicle_command( + VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0) + self.get_logger().info('Disarm command sent') + + def engage_offboard_mode(self): + """Switch to offboard mode.""" + self.publish_vehicle_command( + VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0) + self.get_logger().info("Switching to offboard mode") + + def land(self): + """Switch to land mode.""" + self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND) + self.get_logger().info("Switching to land mode") + + def publish_offboard_control_heartbeat_signal(self): + """Publish the offboard control mode.""" + msg = OffboardControlMode() + msg.position = True + msg.velocity = False + msg.acceleration = False + msg.attitude = False + msg.body_rate = False + msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) + self.offboard_control_mode_publisher.publish(msg) + + def publish_position_setpoint(self, x: float, y: float, z: float): + """Publish the trajectory setpoint.""" + msg = TrajectorySetpoint() + msg.position = [x, y, z] + msg.yaw = 1.57079 # (90 degree) + msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) + self.trajectory_setpoint_publisher.publish(msg) + self.get_logger().info(f"Publishing position setpoints {[x, y, z]}") + + def publish_vehicle_command(self, command, **params) -> None: + """Publish a vehicle command.""" + msg = VehicleCommand() + msg.command = command + msg.param1 = params.get("param1", 0.0) + msg.param2 = params.get("param2", 0.0) + msg.param3 = params.get("param3", 0.0) + msg.param4 = params.get("param4", 0.0) + msg.param5 = params.get("param5", 0.0) + msg.param6 = params.get("param6", 0.0) + msg.param7 = params.get("param7", 0.0) + msg.target_system = 1 + msg.target_component = 1 + msg.source_system = 1 + msg.source_component = 1 + msg.from_external = True + msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) + self.vehicle_command_publisher.publish(msg) + + def timer_callback(self) -> None: + """Callback function for the timer.""" + self.publish_offboard_control_heartbeat_signal() + + if self.offboard_setpoint_counter == 10: + self.engage_offboard_mode() + self.arm() + + if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD: + self.publish_position_setpoint(0.0, 0.0, self.takeoff_height) + + elif self.vehicle_local_position.z <= self.takeoff_height: + self.land() + exit(0) + + if self.offboard_setpoint_counter < 11: + self.offboard_setpoint_counter += 1 + + +def main(args=None) -> None: + print('Starting offboard control node...') + rclpy.init(args=args) + offboard_control = OffboardControl() + rclpy.spin(offboard_control) + offboard_control.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + try: + main() + except Exception as e: + print(e) diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/__init__.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/__init__.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/__init__.py rename to ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/__init__.py diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/pipeline_io_test.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/pipeline_io_test.py new file mode 100644 index 00000000..6eff5837 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/pipeline_io_test.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 + +################################################################################ +# +# Copyright 2019 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. +# +################################################################################ + +# This script launches the microRTPS bridge pipeline to verify if there is +# data being exchanged between the flight controller (in SITL) and companion +# computer side, with its DDS participants + +import argparse +import os +from time import sleep +from sys import exit +from subprocess import call, CalledProcessError, check_output, DEVNULL, Popen, STDOUT + +if __name__ == "__main__": + + default_px4_ros_com_install_dir = os.path.expanduser("~") + "/PX4/px4_ros2_ws/install" + default_px4_dir = os.path.expanduser("~") + "/PX4/Firmware" + default_px4_target = "iris_rtps" + default_test_dir = os.path.dirname(os.path.realpath(__file__)) + default_test_type = "fcu_output" + default_subscriber_topic = "sensor_combined" + default_publisher_topic = "debug_vect" + + parser = argparse.ArgumentParser() + parser.add_argument("-d", "--test-dir", dest='test_dir', type=str, + help="Absolute path to test script", default=default_test_dir) + parser.add_argument("-f", "--px4-firmware-dir", dest='px4_dir', type=str, + help="Absolute path to PX4 Firmware dir, defaults to $HOME/PX4/Firmware", default=default_px4_dir) + parser.add_argument("-b", "--px4-build-target", dest='px4_target', type=str, + help="PX4 SITL target, defaults to iris_rtps", default=default_px4_target) + parser.add_argument("-t", "--test-type", dest='test_type', type=str, + help="Test type [fcu_input, fcu_output], defaults to 'fcu_output'", default=default_test_type) + parser.add_argument("-s", "--subscriber", dest='subscriber_topic', type=str, + help="ROS2 topic to test, defaults to 'sensor_combined'", default=default_subscriber_topic) + parser.add_argument("-p", "--publisher", dest='publisher_topic', type=str, + help="ROS2 publisher data, meaning the data to be received on the flight controller side, defaults to 'debug_vect'", default=default_publisher_topic) + + # Parse arguments + args = parser.parse_args() + if os.path.isabs(args.px4_dir): + px4_dir = args.px4_dir + else: + raise Exception("Please provide PX4 Firmware absolute path") + px4_target = args.px4_target + test_dir = args.test_dir + test_type = args.test_type + listener = args.subscriber_topic + advertiser = args.publisher_topic + + # get ROS distro + ros_distro = check_output("rosversion -d", shell=True) + + # get PX4 Firmware tag + px4_tag = check_output( + "cd " + px4_dir + " && git describe --abbrev=0 && cd " + os.getcwd(), shell=True) + + print( + "\n\033[34m-------------- PX4 XRCE-DDS COMMUNICATION TEST --------------\033[0m") + print("\n-- Test configuration:\n") + print(" > ROS 2 distro: \033[36m" + + str(ros_distro.strip().decode("utf-8")).capitalize() + "\033[0m") + print(" > PX4 Firmware: \033[36m" + px4_tag.decode() + "\033[0m") + print("\033[5m-- Running " + ("Output test" if(test_type == + "fcu_output") else "Input test") + "...\033[0m") + + # launch the micro-ros-agent + print("\n\033[93m-- Starting micro-ros-agent..." + "\033[0m\n") + call("micro-ros-agent udp4 --port 2019 &", shell=True, stderr=STDOUT) + + # waits for the agent to load + sleep(3) + + # launch PX4 SITL daemon, in headless mode + print( + "\n\033[93m-- Starting the PX4 SITL daemon and Gazebo (without GUI)...\033[0m\n") + call("cd " + px4_dir + " && (NO_PXH=1 HEADLESS=1 make px4_sitl_default gazebo_" + + px4_target + " &) && cd " + os.getcwd(), shell=True, stderr=DEVNULL) + + # waits for PX4 daemon and Gazebo to load + sleep(20) + + # setup the PX4 SITL bin dir + px4_bin_dir = os.path.join(px4_dir, "build/px4_sitl_default/bin") + + # launch the specified test + topic = "" + test_format = list() + test_result = -1 + + if(test_type == "fcu_output"): + # Flight controller output tests + if (listener == "sensor_combined"): + node = "sensor_combined_listener" + topic = "sensor_combined" + test_format = ["output", "from"] + test_result = call( + "python3 " + test_dir + "/test_output.py -t " + topic.replace("_", " ").title().replace(" ", ""), stderr=STDOUT, shell=True, + universal_newlines=True) + + elif(test_type == "fcu_input"): + # Flight controller input tests + if (advertiser == "debug_vect"): + package_name = "px4_ros_com" + node = "debug_vect_advertiser" + topic = "debug_vect" + test_format = ["input", "on"] + test_result = call( + "python3 " + test_dir + "/test_input.py -b " + px4_bin_dir + " -p " + package_name + " -n " + node + " -t " + topic, stderr=STDOUT, shell=True, + universal_newlines=True) + elif (advertiser == "onboard_computer_status"): + # specific test for https://github.com/Auterion/system_monitor_ros + package_name = "system_monitor_ros" + node = "system_monitor_node" + topic = "onboard_computer_status" + test_format = ["input", "on"] + test_result = call( + "python3 " + test_dir + "/test_input.py -b " + px4_bin_dir + " -p " + package_name + " -n " + node + " -t " + topic, stderr=STDOUT, shell=True, + universal_newlines=True) + + call("killall gzserver micro-ros-agent px4 ros2", + shell=True, stdout=DEVNULL, stderr=DEVNULL) + + print( + "\033[34m------------------------ TEST RESULTS ------------------------\033[0m") + if (test_result): + print("\033[91m" + ("Output test" if(test_type == "fcu_output") else "Input test") + ": [FAILED]\tFlight controller " + test_format[0] + " test failed! Failed to get data " + test_format[1] + " the '" + + topic + "' uORB topic\033[0m") + print( + "\033[34m" + "--------------------------------------------------------------" + "\033[0m\n") + exit(1) + else: + print("\033[92m" + ("Output test" if(test_type == "fcu_output") else "Input test") + ": [SUCCESS]\tFlight controller " + test_format[0] + " test successfull! Successfully retrieved data " + test_format[1] + " the '" + + topic + "' uORB topic\033[0m") + print( + "\033[34m--------------------------------------------------------------" + "\033[0m\n") + exit(0) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/test_input.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/test_input.py new file mode 100644 index 00000000..db52a3ee --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/test_input.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +################################################################################ +# +# Copyright 2019 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. +# +################################################################################ + +# This script tests the input data on a specific topic using the "listener" +# app in PX4. If no output comes or there's a "never published" output +# out of the app, the test fails. + +import argparse +import os +from sys import exit +from subprocess import call, check_output, DEVNULL +from time import sleep + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-b", "--px4-binary-dir", dest='px4_binary', type=str, + help="Directory where the PX4 SITL daemon binaries are stored", required=True) + parser.add_argument("-n", "--node-name", dest='node_name', type=str, + help="Name of the node publishing the to the topic", required=True) + parser.add_argument("-p", "--package-name", dest='package_name', type=str, + help="ROS 2 package name of the node", required=True) + parser.add_argument("-t", "--topic-name", dest='topic_name', type=str, + help="Topic name to test the output to the autopilot", required=True) + + # Parse arguments + args = parser.parse_args() + + test_cmd = "/bin/bash -c '" + \ + os.path.join(args.px4_binary, + "px4-listener") + " " + args.topic_name + "'" + + print("\n\033[93m-- " + args.topic_name + + " uORB data test launched:\033[0m") + # start the ROS 2 node + call("ros2 run " + args.package_name + " " + args.node_name + " &", shell=True) + + sleep(3) + + # call the 'listener' command for the '' uORB topic + output = check_output(test_cmd, shell=True, universal_newlines=True) + + call("killall " + args.node_name, + shell=True, stdout=DEVNULL, stderr=DEVNULL) + + if output and "never published" not in output: + print( + "\n\033[42m-- Successfully obtained data on '" + args.topic_name + "' uORB topic. microRTPS bridge is up! Output:\033[0m") + print("\033[97m" + output + "\033[0m") + exit(0) + else: + print( + "\n\033[41m-- Something went wrong. Please check if the microRTPS bridge is functioning correctly.\033[0m\n") + exit(1) diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/test_output.py b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/test_output.py new file mode 100755 index 00000000..74aaafe6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/px4_ros_com/test/test_output.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +################################################################################ +# +# Copyright 2019 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. +# +################################################################################ + +# This script tests the output of 'ros2 topic echo /' +# to evaluate if there's data output coming out of it. If the log output file +# is empty, means that there's no output on the topic, so the test fails + +import argparse +from os import remove +from sys import exit +from subprocess import call, TimeoutExpired + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-t", "--topic-name", dest='topic_name', type=str, + help="Topic name to test the output to the autopilot", required=True) + + # Parse arguments + args = parser.parse_args() + + timeout = 3 # seconds + + try: + print( + "\n\033[93m" + "-- " + args.topic_name + "_PubSubTopic output test launched:\033[0m") + call("ros2 topic echo /" + args.topic_name + "_PubSubTopic", timeout=timeout, stdout=open( + "ros2_topic_echo_out", "w"), shell=True) + except TimeoutExpired as e: + output = open("ros2_topic_echo_out", "r").read() + if output: + print( + "\n\033[42m" + "-- Successfully obtained data on " + args.topic_name + "_PubSubTopic topic. microRTPS bridge is up! Output:\033[0m\n\n") + print("\033[97m" + output + "\033[0m") + remove("ros2_topic_echo_out") + exit(0) + else: + print( + "\n\033[41m" + "-- Something went wrong. Please check if the microRTPS bridge is functioning correctly.\033[0m\n") + remove("ros2_topic_echo_out") + exit(1) diff --git a/ros_ws/src/robot/autonomy/controls/robot_interface/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/robot_interface/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/robot_interface/include/robot_interface/robot_interface.hpp b/ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/include/robot_interface/robot_interface.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/robot_interface/include/robot_interface/robot_interface.hpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/include/robot_interface/robot_interface.hpp diff --git a/ros_ws/src/robot/autonomy/controls/robot_interface/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/robot_interface/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/robot_interface/src/robot_interface.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/src/robot_interface.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/robot_interface/src/robot_interface.cpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/src/robot_interface.cpp diff --git a/ros_ws/src/robot/autonomy/controls/robot_interface/src/robot_interface_node.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/src/robot_interface_node.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/robot_interface/src/robot_interface_node.cpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/robot_interface/src/robot_interface_node.cpp diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_controller/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_controller/CMakeLists.txt rename to ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_controller/launch/trajectory_controller.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_controller/launch/trajectory_controller.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.xml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_controller/launch/trajectory_controller_bag.launch b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller_bag.launch similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_controller/launch/trajectory_controller_bag.launch rename to ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller_bag.launch diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_controller/msg/Trajectory.msg b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/msg/Trajectory.msg similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_controller/msg/Trajectory.msg rename to ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/msg/Trajectory.msg diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_controller/package.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_controller/package.xml rename to ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/package.xml diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_controller/src/tracking_point.py b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/tracking_point.py similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_controller/src/tracking_point.py rename to ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/tracking_point.py diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_controller/src/trajectory_controller.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/controls/trajectory_controller/src/trajectory_controller.cpp rename to ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv new file mode 100644 index 00000000..c67d38fb --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv @@ -0,0 +1,9 @@ +int32 PAUSE=0 +int32 ROBOT_POSE=1 +int32 TRACK=2 +int32 SEGMENT=3 +int32 REWIND=4 + +int32 mode +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping b/ros_ws/src/robot/autonomy/4_global/a_world_models/vdb_mapping similarity index 100% rename from ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping rename to ros_ws/src/robot/autonomy/4_global/a_world_models/vdb_mapping diff --git a/ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping_ros2 b/ros_ws/src/robot/autonomy/4_global/a_world_models/vdb_mapping_ros2 similarity index 100% rename from ros_ws/src/robot/autonomy/world_model/mapping/vdb_mapping_ros2 rename to ros_ws/src/robot/autonomy/4_global/a_world_models/vdb_mapping_ros2 diff --git a/ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/CMakeLists.txt rename to ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/README.md b/ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/README.md similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/README.md rename to ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/README.md diff --git a/ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp b/ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp rename to ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp diff --git a/ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/launch/canary_takeoff_landing_planner.launch b/ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/launch/canary_takeoff_landing_planner.launch similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/launch/canary_takeoff_landing_planner.launch rename to ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/launch/canary_takeoff_landing_planner.launch diff --git a/ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/launch/takeoff_landing_planner.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/launch/takeoff_landing_planner.xml rename to ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/package.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/package.xml rename to ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/src/takeoff_landing_planner.cpp b/ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/takeoff_landing_planner/src/takeoff_landing_planner.cpp rename to ros_ws/src/robot/autonomy/4_global/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/CMakeLists.txt b/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/CMakeLists.txt new file mode 100644 index 00000000..a6f2b551 --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(trivial_planners) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(random_walk_planner_node src/random_walk_planner_node.cpp) +target_include_directories(random_walk_planner_node PUBLIC + $ + $) +target_compile_features(random_walk_planner_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS random_walk_planner_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/package.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/package.xml new file mode 100644 index 00000000..41317518 --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/package.xml @@ -0,0 +1,18 @@ + + + + trivial_planners + 0.0.0 + TODO: Package description + andrew + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/src/random_walk_planner_node.cpp b/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/src/random_walk_planner_node.cpp new file mode 100644 index 00000000..620e7e64 --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/src/random_walk_planner_node.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world trivial_planners package\n"); + return 0; +} diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_executive/CMakeLists.txt b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_executive/CMakeLists.txt rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_executive/include/behavior_executive/behavior_executive.hpp b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/include/behavior_executive/behavior_executive.hpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_executive/include/behavior_executive/behavior_executive.hpp rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/include/behavior_executive/behavior_executive.hpp diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_executive/package.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_executive/package.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_executive/src/behavior_executive.cpp b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/src/behavior_executive.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_executive/src/behavior_executive.cpp rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_executive/src/behavior_executive.cpp diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree/.gitignore b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/.gitignore similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree/.gitignore rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/.gitignore diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree/CMakeLists.txt b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree/CMakeLists.txt rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree/include/behavior_tree/behavior_tree.h b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/include/behavior_tree/behavior_tree.h similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree/include/behavior_tree/behavior_tree.h rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/include/behavior_tree/behavior_tree.h diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree/include/behavior_tree/behavior_tree_implementation.h b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/include/behavior_tree/behavior_tree_implementation.h similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree/include/behavior_tree/behavior_tree_implementation.h rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/include/behavior_tree/behavior_tree_implementation.h diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree/package.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree/package.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree/src/behavior_tree.cpp b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/src/behavior_tree.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree/src/behavior_tree.cpp rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/src/behavior_tree.cpp diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree/src/behavior_tree_implementation.cpp b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/src/behavior_tree_implementation.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree/src/behavior_tree_implementation.cpp rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree/src/behavior_tree_implementation.cpp diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/.gitignore b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/.gitignore similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/.gitignore rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/.gitignore diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/CMakeLists.txt b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/CMakeLists.txt rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example2.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example2.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example2.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example2.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example3.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example3.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example3.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example3.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example3_nested_subtree.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example3_nested_subtree.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example3_nested_subtree.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example3_nested_subtree.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example3_subtree.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example3_subtree.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/example3_subtree.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/example3_subtree.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/ugv_tree.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/ugv_tree.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/ugv_tree.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/ugv_tree.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/ugv_tree_basic.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/ugv_tree_basic.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/config/ugv_tree_basic.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/config/ugv_tree_basic.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/launch/behavior_tree_example.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/launch/behavior_tree_example.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/launch/behavior_tree_example.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/launch/behavior_tree_example.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/package.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/package.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/rviz/behavior_tree.perspective b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/rviz/behavior_tree.perspective similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/rviz/behavior_tree.perspective rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/rviz/behavior_tree.perspective diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/rviz/behavior_tree_example.rviz b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/rviz/behavior_tree_example.rviz similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/rviz/behavior_tree_example.rviz rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/rviz/behavior_tree_example.rviz diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/src/behavior_tree_example.cpp b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/src/behavior_tree_example.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/src/behavior_tree_example.cpp rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/src/behavior_tree_example.cpp diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/src/robot_node.cpp b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/src/robot_node.cpp similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_example/src/robot_node.cpp rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_example/src/robot_node.cpp diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/.gitignore b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/.gitignore similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/.gitignore rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/.gitignore diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/CMakeLists.txt b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/CMakeLists.txt rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/Active.msg b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/Active.msg similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/Active.msg rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/Active.msg diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/BehaviorTreeCommand.msg b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/BehaviorTreeCommand.msg similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/BehaviorTreeCommand.msg rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/BehaviorTreeCommand.msg diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/BehaviorTreeCommands.msg b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/BehaviorTreeCommands.msg similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/BehaviorTreeCommands.msg rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/BehaviorTreeCommands.msg diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/Status.msg b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/Status.msg similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/msg/Status.msg rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/msg/Status.msg diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/package.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/behavior_tree_msgs/package.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/behavior_tree_msgs/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/central/CMakeLists.txt b/ros_ws/src/robot/autonomy/5_behavior_managers/central/CMakeLists.txt similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/CMakeLists.txt rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/CMakeLists.txt diff --git a/ros_ws/src/robot/autonomy/planning/global/central/config/core.perspective b/ros_ws/src/robot/autonomy/5_behavior_managers/central/config/core.perspective similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/config/core.perspective rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/config/core.perspective diff --git a/ros_ws/src/robot/autonomy/planning/global/central/config/core.rviz b/ros_ws/src/robot/autonomy/5_behavior_managers/central/config/core.rviz similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/config/core.rviz rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/config/core.rviz diff --git a/ros_ws/src/robot/autonomy/planning/global/central/config/drone.tree b/ros_ws/src/robot/autonomy/5_behavior_managers/central/config/drone.tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/config/drone.tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/config/drone.tree diff --git a/ros_ws/src/robot/autonomy/planning/global/central/launch/ascent_drone.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_drone.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/launch/ascent_drone.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_drone.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/central/launch/ascent_isaac_one_drone.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_one_drone.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/launch/ascent_isaac_one_drone.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_one_drone.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/central/launch/ascent_isaac_two_drones.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_two_drones.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/launch/ascent_isaac_two_drones.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_two_drones.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/central/launch/px4_gazebo.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/px4_gazebo.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/launch/px4_gazebo.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/px4_gazebo.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/central/launch/test.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/test.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/launch/test.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/test.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/central/package.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/central/package.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/central/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/.gitignore b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/.gitignore similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/.gitignore rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/.gitignore diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/CHANGELOG.rst b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/CHANGELOG.rst similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/CHANGELOG.rst rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/CHANGELOG.rst diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/package.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/package.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/plugin.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/plugin.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/plugin.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/plugin.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/resource/py_console_widget.ui b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/resource/py_console_widget.ui similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/resource/py_console_widget.ui rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/resource/py_console_widget.ui diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/resource/rqt_behavior_tree b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/resource/rqt_behavior_tree similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/resource/rqt_behavior_tree rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/resource/rqt_behavior_tree diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/setup.cfg b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/setup.cfg similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/setup.cfg rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/setup.cfg diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/setup.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/setup.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/setup.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/setup.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/resource/rqt_behavior_tree_command b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/resource/rqt_behavior_tree_command rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/main.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/main.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/main.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/main.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/test.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/test.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/test.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/test.py diff --git a/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/CHANGELOG.rst b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/CHANGELOG.rst similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/CHANGELOG.rst rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/CHANGELOG.rst diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/config/gui_config.yaml b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/config/gui_config.yaml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/config/gui_config.yaml rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/config/gui_config.yaml diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/package.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/package.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/plugin.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/plugin.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/plugin.xml rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/plugin.xml diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/resource/py_console_widget.ui b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/resource/py_console_widget.ui similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/resource/py_console_widget.ui rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/resource/py_console_widget.ui diff --git a/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/resource/rqt_behavior_tree_command b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/resource/rqt_behavior_tree_command new file mode 100644 index 00000000..e69de29b diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/setup.cfg b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/setup.cfg similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/setup.cfg rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/setup.cfg diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/setup.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/setup.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/setup.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/setup.py diff --git a/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/__init__.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/main.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/main.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/main.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/main.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_text_edit.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_text_edit.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_text_edit.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_text_edit.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_widget.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_widget.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_widget.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_widget.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/spyder_console_widget.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/spyder_console_widget.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/spyder_console_widget.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/spyder_console_widget.py diff --git a/ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/template.py b/ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/template.py similarity index 100% rename from ros_ws/src/robot/autonomy/planning/global/rqt_behavior_tree_command/src/rqt_behavior_tree_command/template.py rename to ros_ws/src/robot/autonomy/5_behavior_managers/rqt_behavior_tree_command/src/rqt_behavior_tree_command/template.py diff --git a/ros_ws/src/robot/autonomy/controls/px4_msgs b/ros_ws/src/robot/autonomy/controls/px4_msgs deleted file mode 160000 index 9e356516..00000000 --- a/ros_ws/src/robot/autonomy/controls/px4_msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9e356516e3667ac32376f8781ed1e5fc43fe0b71 diff --git a/ros_ws/src/robot/autonomy/controls/px4_ros_com b/ros_ws/src/robot/autonomy/controls/px4_ros_com deleted file mode 160000 index 86e9aeb2..00000000 --- a/ros_ws/src/robot/autonomy/controls/px4_ros_com +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 86e9aeb20e55a4673fa8a9f1c29ea06a6c5ad1af diff --git a/ros_ws/src/robot/autonomy/install_geographiclib_datasets.sh b/ros_ws/src/robot/autonomy/install_geographiclib_datasets.sh deleted file mode 100644 index 65a9238e..00000000 --- a/ros_ws/src/robot/autonomy/install_geographiclib_datasets.sh +++ /dev/null @@ -1,41 +0,0 @@ -#!/bin/bash -# Script to install the model datasets required -# to GeographicLib apply certain conversions - -if [[ $UID != 0 ]]; then - echo "This script require root privileges!" 1>&2 - exit 1 -fi - -# Install datasets -run_get() { - local dir="$1" - local tool="$2" - local model="$3" - - files=$(shopt -s nullglob dotglob; echo /usr/share/GeographicLib/$dir/$model* /usr/local/share/GeographicLib/$dir/$model*) - if (( ${#files} )); then - echo "GeographicLib $tool dataset $model already exists, skipping" - return - fi - - echo "Installing GeographicLib $tool $model" - geographiclib-get-$tool $model >/dev/null 2>&1 - - files=$(shopt -s nullglob dotglob; echo /usr/share/GeographicLib/$dir/$model* /usr/local/share/GeographicLib/$dir/$model*) - if (( ! ${#files} )); then - echo "Error while installing GeographicLib $tool $model" - return - fi -} - -# check which command script is available -if hash geographiclib-get-geoids; then - run_get geoids geoids egm96-5 - run_get gravity gravity egm96 - run_get magnetic magnetic emm2015 -elif hash geographiclib-datasets-download; then # only allows install the goid model dataset - geographiclib-datasets-download egm96_5; -else - echo "OS not supported! Check GeographicLib page for supported OS and lib versions." 1>&2 -fi diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/base_main_class/CMakeLists.txt deleted file mode 100644 index a71ca1e5..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/CMakeLists.txt +++ /dev/null @@ -1,80 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(base) - -add_compile_options(-Wno-reorder -Wno-unused-parameter) - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - nodelet -) - - -catkin_package( - INCLUDE_DIRS include - LIBRARIES base_node base_nodelet - CATKIN_DEPENDS roscpp std_msgs nodelet -) - -include_directories( - ${catkin_INCLUDE_DIRS} - include -) - -#============================================================================================ -# ---------------------------- Base Nodelet Library ----------------------------------------- -#============================================================================================ - -add_library(base_nodelet - src/Base.cpp - src/BaseNodelet.cpp - src/HealthMonitor.cpp -) - -add_dependencies(base_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(base_nodelet - ${catkin_LIBRARIES} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - - -#target_link_libraries(base_nodelet ${catkin_LIBRARIES}) -#if(catkin_EXPORTED_LIBRARIES) -# add_dependencies(base_nodelet ${catkin_EXPORTED_LIBRARIES}) -#endif() - -#install(FILES base_nodelet.xml -#DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -#============================================================================================ -# ---------------------------- Base Node Library -------------------------------------------- -#============================================================================================ - -add_library(base_node - src/Base.cpp - src/BaseNode.cpp - src/HealthMonitor.cpp - src/main.cpp -) - -add_dependencies(base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(base_node - ${catkin_LIBRARIES} -) - -install(TARGETS base_nodelet base_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY include/base/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/LICENSE b/ros_ws/src/robot/autonomy/planning/local/base_main_class/LICENSE deleted file mode 100644 index b039fc08..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/LICENSE +++ /dev/null @@ -1,11 +0,0 @@ -Copyright 2020 Carnegie Mellon University - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. - -* 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. - -* 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. diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/README.md b/ros_ws/src/robot/autonomy/planning/local/base_main_class/README.md deleted file mode 100644 index 5fb8cd90..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/README.md +++ /dev/null @@ -1,18 +0,0 @@ -This package contains the BaseNode and BaseNodelet classes which monitor the status of the process. All nodes and nodelets you create should inherit from these classes. - -For both nodes and nodelets three functions need to be implemented by derived classes: - -initialize(): This is where you should set up ROS related objects like publishers and subscribers and check for ROS parameters. - -execute(): This is where your main program logic should go. It gets called in a loop at a rate set by the execute_target parameter in your node’s namespace. - -~Destructor(): Do cleanup, like memory deallocation etc., in the destructor. - -For nodes, you also need to implement the BaseNode::get() function to return a pointer to an instance of your class derived from BaseNode. This is used by the main function to run your node. For nodelets, this is not necessary. They are started automatically. - -This package also contains a HealthMonitor class. This lets you time code's running time by calling tic("name") then toc("name"). When tic("name") is called for the first time, a ROS parameter with the name "name_target" is looked up which indicates what the target update rate for the code between tic and toc is. For now, a message is printed if the target rate is not achieved. - -An example implementation of a BaseNode can be found in the example_node package. -An example implementation of a BaseNodelet can be found in the example_nodelet package. - -Author: John Keller jkeller2@andrew.cmu.edu slack: kellerj diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/Base.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/Base.h deleted file mode 100644 index 6a984ed3..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/Base.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef _BASE_H_ -#define _BASE_H_ - -#include -#include -#include "HealthMonitor.h" - -class Base{ - private: - bool failed_; - ros::Timer execute_timer; - - public: - HealthMonitor monitor; - - // These functions will be called by main, do health monitoring - // and call the dervied funtions below - bool _initialize(); - bool _execute(); - virtual ~Base(); - - void execute_timer_callback(const ros::TimerEvent& te); - - // These functions are implemented by the user - virtual bool initialize() = 0; - virtual bool execute() = 0; - - // Failing will stop execution and deinitialize - void fail(std::string reason); - - // Returns the name the user gave to the node/nodelet - std::string get_node_name(); - - // Get node handles. - ros::NodeHandle* get_node_handle(); - ros::NodeHandle* get_private_node_handle(); - - protected: - Base(std::string node_name); - - ros::NodeHandle* node_handle_; - ros::NodeHandle* private_node_handle_; - - std::string node_name_; - - // If this is true, the pure virtual deinitialize() function will not be - // called in _deinitialize(), since you can only catch the end of - // a nodelet by implementing its destructor. - bool is_nodelet_; -}; - - -#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNode.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNode.h deleted file mode 100644 index 0880aeec..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNode.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef _BASE_NODE_H_ -#define _BASE_NODE_H_ - -#include -#include "Base.h" - -class BaseNode : public Base { - public: - // This will be implemented by a specific task and will return an instance of itself, it will be called by the main function - static BaseNode* get(); - - virtual ~BaseNode(); - - // Needed to call ros init - static int argc; - static char** argv; - - protected: - BaseNode(std::string node_name, bool anonymous=false); - - private: - bool anonymous_; -}; - - - -#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNodelet.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNodelet.h deleted file mode 100644 index 4d799c14..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNodelet.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef _BASE_NODELET_H_ -#define _BASE_NODELET_H_ - -#include -#include -#include -#include "Base.h" - -class BaseNodelet : public Base, public nodelet::Nodelet { - public: - // This is inherited from nodelet::Nodelet - virtual void onInit(); - - protected: - BaseNodelet(std::string node_name); - ~BaseNodelet(); -}; - - -#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/HealthMonitor.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/HealthMonitor.h deleted file mode 100644 index 0ec4a2ab..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/HealthMonitor.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef _HEALTH_MONITOR_H_ -#define _HEALTH_MONITOR_H_ - -#include -#include -#include -#include - -class Base; //forward declaration - -// Struct for time logging. -struct time_info { - boost::chrono::time_point start; - int64_t elapsed, min_us, max_us; - uint64_t calls; - double target_hz; -}; - -class HealthMonitor { - private: - std::map times; - Base* base_; - - public: - HealthMonitor(Base* base); - - - //========================================================================================= - // ---------------------------- Time Measurement Functions -------------------------------- - //========================================================================================= - - void tic(std::string id); - int64_t toc(std::string id); - double get_target_hz(std::string id); - void print_time_statistics(); -}; - - -#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/package.xml b/ros_ws/src/robot/autonomy/planning/local/base_main_class/package.xml deleted file mode 100644 index ca89dd11..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - base - 0.0.0 - The base package - - john - TODO - - catkin - roscpp - std_msgs - nodelet - - roscpp - std_msgs - nodelet - - - - diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/Base.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/Base.cpp deleted file mode 100644 index 102372a4..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/Base.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include -#include -#include "base/Base.h" -#include "base/HealthMonitor.h" -#include -#include - -Base::Base(std::string node_name) - : node_name_(node_name) - , monitor(this) - , is_nodelet_(false) - , failed_(false){ - -} - -Base::~Base(){ - monitor.print_time_statistics(); -} - -bool Base::_initialize(){ - // get the rate that execute should be called at - double execute_timer_rate; - bool found_param = get_private_node_handle()->getParam("execute_target", execute_timer_rate); - if(!found_param){ - ROS_FATAL_STREAM("The execute_target parameter was not set. Exiting."); - return false; - } - - if (execute_timer_rate <= 0.0){ - ROS_FATAL_STREAM("The execute_target parameter must be larger than 0.0, Exiting."); - return false; - } - - // call the derived class's initialize - bool status = initialize(); - if(!status){ - ROS_FATAL_STREAM("The initialize() function failed. Exiting."); - return false; - } - - // setup the execute timer - execute_timer = get_node_handle()->createTimer(ros::Duration(1./execute_timer_rate), - &Base::execute_timer_callback, this); - - return true; -} - -void Base::execute_timer_callback(const ros::TimerEvent& te){ - bool status = _execute(); - - if(!status){ - execute_timer.stop(); - - if(!is_nodelet_) - ros::shutdown(); - } -} - -bool Base::_execute(){ - monitor.tic("execute"); - bool status = execute(); - monitor.toc("execute"); - - return status && !failed_; -} - - -void Base::fail(std::string reason){ - ROS_FATAL_STREAM("Node " << get_node_name() << " has failed." << std::endl - << "Reason: " << reason); - - failed_ = true; -} - -std::string Base::get_node_name(){ - return ros::this_node::getName(); -} - - -ros::NodeHandle* Base::get_node_handle(){ - return node_handle_; -} - -ros::NodeHandle* Base::get_private_node_handle(){ - return private_node_handle_; -} diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNode.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNode.cpp deleted file mode 100644 index d6a213ef..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNode.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - -#include "base/BaseNode.h" - - -BaseNode::BaseNode(std::string node_name, bool anonymous) - : Base(node_name) - , anonymous_(anonymous){ - - // call ros init - if(anonymous_) - ros::init(argc, argv, node_name_, ros::init_options::AnonymousName | ros::init_options::NoSigintHandler); - else - ros::init(argc, argv, node_name_, ros::init_options::NoSigintHandler); - - // set up the node handles - node_handle_ = new ros::NodeHandle(); - private_node_handle_ = new ros::NodeHandle("~"); -} - -BaseNode::~BaseNode(){ - -} - -int BaseNode::argc = 0; -char** BaseNode::argv = NULL; diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNodelet.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNodelet.cpp deleted file mode 100644 index f70f5263..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNodelet.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include -#include -#include -#include - -#include "base/BaseNodelet.h" - - -BaseNodelet::BaseNodelet(std::string node_name) - : Base(node_name){ - is_nodelet_ = true; -} - -BaseNodelet::~BaseNodelet(){ - -} - -void BaseNodelet::onInit(){ - node_handle_ = &getNodeHandle(); - private_node_handle_ = &getPrivateNodeHandle(); - _initialize(); -} diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/HealthMonitor.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/HealthMonitor.cpp deleted file mode 100644 index 8c7aedf2..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/HealthMonitor.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "base/HealthMonitor.h" - -#include - -#include - -#include "base/Base.h" - -HealthMonitor::HealthMonitor(Base* base) : base_(base) {} - -//========================================================================================= -// ---------------------------- Time Measurement Functions -------------------------------- -//========================================================================================= - -void HealthMonitor::tic(std::string id) { - // initialize time struct if tic is called for the first time on id - if (times.count(id) == 0) { - times[id].elapsed = 0; - times[id].calls = 0; - times[id].min_us = std::numeric_limits::max(); - times[id].max_us = std::numeric_limits::min(); - - base_->get_private_node_handle()->param(id + "_target", times[id].target_hz, -1); - } - - // log the current time - times[id].start = boost::chrono::system_clock::now(); -} - -int64_t HealthMonitor::toc(std::string id) { - // get elapsed time if tic has been called. - if (times.count(id) > 0) { - typedef boost::chrono::duration microseconds; - microseconds delta_us = boost::chrono::duration_cast( - boost::chrono::system_clock::now() - times[id].start); - int64_t count = delta_us.count(); - times[id].elapsed += count; - times[id].calls++; - times[id].min_us = std::min(count, times[id].min_us); - times[id].max_us = std::max(count, times[id].max_us); - - double hz = 1000000. / count; - bool is_hitting_target_hz = hz >= times[id].target_hz; - if (!is_hitting_target_hz) - ROS_ERROR_STREAM(std::setprecision(2) - << "execute function is not running fast enought." - << " Actual Hz: " << hz << " Target Hz: " << times[id].target_hz); - - return count; - } - - // otherwise return -1 - return -1; -} - -double HealthMonitor::get_target_hz(std::string id) { return times[id].target_hz; } - -void HealthMonitor::print_time_statistics() { - ROS_INFO_STREAM("Time Statistics for " << base_->get_node_name()); // node_name_); - int col_width = 15; - ROS_INFO_STREAM(std::setw(col_width) - << "Name" << std::setw(col_width) << "Avg Hz" << std::setw(col_width) - << "Target Hz" << std::setw(col_width) << "Avg ms" << std::setw(col_width) - << "Min ms" << std::setw(col_width) << "Max ms" << std::setw(col_width) - << "Calls"); - - for (std::map::iterator it = times.begin(); it != times.end(); it++) { - if (it->second.calls == 0) { - ROS_INFO_STREAM(it->first << ": toc(\"" << it->first << "\"); was never called"); - } else { - double average_elapsed = (double)it->second.elapsed / (double)it->second.calls; - double average_hz = 1000000. / average_elapsed; - ROS_INFO_STREAM(std::setprecision(2) - << std::setw(col_width) << it->first << std::setw(col_width) - << average_hz << std::setw(col_width) << it->second.target_hz - << std::setw(col_width) << average_elapsed / 1000. - << std::setw(col_width) << (double)it->second.min_us / 1000. - << std::setw(col_width) << (double)it->second.max_us / 1000. - << std::setw(col_width) << it->second.calls); - } - } -} diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/main.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/main.cpp deleted file mode 100644 index 9e2696ee..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/main.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "base/BaseNode.h" -#include -#include - -BaseNode* base_node; - -void interrupt_handler(int sig){ - delete base_node; - ros::shutdown(); -} - -int main(int argc, char** argv){ - BaseNode::argc = argc; - BaseNode::argv = argv; - base_node = BaseNode::get(); - - signal(SIGINT, interrupt_handler); - - if(base_node->_initialize()) - ros::spin(); - else - ROS_FATAL_STREAM("Node initialization failed. Exiting."); - - return 0; -} diff --git a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/CMakeLists.txt deleted file mode 100644 index f1d6d174..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(core_map_representation_interface) - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(airstack_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclpy REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(visualization_msgs REQUIRED) - -# Include directories -include_directories( - include - ${ament_INCLUDE_DIRS} -) - -add_library(core_map_representation_interface INTERFACE) - -ament_export_targets(core_map_representation_interfaceTargets HAS_LIBRARY_TARGET) -#ament_export_include_directories(include) -ament_export_dependencies( - airstack_msgs - geometry_msgs - nav_msgs - pluginlib - rclcpp - rclpy - sensor_msgs - std_msgs - tf2 - tf2_ros - visualization_msgs -) -install( - TARGETS core_map_representation_interface - EXPORT core_map_representation_interfaceTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) -# Install include files -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} - FILES_MATCHING PATTERN "*.h" -) - -# Install Python scripts (if you have any)### -# install(PROGRAMS -# scripts/your_python_script.py -# DESTINATION lib/${PROJECT_NAME} -# ) - - -# Export dependencies - - -ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/CMakeLists.txt deleted file mode 100644 index d3b17812..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/CMakeLists.txt +++ /dev/null @@ -1,100 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(disparity_map_representation) - -find_package(ament_cmake REQUIRED) - -find_package(airstack_common REQUIRED) -find_package(airstack_msgs REQUIRED) -find_package(core_map_representation_interface REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(disparity_graph REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(image_geometry REQUIRED) -find_package(image_transport REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(pcl_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclpy REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(stereo_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_eigen REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(visualization_msgs REQUIRED) - -ament_export_include_directories(include) -ament_export_libraries(disparity_graph) - -include_directories( - ${ament_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${core_map_representation_interface_INCLUDE_DIRS} - ${disparity_graph_INCLUDE_DIRS} - ${pluginlib_INCLUDE_DIRS} - - # ${CMAKE_CURRENT_SOURCE_DIR}/include -) - -add_library(disparity_map_representation - src/disparity_map_representation.cpp -) - -target_link_libraries(disparity_map_representation - disparity_graph - ${pluginlib_LIBRARIES} # Ensure this variable is set correctly -) - -ament_target_dependencies(disparity_map_representation - airstack_common - airstack_msgs - core_map_representation_interface - cv_bridge - disparity_graph - pluginlib - rclcpp - tf2 - tf2_ros -) - -ament_export_targets(disparity_map_representationTargets HAS_LIBRARY_TARGET) -ament_export_dependencies(rclcpp - airstack_common - airstack_msgs - core_map_representation_interface - cv_bridge - disparity_graph - pluginlib - tf2 - tf2_ros -) - -install( - TARGETS disparity_map_representation - EXPORT disparity_map_representationTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -# install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -install(FILES package.xml - DESTINATION share/${PROJECT_NAME} -) -target_link_libraries(disparity_map_representation - ${disparity_graph_LIBRARIES} -) - -# ament_export_include_directories(include) -# ament_export_libraries(disparity_map_representation) -# ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} - FILES_MATCHING PATTERN "*.h" -) - -ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/disparity_map_representation_plugin.xml b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/disparity_map_representation_plugin.xml deleted file mode 100644 index a261b304..00000000 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/disparity_map_representation_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Disparity map representation plugin. - -