Hi The Construct Team,
I am currently learning ROS2 Basics with Python. I am currently on the Topics Quiz and stuck there unable to get any message from /odom
topic.
What I have tried so far:
- Printed out values in odom callback function - THEY WORK FINE !
- Tried
ros2 topic echo /odom
- THIS ALSO WORKS FINE ! - Tried changing QoS reliability from BEST_EFFORT to RELIABLE and back to BEST_EFFORT - Does not seem to have any effect on my program.
- Moved
rclpy.spin(...)
function into a while loop with try-except - still the problem exists.
At this point I have run out of ideas! [Probably I am making a basic mistake that I don’t seem to know!]
ros2 topic info /odom -v
prints out saying reliability is RELIABLE, but in both cases, RELIABLE or BEST_EFFORT, I still have the problem.
Type: nav_msgs/msg/Odometry
Publisher count: 1
Node name: turtlebot3_diff_drive
Node namespace: /
Topic type: nav_msgs/msg/Odometry
Endpoint type: PUBLISHER
GID: 01.0f.f7.c7.0b.02.00.00.01.00.00.00.00.00.63.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
Durability: RMW_QOS_POLICY_DURABILITY_VOLATILE
Lifespan: 2147483651294967295 nanoseconds
Deadline: 2147483651294967295 nanoseconds
Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
Liveliness lease duration: 2147483651294967295 nanoseconds
Subscription count: 0
BUT,
When I run my code (after compiling) - the odom callback does not receive any message and values do not get updated. I cannot figure out why.
Here is my code:
import time
import rclpy
import numpy as np
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from rclpy.qos import ReliabilityPolicy, QoSProfile
class MoveTurtleBot3(Node):
def __init__(self):
super().__init__('topics_quiz_node')
timer_period = 0.5
self.timer = self.create_timer(timer_period, self.timer_callback)
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
self.subscriber = self.create_subscription(
Odometry, '/odom', self.odom_callback,
QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)) # RELIABLE
self.twist_cmd = Twist()
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.get_logger().info("Moving front a little bit...")
self.twist_cmd.linear.x = 0.1
self.twist_cmd.angular.z = 0.0
self.publisher_.publish(self.twist_cmd)
time.sleep(0.5)
self.twist_cmd.linear.x = 0.0
self.twist_cmd.angular.z = 0.0
self.publisher_.publish(self.twist_cmd)
self.get_logger().info("done")
self.move()
# return None
def move(self):
# move straight till left opening
self.get_logger().info("Moving Straight...")
self.twist_cmd.linear.x = 0.5
self.twist_cmd.angular.z = 0.0
self.publisher_.publish(self.twist_cmd)
time.sleep(7.00)
self.twist_cmd.linear.x = 0.0
self.twist_cmd.angular.z = 0.0
self.publisher_.publish(self.twist_cmd)
# turn left 90 degs or 1.57 rads
self.get_logger().info("Turning 90 degs...")
while (self.yaw < 1.569):
# print(self.odom_msg)
self.get_logger().info('Yaw: %s' % str(self.yaw))
self.twist_cmd.linear.x = 0.0
self.twist_cmd.angular.z = 0.1
self.publisher_.publish(self.twist_cmd)
time.sleep(0.5)
# self.twist_cmd.linear.x = 0.0
# self.twist_cmd.angular.z = 0.0
# self.publisher_.publish(self.twist_cmd)
# return None
def timer_callback(self):
# self.move()
pass
# return None
def odom_callback(self, odom_msg):
# self.odom_msg = odom_msg
quat = odom_msg.pose.pose.orientation
# self.get_logger().info("Quaternion: %s" % str(quat))
# self.get_logger().info("QuatX: %s" % quat.x)
# self.get_logger().info("QuatY: %s" % quat.y)
# self.get_logger().info("QuatZ: %s" % quat.z)
# self.get_logger().info("QuatW: %s" % quat.w)
self.roll, self.pitch, self.yaw = self.euler_from_quaternion(
[quat.x, quat.y, quat.z, quat.w])
self.get_logger().info("Roll: %s, Pitch: %s, Yaw: %s" %
(str(self.roll), str(self.pitch), str(self.yaw)))
# return None
def euler_from_quaternion(self, quaternion):
"""
Converts quaternion (w in last place) to euler roll, pitch, yaw
quaternion = [x, y, z, w]
Below should be replaced when porting for ROS2 Python tf_conversions is done.
"""
x = quaternion[0]
y = quaternion[1]
z = quaternion[2]
w = quaternion[3]
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = np.arctan2(sinr_cosp, cosr_cosp)
sinp = 2 * (w * y - z * x)
pitch = np.arcsin(sinp)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = np.arctan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def main(args=None):
rclpy.init(args=args)
move_turtlebot3 = MoveTurtleBot3()
while True:
try:
rclpy.spin(move_turtlebot3)
except:
break
# rclpy.spin(move_turtlebot3)
move_turtlebot3.destroy_node()
rclpy.shutdown()
# return None
if __name__ == '__main__':
main()
# End of Code
This is the output of my program: The yaw value never changes and stuck at 0.0.
user:~/ros2_ws$ ros2 launch topics_quiz topics_quiz.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2022-09-17-06-19-27-649269-4_xterm-10026
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [move_turtlebot3-1]: process started with pid [10028]
[move_turtlebot3-1] [INFO] [1663395568.602659797] [topics_quiz_node]: Moving front a little bit...
[move_turtlebot3-1] [INFO] [1663395569.104318385] [topics_quiz_node]: done
[move_turtlebot3-1] [INFO] [1663395569.104947102] [topics_quiz_node]: Moving Straight...
[move_turtlebot3-1] [INFO] [1663395576.110771928] [topics_quiz_node]: Turning 90 degs...
[move_turtlebot3-1] [INFO] [1663395576.111307290] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395576.612531274] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395577.114181184] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395577.615251677] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395578.116688925] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395578.618135039] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395579.119771268] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395579.621020665] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395580.122279405] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395580.623943868] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395581.125656957] [topics_quiz_node]: Yaw: 0.0
Please help!
Thanks,
Girish