diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml
new file mode 100644
index 0000000..61fcc05
--- /dev/null
+++ b/.github/workflows/ros2.yml
@@ -0,0 +1,51 @@
+name: ros2
+
+on:
+ push:
+ paths:
+ - ".github/workflows/ros2.yml"
+ - "include/**"
+ - "launch/**"
+ - "src/**"
+ - "CMakeLists.txt"
+ - "package.xml"
+ pull_request:
+ paths:
+ - ".github/workflows/ros2.yml"
+ - "include/**"
+ - "launch/**"
+ - "src/**"
+ - "CMakeLists.txt"
+ - "package.xml"
+
+env:
+ BUILD_TYPE: Release
+
+jobs:
+ build:
+ name: Build on ros2 ${{ matrix.ros_distro }}
+ runs-on: ubuntu-22.04
+ strategy:
+ matrix:
+ ros_distro: [ humble ]
+
+ steps:
+ - uses: ros-tooling/setup-ros@v0.7
+ with:
+ required-ros-distributions: ${{ matrix.ros_distro }}
+
+ - name: Setup ros2 workspace
+ run: |
+ mkdir -p ${{github.workspace}}/ros2_ws/src
+
+ - uses: actions/checkout@v4
+ with:
+ path: 'ros2_ws/src/pika-spark-bno085-driver'
+ submodules: true
+ fetch-depth: 1
+
+ - name: colcon build
+ run: |
+ source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+ cd ${{github.workspace}}/ros2_ws
+ colcon build --event-handlers console_direct+
diff --git a/.github/workflows/smoke-test.yml b/.github/workflows/smoke-test.yml
deleted file mode 100644
index 0025ac4..0000000
--- a/.github/workflows/smoke-test.yml
+++ /dev/null
@@ -1,31 +0,0 @@
-name: Build
-
-# See: https://docs.github.com/en/actions/reference/events-that-trigger-workflows
-on:
- push:
- pull_request:
- schedule:
- # Run every Tuesday at 8 AM UTC
- - cron: "0 8 * * TUE"
- workflow_dispatch:
- repository_dispatch:
-
-permissions:
- contents: read
-
-jobs:
- smoke-test:
- runs-on: ubuntu-latest
-
- steps:
- - name: Checkout repository
- uses: actions/checkout@v4
- with:
- submodules: true
- fetch-depth: 1
-
- - name: Install CMake
- run: sudo apt-get install cmake
-
- - name: Create build directory, run CMake and Make
- run: mkdir build && cd build && cmake .. && make
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b14106a..3ea4772 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,11 +1,16 @@
##########################################################################
cmake_minimum_required(VERSION 3.15)
##########################################################################
-project("pika-spark-bno085-driver")
+project("pika_spark_bno085_driver")
+set(PIKA_SPARK_BNO085_TARGET ${PROJECT_NAME}_node)
+##########################################################################
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
##########################################################################
add_subdirectory(sh2)
##########################################################################
-add_executable(${PROJECT_NAME}
+add_executable(${PIKA_SPARK_BNO085_TARGET}
main.cpp
bno085.cpp
bno085_hal.cpp
@@ -14,10 +19,17 @@ add_executable(${PROJECT_NAME}
)
##########################################################################
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Werror -Wextra -Wpedantic)
+ target_compile_options(${PIKA_SPARK_BNO085_TARGET} PRIVATE -Wall -Werror -Wextra -Wpedantic)
endif()
##########################################################################
-target_link_libraries(${PROJECT_NAME} pika-spark-sh2)
+target_link_libraries(${PIKA_SPARK_BNO085_TARGET} pika-spark-sh2)
+##########################################################################
+target_compile_features(${PIKA_SPARK_BNO085_TARGET} PUBLIC cxx_std_17)
+##########################################################################
+ament_target_dependencies(${PIKA_SPARK_BNO085_TARGET} rclcpp sensor_msgs)
+##########################################################################
+install(TARGETS ${PIKA_SPARK_BNO085_TARGET} DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
##########################################################################
-target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+ament_package()
##########################################################################
diff --git a/README.md b/README.md
index 599440e..019fdc5 100644
--- a/README.md
+++ b/README.md
@@ -1,18 +1,35 @@
:sparkles: `pika-spark-bno085-driver`
=====================================
-[](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/smoke-test.yml)
+[](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/ros2.yml)
[](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/spell-check.yml)
-Linux user space driver for the [BNO085](https://www.ceva-dsp.com/wp-content/uploads/2019/10/BNO080_085-Datasheet.pdf) 9-DoF IMU driver.
+Linux user space ROS driver for the [BNO085](https://www.ceva-dsp.com/wp-content/uploads/2019/10/BNO080_085-Datasheet.pdf) 9-DoF IMU.
+
+**Note**: In order to run the BNO085 ROS driver on [Pika Spark](https://pika-spark.io/) take a look at ready-to-use build/run scripts at [pika-spark-container/ros-imu-bno085](https://github.com/pika-spark/pika-spark-containers/tree/main/ros-imu-bno085).
-### How-to-build/run
+### How-to-build
+```bash
+cd $COLCON_WS/src
+git clone --recursive https://github.com/pika-spark/pika-spark-bno085-driver
+cd $COLCON_WS
+source /opt/ros/humble/setup.bash
+colcon build --packages-select pika_spark_bno085_driver
+```
+
+#### How-to-run
+```bash
+cd $COLCON_WS
+. install/setup.bash
+ros2 launch pika_spark_bno085_driver imu.py
+```
+
+#### How-to-visualize your IMU data
```bash
-git clone https://github.com/pika-spark/pika-spark-bno085-driver && cd pika-spark-bno085-driver/docker
-./docker-build.sh
-sudo ./docker-run.sh
+sudo apt-get install ros-humble-imu-tools
+ros2 run rviz2 rviz2
```
diff --git a/bno085.cpp b/bno085.cpp
index 1c946ec..72558b7 100644
--- a/bno085.cpp
+++ b/bno085.cpp
@@ -19,9 +19,13 @@
BNO085::BNO085(std::shared_ptr const spi,
std::shared_ptr const nirq,
- StabilizedRotationVectorWAccuracyCallbackFunc const sensor_func)
+ LinearAccelerationCallbackFunc const acc_callback,
+ CalibratedGyroscopeCallbackFunc const gyro_callback,
+ StabilizedRotationVectorWAccuracyCallbackFunc const attitude_callback)
: BNO085_HAL{spi, nirq}
-, _sensor_func{sensor_func}
+, _acc_callback{acc_callback}
+, _gyro_callback{gyro_callback}
+, _attitude_callback{attitude_callback}
{
}
@@ -30,7 +34,43 @@ BNO085::BNO085(std::shared_ptr const spi,
* PUBLIC MEMBER FUNCTIONS
**************************************************************************************/
-int BNO085::config()
+int BNO085::enableAccelerometer()
+{
+ sh2_SensorConfig_t const bno085_config =
+ {
+ /* .changeSensitivityEnabled = */ false,
+ /* .changeSensitivityRelative = */ false,
+ /* .wakeupEnabled = */ false,
+ /* .alwaysOnEnabled = */ false,
+ /* .sniffEnabled = */ false,
+ /* .changeSensitivity = */ 0,
+ /* .reportInterval_us = */ 5000, /* 200 Hz */
+ /* .batchInterval_us = */ 0,
+ /* .sensorSpecific = */ 0
+ };
+
+ return sh2_setSensorConfig(SH2_LINEAR_ACCELERATION, &bno085_config);
+}
+
+int BNO085::enableGyroscope()
+{
+ sh2_SensorConfig_t const bno085_config =
+ {
+ /* .changeSensitivityEnabled = */ false,
+ /* .changeSensitivityRelative = */ false,
+ /* .wakeupEnabled = */ false,
+ /* .alwaysOnEnabled = */ false,
+ /* .sniffEnabled = */ false,
+ /* .changeSensitivity = */ 0,
+ /* .reportInterval_us = */ 5000, /* 200 Hz */
+ /* .batchInterval_us = */ 0,
+ /* .sensorSpecific = */ 0
+ };
+
+ return sh2_setSensorConfig(SH2_GYROSCOPE_CALIBRATED, &bno085_config);
+}
+
+int BNO085::enableAttitude()
{
sh2_SensorConfig_t const bno085_config =
{
@@ -64,10 +104,12 @@ void BNO085::internal_sh2_sensor_callback(sh2_SensorEvent_t * event)
if (auto const rc = sh2_decodeSensorEvent(&sensor_value, event); rc != SH2_OK)
throw BNO085_Exception("error decoding sensor event, rc = %d", rc);
- if (sensor_value.sensorId == SH2_ARVR_STABILIZED_RV) {
- if (_sensor_func)
- _sensor_func(sensor_value.un.arvrStabilizedRV);
- }
+ if (sensor_value.sensorId == SH2_LINEAR_ACCELERATION)
+ _acc_callback(sensor_value.un.linearAcceleration);
+ else if (sensor_value.sensorId == SH2_GYROSCOPE_CALIBRATED)
+ _gyro_callback(sensor_value.un.gyroscope);
+ else if (sensor_value.sensorId == SH2_ARVR_STABILIZED_RV)
+ _attitude_callback(sensor_value.un.arvrStabilizedRV);
else
throw BNO085_Exception("unhandled sensor event decoded, sensorId = %d", sensor_value.sensorId);
}
diff --git a/bno085.h b/bno085.h
index 4d13492..4d81b9f 100644
--- a/bno085.h
+++ b/bno085.h
@@ -24,15 +24,21 @@
class BNO085 : private BNO085_HAL
{
public:
+ typedef std::function LinearAccelerationCallbackFunc;
+ typedef std::function CalibratedGyroscopeCallbackFunc;
typedef std::function StabilizedRotationVectorWAccuracyCallbackFunc;
BNO085(std::shared_ptr const spi,
std::shared_ptr const nirq,
- StabilizedRotationVectorWAccuracyCallbackFunc const sensor_func);
+ LinearAccelerationCallbackFunc const acc_callback,
+ CalibratedGyroscopeCallbackFunc const gyro_callback,
+ StabilizedRotationVectorWAccuracyCallbackFunc const attitude_callback);
virtual ~BNO085() { }
- int config();
+ int enableAccelerometer();
+ int enableGyroscope();
+ int enableAttitude();
void spinOnce();
@@ -41,5 +47,7 @@ class BNO085 : private BNO085_HAL
private:
- StabilizedRotationVectorWAccuracyCallbackFunc const _sensor_func;
+ LinearAccelerationCallbackFunc const _acc_callback;
+ CalibratedGyroscopeCallbackFunc const _gyro_callback;
+ StabilizedRotationVectorWAccuracyCallbackFunc const _attitude_callback;
};
diff --git a/docker/Dockerfile b/docker/Dockerfile
deleted file mode 100644
index 6990496..0000000
--- a/docker/Dockerfile
+++ /dev/null
@@ -1,17 +0,0 @@
-# Copyright Alexander Entinger, MSc / LXRobotics GmbH
-# This docker file allows building and running of 10BASE-T1S user space driver for the onsemi NCN26010 SPI-MAC-PHY.
-FROM alpine:3.18
-
-RUN apk add git g++ make cmake linux-headers
-
-RUN cd /tmp && \
- git clone --recursive https://github.com/pika-spark/pika-spark-bno085-driver && \
- cd pika-spark-bno085-driver && \
- mkdir build && \
- cd build && \
- cmake .. && \
- make
-
-COPY start.sh /
-RUN chmod ugo+x /start.sh
-ENTRYPOINT ["/start.sh"]
diff --git a/docker/docker-build.sh b/docker/docker-build.sh
deleted file mode 100755
index 4cc6b66..0000000
--- a/docker/docker-build.sh
+++ /dev/null
@@ -1,4 +0,0 @@
-#!/bin/bash
-set -euo pipefail
-IFS=$'\n\t'
-docker build --pull --no-cache --tag pika_spark_bno085_driver .
diff --git a/docker/docker-run.sh b/docker/docker-run.sh
deleted file mode 100755
index 894b577..0000000
--- a/docker/docker-run.sh
+++ /dev/null
@@ -1,32 +0,0 @@
-#!/bin/bash
-set -euo pipefail
-IFS=$'\n\t'
-
-if [ "$(id -u)" != "0" ]; then
- echo "This script must be run as root."
- exit 1
-fi
-
-# SYSFS_GPIO_NUMBER = ((GPIO_PORT - 1) * 32) + GPIO_PIN
-# BNO085_nIRQ = MX8MM_IOMUXC_SPDIF_TX_GPIO5_IO3
-# BNO085_nRST = MX8MM_IOMUXC_SAI5_RXD1_GPIO3_IO22
-# BNO085_nBOOT = MX8MM_IOMUXC_SAI5_RXD2_GPIO3_IO23
-GPIO_NIRQ_NUM=131
-GPIO_NRST_NUM=86
-GPIO_NBOOT_NUM=87
-
-function finish {
- echo $GPIO_NIRQ_NUM > /sys/class/gpio/unexport
- echo $GPIO_NRST_NUM > /sys/class/gpio/unexport
- echo $GPIO_NBOOT_NUM > /sys/class/gpio/unexport
-}
-trap finish EXIT
-
-echo $GPIO_NIRQ_NUM > /sys/class/gpio/export
-echo $GPIO_NRST_NUM > /sys/class/gpio/export
-echo $GPIO_NBOOT_NUM > /sys/class/gpio/export
-
-modprobe spidev
-chmod ugo+rw /dev/spidev0.0
-
-sudo -u fio docker run -it --ulimit nofile=1024:1024 --rm -u 0 --privileged --device /dev/spidev0.0 -v /sys/class/gpio:/sys/class/gpio pika_spark_bno085_driver
diff --git a/docker/start.sh b/docker/start.sh
deleted file mode 100755
index aa7e26f..0000000
--- a/docker/start.sh
+++ /dev/null
@@ -1,4 +0,0 @@
-#!/bin/sh
-export PATH="$PATH:/tmp/pika-spark-bno085-driver/build"
-echo "Starting pika-spark-bno085-driver ..."
-pika-spark-bno085-driver
diff --git a/launch/imu.py b/launch/imu.py
new file mode 100644
index 0000000..5f350c8
--- /dev/null
+++ b/launch/imu.py
@@ -0,0 +1,20 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '[{severity}] [{time}]: {message}'
+
+ return LaunchDescription([
+ Node(
+ package='pika_spark_bno085_driver',
+ executable='pika_spark_bno085_driver_node',
+ name='pika_spark_bno085_driver',
+ namespace='',
+ output='screen',
+ emulate_tty=True,
+ parameters=[
+ {'imu_topic': 'imu'},
+ ]
+ )
+ ])
diff --git a/main.cpp b/main.cpp
index f4538c3..8c65ebf 100644
--- a/main.cpp
+++ b/main.cpp
@@ -17,6 +17,11 @@
#include
#include
+#include
+#include
+
+#include
+
#include "spi.h"
#include "gpio-sysfs.h"
@@ -35,8 +40,26 @@ static int constexpr nIRQ_PIN = 131;
* MAIN
**************************************************************************************/
-int main(int /* argc */, char ** /* argv */) try
+int main(int argc, char ** argv) try
{
+ rclcpp::init(argc, argv);
+
+ auto const node = rclcpp::Node::make_shared("pika_spark_bno085_driver_node");
+
+ rclcpp::QoS imu_qos_profile(rclcpp::KeepLast(1), rmw_qos_profile_sensor_data);
+
+ node->declare_parameter("imu_topic", "joy");
+ auto const imu_topic = node->get_parameter("imu_topic").as_string();
+ auto const imu_topic_deadline = std::chrono::milliseconds(100);
+ auto const imu_topic_liveliness_lease_duration = std::chrono::milliseconds(1000);
+
+ imu_qos_profile.deadline(imu_topic_deadline);
+ imu_qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC);
+ imu_qos_profile.liveliness_lease_duration(imu_topic_liveliness_lease_duration);
+
+ sensor_msgs::msg::Imu imu_msg;
+ auto const imu_pub = node->create_publisher(imu_topic, imu_qos_profile);
+
auto const gpio_nboot = std::make_shared(nBOOT_PIN);
gpio_nboot->gpio_set_dir(true);
gpio_nboot->gpio_set_value(1); /* Note: setting it to '0' activates bootloader mode. */
@@ -53,43 +76,74 @@ int main(int /* argc */, char ** /* argv */) try
gpio_nirq->gpio_set_dir(false);
gpio_nirq->gpio_set_edge("falling");
- auto arvrStabilizedRV_callback_last = std::chrono::steady_clock::now();
- auto const arvrStabilizedRV_callback = [&arvrStabilizedRV_callback_last](sh2_RotationVectorWAcc_t const & data)
+ auto const acc_callback = [node, &imu_msg](sh2_Accelerometer_t const & data)
+ {
+ RCLCPP_DEBUG_THROTTLE(node->get_logger(), *node->get_clock(), 1000UL,
+ "Acceleration [x, y, z,] = [%0.3f, %0.3f, %0.3f] m/s²",
+ data.x, data.y, data.z);
+
+ imu_msg.linear_acceleration.x = data.x;
+ imu_msg.linear_acceleration.y = data.y;
+ imu_msg.linear_acceleration.z = data.z;
+ };
+
+ auto const gyro_callback = [node, &imu_msg](sh2_Gyroscope_t const & data)
{
- auto const now = std::chrono::steady_clock::now();
- auto const diff_time = (now - arvrStabilizedRV_callback_last);
- auto const diff_time_us = std::chrono::duration_cast(diff_time).count();
- arvrStabilizedRV_callback_last = now;
-
- char msg[128] = {0};
- snprintf(msg,
- sizeof(msg),
- "[%4ld] [i, j, k, real, accuracy] = [%0.3f, %0.3f, %0.3f, %0.3f, %0.3f]",
- diff_time_us,
- data.i,
- data.j,
- data.k,
- data.real,
- data.accuracy);
- std::cout << msg << std::endl;
+ RCLCPP_DEBUG_THROTTLE(node->get_logger(), *node->get_clock(), 1000UL,
+ "Gyroscope [x, y, z,] = [%0.3f, %0.3f, %0.3f] rad/s",
+ data.x, data.y, data.z);
+
+ imu_msg.angular_velocity.x = data.x;
+ imu_msg.angular_velocity.y = data.y;
+ imu_msg.angular_velocity.z = data.z;
+ };
+
+ auto const attitude_callback = [node, imu_pub, &imu_msg](sh2_RotationVectorWAcc_t const & data)
+ {
+ RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 250UL,
+ "Attitude [i, j, k, real, accuracy] = [%0.3f, %0.3f, %0.3f, %0.3f, %0.3f]",
+ data.i, data.j, data.k, data.real, data.accuracy);
+
+ imu_msg.header.stamp = node->now();
+ imu_msg.orientation.x = data.i;
+ imu_msg.orientation.y = data.j;
+ imu_msg.orientation.z = data.k;
+ imu_msg.orientation.w = data.real;
+
+ imu_pub->publish(imu_msg);
};
auto spi = std::make_shared("/dev/spidev0.0", SPI_MODE_3, 8, 3*1000*1000UL);
- auto bno085 = std::make_shared(spi, gpio_nirq, arvrStabilizedRV_callback);
+ auto bno085 = std::make_shared(spi, gpio_nirq, acc_callback, gyro_callback, attitude_callback);
/* Configure sensor for obtaining current orientation
* as a quaternion with accuracy estimation.
*/
- if (auto const rc = bno085->config(); rc != SH2_OK) {
- std::cerr << "config failed with error code " << rc << std::endl;
+ if (auto const rc = bno085->enableAccelerometer(); rc != SH2_OK) {
+ std::cerr << "\"enableAccelerometer()\" failed with error code " << rc << std::endl;
+ return EXIT_FAILURE;
+ }
+ if (auto const rc = bno085->enableGyroscope(); rc != SH2_OK) {
+ std::cerr << "\"enableGyroscope()\" failed with error code " << rc << std::endl;
+ return EXIT_FAILURE;
+ }
+ if (auto const rc = bno085->enableAttitude(); rc != SH2_OK) {
+ std::cerr << "\"enableAttitude()\" failed with error code " << rc << std::endl;
return EXIT_FAILURE;
}
- /* Run until killed by Ctrl+C to service
- * the sensor hub.
+ /* Run until killed by Ctrl+C to service the sensor hub.
*/
- for (;;)
+ RCLCPP_INFO(node->get_logger(), "%s init complete.", node->get_name());
+
+ while (rclcpp::ok())
{
+ /* Let ROS do its things. */
+ rclcpp::spin_some(node);
+
+ /* Check if an event has been signalled via the
+ * BNO085 nIRQ pin.
+ */
auto const gpio_nirq_fd = gpio_nirq->gpio_fd_open();
auto const rc_poll = gpio_nirq->gpio_poll(gpio_nirq_fd, 1000);
gpio_nirq->gpio_fd_close(gpio_nirq_fd);
@@ -102,6 +156,9 @@ int main(int /* argc */, char ** /* argv */) try
bno085->spinOnce();
}
+ RCLCPP_INFO(node->get_logger(), "%s shut down.", node->get_name());
+
+ rclcpp::shutdown();
return EXIT_SUCCESS;
}
catch (BNO085_Exception const & err)
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..789686c
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,24 @@
+
+
+
+ pika_spark_bno085_driver
+ 1.0.0
+ This package implements a ROS driver for the BNO085 9-DoF IMU.
+
+ Alexander Entinger
+
+ MIT
+
+ ament_cmake
+
+ l4xz
+ rclcpp
+ sensor_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+