I am not getting points for publishing to the topic “/cmd_vel” nor am I getting credit for subscribing to “/kobuki/laser/scan” even though I am doing both. The following is the code I used to submit for the quiz.
#! /usr/bin/env python
import rospy
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from robot_control_class import RobotControl
rospy.init_node('topics_quiz_node')
rate = rospy.Rate(2)
var = Twist()
def turn_direction(radians):
var.linear.x = 0.0
var.angular.z = radians
pub.publish(var)
def move_straight():
var.linear.x = 0.1
#1.5708 radians in 90degrees
var.angular.z = 0.0
pub.publish(var)
def stop_evaluate(ranges):
var.linear.x = 0.0
#1.5708 radians in 90degrees
var.angular.z = 0.0
pub.publish(var)
def get_max_direction_distance(ranges):
readings = {k: v for k, v in enumerate(ranges)}
#direction, distance,
maxim = [0,0]
for k,v in readings.items():
if v > maxim[1]:
maxim[1] = v
maxim[0] = k
return maxim
def evaluate_direction(ranges):
left = ranges[:360]
center_left = ranges[350:360]
straight = ranges[360]
right = ranges[361:]
center_right = ranges[361:371]
# if min(right) < 0.3):
# turn_direction(1.5708* 1.8)
# elif min(left) < 0.3):
# turn_direction(-1.5708* 1.8)
if straight > 1:
if(abs(min(center_left) - min(center_right)) > 0.3 and max(right) > 0.9):
#if (max(right) > 0.9):
turn_direction(1.5708* 1.8)
elif(abs(min(center_left) - min(center_right)) > 0.3 and max(left) > 0.9):
#elif(max(left) > 0.9):
turn_direction(-1.5708* 1.8)
else:
move_straight()
elif straight < 1:
if max(right) == max(left):
#1.5708 radians in 90degrees
#left is positive
#right is negative
#turn_left
turn_direction(1.5708* 1.8)
elif min(right) < 1: #right side
#turn_left
turn_direction(1.5708 * 1.8)
elif min(left) < 1: #left side
#turn right
turn_direction(-1.5708 * 1.8)
def callback(msg):
evaluate_direction(msg.ranges)
sub = rospy.Subscriber('/kobuki/laser/scan', LaserScan, callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rospy.spin()