diff --git a/ROSNavBench/benchmarking_local_global_planners_pdf.py b/ROSNavBench/benchmarking_local_global_planners_pdf.py index 3d06f09..5eabb17 100644 --- a/ROSNavBench/benchmarking_local_global_planners_pdf.py +++ b/ROSNavBench/benchmarking_local_global_planners_pdf.py @@ -1,4 +1,5 @@ #! /usr/bin/env python3 +from hmac import new from pydoc import plain import numpy as np import yaml @@ -24,7 +25,8 @@ from numpy import asarray from ROSNavBench.follow_path import circle_points,square_points from ROSNavBench.performace_analysis import performance_analysis - +from scipy import ndimage +from scipy.interpolate import interp1d def main(): ''' @@ -69,11 +71,10 @@ def path_length(controller_number): trajectory_type= robot_specs['trajectory_type'] map_path=robot_specs['map_path'] map_png_path=robot_specs['map_png_path'] - trails_num = robot_specs['trails_num'] criteria= robot_specs['criteria'] weights= robot_specs['weights'] - if trails_num>0: - controller_type=[controller_type[0]]*trails_num + instances_num= robot_specs['instances_num'] + # list of arrays to hold data of experiment # This is a way to arrange data to be used for analysis and ploting data=[] @@ -94,7 +95,7 @@ def path_length(controller_number): distance_to_obstacles=[] # nested arrays equal to number of controllers # E.g., x_points=[[x points for the 1st controller],[x points for the 2nd controller],...] - for i in range(len(controller_type)*len(planner_type)): + for i in range(len(controller_type)*len(planner_type)*instances_num): data.append([]) CPU.append([]) Memory.append([]) @@ -108,18 +109,19 @@ def path_length(controller_number): # open each csv file for the different used controllers, and add all data to data array for k in range(len(planner_type)): for i in range(len(controller_type)): - round_num=len(controller_type)*k+i - f=open(os.path.join(get_package_share_directory('ROSNavBench'), - 'raw_data', - pdf_name+'_'+controller_type[i]+planner_type[k]+str(round_num+1)+'.csv'),'r') - writer=csv.reader(f,quoting=csv.QUOTE_NONNUMERIC,delimiter=' ') - for lines in writer: - data[round_num].append(lines[:]) + for q in range(instances_num): + round_num=k*len(controller_type)*instances_num+i*instances_num+q + f=open(os.path.join(get_package_share_directory('ROSNavBench'), + 'raw_data', + pdf_name+'_'+controller_type[i]+planner_type[k]+str(round_num+1)+'.csv'),'r') + writer=csv.reader(f,quoting=csv.QUOTE_NONNUMERIC,delimiter=' ') + for lines in writer: + data[round_num].append(lines[:]) # Extarct data from data array and arrange them into different arrays - for k in range(len(controller_type)*len(planner_type)): + for k in range(len(controller_type)*len(planner_type)*instances_num): for i in range(len(data[k])-5): #EDIT FOR ADDING THE PATH CPU[k].append(data[k][i+2][0]) Memory[k].append(data[k][i+2][1]) @@ -135,9 +137,19 @@ def path_length(controller_number): global_y_points.append(data[k][i+2][3]) global_time.append(data[k][i+2][6]) distance_to_obstacles[k].append(data[k][i+2][7]) - + print("CPU ",CPU) + print(len(CPU)) + print("CPU global ",global_CPU) + print("x points",x_points) + print(len(x_points)) + print("y points",y_points) + print(len(y_points)) + print("xy points",xy_points) + print(len(xy_points)) + print("obstacles",distance_to_obstacles) + print("time ",time) # Convert the nested array into tuples to satisfy the requirment of Label() function of reportlab - for k in range(len(controller_type)*len(planner_type)): + for k in range(len(controller_type)*len(planner_type)*instances_num): CPU_data[k]=tuple(CPU_data[k]) Memory_data[k]=tuple(Memory_data[k]) xy_points[k]=tuple(xy_points[k]) @@ -158,39 +170,75 @@ def result(round_num): 'results', pdf_name+".pdf"),pagesize=A4) - + # PDF title d=shapes.Drawing(5,40) d.add(String(1,20,pdf_name,fontSize=20)) elements.append(d) - - - # A table summarize the results d=shapes.Drawing(250,40) d.add(String(1,20,"Comparsion of controllers",fontSize=15)) elements.append(d) analysis_data=[] + box_plot_data=[[],[],[],[],[],[],[]] for i in range(len(planner_type)): d=shapes.Drawing(250,40) d.add(String(1,20,"-Global planner: "+planner_type[i],fontSize=12,fontName= 'Times-Bold')) elements.append(d) - table= [["Controller\ntype","Result","Execution\nTime (sec)","CPU(%)","","Memory usage(%)","Memory usage (%)","Number of\nrecoveries","Path\nlength\n(m)","Proximity\nto\n obstacles(m)"]] + table= [["Controller\ntype","Success\n rate\n (%)","Average \nexecution\ntime (sec)","CPU(%)","","Memory usage(%)","Memory usage (%)","Average\nnumber of\nrecoveries","Average\n path\nlength(m)","Min\nproximity to\n obstacles(m)"]] table.append(["","","","Average","Max","Average","Max","","",""]) for k in range(len(controller_type)): + #a loop for each instance + instance_results=[] + instance_CPU=[] + instance_Memory=[] + instance_execution_time=[] + instance_path_length=[] + instance_recovery=[] + instance_obstacles=[] + for q in range(instances_num): + round_num=i*len(controller_type)*instances_num+k*instances_num+q + instance_results.append(result(round_num)) + instance_CPU+=CPU[round_num] + instance_Memory+=Memory[round_num] + instance_execution_time.append(data[round_num][len(data[round_num])-4][6]) + instance_path_length.append(round(path_length(round_num),2)) + instance_recovery.append(data[round_num][len(data[round_num])-4][4]) + instance_obstacles.append(min(distance_to_obstacles[round_num])) + box_plot_data[0].append(planner_type[i]+"\n"+controller_type[k]) + box_plot_data[1].append(instance_Memory) + box_plot_data[2].append(instance_CPU) + box_plot_data[3].append(instance_obstacles) + box_plot_data[4].append(instance_execution_time) + box_plot_data[5].append(instance_path_length) + box_plot_data[6].append(i*len(controller_type)+k+1) + print(instance_results) + print("CPU") + print(instance_CPU) + print("memory") + print(instance_Memory) + print("time") + print(instance_execution_time) + print("path") + print(instance_path_length) + print("recovery") + print(instance_recovery) + print("obstacles") + print(instance_obstacles) + + ## table_data=[] table_data.append(controller_type[k]) - round_num=len(controller_type)*i+k - table_data.append(result(round_num)) - table_data.append(str(data[round_num][len(data[round_num])-4][6])) #EDITNG for addin path - table_data.append(str('{0:.2f}'.format(sum(CPU[round_num])/len(CPU[round_num])))) - table_data.append(str(max(CPU[round_num]))) - table_data.append(str('{0:.2f}'.format(sum(Memory[round_num])/len(Memory[round_num])))) - table_data.append(str(max(Memory[round_num]))) - table_data.append(str(data[round_num][len(data[round_num])-4][4])) #EDITNG for addin path - table_data.append(str(round(path_length(round_num),2))) - table_data.append(str(min(distance_to_obstacles[round_num]))) + table_data.append(str(round((instance_results.count('succeeded')/len(instance_results))*100,2))) #success rate of this combination + table_data.append(str('{0:.2f}'.format(sum(instance_execution_time)/len(instance_execution_time)))) #Execution time + table_data.append(str('{0:.2f}'.format(sum(instance_CPU)/len(instance_CPU)))) # average CPU + table_data.append(str(max(instance_CPU))) #Max CPU + table_data.append(str('{0:.2f}'.format(sum(instance_Memory)/len(instance_Memory)))) #average Memory + table_data.append(str(max(instance_Memory))) #Max memory + table_data.append(str('{0:.2f}'.format(sum(instance_recovery)/len(instance_recovery)))) #Number of recoveries + table_data.append(str('{0:.2f}'.format(sum(instance_path_length)/len(instance_path_length)))) #Path length + table_data.append(str(min(instance_obstacles))) #Proximity to obstacles table.append(table_data) analysis_data.append(table) @@ -200,24 +248,19 @@ def result(round_num): ('SPAN',(0,0),(0,1)), ('SPAN',(1,0),(1,1)), ('SPAN',(2,0),(2,1)), - #('SPAN',(3,0),(3,1)), - #('SPAN',(4,0),(4,1)), - #('SPAN',(5,0),(5,1)), - #('SPAN',(6,0),(6,1)), ('SPAN',(7,0),(7,1)), ('SPAN',(8,0),(8,1)), ('SPAN',(9,0),(9,1)), ('SPAN',(3,0),(4,0)), ('SPAN',(5,0),(6,0)), ('FONTNAME',(0,0),(9,1),'Times-Bold'), - #('SPAN',(0,len(data)-1),(6,len(data)-1)), - #('FONTNAME',(0,len(data)-1),(6,len(data)-1),'Times-Bold'), ])) elements.append(t) # Performace analysis d=shapes.Drawing(250,70) d.add(String(1,40,"Performace analysis",fontSize=15)) elements.append(d) + print(analysis_data) ranking,planners_success_rate,controllers_success_rate=performance_analysis(criteria,analysis_data,weights,planner_type,controller_type) d.add(String(1,20,"Based on the criteria:" +", ".join(criteria),fontName= 'Times-Bold')) d=shapes.Drawing(250,20*(len(ranking)+1)) @@ -241,7 +284,33 @@ def result(round_num): for i in range(len(controllers_success_rate)): d.add(String(1,20*(row_increament),controllers_success_rate[i])) row_increament+=1 - elements.append(d) + print("--------------",box_plot_data[0]) + elements.append(d) + fig = plt.figure(figsize =(10, 12),constrained_layout=True) + plt.subplot(3,1,1).set_title ("Memory(%)",fontdict={'family':'serif','size':12}) + plt.boxplot(box_plot_data[1]) + plt.xticks(ticks =box_plot_data[6] ,labels = box_plot_data[0], rotation = 'horizontal',fontdict={'family':'serif','size':12}) + plt.subplot(3,1,2).set_title("CPU(%)",fontdict={'family':'serif','size':12}) + plt.boxplot(box_plot_data[2]) + plt.xticks(ticks =box_plot_data[6] ,labels = box_plot_data[0], rotation = 'horizontal',fontdict={'family':'serif','size':12}) + plt.subplot(3,1,3).set_title ("Proximity to obstacles(m)",fontdict={'family':'serif','size':12}) + plt.boxplot(box_plot_data[3]) + plt.xticks(ticks = box_plot_data[6],labels = box_plot_data[0], rotation = 'horizontal',fontdict={'family':'serif','size':12}) + plt.savefig(os.path.join(get_package_share_directory('ROSNavBench'), + 'raw_data','graph_box_plot.png')) + elements.append(Image_pdf(os.path.join(get_package_share_directory('ROSNavBench'), + 'raw_data','graph_box_plot.png'),500,600)) + fig = plt.figure(figsize =(10, 8),constrained_layout=True) + plt.subplot(2,1,1).set_title ("Time (sce)",fontdict={'family':'serif','size':12}) + plt.boxplot(box_plot_data[4]) + plt.xticks(ticks =box_plot_data[6] ,labels = box_plot_data[0], rotation ='horizontal',fontdict={'family':'serif','size':12}) + plt.subplot(2,1,2).set_title ("Path Length (m)",fontdict={'family':'serif','size':12}) + plt.boxplot(box_plot_data[5]) + plt.xticks(ticks = box_plot_data[6] ,labels = box_plot_data[0], rotation = 'horizontal',fontdict={'family':'serif','size':12}) + plt.savefig(os.path.join(get_package_share_directory('ROSNavBench'), + 'raw_data','graph_box_plot2.png')) + elements.append(Image_pdf(os.path.join(get_package_share_directory('ROSNavBench'), + 'raw_data','graph_box_plot2.png'),500,400)) d=shapes.Drawing(250,55) d.add(String(1,20,"Graphs",fontSize=15)) elements.append(d) @@ -263,96 +332,113 @@ def result(round_num): for i in range(0, r): l = LineSwatch() l.strokeColor = getattr(colors, items[i+v]) - if trails_num>0: - cnp.append((l, controller_type[i+v]+' #'+str(i+1+v))) - else: - cnp.append((l, controller_type[i+v])) + + cnp.append((l, controller_type[i+v])) legend.colorNamePairs = cnp legend_list.append(legend) #d.add(legend, 'legend') #elements.append(d) drawing = shapes.Drawing(500, 10) elements.append(drawing) - for k in range(len(planner_type)): - # CPU plot - drawing = shapes.Drawing(500, 290) - lab=Label() - lab.setOrigin(0,130) - lab.angle=90 - lab.setText('CPU(%)') - lp = LinePlot() - lp.x = 40 - lp.y = 35 - lp.height = 220 - lp.width = 450 - CPU_data_plot=[] - for i in range(len(controller_type)): - CPU_data_plot.append(CPU_data[len(controller_type)*k+i]) + #Finding a mean line and the probabilty region + def interpolate_trajectory(x, y, num_points): + # Create a cumulative distance array as reference + distance = np.cumsum(np.sqrt(np.ediff1d(x, to_begin=0)**2 + np.ediff1d(y, to_begin=0)**2)) + print("distance",distance) + distance = distance / distance[-1] # Normalize distance to [0,1] - lp.data = CPU_data_plot - lp.joinedLines = 1 - for i in range(len(controller_type)): - lp.lines[i].strokeColor=getattr(colors, items[i]) + # Interpolate using the distance as reference + fx = interp1d(distance, x, kind='linear') + fy = interp1d(distance, y, kind='linear') - lp.strokeColor = colors.black - lp.xValueAxis.valueMin = 0 - lp.xValueAxis.valueMax = max(global_time) - lp.xValueAxis.configure(global_time) - lp.xValueAxis.labelTextFormat = '%2.1f' - lp.yValueAxis.valueMin = 0 - lp.yValueAxis.valueMax=100 - lp.yValueAxis.configure(global_CPU) - drawing.add(String(1,280,"-Global planner: "+planner_type[k],fontSize=12,fontName= 'Times-Bold')) - drawing.add(String(200,270,'CPU usage(%) ', fontSize=12, fillColor=colors.black)) - drawing.add(lab) - drawing.add(lp) - drawing.add(String(200,5,'Time(sec) ', fontSize=12, fillColor=colors.black)) - elements.append(drawing) - for r in range(len(legend_list)): - d = Drawing(500, 30) - d.add(legend_list[r], 'legend') - elements.append(d) - # Memory usage plot - for k in range(len(planner_type)): - drawing = shapes.Drawing(500, 290) - lab=Label() - lab.setOrigin(0,130) - lab.angle=90 - lab.setText('Memory') - lp = LinePlot() - lp.x = 40 - lp.y = 35 - lp.height = 220 - lp.width = 450 - Memory_data_plot=[] - for i in range(len(controller_type)): - Memory_data_plot.append(Memory_data[len(controller_type)*k+i]) - lp.data = Memory_data_plot - lp.joinedLines = 1 - for i in range(len(controller_type)): - lp.lines[i].strokeColor=getattr(colors, items[i]) - lp.strokeColor = colors.black - lp.xValueAxis.valueMin = 0 - lp.xValueAxis.valueMax = max(global_time) - lp.xValueAxis.configure(global_time) - lp.xValueAxis.labelTextFormat = '%2.1f' - lp.yValueAxis.valueMin = 0 - lp.yValueAxis.valueMax = 100 - lp.yValueAxis.configure(global_Memory) - drawing.add(String(1,280,"-Global planner: "+planner_type[k],fontSize=12,fontName= 'Times-Bold')) - drawing.add(String(200,270,'Memory usage', fontSize=12, fillColor=colors.black)) - drawing.add(lab) - drawing.add(lp) - drawing.add(String(200,5,'Time(sec) ', fontSize=12, fillColor=colors.black)) + # New normalized distance values + new_distances = np.linspace(0, 1, num_points) + return fx(new_distances), fy(new_distances) + + def mean_line(x_arrays,y_arrays): + new_x_array=[] + new_y_array=[] + arrays_length=[] + for i in range(len(x_arrays)): + arrays_length.append(len(x_arrays[i])) + print(arrays_length) + points_num=max(arrays_length) + print(points_num) + for i in range(len(x_arrays)): + new_x,new_y=interpolate_trajectory(x_arrays[i], y_arrays[i], points_num) + new_x_array.append(new_x) + new_y_array.append(new_y) + + x_mean=np.mean(new_x_array,axis=0) + y_mean=np.mean(new_y_array,axis=0) + std=np.std(new_y_array,axis=0) + return x_mean,y_mean,std + #CPU plot + #Oter loop prodcuce new graph + for i in range(len(planner_type)): + # start the graph + plt.figure(figsize=(10, 4)) + plt.ylim(0,100) + labels_list=[] + labels_tag=[] + #inner loop plot a new line + for k in range(len(controller_type)): + round_num=i*len(controller_type)*instances_num+k*instances_num + CPU_array=CPU[round_num:round_num+instances_num] + time_array=time[round_num:round_num+instances_num] + print("CPU_array",CPU_array) + print("time_array",time_array) + + #Shaded area or dots of actual lines + # for q in range(instances_num): + # actual_points,=plt.plot(time_array[q],CPU_array[q],color=items[k] ,linestyle='dotted',alpha=0.3) + + mean_time,mean_CPU,std=mean_line(time_array,CPU_array) + lmean,=plt.plot(mean_time,mean_CPU,label=planner_type[i]+" "+controller_type[k],color=items[k]) + lsigma=plt.fill_between(mean_time, mean_CPU-std,mean_CPU+std,color=items[k], alpha=0.2) + labels_list.append((lmean,lsigma)) + labels_tag.append(controller_type[k]+" Mean +/- stdev") + #labels_list.append(actual_points) + #labels_tag.append("Actual CPU") + + plt.legend(labels_list,labels_tag,prop={'family':'serif','size':8}) + plt.xlabel('Time(sec)',fontdict={'family':'serif','size':12}) + plt.ylabel('CPU (%)',fontdict={'family':'serif','size':12}) + #save the graph + plt.savefig(os.path.join(get_package_share_directory('ROSNavBench'), + 'raw_data','CPU_plot'+str(i)+'.png')) + + # CPU plot + drawing = shapes.Drawing(500, 30) + drawing.add(String(1,20,"-Global planner: "+planner_type[i],fontSize=12,fontName= 'Times-Bold')) + drawing.add(String(200,10,'CPU usage(%) ', fontSize=12, fillColor=colors.black)) elements.append(drawing) - for r in range(len(legend_list)): - d = Drawing(500, 30) - d.add(legend_list[r], 'legend') - elements.append(d) + # insert the graph to pdf + elements.append(Image_pdf(os.path.join(get_package_share_directory('ROSNavBench'), + 'raw_data','CPU_plot'+str(i)+'.png'),500,200)) + # Trajectory plot #Trajectory plot on map image # Open map yaml file and extact data - for k in range(len(planner_type)): + #new + if trajectory_type=="circle": + r= robot_specs['radius'] + waypoints_array=circle_points(x,y,r) + elif trajectory_type=="several_waypoints": + waypoints_array=robot_specs['waypoints'] + elif trajectory_type=="square": + side_length=robot_specs['side_length'] + waypoints_array=square_points(x,y,side_length) + elif trajectory_type=="one_goal": + waypoints_array=[[robot_specs['goal_pose_x'],robot_specs['goal_pose_y']]] + x_trajectory=[] + y_trajectory=[] + # plot the waypoints + for i in range(len(waypoints_array)): + x_trajectory.append(waypoints_array[i][0]) + y_trajectory.append(waypoints_array[i][1]) + for i in range(len(planner_type)): + # start the graph with open(map_path, 'r') as file: map_specs = yaml.safe_load(file) @@ -365,52 +451,79 @@ def result(round_num): y_length=len(numpydata[0]) # width img=plt.imread(map_png_path) fig, ax=plt.subplots() - ax.imshow(img,cmap=plt.cm.gray,extent=[origin[0],0.05*y_length+origin[0],origin[1],0.05*x_length+origin[1]]) - if trajectory_type=="circle": - r= robot_specs['radius'] - waypoints_array=circle_points(x,y,r) - elif trajectory_type=="several_waypoints": - waypoints_array=robot_specs['waypoints'] - elif trajectory_type=="square": - side_length=robot_specs['side_length'] - waypoints_array=square_points(x,y,side_length) - elif trajectory_type=="one_goal": - waypoints_array=[[robot_specs['goal_pose_x'],robot_specs['goal_pose_y']]] - - x_trajectory=[] - y_trajectory=[] - # plot the waypoints - for i in range(len(waypoints_array)): - x_trajectory.append(waypoints_array[i][0]) - y_trajectory.append(waypoints_array[i][1]) - plt.scatter(x_trajectory,y_trajectory,marker='*',c='yellowgreen') - # plot the trajectory of each controller - for p in range(len(controller_type)): - round_num=len(controller_type)*k+p - plt.plot(x_points[round_num],y_points[round_num],c=items[p]) - plt.plot(x_points[round_num][len(x_points[round_num])-1],y_points[round_num][len(y_points[round_num])-1],c=items[p],marker='o', ms=10) - - plt.xlabel('x-axis (meters)',fontdict={'family':'serif','size':12}) - plt.ylabel('y-axis (meters)',fontdict={'family':'serif','size':12}) - # Plot the initial pose - if trajectory_type=="circle": - plt.plot(x+r,y,marker='*',c='yellowgreen',ms=13) + print("-----------------x is ",x_length,"-----------y is ",y_length) + labels_list=[] + labels_tag=[] + if y_length0: - controller_type=[controller_type[0]]*trails_num - # list of arrays to hold data of experiment - # This is a way to arrange data to be used for analysis and ploting - controller_num=len(controller_type) - data=[] - summary=[] - CPU=[] - Memory=[] - CPU_data=[] - Memory_data=[] - xy_points=[] - x_points=[] - y_points=[] - time=[] - global_CPU=[] - global_Memory=[] - global_x_points=[] - global_y_points=[] - global_time=[] - distance_to_obstacles=[] - global_distance_to_obstacles=[] - # nested arrays equal to number of controllers - # E.g., x_points=[[x points for the 1st controller],[x points for the 2nd controller],...] - for i in range(len(controller_type)): - data.append([]) - #log_msgs.append([]) - CPU.append([]) - Memory.append([]) - CPU_data.append([]) - Memory_data.append([]) - xy_points.append([]) - x_points.append([]) - y_points.append([]) - time.append([]) - distance_to_obstacles.append([]) - # open each csv file for the different used controllers, and add all data to data array - for i in range(len(controller_type)): - f=open(os.path.join(get_package_share_directory('ROSNavBench'), - 'raw_data', - pdf_name+'_'+controller_type[i]+planner_type[0]+str(i+1)+'.csv'),'r') - writer=csv.reader(f,quoting=csv.QUOTE_NONNUMERIC,delimiter=' ') - for lines in writer: - data[i].append(lines[:]) - - - # Extarct data from data array and arrange them into different arrays - for k in range(len(controller_type)): - for i in range(len(data[k])-5): - CPU[k].append(data[k][i+2][0]) - Memory[k].append(data[k][i+2][1]) - time[k].append(data[k][i+2][6]) - x_points[k].append(data[k][i+2][2]) - y_points[k].append(data[k][i+2][3]) - CPU_data[k].append((data[k][i+2][6],data[k][i+2][0])) - Memory_data[k].append((data[k][i+2][6],data[k][i+2][1])) - xy_points[k].append((data[k][i+2][2],data[k][i+2][3])) - global_CPU.append(data[k][i+2][0]) - global_Memory.append(data[k][i+2][1]) - global_x_points.append(data[k][i+2][2]) - global_y_points.append(data[k][i+2][3]) - global_time.append(data[k][i+2][6]) - distance_to_obstacles[k].append(data[k][i+2][7]) - global_distance_to_obstacles.append(data[k][i+2][7]) - - # Convert the nested array into tuples to satisfy the requirment of Label() function of reportlab - for k in range(len(controller_type)): - CPU_data[k]=tuple(CPU_data[k]) - Memory_data[k]=tuple(Memory_data[k]) - xy_points[k]=tuple(xy_points[k]) - # Extracting the result as a string from data array - def result(controller_num): - result='' - for i in range(len(data[controller_num][len(data[controller_num])-3])): - result+=data[controller_num][len(data[controller_num])-3][i] - return result - # Generating the pdf - elements=[] - if results_directory!='': - doc=SimpleDocTemplate(os.path.join(results_directory, - pdf_name+".pdf"),pagesize=A4) - else: - doc=SimpleDocTemplate(os.path.join(get_package_share_directory('ROSNavBench'), - 'results', - pdf_name+".pdf"),pagesize=A4) - d=shapes.Drawing(5,40) - d.add(String(1,20,pdf_name,fontSize=20)) - elements.append(d) - - d=shapes.Drawing(250,40) - d.add(String(1,20,"Comparsion of controllers",fontSize=15)) - elements.append(d) - d=shapes.Drawing(250,40) - d.add(String(1,20,"-Global planner: "+planner_type[0],fontSize=12,fontName= 'Times-Bold')) - elements.append(d) - table= [["Controller\ntype","Result","Execution\nTime (sec)","CPU(%)","","Memory usage(%)","Memory usage (%)","Number of\nrecoveries","Path\nlength\n(m)","Proximity\nto\n obstacles(m)"]] - table.append(["","","","Average","Max","Average","Max","","",""]) - for k in range(len(controller_type)): - table_data=[] - if trails_num>0: - table_data.append(controller_type[k]+' #'+str(k+1)) - else: - table_data.append(controller_type[k]) - table_data.append(result(k)) - table_data.append(str(data[k][len(data[k])-4][6])) - table_data.append(str('{0:.2f}'.format(sum(CPU[k])/len(CPU[k])))) - table_data.append(str(max(CPU[k]))) - table_data.append(str('{0:.2f}'.format(sum(Memory[k])/len(Memory[k])))) - table_data.append(str(max(Memory[k]))) - table_data.append(str(data[k][len(data[k])-4][4])) - table_data.append(str(round(path_length(k),2))) - table_data.append(str(min(distance_to_obstacles[k]))) - table.append(table_data) - - - t=Table(table) - t.setStyle(TableStyle([('INNERGRID',(0,0), (-1,-1), 0.25, colors.black), - ('BOX',(0,0), (-1,-1), 0.25, colors.black), - ('SPAN',(0,0),(0,1)), - ('SPAN',(1,0),(1,1)), - ('SPAN',(2,0),(2,1)), - ('SPAN',(7,0),(7,1)), - ('SPAN',(8,0),(8,1)), - ('SPAN',(9,0),(9,1)), - ('SPAN',(3,0),(4,0)), - ('SPAN',(5,0),(6,0)), - ('FONTNAME',(0,0),(9,1),'Times-Bold'), - ])) - elements.append(t) - # Performace analysis - data_variation,success_rate,time_11,path_11=performance_analysis_repeatability([table],planner_type,controller_type) - d=shapes.Drawing(250,55) - d.add(String(1,20,"Graphs",fontSize=15)) - elements.append(d) - - for j in range(math.ceil(len(controller_type)/8)): - d = Drawing(500, 50) - v=j*8 - legend = LineLegend() - legend.alignment = 'right' - legend.x = 1 - legend.y = 40 - legend.deltax = 60 - legend.dxTextSpace = 10 - legend.columnMaximum = 1 - items = 'red green blue yellow pink black aqua bisque blueviolet brown burlywood cadetblue chartreuse chocolate cornflowerblue crimson cyan darkblue darkcyan darkgoldenrod darkgray coral darkgreen darkkhaki darkmagenta darkolivegreen darkorange darkred darksalmon darkseagreen darkslateblue darkslategray darkturquoise darkviolet deeppink deepskyblue dimgray firebrick forestgreen fuchsia grey greenyellow gold hotpink indianred ivory lavender lime maroon navy olive'.split() - cnp = [] - r=8 - if j==(math.ceil(len(controller_type)/8)-1) and len(controller_type)%8!=0: - r=len(controller_type)%8 - for i in range(0, r): - l = LineSwatch() - l.strokeColor = getattr(colors, items[i+v]) - if trails_num>0: - cnp.append((l, controller_type[i+v]+' #'+str(i+1+v))) - else: - cnp.append((l, controller_type[i+v])) - legend.colorNamePairs = cnp - d.add(legend, 'legend') - #elements.append(d) - # Box plot - - data_1 = global_Memory - data_2 = global_CPU - data_3 = global_distance_to_obstacles - data_4 = time_11 - data_5 =path_11 - - fig = plt.figure(figsize =(10, 6)) - plt.subplots_adjust(wspace= 0.75) - plt.subplot(1,5,1) - plt.boxplot(data_1) - plt.xticks(ticks = [1] ,labels = ["Memory (%)"], rotation = 'horizontal',fontdict={'family':'serif','size':12}) - plt.subplot(1,5,2) - plt.boxplot(data_2) - plt.xticks(ticks = [1] ,labels = ["CPU (%)"], rotation = 'horizontal',fontdict={'family':'serif','size':12}) - plt.subplot(1,5,3) - plt.boxplot(data_3) - plt.xticks(ticks = [1] ,labels = ["Proximity to\n obstcales (m)"], rotation = 'horizontal',fontdict={'family':'serif','size':12}) - plt.subplot(1,5,4) - plt.boxplot(data_4) - plt.xticks(ticks = [1] ,labels = ["Time (sec)"], rotation = 'horizontal',fontdict={'family':'serif','size':12}) - plt.subplot(1,5,5) - plt.boxplot(data_5) - plt.xticks(ticks = [1] ,labels = ["Path \nLength (m)"], rotation = 'horizontal',fontdict={'family':'serif','size':12}) - plt.savefig(os.path.join(get_package_share_directory('ROSNavBench'), - 'raw_data','graph_box_plot.png')) - drawing = shapes.Drawing(500,10) - drawing.add(String(200,10,'Performace analysis ', fontSize=12, fillColor=colors.black)) - elements.append(drawing) - elements.append(Image_pdf(os.path.join(get_package_share_directory('ROSNavBench'), - 'raw_data','graph_box_plot.png'),500,300)) - - # CPU plot - legend = LineLegend() - legend.alignment = 'right' - legend.x = 1 - legend.y = 270 - legend.deltax = 60 - legend.dxTextSpace = 10 - legend.columnMaximum = 1 - items = 'red blue yellow pink black aqua bisque blueviolet brown burlywood cadetblue chartreuse chocolate cornflowerblue crimson cyan green darkblue darkcyan darkgoldenrod darkgray coral darkgreen darkkhaki darkmagenta darkolivegreen darkorange darkred darksalmon darkseagreen darkslateblue darkslategray darkturquoise darkviolet deeppink deepskyblue dimgray firebrick forestgreen fuchsia grey greenyellow gold hotpink indianred ivory lavender lime maroon navy olive'.split() - cnp = [] - l = LineSwatch() - l.strokeColor = getattr(colors, items[0]) - cnp.append((l, "Max CPU")) - l = LineSwatch() - l.strokeColor = getattr(colors, items[1]) - cnp.append((l, "Average CPU")) - legend.colorNamePairs = cnp - - - drawing = Drawing(500, 310) - lab=Label() - lab.setOrigin(0,130) - lab.angle=90 - lab.setText('CPU(%)') - plot_data=[[],[]] - for i in range(len(controller_type)): - plot_data[0].append(max(CPU[i])) - plot_data[1].append((sum(CPU[i])/len(CPU[i]))) - - plot_data[0]=tuple(plot_data[0]) - plot_data[1]=tuple(plot_data[1]) - catogries=[] - for i in range(len(controller_type)): - catogries.append(str(i+1)) - bc = VerticalBarChart() - bc.x = 40 - bc.y = 35 - bc.height = 220 - bc.width = 450 - bc.data = plot_data - bc.strokeColor = colors.black - - bc.valueAxis.valueMin =0 - bc.valueAxis.valueMax = 100 - bc.valueAxis.configure(global_CPU) - bc.groupSpacing=2 - bc.categoryAxis.labels.boxAnchor = 'ne' - bc.categoryAxis.labels.dx = 1 - bc.categoryAxis.labels.dy = -2 - bc.categoryAxis.labels.angle = 30 - bc.categoryAxis.categoryNames = catogries - drawing.add(String(200,300,'CPU usage(%) ', fontSize=12, fillColor=colors.black)) - drawing.add(legend, 'legend') - drawing.add(lab) - drawing.add(bc) - drawing.add(String(200,5,'Trail # ', fontSize=12, fillColor=colors.black)) - elements.append(drawing) - - # Memory usage plot - legend = LineLegend() - legend.alignment = 'right' - legend.x = 1 - legend.y = 270 - legend.deltax = 60 - legend.dxTextSpace = 10 - legend.columnMaximum = 1 - items = 'red green blue yellow pink black aqua bisque blueviolet brown burlywood cadetblue chartreuse chocolate cornflowerblue crimson cyan darkblue darkcyan darkgoldenrod darkgray coral darkgreen darkkhaki darkmagenta darkolivegreen darkorange darkred darksalmon darkseagreen darkslateblue darkslategray darkturquoise darkviolet deeppink deepskyblue dimgray firebrick forestgreen fuchsia grey greenyellow gold hotpink indianred ivory lavender lime maroon navy olive'.split() - cnp = [] - l = LineSwatch() - l.strokeColor = getattr(colors, items[0]) - cnp.append((l, "Max Memory Usage (%)")) - l = LineSwatch() - l.strokeColor = getattr(colors, items[1]) - cnp.append((l, "Average Memory Usage (%)")) - legend.colorNamePairs = cnp - - drawing = Drawing(500, 350) - lab=Label() - lab.setOrigin(0,130) - lab.angle=90 - lab.setText('Memory(%)') - plot_data=[[],[]] - for i in range(len(controller_type)): - plot_data[0].append(max(Memory[i])) - plot_data[1].append((sum(Memory[i])/len(Memory[i]))) - - plot_data[0]=tuple(plot_data[0]) - plot_data[1]=tuple(plot_data[1]) - bc = VerticalBarChart() - bc.x = 40 - bc.y = 35 - bc.height = 220 - bc.width = 450 - bc.data = plot_data - bc.strokeColor = colors.black - - bc.valueAxis.valueMin =0 - bc.valueAxis.valueMax = 100 - - bc.valueAxis.configure(global_Memory) - bc.groupSpacing=2 - bc.categoryAxis.labels.boxAnchor = 'ne' - bc.categoryAxis.labels.dx = 1 - bc.categoryAxis.labels.dy = -2 - bc.categoryAxis.labels.angle = 30 - bc.categoryAxis.categoryNames = catogries - drawing.add(String(200,300,'Memory usage ', fontSize=12, fillColor=colors.black)) - drawing.add(legend, 'legend') - drawing.add(lab) - drawing.add(bc) - drawing.add(String(200,5,'Trail # ', fontSize=12, fillColor=colors.black)) - elements.append(drawing) - - - # Trajectory plot - with open(map_path, 'r') as file: - map_specs = yaml.safe_load(file) - - origin=map_specs['origin'] - resolution=map_specs['resolution'] - img=Image.open(map_png_path) - numpydata=asarray(img) - - x_length=len(numpydata) - y_length=len(numpydata[0]) # width - img=plt.imread(map_png_path) - fig, ax=plt.subplots() - ax.imshow(img,cmap=plt.cm.gray,extent=[origin[0],0.05*y_length+origin[0],origin[1],0.05*x_length+origin[1]]) - if trajectory_type=="circle": - r= robot_specs['radius'] - waypoints_array=[[x+r,y]] - waypoints_array+=circle_points(x,y,r) - elif trajectory_type=="several_waypoints": - waypoints_array=[[x,y]] - waypoints_array+=robot_specs['waypoints'] - elif trajectory_type=="square": - waypoints_array=[[x,y]] - side_length=robot_specs['side_length'] - waypoints_array=square_points(x,y,side_length) - elif trajectory_type=="one_goal": - waypoints_array=[[x,y],[robot_specs['goal_pose_x'],robot_specs['goal_pose_y']]] - - x_trajectory=[] - y_trajectory=[] - for i in range(len(waypoints_array)): - x_trajectory.append(waypoints_array[i][0]) - y_trajectory.append(waypoints_array[i][1]) - plt.scatter(x_trajectory,y_trajectory,marker='*',c='yellowgreen') - for p in range(len(controller_type)): - plt.plot(x_points[p],y_points[p],c='grey',linestyle='dashed') - plt.plot(x_points[p][len(x_points[p])-1],y_points[p][len(y_points[p])-1],c='grey',marker='o', ms=10) - - plt.xlabel('x-axis (meters)',fontdict={'family':'serif','size':12}) - plt.ylabel('y-axis (meters)',fontdict={'family':'serif','size':12}) - - if trajectory_type=="circle": - plt.plot(x+r,y,marker='*',c='yellowgreen',ms=13) - else: - plt.plot(x,y,marker='*',c='yellowgreen',ms=13) - plt.plot(data[0][len(data[0])-2],data[0][len(data[0])-1],c='yellowgreen',ms=13) - plt.savefig(os.path.join(get_package_share_directory('ROSNavBench'), - 'raw_data','map_plot.png')) - image_ratio=y_length/x_length - scaling_factor=image_ratio/(500/300) - x_length=int(300*scaling_factor) - y_length=int(500*scaling_factor) - drawing = shapes.Drawing(500,60) - drawing.add(String(200,40,'Traveled path ', fontSize=12, fillColor=colors.black)) - drawing.add(Circle(300,10,3,fillColor=colors.grey,strokeColor=colors.grey)) - drawing.add(String(308,10,'Final pose ',fontSize=12, fillColor=colors.black)) - star_vertices=[201.99,9.78,205.88,11.194,201.04,11.708,200,16,198.8,11.6,194.1,11.091,198.07,9.46,196.3,5.277,200,8,203.86,5.406,201.99,9.78] - drawing.add(Polygon(star_vertices, fillColor=colors.yellowgreen,strokeColor=colors.yellowgreen)) - drawing.add(String(210,10,'Initial pose ',fontSize=12, fillColor=colors.black)) - drawing.add(Line(1,13,7,13, strokeColor=colors.yellowgreen, strokeWidth=3)) - drawing.add(String(9,10,'Global planner path ',fontSize=12, fillColor=colors.black)) - drawing.add(String(115,10,'x ',fontSize=13, fillColor=colors.yellowgreen)) - drawing.add(String(124,10,'Waypoints ',fontSize=12, fillColor=colors.black)) - drawing.add(String(370,10,'-- ',fontSize=15, fillColor=colors.grey)) - drawing.add(String(382,10,'Robot path ',fontSize=12, fillColor=colors.black)) - elements.append(drawing) - elements.append(Image_pdf(os.path.join(get_package_share_directory('ROSNavBench'), - 'raw_data','map_plot.png'),y_length,x_length)) - - - first_failure=0 - for i in range(len(controller_type)): - log_msgs=[] - if result(i)=="failed" or result(i)=='goal has an invalid return status!': - if first_failure==0: - d=shapes.Drawing(250,40) - d.add(String(1,20,"Failure report",fontSize=15)) - elements.append(d) - first_failure=1 - # Opening the csv of the error msgs - f=open(os.path.join(get_package_share_directory('ROSNavBench'), - 'raw_data', - pdf_name+'_'+controller_type[i]+"_error_msgs_"+str(i+1)+'.csv'),'r') - writer=csv.reader(f,quoting=csv.QUOTE_NONNUMERIC,delimiter=' ') - for lines in writer: - log_msgs.append(lines[:]) - d=shapes.Drawing(250,40) - d.add(String(1,20,"Log messages of "+controller_type[i]+ " #"+str(i),fontSize=15)) - elements.append(d) - table= [["Logger_name", "Level", "Message"]] - table.append([""]) - for k in range(len(log_msgs[i])): - table.append(log_msgs[i][k]) - - - - t=Table(table) - t.setStyle(TableStyle([('INNERGRID',(0,0), (-1,-1), 0.25, colors.black), - ('BOX',(0,0), (-1,-1), 0.25, colors.black), - ('SPAN',(0,0),(0,1)), - ('SPAN',(1,0),(1,1)), - ('SPAN',(2,0),(2,1)), - ('FONTNAME',(0,0),(8,1),'Helvetica-Bold'), - ('FONTSIZE',(0,0), (-1,-1),8) - ])) - elements.append(t) - elements.append(Drawing(500, 10)) - doc.build(elements) - \ No newline at end of file diff --git a/ROSNavBench/marker_publisher.py b/ROSNavBench/marker_publisher.py index 174b4df..4ed50e9 100644 --- a/ROSNavBench/marker_publisher.py +++ b/ROSNavBench/marker_publisher.py @@ -59,7 +59,7 @@ def main(args=None): marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 if os.environ["controller"]!= None: - marker.text=os.environ["controller"]+"\n"+os.environ["planner"] + marker.text=os.environ["controller"]+"\n"+os.environ["planner"]+" "+os.environ["round_num"] else: marker.text='None' for i in range(4): diff --git a/ROSNavBench/performace_analysis.py b/ROSNavBench/performace_analysis.py index 40fc79d..94b0ada 100644 --- a/ROSNavBench/performace_analysis.py +++ b/ROSNavBench/performace_analysis.py @@ -15,9 +15,21 @@ # success rate will be provided on seperate analysis #Data will be recived as a table of data [[name of critera as in table],[1m,1,1,1,]] +analysis_data=[[['Controller\ntype', 'Success\n rate\n (%)', 'Average \nexecution\ntime (sec)', 'CPU(%)', '', 'Memory usage(%)', 'Memory usage (%)', 'Average\nnumber of\nrecoveries', 'Average\n path\nlength(m)', 'Min\nproximity to\n obstacles(m)'], + ['', '', '', 'Average', 'Max', 'Average', 'Max', '', '', ''], + ['RPP', '100.0', '17.53', '21.21', '25.2', '10.56', '10.6', '0.00', '3.13', '0.58'], + ['DWB', '100.0', '17.87', '20.36', '24.9', '10.60', '10.6', '0.00', '3.15', '0.58']], + [['Controller\ntype', 'Success\n rate\n (%)', 'Average \nexecution\ntime (sec)', 'CPU(%)', '', 'Memory usage(%)', 'Memory usage (%)', 'Average\nnumber of\nrecoveries', 'Average\n path\nlength(m)', 'Min\nproximity to\n obstacles(m)'], + ['', '', '', 'Average', 'Max', 'Average', 'Max', '', '', ''], + ['RPP', '100.0', '16.82', '18.93', '22.9', '10.60', '10.6', '0.00', '3.08', '0.53'], + ['DWB', '100.0', '17.90', '19.75', '25.8', '10.68', '10.7', '0.00', '3.11', '0.52']]] +criteria= ["Time", "Path Length", "CPU", "Memory", "Safety"] +planner_type= ["NavFn", "smac_planner"] +controller_type= ["RPP", "DWB"] def performance_analysis(criteria,data,weights,planner_type,controller_type): data,combinations,iterations_result=extract_data(criteria,data,planner_type,controller_type) + print(data) if weights=='None': #The user have not spceified weights normalized_weights=assign_weight(criteria) @@ -47,7 +59,7 @@ def extract_data(criteria, data,planner_type,controller_type): iteration=len(controller_type)*i+j combinations[iteration]+=" "+planner_type[i] - iterations_results=[inner_list[1] for outer_list in data for inner_list in outer_list[2:]] + iterations_results=[float(inner_list[1]) for outer_list in data for inner_list in outer_list[2:]] for i in range(len(criteria)): if criteria[i]=="Time": arranged_data.append([float(inner_list[2]) for outer_list in data for inner_list in outer_list[2:]]) @@ -87,6 +99,7 @@ def assign_weight(criteria_order): # Normalize the weights total_weight = sum(weights.values()) normalized_weights = {criterion: weight / total_weight for criterion, weight in weights.items()} + normalized_weights return normalized_weights @@ -105,14 +118,14 @@ def normalizing_data(data): if not identical_element(data[i+1]): for j in range(len(data[i+1])): - if data[0][i]=="Safety": - data[i+1][j]=(data[i+1][j]-min_value[i])/(max_value[i]-min_value[i]) - else: - data[i+1][j]=(max_value[i]-data[i+1][j])/(max_value[i]-min_value[i]) + #if data[0][i]=="Safety": + # data[i+1][j]=(data[i+1][j]-min_value[i])/(max_value[i]-min_value[i]) + #else: + data[i+1][j]=(max_value[i]-data[i+1][j])/(max_value[i]-min_value[i]) else: # if all values of a criterion are identical , set all of them to zero as they are not going to affect the final result data[i+1]=[0]*len(data[i+1]) - + print("metric data",data) return data def identical_element(array): @@ -125,14 +138,18 @@ def identical_element(array): def rank(normailzed_data,weights,combinations,results): for i in range(len(normailzed_data[0])): for j in range(len(normailzed_data[i+1])): - normailzed_data[i+1][j]=round(normailzed_data[i+1][j]*weights[normailzed_data[0][i]],4) - + if normailzed_data[0][i]=="Safety": + + normailzed_data[i+1][j]=normailzed_data[i+1][j]*(-1)*weights[normailzed_data[0][i]] + else: + normailzed_data[i+1][j]=normailzed_data[i+1][j]*weights[normailzed_data[0][i]] + + ranking=[] for j in range(len(normailzed_data[1])): - ranking.append(sum([sublist[j] for sublist in normailzed_data[1:]])) - + ranking.append(round(sum([sublist[j] for sublist in normailzed_data[1:]]),2)) for i in range(len(results)-1,-1,-1): - if results[i]!='succeeded': + if results[i]==0.0: del combinations[i] del ranking[i] ranking=dict(zip(combinations,ranking)) @@ -164,7 +181,7 @@ def success_rate(result,controller_type,planner_type): controllers=[] planners=[] for i in range(len(planner_type)): - planners.append("The success rate of "+planner_type[i]+" is "+str(round((result[i*len(controller_type):((i+1)*len(controller_type))].count('succeeded')/len(controller_type))*100,2))+" %") + planners.append("The success rate of "+planner_type[i]+" is "+str(round((sum(result[i*len(controller_type):((i+1)*len(controller_type))])/len(controller_type)),2))+" %") for i in range(len(controller_type)): controllers.append([]) for i in range(len(controller_type)): @@ -173,7 +190,9 @@ def success_rate(result,controller_type,planner_type): controllers[i].append(result[j*len(controller_type)+i]) for i in range(len(controllers)): - controllers[i]="The success rate of "+controller_type[i]+" is "+str(round((controllers[i].count('succeeded')/len(planner_type))*100,2))+" %" + controllers[i]="The success rate of "+controller_type[i]+" is "+str(round((sum(controllers[i])/len(planner_type)),2))+" %" return planners,controllers +ranking,planners_success_rate,controllers_success_rate=performance_analysis(criteria,analysis_data,'None',planner_type,controller_type) +print(ranking) \ No newline at end of file diff --git a/config/circle_path_scenario.yaml b/config/circle_path_scenario.yaml index d8a313a..0f05e52 100644 --- a/config/circle_path_scenario.yaml +++ b/config/circle_path_scenario.yaml @@ -2,45 +2,46 @@ experiment_name: "circular_path_scenario_two_controllers" #Save directory of results -results_directory: "/home/riotu/ros2_ws2/src/ROSNavBench/results" +results_directory: "/home/riotu/ros5_ws/src/ROSNavBench/results" # Add the absolute path of the world -world_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/worlds/scenario_world.world" +world_path: "/home/riotu/industrial_warehouse.world" # Add the absolute path of the map -map_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.yaml" +map_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.yaml" # Add the absolute path of the map -map_png_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.pgm" +map_png_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.pgm" # Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros2_ws2/src/ROSNavBench/simulations/models" +models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros5_ws/src/ROSNavBench/simulations/models" # Choose a type of global local planners to be tested -planner_type: ["NavFn", "smac_planner"] +planner_type: ["NavFn"] # Choose one of the following controllers, by adding its name to the controller type list # Note that these are the avaible controllers for the provided example # DWB # RPP # RPP_RSC # DWB_RSC -controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree +controller_type: ["RPP"] #name of behaviour tree -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 +# This option is intended for testing the same controller and planner combination several times +# Min value is one +instances_num: 1 # Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" +nav_config: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle_pi.yaml" # Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" +behaviour_tree_directory: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" # Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" +urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf" # If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" +model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf" + #Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] @@ -49,8 +50,8 @@ criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] weights: "None" # Provide the pose in a decimal form -spawn_pose_x: 4.0 -spawn_pose_y: -3.0 +spawn_pose_x: -1.25 +spawn_pose_y: -2.40 spawn_pose_yaw: -3.13 #radians # Specify either to send one goal, specific trajectory, or several waypoints @@ -58,4 +59,4 @@ spawn_pose_yaw: -3.13 #radians trajectory_type: "circle" # Circule parameter, note that the place of spawn will be the center of the circle -radius: 1.1 +radius: 2.0 diff --git a/config/narrow_path.yaml b/config/differen_tunning_DWB.yaml similarity index 58% rename from config/narrow_path.yaml rename to config/differen_tunning_DWB.yaml index 3282444..d0feb72 100644 --- a/config/narrow_path.yaml +++ b/config/differen_tunning_DWB.yaml @@ -1,63 +1,64 @@ # Name the experiment. This name will be the name of the report -experiment_name: "narrow_path_scenario_report" +experiment_name: "DWB_tuning" #Save directory of results -results_directory: "/home/riotu/perform_ws/src/ROSNavBench/results" +results_directory: "/home/riotu/ros5_ws/src/ROSNavBench/results" # Add the absolute path of the world -world_path: "/home/riotu/perform_ws/src/ROSNavBench/simulations/worlds/new_dynamic.world" +world_path: "/home/riotu/industrial_warehouse.world" # Add the absolute path of the map -map_path: "/home/riotu/perform_ws/src/ROSNavBench/simulations/maps/map.yaml" +map_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.yaml" # Add the absolute path of the map -map_png_path: "/home/riotu/perform_ws/src/ROSNavBench/simulations/maps/map.pgm" +map_png_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.pgm" # Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/perform_ws/src/ROSNavBench/simulations/models" +models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros5_ws/src/ROSNavBench/simulations/models" # Choose a type of global local planners to be tested -planner_type: ["NavFn", "smac_planner"] +planner_type: ["NavFn"] # Choose one of the following controllers, by adding its name to the controller type list # Note that these are the avaible controllers for the provided example # DWB # RPP # RPP_RSC # DWB_RSC -controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree +controller_type: ["DWB","DWB_1","DWB_2"] #name of behaviour tree -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 +# This option is intended for testing the same controller and planner combination several times +# Min value is one +instances_num: 1 # Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" +nav_config: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle_pi.yaml" # Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" +behaviour_tree_directory: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" # Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" +urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf" # If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" +model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf" + #Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" -criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] +criteria: ["Time", "CPU"] + #Wieght for each criteria as a number from 1 to 9 #Set None if the wieght to be set automatcally giving that the importance to the eariler criteria in the matrix weights: "None" # Provide the pose in a decimal form -spawn_pose_x: 7.67 -spawn_pose_y: 6.22 +spawn_pose_x: 1.57 +spawn_pose_y: 8.66 spawn_pose_yaw: -3.13 #radians # Specify either to send one goal, specific trajectory, or several waypoints # square, circle, several_waypoints, one_goal trajectory_type: "one_goal" - # One goal parameter -goal_pose_x: 4.00 -goal_pose_y: 6.22 -goal_pose_yaw: -3.13 #radians +goal_pose_x: -3.86 +goal_pose_y: 4.29 +goal_pose_yaw: 2.58 #radians diff --git a/config/dynamic_obstacles.yaml b/config/dynamic_obstacles.yaml deleted file mode 100644 index d3f55ca..0000000 --- a/config/dynamic_obstacles.yaml +++ /dev/null @@ -1,67 +0,0 @@ -## Name the experiment. This name will be the name of the report -experiment_name: "waypoints_all_controllers1" - -#Save directory of results -results_directory: "/home/riotu/perform_ws/src/ROSNavBench/results" - -# Add the absolute path of the world -world_path: "/home/riotu/perform_ws/src/ROSNavBench/simulations/worlds/scenario_world.world" - -# Add the absolute path of the map -map_path: "/home/riotu/perform_ws/src/ROSNavBench/simulations/maps/map.yaml" - -# Add the absolute path of the map -map_png_path: "/home/riotu/perform_ws/src/ROSNavBench/simulations/maps/map.pgm" - -# Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/perform_ws/src/ROSNavBench/simulations/models" - -# Choose a type of global local planners to be tested -planner_type: ["NavFn", "smac_planner"] -# Choose one of the following controllers, by adding its name to the controller type list -# Note that these are the avaible controllers for the provided example -# DWB -# RPP -# RPP_RSC -# DWB_RSC -controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree - -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 - -# Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" - -# Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" - -# Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" - -# If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" -#Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" -criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] -#["Time", "Path Length", "CPU", "Memory", "Safety"] -#Wieght for each criteria as a number from 1 to 9 -#Set None if the wieght to be set automatcally giving that the importance to the eariler criteria in the matrix -weights: "None" -# Provide the pose in a decimal form -spawn_pose_x: 4.0 -spawn_pose_y: -4.0 -spawn_pose_yaw: 1.58 #radians - -# Specify either to send one goal, specific trajectory, or several waypoints -# square, circle, several_waypoints, one_goal -trajectory_type: "several_waypoints" -# One goal parameter -goal_pose_x: -2.30 -goal_pose_y: -3.00 -goal_pose_yaw: -3.13 #radians -# Specify way points in this formate [[x0,y0,yaw0_radians], [x1,y1,yaw1_radians], ...]. Type should be float -waypoints: [[-1.83, 3.26, 1.5], [-4.0, 0.17, 1.5], [-1.39, -3.82, 1.5]] -#[[-2.0, -3.00, -3.13]] -# failure [[-1.83, 3.26, 1.5], [-4.0, 0.17, 1.5], [-1.39, -3.82, 1.5]] -#[[-1.83,3.26,1.5],[-4.0,0.17,1.5],[-1.39,-3.82,1.5],[4.0,-4.0,1.5]] - diff --git a/config/params.yaml b/config/params.yaml index a76fa0a..c76813c 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -2,19 +2,19 @@ experiment_name: "param_guide" #Save directory of results -results_directory: "/home/riotu/ros2_ws2/src/ROSNavBench/results" +results_directory: "/home/riotu/ros_ws/src/ROSNavBench/results" # Add the absolute path of the world -world_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/worlds/scenario_world.world" +world_path: "/home/riotu/ros_ws/src/ROSNavBench/simulations/worlds/scenario_world.world" # Add the absolute path of the map -map_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.yaml" +map_path: "/home/riotu/ros_ws/src/ROSNavBench/simulations/maps/map.yaml" # Add the absolute path of the map -map_png_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.pgm" +map_png_path: "/home/riotu/ros_ws/src/ROSNavBench/simulations/maps/map.pgm" # Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros2_ws2/src/ROSNavBench/simulations/models" +models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros_ws/src/ROSNavBench/simulations/models" # Choose a type of global local planners to be tested planner_type: ["NavFn", "smac_planner"] @@ -26,21 +26,21 @@ planner_type: ["NavFn", "smac_planner"] # DWB_RSC controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 +# This option is intended for testing the same controller and planner combination several times +# Min value is one +instances_num: 1 # Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" +nav_config: "/home/riotu/ros_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle_pi.yaml" # Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" +behaviour_tree_directory: "/home/riotu/ros_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" # Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" +urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf" # If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" +model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf" #Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] diff --git a/config/random_waypoints.yaml b/config/random_waypoints_2.yaml similarity index 63% rename from config/random_waypoints.yaml rename to config/random_waypoints_2.yaml index 7145e18..04514b2 100644 --- a/config/random_waypoints.yaml +++ b/config/random_waypoints_2.yaml @@ -1,20 +1,20 @@ # Name the experiment. This name will be the name of the report -experiment_name: "several_waypoints_scenario_report" +experiment_name: "several_waypoints" #Save directory of results -results_directory: "/home/riotu/ros2_ws2/src/ROSNavBench/results" +results_directory: "/home/riotu/ros5_ws/src/ROSNavBench/results" # Add the absolute path of the world -world_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/worlds/scenario_world.world" +world_path: "/home/riotu/industrial_warehouse.world" # Add the absolute path of the map -map_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.yaml" +map_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.yaml" # Add the absolute path of the map -map_png_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.pgm" +map_png_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.pgm" # Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros2_ws2/src/ROSNavBench/simulations/models" +models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros5_ws/src/ROSNavBench/simulations/models" # Choose a type of global local planners to be tested planner_type: ["NavFn", "smac_planner"] @@ -24,23 +24,23 @@ planner_type: ["NavFn", "smac_planner"] # RPP # RPP_RSC # DWB_RSC -controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree +controller_type: ["RPP", "DWB","MPPI"] #name of behaviour tree -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 +# This option is intended for testing the same controller and planner combination several times +# Min value is one +instances_num: 5 # Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" +nav_config: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle_pi.yaml" # Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" +behaviour_tree_directory: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" # Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" +urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf" # If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" +model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf" #Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] @@ -50,8 +50,8 @@ criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] weights: "None" # Provide the pose in a decimal form -spawn_pose_x: -6.0 -spawn_pose_y: 2.0 +spawn_pose_x: 1.05 +spawn_pose_y: 6.64 spawn_pose_yaw: -3.13 #radians # Specify either to send one goal, specific trajectory, or several waypoints @@ -59,4 +59,5 @@ spawn_pose_yaw: -3.13 #radians trajectory_type: "several_waypoints" # Specify way points in this formate [[x0,y0,yaw0_radians], [x1,y1,yaw1_radians], ...] -waypoints: [[-2.4, 3.5, -3.13], [6.0, 2.0, -3.13], [7.3, 2.6, -3.13]] +waypoints: [[1.38,2.12, -3.13],[5.45,1.99, -3.13],[5.5,-2.18, -3.13]] + diff --git a/config/square_path_scenario.yaml b/config/square_path_scenario.yaml index e58c87b..ec9d116 100644 --- a/config/square_path_scenario.yaml +++ b/config/square_path_scenario.yaml @@ -2,19 +2,19 @@ experiment_name: "square_path_scenario" #Save directory of results -results_directory: "/home/riotu/ros2_ws2/src/ROSNavBench/results" +results_directory: "/home/riotu/ros5_ws/src/ROSNavBench/results" # Add the absolute path of the world -world_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/worlds/scenario_world.world" +world_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/worlds/scenario_world.world" # Add the absolute path of the map -map_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.yaml" +map_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/map.yaml" # Add the absolute path of the map -map_png_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.pgm" +map_png_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/map.pgm" # Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros2_ws2/src/ROSNavBench/simulations/models" +models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros5_ws/src/ROSNavBench/simulations/models" # Choose a type of global local planners to be tested planner_type: ["NavFn", "smac_planner"] @@ -26,21 +26,21 @@ planner_type: ["NavFn", "smac_planner"] # DWB_RSC controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 +# This option is intended for testing the same controller and planner combination several times +# Min value is one +instances_num: 1 # Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" +nav_config: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle_pi.yaml" # Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" +behaviour_tree_directory: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" # Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" +urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf" # If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" +model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf" #Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] diff --git a/config/static_obstacles.yaml b/config/static_obstacles.yaml index bb97527..987d1be 100644 --- a/config/static_obstacles.yaml +++ b/config/static_obstacles.yaml @@ -1,20 +1,20 @@ # Name the experiment. This name will be the name of the report -experiment_name: "static_obstacles_scenario_report" +experiment_name: "static_obstacles_scenario_report_f" #Save directory of results -results_directory: "/home/riotu/ros2_ws2/src/ROSNavBench/results" +results_directory: "/home/riotu/ros5_ws/src/ROSNavBench/results" # Add the absolute path of the world -world_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/worlds/scenario_world.world" +world_path: "/home/riotu/industrial_warehouse.world" # Add the absolute path of the map -map_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.yaml" +map_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.yaml" # Add the absolute path of the map -map_png_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.pgm" +map_png_path: "/home/riotu/ros5_ws/src/ROSNavBench/simulations/maps/warehouse_slam_toolbox.pgm" # Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros2_ws2/src/ROSNavBench/simulations/models" +models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros5_ws/src/ROSNavBench/simulations/models" # Choose a type of global local planners to be tested planner_type: ["NavFn", "smac_planner"] @@ -24,41 +24,40 @@ planner_type: ["NavFn", "smac_planner"] # RPP # RPP_RSC # DWB_RSC -controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree - -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 +controller_type: ["RPP", "DWB", "DWB_RSC", "RPP_RSC"] #name of behaviour tree +# This option is intended for testing the same controller and planner combination several times +# Min value is one +instances_num: 1 # Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" +nav_config: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle_pi.yaml" # Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" +behaviour_tree_directory: "/home/riotu/ros5_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" # Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" +urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf" # If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" +model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf" #Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" -criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] +criteria: ["Time", "CPU"] #Wieght for each criteria as a number from 1 to 9 #Set None if the wieght to be set automatcally giving that the importance to the eariler criteria in the matrix weights: "None" # Provide the pose in a decimal form -spawn_pose_x: 0.0 -spawn_pose_y: -3.4 -spawn_pose_yaw: 1.58 #radians +spawn_pose_x: 1.05 +spawn_pose_y: 6.64 +spawn_pose_yaw: -3.13 #radians # Specify either to send one goal, specific trajectory, or several waypoints # square, circle, several_waypoints, one_goal trajectory_type: "one_goal" # One goal parameter -goal_pose_x: -1.3 -goal_pose_y: 2.3 -goal_pose_yaw: 1.58 #radians +goal_pose_x: -5.6 +goal_pose_y: 2.2 +goal_pose_yaw: -3.13 #radians diff --git a/config/straight_line.yaml b/config/straight_line.yaml deleted file mode 100644 index 4d19aa0..0000000 --- a/config/straight_line.yaml +++ /dev/null @@ -1,65 +0,0 @@ -# Name the experiment. This name will be the name of the report -experiment_name: "straight_line_scenario_report" - -#Save directory of results -results_directory: "/home/riotu/ros2_ws2/src/ROSNavBench/results" - -# Add the absolute path of the world -world_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/worlds/scenario_world.world" - -# Add the absolute path of the map -map_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.yaml" - -# Add the absolute path of the map -map_png_path: "/home/riotu/ros2_ws2/src/ROSNavBench/simulations/maps/map.pgm" - -# Add gazebo model path, if multiple path, please spereate them by a : -models_path: "/opt/ros/humble/share/turtlebot3_gazebo/models:/home/riotu/ros2_ws2/src/ROSNavBench/simulations/models" - -# Choose a type of global local planners to be tested -planner_type: ["NavFn", "smac_planner"] -# Choose one of the following controllers, by adding its name to the controller type list -# Note that these are the avaible controllers for the provided example -# DWB -# RPP -# RPP_RSC -# DWB_RSC -controller_type: ["RPP", "DWB", "MPPI"] #name of behaviour tree - -# This option is intended for testing the same controller several times -# if the user do not need this test, set it to zero -trails_num: 0 - -# Add the absolute path of the robot navigation configuration -nav_config: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/nav2_config/waffle.yaml" - -# Add the directory of the behaviour tree -behaviour_tree_directory: "/home/riotu/perform_ws/src/ROSNavBench/example/turtlebot3/behavior_trees" - -# Add the absolute path of the urdf and model files inside single qoutes '' -urdf_file: "/opt/ros/humble/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf" - -# If the robot is spawned by only urdf, then set the model_file to 'None' -model_file: "/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf" - -#Citeria for performace analysis includes: "Time" ,"CPU", "Memory", "Path Length" , "Safety" -criteria: ["Time", "Path Length", "CPU", "Memory", "Safety"] - -#Wieght for each criteria as a number from 1 to 9 -#Set None if the wieght to be set automatcally giving that the importance to the eariler criteria in the matrix -weights: "None" - -# Provide the pose in a decimal form -spawn_pose_x: -6.28 -spawn_pose_y: -4.0 -spawn_pose_yaw: 1.58 #radians - -# Specify either to send one goal, specific trajectory, or several waypoints -# square, circle, several_waypoints, one_goal -trajectory_type: "one_goal" - -# One goal parameter -goal_pose_x: -6.28 -goal_pose_y: 1.0 -goal_pose_yaw: 1.58 #radians - diff --git a/example/turtlebot3/behavior_trees/pose.xml b/example/turtlebot3/behavior_trees/pose.xml index 4c2be4c..4f853f9 100644 --- a/example/turtlebot3/behavior_trees/pose.xml +++ b/example/turtlebot3/behavior_trees/pose.xml @@ -1,9 +1,4 @@ - @@ -11,12 +6,18 @@ - + + + + - - + + + + + @@ -28,7 +29,7 @@ - + diff --git a/example/turtlebot3/behavior_trees/poses.xml b/example/turtlebot3/behavior_trees/poses.xml index 1f4f15d..e8df1e5 100644 --- a/example/turtlebot3/behavior_trees/poses.xml +++ b/example/turtlebot3/behavior_trees/poses.xml @@ -5,15 +5,21 @@ - - + + - + + + + - - - + + + + + + @@ -25,9 +31,9 @@ - + - \ No newline at end of file + diff --git a/example/turtlebot3/nav2_config/waffle.yaml b/example/turtlebot3/nav2_config/waffle_pi.yaml similarity index 75% rename from example/turtlebot3/nav2_config/waffle.yaml rename to example/turtlebot3/nav2_config/waffle_pi.yaml index 8c655de..431ef67 100644 --- a/example/turtlebot3/nav2_config/waffle.yaml +++ b/example/turtlebot3/nav2_config/waffle_pi.yaml @@ -60,37 +60,37 @@ bt_navigator: groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -105,15 +105,16 @@ controller_server: min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] - controller_plugins: ["MPPI","DWB_RSC","RPP", "DWB", "RPP_RSC"] + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: + ["MPPI", "DWB_RSC", "RPP", "DWB", "RPP_RSC", "DWB_1", "DWB_2"] # Progress checker parameters progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 movement_time_allowance: 10.0 - + general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" @@ -160,7 +161,16 @@ controller_server: trans_stopped_velocity: 0.05 short_circuit_trajectory_evaluation: True stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 PathAlign.forward_point_distance: 0.1 @@ -184,7 +194,7 @@ controller_server: transform_tolerance: 0.2 use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 - approach_velocity_scaling_dist: 0.6 + approach_ve7740404e8c4f locity_scaling_dist: 0.6 use_collision_detection: true max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true @@ -227,7 +237,114 @@ controller_server: trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + + DWB_1: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.22 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.22 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 0 + vtheta_samples: 40 + sim_time: 3.0 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + + DWB_2: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.22 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.22 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 0 + vtheta_samples: 40 + sim_time: 4.0 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 PathAlign.forward_point_distance: 0.1 @@ -270,19 +387,20 @@ controller_server: max_robot_pose_search_dist: 10.0 use_interpolation: false + ############# - MPPI: + MPPI: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 model_dt: 0.05 batch_size: 2000 - vx_std: 0.2 + vx_std: 0.22 vy_std: 0.2 wz_std: 0.4 - vx_max: 0.5 + vx_max: 0.22 vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 + vy_max: 0.0 + wz_max: 1.0 iteration_count: 1 prune_distance: 1.7 transform_tolerance: 0.1 @@ -290,12 +408,24 @@ controller_server: gamma: 0.015 motion_model: "DiffDrive" visualize: false + reset_period: 1.0 # (only in Humble) + regenerate_noises: false TrajectoryVisualizer: trajectory_step: 5 time_step: 3 - AckermannConstrains: + AckermannConstraints: min_turning_r: 0.2 - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: + [ + "ConstraintCritic", + "ObstaclesCritic", + "GoalCritic", + "GoalAngleCritic", + "PathAlignCritic", + "PathFollowCritic", + "PathAngleCritic", + "PreferForwardCritic", + ] ConstraintCritic: enabled: true cost_power: 1 @@ -324,6 +454,8 @@ controller_server: collision_cost: 10000.0 collision_margin_distance: 0.1 near_goal_distance: 0.5 + inflation_radius: 0.55 # (only in Humble) + cost_scaling_factor: 10.0 # (only in Humble) PathAlignCritic: enabled: true cost_power: 1 @@ -346,11 +478,12 @@ controller_server: offset_from_furthest: 4 threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 - forward_preference: true + mode: 0 # TwirlingCritic: # enabled: true # twirling_cost_power: 1 # twirling_cost_weight: 10.0 + controller_server_rclcpp_node: ros__parameters: use_sim_time: true @@ -424,7 +557,8 @@ global_costmap: robot_radius: 0.22 resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] + plugins: + ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -491,8 +625,8 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 use_sim_time: true - planner_plugins: ["NavFn", "smac_planner", "HybridA", "ThetaStar"] - + planner_plugins: ["NavFn", "smac_planner", "HybridA", "ThetaStar"] + # ROS2 Planner Parameters and Evaluation Criteria # Planner 1: NavFn @@ -589,8 +723,6 @@ planner_server: w_traversal_cost: 2.0 w_heuristic_cost: 1.0 - - planner_server_rclcpp_node: ros__parameters: use_sim_time: true @@ -624,7 +756,7 @@ waypoint_follower: ros__parameters: loop_rate: 200 stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" + waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True diff --git a/launch/main.launch.py b/launch/main.launch.py index 37d5537..5e16e05 100644 --- a/launch/main.launch.py +++ b/launch/main.launch.py @@ -25,11 +25,6 @@ def generate_launch_description(): # Get the name of config file of the current experiment specs = os.environ['PARAMS_FILE'] # Opening the config file to take the experiment data such as spawn pose - # specs= os.path.join( - # get_package_share_directory('ROSNavBench'), - # 'config', - # params_file+'.yaml' - # ) with open(specs, 'r') as file: robot_specs = yaml.safe_load(file) @@ -39,19 +34,10 @@ def generate_launch_description(): y = robot_specs['spawn_pose_y'] yaw = robot_specs['spawn_pose_yaw'] trajectory_type = robot_specs['trajectory_type'] - trails_num = robot_specs['trails_num'] - if trails_num>0: - controller_type=[controller_type[0]]*trails_num - planner_type=[planner_type[0]] - # Node for generating pdf - pdf_generator=Node( - name='benchmarking_single_controller', - executable='benchmarking_single_controller', - package='ROSNavBench', - ) - else: - # Node for generating pdf - pdf_generator=Node( + instances_num= robot_specs['instances_num'] + + # Node for generating pdf + pdf_generator=Node( name='main_pdf_generator', executable='main_pdf_generator', package='ROSNavBench', @@ -83,7 +69,7 @@ def generate_launch_description(): ld.add_action(SetEnvironmentVariable(name='round_num',value="1")) # Generating different nodes to publish the type of running controller controller_node=[] - for j in range(len(controller_type)*len(planner_type)): + for j in range(len(controller_type)*len(planner_type)*instances_num): controller_node.append(Node( name='marker_publisher', executable='marker_publisher', @@ -91,7 +77,7 @@ def generate_launch_description(): )) # Generating different nodes for sending the goal and recording the data nodes=[] - for j in range(len(controller_type)*len(planner_type)): + for j in range(len(controller_type)*len(planner_type)*instances_num): nodes.append(Node( name='follow_path_0', executable='follow_path', @@ -99,7 +85,7 @@ def generate_launch_description(): )) # Generating different nodes for reseting the pose of the robot to intial pose after each controller scenario state_nodes=[] - for k in range(len(controller_type)*len(planner_type)-1): + for k in range(len(controller_type)*len(planner_type)*instances_num-1): state_nodes.append(ExecuteProcess( cmd=[[ FindExecutable(name='ros2'), @@ -112,23 +98,20 @@ def generate_launch_description(): ) ) - # This loop is responsible for assgining the events of sending the goal and reseting the state - # Each event will start once the previous event is done +# Each event will start once the previous event is done for k in range(len(planner_type)): for i in range(len(controller_type)): - if i==0 and k==0: - ld.add_action(nodes[0]) - ld.add_action(controller_node[0]) - else: - if k==0: - increament=i-1 + for q in range(instances_num): + if i==0 and k==0 and q==0: + ld.add_action(nodes[0]) + ld.add_action(controller_node[0]) else: - increament=len(controller_type)*k+i-1 - ld.add_action(RegisterEventHandler(OnProcessExit(target_action= nodes[increament], on_exit=[state_nodes[increament]]))) #edit - ld.add_action(RegisterEventHandler(OnProcessExit(target_action=state_nodes[increament], on_exit=[SetEnvironmentVariable(name='planner',value=planner_type[k]),SetEnvironmentVariable(name='controller',value=controller_type[i]),SetEnvironmentVariable(name='round_num',value=str(increament+2)),nodes[increament+1],controller_node[increament+1]]))) + increament=k*len(controller_type)*instances_num+i*instances_num+q-1 + ld.add_action(RegisterEventHandler(OnProcessExit(target_action= nodes[increament], on_exit=[state_nodes[increament]]))) #edit + ld.add_action(RegisterEventHandler(OnProcessExit(target_action=state_nodes[increament], on_exit=[SetEnvironmentVariable(name='planner',value=planner_type[k]),SetEnvironmentVariable(name='controller',value=controller_type[i]),SetEnvironmentVariable(name='round_num',value=str(increament+2)),nodes[increament+1],controller_node[increament+1]]))) # Once all events are done, the node of generating a pdf will start - ld.add_action(RegisterEventHandler(OnProcessExit(target_action=nodes[len(controller_type)*len(planner_type)-1], on_exit=[pdf_generator] ))) + ld.add_action(RegisterEventHandler(OnProcessExit(target_action=nodes[len(controller_type)*len(planner_type)*instances_num-1], on_exit=[pdf_generator] ))) return ld