Hi, I am currently trying to do the Part 2 of ROS2 Basics Python Real Robot Project .
I am trying to move forward & detect laser in a loop until the distance to wall is less them 0.3m.
Here’s my code…
def CustomService_callback(self, request, response):
# The callback function recives the self class parameter,
# received along with two parameters called request and response
# - receive the data by request
# - return a result as response
# create a Twist message
msg = Twist()
self.get_logger().info(f'----A {self.laser_f}')
if self.laser_f > 0.3:
self.get_logger().info('GO~~~!!')
msg.linear.x = 0.3
msg.angular.z = 0.0
self.publisher_.publish(msg)
while self.laser_f > 0.3:
self.get_logger().info(f"----B {self.laser_f}")
pass
self.get_logger().info('Found~~~!!')
msg.linear.x = 0.0
msg.angular.z = 0.0
self.publisher_.publish(msg)
# response state
response.wallfound = True
# response.message = "Found wall"
# return the response parameter
return response
and launch the service with the command
ros2 service call /find_wall custom_interfaces/FindWall '{}'
My question is that the subscriber never triggered if the loop start in CustomService_callback (while self.laser_f > 0.3)
What should I do to return the service response after completing a series of motor controling and laster detecting
I don’t quite understand your issue. What is the problem? Is the subscriber to scan not working?
It looks like in the output the program did enter the condition you made of
Howerver the self.subscriber (i.e. update_scan() or “XXXXXX”) stop updating the self.laser_f data after the loop start
while self.laser_f > 0.3:
self.get_logger().info(f"----B {self.laser_f}")
pass
The requirement of Part II is to receive a msg, do something, then make a response.
I think I have to make a loop to make sure everything is done before return a response, but it seems the loop blocks the self.subscriber.
When the service is called, the robot must do the following behavior:
Identify which laser ray is the shortest. Assume that that is the one pointing to a wall.
Rotate the robot until the front of the robot is facing the wall. This can be done by rotating the robot until ray 0 is the smaller one.
Move the robot forward until ray 0 is shorter than 0.3m.
Now, rotate the robot again until ray number 270 of the laser range is pointing to the wall.
At this point, assume that the robot is situated to start moving along the wall.
Return the service message with a True.
How to test the service
Launch the service server node
Use the terminal to test the service with a service call command
ros2 service call /find_wall std_srvs/Empty '{}'
Step1 is only service server part, no client code is involved, and Service server is triggerred from basic terminal command.
I want to move robot and receive laser scanner info at the same time to adjust its location (or rotation) until it matches the condition. That’s why I need a loop to control this before response.
I now understand where you are in the project, sorry about that.
So I assume your server makes the robot rotate but it doesn’t allow to stay subscribed to the laser.
Can you share the whole script? It is difficult to understand only with snippets.
Also, make sure you are setting up your service and topic subscribers callbacks as a ReentrantCallbackGroup() and then in your main you can setup a multi threaded executor with 3 threads:
Thank you! I think what I need is how to apply the multithread in ROS2.
Here is my code after your suggestion.
class FindwallService(Node):
def __init__(self):
# Here we have the class constructor
# call the class constructor to initialize the node as service_stop
super().__init__('findwall_server')
# create the service server object
# defines the type, name and callback function
self.group1 = ReentrantCallbackGroup()
self.group2 = ReentrantCallbackGroup()
self.group3 = ReentrantCallbackGroup()
self.srv = self.create_service(
Empty, 'find_wall', self.CustomService_callback, callback_group=self.group1)
# 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, callback_group=self.group2)
self.subscriber = self.create_subscription(
LaserScan, '/scan', self.update_scan, QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT), callback_group=self.group3)
self.laser_f = 0
def update_scan(self, msg): # LaserScan
self.get_logger().info(f'XXXXXX {self.laser_f}')
self.laser_f = min(msg.ranges[170:189])
def CustomService_callback(self, request, response):
# The callback function recives the self class parameter,
# received along with two parameters called request and response
# - receive the data by request
# - return a result as response
# create a Twist message
msg = Twist()
self.get_logger().info(f'----A {self.laser_f}')
if self.laser_f > 0.3:
self.get_logger().info('GO~~~!!')
msg.linear.x = 0.3
msg.angular.z = 0.0
self.publisher_.publish(msg)
while self.laser_f > 0.3:
self.get_logger().info(f"----B {self.laser_f}")
time.sleep(1)
pass
self.get_logger().info('Found~~~!!')
msg.linear.x = 0.0
msg.angular.z = 0.0
self.publisher_.publish(msg)
# response state
# response.wallfound = True
# response.message = "Found wall"
# return the response parameter
return response
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
# # declare the node constructor
# service = FindwallService()
# # pause the program execution, waits for a request to kill the node (ctrl+c)
# rclpy.spin(service)
# # shutdown the ROS communication
# rclpy.shutdown()
try:
# declare the node constructor
findwall_service = FindwallService()
executor = MultiThreadedExecutor(num_threads=3)
executor.add_node(findwall_service)
try:
# pause the program execution, waits for a request to kill the node (ctrl+c)
executor.spin()
finally:
executor.shutdown()
findwall_service.destroy_node()
finally:
# shutdown the ROS communication
rclpy.shutdown()
if __name__ == '__main__':
main()