0
$\begingroup$
import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Imu
from std_msgs.msg import Header
from geometry_msgs.msg import (Quaternion, Vector3)
from std_msgs.msg import String
from rclpy.qos import QoSProfile

import time
import board
import busio
# import serial

from adafruit_bno08x import (
    BNO_REPORT_ACCELEROMETER,
    BNO_REPORT_GYROSCOPE,
    BNO_REPORT_ROTATION_VECTOR,
)
from adafruit_bno08x.i2c import BNO08X_I2C

i2c = busio.I2C(board.SCL, board.SDA, frequency=400000)
bno = BNO08X_I2C(i2c)
bno.enable_feature(BNO_REPORT_ACCELEROMETER)
bno.enable_feature(BNO_REPORT_GYROSCOPE)
bno.enable_feature(BNO_REPORT_ROTATION_VECTOR)


class ImuPublisher(Node):

    def __init__(self):
        super().__init__('imu_publisher')
        #prefix = self.param.ros_topic_prefix.value
        #QoSProf = QoSProfile(depth=10)
        #self.publisher_ = self.create_publisher(String, 'topic', 10)
        #self.pub_imu_msg = Node.create_publisher(Imu, 'imu_msg', QoSProfile, 10)
        
        self.publisher_ = self.create_publisher(Imu, 'imu', 10)
        #self.diag_pub_time = self.get_clock().now()
        self.imu_msg = Imu()
        self.frame_id = self.declare_parameter('frame_id', "base_imu_link").value
        self.get_logger().info(f"Imu publisher successfully created.")
        
    def publish_imu_message(self):
        """Publish the sensor message with new data
        """
        #try:
        imu_msg = Imu()
        # data = self.sensor.getMotion6()
        # yaw, pitch, roll, x_accel, y_accel, z_accel = rvc.heading

        gyro = Vector3()
        gyro.x, gyro.y, gyro.z = bno.gyro
        self.get_logger().info(f"Now gathering data for message")
            
        # fetch all accel values - return in m/s²
        accel = Vector3()
        accel.x, accel.y, accel.z = bno.acceleration
            
        imu_msg.angular_velocity = gyro
        imu_msg.angular_velocity_covariance[0] = 0.00001
        imu_msg.angular_velocity_covariance[4] = 0.00001
        imu_msg.angular_velocity_covariance[8] = 0.00001
            
        imu_msg.linear_acceleration = accel
        imu_msg.linear_acceleration_covariance[0] = 0.00001
        imu_msg.linear_acceleration_covariance[4] = 0.00001
        imu_msg.linear_acceleration_covariance[8] = 0.00001
            
        # imu_msg.orientation_covariance = constants.EMPTY_ARRAY_9
        quat_i, quat_j, quat_k, quat_real = bno.quaternion
        imu_msg.orientation[w] = quat_real
        imu_msg.orientation[x] = quat_i
        imu_msg.orientation[y] = quat_j
        imu_msg.orientation[z] = quat_k
            
        imu_msg.orientation_covariance[0] = 0.00001
        imu_msg.orientation_covariance[4] = 0.00001
        imu_msg.orientation_covariance[8] = 0.00001
            
        # add header
        imu_msg.header.stamp = self.get_clock().now().to_msg()
        imu_msg.header.frame_id = self.imu_frame
            
        node.get_logger().info('Publishing imu message')
        #self.pub_imu_raw.publish(imu_raw_msg)
        self.pub_imu.publish(imu_msg)

        self.get_logger().debug('gz: {:+.0f}'.format(gyro.z))
        #self.imu_message_publisher.pub.publish(self.imu_msg)
        #self.publish_imu_message()
        #self.pub.publish(self.imu_msg)
        #self.publish_imu_message()
        #self.rate.sleep()

        #except Exception as ex:
        #self.get_logger().error(f"Error in publishing sensor message: {ex}")


def main(args=None):
    rclpy.init(args=args)
    node = ImuPublisher()

    try:
        rclpy.spin(node)

    except KeyboardInterrupt:
        pass

        #imu_publisher = ImuPublisher()
        #rclpy.spin(imu_publisher)
        # Destroy the node explicitly
        # (optional - otherwise it will be done automatically
        # when the garbage collector destroys the node object)
        #imu_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

It seems to execute properly to the first message about node creation. Checking topics shows the /imu topic. But no messages are published! I'll try to include a photo of the error: code of error on screen

$\endgroup$
4
  • $\begingroup$ It's early for me and I'm not yet through my first cup of coffee, but I'm not seeing where that code even attempts to publish. Is there a call for the publishing function I'm not seeing? $\endgroup$
    – billy
    Commented Oct 28, 2023 at 17:08
  • $\begingroup$ self.pub_imu.publish(imu_msg) $\endgroup$
    – Russ76
    Commented Oct 31, 2023 at 2:37
  • $\begingroup$ I meant the function "publish_imu_message()". I don't see where that function is getting called. $\endgroup$
    – billy
    Commented Oct 31, 2023 at 6:30
  • $\begingroup$ Billy, you are right. Thanks $\endgroup$
    – Russ76
    Commented Nov 11, 2023 at 18:20

1 Answer 1

2
$\begingroup$

As I am not able to write a comment, I am writing my suggestion here.

You need to add the timer,which calls your publish_imu_message function at a required frequency. you can add the following line in your constructor after publisher, to create a timer and callback function for the timer.

self.timer = self.create_timer(1.0, self.publish_imu_message)

the first parameter is the frequency, and the second parameter is the callback function.

$\endgroup$
3
  • 1
    $\begingroup$ I was leaving open the possibility that she/he is calling the function from other code...but if not, then seems like it should should be called on the most recent update of the IMU data. Failing that, the timer as you wrote it seems reasonable. $\endgroup$
    – billy
    Commented Nov 1, 2023 at 1:42
  • $\begingroup$ Sounds reasonable! I'll attempt this tomorrow when I can get my hands on the Raspberry Pi. $\endgroup$
    – Russ76
    Commented Nov 1, 2023 at 14:19
  • $\begingroup$ That took care of it, thanks! $\endgroup$
    – Russ76
    Commented Nov 11, 2023 at 18:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.