I’m having trouble updating my laser scan values so that my while loop breaks when the robot is oriented properly with respect to the nearest wall. Is there a way to call the subscriber callback in my while loop to update the laser scan values as it iterates through the loop? Or should I go about this a different way?
Hi @ARodas ,
Is it possible for you to share your code?
A question about a program’s behavior can be answered only when there is supporting program context / code.
Regards,
Girish
Heres my code, sorry about the mess. Was trying to get it to work and left organization slip. I tried to use multithreading since I heard this is a way to have two callback functions running simultaneously in the same code. It does not work properly, the terminal only gets Finding Nearest Wall but does not even print the values I am trying to get to verify the scan values in the while loop are in fact changing.
# import the Twist module from geometry_msgs messages interface
from geometry_msgs.msg import Twist
# import the MyCustomServiceMessage module from custom_interfaces package
from custom_interfaces.srv import FindWall
# import the ROS2 Python client libraries
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
class Service(Node):
def __init__(self):
# Here you have the class constructor
self.g1 = MutuallyExclusiveCallbackGroup()
self.g2 = MutuallyExclusiveCallbackGroup()
self.laser_forward = None
self.laser_min = None
self.laser_right = None
# call the class constructor to initialize the node as service_stop
super().__init__('find_wall_server')
# define the variable to save the received info
# create the Publisher object
# in this case, the Publisher will publish on /cmd_vel topic with a queue size of 10 messages.
# use the Twist module
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
self.laser_forward = None
self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE),callback_group=self.g2)
# create the Service Server object
# defines the type, name, and callback function
self.srv = self.create_service(FindWall, 'find_wall', self.custom_service_callback,callback_group=self.g1)
# define the timer period for 0.5 seconds
self.timer_period = 0.5
# create a Twist message
self.cmd = Twist()
#self.timer = self.create_timer(self.timer_period, self.laser_callback)
def laser_callback(self,msg):
self.laser_min = min(msg.ranges)
self.laser_forward = msg.ranges[359]
self.laser_right = msg.ranges[270]
self.get_logger().info('Forward Scan: "%s"' % str(self.laser_forward))
self.get_logger().info('Min Scan: "%s"' % str(self.laser_right))
self.get_logger().info('Min Scan: "%s"' % str(self.laser_min))
def custom_service_callback(self, request, response):
# The callback function receives the self-class parameter,
# received along with two parameters called request and response
# - receive the data by request
# - return a result as a response
self.get_logger().info('Finding Nearest Wall')
while self.laser_forward>self.laser_min+0.1:
self.cmd.angular.z=0.2
self.publisher_.publish(self.cmd)
self.get_logger().info('Forward Scan: "%s"' % str(self.laser_forward))
self.get_logger().info('Min Scan: "%s"' % str(self.laser_right))
self.get_logger().info('Min Scan: "%s"' % str(self.laser_min))
self.cmd.angular.z=0.0
self.publisher_.publish(self.cmd)
self.get_logger().info('Appproaching Nearest Wall')
while self.laser_forward>0.3:
self.cmd.linear.x=0.2
self.publisher_.publish(self.cmd)
self.cmd.linear.x=0.0
self.publisher_.publish(self.cmd)
self.get_logger().info('Properly Orienting Robot')
while self.laser_right>self.laser_min+0.1:
self.cmd.angular.z=0.2
self.publisher_.publish(self.cmd)
self.cmd.angular.z=0.0
self.publisher_.publish(self.cmd)
if self.laser_right<0.35:
self.get_logger().info('Robot Properly Oriented')
response.wallfound = True
else:
# response state
response.wallfound = False
# return the response parameter
return response
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
try:
# declare the node constructor
service = Service()
executor = MultiThreadedExecutor(num_threads=2)
executor.add_node(service)
try:
executor.spin()
finally:
executor.shutdown()
service.destroy_node()
finally:
rclpy.shutdown()
# pause the program execution, waits for a request to kill the node (ctrl+c)
#rclpy.spin(service)
# shutdown the ROS communication
#rclpy.shutdown()
if __name__ == '__main__':
main()```Hi @ARodas ,
As I started going through your code, I saw you had two MutuallyExclusiveCallbackGroups for the callbacks of subscriber and service. I stopped going through your code right there.
You need to use ReentrantCallbackGroup to have more than one callback work in parallel.
Make this change and let me know if your problem is fixed. If not, then let me know.
Regards,
Girish
I made the change however, the code is still not working properly. The subscriber call back seems to not be running at all. The service callback seems to be working fine when I call the service. Here’s , my updated code for reference.
# import the Twist module from geometry_msgs messages interface
from geometry_msgs.msg import Twist
# import the MyCustomServiceMessage module from custom_interfaces package
from custom_interfaces.srv import FindWall
# import the ROS2 Python client libraries
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
class Service(Node):
def __init__(self):
# Here you have the class constructor
# call the class constructor to initialize the node as service_stop
super().__init__('find_wall_server')
# define the variable to save the received info
self.g1 = ReentrantCallbackGroup()
self.g2 = ReentrantCallbackGroup()
self.laser_forward = None
self.laser_min = None
self.laser_right = None
# create the Publisher object
# in this case, the Publisher will publish on /cmd_vel topic with a queue size of 10 messages.
# use the Twist module
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
self.laser_forward = None
self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE),callback_group=self.g2)
# create the Service Server object
# defines the type, name, and callback function
self.srv = self.create_service(FindWall, 'find_wall', self.custom_service_callback,callback_group=self.g1)
# define the timer period for 0.5 seconds
# create a Twist message
self.cmd = Twist()
#self.timer = self.create_timer(self.timer_period, self.laser_callback)
def laser_callback(self,msg):
self.laser_min = min(msg.ranges)
self.laser_forward = msg.ranges[359]
self.laser_right = msg.ranges[270]
self.get_logger().info('Forward Scan: "%s"' % str(self.laser_forward))
self.get_logger().info('Min Scan: "%s"' % str(self.laser_right))
self.get_logger().info('Min Scan: "%s"' % str(self.laser_min))
def custom_service_callback(self, request, response):
# The callback function receives the self-class parameter,
# received along with two parameters called request and response
# - receive the data by request
# - return a result as a response
self.get_logger().info('Finding Nearest Wall')
while self.laser_forward>self.laser_min+0.1:
self.cmd.angular.z=0.2
self.publisher_.publish(self.cmd)
self.cmd.angular.z=0.0
self.publisher_.publish(self.cmd)
self.get_logger().info('Appproaching Nearest Wall')
while self.laser_forward>0.3:
self.cmd.linear.x=0.2
self.publisher_.publish(self.cmd)
self.cmd.linear.x=0.0
self.publisher_.publish(self.cmd)
self.get_logger().info('Properly Orienting Robot')
while self.laser_right>self.laser_min+0.1:
self.cmd.angular.z=0.2
self.publisher_.publish(self.cmd)
self.cmd.angular.z=0.0
self.publisher_.publish(self.cmd)
if self.laser_right<0.35:
self.get_logger().info('Robot Properly Oriented')
response.wallfound = True
else:
# response state
response.wallfound = False
# return the response parameter
return response
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
try:
# declare the node constructor
service = Service()
executor = MultiThreadedExecutor(num_threads=2)
executor.add_node(service)
try:
executor.spin()
finally:
executor.shutdown()
service.destroy_node()
finally:
rclpy.shutdown()
# pause the program execution, waits for a request to kill the node (ctrl+c)
#rclpy.spin(service)
# shutdown the ROS communication
#rclpy.shutdown()
if __name__ == '__main__':
main()
The issue is that there is an error but yu don’t see it until you stop the service server.
The error appears in the part of the code:
while self.laser_forward>self.laser_min+0.1:
Here when the service is called, if the laser readings weren’t initialised. This happens if the laser readings are note received before the service call.
The following exception was never retrieved: unsupported operand type(s) for +: ‘NoneType’ and ‘float’
Otherwise, it works.
I posted here the code in a git. I changed the name of the packages, and I imagine that the custom message is not exactly the same but sure you will get the idea.
To execute a dry test ( no need of simulation) , execute the following :
ros2 run issues_playground service_subscriber_test.py
ros2 run issues_playground service_findwall_client.py
ros2 run issues_playground scan_publisher.py
ros2 topic echo /cmd_vel
You should see that the service callback ends and the cmd_vel topic is being published.
Hope it helps:
1 Like
The code works great now! Thank you very very much for your time and help!
1 Like