diff --git a/AUTHORS b/AUTHORS index 7c377ae..085a4f9 100644 --- a/AUTHORS +++ b/AUTHORS @@ -1,3 +1,4 @@ Michal Drweiga Evan Flynn Manfred Novotny +Pascal Voser diff --git a/README.md b/README.md index a26ed38..7d2148e 100644 --- a/README.md +++ b/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}` diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py index ab88dc4..947fdaf 100644 --- a/bno055/params/NodeParameters.py +++ b/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) diff --git a/bno055/params/bno055_params.yaml b/bno055/params/bno055_params.yaml index 98eb0c0..3add21f 100644 --- a/bno055/params/bno055_params.yaml +++ b/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] diff --git a/bno055/params/bno055_params_i2c.yaml b/bno055/params/bno055_params_i2c.yaml index ea2cb91..745425b 100644 --- a/bno055/params/bno055_params_i2c.yaml +++ b/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] diff --git a/bno055/params/bno055_params_test.yaml b/bno055/params/bno055_params_test.yaml index 1f00e20..353bb31 100644 --- a/bno055/params/bno055_params_test.yaml +++ b/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] diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 97dd69a..10f1cf3 100644 --- a/bno055/sensor/SensorService.py +++ b/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