This is my feedback of the last attempt
It said that I didn’t subcribe /odom correctly or wrong name of the node. So I use ros2 node list to checking whether topics_quiz_node is working but its not. Even the terminal output is printing.
I don’t know why the topics_quiz_node not init, even though I follow the template in the lesson. And 1 thing weird, why my terminal still running but ros2 node topic cannot regcognize my node.
This is my topics_quiz_node.py
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import numpy as np
class TopicsQuizNode(Node):
def __init__(self):
super().__init__('topics_quiz_node')
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
self.subscription_ = self.create_subscription(Odometry, '/odom', self.odom_callback, 10)
self.is_turned_left = False
self.start_time = None
def odom_callback(self, msg):
# Extract orientation data from odometry message
orientation = msg.pose.pose.orientation
# Calculate euler angles from quaternion orientation
roll, pitch, yaw = self.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
# Print current orientation
self.get_logger().info(f"Current orientation: yaw={yaw}")
# Get the current time in seconds
current_time = self.get_clock().now().to_msg().sec
# If start_time is not set, set it to the current time
if self.start_time is None:
self.start_time = current_time
# Logic to follow trajectory
if current_time - self.start_time < 23:
self.send_velocity(0.2, 0.0, 0.0, 0.0) # Move forward for 23 seconds
elif not self.is_turned_left:
if yaw < 1.57: # Rotate until reaching 90 degrees (1.57 radians)
self.send_velocity(0.0, 0.0, 0.2, 0.0) # Rotate left (angular_z=0.2)
else:
self.is_turned_left = True
self.get_logger().info("Turning left done")
elif current_time - self.start_time < 63:
self.send_velocity(0.2, 0.0, 0.0, 0.0) # Move forward for 5 seconds after left turn
else:
self.send_velocity(0.0, 0.0, 0.0, 0.0) # Stop the robot
def send_velocity(self, linear_x, linear_y, angular_z, duration):
msg = Twist()
msg.linear.x = linear_x
msg.linear.y = linear_y
msg.angular.z = angular_z
self.publisher_.publish(msg)
self.get_logger().info(f"Sending command: linear_x={linear_x}, angular_z={angular_z}")
self.get_logger().info("Duration: {}".format(duration))
def euler_from_quaternion(self, quaternion):
x, y, z, w = quaternion
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)
node = TopicsQuizNode()
rclpy.logging.set_logger_level("topics_quiz_node", rclpy.logging.LoggingSeverity.INFO)
try:
rclpy.spin(node)
finally:
node.send_velocity(0.0, 0.0, 0.0, 0.0) # Make sure the robot stops before shutting down
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
setup.py
from setuptools import setup
import os
from glob import glob
package_name = 'topics_quiz'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name), glob('launch/*.launch.py'))
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='user',
maintainer_email='user@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'topics_quiz_node = topics_quiz.topics_quiz_node:main'
],
},
)
package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>topics_quiz</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="user@todo.todo">user</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
And lastly topics_quiz.launch.py file
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='topics_quiz',
executable='topics_quiz_node',
output='screen'
)
])
Thanks for your correcting, I’m figuring out where am I wrong but seem hopeless 
