Dear all, I tried to use the left side of the laser data to detect the wall opening (laser distance at pi/2 will become inf) but the laser data becomes inf even before the robot is in parallel with the opening. Do you know why?
Most of the time, my simulation environment does not work properly (It only shows the wheels and the laser of the robot, and sometimes the robot does not move at all…)
I did not find the debugger of the IDE. How can we debug in this case? When I tried to open the debug tool, there was only a desktop.
This is the Python script:
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
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
# import the Odometry module from nav_msgs interface
from nav_msgs.msg import Odometry
from rclpy.qos import ReliabilityPolicy, QoSProfile
import numpy as np
class Topic_quiz(Node):
def __init__(self):
# Here you have the class constructor
# call the class constructor
super().__init__('topic_quiz')
# create the publisher object to set the velocity
self.publisher_cmd_vel = self.create_publisher(Twist, 'cmd_vel', 10)
# create a Subscriber for laser scan data to detect the opening
self.subscriber_laser = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
# create a Subscriber for odometry data to perform navigation
self.subscriber_odom = self.create_subscription(Odometry, '/odom', self.odom_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
# define the timer period for 0.5 seconds
self.timer_period = 0.5
# initialize the state of the robot
self.state= 0
# define the variable to save the received info from the laser
self.specific_angle = 1.57 # Change this value if you're looking for a different angle
self.laser_index = None
self.laser_left = 0
self.large_distance_threshold = 20
self.detect_opening = False
# define the variable to save the received info from Odometry
self.current_yaw = None
self.target_yaw = 1.57 # Rotate 90 degrees clockwise in state 1
self.odom_pose = 0.0
self.odom_pose_euler = 0.0
# create a Twist message object to store velocity commands.
self.cmd = Twist()
self.timer = self.create_timer(self.timer_period, self.motion) # every 0.5s, exe motion function
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 laser_callback(self,msg): # save the laser measured distance at left side of the robot
if self.laser_index == None:
# Calculate the index for the specific angle (90 degree = 1.57 degrees in this case)
self.laser_index = int((self.specific_angle - msg.angle_min) / msg.angle_increment)
# Ensure the index is within the valid range
if 0 <= self.laser_index < len(msg.ranges):
self.laser_left = msg.ranges[self.laser_index]
self.get_logger().info(f'Laser reading at {self.specific_angle} degrees: {self.laser_left}')
else:
self.get_logger().error(f'Calculated index ({self.laser_index}) is out of range. Check your laser configuration.')
if self.state == 0:
if msg.ranges[self.laser_index] == float('inf') or msg.ranges[self.laser_index] > self.large_distance_threshold:
# An opening has been detected
self.detect_opening = True
self.get_logger().info('Opening detected on the left side!')
self.state = 1
self.get_logger().info('Entering in state 1!')
elif self.state ==2:
if msg.ranges[self.laser_index] < 1:
self.get_logger().info('close to the opening')
self.detect_opening = False
self.state = 3
self.get_logger().info('Entering in state 3!')
elif self.state==3:
if msg.ranges[self.laser_index] == float('inf') or msg.ranges[self.laser_index] > self.large_distance_threshold:
# An opening has been detected
self.detect_opening = True
self.get_logger().info('Opening detected on the left side')
self.state = 4
self.get_logger().info('Entering in state 4!')
def odom_callback(self,msg): # save the Odometry pose in euler format and rotate 90 degree
# Get the current orientation from the odometry message
if self.state == 1:
current_orientation = msg.pose.pose.orientation
current_orientation_list = [current_orientation.x, current_orientation.y,
current_orientation.z, current_orientation.w]
# Convert quaternion to Euler angles
_, _, self.current_yaw = self.euler_from_quaternion(quaternion = current_orientation_list)
self.get_logger().info(f'Current Yaw is ({self.current_yaw})')
def motion(self): # move according to the laser measured distance
# print the state
self.get_logger().info('Robot is in state: "%s"' % str(self.state))
# Logic of move based on the message from laserScan
# 0. move forward 1. rotate 90 degrees 2. move forward 3. close to the opening 4. stop
if self.state == 0:
self.cmd.linear.x = 0.5
self.cmd.angular.z = 0.0
elif self.state == 1:
# Rotate until the target yaw is reached
if abs(self.target_yaw - self.current_yaw) > 0.001:
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.5 # Set appropriate angular speed
self.get_logger().info('Robot is rotating')
else:
self.state = 2
self.get_logger().info('Entering in state 2!')
elif self.state == 2:
self.cmd.linear.x = 0.5
self.cmd.angular.z = 0.0
elif self.state == 3:
self.cmd.linear.x = 0.3
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 according the received laser msg
self.publisher_cmd_vel.publish(self.cmd)
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
# declare the node constructor
topics_quiz_node = Topic_quiz()
# 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()
type or paste code here