The task I am currently working on is: A robot that is installed with a LIDAR sensor and placed in an enclosed warehouse. The robot initially is placed at any random position in the environment. My task is to guide the robot around the warehouse using the values from the sensor, such as wall following etc. The problem I am facing currently is that I need the robot to start at a position before it does the wall following task. This position is such that the robot first needs to get close to the nearest wall and place itself parallel to the wall, ready to begin the wall following.
I created two '.py' files, a server file and a client file.
The client file calls the server file to do the wall search task. Once the task is complete the rest of the code in the client file is to begin the wall following. I included the code with the problems in it.
msg.ranges[360] (value of front laser range) is the variable that I need help with.
# The lidar sensor has a range form 0 to 180 degress with index range 0-720. The laser at 360 is the front laser or the laser with 0 angle.
# The laser at 0 and 719 are at -90 and 90 angle.
#! /usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from rosject.srv import findwall, findwallResponse #custom service message
def callback(msg):
r = list(msg.ranges)
minimum_value = min(r)
print(minimum_value)
index_minimum = r.index(minimum_value)
ray_angle = msg.angle_min + (index_minimum * msg.angle_increment)
#The distance and angle of an obstacle with respect to the laser scanner device must be calculated using
#The fields angle_min and angle_increment: ray_angle
print(ray_angle)
#Logic for duration of rotation
countdown_sec1 = 0
if ray_angle < 0:
twist_instance.angular.z = 0.2 #angular velocity in radians/second
else:
twist_instance.angular.z = -0.2
rotation_duration = (abs(ray_angle)/0.2) - 1
while countdown_sec1 <= rotation_duration:
pub.publish(twist_instance)
rate.sleep()
countdown_sec1 = countdown_sec1 + 1
#Once the robot faces the wall it is stopped
twist_instance.angular.z = 0
twist_instance.linear.x = 0
pub.publish(twist_instance)
rate.sleep()
print('front distance',msg.ranges[360]) #once stopped, I print the range of the front laser facing the wall
#This is where I faced the problem. the value that prints for the front laser is incorrect. I am unable to understand why.
#The following code is just to move the robot forward until it's within a certain distance from the wall.
difference = 0.3
forward_duration = round(difference/0.1)
countdown_sec2 = 0
twist_instance.linear.x = 0.1
twist_instance.angular.z = 0
while countdown_sec2 <= forward_duration:
pub.publish(twist_instance)
rate.sleep()
countdown_sec2 = countdown_sec2 + 1
twist_instance.linear.x = 0
pub.publish(twist_instance)
print('final distance', msg.ranges[360]) #Even here once the task is complete the the final value remains the same and incorrect.
# I let the callback run again and even then the value for the front laser does not change
def my_callback(request):
print('i am working boy')
my_response = findwallResponse()
my_response.wallfound = True
return my_response
if __name__ == '__main__':
rospy.init_node('findwall_node')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/scan', LaserScan, callback)
twist_instance = Twist()
rate = rospy.Rate(1)
my_service = rospy.Service('/find_wall', findwall, my_callback)
rospy.spin()
Where am I going wrong, why wont the value for msg.ranges[360] not change when the loop is run again by rospy.spin()?
msg won't change as you're still inside callback(msg). In your code, callback(msg) will be called everytime sub will receive a LiDAR scan message.
I suggest that you separate the code that receives the message from the code that waits and publishes, as you will lose laser scan messages if your node is still processing a msg.
Here is a way to do it in ROS1:
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int64, String
class MyNode:
# CLASS_VARIABLES
def __init__(self, start_value=0):
# INSTANCE_VARIABLES
self.counter = start_value
pass
def counter_callback(self, msg):
counter_data = msg.data
self.counter += counter_data
if __name__ == "__main__":
rospy.init_node("my_node")
rate = rospy.Rate(2)
node = MyNode()
sub = rospy.Subscriber("mynode_pub_topic", Int64, node.counter_callback)
string_pub = rospy.Publisher("mynode_pub_topic", String, queue_size=10)
string_instance = String()
while not rospy.is_shutdown():
string_instance.data = str(node.counter)
string_pub.publish(string_instance)
rate.sleep()
Other improvements could be to use publisher and subscribers as class attributes, as it is now done in ROS2.