Merge pull request #7 from pika-spark/ros

Publish ROS sensor_msgs/Imu .

.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+

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()
 ##########################################################################

README.md

@@ -1,18 +1,35 @@
 <a href="https://pika-spark.io/"><img align="right" src="https://raw.githubusercontent.com/pika-spark/.github/main/logo/logo-pika-spark-bg-white.png" width="15%"></a>
 :sparkles: `pika-spark-bno085-driver`
 =====================================
-[![Smoke test status](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/smoke-test.yml/badge.svg)](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/smoke-test.yml)
+[![Build Status](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/ros2.yml/badge.svg)](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/ros2.yml)
 [![Spell Check status](https://github.com/pika-spark/pika-spark-bno085-driver/actions/workflows/spell-check.yml/badge.svg)](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).
 
 <p align="center">
   <a href="https://pika-spark.io/"><img src="https://raw.githubusercontent.com/pika-spark/.github/main/logo/logo-pika-spark-bg-white-github.png" width="40%"></a>
 </p>
 
-### 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  
 ```

bno085.cpp

@@ -19,9 +19,13 @@
 
 BNO085::BNO085(std::shared_ptr<SPI> const spi,
                std::shared_ptr<SysGPIO> 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<SPI> 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);
 }

bno085.h

@@ -24,15 +24,21 @@
 class BNO085 : private BNO085_HAL
 {
 public:
+  typedef std::function<void(sh2_Accelerometer_t const &)> LinearAccelerationCallbackFunc;
+  typedef std::function<void(sh2_Gyroscope_t const &)> CalibratedGyroscopeCallbackFunc;
   typedef std::function<void(sh2_RotationVectorWAcc_t const &)> StabilizedRotationVectorWAccuracyCallbackFunc;
 
   BNO085(std::shared_ptr<SPI> const spi,
          std::shared_ptr<SysGPIO> 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;
 };

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'},
+      ]
+    )
+  ])