ros2 python odometry

asked 2021-08-05 03:44:22 -0500

Sarwan gravatar image

Hello, I am new to ROS and trying to publish odometry msg using wheel encoders and I am having trouble calculating the ticks for a particular step time. I am getting the error:TypeError: unsupported operand type(s) for -: 'Time' and 'Time' - because I could not calculate the time difference from the line: dt = (current_time - prev_update_time). Kindly inform me how it can be done or is there a better way to do this? Thanks

I have mentioned the code below

class Odom_node(Node):

def __init__(self):
    rclpy.init()
    super().__init__('odom_node')

    #Publishers and subscribers
    self.odom_pub = self.create_publisher(Odometry,"odom",10)
    self.joint_pub = self.create_publisher(JointState, "joint_states", 10)
    self.cmd_vel_sub = self.create_subscription(Twist,"cmd_vel",self.dummy,10)

    self.nodeName = self.get_name()
    self.get_logger().info("{} started".format(self.nodeName))

    #current_time = self.get_clock().now()
    prev_update_time = self.get_clock().now().to_msg()
    current_time = self.get_clock().now().to_msg()
    self.get_logger().info("{} started".format(prev_update_time))

    self.broadcaster = TransformBroadcaster(self, 10) #odom frame broadcaster
    joint_state = JointState()
    self.odometry = Odometry()  #odom information
    odom_trans = TransformStamped()
    odom_trans.header.frame_id = 'odom'
    odom_trans.child_frame_id = 'base_link'
    self.odometry.header.frame_id = 'odom'
    self.odometry.child_frame_id = 'base_link'
    self.tw_msg = TwistWithCovariance()
    try:
        while rclpy.ok():
            rclpy.spin_once(self)

            current_time = self.get_clock().now().to_msg()
            dt = (current_time - prev_update_time)
            delta_th = (self.tw_msg.twist.angular.z) * dt

            delta_x = self.tw_msg.twist.linear.x * cos(delta_th) * dt
            delta_y = self.tw_msg.twist.linear.x * sin(delta_th) * dt

            x += delta_x
            y += delta_y
            dth += delta_th

            self.odometry.pose.pose.position.x = x
            self.odometry.pose.pose.position.y = y
            self.odometry.pose.pose.position.z = 0
            self.odometry.pose.pose.orientation = euler_to_quaternion(0, 0, dth) # roll,pitch,yaw
            #self.odometry.pose.pose.orientation.y =
            #self.odometry.pose.pose.orientation.z =
            #self.odometry.pose.pose.orientation.w = 1.
            self.odometry.twist.twist.linear.x =self.tw_msg.twist.linear.x
            self.odometry.twist.twist.linear.y = 0
            self.odometry.twist.twist.linear.z = 0
            self.odometry.twist.twist.angular.x = 0
            self.odometry.twist.twist.angular.y = 0
            self.odometry.twist.twist.angular.z = self.tw_msg.twist.angular.z
            self.odometry.header.stamp = current_time
            self.odometry_pub.publish(self.odometry)

            odom_trans.transform.header.stamp = current_time
            odom_trans.transform.translation.x = self.odometry.pose.pose.position.x
            odom_trans.transform.translation.y = self.odometry.pose.pose.position.y
            odom_trans.transform.translation.z = self.odometry.pose.pose.position.z
            odom_trans.transform.rotation = self.odometry.pose.pose.orientation     #includes x,y,z,w
            self.broadcaster.sendTransform(odom_trans)

    except KeyboardInterrupt:
        pass
edit retag flag offensive close merge delete

Comments

Hi Sarwan, Did you get a solution to this problem?

Titus gravatar image Titus  ( 2021-12-18 11:33:41 -0500 )edit
1

Hey, Yes, I found a way to do it.

import time self.prev_update_time = time.time()

In the try and except part, I used this)

current_time = time.time() drive_pub_frequency = current_time - self.prev_update_time

This made it work for me. Sorry, the code is pretty messed up and I changed a lot to paste it here again. Hope this helps.

Thanks

Sarwan gravatar image Sarwan  ( 2021-12-22 01:37:00 -0500 )edit