The automatic grader is saying that my node is not subscribed to /odom but it is and if one runs ros2 topic info /odom it shows the increased count. In addition I am using the info to correctly solve the problem.
Hi @antonio.pinera ,
Welcome to this Community!
Please provide relevant screenshots as attachments so that we can understand your problem and help you out accordingly.
- Screenshot of the autograder output.
- Output of your execution of the program on terminal.
Regards,
Girish
import math
import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, QoSProfile
# import the Twist interface from the geometry_msgs package
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
import numpy as np
class Mover(Node):
def __init__(self):
super().__init__('mover')
self.setpoint = Twist()
# Variables to hold the sensor data
self.ranges = None
self.odom = None
self.state = 'find_door'
self.velocity_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
self.laser_subscriber = self.create_subscription(
LaserScan,
'/scan',
self.update_scan,
QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE)
)
self.odom_subscriber = self.create_subscription(
Odometry,
'/odom',
self.update_odom,
QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE)
)
# define the timer period for 0.5 seconds
timer_period = 0.02
# create a timer sending two parameters:
# - the duration between 2 callbacks (0.5 seconds)
# - the timer function (timer_callback)
self.timer = self.create_timer(timer_period, self.move)
def update_scan(self, msg):
self.ranges = msg.ranges
def update_odom(self, msg):
self.odom = msg.pose
def move(self):
# states are: find_door, turn_to_door, go_through_door, stopped
v = Twist()
self.get_logger().info(f"The robot is currently in {self.state}")
if self.ranges is None or self.odom is None:
return
# find_door
if self.state == 'find_door':
v.linear.x = .1
middle_delta = 7 # Number of steps to displace the angle to the left to ensure we are in the middle of the door
ang_ratio = 1/4 # (desired_angle - start_span)/length_of_span
index = int(ang_ratio*len(self.ranges)) + middle_delta
dist = self.ranges[index]
self.get_logger().info(f"Dist {dist}")
if dist > 100:
self.state = 'turn_to_door'
# turn_to_door
elif self.state == 'turn_to_door':
threshold = 0.02
v.angular.z = .3
quat = (
self.odom.pose.orientation.x,
self.odom.pose.orientation.y,
self.odom.pose.orientation.z,
self.odom.pose.orientation.w,
)
angs = self.euler_from_quaternion(quat)
yaw = angs[2]
self.get_logger().info(f"Yaw angle {yaw}")
if 1.50 - yaw < threshold:
self.goal_y = self.odom.pose.position.y + 4.5
self.state = 'go_through_door'
# go_through
elif self.state == 'go_through_door':
v.linear.x = .5
threshold_y = 0.1
y_pos = self.odom.pose.position.y
if abs(y_pos - self.goal_y) < threshold_y:
self.state = 'stop'
self.velocity_publisher.publish(v)
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
mover = Mover()
# pause the program execution, waits for a request to kill the node (ctrl+c)
rclpy.spin(mover)
# Explicity destroys the node
mover.destroy_node()
# shutdown the ROS communication
rclpy.shutdown()
if __name__ == '__main__':
main()
The launch file is:
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'),
])
And the setup.file
from setuptools import setup
from glob import glob
import os
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.door:main'
],
},
)
Hi @antonio.pinera ,
Hint: /odom QoS is different. That is why /odom messages could not be recognized.
I will let you figure this out on your own first - to help you with your learning experience.
Also, as a good practice, launch and run your code to check if your code works BEFORE submitting it to the autograder.
Let me know only if you can’t figure out your issue.
Regards,
Girish
Many thanks, the code runs well and it solves the task, that is what was shocking to me. I will look into this issue with QoS. Many thanks for the suggestion.
Regards
Is there any command I can use to see the configuration of QoS to be correctly used in a topic?
Hi @antonio.pinera ,
Yes, it is specified in the course notes itself.
Anyways, I will tell you here again.
ros2 topic info /odom -v
The -v displays the information on the topic in verbose manner.
Regards,
Girish
PS: Pay attention when you learn! ROS2 is not too easy to learn!
Hi @antonio.pinera ,
I just realized something from your code, here are my observations:
-
In your code, in
class Mover(), you will initialize the parent classNodewith the node name. Therefore, your first line underdef __init__(self)should readsuper().__init__("topics_quiz_node"). Thissuper()line calls the Parent classNodewith the node name which is the("topics_quiz_node"). So replace('mover')with('topics_quiz_node'). (Single quotes and double quotes mean the same in Python, I prefer double quotes for long strings and single quotes for characters just like in C/C++, it is just my preference, you do not have to follow). -
I think
/scanQoS must be different. It should have Reliability asBEST_EFFORTand Durability asVOLATILE. This was how it was when I did the course. I am not sure how it is in Humble course. I believe it would be the same. Get this info usingros2 topic info /scan -v. -
Similarly for
/odom. I think the QoS is correct for/odom. Reliability must beRELIABLEand Durability must beTRANSIENT_LOCAL. Again, check this info usingros2 topic info /odom -v. -
In your launch file, the
executablename is NOT the node name. It can just be'topics_quiz'. Node name is what you specify in your code class when you callsuper(). -
In your
setup.py, under entry_points console_scripts, you should specify the same name you specified in the launch fileexecutablefield here. So, if you plan to change'topics_quiz_node'to'topics_quiz'in your launch fileexecutable. Then your setup file should have the line'topics_quiz = topics_quiz.door:main'. I am assuming the name of the script file containing theMover()class is nameddoor.py. -
When you run the code before submitting to autograder, just make sure that your subscriber callbacks are printing the message correctly. This is one way of debugging your code to see if it works.
More info on QoS setting:
To set Realiabiliy and Durability you must import both Policies.
# import
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
# you can also add HistoryPolicy and LivelinessPolicy for QoS - if you want to!
# set the values
self.scan_sub_qos = QoSProfile(depth=10,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE)
self.scan_sub = self.create_subscription(msg_type=LaserScan,
topic="/scan",
callback=self.scan_callback,
qos_profile=self.scan_sub_qos)
Sorry for the long post. Hope you understood this clearly.
Let me know if anything in still unclear.
Regards,
Girish
Many thanks for the detailed answer and feedback.
Regards,
Juan Antonio
This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.