"""
.. module:: middleman
:platform: Unix
:synopsis: Python module for controlling the robot
.. moduleauthor:: Samuele Depalo <depalo.samuele@gmail.com>
The node choose the correct speed for the robot, basing on the user desires and the surrounding obstacles
Subscribes to:
/scan where the latest laser scans are published
/middleman/control where the commands are published by the user_interface node
/middleman/cmd_vel where the keyboard-given velocities are published
Publishes to:
/cmd_vel the new twist
"""
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from final_assignment.msg import CommandMessage
pub = rospy.Publisher("/cmd_vel", Twist) #Publisher of the velocity commands
"""
Global publisher for the velocities
"""
keyboard_status = False #Enable the keyboard-given control commands
"""
Global boolean to enable/disable the keyboard-given control commands
"""
helper_status = False #Enable the driving assistant
"""
Global boolean to enable/disable the driving assistant
"""
desired_speed = {'linear': 0, 'angular': 0} #Desired velocity due to keyboard-given control commands
"""
Global vector where the latest desired velocitties are saved
"""
[docs]def clbk_laser(msg): #Each time a new laser scan is received
"""
Function for defining the closest obstacle in the right, frontal and left regions
Parameters:
msg:LaserScan the latest laser scan result published on /scan
Returns:
None
"""
if helper_status: #If the assistant is enabled update the regions
regions = {
'right': min(min(msg.ranges[0:287]), 10),
'front': min(min(msg.ranges[288:431]), 10),
'left': min(min(msg.ranges[432:719]), 10),
}
update_vel(regions) #update the velocity
[docs]def update_vel(regions): # The velocity is updated depending on the close obstacles and on the desired speed
"""
Function for updating the robot velocities depending on the surrounding obstacles
Parameters:
regions:vector the closest obstacles distances in the right, frontal and left regions
Returns:
None
"""
vel = Twist()
if regions['front'] < 1 and desired_speed['linear'] > 0: #If there are close obstacle in the front sector
vel.linear.x = 0.0 # set the linear speed to zero
else:
vel.linear.x = desired_speed['linear'] #the robot drive at the desired speed
if (regions['left'] < 1 and desired_speed['angular'] > 0) or (regions['right'] < 1 and desired_speed['angular'] < 0): #If there are close obstacle in the lateral sector
vel.angular.z = 0.0 # set the angular speed to zero
else:
vel.angular.z = desired_speed['angular'] #the robot drive at the desired speed
pub.publish(vel)
[docs]def commandCallBack(cmd): #Each time a new command from the UI is received
"""
Function for setting the behaviour of the robot depending on the command received
Parameters:
cmd:CommandMessage sent by the user_interface node
Returns:
None
"""
global desired_speed, keyboard_status, helper_status
vel = Twist()
#set both the desired speed and the actual speed to 0
desired_speed['linear'] = 0
desired_speed['angular'] = 0
vel.linear.x = 0
vel.angular.z = 0
pub.publish(vel)
#Set the booleans as requested by the user
keyboard_status = cmd.enable_userCtrl
helper_status = cmd.enable_helper
[docs]def new_vel(vel): #Each time a new command from the keyboard is received
"""
Function for updating the desired velocities depending on keyboard-given command
Parameters:
vel:Twist sent by the keyboard
Returns:
None
"""
global desired_speed
if keyboard_status: #Do something only if the keyboard it enabled
if helper_status: #If the assistant is enabled, just update the desired speed
desired_speed['linear'] = vel.linear.x
desired_speed['angular'] = vel.angular.z
else:
pub.publish(vel) #publish the new speed without any modifications
[docs]def main():
"""
This function initializes the ROS node and wait for the messages to be published on the topics in order to process them
"""
rospy.init_node("middleman")
rospy.Subscriber('/scan', LaserScan, clbk_laser)
rospy.Subscriber('/middleman/control', CommandMessage, commandCallBack)
rospy.Subscriber('/middleman/cmd_vel', Twist, new_vel)
rospy.spin()
if __name__ == '__main__':
main()