ros2 python odometry
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 = (currenttime - prevupdate_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
Asked by Sarwan on 2021-08-05 03:44:22 UTC
Answers
self.current_time = self.get_clock().now().to_msg()
ct = self.current_time.sec + (self.current_time.nanosec/1e+9)
lt = self.last_time.sec + (self.last_time.nanosec/1e+9)
dt = (ct - lt)
print(dt)
Asked by faheemali on 2022-12-31 00:02:14 UTC
Comments
Hi Sarwan, Did you get a solution to this problem?
Asked by Titus on 2021-12-18 12:33:41 UTC
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
Asked by Sarwan on 2021-12-22 02:37:00 UTC