Added Gravity publisher (#64)

* Added Gravity publisher

* Updated Readme to introduce Gravity publisher

AUTHORS

@@ -1,3 +1,4 @@
 Michal Drweiga
 Evan Flynn
 Manfred Novotny
+Pascal Voser

README.md

@@ -79,6 +79,7 @@ ROS topics published by this ROS2 Node:
   - **bno055/imu_raw** [(sensor_msgs/Imu)](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html)
   - **bno055/temp** [(sensor_msgs/Temperature)](http://docs.ros.org/api/sensor_msgs/html/msg/Temperature.html); The sensor's ambient temperature
   - **bno055/mag** [(sensor_msgs/MagneticField)](http://docs.ros.org/api/sensor_msgs/html/msg/MagneticField.html)
+  - **bno055/grav** [(geometry_msgs/Vector3)](http://docs.ros.org/en/api/geometry_msgs/html/msg/Vector3.html)
   - **bno055/calib_status** [(std_msgs/String)](http://docs.ros.org/en/api/std_msgs/html/msg/String.html) : 
   Sensor Calibration Status as JSON string - e.g. `{"sys": 3, "gyro": 3, "accel": 0, "mag": 3}`
 

bno055/params/NodeParameters.py

@@ -81,6 +81,8 @@ def __init__(self, node: Node):
         node.declare_parameter('mag_factor', value=16000000.0)
         # scaling factor for gyroscope
         node.declare_parameter('gyr_factor', value=900.0)
+        # scaling factor for gyroscope
+        node.declare_parameter('grav_factor', value=100.0)
         # determines whether to use default offsets or not
         node.declare_parameter('set_offsets', value=False)
         # +/- 2000 units (at max 2G) (1 unit = 1 mg = 1 LSB = 0.01 m/s2)
@@ -155,6 +157,9 @@ def __init__(self, node: Node):
             self.gyr_factor = node.get_parameter('gyr_factor')
             node.get_logger().info('\tgyr_factor:\t\t"%s"' % self.gyr_factor.value)
 
+            self.grav_factor = node.get_parameter('grav_factor')
+            node.get_logger().info('\tgrav_factor:\t\t"%s"' % self.grav_factor.value)
+
             self.set_offsets = node.get_parameter('set_offsets')
             node.get_logger().info('\tset_offsets:\t\t"%s"' % self.set_offsets.value)
 

bno055/params/bno055_params.yaml

@@ -42,6 +42,7 @@ bno055:
     acc_factor: 100.0
     mag_factor: 16000000.0
     gyr_factor: 900.0
+    grav_factor: 100.0
     set_offsets: false # set to true to use offsets below
     offset_acc: [0xFFEC, 0x00A5, 0xFFE8]
     offset_mag: [0xFFB4, 0xFE9E, 0x027D]

bno055/params/bno055_params_i2c.yaml

@@ -41,6 +41,7 @@ bno055:
     acc_factor: 100.0
     mag_factor: 16000000.0
     gyr_factor: 900.0
+    grav_factor: 100.0
     set_offsets: false # set to true to use offsets below
     offset_acc: [0xFFEC, 0x00A5, 0xFFE8]
     offset_mag: [0xFFB4, 0xFE9E, 0x027D]

bno055/params/bno055_params_test.yaml

@@ -41,6 +41,7 @@ bno055:
     acc_factor: 100.0
     mag_factor: 16000000.0
     gyr_factor: 900.0
+    grav_factor: 100.0
     set_offsets: false # set to true to use offsets below
     offset_acc: [0xFFEC, 0x00A5, 0xFFE8]
     offset_mag: [0xFFB4, 0xFE9E, 0x027D]

bno055/sensor/SensorService.py

@@ -35,7 +35,7 @@
 from bno055.connectors.Connector import Connector
 from bno055.params.NodeParameters import NodeParameters
 
-from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import Quaternion, Vector3
 from rclpy.node import Node
 from rclpy.qos import QoSProfile
 from sensor_msgs.msg import Imu, MagneticField, Temperature
@@ -58,6 +58,7 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters):
         self.pub_imu_raw = node.create_publisher(Imu, prefix + 'imu_raw', QoSProf)
         self.pub_imu = node.create_publisher(Imu, prefix + 'imu', QoSProf)
         self.pub_mag = node.create_publisher(MagneticField, prefix + 'mag', QoSProf)
+        self.pub_grav = node.create_publisher(Vector3, prefix + 'grav', QoSProf)
         self.pub_temp = node.create_publisher(Temperature, prefix + 'temp', QoSProf)
         self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf)
         self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback)
@@ -142,6 +143,7 @@ def get_sensor_data(self):
         imu_raw_msg = Imu()
         imu_msg = Imu()
         mag_msg = MagneticField()
+        grav_msg = Vector3()
         temp_msg = Temperature()
 
         # read from sensor
@@ -238,6 +240,14 @@ def get_sensor_data(self):
         ]
         self.pub_mag.publish(mag_msg)
 
+        grav_msg.x = \
+            self.unpackBytesToFloat(buf[38], buf[39]) / self.param.grav_factor.value
+        grav_msg.y = \
+            self.unpackBytesToFloat(buf[40], buf[41]) / self.param.grav_factor.value
+        grav_msg.z = \
+            self.unpackBytesToFloat(buf[42], buf[43]) / self.param.grav_factor.value
+        self.pub_grav.publish(grav_msg)
+
         # Publish temperature
         temp_msg.header.stamp = self.node.get_clock().now().to_msg()
         temp_msg.header.frame_id = self.param.frame_id.value