Problem
After detecting an obstacle and turning 90 degrees left, the robot immediately turns left again instead of moving forward. It stays stuck in a loop of turning.
Possible Cause
It seems that the laser scan data doesn’t update fast enough after turning. As a result:
- The old obstacle data is still being used.
- The robot thinks there’s still an obstacle in front and turns again.
- This only happens after turning, meaning the sensor might need extra time to refresh.
Tried Solutions
- Added a short pause (sleep) after turning to wait for updated LaserScan data.
- Implemented a “WAIT_FOR_UPDATE” state to force a sensor refresh.
- Printed real-time sensor values (
front
,left
,right
) to check if they update correctly.
Results
- The problem still occurs; the front distance does not update immediately after a turn.
- This was not an issue in an earlier simpler version of the code, suggesting the bug is related to how data is being processed after a turn.
Question for the Forum
Why is the LaserScan data not updating immediately after a turn?
How can I ensure the robot correctly detects the new front distance after turning?
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import time
class LaserAvoidance:
def init(self):
rospy.init_node(‘topics_quiz_node’, anonymous=True)
# Publisher to move the robot
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
# Subscriber to laser scan data
self.laser_sub = rospy.Subscriber('/kobuki/laser/scan', LaserScan, self.laser_callback)
self.twist = Twist()
self.safe_distance = 1.5 # Distance threshold in meters
self.turn_speed = 1.0 # Speed for 90-degree turn
self.turn_duration = 1.3 # Time to complete a 90-degree turn
self.state = "MOVE_FORWARD" # Initial state
self.front_distance = 10.0 # Store updated front distance
def turn_90_degrees(self, direction="left"):
"""
Rotates the robot by 90 degrees using a time-based approach.
"""
rospy.loginfo(f"Turning 90 degrees {direction}...")
twist = Twist()
twist.linear.x = 0.0
twist.angular.z = self.turn_speed if direction == "left" else -self.turn_speed
start_time = time.time()
while time.time() - start_time < self.turn_duration:
self.cmd_vel_pub.publish(twist)
rospy.sleep(0.1)
# Stop turning after 90 degrees
twist.angular.z = 0.0
self.cmd_vel_pub.publish(twist)
# Transition to waiting for new sensor data
self.state = "WAIT_FOR_UPDATE"
def laser_callback(self, msg):
ranges = msg.ranges
num_ranges = len(ranges)
# Ensure valid data is received
if num_ranges == 0:
rospy.logwarn("Empty LaserScan data received!")
return
# Identify key laser readings
front_index = num_ranges // 2
left_index = int(num_ranges * 3 / 4) # 75% index (left side)
right_index = int(num_ranges * 1 / 4) # 25% index (right side)
self.front_distance = ranges[front_index] if ranges[front_index] != float('inf') else 10.0 # Replace inf with 10m
left = ranges[left_index] if ranges[left_index] != float('inf') else 10.0
right = ranges[right_index] if ranges[right_index] != float('inf') else 10.0
rospy.loginfo(f"State: {self.state} | Front: {self.front_distance:.2f}, Left: {left:.2f}, Right: {right:.2f}")
# State-based decision making
if self.state == "MOVE_FORWARD":
if self.front_distance > self.safe_distance:
# Continue moving forward
self.twist.linear.x = 0.2
self.twist.angular.z = 0.0
else:
# Obstacle detected, stop and transition to turning
self.twist.linear.x = 0.0
self.cmd_vel_pub.publish(self.twist)
rospy.sleep(2)
self.state = "TURN_LEFT"
elif self.state == "TURN_LEFT":
self.turn_90_degrees("left") # Turn 90 degrees left
self.state = "WAIT_FOR_UPDATE"
elif self.state == "WAIT_FOR_UPDATE":
rospy.sleep(2) # Allow time for sensor data to refresh
if self.front_distance < self.safe_distance:
# If front still blocked, turn again
self.state = "TURN_LEFT"
else:
# If path is clear, move forward
self.state = "MOVE_FORWARD"
# Publish movement command
self.cmd_vel_pub.publish(self.twist)
def run(self):
rospy.spin()
if name == ‘main’:
try:
robot = LaserAvoidance()
robot.run()
except rospy.ROSInterruptException:
pass
[INFO] [1739887115.211071, 12.431000]: State: MOVE_FORWARD | Front: 1.55, Left: 2.17, Right: 10.00
[INFO] [1739887115.240988, 12.458000]: State: MOVE_FORWARD | Front: 1.55, Left: 2.15, Right: 10.00
[INFO] [1739887115.265539, 12.480000]: State: MOVE_FORWARD | Front: 1.55, Left: 2.17, Right: 10.00
[INFO] [1739887115.302420, 12.519000]: State: MOVE_FORWARD | Front: 1.55, Left: 2.14, Right: 10.00
[INFO] [1739887115.315217, 12.533000]: State: MOVE_FORWARD | Front: 1.56, Left: 2.12, Right: 10.00
[INFO] [1739887115.336326, 12.555000]: State: MOVE_FORWARD | Front: 1.54, Left: 2.12, Right: 10.00
[INFO] [1739887115.361659, 12.580000]: State: MOVE_FORWARD | Front: 1.53, Left: 2.11, Right: 10.00
[INFO] [1739887115.387551, 12.607000]: State: MOVE_FORWARD | Front: 1.53, Left: 2.13, Right: 10.00
[INFO] [1739887115.421732, 12.636000]: State: MOVE_FORWARD | Front: 1.53, Left: 2.11, Right: 10.00
[INFO] [1739887115.449797, 12.659000]: State: MOVE_FORWARD | Front: 1.50, Left: 2.10, Right: 10.00
[INFO] [1739887115.470958, 12.681000]: State: MOVE_FORWARD | Front: 1.52, Left: 2.09, Right: 10.00
[INFO] [1739887115.501245, 12.706000]: State: MOVE_FORWARD | Front: 1.51, Left: 2.08, Right: 10.00
[INFO] [1739887115.525626, 12.732000]: State: MOVE_FORWARD | Front: 1.51, Left: 2.08, Right: 10.00
[INFO] [1739887115.554157, 12.756000]: State: MOVE_FORWARD | Front: 1.49, Left: 2.07, Right: 10.00
[INFO] [1739887117.584153, 14.759000]: State: TURN_LEFT | Front: 1.48, Left: 2.06, Right: 10.00
[INFO] [1739887117.586531, 14.759000]: Turning 90 degrees left…
[INFO] [1739887118.908810, 16.062000]: State: WAIT_FOR_UPDATE | Front: 1.49, Left: 2.07, Right: 10.00
[INFO] [1739887120.913808, 18.063000]: State: TURN_LEFT | Front: 1.49, Left: 2.03, Right: 10.00
[INFO] [1739887120.916002, 18.064000]: Turning 90 degrees left…
[INFO] [1739887122.247642, 19.365000]: State: WAIT_FOR_UPDATE | Front: 1.47, Left: 2.05, Right: 10.00
[INFO] [1739887124.288336, 21.370000]: State: TURN_LEFT | Front: 1.50, Left: 2.03, Right: 10.00
[INFO] [1739887124.290680, 21.372000]: Turning 90 degrees left…
[INFO] [1739887125.611361, 22.672000]: State: WAIT_FOR_UPDATE | Front: 1.47, Left: 2.06, Right: 10.00
[INFO] [1739887127.667812, 24.673000]: State: TURN_LEFT | Front: 1.48, Left: 2.05, Right: 10.00
[INFO] [1739887127.672869, 24.677000]: Turning 90 degrees left…
[INFO] [1739887129.002014, 25.979000]: State: WAIT_FOR_UPDATE | Front: 1.48, Left: 2.05, Right: 10.00
[INFO] [1739887131.026794, 27.981000]: State: TURN_LEFT | Front: 1.49, Left: 2.06, Right: 10.00
[INFO] [1739887131.029142, 27.983000]: Turning 90 degrees left…
[INFO] [1739887132.348461, 29.287000]: State: WAIT_FOR_UPDATE | Front: 1.49, Left: 2.06, Right: 10.00
[INFO] [1739887134.393447, 31.289000]: State: TURN_LEFT | Front: 1.50, Left: 2.06, Right: 10.00
[INFO] [1739887134.395219, 31.290000]: Turning 90 degrees left…
[INFO] [1739887135.716043, 32.594000]: State: WAIT_FOR_UPDATE | Front: 1.49, Left: 2.06, Right: 10.00
[INFO] [1739887137.756216, 34.600000]: State: TURN_LEFT | Front: 1.50, Left: 2.04, Right: 10.00
[INFO] [1739887137.758357, 34.601000]: Turning 90 degrees left…
[INFO] [1739887139.078719, 35.903000]: State: WAIT_FOR_UPDATE | Front: 1.50, Left: 2.06, Right: 10.00
[INFO] [1739887141.114462, 37.903000]: State: MOVE_FORWARD | Front: 1.49, Left: 2.06, Right: 10.00
[INFO] [1739887143.144590, 39.905000]: State: TURN_LEFT | Front: 1.50, Left: 2.07, Right: 10.00
[INFO] [1739887143.146632, 39.907000]: Turning 90 degrees left…
[INFO] [1739887144.487925, 41.208000]: State: WAIT_FOR_UPDATE | Front: 1.49, Left: 2.05, Right: 10.00
[INFO] [1739887146.504573, 43.212000]: State: TURN_LEFT | Front: 1.49, Left: 2.05, Right: 10.00
[INFO] [1739887146.506871, 43.214000]: Turning 90 degrees left…
[INFO] [1739887147.825577, 44.514000]: State: WAIT_FOR_UPDATE | Front: 1.48, Left: 2.06, Right: 10.00
^C[topics_quiz_node-1] killing on exit
shutting down processing monitor…
… shutting down processing monitor complete