├── .github └── workflows │ ├── main.yml │ ├── release.yml │ └── test-release.yml ├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── release_packages.yaml ├── uavcan_ros_bridge.rosinstall ├── uavcan_ros_bridge ├── CMakeLists.txt ├── cmake │ └── uavcan_ros_bridge-extras.cmake.in ├── dsdl │ ├── dsdlc_generated │ │ └── dummy │ ├── smarc_uavcan_messages │ │ ├── 20100.SensorPressure.uavcan │ │ ├── README.md │ │ └── test.txt │ └── smarc_uavcan_services │ │ ├── 200.ArmEmergencySystems.uavcan │ │ └── README.md ├── include │ └── uavcan_ros_bridge │ │ ├── ros_to_uav │ │ ├── array_command.h │ │ ├── command.h │ │ ├── rpm_command.h │ │ ├── uavcan_node_info.h │ │ ├── uavcan_restart.h │ │ └── uavcan_transport_stats.h │ │ ├── uav_to_ros │ │ ├── battery_state.h │ │ ├── circuit_status.h │ │ ├── esc_status.h │ │ ├── gps_fix.h │ │ ├── imu.h │ │ ├── magnetic_field.h │ │ ├── pressure.h │ │ └── sensor_pressure.h │ │ └── uavcan_ros_bridge.h ├── launch │ └── bridge.launch ├── package.xml └── src │ ├── ros_to_uav │ ├── array_command.cpp │ ├── command.cpp │ ├── rpm_command.cpp │ ├── uavcan_node_info.cpp │ ├── uavcan_restart.cpp │ └── uavcan_transport_stats.cpp │ ├── ros_to_uavcan_bridge.cpp │ ├── ros_to_uavcan_services.cpp │ ├── uav_to_ros │ ├── battery_state.cpp │ ├── circuit_status.cpp │ ├── esc_status.cpp │ ├── gps_fix.cpp │ ├── imu.cpp │ ├── magnetic_field.cpp │ ├── pressure.cpp │ └── sensor_pressure.cpp │ ├── uavcan_platform_linux.cpp │ └── uavcan_to_ros_bridge.cpp └── uavcan_ros_msgs ├── CMakeLists.txt ├── msg ├── CircuitStatus.msg ├── ESCStatus.msg ├── UavcanNodeStatus.msg ├── UavcanNodeStatusNamed.msg └── UavcanNodeStatusNamedArray.msg ├── package.xml └── srv ├── UavcanGetDataTypeInfo.srv ├── UavcanGetNodeInfo.srv ├── UavcanGetTransportStats.srv └── UavcanRestartNode.srv /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - {ROS_DISTRO: noetic, ROS_REPO: testing, UPSTREAM_WORKSPACE: 'uavcan_ros_bridge.rosinstall -uavcan_ros_bridge', ADDITIONAL_DEBS: 'python'} 11 | - {ROS_DISTRO: noetic, ROS_REPO: main, UPSTREAM_WORKSPACE: 'uavcan_ros_bridge.rosinstall -uavcan_ros_bridge', ADDITIONAL_DEBS: 'python'} 12 | - {ROS_DISTRO: melodic, ROS_REPO: main, UPSTREAM_WORKSPACE: 'uavcan_ros_bridge.rosinstall -uavcan_ros_bridge', ADDITIONAL_DEBS: 'python'} 13 | runs-on: ubuntu-20.04 14 | steps: 15 | - uses: actions/checkout@v1 16 | with: 17 | submodules: recursive 18 | - name: Set up Python 2.7 19 | uses: actions/setup-python@v2 20 | with: 21 | python-version: 2.7 22 | - name: Install pyuavcan 23 | run: | 24 | cd uavcan_ros_bridge/libuavcan/libuavcan/dsdl_compiler/pyuavcan/ 25 | python setup.py install 26 | - uses: 'ros-industrial/industrial_ci@master' 27 | env: ${{matrix.env}} 28 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: release-deb 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | 8 | jobs: 9 | release_build: 10 | strategy: 11 | matrix: 12 | include: 13 | - distro: melodic 14 | ubuntu: bionic 15 | bloom: python-bloom 16 | - distro: noetic 17 | ubuntu: focal 18 | bloom: python3-bloom 19 | runs-on: ubuntu-latest 20 | container: 21 | image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu }}-ros-${{ matrix.distro }}-ros-base-latest 22 | #options: -u root # setup-node requires root access 23 | steps: 24 | - uses: actions/checkout@v2 25 | with: 26 | path: '.' 27 | - name: Setup environment 28 | run: | 29 | sudo curl https://raw.githubusercontent.com/smarc-project/rosinstall/master/sources.list.d/smarc-${{ matrix.distro }}-latest.list -o /etc/apt/sources.list.d/smarc-latest.list 30 | sudo curl https://raw.githubusercontent.com/smarc-project/rosinstall/master/rosdep/50-smarc-${{ matrix.distro }}.list -o /etc/ros/rosdep/sources.list.d/50-smarc.list 31 | sudo apt update 32 | rosdep update 33 | - name: Install deps 34 | run: | 35 | ls 36 | sudo apt install -y ${{ matrix.bloom }} fakeroot dpkg-dev debhelper zip 37 | - name: Build package 38 | run: | 39 | mkdir bloom-release-debs 40 | while read line; do 41 | pkg=$(echo $line | cut -c3-) 42 | echo "Doing ${pkg}" 43 | cd $pkg 44 | ls 45 | rosdep install --from-path . --ignore-src --rosdistro ${{ matrix.distro }} -y 46 | bloom-generate rosdebian --os-name ubuntu --os-version ${{ matrix.ubuntu }} --ros-distro ${{ matrix.distro }} 47 | fakeroot debian/rules binary 48 | cd .. 49 | sudo dpkg -i ros-${{ matrix.distro }}-*.deb 50 | mv ros-${{ matrix.distro }}-*.deb bloom-release-debs 51 | done < release_packages.yaml 52 | zip -j bloom-${{ matrix.distro }}-release-deb.zip bloom-release-debs/* 53 | ls 54 | - name: Upload binaries to release 55 | uses: svenstaro/upload-release-action@v2 56 | with: 57 | repo_token: ${{ secrets.GITHUB_TOKEN }} 58 | file: ./bloom-${{ matrix.distro }}-release-deb.zip 59 | asset_name: bloom-${{ matrix.distro }}-release-deb.zip 60 | tag: ${{ github.ref }} 61 | overwrite: true 62 | body: "Debian release generated using bloom" 63 | -------------------------------------------------------------------------------- /.github/workflows/test-release.yml: -------------------------------------------------------------------------------- 1 | name: release-CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | test-release-job: 7 | strategy: 8 | matrix: 9 | include: 10 | - distro: melodic 11 | ubuntu: bionic 12 | - distro: noetic 13 | ubuntu: focal 14 | runs-on: ubuntu-latest 15 | name: Test building a ROS debian release 16 | steps: 17 | - uses: actions/checkout@v2 18 | - name: Run the release build 19 | uses: smarc-project/test-ros-release-action@v2.0.1 20 | with: 21 | ubuntu: ${{ matrix.ubuntu }} 22 | distro: ${{ matrix.distro }} 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | uavcan_ros_bridge/dsdl/dsdlc_generated/* -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "libuavcan"] 2 | path = uavcan_ros_bridge/libuavcan 3 | url = https://github.com/UAVCAN/libuavcan.git 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, SMaRC - Swedish Maritime Robotics Centre 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | uavcan_ros_bridge 2 | ================= 3 | ![CI](https://github.com/smarc-project/uavcan_ros_bridge/workflows/CI/badge.svg?branch=noetic-devel) [![license](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) 4 | 5 | Bridge for communication between ROS and the uavcan CAN-bus protocol 6 | 7 | ## Dependencies & building 8 | 9 | After cloning, execute the following command in the cloned repo: 10 | ``` 11 | git submodule update --init --recursive 12 | ``` 13 | Once done, go into the folder `libuavcan/libuavcan/dsdl_compiler/pyuavcan/` and execute: 14 | ``` 15 | sudo python setup.py install 16 | ``` 17 | Then, simply put this project into a catkin workspace and run `catkin_make` or `catkin build` to build everything. 18 | 19 | ## Usage 20 | 21 | Launch conversion in both directions (between uavcan and ros) by running the launch file: 22 | ``` 23 | roslaunch uavcan_ros_bridge bridge.launch 24 | ``` 25 | This will launch two nodes: the `uavcan_to_ros_bridge_node` and the `ros_to_uavcan_bridge_node` 26 | which handle the conversion in the respective directions. Any new type conversions should 27 | be added to the node that handles the relevant direction. 28 | 29 | ## Adding new conversions 30 | 31 | You can add new conversion headers for [ros to uavcan](https://gitr.sys.kth.se/smarc-project/sam_drivers/tree/master/uavcan_ros_bridge/include/uavcan_ros_bridge/ros_to_uav) 32 | and [uavcan to ros](https://gitr.sys.kth.se/smarc-project/sam_drivers/tree/master/uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros) 33 | with corresponding implementations in the [src folder](https://gitr.sys.kth.se/smarc-project/sam_drivers/tree/master/uavcan_ros_bridge/src). An example implementation for an IMU might look like this: 34 | ```cpp 35 | template <> 36 | bool convert(const uavcan::equipment::ahrs::RawIMU& uav_msg, sensor_msgs::Imu& ros_msg) 37 | { 38 | ros_msg.header.stamp = convert_timestamp(uav_msg.timestamp); 39 | ros_msg.linear_acceleration.x = uav_msg.accelerometer_latest[0]; 40 | ros_msg.linear_acceleration.y = uav_msg.accelerometer_latest[1]; 41 | ros_msg.linear_acceleration.z = uav_msg.accelerometer_latest[2]; 42 | ros_msg.angular_velocity.x = uav_msg.rate_gyro_latest[0]; 43 | ros_msg.angular_velocity.y = uav_msg.rate_gyro_latest[1]; 44 | ros_msg.angular_velocity.z = uav_msg.rate_gyro_latest[2]; 45 | return true; 46 | } 47 | ``` 48 | You then need to add them to the build configuration in the cmake file [like this](https://gitr.sys.kth.se/smarc-project/sam_drivers/blob/master/uavcan_ros_bridge/CMakeLists.txt#L133) 49 | and link them into one of the bridges (the publisher or the subscriber) [like this](https://gitr.sys.kth.se/smarc-project/sam_drivers/blob/master/uavcan_ros_bridge/CMakeLists.txt#L181). 50 | Finally add them to either of the bridge nodes `uavcan_to_ros_bridge` or `ros_to_uavcan_bridge` similar to the 51 | [already existing conversions](https://gitr.sys.kth.se/smarc-project/sam_drivers/blob/master/uavcan_ros_bridge/src/ros_to_uavcan_bridge.cpp#L46), 52 | i.e. like: 53 | ``` 54 | ros_to_uav::ConversionServer server(uav_node, ros_node, "uavcan_command"); 55 | ``` 56 | or 57 | ``` 58 | uav_to_ros::ConversionServer server(uav_node, ros_node, "uavcan_imu"); 59 | ``` 60 | where `uavcan_command` and `uavcan_imu` are the ros topics being subscribed to and published to respectively. 61 | 62 | ## Existing conversions (*work in progress*) 63 | 64 | ### ROS to UAVCAN 65 | * `std_msgs/Float32` to `uavcan.equipment.actuator.ArrayCommand` on ros topic `/uavcan_command` 66 | * `sam_msgs/ArrayCommand` to `uavcan.equipment.actuator.ArrayCommand` on ros topic `/uavcan_array_command` 67 | * `std_msgs/Int32` to `uavcan.equipment.esc.RPMCommand` on ros topic `/ros_to_uavcan_bridge_node/rpm_command` 68 | 69 | 70 | ### UAVCAN to ROS 71 | * `uavcan.equipment.ahrs.RawIMU` to `sensor_msgs/Imu` on ros topic `/uavcan_imu` 72 | * `uavcan.equipment.gnss.Fix` to `sensor_msgs/NavSatFix` on ros topic `/uavcan_to_ros_bridge_node/gps_fix` 73 | * `uavcan.equipment.power.BatteryInfo` to `sensor_msgs/BatteryState` on ros topic `/uavcan_to_ros_bridge_node/battery_state` 74 | * `uavcan.equipment.ahrs.MagneticFieldStrength` to `sensor_msgs/MagneticField` on ros topic `/uavcan_to_ros_bridge_node/magnetic_field` 75 | -------------------------------------------------------------------------------- /release_packages.yaml: -------------------------------------------------------------------------------- 1 | - uavcan_ros_msgs 2 | -------------------------------------------------------------------------------- /uavcan_ros_bridge.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: sam_common, uri: 'https://github.com/smarc-project/sam_common.git', version: noetic-devel} 2 | - git: {local-name: uavcan_ros_bridge, uri: 'https://github.com/smarc-project/uavcan_ros_bridge.git', version: noetic-devel} 3 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uavcan_ros_bridge) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | set(CMAKE_CXX_FLAGS "-Wall -Wextra -pedantic -std=c++11") 8 | 9 | ## Find catkin macros and libraries 10 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 11 | ## is used, also find other catkin packages 12 | find_package(catkin REQUIRED COMPONENTS 13 | sam_msgs 14 | geometry_msgs 15 | roscpp 16 | sensor_msgs 17 | std_msgs 18 | std_srvs 19 | message_generation 20 | message_runtime 21 | uavcan_ros_msgs 22 | ) 23 | 24 | #find_library(UAVCAN_LIB uavcan libuavcan_dsdlc REQUIRED) 25 | 26 | #set(UAVCAN_DSDL_DEFINITIONS "/usr/local/share/uavcan/dsdl/uavcan") # For Linux, if the library is installed 27 | 28 | ## System dependencies are found with CMake's conventions 29 | # find_package(Boost REQUIRED COMPONENTS system) 30 | 31 | 32 | ## Uncomment this if the package has a setup.py. This macro ensures 33 | ## modules and global scripts declared therein get installed 34 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 35 | # catkin_python_setup() 36 | 37 | ################################################ 38 | ## Declare ROS messages, services and actions ## 39 | ################################################ 40 | 41 | ## To declare and build messages, services or actions from within this 42 | ## package, follow these steps: 43 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 44 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 45 | ## * In the file package.xml: 46 | ## * add a build_depend tag for "message_generation" 47 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 48 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 49 | ## but can be declared for certainty nonetheless: 50 | ## * add a run_depend tag for "message_runtime" 51 | ## * In this file (CMakeLists.txt): 52 | ## * add "message_generation" and every package in MSG_DEP_SET to 53 | ## find_package(catkin REQUIRED COMPONENTS ...) 54 | ## * add "message_runtime" and every package in MSG_DEP_SET to 55 | ## catkin_package(CATKIN_DEPENDS ...) 56 | ## * uncomment the add_*_files sections below as needed 57 | ## and list every .msg/.srv/.action file to be processed 58 | ## * uncomment the generate_messages entry below 59 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 60 | 61 | ################################################ 62 | ## Declare ROS dynamic reconfigure parameters ## 63 | ################################################ 64 | 65 | ## To declare and build dynamic reconfigure parameters within this 66 | ## package, follow these steps: 67 | ## * In the file package.xml: 68 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 69 | ## * In this file (CMakeLists.txt): 70 | ## * add "dynamic_reconfigure" to 71 | ## find_package(catkin REQUIRED COMPONENTS ...) 72 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 73 | ## and list every .cfg file to be processed 74 | 75 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 76 | # generate_dynamic_reconfigure_options( 77 | # cfg/DynReconf1.cfg 78 | # cfg/DynReconf2.cfg 79 | # ) 80 | 81 | #message("UACAN_LIB: ${UAVCAN_LIB}") 82 | 83 | ################################### 84 | ## catkin specific configuration ## 85 | ################################### 86 | ## The catkin_package macro generates cmake config files for your package 87 | ## Declare things to be passed to dependent projects 88 | ## INCLUDE_DIRS: uncomment this if your package contains header files 89 | ## LIBRARIES: libraries you create in this project that dependent projects also need 90 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 91 | ## DEPENDS: system dependencies of this project that dependent projects also need 92 | catkin_package( 93 | INCLUDE_DIRS 94 | include 95 | dsdl/dsdlc_generated 96 | ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan/include/ 97 | ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan_drivers/posix/include 98 | ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan_drivers/linux/include 99 | #${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan/include/dsdlc_generated 100 | LIBRARIES 101 | uavcan 102 | uav_to_ros_imu 103 | uav_to_ros_gps_fix 104 | uav_to_ros_battery_state 105 | uav_to_ros_magnetic_field 106 | uav_to_ros_pressure 107 | uav_to_ros_sensor_pressure 108 | uav_to_ros_esc_status 109 | uav_to_ros_circuit_status 110 | ros_to_uav_command 111 | ros_to_uav_rpm_command 112 | ros_to_uav_array_command 113 | ros_to_uav_uavcan_node_info 114 | ros_to_uav_uavcan_transport_stats 115 | ros_to_uav_uavcan_restart 116 | CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs uavcan_ros_msgs 117 | # DEPENDS system_lib 118 | CFG_EXTRAS uavcan_ros_bridge-extras.cmake 119 | ) 120 | 121 | set(UAVCAN_DSDL_DEFINITIONS "${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/dsdl/uavcan") 122 | add_subdirectory(libuavcan) 123 | 124 | add_custom_target(dsdlc ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan/dsdl_compiler/libuavcan_dsdlc 125 | "smarc_uavcan_messages" -I${UAVCAN_DSDL_DEFINITIONS} 126 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/dsdl) 127 | 128 | ########### 129 | ## Build ## 130 | ########### 131 | 132 | ## Specify additional locations of header files 133 | ## Your package locations should be listed before other locations 134 | include_directories( 135 | include 136 | ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan/include/ 137 | ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan_drivers/posix/include 138 | ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan_drivers/linux/include 139 | ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan/include/dsdlc_generated 140 | dsdl/dsdlc_generated 141 | ${catkin_INCLUDE_DIRS} 142 | ) 143 | 144 | ## Declare a C++ library 145 | add_library(uavcan_platform_linux 146 | src/uavcan_platform_linux.cpp 147 | ) 148 | 149 | add_library(uav_to_ros_imu 150 | src/uav_to_ros/imu.cpp 151 | ) 152 | 153 | add_library(uav_to_ros_gps_fix 154 | src/uav_to_ros/gps_fix.cpp 155 | ) 156 | 157 | add_library(uav_to_ros_battery_state 158 | src/uav_to_ros/battery_state.cpp 159 | ) 160 | 161 | add_library(uav_to_ros_magnetic_field 162 | src/uav_to_ros/magnetic_field.cpp 163 | ) 164 | 165 | add_library(uav_to_ros_pressure 166 | src/uav_to_ros/pressure.cpp 167 | ) 168 | 169 | add_library(uav_to_ros_sensor_pressure 170 | src/uav_to_ros/sensor_pressure.cpp 171 | ) 172 | 173 | add_library(uav_to_ros_esc_status 174 | src/uav_to_ros/esc_status.cpp 175 | ) 176 | 177 | add_library(uav_to_ros_circuit_status 178 | src/uav_to_ros/circuit_status.cpp 179 | ) 180 | 181 | #add_library(uav_to_ros_actuator_status 182 | # src/uav_to_ros/actuator_status.cpp 183 | #) 184 | 185 | add_library(ros_to_uav_command 186 | src/ros_to_uav/command.cpp 187 | ) 188 | 189 | add_library(ros_to_uav_rpm_command 190 | src/ros_to_uav/rpm_command.cpp 191 | ) 192 | 193 | add_library(ros_to_uav_array_command 194 | src/ros_to_uav/array_command.cpp 195 | ) 196 | 197 | #add_library(ros_to_uav_percent_stamped 198 | # src/ros_to_uav/percent_stamped.cpp 199 | #) 200 | 201 | #add_library(ros_to_uav_ballast_angles 202 | # src/ros_to_uav/ballast_angles.cpp 203 | #) 204 | 205 | add_library(ros_to_uav_uavcan_node_info 206 | src/ros_to_uav/uavcan_node_info.cpp 207 | ) 208 | 209 | add_library(ros_to_uav_uavcan_transport_stats 210 | src/ros_to_uav/uavcan_transport_stats.cpp 211 | ) 212 | 213 | add_library(ros_to_uav_uavcan_restart 214 | src/ros_to_uav/uavcan_restart.cpp 215 | ) 216 | 217 | ## Add cmake target dependencies of the library 218 | ## as an example, code may need to be generated before libraries 219 | ## either from message generation or dynamic reconfigure 220 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 221 | 222 | ## Declare a C++ executable 223 | ## With catkin_make all packages are built within a single CMake context 224 | ## The recommended prefix ensures that target names across packages don't collide 225 | add_executable(uavcan_to_ros_bridge src/uavcan_to_ros_bridge.cpp) 226 | add_executable(ros_to_uavcan_bridge src/ros_to_uavcan_bridge.cpp) 227 | add_executable(ros_to_uavcan_services src/ros_to_uavcan_services.cpp) 228 | 229 | ## Rename C++ executable without prefix 230 | ## The above recommended prefix causes long target names, the following renames the 231 | ## target back to the shorter version for ease of user use 232 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 233 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 234 | 235 | ## Add cmake target dependencies of the executable 236 | ## same as for the library above 237 | add_dependencies(uav_to_ros_imu ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 238 | add_dependencies(uav_to_ros_gps_fix ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 239 | add_dependencies(uav_to_ros_battery_state ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 240 | add_dependencies(uav_to_ros_magnetic_field ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 241 | add_dependencies(uav_to_ros_pressure ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 242 | add_dependencies(uav_to_ros_sensor_pressure ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 243 | add_dependencies(uav_to_ros_esc_status ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 244 | add_dependencies(uav_to_ros_circuit_status ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 245 | #add_dependencies(uav_to_ros_actuator_status ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 246 | add_dependencies(ros_to_uav_command ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 247 | add_dependencies(ros_to_uav_rpm_command ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 248 | add_dependencies(ros_to_uav_array_command ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 249 | #add_dependencies(ros_to_uav_percent_stamped ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 250 | #add_dependencies(ros_to_uav_ballast_angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 251 | add_dependencies(ros_to_uav_uavcan_node_info ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 252 | add_dependencies(ros_to_uav_uavcan_transport_stats ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 253 | add_dependencies(ros_to_uav_uavcan_restart ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dsdlc libuavcan_dsdlc) 254 | 255 | ## Specify libraries to link a library or executable target against 256 | target_link_libraries(uavcan_to_ros_bridge 257 | uavcan_platform_linux 258 | uav_to_ros_imu 259 | uav_to_ros_gps_fix 260 | uav_to_ros_battery_state 261 | uav_to_ros_magnetic_field 262 | uav_to_ros_pressure 263 | uav_to_ros_sensor_pressure 264 | uav_to_ros_esc_status 265 | uav_to_ros_circuit_status 266 | ${catkin_LIBRARIES} 267 | #${UAVCAN_LIB} 268 | uavcan 269 | rt 270 | ) 271 | 272 | target_link_libraries(ros_to_uavcan_bridge 273 | uavcan_platform_linux 274 | ros_to_uav_command 275 | ros_to_uav_rpm_command 276 | ros_to_uav_array_command 277 | ${catkin_LIBRARIES} 278 | #${UAVCAN_LIB} 279 | uavcan 280 | #uavcan_dsdlc 281 | rt 282 | 283 | ) 284 | 285 | target_link_libraries(ros_to_uavcan_services 286 | uavcan_platform_linux 287 | ros_to_uav_uavcan_node_info 288 | ros_to_uav_uavcan_transport_stats 289 | ros_to_uav_uavcan_restart 290 | ${catkin_LIBRARIES} 291 | #${UAVCAN_LIB} 292 | uavcan 293 | #uavcan_dsdlc 294 | rt 295 | ) 296 | 297 | ############# 298 | ## Install ## 299 | ############# 300 | 301 | # all install targets should use catkin DESTINATION variables 302 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 303 | 304 | ## Mark executable scripts (Python etc.) for installation 305 | ## in contrast to setup.py, you can choose the destination 306 | # install(PROGRAMS 307 | # scripts/my_python_script 308 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 309 | # ) 310 | 311 | ## Mark executables and/or libraries for installation 312 | install(TARGETS 313 | uav_to_ros_imu 314 | uav_to_ros_gps_fix 315 | uav_to_ros_battery_state 316 | uav_to_ros_magnetic_field 317 | uav_to_ros_pressure 318 | uav_to_ros_sensor_pressure 319 | uav_to_ros_esc_status 320 | uav_to_ros_circuit_status 321 | ros_to_uav_command 322 | ros_to_uav_rpm_command 323 | ros_to_uav_array_command 324 | ros_to_uav_uavcan_node_info 325 | ros_to_uav_uavcan_transport_stats 326 | ros_to_uav_uavcan_restart 327 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 328 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 329 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 330 | ) 331 | 332 | ## Mark cpp header files for installation 333 | install(DIRECTORY include/${PROJECT_NAME}/ 334 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 335 | FILES_MATCHING PATTERN "*.h" 336 | PATTERN ".svn" EXCLUDE 337 | ) 338 | 339 | ## Mark other files for installation (e.g. launch and bag files, etc.) 340 | # install(FILES 341 | # # myfile1 342 | # # myfile2 343 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 344 | # ) 345 | 346 | ############# 347 | ## Testing ## 348 | ############# 349 | 350 | ## Add gtest based cpp test target and link libraries 351 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_uavcan_ros_bridge.cpp) 352 | # if(TARGET ${PROJECT_NAME}-test) 353 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 354 | # endif() 355 | 356 | ## Add folders to be run by python nosetests 357 | # catkin_add_nosetests(test) 358 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/cmake/uavcan_ros_bridge-extras.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | #set(UAVCAN_DSDL_DEFINITIONS "${CATKIN_DEVEL_PREFIX}/../src/uavcan_ros_bridge/uavcan_ros_bridge/libuavcan/dsdl/uavcan") 3 | #set(DSDL_COMPILER "${CATKIN_DEVEL_PREFIX}/../src/uavcan_ros_bridge/uavcan_ros_bridge/libuavcan/libuavcan/dsdl_compiler/libuavcan_dsdlc") 4 | set(UAVCAN_DSDL_DEFINITIONS "${CMAKE_CURRENT_SOURCE_DIR}/../../uavcan_ros_bridge/uavcan_ros_bridge/libuavcan/dsdl/uavcan") 5 | set(DSDL_COMPILER "${CMAKE_CURRENT_SOURCE_DIR}/../../uavcan_ros_bridge/uavcan_ros_bridge/libuavcan/libuavcan/dsdl_compiler/libuavcan_dsdlc") 6 | 7 | # This is just a temporary fix because it does not exist when starting, maybe add a dummy dir in a fork of libuavcan 8 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../uavcan_ros_bridge/uavcan_ros_bridge/libuavcan/libuavcan/include/dsdlc_generated) 9 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/dsdl/dsdlc_generated/dummy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smarc-project/uavcan_ros_bridge/830996ef39e9cabc22831d4e5c638927ff21e8dc/uavcan_ros_bridge/dsdl/dsdlc_generated/dummy -------------------------------------------------------------------------------- /uavcan_ros_bridge/dsdl/smarc_uavcan_messages/20100.SensorPressure.uavcan: -------------------------------------------------------------------------------- 1 | # 2 | # Static pressure with device_id 3 | # 4 | 5 | uavcan.equipment.air_data.StaticPressure pressure 6 | uint8 device_id -------------------------------------------------------------------------------- /uavcan_ros_bridge/dsdl/smarc_uavcan_messages/README.md: -------------------------------------------------------------------------------- 1 | # smarc_uavcan_messages 2 | Custom UAVCAN messages for the SMaRC project. 3 | 4 | ## How to create a new message type 5 | 6 | Give your message type a new UNUSED ID in the range 20000-20999. 7 | Decide on a descriptive name that starts with a letter [a-zA-Z] 8 | 9 | Create a new file with `..uavcan` 10 | Example: `20100.SensorPressure.uavcan` 11 | 12 | Populate your message type. 13 | See files in this repo and [UAVCAN types](https://uavcan.org/Specification/7._List_of_standard_data_types/) for examples. -------------------------------------------------------------------------------- /uavcan_ros_bridge/dsdl/smarc_uavcan_messages/test.txt: -------------------------------------------------------------------------------- 1 | test 2 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/dsdl/smarc_uavcan_services/200.ArmEmergencySystems.uavcan: -------------------------------------------------------------------------------- 1 | # 2 | # Arm emergency systems 3 | # 4 | 5 | bool ARM = 1 6 | bool DISARM = 0 7 | bool command 8 | 9 | --- 10 | 11 | bool ARMED = 1 12 | bool DISARMED = 0 13 | bool state 14 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/dsdl/smarc_uavcan_services/README.md: -------------------------------------------------------------------------------- 1 | # smarc_uavcan_services 2 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/ros_to_uav/array_command.h: -------------------------------------------------------------------------------- 1 | #ifndef ROS_TO_UAV_ARRAY_COMMAND_H 2 | #define ROS_TO_UAV_ARRAY_COMMAND_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros_to_uav { 9 | 10 | template <> 11 | bool convert(const sam_msgs::ArrayCommand& ros_msg, uavcan::equipment::actuator::ArrayCommand& uav_msg); 12 | 13 | } 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/ros_to_uav/command.h: -------------------------------------------------------------------------------- 1 | #ifndef ROS_TO_UAV_COMMAND_H 2 | #define ROS_TO_UAV_COMMAND_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros_to_uav { 9 | 10 | template <> 11 | bool convert(const std_msgs::Float32& ros_msg, uavcan::equipment::actuator::ArrayCommand& uav_msg); 12 | 13 | } 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/ros_to_uav/rpm_command.h: -------------------------------------------------------------------------------- 1 | #ifndef ROS_TO_UAV_RPM_COMMAND_H 2 | #define ROS_TO_UAV_RPM_COMMAND_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros_to_uav { 9 | 10 | // TODO: convert ros msg to sam_msgs/ThrusterRPMs 11 | template <> 12 | bool convert(const std_msgs::Int32& ros_msg, uavcan::equipment::esc::RPMCommand& uav_msg); 13 | 14 | } 15 | 16 | #endif // ROS_TO_UAV_RPM_COMMAND_H 17 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/ros_to_uav/uavcan_node_info.h: -------------------------------------------------------------------------------- 1 | #ifndef UAVCAN_NODE_INFO_H 2 | #define UAVCAN_NODE_INFO_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros_to_uav { 9 | 10 | template <> 11 | bool convert_request(const uavcan_ros_msgs::UavcanGetNodeInfo::Request& ros_request, uavcan::protocol::GetNodeInfo::Request& uav_request); 12 | 13 | template <> 14 | bool convert_response(const uavcan::protocol::GetNodeInfo::Response& uav_response, uavcan_ros_msgs::UavcanGetNodeInfo::Response& ros_response); 15 | 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/ros_to_uav/uavcan_restart.h: -------------------------------------------------------------------------------- 1 | #ifndef UAVCAN_RESTART_H 2 | #define UAVCAN_RESTART_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros_to_uav { 9 | 10 | template <> 11 | bool convert_request(const uavcan_ros_msgs::UavcanRestartNode::Request& ros_request, uavcan::protocol::RestartNode::Request& uav_request); 12 | 13 | template <> 14 | bool convert_response(const uavcan::protocol::RestartNode::Response& uav_response, uavcan_ros_msgs::UavcanRestartNode::Response& ros_response); 15 | 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/ros_to_uav/uavcan_transport_stats.h: -------------------------------------------------------------------------------- 1 | #ifndef UAVCAN_TRANSPORT_STATS_H 2 | #define UAVCAN_TRANSPORT_STATS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros_to_uav { 9 | 10 | template <> 11 | bool convert_request(const uavcan_ros_msgs::UavcanGetTransportStats::Request& ros_request, uavcan::protocol::GetTransportStats::Request& uav_request); 12 | 13 | template <> 14 | bool convert_response(const uavcan::protocol::GetTransportStats::Response& uav_response, uavcan_ros_msgs::UavcanGetTransportStats::Response& ros_response); 15 | 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/battery_state.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_BATTERY_STATE_H 2 | #define UAV_TO_ROS_BATTERY_STATE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uav_to_ros { 9 | 10 | template <> 11 | bool convert(const uavcan::equipment::power::BatteryInfo& uav_msg, sensor_msgs::BatteryState& ros_msg); 12 | 13 | } 14 | 15 | #endif // UAV_TO_ROS_BATTERY_STATE_H 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/circuit_status.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_CIRCUIT_STATUS_H 2 | #define UAV_TO_ROS_CIRCUIT_STATUS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uav_to_ros { 9 | 10 | template <> 11 | bool convert(const uavcan::equipment::power::CircuitStatus& uav_msg, uavcan_ros_msgs::CircuitStatus& ros_msg); 12 | 13 | } 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/esc_status.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_ESC_STATUS_H 2 | #define UAV_TO_ROS_ESC_STATUS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uav_to_ros { 9 | 10 | template <> 11 | bool convert(const uavcan::ReceivedDataStructure& uav_msg, uavcan_ros_msgs::ESCStatus& ros_msg, unsigned char uid); 12 | 13 | } 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/gps_fix.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_GPS_FIX_H 2 | #define UAV_TO_ROS_GPS_FIX_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uav_to_ros { 9 | 10 | template <> 11 | bool convert(const uavcan::equipment::gnss::Fix& uav_msg, sensor_msgs::NavSatFix& ros_msg); 12 | 13 | } 14 | 15 | #endif // UAV_TO_ROS_GPS_FIX_H 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/imu.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_IMU_H 2 | #define UAV_TO_ROS_IMU_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uav_to_ros { 9 | 10 | template <> 11 | bool convert(const uavcan::equipment::ahrs::Solution& uav_msg, sensor_msgs::Imu& ros_msg); 12 | 13 | } 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/magnetic_field.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_MAGNETIC_FIELD_H 2 | #define UAV_TO_ROS_MAGNETIC_FIELD_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uav_to_ros { 9 | 10 | template <> 11 | bool convert(const uavcan::equipment::ahrs::MagneticFieldStrength& uav_msg, sensor_msgs::MagneticField& ros_msg); 12 | 13 | } 14 | 15 | #endif // UAV_TO_ROS_MAGNETIC_FIELD_H 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/pressure.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_PRESSURE_H 2 | #define UAV_TO_ROS_PRESSURE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace uav_to_ros { 9 | 10 | template <> 11 | bool convert(const uavcan::equipment::air_data::StaticPressure& uav_msg, sensor_msgs::FluidPressure& ros_msg); 12 | 13 | } 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uav_to_ros/sensor_pressure.h: -------------------------------------------------------------------------------- 1 | #ifndef UAV_TO_ROS_SENSOR_PRESSURE_H 2 | #define UAV_TO_ROS_SENSOR_PRESSURE_H 3 | 4 | //#include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace uav_to_ros { 10 | 11 | template <> 12 | bool convert(const uavcan::ReceivedDataStructure& uav_msg, sensor_msgs::FluidPressure& ros_msg, unsigned char uid); 13 | 14 | } 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/include/uavcan_ros_bridge/uavcan_ros_bridge.h: -------------------------------------------------------------------------------- 1 | #ifndef UAVCAN_ROS_BRIDGE_H 2 | #define UAVCAN_ROS_BRIDGE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace uav_to_ros { 10 | 11 | ros::Time convert_timestamp(const uavcan::Timestamp& uav_time) 12 | { 13 | ros::Time ros_time; 14 | ros_time.fromNSec(1000*uint64_t(uav_time.usec)); 15 | return ros_time; 16 | } 17 | 18 | template 19 | bool convert(const UAVMSG&, ROSMSG&) 20 | { 21 | //ROS_WARN("Can't find conversion for uavcan type %s", uav_msg.getDataTypeFullName()); 22 | static_assert(sizeof(UAVMSG) == -1 || sizeof(ROSMSG) == -1, "ERROR: You need to supply a convert specialization for the UAVCAN -> ROS msg types provided"); 23 | return false; 24 | } 25 | 26 | template 27 | bool convert(const uavcan::ReceivedDataStructure& uav_msg, ROSMSG& ros_msg, unsigned char uid) 28 | { 29 | uavcan::NodeID node_id = uav_msg.getSrcNodeID(); 30 | if (uid == 255 || uid == node_id.get()) { 31 | return convert((const UAVMSG&)uav_msg, ros_msg); 32 | } 33 | return false; 34 | } 35 | 36 | template 37 | class ConversionServer { 38 | public: 39 | typedef uavcan::ReceivedDataStructure ReceivedDataStructure; 40 | typedef uavcan::Node UavNode; 41 | typedef uavcan::MethodBinder UavMsgCallbackBinder; 42 | typedef uavcan::MethodBinder UavMsgExtendedBinder; 43 | 44 | //uavcan::Subscriber uav_sub; 45 | uavcan::Subscriber uav_sub; 46 | ros::Publisher ros_pub; 47 | unsigned char uid; 48 | 49 | ConversionServer(UavNode& uav_node, ros::NodeHandle& ros_node, const std::string& ros_topic, unsigned char uid=255) : uav_sub(uav_node), uid(uid) 50 | { 51 | ros_pub = ros_node.advertise(ros_topic, 10); 52 | 53 | //const int uav_sub_start_res = uav_sub.start(UavMsgCallbackBinder(this, &ConversionServer::conversion_callback)); 54 | const int uav_sub_start_res = uav_sub.start(UavMsgExtendedBinder(this, &ConversionServer::conversion_callback)); 55 | if (uav_sub_start_res < 0) { 56 | ROS_ERROR("Failed to start the uav subscriber, error: %d", uav_sub_start_res); 57 | } 58 | } 59 | 60 | void conversion_callback(const ReceivedDataStructure& uav_msg) const 61 | { 62 | ROSMSG ros_msg; 63 | bool success = convert(uav_msg, ros_msg, uid); 64 | if (success) { 65 | ros_pub.publish(ros_msg); 66 | } 67 | /* 68 | else { 69 | ROS_WARN("There was an error trying to convert uavcan type %s", uav_msg.getDataTypeFullName()); 70 | } 71 | */ 72 | } 73 | }; 74 | 75 | } 76 | 77 | namespace ros_to_uav { 78 | 79 | uavcan::Timestamp convert_timestamp(const ros::Time& ros_time) 80 | { 81 | uavcan::Timestamp uav_time; 82 | uint64_t nsec = ros_time.toNSec(); 83 | uav_time.usec = nsec / 1000; 84 | return uav_time; 85 | } 86 | 87 | template 88 | bool convert(const ROSMSG&, UAVMSG&) 89 | { 90 | //ROS_WARN("Can't find conversion for uavcan type %s", uav_msg.getDataTypeFullName()); 91 | static_assert(sizeof(UAVMSG) == -1 || sizeof(ROSMSG) == -1, "ERROR: You need to supply a convert specialization for the ROS -> UAVCAN msg types provided"); 92 | return false; 93 | } 94 | 95 | template 96 | bool convert(const ROSMSG& ros_msg, UAVMSG& uav_msg, unsigned char) 97 | { 98 | return convert(ros_msg, uav_msg); 99 | } 100 | 101 | template 102 | bool convert_request(const ROSREQ&, UAVREQ&) 103 | { 104 | static_assert(sizeof(UAVREQ) == -1 || sizeof(ROSREQ) == -1, "ERROR: You need to supply a convert specialization for the ROS -> UAVCAN service request"); 105 | return false; 106 | } 107 | 108 | template 109 | bool convert_response(const UAVRES&, ROSRES&) 110 | { 111 | static_assert(sizeof(UAVRES) == -1 || sizeof(ROSRES) == -1, "ERROR: You need to supply a convert specialization for the UAVCAN -> ROS service response"); 112 | return false; 113 | } 114 | 115 | template 116 | class ConversionServer { 117 | public: 118 | typedef uavcan::Node UavNode; 119 | 120 | uavcan::Publisher uav_pub; 121 | ros::Subscriber ros_sub; 122 | ros::ServiceServer ros_service; 123 | unsigned char uid; 124 | bool running; 125 | 126 | ConversionServer(UavNode& uav_node, ros::NodeHandle& ros_node, const std::string& ros_topic, unsigned char uid=0) : uav_pub(uav_node), uid(uid), running(true) 127 | { 128 | const int uav_pub_init_res = uav_pub.init(); 129 | if (uav_pub_init_res < 0) { 130 | ROS_ERROR("Failed to start the uav publisher, error: %d, type: %s", uav_pub_init_res, UAVMSG::getDataTypeFullName()); 131 | } 132 | ros_sub = ros_node.subscribe(ros_topic, 10, &ConversionServer::conversion_callback, this); 133 | std::string service_name = ros_sub.getTopic(); // ros_topic; 134 | std::replace(service_name.begin(), service_name.end(), '/', '_'); 135 | service_name = std::string("start_stop") + service_name; 136 | ros_service = ros_node.advertiseService(service_name, &ConversionServer::start_stop, this); 137 | ROS_INFO("Announcing service: %s", service_name.c_str()); 138 | } 139 | 140 | bool start_stop(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) 141 | { 142 | running = req.data; 143 | res.success = true; 144 | if (!running) { 145 | res.message = std::string("Stopping subscriber ") + ros_sub.getTopic(); 146 | } 147 | else { 148 | res.message = std::string("Starting subscriber ") + ros_sub.getTopic(); 149 | } 150 | return true; 151 | } 152 | 153 | void conversion_callback(const ROSMSG& ros_msg) 154 | { 155 | if (!running) { 156 | return; 157 | } 158 | UAVMSG uav_msg; 159 | bool success = convert(ros_msg, uav_msg, uid); 160 | if (success) { 161 | const int pub_res = uav_pub.broadcast(uav_msg); 162 | if (pub_res < 0) { 163 | ROS_WARN("There was an error broadcasting the uavcan type %s", uav_msg.getDataTypeFullName()); 164 | } 165 | } 166 | else { 167 | ROS_WARN("There was an error trying to convert uavcan type %s", uav_msg.getDataTypeFullName()); 168 | } 169 | } 170 | }; 171 | 172 | template 173 | class ServiceConversionServer { 174 | public: 175 | typedef uavcan::Node UavNode; 176 | 177 | ros::ServiceServer ros_service; 178 | UavNode& uav_node; 179 | uavcan::ServiceClient uav_client; 180 | 181 | ServiceConversionServer(UavNode& uav_node, ros::NodeHandle& ros_node, const std::string& ros_service_name) : uav_node(uav_node), uav_client(uav_node) //, node_id(node_id) 182 | { 183 | const int uav_client_init_res = uav_client.init(); 184 | if (uav_client_init_res < 0) { 185 | ROS_ERROR("Failed to start the uav publisher, error: %d, type: %s", uav_client_init_res, UAVSRV::getDataTypeFullName()); 186 | } 187 | uav_client.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(200)); 188 | uav_client.setPriority(uavcan::TransferPriority::OneHigherThanLowest); 189 | 190 | ros_service = ros_node.advertiseService(ros_service_name, &ServiceConversionServer::service_callback, this); 191 | } 192 | 193 | bool service_callback(typename ROSSRV::Request& ros_req, typename ROSSRV::Response& ros_res) 194 | { 195 | typename UAVSRV::Request uav_req; 196 | bool success = convert_request(ros_req, uav_req); 197 | if (!success) { 198 | ROS_WARN("There was an error trying to convert uavcan service %s", UAVSRV::getDataTypeFullName()); 199 | return false; 200 | } 201 | 202 | uav_client.setCallback([&](const uavcan::ServiceCallResult& uav_res) { 203 | if (uav_res.isSuccessful()) { 204 | success = convert_response((const typename UAVSRV::Response&)uav_res.getResponse(), ros_res); 205 | } 206 | else { 207 | ROS_WARN("There was an error call the uavcan service %s on node id %d", UAVSRV::getDataTypeFullName(), static_cast(uav_res.getCallID().server_node_id.get())); 208 | success = false; 209 | } 210 | }); 211 | 212 | const int uav_call_res = uav_client.call(ros_req.node_id, uav_req); 213 | if (uav_call_res < 0) { 214 | ROS_WARN("Unable to perform service call: %d", uav_call_res); 215 | return false; 216 | } 217 | 218 | uav_node.setModeOperational(); 219 | while (uav_client.hasPendingCalls()) { 220 | const int res = uav_node.spin(uavcan::MonotonicDuration::fromMSec(10)); 221 | if (res < 0) { 222 | ROS_WARN("Transient failure: %d", res); 223 | success = false; 224 | break; 225 | } 226 | } 227 | 228 | return success; 229 | } 230 | }; 231 | 232 | } 233 | 234 | #endif // UAVCAN_ROS_BRIDGE 235 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/launch/bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uavcan_ros_bridge 4 | 0.0.16 5 | The uavcan_ros_bridge package 6 | 7 | 8 | 9 | 10 | Nils Bore 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | sensor_msgs 55 | std_msgs 56 | std_srvs 57 | sam_msgs 58 | python-dev 59 | uavcan-pip 60 | uavcan_ros_msgs 61 | geometry_msgs 62 | roscpp 63 | sensor_msgs 64 | std_msgs 65 | uavcan_ros_msgs 66 | geometry_msgs 67 | roscpp 68 | sensor_msgs 69 | std_msgs 70 | std_srvs 71 | uavcan_ros_msgs 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uav/array_command.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace ros_to_uav { 4 | 5 | template <> 6 | bool convert(const sam_msgs::ArrayCommand& ros_msg, uavcan::equipment::actuator::ArrayCommand& uav_msg) 7 | { 8 | uav_msg.commands.resize(ros_msg.commands.size()); 9 | for (int i = 0; i < ros_msg.commands.size(); ++i) { 10 | uav_msg.commands[i].actuator_id = ros_msg.commands[i].actuator_id; 11 | uav_msg.commands[i].command_value = ros_msg.commands[i].command_value; 12 | uav_msg.commands[i].command_type = ros_msg.commands[i].command_type; 13 | } 14 | return true; 15 | } 16 | 17 | } 18 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uav/command.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace ros_to_uav { 4 | 5 | template <> 6 | bool convert(const std_msgs::Float32& ros_msg, uavcan::equipment::actuator::ArrayCommand& uav_msg) 7 | { 8 | uav_msg.commands.resize(1); 9 | uav_msg.commands[0].command_value = ros_msg.data; 10 | return true; 11 | } 12 | 13 | } 14 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uav/rpm_command.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace ros_to_uav { 4 | 5 | template <> 6 | bool convert(const std_msgs::Int32& ros_msg, uavcan::equipment::esc::RPMCommand& uav_msg) 7 | { 8 | uav_msg.rpm.resize(1); 9 | uav_msg.rpm[0] = ros_msg.data; 10 | return true; 11 | } 12 | 13 | } 14 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uav/uavcan_node_info.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace ros_to_uav { 4 | 5 | template <> 6 | bool convert_request(const uavcan_ros_msgs::UavcanGetNodeInfo::Request& ros_request, uavcan::protocol::GetNodeInfo::Request& uav_request) 7 | { 8 | return true; 9 | } 10 | 11 | template <> 12 | bool convert_response(const uavcan::protocol::GetNodeInfo::Response& uav_response, uavcan_ros_msgs::UavcanGetNodeInfo::Response& ros_response) 13 | { 14 | ros_response.name.resize(uav_response.name.size()); //= std::string((char*)&uav_response.name[0], 4); //uav_response.name.size()); 15 | for (int i = 0; i < uav_response.name.size(); ++i) { 16 | ros_response.name[i] = uav_response.name[i]; 17 | } 18 | 19 | ros_response.status.uptime_sec = uav_response.status.uptime_sec; 20 | ros_response.status.mode = uav_response.status.mode; 21 | ros_response.status.sub_mode = uav_response.status.sub_mode; 22 | ros_response.status.vendor_specific_status_code = uav_response.status.vendor_specific_status_code; 23 | return true; 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uav/uavcan_restart.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace ros_to_uav { 4 | 5 | #pragma GCC diagnostic ignored "-Wunused-parameter" 6 | template <> 7 | bool convert_request(const uavcan_ros_msgs::UavcanRestartNode::Request& ros_request, uavcan::protocol::RestartNode::Request& uav_request) 8 | { 9 | uav_request.magic_number = uav_request.MAGIC_NUMBER; 10 | return true; 11 | } 12 | 13 | template <> 14 | bool convert_response(const uavcan::protocol::RestartNode::Response& uav_response, uavcan_ros_msgs::UavcanRestartNode::Response& ros_response) 15 | { 16 | return true; 17 | } 18 | #pragma GCC diagnostic pop 19 | 20 | } 21 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uav/uavcan_transport_stats.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace ros_to_uav { 4 | 5 | template <> 6 | bool convert_request(const uavcan_ros_msgs::UavcanGetTransportStats::Request& ros_request, uavcan::protocol::GetTransportStats::Request& uav_request) 7 | { 8 | return true; 9 | } 10 | 11 | template <> 12 | bool convert_response(const uavcan::protocol::GetTransportStats::Response& uav_response, uavcan_ros_msgs::UavcanGetTransportStats::Response& ros_response) 13 | { 14 | ros_response.transfers_tx = uav_response.transfers_tx; 15 | ros_response.transfers_rx = uav_response.transfers_rx; 16 | ros_response.transfer_errors = uav_response.transfer_errors; 17 | 18 | return true; 19 | } 20 | 21 | } 22 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uavcan_bridge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | /* 13 | #include 14 | #include 15 | */ 16 | 17 | extern uavcan::ICanDriver& getCanDriver(const std::string&); 18 | extern uavcan::ISystemClock& getSystemClock(); 19 | 20 | constexpr unsigned NodeMemoryPoolSize = 16384; 21 | typedef uavcan::Node Node; 22 | 23 | static Node& getNode(const std::string& can_interface) 24 | { 25 | static Node node(getCanDriver(can_interface), getSystemClock()); 26 | return node; 27 | } 28 | 29 | int main(int argc, char** argv) 30 | { 31 | ros::init(argc, argv, "ros_to_uavcan_bridge_node"); 32 | ros::NodeHandle ros_node; 33 | 34 | int self_node_id; 35 | std::string can_interface; 36 | ros::param::param("~uav_node_id", self_node_id, 113); 37 | ros::param::param("~uav_can_interface", can_interface, "can0"); 38 | 39 | auto& uav_node = getNode(can_interface); 40 | uav_node.setNodeID(self_node_id); 41 | uav_node.setName("smarc.sam.uavcan_bridge.publisher"); 42 | 43 | /* 44 | * Dependent objects (e.g. publishers, subscribers, servers, callers, timers, ...) can be initialized only 45 | * if the node is running. Note that all dependent objects always keep a reference to the node object. 46 | */ 47 | const int node_start_res = uav_node.start(); 48 | if (node_start_res < 0) { 49 | ROS_ERROR("Failed to start the node; error: %d", node_start_res); 50 | exit(0); 51 | } 52 | 53 | ros::NodeHandle pn("~"); 54 | ros_to_uav::ConversionServer command_server(uav_node, pn, "command"); 55 | ros_to_uav::ConversionServer rpm_server(uav_node, pn, "rpm_command"); 56 | 57 | /* 58 | ros_to_uav::ConversionServer array_server(uav_node, pn, "array_command"); 59 | ros_to_uav::ConversionServer vbs_server(uav_node, pn, "vbs_command", 13); 60 | ros_to_uav::ConversionServer lcg_server(uav_node, pn, "lcg_command", 14); 61 | ros_to_uav::ConversionServer tcg_server1(uav_node, pn, "tcg_command1", 27); 62 | ros_to_uav::ConversionServer tcg_server2(uav_node, pn, "tcg_command2", 28); 63 | */ 64 | 65 | /* 66 | * Running the node. 67 | */ 68 | uav_node.setModeOperational(); 69 | 70 | std::function callback = [&] (const ros::TimerEvent&) { 71 | // Announce that the uav node is alive and well 72 | uav_node.spinOnce(); 73 | }; 74 | ros::Timer timer = ros_node.createTimer(ros::Duration(0.5), callback); 75 | 76 | ros::spin(); 77 | 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/ros_to_uavcan_services.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | extern uavcan::ICanDriver& getCanDriver(const std::string&); 12 | extern uavcan::ISystemClock& getSystemClock(); 13 | 14 | constexpr unsigned NodeMemoryPoolSize = 16384; 15 | typedef uavcan::Node Node; 16 | 17 | static Node& getNode(const std::string& can_interface) 18 | { 19 | static Node node(getCanDriver(can_interface), getSystemClock()); 20 | return node; 21 | } 22 | 23 | int main(int argc, char** argv) 24 | { 25 | ros::init(argc, argv, "ros_to_uavcan_services_node"); 26 | ros::NodeHandle ros_node; 27 | 28 | int self_node_id; 29 | std::string can_interface; 30 | ros::param::param("~uav_node_id", self_node_id, 115); 31 | ros::param::param("~uav_can_interface", can_interface, "can0"); 32 | 33 | auto& uav_node = getNode(can_interface); 34 | uav_node.setNodeID(self_node_id); 35 | uav_node.setName("smarc.sam.uavcan_bridge.service_caller"); 36 | 37 | /* 38 | * Dependent objects (e.g. publishers, subscribers, servers, callers, timers, ...) can be initialized only 39 | * if the node is running. Note that all dependent objects always keep a reference to the node object. 40 | */ 41 | const int node_start_res = uav_node.start(); 42 | if (node_start_res < 0) { 43 | ROS_ERROR("Failed to start the node; error: %d", node_start_res); 44 | exit(0); 45 | } 46 | 47 | ros::NodeHandle pn("~"); 48 | ros_to_uav::ServiceConversionServer node_info_server(uav_node, pn, "get_node_info"); 49 | ros_to_uav::ServiceConversionServer transport_stats_server(uav_node, pn, "get_transport_stats"); 50 | 51 | /* 52 | * Running the node. 53 | */ 54 | uav_node.setModeOperational(); 55 | 56 | std::function callback = [&] (const ros::TimerEvent&) { 57 | // Announce that the uav node is alive and well 58 | uav_node.spinOnce(); 59 | }; 60 | ros::Timer timer = ros_node.createTimer(ros::Duration(2.), callback); 61 | 62 | ros::AsyncSpinner spinner(4); // Use 4 threads 63 | spinner.start(); 64 | //ros::spin(); 65 | ros::waitForShutdown(); 66 | 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/battery_state.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::equipment::power::BatteryInfo& uav_msg, sensor_msgs::BatteryState& ros_msg) 7 | { 8 | ros_msg.voltage = uav_msg.voltage; 9 | ros_msg.current = uav_msg.current; 10 | // ros_msg.charge is in Ah 11 | ros_msg.charge = uav_msg.remaining_capacity_wh/uav_msg.voltage; 12 | // ros_msg.capacity is in Ah 13 | ros_msg.capacity = uav_msg.full_charge_capacity_wh/uav_msg.voltage; 14 | 15 | // TODO: add support for all the diagnostics flags 16 | 17 | return true; 18 | } 19 | 20 | } 21 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/circuit_status.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::equipment::power::CircuitStatus& uav_msg, uavcan_ros_msgs::CircuitStatus& ros_msg) 7 | { 8 | ros_msg.error_flags = uav_msg.error_flags; 9 | ros_msg.circuit_id = uav_msg.circuit_id; 10 | ros_msg.voltage = uav_msg.voltage; 11 | ros_msg.current = uav_msg.current; 12 | 13 | return true; 14 | } 15 | 16 | } 17 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/esc_status.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::ReceivedDataStructure& uav_msg, uavcan_ros_msgs::ESCStatus& ros_msg, unsigned char uid) 7 | { 8 | if (uid != 255 && uav_msg.esc_index != uid) { 9 | return false; 10 | } 11 | 12 | ros_msg.error_count = uav_msg.error_count; 13 | ros_msg.voltage = uav_msg.voltage; 14 | ros_msg.current = uav_msg.current; 15 | ros_msg.temperature = uav_msg.temperature; 16 | ros_msg.rpm = uav_msg.rpm; 17 | ros_msg.power_rating_pct = uav_msg.power_rating_pct; 18 | ros_msg.esc_index = uav_msg.esc_index; 19 | 20 | return true; 21 | } 22 | 23 | } 24 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/gps_fix.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::equipment::gnss::Fix& uav_msg, sensor_msgs::NavSatFix& ros_msg) 7 | { 8 | ros_msg.header.stamp = convert_timestamp(uav_msg.timestamp); 9 | ros_msg.latitude = 1e-8*double(uav_msg.latitude_deg_1e8); 10 | ros_msg.longitude = 1e-8*double(uav_msg.longitude_deg_1e8); 11 | ros_msg.altitude = 1e-3*double(uav_msg.height_ellipsoid_mm); 12 | 13 | if (uav_msg.status == uavcan::equipment::gnss::Fix::STATUS_NO_FIX) { 14 | ros_msg.status.status = ros_msg.status.STATUS_NO_FIX; 15 | } 16 | else { 17 | ros_msg.status.status = ros_msg.status.STATUS_FIX; 18 | } 19 | 20 | for (int i = 0; i < 9; ++i) { 21 | ros_msg.position_covariance[i] = uav_msg.position_covariance[i]; 22 | } 23 | 24 | return true; 25 | } 26 | 27 | } 28 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/imu.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::equipment::ahrs::Solution& uav_msg, sensor_msgs::Imu& ros_msg) 7 | { 8 | ros_msg.header.stamp = convert_timestamp(uav_msg.timestamp); 9 | ros_msg.orientation.x = uav_msg.orientation_xyzw[0]; 10 | ros_msg.orientation.y = uav_msg.orientation_xyzw[1]; 11 | ros_msg.orientation.z = uav_msg.orientation_xyzw[2]; 12 | ros_msg.orientation.w = uav_msg.orientation_xyzw[3]; 13 | ros_msg.linear_acceleration.x = uav_msg.linear_acceleration[0]; 14 | ros_msg.linear_acceleration.y = uav_msg.linear_acceleration[1]; 15 | ros_msg.linear_acceleration.z = uav_msg.linear_acceleration[2]; 16 | ros_msg.angular_velocity.x = uav_msg.angular_velocity[0]; 17 | ros_msg.angular_velocity.y = uav_msg.angular_velocity[1]; 18 | ros_msg.angular_velocity.z = uav_msg.angular_velocity[2]; 19 | return true; 20 | } 21 | 22 | } 23 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/magnetic_field.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::equipment::ahrs::MagneticFieldStrength& uav_msg, sensor_msgs::MagneticField& ros_msg) 7 | { 8 | ros_msg.magnetic_field.x = uav_msg.magnetic_field_ga[0]; 9 | ros_msg.magnetic_field.y = uav_msg.magnetic_field_ga[1]; 10 | ros_msg.magnetic_field.z = uav_msg.magnetic_field_ga[2]; 11 | 12 | for (int i = 0; i < 9; ++i) { 13 | ros_msg.magnetic_field_covariance[i] = uav_msg.magnetic_field_covariance[i]; 14 | } 15 | 16 | return true; 17 | } 18 | 19 | } 20 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/pressure.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::equipment::air_data::StaticPressure& uav_msg, sensor_msgs::FluidPressure& ros_msg) 7 | { 8 | ros_msg.header.stamp = ros::Time::now(); 9 | ros_msg.fluid_pressure = uav_msg.static_pressure; 10 | ros_msg.variance = uav_msg.static_pressure_variance; 11 | return true; 12 | } 13 | 14 | } 15 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uav_to_ros/sensor_pressure.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace uav_to_ros { 4 | 5 | template <> 6 | bool convert(const uavcan::ReceivedDataStructure& uav_msg, sensor_msgs::FluidPressure& ros_msg, unsigned char uid) 7 | { 8 | if (uid != 255 && uav_msg.device_id != uid) { 9 | return false; 10 | } 11 | 12 | ros_msg.header.stamp = ros::Time::now(); 13 | ros_msg.fluid_pressure = uav_msg.pressure.static_pressure; 14 | ros_msg.variance = uav_msg.pressure.static_pressure_variance; 15 | return true; 16 | } 17 | 18 | } 19 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uavcan_platform_linux.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uavcan::ISystemClock& getSystemClock() 4 | { 5 | static uavcan_linux::SystemClock clock; 6 | return clock; 7 | } 8 | 9 | uavcan::ICanDriver& getCanDriver(const std::string& can_interface="can0") 10 | { 11 | static uavcan_linux::SocketCanDriver driver(dynamic_cast(getSystemClock())); 12 | if (driver.getNumIfaces() == 0) // Will be executed once 13 | { 14 | if (driver.addIface(can_interface) < 0) 15 | { 16 | throw std::runtime_error("Failed to add iface"); 17 | } 18 | } 19 | return driver; 20 | } 21 | -------------------------------------------------------------------------------- /uavcan_ros_bridge/src/uavcan_to_ros_bridge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | /* 18 | #include 19 | */ 20 | 21 | extern uavcan::ICanDriver& getCanDriver(const std::string&); 22 | extern uavcan::ISystemClock& getSystemClock(); 23 | 24 | constexpr unsigned NodeMemoryPoolSize = 16384; 25 | typedef uavcan::Node Node; 26 | 27 | static Node& getNode(const std::string& can_interface) 28 | { 29 | static Node node(getCanDriver(can_interface), getSystemClock()); 30 | return node; 31 | } 32 | 33 | int main(int argc, char** argv) 34 | { 35 | ros::init(argc, argv, "uavcan_to_ros_bridge_node"); 36 | ros::NodeHandle ros_node; 37 | 38 | int self_node_id; 39 | std::string can_interface; 40 | ros::param::param("~uav_node_id", self_node_id, 114); 41 | ros::param::param("~uav_can_interface", can_interface, "can0"); 42 | 43 | auto& uav_node = getNode(can_interface); 44 | uav_node.setNodeID(self_node_id); 45 | uav_node.setName("smarc.sam.uavcan_bridge.subscriber"); 46 | 47 | /* 48 | * Configuring the Data Type IDs. 49 | * See the server sources for details. 50 | */ 51 | /* 52 | auto regist_result = uavcan::GlobalDataTypeRegistry::instance().registerDataType(243); 53 | if (regist_result != uavcan::GlobalDataTypeRegistry::RegistrationResultOk) { 54 | ROS_ERROR("Failed to register the data type: %d", regist_result); 55 | exit(0); 56 | } 57 | */ 58 | 59 | /* 60 | * Dependent objects (e.g. publishers, subscribers, servers, callers, timers, ...) can be initialized only 61 | * if the node is running. Note that all dependent objects always keep a reference to the node object. 62 | */ 63 | const int node_start_res = uav_node.start(); 64 | if (node_start_res < 0) { 65 | ROS_ERROR("Failed to start the node; error: %d", node_start_res); 66 | exit(0); 67 | } 68 | 69 | ros::NodeHandle pn("~"); 70 | uav_to_ros::ConversionServer imu_server(uav_node, pn, "imu"); 71 | uav_to_ros::ConversionServer gps_server(uav_node, pn, "gps_fix"); 72 | uav_to_ros::ConversionServer battery_server(uav_node, pn, "battery_state"); 73 | uav_to_ros::ConversionServer magnetic_server(uav_node, pn, "magnetic_field"); 74 | // NOTE: the last argument is the source node id numbers, these are example values 75 | uav_to_ros::ConversionServer pressure_server1(uav_node, pn, "pressure1", 91); 76 | uav_to_ros::ConversionServer pressure_server2(uav_node, pn, "pressure2", 93); 77 | 78 | uav_to_ros::ConversionServer sensor_pressure_server1(uav_node, pn, "sensor_pressure1", 1); 79 | uav_to_ros::ConversionServer sensor_pressure_server2(uav_node, pn, "sensor_pressure2", 2); 80 | uav_to_ros::ConversionServer motor_oil_pressure_server(uav_node, pn, "motor_oil_pressure", 20); 81 | uav_to_ros::ConversionServer esc_status_server(uav_node, pn, "esc_status"); 82 | 83 | /* 84 | uav_to_ros::ConversionServer vbs_feedback_server(uav_node, pn, "vbs_feedback", 13); 85 | uav_to_ros::ConversionServer lcg_feedback_server(uav_node, pn, "lcg_feedback", 14); 86 | */ 87 | 88 | /* 89 | * Running the node. 90 | */ 91 | uav_node.setModeOperational(); 92 | signal(SIGINT, [] (int) { ros::shutdown(); }); 93 | 94 | while (ros::ok()) { 95 | /* 96 | * The method spin() may return earlier if an error occurs (e.g. driver failure). 97 | * All error codes are listed in the header uavcan/error.hpp. 98 | */ 99 | const int res = uav_node.spin(uavcan::MonotonicDuration::getInfinite()); 100 | if (res < 0) { 101 | ROS_ERROR("Transient failure or shutdown: %d", res); 102 | } 103 | } 104 | 105 | return 0; 106 | } 107 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uavcan_ros_msgs) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | set(CMAKE_CXX_FLAGS "-Wall -Wextra -pedantic -std=c++11") 8 | 9 | ## Find catkin macros and libraries 10 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 11 | ## is used, also find other catkin packages 12 | find_package(catkin REQUIRED COMPONENTS 13 | sam_msgs 14 | geometry_msgs 15 | roscpp 16 | sensor_msgs 17 | std_msgs 18 | std_srvs 19 | message_generation 20 | message_runtime 21 | ) 22 | 23 | ## Generate messages in the 'msg' folder 24 | add_message_files( 25 | FILES 26 | UavcanNodeStatus.msg 27 | UavcanNodeStatusNamed.msg 28 | UavcanNodeStatusNamedArray.msg 29 | ESCStatus.msg 30 | CircuitStatus.msg 31 | ) 32 | 33 | ## Generate services in the 'srv' folder 34 | add_service_files( 35 | FILES 36 | UavcanGetNodeInfo.srv 37 | UavcanGetTransportStats.srv 38 | UavcanRestartNode.srv 39 | ) 40 | 41 | ## Generate added messages and services with any dependencies listed here 42 | generate_messages( 43 | DEPENDENCIES 44 | std_msgs 45 | ) 46 | 47 | ################################### 48 | ## catkin specific configuration ## 49 | ################################### 50 | ## The catkin_package macro generates cmake config files for your package 51 | ## Declare things to be passed to dependent projects 52 | ## INCLUDE_DIRS: uncomment this if your package contains header files 53 | ## LIBRARIES: libraries you create in this project that dependent projects also need 54 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 55 | ## DEPENDS: system dependencies of this project that dependent projects also need 56 | catkin_package( 57 | # INCLUDE_DIRS 58 | # LIBRARIES 59 | # CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs 60 | # CFG_EXTRAS 61 | ) 62 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/msg/CircuitStatus.msg: -------------------------------------------------------------------------------- 1 | # 2 | # From uavcan CiruitStatus msg: Generic electrical circuit info. 3 | # 4 | 5 | uint8 ERROR_FLAG_OVERVOLTAGE = 1 6 | uint8 ERROR_FLAG_UNDERVOLTAGE = 2 7 | uint8 ERROR_FLAG_OVERCURRENT = 4 8 | uint8 ERROR_FLAG_UNDERCURRENT = 8 9 | 10 | uint8 error_flags 11 | 12 | uint16 circuit_id 13 | 14 | float32 voltage 15 | float32 current 16 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/msg/ESCStatus.msg: -------------------------------------------------------------------------------- 1 | # 2 | # Generic ESC status. 3 | # Unknown fields should be set to NAN. 4 | # 5 | 6 | uint32 error_count # Resets when the motor restarts 7 | 8 | float32 voltage # Volt 9 | float32 current # Ampere. Can be negative in case of a regenerative braking. 10 | float32 temperature # Kelvin 11 | 12 | int32 rpm # Negative value indicates reverse rotation 13 | 14 | uint8 power_rating_pct # Instant demand factor in percent (percent of maximum power); range 0% to 127%. 15 | 16 | uint8 esc_index 17 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/msg/UavcanNodeStatus.msg: -------------------------------------------------------------------------------- 1 | # 2 | # Abstract node status information. 3 | # 4 | # Any UAVCAN node is required to publish this message periodically. 5 | # 6 | 7 | # 8 | # Publication period may vary within these limits. 9 | # It is NOT recommended to change it at run time. 10 | # 11 | uint16 MAX_BROADCASTING_PERIOD_MS = 1000 12 | uint16 MIN_BROADCASTING_PERIOD_MS = 2 13 | 14 | # 15 | # If a node fails to publish this message in this amount of time, it should be considered offline. 16 | # 17 | uint16 OFFLINE_TIMEOUT_MS = 3000 18 | 19 | # 20 | # Uptime counter should never overflow. 21 | # Other nodes may detect that a remote node has restarted when this value goes backwards. 22 | # 23 | uint32 uptime_sec 24 | 25 | # 26 | # Abstract node health. 27 | # 28 | uint8 HEALTH_OK = 0 # The node is functioning properly. 29 | uint8 HEALTH_WARNING = 1 # A critical parameter went out of range or the node encountered a minor failure. 30 | uint8 HEALTH_ERROR = 2 # The node encountered a major failure. 31 | uint8 HEALTH_CRITICAL = 3 # The node suffered a fatal malfunction. 32 | uint8 health 33 | 34 | # 35 | # Current mode. 36 | # 37 | # Mode OFFLINE can be actually reported by the node to explicitly inform other network 38 | # participants that the sending node is about to shutdown. In this case other nodes will not 39 | # have to wait OFFLINE_TIMEOUT_MS before they detect that the node is no longer available. 40 | # 41 | # Reserved values can be used in future revisions of the specification. 42 | # 43 | uint8 MODE_OPERATIONAL = 0 # Node is performing its main functions. 44 | uint8 MODE_INITIALIZATION = 1 # Node is initializing; this mode is entered immediately after startup. 45 | uint8 MODE_MAINTENANCE = 2 # Node is under maintenance. 46 | uint8 MODE_SOFTWARE_UPDATE = 3 # Node is in the process of updating its software. 47 | uint8 MODE_OFFLINE = 7 # Node is no longer available. 48 | uint8 mode 49 | 50 | # 51 | # Not used currently, keep zero when publishing, ignore when receiving. 52 | # 53 | uint8 sub_mode 54 | 55 | # 56 | # Optional, vendor-specific node status code, e.g. a fault code or a status bitmask. 57 | # 58 | uint16 vendor_specific_status_code 59 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/msg/UavcanNodeStatusNamed.msg: -------------------------------------------------------------------------------- 1 | # 2 | # Node ID 3 | # 4 | uint8 id 5 | 6 | # 7 | # Node name 8 | # 9 | string name 10 | 11 | # 12 | # NodeStatus 13 | # 14 | uavcan_ros_msgs/UavcanNodeStatus ns 15 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/msg/UavcanNodeStatusNamedArray.msg: -------------------------------------------------------------------------------- 1 | uavcan_ros_msgs/UavcanNodeStatusNamed[] array 2 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uavcan_ros_msgs 4 | 0.0.16 5 | The uavcan_ros_msgs package 6 | 7 | 8 | 9 | 10 | Nils Bore 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | sensor_msgs 55 | std_msgs 56 | std_srvs 57 | sam_msgs 58 | geometry_msgs 59 | roscpp 60 | sensor_msgs 61 | std_msgs 62 | geometry_msgs 63 | roscpp 64 | sensor_msgs 65 | std_msgs 66 | std_srvs 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/srv/UavcanGetDataTypeInfo.srv: -------------------------------------------------------------------------------- 1 | # 2 | # Get the implementation details of a given data type. 3 | # 4 | # Request is interpreted as follows: 5 | # - If the field 'name' is empty, the fields 'kind' and 'id' will be used to identify the data type. 6 | # - If the field 'name' is non-empty, it will be used to identify the data type; the 7 | # fields 'kind' and 'id' will be ignored. 8 | # 9 | 10 | # 11 | # Data type kind (message or service). 12 | # 13 | 14 | uint8 SERVICE = 0 15 | uint8 MESSAGE = 1 16 | 17 | uint16 id # Ignored if 'name' is non-empty 18 | uint8 kind # MESSAGE or SERVICE 19 | 20 | string name # Full data type name, e.g. "uavcan.protocol.GetDataTypeInfo" 21 | 22 | --- 23 | 24 | uint64 signature # Data type signature; valid only if the data type is known (see FLAG_KNOWN) 25 | 26 | uint16 id # Valid only if the data type is known (see FLAG_KNOWN) 27 | uint8 kind # MESSAGE or SERVICE 28 | 29 | uint8 FLAG_KNOWN = 1 # This data type is defined 30 | uint8 FLAG_SUBSCRIBED = 2 # Subscribed to messages of this type 31 | uint8 FLAG_PUBLISHING = 4 # Publishing messages of this type 32 | uint8 FLAG_SERVING = 8 # Providing service of this type 33 | uint8 flags 34 | 35 | string name # Full data type name 36 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/srv/UavcanGetNodeInfo.srv: -------------------------------------------------------------------------------- 1 | # 2 | # Full node info request. 3 | # Note that all fields of the response section are byte-aligned. 4 | # 5 | uint8 node_id 6 | 7 | --- 8 | 9 | # 10 | # Current node status 11 | # 12 | UavcanNodeStatus status 13 | 14 | # 15 | # Version information shall not be changed while the node is running. 16 | # 17 | #SoftwareVersion software_version # TODO: add this back 18 | #HardwareVersion hardware_version # TODO: add this back 19 | 20 | # 21 | # Human readable non-empty ASCII node name. 22 | # Node name shall not be changed while the node is running. 23 | # Empty string is not a valid node name. 24 | # Allowed characters are: a-z (lowercase ASCII letters) 0-9 (decimal digits) . (dot) - (dash) _ (underscore). 25 | # Node name is a reversed internet domain name (like Java packages), e.g. "com.manufacturer.project.product". 26 | # 27 | string name 28 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/srv/UavcanGetTransportStats.srv: -------------------------------------------------------------------------------- 1 | # 2 | # Get transport statistics. 3 | # 4 | uint8 node_id 5 | 6 | --- 7 | 8 | # 9 | # UAVCAN transport layer statistics. 10 | # 11 | uint64 transfers_tx # Number of transmitted transfers. 12 | uint64 transfers_rx # Number of received transfers. 13 | uint64 transfer_errors # Number of errors detected in the UAVCAN transport layer. 14 | 15 | # 16 | # CAN bus statistics, for each interface independently. 17 | # 18 | #CANIfaceStats[<=3] can_iface_stats 19 | -------------------------------------------------------------------------------- /uavcan_ros_msgs/srv/UavcanRestartNode.srv: -------------------------------------------------------------------------------- 1 | # 2 | # Restart the node. 3 | # 4 | # Some nodes may require restart before the new configuration will be applied. 5 | # 6 | uint8 node_id 7 | 8 | --- 9 | 10 | bool ok 11 | --------------------------------------------------------------------------------