Hello,
My question is the about the service callback which is “def find_wall_cb(self, request, response)” below.
Right now I return immediately, but the textbook says:
- Identify which laser ray is the shortest one. Assume that this is the one pointing to a wall
- Rotate the robot until the front of it is facing the wall. This can be done by publishing an angular velocity until front ray is the smallest one
- Move the robot forward until the front ray is smaller than 30cm
- Now rotate the robot again until ray number 270 of the laser ranges is pointing to the wall
- At this point, consider that the robot is ready to start following the wall
6. Return the service message withTrue
If I don’t return right away, the whole program is deadlocked. You can see the timer function starts with a logger: that logger stops until a result returned.
Right now I use a timer, but usually I would put the “find_wall_main_loop” logic directly as part of the subscriber callback. Maybe not best practice, but in this case it’s the same result => deadlock. So unless I end the service callback, nothing “spins”.
As you see, I tried to spin_once, and I was looking at creating a future to use spin_until_… but I’m running out of time today.
Anyway, any good way to keep spinning until a result is ready?
import rclpy
from rclpy.node import Node
from std_srvs.srv import Empty
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup
class FindWallService(Node):
def __init__(self):
super().__init__("FindWallSvcNode")
self.create_service(Empty, "find_wall", callback=self.find_wall_cb)
self.create_subscription(
LaserScan, "/scan",
self.on_laser_data,
qos_profile=QoSProfile(
depth=10,
reliability=ReliabilityPolicy.RELIABLE
)
)
self.vel_pub = self.create_publisher(Twist, "/cmd_vel", 10)
self.is_running = False
self.is_ready = False
self.timer = self.create_timer(0.2, self.find_wall_main_loop)
def on_laser_data(self, msg):
self.msg = msg
def find_wall_main_loop(self):
self.get_logger().info("is running? " + str(self.is_running))
if self.is_running:
max_angle = 719
target_angle = 540
target_distance = 0.3
angle, distance = self.find_min_range(self.msg)
if distance > target_distance:
if self.is_in_front(angle, max_angle):
self.get_logger().info("going towards wall")
msg = Twist()
msg.linear.x = 0.15
self.vel_pub.publish(msg)
else:
self.get_logger().info("turning towards wall")
self.turn_to_target(angle, 0, max_angle)
elif distance <= target_distance:
if abs(target_angle-angle) < 10:
self.get_logger().info("ready!")
self.is_ready = True
self.vel_pub(Twist())
else:
self.get_logger().info("turning towards target angle")
self.turn_to_target(angle, target_angle, max_angle)
def turn_to_target(self, angle, target_angle, max_angle):
msg = Twist()
# some logic
self.vel_pub.publish(msg)
def find_wall_cb(self, request, response):
self.get_logger().info("hello")
self.is_ready = False
self.is_running = True
return response
while not self.is_ready:
rclpy.spin_once(self)
return response
def main(args=None):
rclpy.init(args=args)
my_service = FindWallService()
rclpy.spin(my_service)
my_service.destroy_node()
rclpy.shutdown()
if __name__=="__main__":
main()
)