Hi all,
I searched for this topic before publishing and didn’t find anything similar…
so here it goes:
I am at 96% on my ROS Basics with python - Only the presentation left
Unfortunately, it is over a month I can’t debug a strange problem.
My robot is acting ok when launched on gazebo sim, BUT
when going on a real robot the robot has some kind of delay it recognizes the next wall in front of him very late
i set the code that if the wall detected under 0.5 → TURN LEFT
but the lidar scan comes very late and the robot already stuck face in the wall
I tried different Rates -5hz, 10hz, 25hz
I attach my code below the part in question is under the scan_callback function and starts with a comment
#when the robot needs to turn to next wall
and it where the front scan is <0.5 but bigger than >0.15
i feel i am doing something fundamentally wrong , at the beginning of me working on this project everything was great it looks I added something which made it go bad
p.s rviz on real robot shows good lidar scans around the turtlebot
#! /usr/bin/env python
# this is subscriber and publisher on the same Node!
# I subscribed to the Laser data and publish New Velocities under the Twist() topic!
import os
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from rosject_part2_pkg.srv import FindWall, FindWallRequest
class WallFollower:
def __init__(self):
# Initialize PID constants
self.Kp = 0.50
self.Ki = 0.0
self.Kd = 0.20
self.Need_calibration = True
self.rate = 25 # Hz
self.r = rospy.Rate(self.rate)
# Initialize error values
self.error_sum = 0.0
self.last_error = 0.0
# Initialize obstacle avoidance flag
self.avoid_obstacle = False
self.stuckon_on_wall = False
# Set the desired distance from the wall
self.desired_distance = 0.25
rospy.on_shutdown(self.shutdownhook)
# Subscribe to the LIDAR scan data
rospy.Subscriber("/scan", LaserScan, self.scan_callback)
# Publish to the turtlebot's velocity topic
self.velocity_publisher = rospy.Publisher(
"/cmd_vel", Twist, queue_size=10)
# set front and right variales
# front_distance = min(data.ranges[315:405]) CHANGED
self.front_distance = 0
self.right_distance = 0
self.right_frnt_distance = 0
self.right_rear_distance = 0
# if its the first run we need to find the wall first.
if self.Need_calibration is True:
self.service_call_func()
def service_call_func(self): # this is the service server client call
# Wait for the service client /find_wall to be running
rospy.wait_for_service('/find_wall')
# Create the connection to the service
find_wall_service = rospy.ServiceProxy('/find_wall', FindWall)
# Create an object of type find_wall_object to hold the requset sent to server
find_wall_object = FindWallRequest()
# Fill the variable find_wall_object of this object with the desired value 'go' to find wall / 'END' to terminate server
find_wall_object.str = 'go'
result = find_wall_service(find_wall_object)
rospy.loginfo("result of finding wall = %s", result)
if result.wallfound is True:
self.Need_calibration = False
rospy.logdebug("need calibration is = False")
self.terminate_wall_find_server()
def terminate_wall_find_server(self): # /wall_find node termination call
# Wait for the service client //find_wall to be running
rospy.wait_for_service('/find_wall')
# Create the connection to the service
find_wall_service = rospy.ServiceProxy('/find_wall', FindWall)
# Create an object of type find_wall_object
find_wall_object = FindWallRequest()
# Fill the variable find_wall_object of this object with the desired value 'go' to find wall / 'END' to terminate server
find_wall_object.str = 'END'
result = find_wall_service(find_wall_object)
rospy.loginfo("result of terminating service = %s", result)
if result.wallfound is True:
# KILL SSERVICE NODE
os.system('rosnode kill /findwall_service_server_node ')
# *****************************************************************************************************************************
def scan_callback(self, data):
self.front_distance = data.ranges[359]
self.right_distance = min(data.ranges[90:270])
self.right_frnt_distance = data.ranges[269]
self.right_rear_distance = data.ranges[89]
# main wall following metod
if self.Need_calibration is False:
# wall already found Need_Calibration= False
# minimum of the front 15% of array
rospy.logdebug("front_distance = %f", self.front_distance)
rospy.logdebug("right_distance = %f", self.right_distance)
# robot stuck face to wall/obstacle
if self.front_distance < 0.15:
vel_msg = Twist()
vel_msg.linear.x = -0.05
vel_msg.angular.z = 0.00
self.velocity_publisher.publish(vel_msg)
print("robot stuck face to wall")
#self.r.sleep() CHANGE
# self.r.sleep()
return
# case that robot needs to turn to next wall
if self.front_distance < 0.50 and self.front_distance > 0.15:
self.avoid_obstacle = True
if self.avoid_obstacle:
vel_msg = Twist()
vel_msg.linear.x = 0.02
vel_msg.angular.z = 0.50
self.velocity_publisher.publish(vel_msg)
print("avoid wall")
#self.r.sleep() CHANGE
if self.front_distance > 0.50:
self.avoid_obstacle = False
return
if self.right_distance > 100.0:
self.stuckon_on_wall = True
vel_msg = Twist()
vel_msg.linear.x = 0.01
vel_msg.angular.z = 0.5
self.velocity_publisher.publish(vel_msg)
self.r.sleep()
return
if self.stuckon_on_wall is True and self.right_distance < 100.0:
self.stuckon_on_wall = False
else:
error = self.desired_distance - self.right_distance
#error = self.right_frnt_distance - self.right_rear_distance
# Calculate the integral
self.error_sum += error
# Calculate the derivative
error_derivative = error - self.last_error
# Calculate the output
output = self.Kp * error + self.Ki * self.error_sum + self.Kd * error_derivative
rospy.logdebug("Output = %f", output)
# Create a Twist message to control the turtlebot's velocity
vel_msg = Twist()
vel_msg.linear.x = 0.05
vel_msg.angular.z = output
# Publish the velocity message
str_pub = "publishing"
# before publishing -filter the output speed to top of 0.5 rad/sec
if output > 0.5 or output < -0.5:
rospy.logwarn("filtered the z speed to 0.5")
rospy.loginfo(str_pub)
self.velocity_publisher.publish(vel_msg)
# Update the previous error
self.last_error = error
#self.r.sleep() CHANGE
def shutdownhook(self):
# define the above mentioned function and send the twist messages here to stop the robot
rospy.loginfo("Stopping the robot...")
vel_msg = Twist()
vel_msg.linear.x = 0.0
vel_msg.angular.z = 0.0
self.velocity_publisher.publish(vel_msg)
if __name__ == '__main__':
rospy.init_node('laser_subscriber_node', log_level=rospy.DEBUG)
vel = Twist()
wall_follower = WallFollower()
rospy.spin()