Hello,
I am working on the Rosject of the ROS2 In 5 Days Python course. I created and tested both the wall_following node and the wall_finding service separately using the terminal to launch the controller and to call the service server and both scripts are working as expected. However, my current task is to launch both the the service server node and the robot controller (the ‘wall_following’) node at the same time. In addition I have to modify the ‘wall_following’ node in order to create a synchronous service client to call the ‘wall_finding’ service server. But I am having trouble to make the code work. I am not sure what I am doing wrong. But I think there is a problem with the way I am sending the synchronous call in my main function and checking for the response in my ‘motion’ callback function.
My robot controller code is below:
import sys
from threading import Thread
# import python ros client
import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import Twist module from geometry_msgs.msg
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from custom_interface.srv import FindWall
from std_srvs.srv import Empty
# import Quality of Service library, to set the correct profile and reliability to read sensor data.
from rclpy.qos import ReliabilityPolicy, QoSProfile
class Wall_Following(Node):
def __init__(self):
# naming the class constructor (node name)
super().__init__('wall_following')
# creating client for the /find_wall service server
self.cli = self.create_client(FindWall, 'find_wall')
while not self.cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
# self.req = FindWall.Request()
self.req = Empty.Request()
# creating the publisher object
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
# creating the subscriber object
self.subscriber_ = self.create_subscription(
LaserScan,
'/scan',
self.read_measurement,
# is the most used to read LaserScan data and some sensor data.
QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
)
# define the timer period for 0.5 seconds
self.timer_period = 0.5
# declare variable to store all the laser messages
self.laser_msg = LaserScan()
# declare the variable to save the front laser scan reading
self.forward_laser = 0.0
# declare the variable to save the right laser scan reading
self.right_laser = 0.0
# declare the velocity twist message
self.cmd = Twist()
# create time
self.timer = self.create_timer(self.timer_period, self.motion)
def stop_robot(self):
self.get_logger().info('STOPPING ROBOT...')
self.cmd.linear.x = 0.0
self.cmd.angular.z = 0.0
self.publisher_.publish(self.cmd)
def rotate_left(self):
self.get_logger().info(
f'Moving away from right wall - distance to right wall = {self.right_laser} m')
self.cmd.linear.x = 0.1 # making sure that the robot is not moving forward
self.cmd.angular.z = 0.2 # rotate counterclockwise to point to the closest wall
self.publisher_.publish(self.cmd)
def rotate_right(self):
self.get_logger().info(
f'Moving away from right wall - distance to right wall = {self.right_laser} m')
self.cmd.linear.x = 0.1 # making sure that the robot is not moving forward
self.cmd.angular.z = -0.2 # rotate counterclockwise to point to the closest wall
self.publisher_.publish(self.cmd)
def move_forward(self):
self.get_logger().info('MOVING ROBOT FORWARD...')
self.cmd.linear.x = 0.1
self.cmd.angular.z = 0.0
self.publisher_.publish(self.cmd)
def hard_left_rotation(self):
self.get_logger().info(
f'Getting close to front wall - changing direction - distance to front wall = {self.forward_laser} m')
self.cmd.linear.x = 0.05
self.cmd.angular.z = 0.4
self.publisher_.publish(self.cmd)
def send_request(self):
self.future = self.cli.call(self.req)
def motion(self):
if self.future.result():
# check if the robot is far from the front wall
if self.forward_laser > 0.5:
# if the robot is far from the front wall and far from the right wall, move closer to the right wall
if self.right_laser > 0.3:
self.rotate_right()
# if the robot is far from the front wall and close to the right wall, move away from the right wall
elif self.right_laser < 0.3:
self.rotate_left()
# if the robot is far from the front wall is at a distance between 0.2 m and 0.3 m from the right wall, keep moving forward
else:
self.move_forward()
# if the robot is close to a front wall, increase angular velocity while still going forward
else:
self.hard_left_rotation()
self.get_logger().info(
f'Front laser measurement : {self.forward_laser} m')
self.get_logger().info(
f'Right laser measurement : {self.right_laser} m')
else:
self.get_logger().info('Server response is FALSE')
def read_measurement(self, msg):
self.laser_msg = msg
self.forward_laser = self.laser_msg.ranges[359]
self.right_laser = self.laser_msg.ranges[179]
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
# declare the node constructor
wall_following = Wall_Following()
spin_thread = Thread(target=rclpy.spin, args=(wall_following,))
spin_thread.start()
wall_following.send_request()
wall_following.destroy_node()
# shutdown the ROS communication
rclpy.shutdown()
if __name__ == '__main__':
main()
Moreover, my launch file is the following:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='wall_follower',
executable='wall_finding',
output='screen',
arguments=['--ros-args', '--log-level', 'info']),
Node(
package='wall_follower',
executable='wall_following_v2',
output='screen',
arguments=['--ros-args', '--log-level', 'info'])
])
And, this is the terminal output when I launch the launch file:
ros2 launch wall_follower main.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2022-12-25-11-32-42-413569-3_xterm-9801
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [wall_finding-1]: process started with pid [9898]
[INFO] [wall_following_v2-2]: process started with pid [9902]
[wall_finding-1] [INFO] [1671967965.949427122] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967966.243057828] [find_wall_service]: read laser measurement
[wall_following_v2-2] Traceback (most recent call last):
[wall_following_v2-2] File "/home/user/ros2_ws/install/wall_follower/lib/wall_follower/wall_following_v2", line33, in <module>
[wall_following_v2-2] sys.exit(load_entry_point('wall-follower==0.0.0', 'console_scripts', 'wall_following_v2')())
[wall_following_v2-2] File "/home/user/ros2_ws/install/wall_follower/lib/python3.8/site-packages/wall_follower/wall_following_v2.py", line 174, in main
[wall_following_v2-2] wall_following.send_request()
[wall_following_v2-2] File "/home/user/ros2_ws/install/wall_follower/lib/python3.8/site-packages/wall_follower/wall_following_v2.py", line 132, in send_request
[wall_following_v2-2] self.future = self.cli.call(self.req)
[wall_following_v2-2] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/client.py", line 80, in call
[wall_following_v2-2] raise TypeError()
[wall_following_v2-2] TypeError
[wall_following_v2-2] Exception in thread Thread-1:
[wall_following_v2-2] Traceback (most recent call last):
[wall_following_v2-2] File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
[wall_following_v2-2] self.run()
[wall_following_v2-2] File "/usr/lib/python3.8/threading.py", line 870, in run
[wall_following_v2-2] self._target(*self._args, **self._kwargs)
[wall_following_v2-2] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/__init__.py", line 191, in spin
[wall_following_v2-2] executor.spin_once()
[wall_following_v2-2] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 711, in spin_once
[wall_following_v2-2] raise handler.exception()
[wall_following_v2-2] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/task.py", line 239, in __call__
[wall_following_v2-2] self._handler.send(None)
[wall_following_v2-2] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 426, in handler
[wall_following_v2-2] await call_coroutine(entity, arg)
[wall_following_v2-2] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 340, in _execute_timer
[wall_following_v2-2] await await_or_execute(tmr.callback)
[wall_following_v2-2] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 118, in await_or_execute
[wall_following_v2-2] return callback(*args)
[wall_following_v2-2] File "/home/user/ros2_ws/install/wall_follower/lib/python3.8/site-packages/wall_follower/wall_following_v2.py", line 136, in motion
[wall_following_v2-2] if self.future.result():
[wall_following_v2-2] AttributeError: 'Wall_Following' object has no attribute 'future'
[wall_finding-1] [INFO] [1671967966.762792541] [find_wall_service]: read laser measurement
[ERROR] [wall_following_v2-2]: process has died [pid 9902, exit code 1, cmd '/home/user/ros2_ws/install/wall_follower/lib/wall_follower/wall_following_v2 --ros-args --log-level info --ros-args'].
[wall_finding-1] [INFO] [1671967967.110928746] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967967.442750430] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967967.744603054] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967968.077909535] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967968.373987608] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967968.648504300] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967969.051760631] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967969.335983051] [find_wall_service]: read laser measurement
[wall_finding-1] [INFO] [1671967969.622519798] [find_wall_service]: read laser measurement
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[wall_finding-1] Traceback (most recent call last):
[wall_finding-1] File "/home/user/ros2_ws/install/wall_follower/lib/wall_follower/wall_finding", line 33, in <module>
[wall_finding-1] sys.exit(load_entry_point('wall-follower==0.0.0', 'console_scripts', 'wall_finding')())
[wall_finding-1] File "/home/user/ros2_ws/install/wall_follower/lib/python3.8/site-packages/wall_follower/wall_finder.py", line 148, in main
[wall_finding-1] executor.spin()
[wall_finding-1] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 290, in spin
[wall_finding-1] self.spin_once()
[wall_finding-1] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 756, in spin_once
[wall_finding-1] self._spin_once_impl(timeout_sec)
[wall_finding-1] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 742, in _spin_once_impl
[wall_finding-1] handler, entity, node = self.wait_for_ready_callbacks(
[wall_finding-1] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 689, in wait_for_ready_callbacks
[wall_finding-1] return next(self._cb_iter)
[wall_finding-1] File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 586, in _wait_for_ready_callbacks
[wall_finding-1] _rclpy.rclpy_wait(wait_set, timeout_nsec)
[wall_finding-1] KeyboardInterrupt
[ERROR] [wall_finding-1]: process has died [pid 9898, exit code -2, cmd '/home/user/ros2_ws/install/wall_follower/lib/wall_follower/wall_finding --ros-args --log-level info --ros-args'].
Thank you for your help
