<reponame>ricfrr/Complexity-Aware
import numpy as np 
import pandas as pd
import xml.etree.ElementTree
import roslaunch
import os 
from scipy.stats import truncnorm


def get_truncated_normal(mean=0, sd=1, range_v=10):
    low = mean - range_v
    upp = mean + range_v
    return truncnorm((low - mean) / sd, (upp - mean) / sd, loc=mean, scale=sd)

def run_test(n_test = 200, repeat_n = 4 ):

    base_launch =  "/home/riccardo.franceschin/Phd/ros_code/catkin_ws/src/mav_voxblox_planning/voxblox_rrt_planner/launch/rrt_saved_map.launch"
    et = xml.etree.ElementTree.parse(base_launch)

    for n in range(n_test):

        complexity_w = "110" #str(round(get_truncated_normal(1, range_v=2, sd=0.5).rvs(1)[0],2))
        complexity_radius =  "2.0" #str(round(get_truncated_normal(2, range_v=1, sd=0.5).rvs(1)[0],2))
        second_to_plan = 10


        print("RUNNING TEST N : "+ str(n)+"/"+str(n_test)+" complexity_w : "+complexity_w+" complexity_radius: " + complexity_radius)
        for k in range(repeat_n):
            print("REPETITION TEST N : "+ str(k)+"/"+str(repeat_n))
            for child in et.getroot().iter("param"):
                if (child.attrib["name"] =="deterministic"):
                    child.attrib["value"] = "false" 
                if (child.attrib["name"] =="complexity_w"):
                    child.attrib["value"] = complexity_w
                if (child.attrib["name"] =="complexity_radius"):
                    child.attrib["value"] = complexity_radius
                if (child.attrib["name"]=="num_seconds_to_plan"):
                    child.attrib["value"] = str(second_to_plan)
                    
                    

            file_path = '/home/riccardo.franceschin/Phd/ros_code/catkin_ws/src/mav_voxblox_planning/voxblox_rrt_planner/launch/modified.launch'
            et.write(file_path) 

            os.system('roslaunch voxblox_rrt_planner '+"modified.launch")


if __name__ == '__main__':
    run_test()

import numpy as np 
import pandas as pd
import xml.etree.ElementTree
import roslaunch
import os 
from scipy.stats import truncnorm


def get_truncated_normal(mean=0, sd=1, range_v=10):
    low = mean - range_v
    upp = mean + range_v
    return truncnorm((low - mean) / sd, (upp - mean) / sd, loc=mean, scale=sd)

def run_test(n_test = 200, repeat_n = 4 ):

    base_launch =  "/home/riccardo.franceschin/Phd/ros_code/catkin_ws/src/mav_voxblox_planning/voxblox_rrt_planner/launch/rrt_saved_map.launch"
    et = xml.etree.ElementTree.parse(base_launch)

    for n in range(n_test):

        complexity_w = "110" #str(round(get_truncated_normal(1, range_v=2, sd=0.5).rvs(1)[0],2))
        complexity_radius =  "2.0" #str(round(get_truncated_normal(2, range_v=1, sd=0.5).rvs(1)[0],2))
        second_to_plan = 10


        print("RUNNING TEST N : "+ str(n)+"/"+str(n_test)+" complexity_w : "+complexity_w+" complexity_radius: " + complexity_radius)
        for k in range(repeat_n):
            print("REPETITION TEST N : "+ str(k)+"/"+str(repeat_n))
            for child in et.getroot().iter("param"):
                if (child.attrib["name"] =="deterministic"):
                    child.attrib["value"] = "false" 
                if (child.attrib["name"] =="complexity_w"):
                    child.attrib["value"] = complexity_w
                if (child.attrib["name"] =="complexity_radius"):
                    child.attrib["value"] = complexity_radius
                if (child.attrib["name"]=="num_seconds_to_plan"):
                    child.attrib["value"] = str(second_to_plan)
                    
                    

            file_path = '/home/riccardo.franceschin/Phd/ros_code/catkin_ws/src/mav_voxblox_planning/voxblox_rrt_planner/launch/modified.launch'
            et.write(file_path) 

            os.system('roslaunch voxblox_rrt_planner '+"modified.launch")


if __name__ == '__main__':
    run_test()
#str(round(get_truncated_normal(1, range_v=2, sd=0.5).rvs(1)[0],2)) #str(round(get_truncated_normal(2, range_v=1, sd=0.5).rvs(1)[0],2))