├── .gitignore ├── .project ├── app_LiLO ├── CMakeLists.txt ├── LiLO_pipeline.png ├── README.md ├── launch │ ├── odomEstimation_KITTI_dataset.launch │ ├── odomEstimation_VLP16.launch │ ├── odomEstimation_carla.launch │ ├── odomEstimation_ouster.launch │ └── odomEstimation_ouster_lite.launch ├── package.xml └── rviz │ ├── nav_geoloc.rviz │ ├── nav_geoloc_camera.rviz │ ├── nav_geoloc_cameras.rviz │ └── odomEstimation.rviz ├── app_MTAP ├── CMakeLists.txt ├── README.md ├── launch │ ├── nav_carla.launch │ └── nav_carla_prediction.launch ├── media │ └── MTAP_example1_compress.gif ├── package.xml ├── params │ └── nav_carla.yaml ├── paths │ ├── Town01.osm │ ├── Town02.osm │ ├── Town03.osm │ ├── Town04.osm │ └── Town05.osm ├── rviz │ └── nav_carla.rviz └── scripts │ ├── __pycache__ │ ├── config.cpython-38.pyc │ └── utils.cpython-38.pyc │ ├── change_weather.py │ ├── config.py │ ├── dataset_generation.sh │ ├── evaluation_execution.sh │ ├── evaluator.py │ ├── joystick_keymap.py │ ├── listener_agent.py │ ├── manual_control_joystick.py │ ├── map2OpenStreet.py │ ├── metrics_average.py │ ├── pedestrians_fixed_routes.py │ ├── record_ros_data.py │ ├── routes │ ├── Town01.xml │ ├── Town02.xml │ ├── Town03.xml │ ├── Town04.xml │ ├── Town05.xml │ └── pedestrian_routes │ │ ├── Town01_evaluation1.xml │ │ ├── Town01_evaluation1_full.xml │ │ ├── Town01_evaluation2.xml │ │ ├── Town01_evaluation2_full.xml │ │ ├── Town01_evaluation3.xml │ │ ├── Town01_evaluation3_full.xml │ │ ├── Town01_evaluation4.xml │ │ ├── Town01_evaluation4_full.xml │ │ ├── Town01_evaluation5.xml │ │ ├── Town01_evaluation5_full.xml │ │ ├── Town01_experiment1.xml │ │ ├── Town01_experiment1_full.xml │ │ ├── Town01_experiment2.xml │ │ ├── Town01_experiment2_full.xml │ │ ├── Town01_experiment3.xml │ │ ├── Town01_experiment3_full.xml │ │ ├── Town01_experiment4.xml │ │ ├── Town01_experiment4_full.xml │ │ ├── Town02_evaluation1.xml │ │ ├── Town02_evaluation1_full.xml │ │ ├── Town02_evaluation2.xml │ │ ├── Town02_evaluation2_full.xml │ │ ├── Town03_evaluation1.xml │ │ ├── Town03_evaluation1_full.xml │ │ ├── Town03_experiment1.xml │ │ ├── Town03_experiment1_full.xml │ │ ├── Town04_evaluation1.xml │ │ ├── Town04_evaluation1_full.xml │ │ ├── Town04_experiment1.xml │ │ ├── Town04_experiment1_full.xml │ │ ├── Town05_evaluation1.xml │ │ ├── Town05_evaluation1_full.xml │ │ ├── Town05_experiment1.xml │ │ └── Town05_experiment1_full.xml │ └── utils.py ├── app_force_estimation └── README.md ├── app_geo_localization ├── CMakeLists.txt ├── README.md ├── launch │ ├── nav_GeoLoc_offline.launch │ └── nav_GeoLoc_online.launch ├── maps │ └── scientific_park_01.osm ├── package.xml ├── params │ ├── nav_GeoLoc_offline.yaml │ └── nav_GeoLoc_online.yaml ├── pipeline_weight.png └── rviz │ └── nav_geoloc.rviz ├── app_geo_navigation ├── CMakeLists.txt ├── README.md ├── launch │ ├── nav_NVP_GeoLoc_offline.launch │ └── nav_NVP_GeoLoc_online.launch ├── maps │ └── scientific_park_01.osm ├── package.xml ├── params │ ├── nav_NVP_GeoLoc_offline.yaml │ └── nav_NVP_GeoLoc_online.yaml ├── paths │ ├── campus_01.osm │ ├── iuii_01.osm │ ├── parque_01.osm │ ├── patos_01.osm │ ├── scientific_park_01.osm │ ├── scientific_park_02.osm │ ├── scientific_park_03.osm │ ├── scientific_park_04.osm │ └── urban_01_nl_2.osm ├── pipeline_navigation.png ├── rviz │ ├── nav_geoloc.rviz │ ├── nav_geoloc_camera.rviz │ └── nav_geoloc_cameras.rviz └── script │ └── change_close_loop.sh ├── app_old ├── CMakeLists.txt ├── README.md ├── launch │ ├── loc_GeoLoc_kitti.launch │ ├── loc_GeoLoc_offline.launch │ ├── loc_GeoLoc_online.launch │ ├── nav_NVP_GN_gazebo.launch │ ├── nav_NVP_GN_offline.launch │ ├── nav_NVP_GN_online.launch │ ├── nav_NVP_GeoLoc_gazebo.launch │ ├── nav_NVP_GeoLoc_offline.launch │ ├── nav_NVP_GeoLoc_online.launch │ ├── nav_follow_offline.launch │ ├── nav_follow_online.launch │ └── odom_LO-FRI_offline.launch ├── maps │ ├── kitti_seq10.osm │ └── scientific_park_01.osm ├── package.xml ├── params │ ├── loc_GeoLoc_kitti.yaml │ ├── loc_GeoLoc_offline.yaml │ ├── loc_GeoLoc_online.yaml │ ├── nav_NVP_GN_gazebo.yaml │ ├── nav_NVP_GN_offline.yaml │ ├── nav_NVP_GN_online.yaml │ ├── nav_NVP_GeoLoc_gazebo.yaml │ ├── nav_NVP_GeoLoc_offline.yaml │ ├── nav_NVP_GeoLoc_online.yaml │ ├── nav_follow_offline.yaml │ └── nav_follow_online.yaml ├── paths │ ├── campus_01.osm │ ├── iuii_01.osm │ ├── parque_01.osm │ ├── patos_01.osm │ ├── scientific_park_01.osm │ ├── scientific_park_02.osm │ ├── scientific_park_03.osm │ └── urban_01_nl_2.osm └── rviz │ ├── det_real.rviz │ ├── nav_follow.rviz │ ├── nav_gazebo.rviz │ ├── nav_geoloc-kitti.rviz │ ├── nav_geoloc.rviz │ └── nav_real.rviz ├── app_tracker_fusion ├── CMakeLists.txt ├── README.md ├── launch │ ├── nav_follow_offline.launch │ └── nav_follow_online.launch ├── package.xml ├── params │ ├── nav_follow_offline.yaml │ └── nav_follow_online.yaml └── rviz │ └── nav_follow.rviz └── applications ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | #gitignore 2 | *.csv 3 | 4 | 5 | -------------------------------------------------------------------------------- /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | applications 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /app_LiLO/LiLO_pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AUROVA-LAB/applications/3eecfd8c77049629d0ac54d25a33a1790120061c/app_LiLO/LiLO_pipeline.png -------------------------------------------------------------------------------- /app_LiLO/README.md: -------------------------------------------------------------------------------- 1 | # LiLO: Lightweight and lowbias LiDAR Odometry method based on spherical range image filtering 2 | 3 | This repository presents the code for replicating the whole pipeline presented in the works: [LiLO: Lightweight and lowbias LiDAR Odometry method based on spherical range image filtering](https://arxiv.org/abs/2311.07291) 4 | 5 | **Note:** This node uses the dependencies of the [aurova_odom](https://github.com/AUROVA-LAB/aurova_odom/tree/main) and [aurova-preprocessed](https://github.com/AUROVA-LAB/aurova_preprocessed) metapackages. 6 | 7 | 8 | ![pipeline](LiLO_pipeline.png) 9 | 10 | Citations [[Preprint](https://arxiv.org/abs/2311.07291)]: 11 | ``` 12 | @article{velasco2023lilo, 13 | title={LiLO: Lightweight and low-bias LiDAR odometry method based on spherical range image filtering}, 14 | author={Velasco-S{\'a}nchez, Edison P and Mu{\~n}oz-Ba{\~n}{\'o}n, Miguel {\'A}ngel and Candelas, Francisco A and Puente, Santiago T and Torres, Fernando}, 15 | journal={arXiv preprint arXiv:2311.07291}, 16 | year={2023} 17 | } 18 | ``` 19 | 20 | We use this LiDAR odometry method in: 21 | 22 | Applications: 23 | ``` 24 | @article{munoz2024geolocalization, 25 | title={Geo-Localization Based on Dynamically Weighted Factor-Graph}, 26 | author={Muñoz-Bañón, Miguel Ángel and Olivas, Alejandro and Velasco-Sánchez, Edison and Candelas, Francisco A. and Torres, Fernando}, 27 | journal={IEEE Robotics and Automation Letters}, 28 | volume={9}, 29 | number={6}, 30 | pages={5599--5606}, 31 | year={2024}, 32 | doi={10.1109/LRA.2024.3396055}, 33 | publisher={IEEE} 34 | } 35 | 36 | @article{munoz2022openstreetmap, 37 | title={Openstreetmap-based autonomous navigation with lidar naive-valley-path obstacle avoidance}, 38 | author={Muñoz-Bañón, Miguel Ángel and Velasco-Sánchez, Edison and Candelas, Francisco A. and Torres, Fernando}, 39 | journal={IEEE Transactions on Intelligent Transportation Systems}, 40 | volume={23}, 41 | number={12}, 42 | pages={24428--24438}, 43 | year={2022}, 44 | doi={10.1109/TITS.2022.3208829}, 45 | publisher={IEEE} 46 | } 47 | ``` 48 | 49 | ## Installation instructions to use 50 | 51 | ### Requirements to use application: 52 | 53 | - System requirements: Ubuntu 20.04 and ROS Noetic. 54 | - External libraries: [ceres-solver-2.0.0](http://ceres-solver.org/installation.html) (IMPORTANT!! [download](https://drive.google.com/file/d/1acZtn_jaHfj2BVgwaDnQH2Lz-7022F1-/view?usp=share_link) version 2.0.0). Eigen and PCL are usually installed join with ROS. 55 | - External ROS packages: "sudo apt-get install ros-noetic-hector-\*". 56 | - Local ROS packages: [aurova_preprocessed](https://github.com/AUROVA-LAB/aurova_preprocessed) and [aurova_odom](https://github.com/AUROVA-LAB/aurova_odom). 57 | 58 | The [odom_estimation_pc](https://github.com/AUROVA-LAB/aurova_odom/tree/main/odom_estimation_pc) node estimates the pose by subscribing to the edge and surface topics published by the [pc_features](https://github.com/AUROVA-LAB/aurova_preprocessed/tree/master/pc_features) node, which filters these features from a point cloud generated by a LiDAR sensor. 59 | 60 | ### Point cloud feature extraction: 61 | 62 | 63 | ```shell 64 | roslaunch roslaunch pc_feature_extraction ouster_image.launch 65 | ``` 66 | 67 | ### Pose estimation: 68 | 69 | ```shell 70 | roslaunch roslaunch odom_estimation_pc odomEstimation_ouster.launch 71 | ``` 72 | -------------------------------------------------------------------------------- /app_LiLO/launch/odomEstimation_KITTI_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /app_LiLO/launch/odomEstimation_VLP16.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /app_LiLO/launch/odomEstimation_carla.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /app_LiLO/launch/odomEstimation_ouster.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /app_LiLO/launch/odomEstimation_ouster_lite.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /app_LiLO/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | app_geo_navigation 4 | 0.0.0 5 | The geo-navigation applications package 6 | 7 | 8 | 9 | 10 | mice85 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /app_MTAP/launch/nav_carla.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /app_MTAP/launch/nav_carla_prediction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /app_MTAP/media/MTAP_example1_compress.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AUROVA-LAB/applications/3eecfd8c77049629d0ac54d25a33a1790120061c/app_MTAP/media/MTAP_example1_compress.gif -------------------------------------------------------------------------------- /app_MTAP/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | app_MTAP 4 | 0.0.0 5 | The application package for Memory-based Trajectory and Action Prediction (MTAP) 6 | 7 | 8 | 9 | 10 | Alejandro Olivas 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /app_MTAP/paths/Town01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /app_MTAP/paths/Town02.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /app_MTAP/paths/Town03.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /app_MTAP/paths/Town04.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /app_MTAP/paths/Town05.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /app_MTAP/scripts/__pycache__/config.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AUROVA-LAB/applications/3eecfd8c77049629d0ac54d25a33a1790120061c/app_MTAP/scripts/__pycache__/config.cpython-38.pyc -------------------------------------------------------------------------------- /app_MTAP/scripts/__pycache__/utils.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AUROVA-LAB/applications/3eecfd8c77049629d0ac54d25a33a1790120061c/app_MTAP/scripts/__pycache__/utils.cpython-38.pyc -------------------------------------------------------------------------------- /app_MTAP/scripts/change_weather.py: -------------------------------------------------------------------------------- 1 | import carla 2 | import argparse 3 | from utils import * 4 | 5 | class ChangeWeather(object): 6 | def __init__(self, carla_world): 7 | self.world = carla_world 8 | self._weather_presets = find_weather_presets() 9 | self._weather_index = 0 10 | 11 | 12 | def next(self, reverse=False): 13 | self._weather_index += -1 if reverse else 1 14 | self._weather_index %= len(self._weather_presets) 15 | preset = self._weather_presets[self._weather_index] 16 | self.world.set_weather(preset[0]) 17 | 18 | def set(self, id): 19 | if id < 0 or id >= len(self._weather_presets): 20 | print("Invalid weather id.") 21 | return 22 | self._weather_index = id 23 | preset = self._weather_presets[self._weather_index] 24 | self.world.set_weather(preset[0]) 25 | 26 | def main(): 27 | argparser = argparse.ArgumentParser( 28 | description='CARLA Manual Control Client') 29 | argparser.add_argument( 30 | '--host', 31 | metavar='H', 32 | default='127.0.0.1', 33 | help='IP of the host server (default: 127.0.0.1)') 34 | argparser.add_argument( 35 | '-p', '--port', 36 | metavar='P', 37 | default=2000, 38 | type=int, 39 | help='TCP port to listen to (default: 2000)') 40 | argparser.add_argument( 41 | '-t', '--timeout', 42 | metavar='P', 43 | default=500, 44 | help='timeout waiting response from neural network server') 45 | argparser.add_argument( 46 | '--weather', 47 | metavar='P', 48 | default=None, 49 | help='Weather id. If not set, the program is interactive.') 50 | 51 | args = argparser.parse_args() 52 | try: 53 | client = carla.Client(args.host, args.port) 54 | client.set_timeout(2000.0) 55 | 56 | sim_world = client.get_world() 57 | sim_world.wait_for_tick() 58 | weather = ChangeWeather(sim_world) 59 | previous_command="exit" 60 | if args.weather is not None: 61 | try: 62 | id=int(args.weather) 63 | weather.set(id) 64 | except ValueError: 65 | print("Incorrect weather id. Exiting...") 66 | return 67 | else: 68 | print("USAGE:\nn\t: Change to next weather.\np\t: Change to previous weather.\n"+ 69 | "id\t: Change to the weather associated to the id (last id is {}).\n\n".format(len(weather._weather_presets)-1)+ 70 | "Empty input to repeat previous command. Otherwise to close the program.") 71 | command = input("Enter command ") 72 | while True: 73 | if command=="": command=previous_command 74 | previous_command=command 75 | try: 76 | id = int(command) 77 | weather.set(id) 78 | except ValueError: 79 | if command=="n": 80 | weather.next() 81 | elif command=="p": 82 | weather.next(True) 83 | else: 84 | return 85 | command = input("Enter command (Current weather {}) ".format(weather._weather_index)) 86 | 87 | except KeyboardInterrupt: 88 | print('\nCancelled by user. Bye!') 89 | 90 | 91 | 92 | if __name__ == '__main__': 93 | 94 | main() -------------------------------------------------------------------------------- /app_MTAP/scripts/config.py: -------------------------------------------------------------------------------- 1 | import os 2 | #Config file for variables used by multiple files 3 | 4 | PATH_DATASET = os.getenv("PATH_DATASET") 5 | if PATH_DATASET is None: PATH_DATASET = "/home/alolivas/aurova-lab/labrobotica/dataset/CARLA_dataset/Town05_experiment2_session" 6 | ROUTE_FILENAME = os.getenv("CARLA_MAP") 7 | if ROUTE_FILENAME is None: ROUTE_FILENAME = "./routes/Town01.xml" 8 | PEDESTRIANS_ROUTES_FILE = os.getenv("PEDESTRIAN_ROUTES") 9 | if PEDESTRIANS_ROUTES_FILE is None: PEDESTRIANS_ROUTES_FILE="routes/pedestrian_routes/Town01_evaluation5.xml" 10 | WAY_ID = os.getenv("CARLA_WAY") 11 | if WAY_ID is None: WAY_ID = 14 12 | else: WAY_ID = int(WAY_ID) 13 | PATH_RESULTS = os.getenv("CARLA_RESULTS_PATH") 14 | if PATH_RESULTS is None: PATH_RESULTS ="/media/alolivas/MSI_500/aurova_carla/carla/PythonAPI/aurova/results_metrics/" 15 | 16 | PENALTY_COLLISION_SLIGHT=0.9 17 | PENALTY_COLLISION_STATIC=0.65 18 | PENALTY_COLLISION_PEDESTRIAN=0.5 19 | PENALTY_OFF_ROAD=0.7 #In this case the penalty is on "cars" road driving, except pedestrian crossing. Therefore, we penalize every crossed road line. 20 | TIME_DISTANCE_COEFFICIENT = 3 #Coefficient for determining the max trajectory time 21 | MAX_TIME_LOCAL_MINIMUM = 30 #Seconds 22 | MINIMUM_DISTANCE = 3.0 23 | DISTANCE2PEDESTRIAN = 1.0 #Distance to the pedestrian in the direction of movement to consider that it has been avoided. 24 | 25 | 26 | #Ackermann control configuration 27 | MAX_SPEED=1.3 # m/s 28 | MAX_STEERING_ANGLE = 24 # degrees 29 | KP = 2.0 30 | KI = 0.5 31 | KD = 0.5 32 | 33 | DEG2RAD = 3.1415927/180.0 34 | FPS=10 -------------------------------------------------------------------------------- /app_MTAP/scripts/map2OpenStreet.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | """Example script to generate traffic in the simulation""" 7 | 8 | import glob 9 | import os 10 | import sys 11 | import time 12 | 13 | try: 14 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 15 | sys.version_info.major, 16 | sys.version_info.minor, 17 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 18 | except IndexError: 19 | pass 20 | 21 | import carla 22 | 23 | from carla import VehicleLightState as vls 24 | 25 | import argparse 26 | import logging 27 | from utils import * 28 | from numpy import random 29 | 30 | def get_actor_blueprints(world, filter, generation): 31 | bps = world.get_blueprint_library().filter(filter) 32 | 33 | if generation.lower() == "all": 34 | return bps 35 | 36 | # If the filter returns only one bp, we assume that this one needed 37 | # and therefore, we ignore the generation 38 | if len(bps) == 1: 39 | return bps 40 | 41 | try: 42 | int_generation = int(generation) 43 | # Check if generation is in available generations 44 | if int_generation in [1, 2]: 45 | bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] 46 | return bps 47 | else: 48 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 49 | return [] 50 | except: 51 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 52 | return [] 53 | 54 | def main(): 55 | argparser = argparse.ArgumentParser( 56 | description=__doc__) 57 | argparser.add_argument( 58 | '--host', 59 | metavar='H', 60 | default='127.0.0.1', 61 | help='IP of the host server (default: 127.0.0.1)') 62 | argparser.add_argument( 63 | '-p', '--port', 64 | metavar='P', 65 | default=2000, 66 | type=int, 67 | help='TCP port to listen to (default: 2000)') 68 | 69 | argparser.add_argument( 70 | '-m', '--map', 71 | metavar='W', 72 | default="routes/Town04.xml", 73 | type=str) 74 | argparser.add_argument( 75 | '--tm-port', 76 | metavar='P', 77 | default=8000, 78 | type=int, 79 | help='Port to communicate with TM (default: 8000)') 80 | 81 | args = argparser.parse_args() 82 | 83 | logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) 84 | 85 | client = carla.Client(args.host, args.port) 86 | client.set_timeout(10.0) 87 | synchronous_master = False 88 | 89 | world = client.get_world() 90 | 91 | settings = world.get_settings() 92 | map,_ = ReadRouteFromXML(args.map) 93 | output = open(args.map.split(".")[0]+".osm","w") 94 | carla_map = world.get_map() 95 | 96 | output.write("\n"+ 97 | "\n"+ 99 | " \n\n") 100 | for id, node in map["nodes"].items(): 101 | location=carla.Location(node[0],node[1],node[2]) 102 | geolocation=carla_map.transform_to_geolocation(location) 103 | output.write(" \n".format(id,geolocation.latitude,geolocation.longitude)) 104 | output.write("\n \n") 105 | for node in map["way"]: 106 | output.write(" \n".format(node)) 107 | output.write(" \n\n") 108 | output.close() 109 | 110 | if __name__ == '__main__': 111 | 112 | try: 113 | main() 114 | except KeyboardInterrupt: 115 | pass 116 | finally: 117 | print('\ndone.') 118 | -------------------------------------------------------------------------------- /app_MTAP/scripts/metrics_average.py: -------------------------------------------------------------------------------- 1 | import json 2 | import os 3 | import sys 4 | import numpy as np 5 | 6 | def calculate_folder_average(folder_path, prefix): 7 | 8 | sum_values = {"route_completion": 0.0,"infraction_penalty": 0.0,"driving_score": 0.0, "driving_time":0} 9 | sum_per_kilometer_values = {"pedestrian": 0,"static": 0,"slight_collision": 0,"safety_stops": 0} 10 | sum_weighted_values = {"pedestrian_metric_mean": 0.0,"robot_traj_metric": 0.0} 11 | total_length=0 12 | average = {"pedestrian": 0,"static": 0,"slight_collision": 0,"safety_stops": 0,"route_completion": 0.0,"infraction_penalty": 0.0, 13 | "driving_score": 0.0,"pedestrian_metric_mean": 0.0,"robot_traj_metric": 0.0, "driving_time":0} 14 | #The metric pedestrian_metric_min is not averaged, we save the minimum value 15 | # Get the list of files in the folder 16 | files = [file for file in os.listdir(folder_path) if (file.startswith(prefix))] 17 | count=0 18 | for file in files: 19 | file_path = os.path.join(folder_path, file) 20 | # Read the JSON file 21 | with open(file_path, 'r') as f: 22 | data = json.load(f) 23 | count+=1 24 | route_length = data["route_lenght"]*data["route_completion"] 25 | total_length+=route_length 26 | 27 | for key in sum_values.keys(): 28 | value = data[key] 29 | sum_values[key] += value 30 | 31 | for key in sum_per_kilometer_values.keys(): 32 | value = data[key] 33 | sum_per_kilometer_values[key] += value 34 | 35 | for key in sum_weighted_values.keys(): 36 | value = data[key] 37 | sum_weighted_values[key] += value*route_length 38 | 39 | 40 | print("Total lenght ",total_length) 41 | # Calculate the average 42 | if count > 0: 43 | for key in sum_values.keys(): 44 | average[key]=sum_values[key]/count 45 | for key in sum_per_kilometer_values.keys(): 46 | average[key] = sum_per_kilometer_values[key]/(total_length/1000.0) #Calculate collisions per km 47 | for key in sum_weighted_values.keys(): 48 | average[key] = sum_weighted_values[key]/total_length 49 | return average 50 | else: 51 | return None 52 | 53 | # Check if command-line arguments are provided 54 | if len(sys.argv) != 2: 55 | print("Usage: python script.py folder/prefix") 56 | sys.exit(1) 57 | 58 | # Parse command-line arguments 59 | folder, prefix = sys.argv[1].split("/") 60 | 61 | # Calculate and display the average 62 | average_result = calculate_folder_average(folder, prefix) 63 | 64 | if average_result is not None: 65 | if not os.path.exists(folder+"/average"): 66 | os.mkdir(folder+"/average") 67 | file_path = os.path.join(folder,"average", prefix+"_average.json") 68 | file = open(file_path,"w") 69 | json.dump(average_result,file,indent="\t") 70 | file.close() 71 | print(f"The average of values in the folder is: {average_result}") 72 | 73 | else: 74 | print(f"No files with the specified folder and prefix were found.") -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/Town01.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/Town02.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/Town03.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/Town04.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/Town05.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_evaluation2.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_evaluation3.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_evaluation3_full.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_evaluation4.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_evaluation4_full.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_evaluation5.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_evaluation5_full.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_experiment2.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_experiment2_full.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_experiment4.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town01_experiment4_full.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town03_evaluation1.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town03_evaluation1_full.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town03_experiment1.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /app_MTAP/scripts/routes/pedestrian_routes/Town03_experiment1_full.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /app_force_estimation/README.md: -------------------------------------------------------------------------------- 1 | # Grasping Force Estimation in Markerless Visuo-Tactile Sensors 2 | 3 | ## Overview 4 | This repository presents the code of our last work [Grasping Force Estimation in Markerless Visuo-Tactile Sensors](https://aurova-projects.github.io/force_tactile/), which has been implemented with ROS Noetic in Ubuntu 20.04. 5 | 6 | ## Installation instructions 7 | 8 | 1. **Create a catkin workspace**: 9 | ``` 10 | mkdir -p digit_ros/src 11 | cd digit_ros 12 | ``` 13 | 14 | 2. **Add ROS packages**: 15 | - Clone the aurova_tactile_sensing repository: 16 | - `git clone https://github.com/AUROVA-LAB/aurova_tactile_sensing` 17 | - Add the [digit_interface](https://github.com/AUROVA-LAB/aurova_tactile_sensing/tree/main/digit_interface) package to digit_ros/src. 18 | - Add the [force_estimation](https://github.com/AUROVA-LAB/aurova_tactile_sensing/tree/main/force_estimation) package to digit_ros/src. 19 | 20 | 3. **Set up conda environment**: 21 | ``` 22 | conda create --name==digit_ros_torch python=3.8.10 23 | conda activate digit_ros_torch 24 | pip install rospkg empy 25 | pip install numpy 26 | pip install opencv-python 27 | pip install digit-interface 28 | conda install pytorch==1.12.0 torchvision==0.13.0 torchaudio==0.12.0 cudatoolkit=11.3 -c pytorch 29 | pip install tqdm 30 | pip install wandb 31 | ``` 32 | 4. **Compile catkin workspace with conda environment activated**: 33 | ``` 34 | cd aurova_tactile_sensing/digit_ros/ 35 | conda activate digit_ros_torch 36 | catkin_make 37 | source devel/setup.bash 38 | ``` 39 | 40 | ## Usage 41 | 42 | 1. **Run roslaunch to get images from DIGIT sensors**: 43 | 44 | `roslaunch digit_interface digit.launch` 45 | 46 | 2. **Run python script to estimate forces from tactile images**: 47 | 48 | ``` 49 | cd digit_ros/src/scripts/ 50 | python estimate_force.py 51 | ``` 52 | 53 | 54 | ## Citation 55 | If you find our code or papers useful, please cite: 56 | `TODO` 57 | -------------------------------------------------------------------------------- /app_geo_localization/README.md: -------------------------------------------------------------------------------- 1 | # Geo-Localization 2 | This repository presents the code for replicating the whole pipeline presented in the works: [Geo-Localization Based on Dynamically Weighted Factor-Graph](https://aurova-projects.github.io/geo-localization_weighted/) + [Robust Self-Tuning Data Association for Geo-Referencing Using Lane Markings](https://aurova-projects.github.io/geo-localization_self-tunning/) 3 | 4 | **Note:** A version of the pipeline, including path planning, is [here](https://github.com/AUROVA-LAB/applications/tree/main/app_geo_navigation) 5 | 6 | 7 | ![pipeline](pipeline_weight.png) 8 | 9 | Citations: 10 | ``` 11 | @article{munoz2024geolocalization, 12 | title={Geo-Localization Based on Dynamically Weighted Factor-Graph}, 13 | author={Muñoz-Bañón, Miguel Ángel and Olivas, Alejandro and Velasco-Sánchez, Edison and Candelas, Francisco A. and Torres, Fernando}, 14 | journal={IEEE Robotics and Automation Letters}, 15 | volume={9}, 16 | number={6}, 17 | pages={5599--5606}, 18 | year={2024}, 19 | doi={10.1109/LRA.2024.3396055}, 20 | publisher={IEEE} 21 | } 22 | 23 | @article{munoz2022robust, 24 | title={Robust Self-Tuning Data Association for Geo-Referencing Using Lane Markings}, 25 | author={Muñoz-Bañón, Miguel Ángel and Pauls, Jan-Hendrik and Hu, Haohao and Stiller, Christoph and Candelas, Francisco A. and Torres, Fernando}, 26 | journal={IEEE Robotics and Automation Letters}, 27 | volume={7}, 28 | number={4}, 29 | pages={12339--12346}, 30 | year={2022}, 31 | doi={10.1109/LRA.2022.3216991}, 32 | publisher={IEEE} 33 | } 34 | ``` 35 | 36 | ## Installation instructions to replicate 37 | 38 | ### Requirements to use application: 39 | 40 | - System requirements: Ubuntu 20.04 and ROS Noetic. 41 | - External libraries: [ceres-solver-2.0.0](http://ceres-solver.org/installation.html) (IMPORTANT!! [download](https://drive.google.com/file/d/1acZtn_jaHfj2BVgwaDnQH2Lz-7022F1-/view?usp=share_link) version 2.0.0). Eigen and PCL are usually installed join with ROS. 42 | - Local libraries: [lib_localization](https://github.com/AUROVA-LAB/lib_localization). 43 | - External ROS packages: [iri_base_algorithm](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_base_algorithm), "sudo apt-get install ros-noetic-ackermann-\*", "sudo apt-get install ros-noetic-robot-state-\*", "sudo apt-get install ros-noetic-hector-\*". 44 | - Local ROS packages: [robot_blue](https://github.com/AUROVA-LAB/robot_blue), [aurova_preprocessed](https://github.com/AUROVA-LAB/aurova_preprocessed), [aurova_odom](https://github.com/AUROVA-LAB/aurova_odom), [aurova_detections](https://github.com/AUROVA-LAB/aurova_detections), and [aurova_localization](https://github.com/AUROVA-LAB/aurova_localization). 45 | 46 | ### Steps to use application (offline mode via rosbag): 47 | 48 | - [Download](https://drive.google.com/file/d/1oW7MLIJhvlNtgJsetXNRY-BQxufgPUoJ/view?usp=sharing) bag file for this example. 49 | - Modify "launch/nav_GeoLoc_offline.launch" to provide correct link in "bag_file_1" argument. 50 | - Modify "params/nav_GeoLoc_offline.yaml" to provide correct link in "url_to_map" variable. 51 | - Run next command for localization (Odometry + GPS): 52 | 53 | ```shell 54 | roslaunch app_geo_localization nav_GeoLoc_offline.launch 55 | ``` 56 | - Follow the instructions [here](https://github.com/AUROVA-LAB/aurova_detections/tree/main/yolinov2_ros) (in the example section) to run the docker for ground boundaries detection (whole localization pipeline). 57 | 58 | ### Steps to use application (online mode via BLUE robot): 59 | 60 | - Modify "params/nav_GeoLoc_online.yaml" to provide correct link in "url_to_map" variable. 61 | - Run next command for localization: 62 | 63 | ```shell 64 | roslaunch app_geo_localization nav_GeoLoc_online.launch 65 | ``` 66 | - Follow the instructions [here](https://github.com/AUROVA-LAB/aurova_detections/tree/main/yolinov2_ros) (in the example section) to run the docker for ground boundaries detection (whole localization pipeline). 67 | - Drive!! 68 | -------------------------------------------------------------------------------- /app_geo_localization/launch/nav_GeoLoc_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /app_geo_localization/launch/nav_GeoLoc_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /app_geo_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | app_geo_localization 4 | 0.0.0 5 | The geo-localization applications package 6 | 7 | 8 | 9 | 10 | mice85 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /app_geo_localization/params/nav_GeoLoc_offline.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | use_sim_time: true 3 | 4 | geo_localization: { 5 | 6 | # Geo-localization local parameters 7 | lat_zero: 38.38164422311365, 8 | lon_zero: -0.5255029920793286, 9 | offset_map_x: 0.0, 10 | offset_map_y: 0.0, 11 | world_id: "utm", 12 | map_id: "map", 13 | odom_id: "odom", 14 | base_id: "gps", 15 | lidar_id: "os_sensor", 16 | margin_asso_constraints: 20, 17 | margin_gnss_constraints: 12, 18 | margin_gnss_distance: 1.0, 19 | 20 | # Data management library parameters 21 | sample_distance: 0.5, 22 | threshold_asso: 1.0, 23 | voxel_asso: 1.5, 24 | radious_dt: 15.0, 25 | radious_lm: 22.5, 26 | url_to_map: "/home/aurova/aurova-lab/aurova_ws/src/applications/app_geo_localization/maps/scientific_park_01.osm", 27 | # url_to_map: "/home/blue-onboard/aurova-lab/aurova_ws/src/applications/app_geo_localization/maps/scientific_park_01.osm", 28 | acum_tf_da: 10, 29 | acum_tf_varfactor: 2.0, 30 | z_weight: 1.0, 31 | 32 | # Optimization process library parameters 33 | window_size: 30, 34 | max_num_iterations_op: 100, 35 | 36 | # Sigmoid options for weights 37 | type: 1, # 1. phi = x - lambda // 2. phi = (x/k)m - m/2 38 | lambda: 10.0, 39 | k: 50.0, 40 | m: 12.0, 41 | 42 | odom_preweight: 60.0, 43 | asso_preweight: -1, # if w<0 then dynamically adjusted. 44 | 45 | # Save data for evaluation 46 | out_data: "/home/aurova/aurova-lab/aurova_ws/src/applications/app_geo_localization/data/trajectory/C2/method_E/", 47 | out_map: "/home/aurova/aurova-lab/aurova_ws/src/applications/app_geo_localization/data/map/", 48 | save_map: false, 49 | save_data: false, 50 | 51 | # Ground truth configuration. 52 | ground_truth: false, 53 | out_gt: "/home/aurova/aurova-lab/aurova_ws/src/applications/app_geo_localization/data/ground_truth/C4/", 54 | #gt_last_frame: 3465, 55 | #gt_key_frames: [340, 600, 930, 1187, 2055, 2280, 2647, 2915, 2077, 3300] # C1 56 | #gt_last_frame: 4134, 57 | #gt_key_frames: [115, 150, 960, 1440, 1620, 2042, 2171, 2492, 2992, 3305] # C2 58 | #gt_last_frame: 4180, 59 | #gt_key_frames: [275, 540, 816, 1155, 1410, 2170, 2430, 2590, 3110, 3954] # C3 60 | gt_last_frame: 3886, 61 | gt_key_frames: [310, 810, 1140, 1555, 1790, 1870, 1977, 2610, 2880, 3420] # C4 62 | } 63 | 64 | #-variables for gps_to_odom node 65 | gps_to_odom: { 66 | frame_id: "map", 67 | min_speed: 0.4, 68 | max_speed: 1.3 69 | } 70 | 71 | image2pcl: { 72 | range_img: "/ouster/range_image", 73 | mask_img: "/ground_lines_img", 74 | out_pc: "/ouster/points", 75 | out_detections: "/ground_lines_pc", 76 | include_detections: true, 77 | include_pc: true 78 | } 79 | 80 | -------------------------------------------------------------------------------- /app_geo_localization/params/nav_GeoLoc_online.yaml: -------------------------------------------------------------------------------- 1 | 2 | geo_localization: { 3 | 4 | # Geo-localization local parameters 5 | lat_zero: 38.38164422311365, 6 | lon_zero: -0.5255029920793286, 7 | offset_map_x: 0.0, 8 | offset_map_y: 0.0, 9 | world_id: "utm", 10 | map_id: "map", 11 | odom_id: "odom", 12 | base_id: "gps", 13 | lidar_id: "os_sensor", 14 | margin_asso_constraints: 20, 15 | margin_gnss_constraints: 12, 16 | margin_gnss_distance: 1.0, 17 | 18 | # Data management library parameters 19 | sample_distance: 0.5, 20 | threshold_asso: 1.0, 21 | voxel_asso: 1.5, 22 | radious_dt: 15.0, 23 | radious_lm: 22.5, 24 | # url_to_map: "/home/aurova/aurova-lab/aurova_ws/src/applications/app_old/maps/scientific_park_01.osm", 25 | url_to_map: "/home/blue-onboard/aurova-lab/aurova_ws/src/applications/app_old/maps/scientific_park_01.osm", 26 | acum_tf_da: 10, 27 | acum_tf_varfactor: 2.0, 28 | z_weight: 1.0, 29 | 30 | # Optimization process library parameters 31 | window_size: 30, 32 | max_num_iterations_op: 100, 33 | 34 | # Sigmoid options for weights 35 | type: 1, # 1. phi = x - lambda // 2. phi = (x/k)m - m/2 36 | lambda: 10.0, 37 | k: 50.0, 38 | m: 12.0, 39 | 40 | odom_preweight: 60.0, 41 | asso_preweight: -1, # if w<0 then dynamically adjusted. 42 | 43 | # Save data for evaluation 44 | out_data: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/trajectory/C2/method_E/", 45 | out_map: "/home/mice85/aurova-lab/aurova_ws/src/applications/data/map/", 46 | save_map: false, 47 | save_data: false, 48 | 49 | # Ground truth configuration. 50 | ground_truth: false, 51 | out_gt: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/ground_truth/C4/", 52 | #gt_last_frame: 3465, 53 | #gt_key_frames: [340, 600, 930, 1187, 2055, 2280, 2647, 2915, 2077, 3300] # C1 54 | #gt_last_frame: 4134, 55 | #gt_key_frames: [115, 150, 960, 1440, 1620, 2042, 2171, 2492, 2992, 3305] # C2 56 | #gt_last_frame: 4180, 57 | #gt_key_frames: [275, 540, 816, 1155, 1410, 2170, 2430, 2590, 3110, 3954] # C3 58 | gt_last_frame: 3886, 59 | gt_key_frames: [310, 810, 1140, 1555, 1790, 1870, 1977, 2610, 2880, 3420] # C4 60 | } 61 | 62 | #-variables for gps_to_odom node 63 | gps_to_odom: { 64 | frame_id: "map", 65 | min_speed: 0.4, 66 | max_speed: 1.3 67 | } 68 | 69 | image2pcl: { 70 | range_img: "/ouster/range_image", 71 | mask_img: "/ground_lines_img", 72 | out_pc: "/ouster/points", 73 | out_detections: "/ground_lines_pc", 74 | include_detections: true, 75 | include_pc: false 76 | } -------------------------------------------------------------------------------- /app_geo_localization/pipeline_weight.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AUROVA-LAB/applications/3eecfd8c77049629d0ac54d25a33a1790120061c/app_geo_localization/pipeline_weight.png -------------------------------------------------------------------------------- /app_geo_navigation/launch/nav_NVP_GeoLoc_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /app_geo_navigation/launch/nav_NVP_GeoLoc_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /app_geo_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | app_geo_navigation 4 | 0.0.0 5 | The geo-navigation applications package 6 | 7 | 8 | 9 | 10 | mice85 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /app_geo_navigation/paths/campus_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /app_geo_navigation/paths/iuii_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /app_geo_navigation/paths/parque_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /app_geo_navigation/paths/patos_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /app_geo_navigation/paths/scientific_park_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /app_geo_navigation/paths/scientific_park_04.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /app_geo_navigation/paths/urban_01_nl_2.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /app_geo_navigation/pipeline_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AUROVA-LAB/applications/3eecfd8c77049629d0ac54d25a33a1790120061c/app_geo_navigation/pipeline_navigation.png -------------------------------------------------------------------------------- /app_geo_navigation/script/change_close_loop.sh: -------------------------------------------------------------------------------- 1 | closed_loop=( 2 | "0 1 2 3 4 5 1 0 3 2 1 5 4" # scientific_park_04.osm 3 | "6 0 3 7 8 4 0 6 7 3 4 8" # scientific_park_04.osm 4 | "9 6 7 10 11 5 6 9 10 7 8 11" # scientific_park_04.osm 5 | "12 9 10 13 14 11 9 12 13 10 11 14" # scientific_park_04.osm 6 | ) 7 | 8 | if [ "$#" -ne 1 ]; then 9 | echo "Usage: $0 " 10 | exit 1 11 | fi 12 | 13 | loop_number=$1 14 | 15 | if ! [[ "$loop_number" =~ ^[0-9]+$ ]]; then 16 | echo "Error: Argument is not an integer." 17 | exit 1 18 | fi 19 | 20 | if [ "$loop_number" -lt 1 ] || [ "$loop_number" -gt ${#closed_loop[@]} ]; then 21 | echo "Error: Argument must be between 1 and ${#closed_loop[@]}." 22 | exit 1 23 | fi 24 | 25 | selected_loop=${closed_loop[$((loop_number-1))]} 26 | 27 | rosparam set /global_planning/closed_loop "$selected_loop" 28 | -------------------------------------------------------------------------------- /app_old/launch/loc_GeoLoc_kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /app_old/launch/loc_GeoLoc_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /app_old/launch/loc_GeoLoc_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /app_old/launch/nav_NVP_GN_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /app_old/launch/nav_NVP_GN_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /app_old/launch/nav_NVP_GN_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /app_old/launch/nav_NVP_GeoLoc_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /app_old/launch/nav_NVP_GeoLoc_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /app_old/launch/nav_NVP_GeoLoc_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /app_old/launch/nav_follow_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /app_old/launch/nav_follow_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /app_old/launch/odom_LO-FRI_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /app_old/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | app_old 4 | 0.0.0 5 | The old applications package 6 | 7 | 8 | 9 | 10 | mice85 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /app_old/params/loc_GeoLoc_kitti.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | use_sim_time: true 3 | 4 | geo_localization: { 5 | 6 | # Geo-localization local parameters 7 | lat_zero: 48.97253396005, # Sec. 10 8 | lon_zero: 8.4785980847297, 9 | offset_map_x: 2.0, 10 | offset_map_y: 0.7, 11 | world_id: "utm", 12 | map_id: "map", 13 | odom_id: "odom", 14 | base_id: "imu_link", 15 | lidar_id: "velo_link", 16 | margin_asso_constraints: 20, 17 | margin_gnss_constraints: 12, 18 | margin_gnss_distance: 1.0, 19 | 20 | # Data management library parameters 21 | sample_distance: 0.5, 22 | threshold_asso: 1.0, 23 | voxel_asso: 1.5, 24 | radious_dt: 30.0, 25 | radious_lm: 30.0, 26 | url_to_map: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/maps/kitti_seq10.osm", 27 | acum_tf_da: 10, 28 | acum_tf_varfactor: 2.0, 29 | z_weight: 1.0, 30 | 31 | # Optimization process library parameters 32 | window_size: 30, 33 | max_num_iterations_op: 100, 34 | 35 | # Sigmoid options for weights 36 | type: 1, # 1. phi = x - lambda // 2. phi = (x/k)m - m/2 37 | lambda: 10.0, 38 | k: 50.0, 39 | m: 12.0, 40 | 41 | odom_preweight: 60.0, 42 | asso_preweight: -1, # if w<0 then dynamically adjusted. 43 | 44 | # Save data for evaluation 45 | out_data: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/trajectory/C2/method_E/", 46 | out_map: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/map/", 47 | save_map: false, 48 | save_data: false, 49 | 50 | # Ground truth configuration. 51 | ground_truth: false, 52 | out_gt: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/ground_truth/C4/", 53 | #gt_last_frame: 3465, 54 | #gt_key_frames: [340, 600, 930, 1187, 2055, 2280, 2647, 2915, 2077, 3300] # C1 55 | #gt_last_frame: 4134, 56 | #gt_key_frames: [115, 150, 960, 1440, 1620, 2042, 2171, 2492, 2992, 3305] # C2 57 | #gt_last_frame: 4180, 58 | #gt_key_frames: [275, 540, 816, 1155, 1410, 2170, 2430, 2590, 3110, 3954] # C3 59 | gt_last_frame: 3886, 60 | gt_key_frames: [310, 810, 1140, 1555, 1790, 1870, 1977, 2610, 2880, 3420] # C4 61 | } 62 | 63 | #-variables for gps_to_odom node 64 | gps_to_odom: { 65 | frame_id: "map", 66 | min_speed: 0.4, 67 | max_speed: 1.3 68 | } 69 | 70 | image2pcl: { 71 | range_img: "/ouster/range_image", 72 | mask_img: "/ground_lines_img", 73 | out_pc: "/ouster/points", 74 | out_detections: "/ground_lines_pc", 75 | include_detections: false, 76 | include_pc: false 77 | } -------------------------------------------------------------------------------- /app_old/params/loc_GeoLoc_offline.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | use_sim_time: true 3 | 4 | geo_localization: { 5 | 6 | # Geo-localization local parameters 7 | lat_zero: 38.38164422311365, 8 | lon_zero: -0.5255029920793286, 9 | offset_map_x: 0.0, 10 | offset_map_y: 0.0, 11 | world_id: "utm", 12 | map_id: "map", 13 | odom_id: "odom", 14 | base_id: "gps", 15 | lidar_id: "os_sensor", 16 | margin_asso_constraints: 20, 17 | margin_gnss_constraints: 12, 18 | margin_gnss_distance: 1.0, 19 | 20 | # Data management library parameters 21 | sample_distance: 0.5, 22 | threshold_asso: 1.0, 23 | voxel_asso: 1.5, 24 | radious_dt: 15.0, 25 | radious_lm: 22.5, 26 | url_to_map: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/maps/scientific_park_01.osm", 27 | acum_tf_da: 10, 28 | acum_tf_varfactor: 2.0, 29 | z_weight: 1.0, 30 | 31 | # Optimization process library parameters 32 | window_size: 30, 33 | max_num_iterations_op: 100, 34 | 35 | # Sigmoid options for weights 36 | type: 1, # 1. phi = x - lambda // 2. phi = (x/k)m - m/2 37 | lambda: 10.0, 38 | k: 50.0, 39 | m: 12.0, 40 | 41 | odom_preweight: 60.0, 42 | asso_preweight: -1, # if w<0 then dynamically adjusted. 43 | 44 | # Save data for evaluation 45 | out_data: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/trajectory/C2/method_E/", 46 | out_map: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/map/", 47 | save_map: false, 48 | save_data: false, 49 | 50 | # Ground truth configuration. 51 | ground_truth: false, 52 | out_gt: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/ground_truth/C4/", 53 | #gt_last_frame: 3465, 54 | #gt_key_frames: [340, 600, 930, 1187, 2055, 2280, 2647, 2915, 2077, 3300] # C1 55 | #gt_last_frame: 4134, 56 | #gt_key_frames: [115, 150, 960, 1440, 1620, 2042, 2171, 2492, 2992, 3305] # C2 57 | #gt_last_frame: 4180, 58 | #gt_key_frames: [275, 540, 816, 1155, 1410, 2170, 2430, 2590, 3110, 3954] # C3 59 | gt_last_frame: 3886, 60 | gt_key_frames: [310, 810, 1140, 1555, 1790, 1870, 1977, 2610, 2880, 3420] # C4 61 | } 62 | 63 | #-variables for gps_to_odom node 64 | gps_to_odom: { 65 | frame_id: "map", 66 | min_speed: 0.4, 67 | max_speed: 1.3 68 | } 69 | 70 | image2pcl: { 71 | range_img: "/ouster/range_image", 72 | mask_img: "/ground_lines_img", 73 | out_pc: "/ouster/points", 74 | out_detections: "/ground_lines_pc", 75 | include_detections: true, 76 | include_pc: true 77 | } -------------------------------------------------------------------------------- /app_old/params/loc_GeoLoc_online.yaml: -------------------------------------------------------------------------------- 1 | 2 | geo_localization: { 3 | 4 | # Geo-localization local parameters 5 | lat_zero: 38.38164422311365, 6 | lon_zero: -0.5255029920793286, 7 | offset_map_x: 0.0, 8 | offset_map_y: 0.0, 9 | world_id: "utm", 10 | map_id: "map", 11 | odom_id: "odom", 12 | base_id: "gps", 13 | lidar_id: "os_sensor", 14 | margin_asso_constraints: 20, 15 | margin_gnss_constraints: 12, 16 | margin_gnss_distance: 1.0, 17 | 18 | # Data management library parameters 19 | sample_distance: 0.5, 20 | threshold_asso: 1.0, 21 | voxel_asso: 1.5, 22 | radious_dt: 15.0, 23 | radious_lm: 22.5, 24 | url_to_map: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/maps/scientific_park_01.osm", 25 | acum_tf_da: 10, 26 | acum_tf_varfactor: 2.0, 27 | z_weight: 1.0, 28 | 29 | # Optimization process library parameters 30 | window_size: 30, 31 | max_num_iterations_op: 100, 32 | 33 | # Sigmoid options for weights 34 | type: 1, # 1. phi = x - lambda // 2. phi = (x/k)m - m/2 35 | lambda: 10.0, 36 | k: 50.0, 37 | m: 12.0, 38 | 39 | odom_preweight: 60.0, 40 | asso_preweight: -1, # if w<0 then dynamically adjusted. 41 | 42 | # Save data for evaluation 43 | out_data: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/trajectory/C2/method_E/", 44 | out_map: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/map/", 45 | save_map: false, 46 | save_data: false, 47 | 48 | # Ground truth configuration. 49 | ground_truth: false, 50 | out_gt: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/data/ground_truth/C4/", 51 | #gt_last_frame: 3465, 52 | #gt_key_frames: [340, 600, 930, 1187, 2055, 2280, 2647, 2915, 2077, 3300] # C1 53 | #gt_last_frame: 4134, 54 | #gt_key_frames: [115, 150, 960, 1440, 1620, 2042, 2171, 2492, 2992, 3305] # C2 55 | #gt_last_frame: 4180, 56 | #gt_key_frames: [275, 540, 816, 1155, 1410, 2170, 2430, 2590, 3110, 3954] # C3 57 | gt_last_frame: 3886, 58 | gt_key_frames: [310, 810, 1140, 1555, 1790, 1870, 1977, 2610, 2880, 3420] # C4 59 | } 60 | 61 | #-variables for gps_to_odom node 62 | gps_to_odom: { 63 | frame_id: "map", 64 | min_speed: 0.4, 65 | max_speed: 1.3 66 | } 67 | 68 | image2pcl: { 69 | range_img: "/ouster/range_image", 70 | mask_img: "/ground_lines_img", 71 | out_pc: "/ouster/points", 72 | out_detections: "/ground_lines_pc", 73 | include_detections: true, 74 | include_pc: false 75 | } 76 | -------------------------------------------------------------------------------- /app_old/params/nav_NVP_GN_gazebo.yaml: -------------------------------------------------------------------------------- 1 | 2 | #-variables for gps_to_odom node 3 | gps_to_odom: { 4 | frame_id: "map", 5 | min_speed: 0.4, 6 | max_speed: 1.3 7 | } 8 | 9 | #-variables for gps_odom_optimization node 10 | gps_odom_optimization: { 11 | x_model: 0.05, 12 | y_model: 0.05, 13 | theta_model: 0.01, 14 | } 15 | 16 | #-variables for global_planning node 17 | global_planning: { 18 | #url_path: "/home/mice85/aurova-lab/aurova_ws/src/application_navigation/paths/parque_01.osm", 19 | #url_path: "/home/aurova/iri-lab/iri_ws/src/application_navigation/paths/parque_01.osm", 20 | #url_path: "/home/mice85/aurova-lab/aurova_ws/src/application_navigation/paths/patos_01.osm", 21 | #url_path: "/home/aurova/iri-lab/iri_ws/src/application_navigation/paths/patos_01.osm", 22 | #url_path: "/home/mice85/aurova-lab/aurova_ws/src/application_navigation/paths/iuii_01.osm", 23 | #url_path: "/home/aurova/iri-lab/iri_ws/src/application_navigation/paths/iuii_01.osm", 24 | #url_path: "/home/mice85/aurova-lab/aurova_ws/src/application_navigation/paths/campus_01.osm", 25 | #url_path: "/home/aurova/iri-lab/iri_ws/src/application_navigation/paths/campus_01.osm", 26 | url_path: "/home/alolivas/aurova-lab/aurova_ws/src/applications/app_old/paths/urban_01_nl_2.osm", 27 | #url_path: "/home/aurova/iri-lab/iri_ws/src/application_navigation/paths/urban_01_nl_2.osm", 28 | 29 | url_file_out: "/home/mice85/aurova-lab/aurova_ws/src/application_navigation/output_files/", 30 | 31 | closed_loop: "1 2 3 4 5 6 16 15 12 11 10 9 8 7 6 16 15 12 13 14 0", 32 | 33 | save_data: false, 34 | frame_id: "map", 35 | var_x: 0.2, 36 | var_y: 0.01, 37 | var_z: 100.0, #0.01, 38 | var_w: 0.1, 39 | type_dist: "E", 40 | rad_reached: 4.0, 41 | operation_mode: 2, # 1) follow path to global goal. 2) follow closed loop 3) bypass global goal 42 | stop_code: 10.0 # in 'z' coordinate 43 | } 44 | local_planning: { 45 | frame_id: "map", 46 | frame_lidar: "velodyne", 47 | save_data: false, 48 | url_file_out: "/home/mice85/aurova-lab/aurova_ws/src/application_navigation/output_files/scans/", 49 | wa: 100.0, 50 | wr: 1.0, 51 | aa: 0.3, 52 | ar: 0.8, 53 | wa2: 3.0 54 | } 55 | lidar_configuration: { 56 | max_elevation_angle: 105.0, 57 | min_elevation_angle: 75.0, 58 | max_azimuth_angle: 360.0, 59 | min_azimuth_angle: 0.0, 60 | grid_azimuth_angular_resolution: 0.82, 61 | grid_elevation_angular_resolution: 2.0 62 | } 63 | filter_configuration: { 64 | max_range: 100.0, 65 | min_range: 0.0, 66 | a: 38.0, 67 | b: -66.0, 68 | c: -1.105, 69 | variance: 0.1, 70 | radious: 8.0, 71 | var_factor: 0.27, 72 | ground_in_sim: -0.5, 73 | is_simulation: true, 74 | is_reconfig: false 75 | } 76 | 77 | #-variables for ackermann_control node 78 | ackermann_control: { 79 | max_angle: 24.0, 80 | delta_angle: 2.0, 81 | v_length: 1.05, 82 | delta_arc: 1.0, 83 | max_arc: 2.0, 84 | v_min: 0.4, 85 | v_max: 0.8, 86 | kp: 0.3, # if 1.0, proportional disactived 87 | margin: 1.0 #from base_link 88 | } -------------------------------------------------------------------------------- /app_old/params/nav_NVP_GN_offline.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | use_sim_time: true 3 | 4 | #-variables for global_planning node 5 | global_planning: { 6 | url_path: "/home/alolivas/aurova-lab/aurova_ws/src/applications/app_old/paths/parque_01.osm", 7 | #url_path: "/home/blue-onboard/aurova-lab/aurova_ws/src/applications/app_old/paths/parque_01.osm", 8 | 9 | url_file_out: "/home/aurova/aurova-lab/aurova_ws/src/application_navigation/output_files/", 10 | 11 | #closed_loop: "0 1 2 3 4 5 6 7 8 7 6 5 4 3 2 1", 12 | closed_loop: "0 1 2 3 4 5 6 28 29 30 31 32 13 25 26 24 27 4 3 2 1", 13 | #closed_loop: "0 1 2 3 4 27 24 23 22 21 15 16 17 18 19 20", 14 | #closed_loop: "4 27 24 14 13 32 12 11 10 9 8 7 6 5", 15 | 16 | save_data: false, 17 | frame_id: "map", 18 | var_x: 0.2, 19 | var_y: 0.01, 20 | var_z: 100.0, #0.01, 21 | var_w: 0.1, 22 | type_dist: "E", 23 | rad_reached: 4.0, 24 | operation_mode: 2, # 1) follow path to global goal. 2) follow closed loop 3) bypass global goal 25 | stop_code: 10.0 # in 'z' coordinate 26 | } 27 | 28 | #-variables for local_planning node 29 | local_planning: { 30 | frame_id: "map", 31 | frame_lidar: "os_sensor", 32 | save_data: false, 33 | url_file_out: "/home/aurova/aurova-lab/aurova_ws/src/application_navigation/output_files/scans/", 34 | external_obstacle_detection: true, 35 | wa: 100.0, 36 | wr: 1.0, 37 | aa: 0.3, 38 | ar: 0.8, 39 | wa2: 3.0 40 | } 41 | lidar_configuration: { 42 | max_elevation_angle: 112.5, 43 | min_elevation_angle: 67.5, 44 | max_azimuth_angle: 360.0, 45 | min_azimuth_angle: 0.0, 46 | grid_azimuth_angular_resolution: 0.18, 47 | grid_elevation_angular_resolution: 0.35 48 | } 49 | filter_configuration: { 50 | max_range: 100.0, 51 | min_range: 0.0, 52 | a: 52.0, 53 | b: 90.0, 54 | c: -1.105, 55 | lidar_protect: 1.0, 56 | variance: 0.1, 57 | radious: 8.0, 58 | var_factor: 0.4, 59 | ground_in_sim: -0.5, 60 | is_simulation: false, 61 | is_reconfig: false 62 | } 63 | ackermann_control: { 64 | max_angle: 24.0, 65 | delta_angle: 2.0, 66 | v_length: 1.05, 67 | delta_arc: 1.0, 68 | max_arc: 2.0, 69 | v_min: 0.4, 70 | v_max: 1.3, 71 | kp: 0.6, # if 1.0, proportional disactived 72 | margin: 1.0, #from base_link 73 | carrot_ctrl: false, 74 | carrot_distance: 4.0 75 | } 76 | 77 | #-variables for gps_to_odom node 78 | gps_to_odom: { 79 | frame_id: "map", 80 | min_speed: 0.4, 81 | max_speed: 1.3 82 | } 83 | -------------------------------------------------------------------------------- /app_old/params/nav_NVP_GN_online.yaml: -------------------------------------------------------------------------------- 1 | 2 | 3 | #-variables for global_planning node 4 | global_planning: { 5 | #url_path: "/home/mice85/aurova-lab/aurova_ws/src/applications/app_old/paths/parque_01.osm", 6 | url_path: "/home/blue-onboard/aurova-lab/aurova_ws/src/applications/app_old/paths/parque_01.osm", 7 | 8 | url_file_out: "/home/aurova/aurova-lab/aurova_ws/src/application_navigation/output_files/", 9 | 10 | #closed_loop: "0 1 2 3 4 5 6 7 8 7 6 5 4 3 2 1", 11 | #closed_loop: "0 1 2 3 4 5 6 28 29 30 31 32 13 25 26 24 27 4 3 2 1", 12 | closed_loop: "0 1 2 3 4 27 24 23 22 21 15 16 17 18 19 20", 13 | #closed_loop: "4 27 24 14 13 32 12 11 10 9 8 7 6 5", 14 | 15 | save_data: false, 16 | frame_id: "map", 17 | var_x: 0.2, 18 | var_y: 0.01, 19 | var_z: 100.0, #0.01, 20 | var_w: 0.1, 21 | type_dist: "E", 22 | rad_reached: 4.0, 23 | operation_mode: 2, # 1) follow path to global goal. 2) follow closed loop 3) bypass global goal 24 | stop_code: 10.0 # in 'z' coordinate 25 | } 26 | 27 | #-variables for local_planning node 28 | local_planning: { 29 | frame_id: "map", 30 | frame_lidar: "os_sensor", 31 | save_data: false, 32 | url_file_out: "/home/aurova/aurova-lab/aurova_ws/src/application_navigation/output_files/scans/", 33 | external_obstacle_detection: true, 34 | wa: 100.0, 35 | wr: 1.0, 36 | aa: 0.3, 37 | ar: 0.8, 38 | wa2: 3.0 39 | } 40 | lidar_configuration: { 41 | max_elevation_angle: 112.5, 42 | min_elevation_angle: 67.5, 43 | max_azimuth_angle: 360.0, 44 | min_azimuth_angle: 0.0, 45 | grid_azimuth_angular_resolution: 0.18, 46 | grid_elevation_angular_resolution: 0.35 47 | } 48 | filter_configuration: { 49 | max_range: 100.0, 50 | min_range: 0.0, 51 | a: 52.0, 52 | b: 90.0, 53 | c: -1.105, 54 | lidar_protect: 1.0, 55 | variance: 0.1, 56 | radious: 8.0, 57 | var_factor: 0.4, 58 | ground_in_sim: -0.5, 59 | is_simulation: false, 60 | is_reconfig: false 61 | } 62 | ackermann_control: { 63 | max_angle: 24.0, 64 | delta_angle: 2.0, 65 | v_length: 1.05, 66 | delta_arc: 1.0, 67 | max_arc: 2.0, 68 | v_min: 0.4, 69 | v_max: 1.3, 70 | kp: 0.6, # if 1.0, proportional disactived 71 | margin: 1.0, #from base_link 72 | carrot_ctrl: false, 73 | carrot_distance: 4.0 74 | } 75 | 76 | #-variables for gps_to_odom node 77 | gps_to_odom: { 78 | frame_id: "map", 79 | min_speed: 0.4, 80 | max_speed: 1.3 81 | } -------------------------------------------------------------------------------- /app_old/params/nav_follow_offline.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | use_sim_time: true 3 | 4 | global_planning/stop_code: 10.0 # in 'z' coordinate 5 | 6 | 7 | #-variables for local_planning node 8 | local_planning: { 9 | frame_id: "os_sensor", 10 | frame_lidar: "os_sensor", 11 | save_data: false, 12 | url_file_out: "/home/alolivas/aurova-lab/aurova_ws/src/application_navigation/output_files/scans/", 13 | external_obstacle_detection: true, 14 | wa: 100.0, 15 | wr: 1.0, 16 | aa: 0.3, 17 | ar: 0.8, 18 | wa2: 3.0 19 | } 20 | lidar_configuration: { 21 | max_elevation_angle: 112.5, 22 | min_elevation_angle: 67.5, 23 | max_azimuth_angle: 360.0, 24 | min_azimuth_angle: 0.0, 25 | grid_azimuth_angular_resolution: 0.18, 26 | grid_elevation_angular_resolution: 0.35 27 | } 28 | filter_configuration: { 29 | max_range: 100.0, 30 | min_range: 0.0, 31 | a: 52.0, 32 | b: 90.0, 33 | c: -1.105, 34 | lidar_protect: 1.0, 35 | variance: 0.1, 36 | radious: 8.0, 37 | var_factor: 0.4, 38 | ground_in_sim: -0.5, 39 | is_simulation: false, 40 | is_reconfig: false 41 | } 42 | ackermann_control: { 43 | max_angle: 24.0, 44 | delta_angle: 2.0, 45 | v_length: 1.05, 46 | delta_arc: 1.0, 47 | max_arc: 2.0, 48 | v_min: 0.4, 49 | v_max: 1.3, 50 | kp: 0.6, # if 1.0, proportional disactived 51 | margin: 1.0, #from base_link 52 | carrot_ctrl: true, 53 | carrot_distance: 4.0, 54 | carrot_stop_distance: 1.5 55 | } 56 | 57 | tracker_filter: { 58 | metrics: false, 59 | mute_image_trackers: false, 60 | label_file: "/media/alolivas/MSI_500/follow_experiments/label_follow4.txt", 61 | output_file: "/media/alolivas/MSI_500/follow_experiments/metrics_follow4.txt" 62 | } 63 | 64 | -------------------------------------------------------------------------------- /app_old/params/nav_follow_online.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | global_planning/stop_code: 10.0 # in 'z' coordinate 3 | 4 | 5 | #-variables for local_planning node 6 | local_planning: { 7 | frame_id: "os_sensor", 8 | frame_lidar: "os_sensor", 9 | save_data: false, 10 | url_file_out: "/home/alolivas/aurova-lab/aurova_ws/src/application_navigation/output_files/scans/", 11 | external_obstacle_detection: true, 12 | wa: 100.0, 13 | wr: 1.0, 14 | aa: 0.3, 15 | ar: 0.8, 16 | wa2: 3.0 17 | } 18 | lidar_configuration: { 19 | max_elevation_angle: 112.5, 20 | min_elevation_angle: 67.5, 21 | max_azimuth_angle: 360.0, 22 | min_azimuth_angle: 0.0, 23 | grid_azimuth_angular_resolution: 0.18, 24 | grid_elevation_angular_resolution: 0.35 25 | } 26 | filter_configuration: { 27 | max_range: 100.0, 28 | min_range: 0.0, 29 | a: 52.0, 30 | b: 90.0, 31 | c: -1.105, 32 | lidar_protect: 1.0, 33 | variance: 0.1, 34 | radious: 8.0, 35 | var_factor: 0.4, 36 | ground_in_sim: -0.5, 37 | is_simulation: false, 38 | is_reconfig: false 39 | } 40 | ackermann_control: { 41 | max_angle: 24.0, 42 | delta_angle: 2.0, 43 | v_length: 1.05, 44 | delta_arc: 1.0, 45 | max_arc: 2.0, 46 | v_min: 0.4, 47 | v_max: 1.3, 48 | kp: 0.6, # if 1.0, proportional disactived 49 | margin: 1.0, #from base_link 50 | carrot_ctrl: true, 51 | carrot_distance: 4.75, 52 | carrot_stop_distance: 1.5 53 | } 54 | 55 | -------------------------------------------------------------------------------- /app_old/paths/campus_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /app_old/paths/iuii_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /app_old/paths/parque_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /app_old/paths/patos_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /app_old/paths/scientific_park_01.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /app_old/paths/scientific_park_03.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /app_old/paths/urban_01_nl_2.osm: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /app_tracker_fusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(app_tracker_fusion) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | catkin_package( 13 | 14 | ) 15 | 16 | include_directories( 17 | 18 | ) 19 | -------------------------------------------------------------------------------- /app_tracker_fusion/README.md: -------------------------------------------------------------------------------- 1 | # Robust Single Object Tracking and Following by Fusion Strategy 2 | 3 | ## Overview 4 | This repository contains the code for the ICINCO 2023 paper [Robust Single Object Tracking and Following by Fusion Strategy](https://aurova-projects.github.io/tracker_fusion/). The code has been implemented with ROS Noetic in Ubuntu 20.04. 5 | 6 | ## Installation instructions 7 | 8 | 1. **Create a catkin workspace**: 9 | ``` 10 | mkdir tracker_fusion_ws 11 | cd tracker_fusion_ws 12 | catkin init 13 | ``` 14 | 15 | 2. **Clone Repositories**: 16 | - Clone the [applications](https://github.com/AUROVA-LAB/applications) repository. 17 | 18 | - Clone the repository [aurova_detections](https://github.com/AUROVA-LAB/aurova_detections). To compile only the needed files, leave the dependecies: 19 | ``` 20 | tracker_filter 21 | detection_msgs 22 | ``` 23 | of the file `aurova_detections/aurova_detections/package.xml`, and comment the others dependecies. 24 | 25 | - Clone the repository [aurova_planning](https://github.com/AUROVA-LAB/aurova_planning). To compile only the needed files, leave the dependecies: 26 | ``` 27 | local_planning 28 | ``` 29 | of the file `aurova_planning/aurova_planning/package.xml`, and comment the others dependecies. 30 | 31 | - Clone the repository [aurova_preprocessed](https://github.com/AUROVA-LAB/aurova_preprocessed). To compile only the needed files, leave the dependecies: 32 | ``` 33 | lidar_obstacle_detection 34 | ``` 35 | of the file `aurova_planning/aurova_planning/package.xml`, and comment the others dependecies. 36 | 37 | 3. **Install Libraries**: 38 | - Follow the instructions to install the [lib_planning](https://github.com/AUROVA-LAB/lib_planning) library. 39 | 40 | 4. **Build Docker Images**: 41 | - Follow the instuctions of [image_trackers_ouster](https://github.com/AUROVA-LAB/aurova_detections/tree/main/image_trackers_ouster) to build the Docker image with the images trackers. 42 | - Optionally, follow the instructions of [m2track_ros](https://github.com/AUROVA-LAB/aurova_detections/tree/main//m2track_ros) to build the Docker image with the point cloud tracker (consider that better results are reported without using it). 43 | 44 | 5. **Requirements for BLUE robot (Optional)**. 45 | - Clone the repository [robot_blue](https://github.com/AUROVA-LAB/robot_blue). 46 | 47 | 6. **Install ROS packages**: 48 | ``` 49 | cd tracker_fusion_ws 50 | catkin_make 51 | ``` 52 | 53 | ## Usage 54 | 55 | 1. **Run Docker Images**: 56 | 57 | - For the image tracker: 58 | ``` 59 | sudo docker run --rm --gpus all --ipc=host --ulimit memlock=-1 --ulimit stack=67108864 --net host -it aurova_image_tracker 60 | ``` 61 | - Optionally for the point cloud tracker: 62 | ``` 63 | sudo docker run --rm --gpus all --ipc=host --ulimit memlock=-1 --ulimit stack=67108864 --net host -it aurova_pc_tracker 64 | ``` 65 | 66 | 2. **Launch ROS Nodes**: 67 | - Offline mode: 68 | ``` 69 | roslaunch app_tracker_fusion nav_follow_offline.launch 70 | ``` 71 | 72 | - Online mode: 73 | ``` 74 | roslaunch app_tracker_fusion nav_follow_online.launch 75 | ``` 76 | 77 | ## Citation 78 | If you find our code or papers useful, please cite: 79 | ``` 80 | @conference{olivas2023robust, 81 | author={Alejandro Olivas. and Miguel Muñoz{-}Bañón. and Edison Velasco. and Fernando Torres.}, 82 | title={Robust Single Object Tracking and Following by Fusion Strategy}, 83 | booktitle={Proceedings of the 20th International Conference on Informatics in Control, Automation and Robotics - Volume 1: ICINCO}, 84 | year={2023}, 85 | pages={624-631}, 86 | publisher={SciTePress}, 87 | organization={INSTICC}, 88 | doi={10.5220/0012178900003543}, 89 | isbn={978-989-758-670-5}, 90 | issn={2184-2809}, 91 | } 92 | ``` 93 | -------------------------------------------------------------------------------- /app_tracker_fusion/launch/nav_follow_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /app_tracker_fusion/launch/nav_follow_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /app_tracker_fusion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | app_tracker_fusion 4 | 0.0.0 5 | The application package of the tracker fusion 6 | 7 | 8 | 9 | 10 | Alejandro Olivas González 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | catkin 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /app_tracker_fusion/params/nav_follow_offline.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | use_sim_time: true 3 | 4 | global_planning/stop_code: 10.0 # in 'z' coordinate 5 | 6 | 7 | #-variables for local_planning node 8 | local_planning: { 9 | frame_id: "os_sensor", 10 | frame_lidar: "os_sensor", 11 | save_data: false, 12 | url_file_out: "/home/alolivas/aurova-lab/aurova_ws/src/application_navigation/output_files/scans/", 13 | external_obstacle_detection: true, 14 | wa: 100.0, 15 | wr: 1.0, 16 | aa: 0.3, 17 | ar: 0.8, 18 | wa2: 3.0 19 | } 20 | lidar_configuration: { 21 | max_elevation_angle: 112.5, 22 | min_elevation_angle: 67.5, 23 | max_azimuth_angle: 360.0, 24 | min_azimuth_angle: 0.0, 25 | grid_azimuth_angular_resolution: 0.18, 26 | grid_elevation_angular_resolution: 0.35 27 | } 28 | filter_configuration: { 29 | max_range: 100.0, 30 | min_range: 0.0, 31 | a: 52.0, 32 | b: 90.0, 33 | c: -1.105, 34 | lidar_protect: 1.0, 35 | variance: 0.1, 36 | radious: 8.0, 37 | var_factor: 0.4, 38 | ground_in_sim: -0.5, 39 | is_simulation: false, 40 | is_reconfig: false 41 | } 42 | ackermann_control: { 43 | max_angle: 24.0, 44 | delta_angle: 2.0, 45 | v_length: 1.05, 46 | delta_arc: 1.0, 47 | max_arc: 2.0, 48 | v_min: 0.4, 49 | v_max: 1.3, 50 | kp: 0.6, # if 1.0, proportional disactived 51 | margin: 1.0, #from base_link 52 | carrot_ctrl: true, 53 | carrot_distance: 4.0, 54 | carrot_stop_distance: 1.5 55 | } 56 | 57 | tracker_filter: { 58 | metrics: false, 59 | mute_image_trackers: false, 60 | label_file: "/media/alolivas/MSI_500/follow_experiments/label_follow4.txt", 61 | output_file: "/media/alolivas/MSI_500/follow_experiments/metrics_follow4.txt", 62 | rate: 10, 63 | bounding_box_percet_reduction: 0.6, 64 | max_traslation_distance: 1.0, 65 | filter_radious: 0.8, 66 | search_time: 5.0, 67 | x_model: 1.0, 68 | y_model: 1.0 69 | } 70 | 71 | -------------------------------------------------------------------------------- /app_tracker_fusion/params/nav_follow_online.yaml: -------------------------------------------------------------------------------- 1 | #-global variable 2 | global_planning/stop_code: 10.0 # in 'z' coordinate 3 | 4 | 5 | #-variables for local_planning node 6 | local_planning: { 7 | frame_id: "os_sensor", 8 | frame_lidar: "os_sensor", 9 | save_data: false, 10 | url_file_out: "/home/alolivas/aurova-lab/aurova_ws/src/application_navigation/output_files/scans/", 11 | external_obstacle_detection: true, 12 | wa: 100.0, 13 | wr: 1.0, 14 | aa: 0.3, 15 | ar: 0.8, 16 | wa2: 3.0 17 | } 18 | lidar_configuration: { 19 | max_elevation_angle: 112.5, 20 | min_elevation_angle: 67.5, 21 | max_azimuth_angle: 360.0, 22 | min_azimuth_angle: 0.0, 23 | grid_azimuth_angular_resolution: 0.18, 24 | grid_elevation_angular_resolution: 0.35 25 | } 26 | filter_configuration: { 27 | max_range: 100.0, 28 | min_range: 0.0, 29 | a: 52.0, 30 | b: 90.0, 31 | c: -1.105, 32 | lidar_protect: 1.0, 33 | variance: 0.1, 34 | radious: 8.0, 35 | var_factor: 0.4, 36 | ground_in_sim: -0.5, 37 | is_simulation: false, 38 | is_reconfig: false 39 | } 40 | ackermann_control: { 41 | max_angle: 24.0, 42 | delta_angle: 2.0, 43 | v_length: 1.05, 44 | delta_arc: 1.0, 45 | max_arc: 2.0, 46 | v_min: 0.4, 47 | v_max: 1.3, 48 | kp: 0.6, # if 1.0, proportional disactived 49 | margin: 1.0, #from base_link 50 | carrot_ctrl: true, 51 | carrot_distance: 4.75, 52 | carrot_stop_distance: 1.5 53 | } 54 | 55 | tracker_filter: { 56 | rate: 10, 57 | bounding_box_percet_reduction: 0.6, 58 | max_traslation_distance: 1.0, 59 | filter_radious: 0.8, 60 | search_time: 5.0, 61 | x_model: 1.0, 62 | y_model: 1.0 63 | 64 | } -------------------------------------------------------------------------------- /applications/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(applications) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /applications/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | applications 4 | 0.0.0 5 | The applications metapackage 6 | 7 | 8 | 9 | 10 | Alejandro Olivas González 11 | 12 | 13 | 14 | 15 | 16 | LGPL 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | app_old 34 | app_tracker_fusion 35 | app_MTAP 36 | app_geo_localization 37 | 38 | catkin 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | --------------------------------------------------------------------------------