From a4287a2ca69db99f2c35947da74ca4ade47837c4 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Wed, 20 Oct 2021 18:11:39 +0200
Subject: [PATCH] Added imu_and_calibration launch file and imu config files

---
 calibration/bno055.cal           | Bin 0 -> 22 bytes
 config/bno055.yaml               |  81 +++++++++++++++++++++++++++++++
 launch/imu_and_collisions.launch |  40 +++++++++++++++
 package.xml                      |   2 +
 4 files changed, 123 insertions(+)
 create mode 100644 calibration/bno055.cal
 create mode 100644 config/bno055.yaml
 create mode 100644 launch/imu_and_collisions.launch

diff --git a/calibration/bno055.cal b/calibration/bno055.cal
new file mode 100644
index 0000000000000000000000000000000000000000..415824d33b03a35d9afda9fda66eb71dd64af48e
GIT binary patch
literal 22
TcmZQzKnDN*Gcdehc4Yzp6Ep)b

literal 0
HcmV?d00001

diff --git a/config/bno055.yaml b/config/bno055.yaml
new file mode 100644
index 0000000..3a5659a
--- /dev/null
+++ b/config/bno055.yaml
@@ -0,0 +1,81 @@
+update_rate: 100
+serial_device: '/dev/ttyUSB0'
+tf_prefix: ''
+frame_id: 'imu_bno055'
+# 1 -> "Only enable the accelerometer"
+# 2 -> "Only enable the magnetometer"
+# 3 -> "Only enable the gyroscope"
+# 4 -> "Enable both accelerometer and magnetometer"
+# 5 -> "Enable both accelerometer and gyroscope"
+# 6 -> "Enable both magnetometer and gyroscope"
+# 7 -> "Enable all sensors"
+# 8 -> "IMU"
+# 9 -> "Compass"
+# 10 -> "M4G"
+# 11 -> "ndof off"
+# 12 -> "ndof"
+mode: 5
+# 0 -> "+/- 2G"
+# 1 -> "+/- 4G"
+# 2 -> "+/- 8G"
+# 3 -> "+/- 16G"
+accel_range: 1
+# 0 -> "7.81Hz"
+# 4 -> "15.63Hz"
+# 8 -> "32.25H"
+# 12 -> "62.5Hz"
+# 16 -> "125Hz"
+# 20 -> "250Hz"
+# 24 -> "500Hz"
+# 28 -> "1000Hz"
+accel_bandwidth: 12
+# 0 -> "Normal"
+# 32 -> "Suspend"
+# 64 -> "Low power 1"
+# 96 -> "Stand by"
+# 128 -> "Low power 2"
+# 160 -> "Seep suspend"
+accel_pwr_mode: 0
+# 0 -> "+/- 2000 dps"
+# 1 -> "+/- 1000 dps"
+# 2 -> "+/- 500 dps"
+# 3 -> "+/- 250 dps"
+# 4 -> "+/- 125 dps"
+gyro_range: 0
+# 0 -> "523Hz"
+# 8 -> "230Hz"
+# 16 -> "116Hz"
+# 24 -> "47Hz"
+# 32 -> "23Hz"
+# 40 -> "12Hz"
+# 48 -> "64Hz"
+# 56 -> "32Hz"
+gyro_bandwidth: 56
+# 0 -> "Normal"
+# 1 -> "Fast power up"
+# 2 -> "Deep suspend"
+# 3 -> "Suspend"
+# 4 -> "Advanced power save"
+gyro_pwr_mode: 0
+# 0 -> "2 Hz"
+# 1 -> "6 Hz"
+# 2 -> "8 Hz"
+# 3 -> "10 Hz"
+# 4 -> "15 Hz"
+# 5 -> "20 Hz"
+# 6 -> "25 Hz"
+# 7 -> "30 Hz"
+mag_rate: 3
+# 0 -> "Low power"
+# 8 -> "Regular"
+# 16 -> "Enhanced regular"
+# 24 -> "High accuracy"
+mag_op_mode: 1
+# 0 -> "Normal"
+# 32 -> "Sleep"
+# 64 -> "Suspend"
+# 96 -> "Force mode"
+mag_pwr_mode: 0
+orientation_cov: 0.001
+angular_velocity_cov: 0.001
+linear_acceleration_cov: 0.001
diff --git a/launch/imu_and_collisions.launch b/launch/imu_and_collisions.launch
new file mode 100644
index 0000000..033a47c
--- /dev/null
+++ b/launch/imu_and_collisions.launch
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="output"        default="screen"/>
+  <arg name="launch_prefix" default=""/>
+  <arg name="dr"            default="false"/>
+  <arg name="sim_time"              default="false"/>
+  <param name="/use_sim_time" value="$(arg sim_time)" />
+
+
+  <arg name="collision_config_file"   default="$(find iri_collision_manager)/config/params.yaml"/>
+  <arg name="imu_topic"     default="/bno055_imu/imu"/>
+
+  <arg name="imu_config_file" default="$(find iri_collision_manager)/config/bno055.yaml" />
+  <arg name="imu_calibration_file" default="$(find iri_collision_manager)/calibration/bno055.cal" />
+
+  <include file="$(find iri_collision_manager)/launch/node.launch">
+    <arg name="node_name"     value="iri_collision_manager"/>
+    <arg name="output"        value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="config_file" value="$(arg collision_config_file)"/>
+    <arg name="imu_topic" value="$(arg imu_topic)"/>
+  </include>
+
+  <include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch">
+    <arg name="config_file"  value="$(arg imu_config_file)"/>
+    <arg name="calibration_file"  value="$(arg imu_calibration_file)"/>
+    <arg name="node_name" value="bno055_imu"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+  </include>
+
+  <node name="rqt_reconfigure_iri_collision_manager"
+        pkg ="rqt_reconfigure"
+        type="rqt_reconfigure"
+        if  ="$(arg dr)"
+        args="iri_collision_manager">
+  </node>
+
+</launch>
diff --git a/package.xml b/package.xml
index 1342d37..db0289d 100644
--- a/package.xml
+++ b/package.xml
@@ -54,6 +54,8 @@
   <exec_depend>iri_base_algorithm</exec_depend>
   <depend>tf2_ros</depend>
   <depend>sensor_msgs</depend>
+  <depend>iri_bno055_imu_bringup</depend>
+  <depend>iri_bno055_imu_driver</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
-- 
GitLab