#! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
pub = None
def clbk_laser(msg):
regions = {
'right': min(min(msg.ranges[:143]), 10),
'fright': min(min(msg.ranges[144:287]), 10),
'front': min(min(msg.ranges[288:431]), 10),
'fleft': min(min(msg.ranges[432:575]), 10),
'left': min(min(msg.ranges[576:713]), 10),
}
take_action(regions)
def take_action(regions):
msg = Twist()
linear_x = 0
angular_z = 0
state_description = ''
if regions['front'] > 1 and regions['fleft'] > 1 and regions['fright'] > 1:
state_description = 'case 1 - nothing'
linear_x = 0.6
angular_z = 0
elif regions['front'] < 1 and regions['fleft'] > 1 and regions['fright'] > 1:
state_description = 'case 2 - front'
linear_x = 0
angular_z = -0.3
elif regions['front'] > 1 and regions['fleft'] > 1 and regions['fright'] < 1:
state_description = 'case 3 - fright'
linear_x = 0
angular_z = -0.3
elif regions['front'] > 1 and regions['fleft'] < 1 and regions['fright'] > 1:
state_description = 'case 4 - fleft'
linear_x = 0
angular_z = 0.3
elif regions['front'] < 1 and regions['fleft'] > 1 and regions['fright'] < 1:
state_description = 'case 5 - front and fright'
linear_x = 0
angular_z = -0.3
elif regions['front'] < 1 and regions['fleft'] < 1 and regions['fright'] > 1:
state_description = 'case 6 - front and fleft'
linear_x = 0
angular_z = 0.3
elif regions['front'] < 1 and regions['fleft'] < 1 and regions['fright'] < 1:
state_description = 'case 7 - front and fleft and fright'
linear_x = 0
angular_z = -0.3
elif regions['front'] > 1 and regions['fleft'] < 1 and regions['fright'] < 1:
state_description = 'case 8 - fleft and fright'
linear_x = 0
angular_z = -0.3
else:
state_description = 'unknown case'
rospy.loginfo(regions)
rospy.loginfo(state_description)
msg.linear.x = linear_x
msg.angular.z = angular_z
pub.publish(msg)
def main():
global pub
rospy.init_node('obstacle_detection')
pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10)
sub = rospy.Subscriber('/scan', LaserScan, clbk_laser)
rospy.spin()
if __name__ == '__main__':
main()
Hi @leyla1491625,
Which course/unit are you taking? We can help you better if we knew the specific problem you are trying to solve.
Hi @leyla1491625,
according to the logs I can see in the image you shared, it always goes to else
because you are not taking into consideration when the laser reports nan (not a number)
.
This is normally because the laser is not detecting anything in one of the lasers, so, when you do something like:
nan = float('nan')
right = min(min, 10)
the value assigned to right
in this case would always be nan
.
You just have to consider:
if desired_variable == float('nan'):
do_something()