Hey the Construct-Team and robot-enthusiasts,
i trying to solve the topics_quiz but the Gradebot doesn’t want to let me through.
In my opinion, however, I have met all the requirements. Could you help me here?
I don’t get any errors in the terminal or anything like that either. The Turtlebot drives exactly to the specified destination.
Following is my source code:
topics_quiz_note.py
import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the Twist module from geometry_msgs interface
from geometry_msgs.msg import Twist, Point, Quaternion
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from nav_msgs.msg import Odometry
import numpy as np
class Topics_quiz_node(Node):
def __init__(self):
# Here you have the class constructor
# call the class constructor
super().__init__('topics_quiz_node')
# create the publisher object
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
# create the subscriber object
self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
self.odom_sub = self.create_subscription(Odometry, '/odom',self.odom_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
self.odom_frame = '/odom'
self.base_frame = '/base_link'
# define the timer period for 0.5 seconds
self.timer_period = 0.5
# define the variable to save the received info
self.laser_forward = 0
# create a Twist message
self.cmd = Twist()
self.odometry = Odometry()
self.timer = self.create_timer(self.timer_period, self.motion)
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
def laser_callback(self,msg):
# Save the frontal laser scan info at 0°
self.laser_forward = msg.ranges[359]
def motion(self):
# print the data
#self.get_logger().info('I receive: "%s"' % str(self.laser_forward))
#self.get_logger().info('Yaw: "%s"' % str(yaw))
#self.get_logger().info('X-Value: "%s"' % str(self.position.x))
#self.get_logger().info('Y-Value: "%s"' % str(self.position.y))
# Logic of move
if self.position.x < 0.4 and self.yaw < 1.41 and self.position.y < 0:
self.cmd.linear.x = 1.0
self.cmd.angular.z = 0.0
elif self.position.x > 0.4 and self.yaw < 1.41 and self.position.y < 0:
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.2
elif self.position.x > 0.4 and self.yaw > 1.41 and self.position.y < 0.4:
self.cmd.linear.x = 1.0
self.cmd.angular.z = 0.0
else:
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.0
# Publishing the cmd_vel values to a Topic
self.publisher_.publish(self.cmd)
def odom_callback(self, msg):
self.position = msg.pose.pose.position
self.orientation_q = msg.pose.pose.orientation
self.orientation_list = [self.orientation_q.x, self.orientation_q.y, self.orientation_q.z, self.orientation_q.w]
(self.roll, self.pitch, self.yaw) = self.euler_from_quaternion (self.orientation_list)
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):
# initialize the ROS communication
rclpy.init(args=args)
# declare the node constructor
topics_quiz_node = Topics_quiz_node()
# pause the program execution, waits for a request to kill the node (ctrl+c)
rclpy.spin(topics_quiz_node)
# Explicity destroy the node
topics_quiz_node.destroy_node()
# shutdown the ROS communication
rclpy.shutdown()
if __name__ == '__main__':
main()
topics_quiz.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='topics_quiz',
executable='topics_quiz_node',
output='screen'),
])
setup.py
from setuptools import setup
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']),
],
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'
],
},
)
If you need a video of the programm let me know.
Thanks for your help.
