def __init__(self):
rospy.init_node('maze_pro_solver', anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param('~position')
startX, startY = positions['startX'], positions['startY']
targetX, targetY = positions['targetX'], positions['targetY']
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
self.path = [
Cell(r - self.path_finder.start.row,
c - self.path_finder.start.column) for r, c in raw_path
] # move rows to correct starting position
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5)
# Setup subscribers
odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def load():
starttime = time.time() * 1000
initdict_terrain = MapLoader.loadFile(
"../../def/1_Terrain.yml")["terrain"]
initdict_zones = MapLoader.loadFile("../../def/2_Zones.yml")["zones"]
initdict_waypoints = MapLoader.loadFile(
"../../def/3_Waypoints.yml")["waypoints"]
initdict_entities = MapLoader.loadFile(
"../../def/4_Entities.yml")["entities"]
initdict_objects = MapLoader.loadFile(
"../../def/5_Objects.yml")["objects"]
# Instantiate objects before creating the map dict
for zone in initdict_zones:
initdict_zones[zone] = Zone(initdict_zones[zone])
for waypoint in initdict_waypoints:
initdict_waypoints[waypoint] = Waypoint(
initdict_waypoints[waypoint])
for entity in initdict_entities:
initdict_entities[entity] = Entity(initdict_entities[entity])
for obj in initdict_objects:
initdict_objects[obj] = Object(initdict_objects[obj])
rospy.loginfo("Loaded files in {0:.2f}ms.".format(time.time() * 1000 -
starttime))
# Create main Map dict
Map.MapDict = DictManager({
"terrain": Terrain(initdict_terrain),
"zones": DictManager(initdict_zones),
"waypoints": DictManager(initdict_waypoints),
"entities": DictManager(initdict_entities),
"objects": DictManager(initdict_objects)
})
rospy.loginfo("Loaded map in {0:.2f}ms.".format(time.time() * 1000 -
starttime))
def load():
# Loading XML files
starttime = time.time() * 1000
xml_config = MapLoader.loadFile("1_config.xml")
xml_terrain = MapLoader.loadFile("2_terrain.xml")
xml_waypoints = MapLoader.loadFile("3_waypoints.xml")
xml_objects = MapLoader.loadFile("4_objects.xml")
xml_robot = MapLoader.loadFile("5_robot.xml")
rospy.loginfo("Loaded files in {0:.2f}ms.".format(time.time() * 1000 -
starttime))
# Setting current team to the default set one.
for xml_team in xml_config.find("teams").findall("team"):
if "default" in xml_team.attrib and bool(
xml_team.attrib["default"]):
if MapManager.CurrentTeam != "":
raise ValueError(
"ERROR Two or more teams have been set to default.")
MapManager.CurrentTeam = xml_team.attrib["name"]
MapManager.Teams.append(Team(xml_team))
if MapManager.CurrentTeam == "":
raise ValueError("ERROR One team has to be set as default.")
# Loading the color palette
for xml_color in xml_config.find("colors").findall("color"):
MapManager.Colors.append(Color(xml_color))
# Constructing object classes
if not xml_objects.findall("classes"):
raise KeyError("Objects file must have a 'classes' tag.")
obj_classes = []
for c in xml_objects.find("classes").findall("class"):
obj_classes.append(Class(c, obj_classes))
# Instantiate objects and create the map dict
MapDict.Terrain = Terrain(xml_terrain)
xml_objects.attrib["name"] = "map"
MapDict.Objects = Container(xml_objects, obj_classes)
MapDict.Waypoints = [
Waypoint(w) for w in xml_waypoints.findall("waypoint")
]
MapDict.Robot = Robot(xml_robot, obj_classes)
rospy.loginfo("Loaded map in {0:.2f}ms.".format(time.time() * 1000 -
starttime))
def __init__(self, precision):
rospy.init_node("global_planner", anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param("~position")
startX, startY = positions["startX"], positions["startY"]
targetX, targetY = positions["targetX"], positions["targetY"]
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
# do not crop if target outside of maze
self.map_loader = MapLoader(start, target)
self.map_matrix = self.map_loader.loadMap()
GlobalPlanner.matrix = self.map_matrix
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
self.precision = precision
# Calculate path
t = time.time()
self.path_finder = AStar(self.map_matrix)
raw_path = self.path_finder.search()
print(time.time() - t)
Cell.start = self.path_finder.start
self.path = [Cell(r, c) for r, c in raw_path]
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# self.pid_pub = rospy.Publisher("/pid_err", Float64, queue_size=10)
# Setup subscribers
_ = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def fullPath():
# This is the main part of the demo program
map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image
base_image = "simple" # This is the base file name of the input image for map generation
map_loader.addFrame("/home/kinova/MillenCapstone/MadalynMillenCapstone/animation/","config_space_bw.png")
scale = 0.1
map_loader.createMap(scale) # Discretize the map based on the the scaling factor
# Create a big version of discretized map for better visualization
big_map = resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=INTER_NEAREST)
imshow("Image",map_loader.image)
imshow("Map", map_loader.map)
imshow("Big", big_map)
target_dir = "output"
if not os.path.exists(target_dir):
print "Creating target directory <",target_dir,"> ..."
try: os.makedirs(target_dir)
except:
print "Failed to create target path!"
exit()
print "Writing the base images ..."
imwrite(target_dir+"/"+base_image+"_img.png",map_loader.image)
imwrite(target_dir+"/"+base_image+"_map.png",map_loader.map)
imwrite(target_dir+"/"+base_image+"_big_map.png",big_map)
print "Wait for key input..."
#waitKey()
print "Doing the search ..."
grid = UndirectedGraph() # Using Russell and Norvig code
start=(13,20)
goal=(16,30)
# Define the test cases we want to run
tests = [("depth_first_", depth_first_graph_search),
("breadth_first_",breadth_first_search),
("uniform_cost_", uniform_cost_search)]
'''("astar_search_euclid_", astar_search,0),
("astar_search_euclid2_", astar_search,4),
("astar_search_euclid3_", astar_search,5),
("astar_search_euclid025_", astar_search,6),
("astar_search_euclid05_", astar_search,7),
("astar_search_dx_", astar_search,1),
("astar_search_dy_", astar_search,2),
("astar_search_manhattan_", astar_search,3),
("greedy_search_euclid_", greedy_best_first_graph_search,0),
("greedy_search_dx_", greedy_best_first_graph_search,1),
("greedy_search_dy_", greedy_best_first_graph_search,2),
("greedy_search_manhattan_",greedy_best_first_graph_search,3) ]'''
paths = []
smallPath = []
smallestPath = float('inf')
radPoint = 0.123
radAdd = np.pi / 8
for test in tests:
print "Set up the "+test[0]+" ..."
file_name = target_dir+"/"+test[0]+base_image
video_encoder = VideoEncoder(file_name, map_loader.map, frame_rate = 30.0, fps_factor=1.0, comp_height=1.0/scale, comp_width=2.0/scale)
print " output to ",file_name
problem2 = GridProblem(start, goal, grid, map_loader.map,scale,video_encoder)
# Load the correct grid search algorithm and heuristics
print "------------- call ---------------------"
if (len(test) > 2):
if (test[2] == 0):
result, max_frontier_size = test[1](problem2, problem2.h_euclid)
#
elif (test[2] == 1):
result, max_frontier_size = test[1](problem2, problem2.h_x_distance)
#
elif (test[2] == 2):
result, max_frontier_size = test[1](problem2, problem2.h_y_distance)
#
elif (test[2] == 3):
result, max_frontier_size = test[1](problem2, problem2.h_manhattan)
#
elif (test[2] == 4):
result, max_frontier_size = test[1](problem2, problem2.h_euclid2)
#
elif (test[2] == 5):
result, max_frontier_size = test[1](problem2, problem2.h_euclid3)
#
elif (test[2] == 6):
result, max_frontier_size = test[1](problem2, problem2.h_euclid025)
#
elif (test[2] == 7):
result, max_frontier_size = test[1](problem2, problem2.h_euclid05)
#
else:
print "Help",test[2]
else:
result, max_frontier_size = test[1](problem2)
#result,max_frontier_size=depth_first_graph_search(problem2)
print "-------------return---------------------"
#result = depth_first_graph_search(problem2)
#result = breadth_first_search(problem2)
#result = uniform_cost_search(problem2)
#@result = astar_search(problem2, h=problem2.h_euclid)#manhattan)#y_distance)
ftxt = open(file_name+'.txt','w')
print " Result=",result
print " expansions = ",problem2.expansion
ftxt.write("expansions = "+str(problem2.expansion)+"\n")
ftxt.write("max frontier = "+str(max_frontier_size)+"\n")
if (result is not None):
path = result.path()
ftxt.write("path cost="+str(problem2.total_path_cost(path))+"\n")
ftxt.write("Path="+str(path)+"\n")
print "path cost=",problem2.total_path_cost(path)
print "Path=",path
print "Plotting path ..."
map_loader.plotPath(path, 1.0)# scale)
big_path = resize(map_loader.path, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=INTER_LINEAR)
imshow("Path",big_path)
imwrite(file_name+"_path.png",big_path)
if len(path) < smallestPath:
smallPath = path
smallestPath = len(path)
else:
ftxt.write('no path!')
ftxt.close()
print " Close the video ..."
problem2.video_encoder.release()
waitKey(500)
for p in smallPath:
paths.append((radPoint, ((math.atan(float(p.state[1])/ float(p.state[0]))) * (np.pi / 180)) - radPoint))
print (float(p.state[1])/ float(p.state[0]))
if radPoint < 3.11:
radPoint = radPoint + radAdd
return paths
from copy import deepcopy
from maps import *
from map_loader import MapLoader
from cost_optimization import *
map_loader = MapLoader()
class MapMerge:
def __init__(self):
self.water_shed_map = None
self.water_shed_matrix = None
def get_map_name_by(self, id, basic_name):
return "reg_" + str(float(id)) + "_" + str(basic_name)
def build_watershed_id_to_pixels(self):
water_shed_id_to_pixels = {}
for i in range(len(self.water_shed_matrix)):
row = self.water_shed_matrix[i]
for j in range(len(row)):
cell = row[j]
if cell == self.water_shed_map.no_data_value:
continue
cell_obj = water_shed_id_to_pixels.get(cell, [])
cell_obj.append({"x": i, "y": j})
water_shed_id_to_pixels[cell] = cell_obj
return water_shed_id_to_pixels
def merge_outputs_to_one_by_watershed_map(self, water_shed_map_name, maps,
class ProSolver:
matrix = None
def __init__(self):
rospy.init_node('maze_pro_solver', anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param('~position')
startX, startY = positions['startX'], positions['startY']
targetX, targetY = positions['targetX'], positions['targetY']
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
ProSolver.matrix = self.map_matrix
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
Cell.start = self.path_finder.start
self.path = [Cell(r, c) for r, c in raw_path]
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Setup subscribers
#odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped,
self.odom_callback)
def odom_callback(self, msg):
self.pose.x = msg.pose.pose.position.x
self.pose.y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(_, _, self.pose.theta) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def next_pose(self):
try:
self.path_index += 1
self.goal = self.path[self.path_index].pose()
rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x,
self.goal.y)
except IndexError:
rospy.logwarn("REACHED END OF PATH!")
def run(self):
rate = rospy.Rate(10) # 10hz
speed = Twist()
while not rospy.is_shutdown():
# Calculate command
ang_speed = 0.4
inc_x = self.goal.x - self.pose.x
inc_y = self.goal.y - self.pose.y
angle_to_goal = atan2(inc_y, inc_x)
real_angle = self.pose.theta
# Normalize angle_diff
if angle_to_goal < 0:
angle_to_goal += 2 * pi
real_angle += 2 * pi
if real_angle < 0:
real_angle += 2 * pi
angle_to_goal += 2 * pi
angle_diff = angle_to_goal - real_angle
if (inc_x**2 + inc_y**2)**0.5 < 0.05:
speed.linear.x = 0.0
speed.angular.z = 0.0
self.next_pose()
elif abs(angle_diff) > 0.2: # increase tolerance?
speed.linear.x = 0.0
if angle_diff < 0:
speed.angular.z = -ang_speed
else:
speed.angular.z = +ang_speed
else:
speed.linear.x = 0.08
speed.angular.z = 0.0
self.cmd_vel_pub.publish(speed)
rate.sleep()
def h_euclid025(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.25*np.sqrt(dx*dx + dy*dy)
return int(dist)
def h_euclid05(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.5*np.sqrt(dx*dx + dy*dy)
return int(dist)
# This is the main part of the demo program
map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image
base_image = "config_space_bw" # This is the base file name of the input image for map generation
map_loader.addFrame(".",base_image+".png")
scale = 0.25 # scale of map - smaller scale implies larger grid size
map_loader.createMap(scale, (-np.pi, np.pi), (-np.pi, np.pi)) # Discretize the map based on the the scaling factor
# Create a big version of discretized map for better visualization
big_map = cv2.resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_NEAREST)
cv2.imshow("Image",map_loader.image)
cv2.imshow("Map", map_loader.map)
cv2.imshow("Big", big_map)
def h_euclid025(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.25*np.sqrt(dx*dx + dy*dy)
return int(dist)
def h_euclid05(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.5*np.sqrt(dx*dx + dy*dy)
return int(dist)
# This is the main part of the demo program
map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image
base_image = "simple" # This is the base file name of the input image for map generation
map_loader.addFrame(".",base_image+".png")
scale = 0.1
map_loader.createMap(scale) # Discretize the map based on the the scaling factor
# Create a big version of discretized map for better visualization
big_map = cv2.resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_NEAREST)
cv2.imshow("Image",map_loader.image)
cv2.imshow("Map", map_loader.map)
cv2.imshow("Big", big_map)
target_dir = "output"
def load_map(self, map_id):
self.map_id = map_id
loader = MapLoader(map_id)
self.map = loader.get_map()
self.stations = loader.get_stations()
self.x, self.y = loader.get_map_size()
def h_euclid025(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.25*np.sqrt(dx*dx + dy*dy)
return int(dist)
def h_euclid05(self,node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.5*np.sqrt(dx*dx + dy*dy)
return int(dist)
# This is the main part of the demo program
map_loader = MapLoader() # Create a MapLoader to load the world map from a simple image
base_image = "config_space_bw" # This is the base file name of the input image for map generation
map_loader.addFrame(".",base_image+".png")
scale = 0.25 # scale of map - smaller scale implies larger grid size
map_loader.createMap(scale, (-np.pi, np.pi), (-np.pi, np.pi)) # Discretize the map based on the the scaling factor
# Create a big version of discretized map for better visualization
big_map = cv2.resize(map_loader.map, (0,0),fx=(1.0/scale), fy=(1.0/scale), interpolation=cv2.INTER_NEAREST)
cv2.imshow("Image",map_loader.image)
cv2.imshow("Map", map_loader.map)
cv2.imshow("Big", big_map)
def setup(self):
MapLoader.store_maps(self)
MapLoader.load_map(self, 'map0')
self.player = MapLoader.get_player(self)
class GlobalPlanner:
matrix = None
def __init__(self, precision):
rospy.init_node("global_planner", anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param("~position")
startX, startY = positions["startX"], positions["startY"]
targetX, targetY = positions["targetX"], positions["targetY"]
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
# do not crop if target outside of maze
self.map_loader = MapLoader(start, target)
self.map_matrix = self.map_loader.loadMap()
GlobalPlanner.matrix = self.map_matrix
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
self.precision = precision
# Calculate path
t = time.time()
self.path_finder = AStar(self.map_matrix)
raw_path = self.path_finder.search()
print(time.time() - t)
Cell.start = self.path_finder.start
self.path = [Cell(r, c) for r, c in raw_path]
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# self.pid_pub = rospy.Publisher("/pid_err", Float64, queue_size=10)
# Setup subscribers
_ = rospy.Subscriber("/odom", Odometry, self.odom_callback)
# odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.odom_callback)
def odom_callback(self, msg):
self.pose.x = msg.pose.pose.position.x
self.pose.y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(_, _, self.pose.theta) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def next_pose(self):
try:
self.path_index += 1
self.goal = self.path[self.path_index].pose()
rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x,
self.goal.y)
except IndexError:
rospy.logwarn("REACHED END OF PATH!")
def run(self):
rate = rospy.Rate(10) # 10hz
speed = Twist()
goto = GotoController()
goto.set_max_linear_acceleration(.05)
goto.set_max_angular_acceleration(.2)
goto.set_forward_movement_only(True)
while not rospy.is_shutdown():
speed_pose = goto.get_velocity(self.pose, self.goal, 0)
# self.pid_pub.publish(goto.desiredAngVel)
speed.linear.x = speed_pose.xVel
speed.angular.z = speed_pose.thetaVel
self.cmd_vel_pub.publish(speed)
if goto.get_goal_distance(self.pose,
self.goal) <= .05: # 5 cm precision
self.next_pose()
rate.sleep()
def h_euclid025(self, node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.25 * np.sqrt(dx * dx + dy * dy)
return int(dist)
def h_euclid05(self, node):
dx = self.h_x_distance(node)
dy = self.h_y_distance(node)
dist = 0.5 * np.sqrt(dx * dx + dy * dy)
return int(dist)
# This is the main part of the demo program
map_loader = MapLoader(
) # Create a MapLoader to load the world map from a simple image
base_image = "simple" # This is the base file name of the input image for map generation
map_loader.addFrame(".", base_image + ".png")
scale = 0.1
map_loader.createMap(
scale) # Discretize the map based on the the scaling factor
# Create a big version of discretized map for better visualization
big_map = cv2.resize(map_loader.map, (0, 0),
fx=(1.0 / scale),
fy=(1.0 / scale),
interpolation=cv2.INTER_NEAREST)
cv2.imshow("Image", map_loader.image)
class ProSolver:
def __init__(self):
rospy.init_node('maze_pro_solver', anonymous=True)
# Try to load parameters from launch file, otherwise set to None
try:
positions = rospy.get_param('~position')
startX, startY = positions['startX'], positions['startY']
targetX, targetY = positions['targetX'], positions['targetY']
start = (startX, startY)
target = (targetX, targetY)
except:
start, target = None, None
# Load maze matrix
self.map_loader = MapLoader(
start, target) # do not crop if target outside of maze
self.map_matrix = self.map_loader.loadMap()
Cell.resolution = self.map_loader.occupancy_grid.info.resolution
# Calculate path
self.path_finder = PathFinder(self.map_matrix)
raw_path = self.path_finder.calculate_path()
self.path = [
Cell(r - self.path_finder.start.row,
c - self.path_finder.start.column) for r, c in raw_path
] # move rows to correct starting position
self.goal = self.path[0].pose()
self.path_index = 0
self.pose = Pose(0, 0, 0)
# Setup publishers
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5)
# Setup subscribers
odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
def odom_callback(self, msg):
self.pose.x = msg.pose.pose.position.x
self.pose.y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(_, _, self.pose.theta) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
def next_pose(self):
try:
self.path_index += 1
self.goal = self.path[self.path_index].pose()
except IndexError:
rospy.logwarn("REACHED END OF PATH!")
def run(self):
rate = rospy.Rate(10) # 10hz
speed = Twist()
while not rospy.is_shutdown():
# Calculate command
# Do other stuff
inc_x = self.goal.x - self.pose.x
inc_y = self.goal.y - self.pose.y
angle_to_goal = atan2(inc_y, inc_x)
if (inc_x**2 + inc_y**2)**0.5 < 0.05:
speed.linear.x = 0.0
speed.angular.z = 0.0
self.next_pose()
elif abs(angle_to_goal - self.pose.theta) > 0.02:
speed.linear.x = 0.0
if (self.pose.theta < angle_to_goal):
if (self.pose.theta < -0.2 and angle_to_goal > 0):
if (abs(self.pose.theta > (pi / 2))
and abs(angle_to_goal > (pi / 2))):
speed.angular.z = 0.3
else:
speed.angular.z = -0.3
else:
speed.angular.z = 0.3
elif (self.pose.theta > angle_to_goal):
if (angle_to_goal < -0.2 and self.pose.theta > 0):
if (abs(self.pose.theta > (pi / 2))
or abs(angle_to_goal > (pi / 2))):
speed.angular.z = 0.3
else:
speed.angular.z = -0.3
else:
speed.angular.z = -0.3
else:
speed.linear.x = 0.08
speed.angular.z = 0.0
self.cmd_vel_pub.publish(speed)
rate.sleep()
