diff --git a/README.md b/README.md
index 36640a1d..fee72b5d 100644
--- a/README.md
+++ b/README.md
@@ -76,16 +76,16 @@ in below three files (`sudo` required)
 ### Autoware Build
 ```
 # If you have CUDA
-AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
+AUTOWARE_COMPILE_WITH_CUDA=1 catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release
 
 # Build only some package
-AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select $(pakcage name)
+AUTOWARE_COMPILE_WITH_CUDA=1 catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release --pkg $(package name)
 
 # Build without some package
-AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip $(pakcage name)
+AUTOWARE_COMPILE_WITH_CUDA=1 catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_BLACKLIST_PACKAGES=$(package name)
 
 # If you don't have CUDA
-colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
+catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 
 <!-- Since Autoware recommend to use directory name 'autoware.ai', you should make soft link with autoware.ai to this repository
@@ -96,7 +96,7 @@ ln -s ${WORKSPACE_DIR}/RUBIS-SelfDriving ~/autoware.ai
 
 And it is recommned to add below sourcing command in your `~/.bashrc` file.
 ```
-source ~/autoware.ai/install/setup.bash
+source ~/autoware.ai/devel/setup.bash
 ``` -->
 
 ## How to build package in rubis_ws
diff --git a/autoware.ai/.catkin_workspace b/autoware.ai/.catkin_workspace
new file mode 100644
index 00000000..52fd97e7
--- /dev/null
+++ b/autoware.ai/.catkin_workspace
@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration
diff --git a/autoware.ai/.gitignore b/autoware.ai/.gitignore
index 8fd0d088..05bb0806 100644
--- a/autoware.ai/.gitignore
+++ b/autoware.ai/.gitignore
@@ -1,6 +1,8 @@
 build
 install
+devel
 log
 build/*
+devel/*
 install/*
 log/*
diff --git a/autoware.ai/autoware_files/carla_launch/1_sensing.launch b/autoware.ai/autoware_files/carla_launch/1_sensing.launch
index 7877aaac..b7e35f73 100644
--- a/autoware.ai/autoware_files/carla_launch/1_sensing.launch
+++ b/autoware.ai/autoware_files/carla_launch/1_sensing.launch
@@ -21,9 +21,9 @@
   </include>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <!-- <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw_origin"  /> -->
+  <!-- <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw_origin"  /> -->
 
-  <!-- <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher" output="screen">
+  <!-- <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher">
     <param name="/input_topic" value="$(arg image_input_topic)" />
   </node> -->
 
@@ -34,7 +34,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch b/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch
index dcd33289..5738da2e 100644
--- a/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch
+++ b/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch
@@ -25,7 +25,7 @@
   <arg name="numthreads" default="4" doc="vgicp threads num" />
   <arg name="leafsize" default="0.01" doc="point map downsample leafsize" />
 
-  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node" output="screen">
+  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node">
     <param name="init_x" value="$(arg init_x)" />
     <param name="init_y" value="$(arg init_y)" />
     <param name="init_z" value="$(arg init_z)" />
diff --git a/autoware.ai/autoware_files/carla_launch/2_localization_param.launch b/autoware.ai/autoware_files/carla_launch/2_localization_param.launch
index 89ca7b9c..a712d4cf 100644
--- a/autoware.ai/autoware_files/carla_launch/2_localization_param.launch
+++ b/autoware.ai/autoware_files/carla_launch/2_localization_param.launch
@@ -14,14 +14,12 @@
   <!-- points downsampler -->
   <arg name="leaf_size" default="1.0" />
   <arg name="measurement_range" default="200" />
-  <arg name="instance_mode" default="0"/>
 
   <node pkg="points_downsampler" name="voxel_grid_filter" type="voxel_grid_filter">
     <param name="input_topic_name" value="points_raw" />
     <param name="output_topic_name" value="filtered_points" />
     <param name="leaf_size" value="$(arg leaf_size)" />
     <param name="measurement_range" value="$(arg measurement_range)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
@@ -35,7 +33,7 @@
   <arg name="gnss_pose_diff_threshold" default="5.0"/>
 
   <!-- ndt_matching -->
-  <node pkg="lidar_localizer" type="modular_ndt_matching" name="ndt_matching" output="screen">
+  <node pkg="lidar_localizer" type="modular_ndt_matching" name="ndt_matching">
     <param name="method_type" value="$(arg method_type)" />
 
     <!-- NDT config -->
diff --git a/autoware.ai/autoware_files/carla_launch/4_planning.launch b/autoware.ai/autoware_files/carla_launch/4_planning.launch
index f96e87fe..bdbe0f5b 100644
--- a/autoware.ai/autoware_files/carla_launch/4_planning.launch
+++ b/autoware.ai/autoware_files/carla_launch/4_planning.launch
@@ -20,7 +20,7 @@
   <arg name="goal_ori_z"            value="0.99" />
   <arg name="goal_ori_w"            value="0.02" />
 
-  <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner" output="screen">    
+  <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner">    
     <rosparam command="load" file="$(find rubis_autorunner)/cfg/$(arg LaneInfoFile)"/>
     <param name="pathDensity"         value="$(arg pathDensity)" />
     <param name="enableSmoothing"       value="$(arg enableSmoothing)" />
diff --git a/autoware.ai/autoware_files/data/tf/tf.launch b/autoware.ai/autoware_files/data/tf/tf.launch
index 4c4bfc50..d1b67f61 100755
--- a/autoware.ai/autoware_files/data/tf/tf.launch
+++ b/autoware.ai/autoware_files/data/tf/tf.launch
@@ -1,9 +1,9 @@
 <!---->
 <launch>
 <!-- worldからmapへのtf -->
-<node pkg="tf"  type="static_transform_publisher" name="world_to_map" args="0.0001 0 0 0 0 0 /world /map 10" />
+<node pkg="tf"  type="static_transform_publisher" name="world_to_map" args="0.0001 0 0 0 0 0 /world /map 100" />
 
 <!-- mapからmobilityへのtf -->
-<node pkg="tf"  type="static_transform_publisher" name="map_to_mobility" args="0 0 0 0 0 0 /map /mobility 10" />
+<node pkg="tf"  type="static_transform_publisher" name="map_to_mobility" args="0 0 0 0 0 0 /map /mobility 100" />
 
 </launch>
diff --git a/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml b/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml
index 5ab0bef1..aa3fa2f0 100644
--- a/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml
+++ b/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml
@@ -12,7 +12,7 @@
 # Sensing
 ray_ground_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -22,7 +22,7 @@ ray_ground_filter:
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -33,17 +33,12 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
   task_execution_time: 70000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Detection
 calibration_publisher:
@@ -51,37 +46,27 @@ calibration_publisher:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -91,7 +76,7 @@ imm_ukf_pda_track:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100000000
@@ -108,7 +93,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -117,7 +102,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100000000
@@ -133,7 +118,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -146,7 +131,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 40000000
@@ -156,7 +141,7 @@ op_motion_predictor:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 30 #30
   task_minimum_inter_release_time: 33333333
@@ -167,7 +152,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -176,7 +161,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -187,7 +172,7 @@ twist_gate:
 # Others
 lidar_republisher:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -196,7 +181,7 @@ lidar_republisher:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -205,7 +190,7 @@ vel_relay:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -214,7 +199,7 @@ vel_relay:
 
 republish:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
diff --git a/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml b/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml
index 4705e95b..a4354780 100644
--- a/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml
+++ b/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml
@@ -12,7 +12,7 @@
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -23,17 +23,12 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
   task_execution_time: 70000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Detection
 compare_map_filter:
@@ -42,7 +37,7 @@ compare_map_filter:
   max_clipping_height: 0.5
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -51,37 +46,27 @@ compare_map_filter:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -91,7 +76,7 @@ imm_ukf_pda_track:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100000000
@@ -108,7 +93,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -117,7 +102,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100000000
@@ -133,7 +118,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -146,7 +131,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 40000000
@@ -156,7 +141,7 @@ op_motion_predictor:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 30 #30
   task_minimum_inter_release_time: 33333333
@@ -167,7 +152,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -176,7 +161,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -187,7 +172,7 @@ twist_gate:
 # Others
 lidar_republisher:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -196,7 +181,7 @@ lidar_republisher:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -205,7 +190,7 @@ vel_relay:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -214,7 +199,7 @@ vel_relay:
 
 republish:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
diff --git a/autoware.ai/autoware_files/data/yaml/minicar_params.yaml b/autoware.ai/autoware_files/data/yaml/minicar_params.yaml
index 1a01c619..d6883c36 100644
--- a/autoware.ai/autoware_files/data/yaml/minicar_params.yaml
+++ b/autoware.ai/autoware_files/data/yaml/minicar_params.yaml
@@ -12,7 +12,7 @@
 # Sensing
 ray_ground_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -22,7 +22,7 @@ ray_ground_filter:
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -33,17 +33,12 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
   task_execution_time: 70000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Detection
 calibration_publisher:
@@ -51,37 +46,27 @@ calibration_publisher:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 1
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -90,7 +75,7 @@ imm_ukf_pda_track:
 
 range_vision_fusion:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -100,7 +85,7 @@ range_vision_fusion:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100000000
@@ -116,7 +101,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -125,7 +110,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 10 #100
   task_minimum_inter_release_time: 100000000
@@ -141,7 +126,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 10 #100
   task_minimum_inter_release_time: 10000000
@@ -154,7 +139,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 10 #25
   task_minimum_inter_release_time: 40000000
@@ -164,7 +149,7 @@ op_motion_predictor:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 1 #30
   task_minimum_inter_release_time: 33333333
@@ -175,7 +160,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 1 
   task_minimum_inter_release_time: 100000000
@@ -184,7 +169,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 1 
   task_minimum_inter_release_time: 100000000
diff --git a/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml b/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml
index dc83e4b8..582d2a7a 100644
--- a/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml
+++ b/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml
@@ -12,7 +12,7 @@
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -23,17 +23,12 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
   task_execution_time: 70000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Detection
 compare_map_filter:
@@ -42,7 +37,7 @@ compare_map_filter:
   max_clipping_height: 0.5
 
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -51,37 +46,27 @@ compare_map_filter:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -91,7 +76,7 @@ imm_ukf_pda_track:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100000000
@@ -108,7 +93,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -117,7 +102,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 10 #100
   task_minimum_inter_release_time: 100000000
@@ -133,7 +118,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 10 #100
   task_minimum_inter_release_time: 10000000
@@ -146,7 +131,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 10 #25
   task_minimum_inter_release_time: 40000000
@@ -156,7 +141,7 @@ op_motion_predictor:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 10 #30
   task_minimum_inter_release_time: 33333333
@@ -167,7 +152,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -176,7 +161,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -187,7 +172,7 @@ twist_gate:
 # Others
 lidar_republisher:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -196,7 +181,7 @@ lidar_republisher:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -205,7 +190,7 @@ vel_relay:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -214,7 +199,7 @@ vel_relay:
 
 republish:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
diff --git a/autoware.ai/autoware_files/gnss_mapping.launch b/autoware.ai/autoware_files/gnss_mapping.launch
index 6eec89a1..3c46f99c 100644
--- a/autoware.ai/autoware_files/gnss_mapping.launch
+++ b/autoware.ai/autoware_files/gnss_mapping.launch
@@ -33,8 +33,8 @@
   <arg name="tf_yaw" default="-0.785" />
 
   <!-- rosrun lidar_localizer ndt_mapping  -->
-  <node pkg="lidar_localizer" type="queue_counter" name="queue_counter" output="screen"/>
-  <node pkg="lidar_localizer" type="gnss_mapping" name="gnss_mapping" output="screen">
+  <node pkg="lidar_localizer" type="queue_counter" name="queue_counter"/>
+  <node pkg="lidar_localizer" type="gnss_mapping" name="gnss_mapping">
     <param name="voxel_leaf_size" value="$(arg voxel_leaf_size)" />
     <param name="min_scan_range" value="$(arg min_scan_range)" />
     <param name="max_scan_range" value="$(arg max_scan_range)" />
diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch
index aa5985b4..9c5b57e7 100644
--- a/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch
+++ b/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch
@@ -6,9 +6,9 @@
   <arg name="image_input_topic" default="/image_raw_origin"/>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
 
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
+  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher">
     <param name="/input_topic" value="$(arg lidar_input_topic)" />
     <param name="/output_topic" value="$(arg lidar_output_topic)" />
   </node>
@@ -26,7 +26,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch
index a431a452..d941a1f5 100644
--- a/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch
+++ b/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch
@@ -1,20 +1,20 @@
 <launch>
   <!-- ROS-Bridge node for simulator connection-->
-  <!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" output="screen" clear_params="true" required="true" /> -->  
+  <!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" clear_params="true" required="true" /> -->  
   <rosparam command="load" file="$(env USER_HOME)/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml" />
   <arg name="lidar_input_topic" default="/points_raw_origin"/>
   <arg name="lidar_output_topic" default="/points_raw"/>
   <arg name="image_input_topic" default="/image_raw_origin"/>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
 
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
+  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher">
     <param name="/input_topic" value="$(arg lidar_input_topic)" />
     <param name="/output_topic" value="$(arg lidar_output_topic)" />
   </node>
 
-  <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher" output="screen">
+  <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher">
     <param name="/input_topic" value="$(arg image_input_topic)" />
   </node>
 
@@ -31,7 +31,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch b/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch
index b4bd4276..eb15e97b 100644
--- a/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch
+++ b/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch
@@ -1,11 +1,11 @@
 <launch>
   <!-- ROS-Bridge node for Simulator connection -->
-  <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" output="screen" clear_params="true" required="true" />
+  <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" clear_params="true" required="true" />
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw_origin"  />
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw_origin"  />
 
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
+  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher">
       <param name="/input_topic" value="/points_raw_origin" />
       <param name="/output_topic" value="/points_raw" />
   </node>
diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch b/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch
index 2fe5748b..1fb35397 100644
--- a/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch
+++ b/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch
@@ -1,6 +1,6 @@
 <launch>
 	<!-- ROS-Bridge node for Simulator connection -->
-  	<!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" output="screen" clear_params="true" required="true" /> -->
+  	<!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" clear_params="true" required="true" /> -->
 
 	<!-- Vector Map Loader -->
 	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
@@ -11,7 +11,7 @@
 	    $(env HOME)/autoware.ai/autoware_files/vector_map/borregas_lidar/point.csv"/>
 	
 	<!-- Point Map Loader -->
-	<node pkg="map_file" type="points_map_loader" name="points_map_loader" output="screen"
+	<node pkg="map_file" type="points_map_loader" name="points_map_loader"
 		args="$(env HOME)/autoware.ai/autoware_files/points_map/BorregasAve.pcd"/>
 
 	<!-- Map TF Publisher -->	
diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch
index 3fd2a96f..f6c44d32 100644
--- a/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch
+++ b/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch
@@ -1,19 +1,19 @@
 <launch>
   <!-- ROS-Bridge node for simulator connection-->
-  <!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" output="screen" clear_params="true" required="true" /> -->  
+  <!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" clear_params="true" required="true" /> -->  
   <rosparam command="load" file="$(env USER_HOME)/autoware.ai/autoware_files/data/yaml/minicar_params.yaml" />
   <arg name="lidar_input_topic" default="/points_raw_origin"/>
   <arg name="lidar_output_topic" default="/points_raw"/>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
 
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
+  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher">
     <param name="/input_topic" value="$(arg lidar_input_topic)" />
     <param name="/output_topic" value="$(arg lidar_output_topic)" />
   </node>
 
-  <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher" output="screen">
+  <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher">
     <param name="/input_topic" value="$(arg image_input_topic)" />
   </node>
 
@@ -30,7 +30,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch
index 797d0f89..3c680ac1 100644
--- a/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch
+++ b/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch
@@ -1,20 +1,20 @@
 <launch>
   <!-- ROS-Bridge node for simulator connection-->
-  <!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" output="screen" clear_params="true" required="true" /> -->  
+  <!-- <node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" clear_params="true" required="true" /> -->  
   <rosparam command="load" file="$(env USER_HOME)/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml" />
   <arg name="lidar_input_topic" default="/points_raw_origin"/>
   <arg name="lidar_output_topic" default="/points_raw"/>
   <arg name="image_input_topic" default="/image_raw_origin"/>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
 
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
+  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher">
     <param name="/input_topic" value="$(arg lidar_input_topic)" />
     <param name="/output_topic" value="$(arg lidar_output_topic)" />
   </node>
 
-  <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher" output="screen">
+  <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher">
     <param name="/input_topic" value="$(arg image_input_topic)" />
   </node>
 
@@ -31,7 +31,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/autoware.ai/autoware_files/rviz/carla.rviz b/autoware.ai/autoware_files/rviz/carla.rviz
index 8257e334..3dc0c6ea 100644
--- a/autoware.ai/autoware_files/rviz/carla.rviz
+++ b/autoware.ai/autoware_files/rviz/carla.rviz
@@ -11,7 +11,7 @@ Panels:
         - /Perception1
         - /Perception1/Tracked Objects1
       Splitter Ratio: 0.4564755856990814
-    Tree Height: 547
+    Tree Height: 408
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -30,11 +30,11 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Points Map
+    SyncSource: Filtered Point
   - Class: autoware_rviz_debug/DecisionMakerPanel
     Name: DecisionMakerPanel
   - Class: integrated_viewer/ImageViewerPlugin
-    Image topic: /image_raw
+    Image topic: -----
     Lane topic: -----
     Name: ImageViewerPlugin
     Point size: 3
@@ -78,10 +78,14 @@ Visualization Manager:
               Value: true
             ego_vehicle:
               Value: true
+            ego_vehicle/collision:
+              Value: true
             ego_vehicle/gnss:
               Value: true
             ego_vehicle/imu:
               Value: true
+            ego_vehicle/lane_invasion:
+              Value: true
             ego_vehicle/lidar:
               Value: true
             ego_vehicle/rgb_front:
@@ -96,10 +100,14 @@ Visualization Manager:
               Value: true
             obstacle:
               Value: true
+            obstacle/collision:
+              Value: true
             velodyne:
               Value: true
             walker:
               Value: true
+            walker/collision:
+              Value: true
             world:
               Value: true
           Marker Scale: 5
@@ -128,9 +136,11 @@ Visualization Manager:
                 mobility:
                   {}
                 obstacle:
-                  {}
+                  obstacle/collision:
+                    {}
                 walker:
-                  {}
+                  walker/collision:
+                    {}
           Update Interval: 0
           Value: true
         - Alpha: 1
@@ -352,9 +362,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 4096
           Min Color: 0; 0; 0
-          Min Intensity: 0
           Name: Points Map
           Position Transformer: XYZ
           Queue Size: 10
@@ -394,9 +402,7 @@ Visualization Manager:
           Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 0.9980455636978149
           Min Color: 0; 0; 0
-          Min Intensity: 0.5593598484992981
           Name: Points Raw
           Position Transformer: XYZ
           Queue Size: 10
@@ -424,9 +430,7 @@ Visualization Manager:
           Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 4096
           Min Color: 0; 0; 0
-          Min Intensity: 0
           Name: Points No Ground
           Position Transformer: XYZ
           Queue Size: 10
@@ -454,9 +458,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 0.9974985122680664
           Min Color: 0; 0; 0
-          Min Intensity: 0.5666885375976562
           Name: Filtered Point
           Position Transformer: XYZ
           Queue Size: 10
@@ -484,9 +486,7 @@ Visualization Manager:
           Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 4096
           Min Color: 0; 0; 0
-          Min Intensity: 0
           Name: Aligned Point
           Position Transformer: XYZ
           Queue Size: 10
@@ -567,9 +567,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 4096
           Min Color: 0; 0; 0
-          Min Intensity: 0
           Name: Clustered Points
           Position Transformer: XYZ
           Queue Size: 10
@@ -747,7 +745,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -0.0349983349442482
+      Angle: 0.025001663714647293
       Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -757,23 +755,23 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 10.921646118164062
+      Scale: 4.204458236694336
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz)
-      X: 262.76385498046875
-      Y: 204.9719696044922
+      X: 205.8017120361328
+      Y: 244.97866821289062
     Saved: ~
 Window Geometry:
   DecisionMakerPanel:
     collapsed: false
   Displays:
-    collapsed: false
-  Height: 1410
-  Hide Left Dock: false
+    collapsed: true
+  Height: 846
+  Hide Left Dock: true
   Hide Right Dock: true
   ImageViewerPlugin:
-    collapsed: false
-  QMainWindow State: 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
+    collapsed: true
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -782,6 +780,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2560
-  X: 2560
-  Y: 0
+  Width: 1865
+  X: 44
+  Y: 1254
diff --git a/autoware.ai/autoware_files/rviz/cubetown.rviz b/autoware.ai/autoware_files/rviz/cubetown.rviz
new file mode 100644
index 00000000..63d05258
--- /dev/null
+++ b/autoware.ai/autoware_files/rviz/cubetown.rviz
@@ -0,0 +1,638 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1
+        - /System1/TF1
+        - /System1/TF1/Frames1
+        - /Map1/Vector Map1
+        - /Sensing1
+        - /Perception1
+        - /Perception1/Lidar Objects1
+        - /Planning1
+        - /Planning1/OP Planner1
+        - /Planning1/OP Planner1/GlobalPathAnimation1
+        - /Planning1/Next Traget1
+        - /Planning1/Behavior State1
+      Splitter Ratio: 0.22158685326576233
+    Tree Height: 1675
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Points Map
+  - Class: autoware_rviz_debug/DecisionMakerPanel
+    Name: DecisionMakerPanel
+  - Class: integrated_viewer/ImageViewerPlugin
+    Image topic: -----
+    Lane topic: -----
+    Name: ImageViewerPlugin
+    Point size: 3
+    Point topic: -----
+    Rect topic: /detection/image_detector/objects
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 10
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 20
+      Reference Frame: map
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/TF
+          Enabled: true
+          Frame Timeout: 15
+          Frames:
+            All Enabled: false
+            base_link:
+              Value: true
+            camera:
+              Value: false
+            map:
+              Value: true
+            mobility:
+              Value: false
+            velodyne:
+              Value: true
+            world:
+              Value: false
+          Marker Scale: 30
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: true
+          Tree:
+            world:
+              map:
+                base_link:
+                  velodyne:
+                    camera:
+                      {}
+                mobility:
+                  {}
+          Update Interval: 0
+          Value: true
+        - Alpha: 1
+          Class: rviz/RobotModel
+          Collision Enabled: false
+          Enabled: true
+          Links:
+            All Links Enabled: true
+            Expand Joint Details: false
+            Expand Link Details: false
+            Expand Tree: false
+            Link Tree Style: Links in Alphabetic Order
+          Name: Vehicle Model
+          Robot Description: robot_description
+          TF Prefix: ""
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+        - Class: rviz/Group
+          Displays:
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: Velocity (km/h)
+              Show Value: true
+              Topic: /linear_velocity_viz
+              Value: true
+              auto color change: false
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 1
+              foreground color: 0; 255; 255
+              height: 80
+              left: 40
+              linewidth: 1
+              max color: 255; 0; 0
+              max value: 1
+              min value: -1
+              show caption: true
+              text size: 8
+              top: 30
+              update interval: 0.03999999910593033
+              width: 80
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: NDT Time [ms]
+              Show Value: true
+              Topic: /time_ndt_matching
+              Value: true
+              auto color change: false
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 1
+              foreground color: 0; 255; 255
+              height: 80
+              left: 140
+              linewidth: 1
+              max color: 255; 0; 0
+              max value: 1
+              min value: -1
+              show caption: true
+              text size: 8
+              top: 30
+              update interval: 0.03999999910593033
+              width: 80
+            - Align Bottom: false
+              Background Alpha: 0.8999999761581421
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 0; 255; 255
+              Invert Shadow: false
+              Name: NDT Monitor
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /ndt_monitor/ndt_info_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 50
+              left: 40
+              line width: 2
+              text size: 8
+              top: 150
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.8999999761581421
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 0; 255; 255
+              Invert Shadow: false
+              Name: Decision Maker Panel
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /decision_maker/state_overlay
+              Value: true
+              font: DejaVu Sans Mono
+              height: 200
+              left: 40
+              line width: 2
+              text size: 8
+              top: 220
+              width: 200
+          Enabled: true
+          Name: Monitor
+        - Class: rviz/Group
+          Displays:
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 25; 255; 240
+              Invert Shadow: false
+              Name: OK
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/ok_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 430
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 255; 0
+              Invert Shadow: false
+              Name: WARN
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/warn_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 520
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 85; 0
+              Invert Shadow: false
+              Name: ERROR
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/error_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 620
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 0; 0
+              Invert Shadow: false
+              Name: FATAL
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/fatal_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 720
+              width: 200
+          Enabled: true
+          Name: Health Checker
+      Enabled: true
+      Name: System
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.05000000074505806
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points Map
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_map
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /vector_map_center_lines_rviz
+          Name: Vector Map
+          Namespaces:
+            road_network_vector_map: true
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 23.83440399169922
+            Min Value: -22.532455444335938
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points Raw
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 5
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /filtered_points
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points No Ground
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 5
+          Size (m): 5
+          Style: Points
+          Topic: /debug_points_no_ground
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+      Enabled: false
+      Name: Sensing
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 23.83440399169922
+            Min Value: -22.532455444335938
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Clustered Points
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 10
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_cluster_center
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /detection/lidar_detector/objects_markers_center
+          Name: Lidar Objects
+          Namespaces:
+            /detection/lidar_detector/centroid_markers2: true
+            /detection/lidar_detector/hull_markers2: true
+            /detection/lidar_detector/label_markers2: true
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Perception
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Group
+          Displays:
+            - Class: rviz/MarkerArray
+              Enabled: true
+              Marker Topic: /local_trajectories_eval_rviz
+              Name: Local Rollouts
+              Namespaces:
+                local_lane_array_marker_colored: true
+              Queue Size: 100
+              Value: true
+            - Class: rviz/Marker
+              Enabled: true
+              Marker Topic: /safety_border
+              Name: Safety Box
+              Namespaces:
+                global_lane_array_marker: true
+              Queue Size: 100
+              Value: true
+            - Class: jsk_rviz_plugin/BoundingBoxArray
+              Enabled: false
+              Name: Simulated Obstacle
+              Topic: /dp_planner_tracked_boxes
+              Unreliable: false
+              Value: false
+              alpha: 0.800000011920929
+              color: 25; 255; 0
+              coloring: Value
+              line width: 0.009999999776482582
+              only edge: false
+              show coords: false
+            - Class: rviz/MarkerArray
+              Enabled: false
+              Marker Topic: /global_waypoints_rviz
+              Name: GlobalPathAnimation
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+            - Class: rviz/Marker
+              Enabled: false
+              Marker Topic: /behavior_state
+              Name: Behavior State
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+            - Class: rviz/MarkerArray
+              Enabled: false
+              Marker Topic: /detected_polygons
+              Name: Tracked Contours
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+            - Class: rviz/MarkerArray
+              Enabled: false
+              Marker Topic: /global_waypoints_rviz
+              Name: Global Path
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+          Enabled: true
+          Name: OP Planner
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /next_target_mark
+          Name: Next Traget
+          Namespaces:
+            next_target_marker: true
+          Queue Size: 100
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /behavior_state
+          Name: Behavior State
+          Namespaces:
+            beh_state: true
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Planning
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /trajectory_circle_mark
+          Name: Pure Pursuit Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /next_target_mark
+          Name: Pure Pursuit Target
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /mpc_follower/debug/predicted_traj
+          Name: MPC Predicted Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /mpc_follower/debug/filtered_traj
+          Name: MPC Reference Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+      Enabled: false
+      Name: Control
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: 1.300126314163208e-6
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 8.730535507202148
+      Target Frame: map
+      Value: TopDownOrtho (rviz)
+      X: -18.137195587158203
+      Y: 45.29766082763672
+    Saved: ~
+Window Geometry:
+  DecisionMakerPanel:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 2049
+  Hide Left Dock: false
+  Hide Right Dock: false
+  ImageViewerPlugin:
+    collapsed: false
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 3706
+  X: 134
+  Y: 55
diff --git a/autoware.ai/autoware_files/rviz/cubetown_origin.rviz b/autoware.ai/autoware_files/rviz/cubetown_origin.rviz
new file mode 100644
index 00000000..d6d204f0
--- /dev/null
+++ b/autoware.ai/autoware_files/rviz/cubetown_origin.rviz
@@ -0,0 +1,712 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Grid1
+        - /System1
+        - /System1/TF1
+        - /System1/TF1/Frames1
+        - /Map1
+        - /Map1/Vector Map1
+        - /Perception1
+        - /Perception1/Tracked Objects1
+        - /Planning1
+        - /Planning1/Saved Waypoints1
+      Splitter Ratio: 0.642276406288147
+    Tree Height: 1042
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Points Map
+  - Class: autoware_rviz_debug/DecisionMakerPanel
+    Name: DecisionMakerPanel
+  - Class: integrated_viewer/ImageViewerPlugin
+    Image topic: -----
+    Lane topic: -----
+    Name: ImageViewerPlugin
+    Point size: 3
+    Point topic: -----
+    Rect topic: /detection/image_detector/objects
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 10
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: base_link
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/TF
+          Enabled: true
+          Frame Timeout: 15
+          Frames:
+            All Enabled: false
+            base_link:
+              Value: true
+            camera:
+              Value: false
+            gnss:
+              Value: true
+            map:
+              Value: true
+            mobility:
+              Value: false
+            velodyne:
+              Value: true
+            world:
+              Value: false
+          Marker Scale: 5
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: true
+          Tree:
+            world:
+              {}
+          Update Interval: 0
+          Value: true
+        - Alpha: 1
+          Class: rviz/RobotModel
+          Collision Enabled: false
+          Enabled: true
+          Links:
+            All Links Enabled: true
+            Expand Joint Details: false
+            Expand Link Details: false
+            Expand Tree: false
+            Link Tree Style: Links in Alphabetic Order
+          Name: Vehicle Model
+          Robot Description: robot_description
+          TF Prefix: ""
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+        - Class: rviz/Group
+          Displays:
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: Velocity (km/h)
+              Show Value: true
+              Topic: /linear_velocity_viz
+              Value: true
+              auto color change: false
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 1
+              foreground color: 0; 255; 255
+              height: 80
+              left: 40
+              linewidth: 1
+              max color: 255; 0; 0
+              max value: 1
+              min value: -1
+              show caption: true
+              text size: 8
+              top: 30
+              update interval: 0.03999999910593033
+              width: 80
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: NDT Time [ms]
+              Show Value: true
+              Topic: /time_ndt_matching
+              Value: true
+              auto color change: false
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 1
+              foreground color: 0; 255; 255
+              height: 80
+              left: 140
+              linewidth: 1
+              max color: 255; 0; 0
+              max value: 1
+              min value: -1
+              show caption: true
+              text size: 8
+              top: 30
+              update interval: 0.03999999910593033
+              width: 80
+            - Align Bottom: false
+              Background Alpha: 0.8999999761581421
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 0; 255; 255
+              Invert Shadow: false
+              Name: NDT Monitor
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /ndt_monitor/ndt_info_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 50
+              left: 40
+              line width: 2
+              text size: 8
+              top: 150
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.8999999761581421
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 0; 255; 255
+              Invert Shadow: false
+              Name: Decision Maker Panel
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /decision_maker/state_overlay
+              Value: true
+              font: DejaVu Sans Mono
+              height: 200
+              left: 40
+              line width: 2
+              text size: 8
+              top: 220
+              width: 200
+          Enabled: true
+          Name: Monitor
+        - Class: rviz/Group
+          Displays:
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 25; 255; 240
+              Invert Shadow: false
+              Name: OK
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/ok_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 430
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 255; 0
+              Invert Shadow: false
+              Name: WARN
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/warn_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 520
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 85; 0
+              Invert Shadow: false
+              Name: ERROR
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/error_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 620
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 0; 0
+              Invert Shadow: false
+              Name: FATAL
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/fatal_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 720
+              width: 200
+          Enabled: true
+          Name: Health Checker
+      Enabled: true
+      Name: System
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.05000000074505806
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points Map
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_map
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /vector_map_center_lines_rviz
+          Name: Vector Map
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 23.83440399169922
+            Min Value: -22.532455444335938
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points Raw
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_raw
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.5
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 23.83440399169922
+            Min Value: -22.532455444335938
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 0; 0
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points No Ground
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_no_ground
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Sensing
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Axes Length: 1
+          Axes Radius: 0.10000000149011612
+          Class: rviz/Pose
+          Color: 255; 170; 255
+          Enabled: false
+          Head Length: 2
+          Head Radius: 2
+          Name: Current Pose
+          Shaft Length: 2
+          Shaft Radius: 1
+          Shape: Arrow
+          Topic: /current_pose
+          Unreliable: false
+          Value: false
+        - Alpha: 1
+          Axes Length: 1
+          Axes Radius: 0.10000000149011612
+          Class: rviz/Pose
+          Color: 255; 255; 0
+          Enabled: false
+          Head Length: 2
+          Head Radius: 1
+          Name: EKF Pose
+          Shaft Length: 2
+          Shaft Radius: 0.5
+          Shape: Arrow
+          Topic: /ekf_pose
+          Unreliable: false
+          Value: false
+        - Alpha: 1
+          Axes Length: 1
+          Axes Radius: 0.10000000149011612
+          Class: rviz/Pose
+          Color: 0; 255; 255
+          Enabled: false
+          Head Length: 2
+          Head Radius: 1
+          Name: NDT Pose
+          Shaft Length: 2
+          Shaft Radius: 0.5
+          Shape: Arrow
+          Topic: /ndt_pose
+          Unreliable: false
+          Value: false
+      Enabled: true
+      Name: Localization
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: true
+          Enabled: true
+          Name: Occupancy Grid Map
+          Topic: /semantics/costmap_generator/occupancy_grid
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 23.83440399169922
+            Min Value: -22.532455444335938
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Clustered Points
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 5
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_cluster
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /detection/lidar_detector/objects_markers_center
+          Name: Tracked Objects
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /prediction/motion_predictor/path_markers
+          Name: Predicted Path
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Perception
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /local_trajectories_eval_rviz
+          Name: Saved Waypoints
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /global_waypoints_mark
+          Name: Global Waypoints
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /local_waypoints_mark
+          Name: Local Waypoints
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /detection_range
+          Name: Detection Range
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Group
+          Displays:
+            - Class: rviz/Marker
+              Enabled: true
+              Marker Topic: /safety_border
+              Name: Safety Box
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+            - Class: jsk_rviz_plugin/BoundingBoxArray
+              Enabled: true
+              Name: Simulated Obstacle
+              Topic: /dp_planner_tracked_boxes
+              Unreliable: false
+              Value: true
+              alpha: 0.800000011920929
+              color: 25; 255; 0
+              coloring: Value
+              line width: 0.009999999776482582
+              only edge: false
+              show coords: false
+            - Class: rviz/MarkerArray
+              Enabled: true
+              Marker Topic: /AnimateGlobalPlan
+              Name: GlobalPathAnimation
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+            - Class: rviz/Marker
+              Enabled: true
+              Marker Topic: /behavior_state
+              Name: Behavior State
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+            - Class: rviz/MarkerArray
+              Enabled: true
+              Marker Topic: /detected_polygons
+              Name: Tracked Contours
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+            - Class: rviz/MarkerArray
+              Enabled: true
+              Marker Topic: /local_trajectories
+              Name: Local Rollouts
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+            - Class: rviz/MarkerArray
+              Enabled: true
+              Marker Topic: /global_waypoints_rviz
+              Name: Global Path
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+          Enabled: true
+          Name: OP Planner
+      Enabled: true
+      Name: Planning
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /trajectory_circle_mark
+          Name: Pure Pursuit Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /next_target_mark
+          Name: Pure Pursuit Target
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /mpc_follower/debug/predicted_traj
+          Name: MPC Predicted Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /mpc_follower/debug/filtered_traj
+          Name: MPC Reference Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Control
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: 0
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 4.608127117156982
+      Target Frame: map
+      Value: TopDownOrtho (rviz)
+      X: 59.5871696472168
+      Y: 8.258959770202637
+    Saved: ~
+Window Geometry:
+  DecisionMakerPanel:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1412
+  Hide Left Dock: false
+  Hide Right Dock: false
+  ImageViewerPlugin:
+    collapsed: false
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2711
+  X: 249
+  Y: 273
diff --git a/autoware.ai/autoware_files/rviz/cubetown_straight.rviz b/autoware.ai/autoware_files/rviz/cubetown_straight.rviz
new file mode 100644
index 00000000..34e7c4ed
--- /dev/null
+++ b/autoware.ai/autoware_files/rviz/cubetown_straight.rviz
@@ -0,0 +1,637 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1
+        - /System1/TF1
+        - /System1/TF1/Frames1
+        - /Map1/Vector Map1
+        - /Sensing1
+        - /Perception1
+        - /Perception1/Lidar Objects1
+        - /Planning1
+        - /Planning1/OP Planner1
+        - /Planning1/OP Planner1/GlobalPathAnimation1
+        - /Planning1/Next Traget1
+        - /Planning1/Behavior State1
+      Splitter Ratio: 0.22158685326576233
+    Tree Height: 1730
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Points Map
+  - Class: autoware_rviz_debug/DecisionMakerPanel
+    Name: DecisionMakerPanel
+  - Class: integrated_viewer/ImageViewerPlugin
+    Image topic: -----
+    Lane topic: -----
+    Name: ImageViewerPlugin
+    Point size: 3
+    Point topic: -----
+    Rect topic: /detection/image_detector/objects
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 10
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 20
+      Reference Frame: map
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/TF
+          Enabled: true
+          Frame Timeout: 15
+          Frames:
+            All Enabled: false
+            base_link:
+              Value: true
+            camera:
+              Value: false
+            map:
+              Value: true
+            mobility:
+              Value: false
+            velodyne:
+              Value: true
+            world:
+              Value: false
+          Marker Scale: 30
+          Name: TF
+          Show Arrows: true
+          Show Axes: true
+          Show Names: true
+          Tree:
+            world:
+              map:
+                base_link:
+                  velodyne:
+                    camera:
+                      {}
+                mobility:
+                  {}
+          Update Interval: 0
+          Value: true
+        - Alpha: 1
+          Class: rviz/RobotModel
+          Collision Enabled: false
+          Enabled: true
+          Links:
+            All Links Enabled: true
+            Expand Joint Details: false
+            Expand Link Details: false
+            Expand Tree: false
+            Link Tree Style: Links in Alphabetic Order
+          Name: Vehicle Model
+          Robot Description: robot_description
+          TF Prefix: ""
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+        - Class: rviz/Group
+          Displays:
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: Velocity (km/h)
+              Show Value: true
+              Topic: /linear_velocity_viz
+              Value: true
+              auto color change: false
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 1
+              foreground color: 0; 255; 255
+              height: 80
+              left: 40
+              linewidth: 1
+              max color: 255; 0; 0
+              max value: 1
+              min value: -1
+              show caption: true
+              text size: 8
+              top: 30
+              update interval: 0.03999999910593033
+              width: 80
+            - Buffer length: 100
+              Class: jsk_rviz_plugin/Plotter2D
+              Enabled: true
+              Name: NDT Time [ms]
+              Show Value: true
+              Topic: /time_ndt_matching
+              Value: true
+              auto color change: false
+              auto scale: true
+              background color: 0; 0; 0
+              backround alpha: 0
+              border: true
+              foreground alpha: 1
+              foreground color: 0; 255; 255
+              height: 80
+              left: 140
+              linewidth: 1
+              max color: 255; 0; 0
+              max value: 1
+              min value: -1
+              show caption: true
+              text size: 8
+              top: 30
+              update interval: 0.03999999910593033
+              width: 80
+            - Align Bottom: false
+              Background Alpha: 0.8999999761581421
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 0; 255; 255
+              Invert Shadow: false
+              Name: NDT Monitor
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /ndt_monitor/ndt_info_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 50
+              left: 40
+              line width: 2
+              text size: 8
+              top: 150
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.8999999761581421
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 0; 255; 255
+              Invert Shadow: false
+              Name: Decision Maker Panel
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /decision_maker/state_overlay
+              Value: true
+              font: DejaVu Sans Mono
+              height: 200
+              left: 40
+              line width: 2
+              text size: 8
+              top: 220
+              width: 200
+          Enabled: true
+          Name: Monitor
+        - Class: rviz/Group
+          Displays:
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 25; 255; 240
+              Invert Shadow: false
+              Name: OK
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/ok_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 430
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 255; 0
+              Invert Shadow: false
+              Name: WARN
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/warn_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 520
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 85; 0
+              Invert Shadow: false
+              Name: ERROR
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/error_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 620
+              width: 200
+            - Align Bottom: false
+              Background Alpha: 0.800000011920929
+              Background Color: 0; 0; 0
+              Class: jsk_rviz_plugin/OverlayText
+              Enabled: true
+              Foreground Alpha: 0.800000011920929
+              Foreground Color: 255; 0; 0
+              Invert Shadow: false
+              Name: FATAL
+              Overtake BG Color Properties: false
+              Overtake FG Color Properties: false
+              Overtake Position Properties: true
+              Topic: /health_aggregator/fatal_text
+              Value: true
+              font: DejaVu Sans Mono
+              height: 80
+              left: 40
+              line width: 2
+              text size: 6
+              top: 720
+              width: 200
+          Enabled: true
+          Name: Health Checker
+      Enabled: true
+      Name: System
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.05000000074505806
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points Map
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_map
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /vector_map_center_lines_rviz
+          Name: Vector Map
+          Namespaces:
+            road_network_vector_map: true
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.30000001192092896
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 23.83440399169922
+            Min Value: -22.532455444335938
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points Raw
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 5
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /filtered_points
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Points No Ground
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 5
+          Size (m): 5
+          Style: Points
+          Topic: /debug_points_no_ground
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+      Enabled: false
+      Name: Sensing
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 23.83440399169922
+            Min Value: -22.532455444335938
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: Clustered Points
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 10
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /points_cluster_center
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /detection/lidar_detector/objects_markers_center
+          Name: Lidar Objects
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Perception
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Group
+          Displays:
+            - Class: rviz/MarkerArray
+              Enabled: true
+              Marker Topic: /local_trajectories_eval_rviz
+              Name: Local Rollouts
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+            - Class: rviz/Marker
+              Enabled: true
+              Marker Topic: /safety_border
+              Name: Safety Box
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: true
+            - Class: jsk_rviz_plugin/BoundingBoxArray
+              Enabled: false
+              Name: Simulated Obstacle
+              Topic: /dp_planner_tracked_boxes
+              Unreliable: false
+              Value: false
+              alpha: 0.800000011920929
+              color: 25; 255; 0
+              coloring: Value
+              line width: 0.009999999776482582
+              only edge: false
+              show coords: false
+            - Class: rviz/MarkerArray
+              Enabled: true
+              Marker Topic: /global_waypoints_rviz
+              Name: GlobalPathAnimation
+              Namespaces:
+                global_lane_array_marker: true
+                global_lane_waypoint_orientation_marker: true
+              Queue Size: 100
+              Value: true
+            - Class: rviz/Marker
+              Enabled: false
+              Marker Topic: /behavior_state
+              Name: Behavior State
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+            - Class: rviz/MarkerArray
+              Enabled: false
+              Marker Topic: /detected_polygons
+              Name: Tracked Contours
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+            - Class: rviz/MarkerArray
+              Enabled: false
+              Marker Topic: /global_waypoints_rviz
+              Name: Global Path
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+          Enabled: true
+          Name: OP Planner
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /next_target_mark
+          Name: Next Traget
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /behavior_state
+          Name: Behavior State
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+      Enabled: true
+      Name: Planning
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /trajectory_circle_mark
+          Name: Pure Pursuit Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /next_target_mark
+          Name: Pure Pursuit Target
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /mpc_follower/debug/predicted_traj
+          Name: MPC Predicted Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+        - Class: rviz/Marker
+          Enabled: true
+          Marker Topic: /mpc_follower/debug/filtered_traj
+          Name: MPC Reference Trajectory
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
+      Enabled: false
+      Name: Control
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: 1.580001711845398
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 11.122644424438477
+      Target Frame: map
+      Value: TopDownOrtho (rviz)
+      X: -3.7662081718444824
+      Y: 3.050819158554077
+    Saved: ~
+Window Geometry:
+  DecisionMakerPanel:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 2104
+  Hide Left Dock: false
+  Hide Right Dock: false
+  ImageViewerPlugin:
+    collapsed: false
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 3840
+  X: 3840
+  Y: 41
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv
new file mode 100644
index 00000000..3570fabd
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv
@@ -0,0 +1,539 @@
+DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW
+1001,0,1001,1.5706159461937805,0,90000000000,0,0,2,2
+1002,1,1002,1.5706159461937805,0,90000000000,0,0,2,2
+1003,2,1003,1.5710363578994468,0,2378.6207342040943,0,0,2,2
+1004,3,1004,1.5710077368022535,0,-34939.261525991235,0,0,2,2
+1005,4,1005,1.5711231434926902,0,8665.008902131893,0,0,2,2
+1006,5,1006,1.5690903372401281,0,-491.93079701502967,0,0,2,2
+1007,6,1007,1.5689557145684498,0,-7428.169323438051,0,0,2,2
+1008,7,1008,1.5686363058159303,0,-3130.7845890626863,0,0,2,2
+1009,8,1009,1.5682534958301988,0,-2612.2620549962444,0,0,2,2
+1010,9,1010,1.567912565854563,0,-2933.153642870459,0,0,2,2
+1011,10,1011,1.569028273497871,0,896.2921478559839,0,0,2,2
+1012,11,1012,1.5683676551986594,0,-1513.7334239654833,0,0,2,2
+1013,12,1013,1.5684754110339634,0,9280.239879158233,0,0,2,2
+1014,13,1014,1.5684588800570125,0,-60492.49254730128,0,0,2,2
+1015,14,1015,1.5693912035006823,0,1072.5891393053826,0,0,2,2
+1016,15,1016,1.5515668174636283,0,-56.102914171695225,0,0,2,2
+1017,16,1017,1.5593862076728497,0,127.88721028664114,0,0,2,2
+1018,17,1018,1.559631749229936,0,4072.6303598717514,0,0,2,2
+1019,18,1019,1.5603462402901191,0,1399.5976377140162,0,0,2,2
+1020,19,1020,1.5602731051708514,0,-13673.32151792592,0,0,2,2
+1021,20,1021,1.5605402761532297,0,3742.9214471508494,0,0,2,2
+1022,21,1022,1.5596838425998092,0,-1167.6329074288967,0,0,2,2
+1023,22,1023,1.5604369592339504,0,1327.8155795088473,0,0,2,2
+1024,23,1024,1.560366573502377,0,-14207.424965920718,0,0,2,2
+1025,24,1025,1.56027352852355,0,-10747.490220393849,0,0,2,2
+1026,25,1026,1.5603167873838384,0,23116.65155611479,0,0,2,2
+1027,26,1027,1.5602126432082224,0,-9602.073222867975,0,0,2,2
+1028,27,1028,1.5603395866674272,0,7877.522845713829,0,0,2,2
+1029,28,1029,1.5604457063862052,0,9423.319355873213,0,0,2,2
+1030,29,1030,1.560246370088169,0,-5016.647794967425,0,0,2,2
+1031,30,1031,1.559660768578082,0,-1707.6458697164428,0,0,2,2
+1032,31,1032,1.5592028183805353,0,-2183.6435607133826,0,0,2,2
+1033,32,1033,1.5581253798288566,0,-928.1271757372258,0,0,2,2
+1034,33,1034,1.5566612631984011,0,-683.0056972230019,0,0,2,2
+1035,34,1035,1.5555975918432194,0,-940.1400114128027,0,0,2,2
+1036,35,1036,1.5541259437505452,0,-679.5102748938151,0,0,2,2
+1037,36,1037,1.552885481318075,0,-806.1509754943779,0,0,2,2
+1038,37,1038,1.5531588912852945,0,3657.5111367365535,0,0,2,2
+1039,38,1039,1.5533266894392792,0,5959.541128748756,0,0,2,2
+1040,39,1040,1.5541906323996943,0,1157.4838222184007,0,0,2,2
+1041,40,1041,1.5539019306491981,0,-3463.782253766011,0,0,2,2
+1042,41,1042,1.5537746751005848,0,-7858.203519582626,0,0,2,2
+1043,42,1043,1.553458073462205,0,-3158.543351568176,0,0,2,2
+1044,43,1044,1.5542071389727117,0,1334.996720545278,0,0,2,2
+1045,44,1045,1.5542686707312687,0,16251.770198844073,0,0,2,2
+1046,45,1046,1.5547869612148626,0,1929.4199520429686,0,0,2,2
+1047,46,1047,1.5555746441573521,0,1269.5463441666138,0,0,2,2
+1048,47,1048,1.5564501087977414,0,1142.250587705515,0,0,2,2
+1049,48,1049,1.5576003889319332,0,869.3534472822894,0,0,2,2
+1050,49,1050,1.5580756995278244,0,2103.887455159607,0,0,2,2
+1051,50,1051,1.5585502233487594,0,2107.375764676554,0,0,2,2
+1052,51,1052,1.5584516260756238,0,-10142.26832242346,0,0,2,2
+1053,52,1053,1.5584112331616264,0,-24756.817496873653,0,0,2,2
+1054,53,1054,1.5584451473870529,0,29486.151826450732,0,0,2,2
+1055,54,1055,1.5584130588770102,0,-31163.802827570937,0,0,2,2
+1056,55,1056,1.558453395681946,0,24791.254577367206,0,0,2,2
+1057,56,1057,1.5584612741910804,0,126927.56750530448,0,0,2,2
+1058,57,1058,1.558466983527938,0,175151.69010325428,0,0,2,2
+1059,58,1059,1.5584496873239995,0,-57816.15454740148,0,0,2,2
+1060,59,1060,1.5584670021217941,0,57754.0674664271,0,0,2,2
+1061,60,1061,1.5584616988846198,0,-188564.07268518096,0,0,2,2
+1062,61,1062,1.5583642287230526,0,-10259.550039937772,0,0,2,2
+1063,62,1063,1.5584039512178554,0,25174.652422137453,0,0,2,2
+1064,63,1064,1.5584568342185832,0,18909.668253263793,0,0,2,2
+1065,64,1065,1.5584406089211233,0,-61632.15204384786,0,0,2,2
+1066,65,1066,1.5584747613894863,0,29280.46047429793,0,0,2,2
+1067,66,1067,1.5585924861497782,0,8494.389774251948,0,0,2,2
+1068,67,1068,1.5585615744747112,0,-32350.236531457223,0,0,2,2
+1069,68,1069,1.5584424421160337,0,-8394.025024776187,0,0,2,2
+1070,69,1070,1.5584736998776192,0,31992.054109950375,0,0,2,2
+1071,70,1071,1.5584616247474912,0,-82814.84252327424,0,0,2,2
+1072,71,1072,1.55846993294119,0,120363.10614007954,0,0,2,2
+1073,72,1073,1.558246954101369,0,-4484.730482960295,0,0,2,2
+1074,73,1074,1.558474293779886,0,4398.704205631745,0,0,2,2
+1075,74,1075,1.5589662998091534,0,2032.495417768197,0,0,2,2
+1076,75,1076,1.5592385963707622,0,3672.466497892804,0,0,2,2
+1077,76,1077,1.559661432256271,0,2364.9837543870162,0,0,2,2
+1078,77,1078,1.5592922234671693,0,-2708.4945687052523,0,0,2,2
+1079,78,1079,1.5593257540364545,0,29823.531819475797,0,0,2,2
+1080,79,1080,1.5593587691302953,0,30289.17636346063,0,0,2,2
+1081,80,1081,1.5593124961590616,0,-21610.888026868568,0,0,2,2
+1082,81,1082,1.5594323344706573,0,8344.576844289204,0,0,2,2
+1083,82,1083,1.55933102745173,0,-9870.984366025314,0,0,2,2
+1084,83,1084,1.559328555630133,0,-404559.94123097276,0,0,2,2
+1085,84,1085,1.5592803470653718,0,-20743.202062780034,0,0,2,2
+1086,85,1086,1.5593560395197574,0,13211.356509923131,0,0,2,2
+1087,86,1087,1.559392786498505,0,27213.12157025686,0,0,2,2
+1088,87,1088,1.5594015836176165,0,113673.57737659848,0,0,2,2
+1089,88,1089,1.5593621175983554,0,-25338.253482943648,0,0,2,2
+1090,89,1090,1.5593503574191032,0,-85032.7174913827,0,0,2,2
+1091,90,1091,1.559344540912574,0,-171924.50398892158,0,0,2,2
+1092,91,1092,1.5593063957840103,0,-26215.6673120881,0,0,2,2
+1093,92,1093,1.5593010517229873,0,-187123.61174189628,0,0,2,2
+1094,93,1094,1.5593102443018543,0,108783.40174917986,0,0,2,2
+1095,94,1095,1.5592708891791727,0,-25409.65271764711,0,0,2,2
+1096,95,1096,1.55934742287295,0,13066.140553846302,0,0,2,2
+1097,96,1097,1.5593046241271993,0,-23365.170695042812,0,0,2,2
+1098,97,1098,1.5593959499423824,0,10949.806448427382,0,0,2,2
+1099,98,1099,1.5659936909281864,0,151.56702910157335,0,0,2,2
+1100,99,1100,1.5631484014833397,0,-351.4580921850161,0,0,2,2
+1101,100,1101,1.5628303735240179,0,-3144.3776268368265,0,0,2,2
+1102,101,1102,1.5623443678921973,0,-2057.58932515669,0,0,2,2
+1103,102,1103,1.5621970301683075,0,-6787.128059257383,0,0,2,2
+1104,103,1104,1.561518196533237,0,-1473.1149847873494,0,0,2,2
+1105,104,1105,1.56150959557461,0,-116266.1097887333,0,0,2,2
+1106,105,1106,1.5614280308212418,0,-12260.197679807654,0,0,2,2
+1107,106,1107,1.5615331544577284,0,9512.608519086212,0,0,2,2
+1108,107,1108,1.561521769820582,0,-87837.66993589127,0,0,2,2
+1109,108,1109,1.5615063516353334,0,-64858.476134930985,0,0,2,2
+1110,109,1110,1.5613401362473043,0,-6016.290139304405,0,0,2,2
+1111,110,1111,1.5613462370583207,0,163912.63347054776,0,0,2,2
+1112,111,1112,1.5616346082905865,0,3467.752286323709,0,0,2,2
+1113,112,1113,1.5614798702693682,0,-6462.535788724596,0,0,2,2
+1114,113,1114,1.5611376896383071,0,-2922.4330930103124,0,0,2,2
+1115,114,1115,1.5617762833238056,0,1565.9409460327606,0,0,2,2
+1116,115,1116,1.561901748643473,0,7970.329989601685,0,0,2,2
+1117,116,1117,1.5619377171717277,0,27802.082779656434,0,0,2,2
+1118,117,1118,1.5617090872820676,0,-4373.881304350305,0,0,2,2
+1119,118,1119,1.5617712745468253,0,16080.462839074564,0,0,2,2
+1120,119,1120,1.5620671743245305,0,3379.522647010559,0,0,2,2
+1121,120,1121,1.562234529532306,0,5975.314501965154,0,0,2,2
+1122,121,1122,1.5612933601741283,0,-1062.5080293055378,0,0,2,2
+1123,122,1123,1.561698322658157,0,2469.364544715466,0,0,2,2
+1124,123,1124,1.562024360239134,0,3067.1310865553396,0,0,2,2
+1125,124,1125,1.5617193763911459,0,-3278.8621646585816,0,0,2,2
+1126,125,1126,1.5617870168027121,0,14784.061433753894,0,0,2,2
+1127,126,1127,1.5625182243896565,0,1367.6006894005648,0,0,2,2
+1128,127,1128,1.5622360356231595,0,-3543.727173883281,0,0,2,2
+1129,128,1129,1.5625603701776563,0,3083.2360787200023,0,0,2,2
+1130,129,1130,1.562392918909711,0,-5971.886700355639,0,0,2,2
+1131,130,1131,1.562318653263805,0,-13465.176095886947,0,0,2,2
+1132,131,1132,1.562526849807251,0,4803.153709699076,0,0,2,2
+1133,132,1133,1.576137435117669,0,73.47222600592939,0,0,2,2
+1134,133,1134,1.5918120181402755,0,63.79755037551902,0,0,2,2
+1135,134,1135,1.6220769767862286,0,33.04151218900524,0,0,2,2
+1136,135,1136,1.7186597237164492,0,10.353816098464087,0,0,2,2
+1137,136,1137,1.675493482422447,0,-23.166251450736056,0,0,2,2
+1138,137,1138,1.677164571573219,0,598.4121191487482,0,0,2,2
+1139,138,1139,1.6768159724168918,0,-2868.624269019426,0,0,2,2
+1140,139,1140,1.6768476714325655,0,31546.72089172431,0,0,2,2
+1141,140,1141,1.6905273580959745,0,73.10108956478079,0,0,2,2
+1142,141,1142,1.7442418197685423,0,18.616960290801998,0,0,2,2
+1143,142,1143,1.7225513490834752,0,-46.10319501680768,0,0,2,2
+1144,143,1144,1.7485007254879514,0,38.536571531156355,0,0,2,2
+1145,144,1145,1.8148661989890704,0,15.068076022740032,0,0,2,2
+1146,145,1146,1.8169266574025538,0,485.32889256882964,0,0,2,2
+1147,146,1147,1.8036604558276768,0,-75.37952701500924,0,0,2,2
+1148,147,1148,1.8036313187476025,0,-34320.528942918514,0,0,2,2
+1149,148,1149,1.8030076566400433,0,-1603.4323520368343,0,0,2,2
+1150,149,1150,1.8316436098928863,0,34.921135370295964,0,0,2,2
+1151,150,1151,1.876228798227453,0,22.428973328451868,0,0,2,2
+1152,151,1152,1.9324085263911028,0,17.80001492864172,0,0,2,2
+1153,152,1153,1.9158996090297133,0,-60.57332398663326,0,0,2,2
+1154,153,1154,1.9039984847392084,0,-84.02567485139441,0,0,2,2
+1155,154,1155,1.9849339713438288,0,12.355519710224527,0,0,2,2
+1156,155,1156,1.954096844071843,0,-32.42844222096056,0,0,2,2
+1157,156,1157,1.953953083593038,0,-6956.014673251483,0,0,2,2
+1158,157,1158,2.0180868363442888,0,15.592413621554357,0,0,2,2
+1159,158,1159,2.0020751553324976,0,-62.45440433540919,0,0,2,2
+1160,159,1160,2.019090426427932,0,58.770735675691235,0,0,2,2
+1161,160,1161,2.008152822027425,0,-91.42769873389011,0,0,2,2
+1162,161,1162,2.045952035598901,0,26.455576862970116,0,0,2,2
+1163,162,1163,2.083699401528057,0,26.491914743846912,0,0,2,2
+1164,163,1164,2.120776070766995,0,26.971139008080037,0,0,2,2
+1165,164,1165,2.1655019951470065,0,22.35839759293862,0,0,2,2
+1166,165,1166,2.160816734657125,0,-213.43530464520524,0,0,2,2
+1167,166,1167,2.2342858263354426,0,13.611165963211747,0,0,2,2
+1168,167,1168,2.302663449689519,0,14.624667412345591,0,0,2,2
+1169,168,1169,2.2492885285815243,0,-18.735390689883726,0,0,2,2
+1170,169,1170,2.281647456184398,0,30.903372703587333,0,0,2,2
+1171,170,1171,2.3457521763460143,0,15.599475319116399,0,0,2,2
+1172,171,1172,2.474948546053671,0,7.740155565228202,0,0,2,2
+1173,172,1173,2.5173224404688357,0,23.599435780019412,0,0,2,2
+1174,173,1174,2.5368325471829207,0,51.255485920949084,0,0,2,2
+1175,174,1175,2.5456673035159536,0,113.18931301602866,0,0,2,2
+1176,175,1176,2.5547417551394753,0,110.19949650818774,0,0,2,2
+1177,176,1177,2.603195128490749,0,20.638397924335887,0,0,2,2
+1178,177,1178,2.596074157385382,0,-140.43028474673574,0,0,2,2
+1179,178,1179,2.6531475137908123,0,17.521310520031932,0,0,2,2
+1180,179,1180,2.809796039587864,0,6.383717911878494,0,0,2,2
+1181,180,1181,2.7654462594739497,0,-22.54802611042167,0,0,2,2
+1182,181,1182,2.771217664518214,0,173.26803305788565,0,0,2,2
+1183,182,1183,2.8449752803934008,0,13.557921960116037,0,0,2,2
+1184,183,1184,2.8084961628169074,0,-27.4129438000547,0,0,2,2
+1185,184,1185,2.869009574824431,0,16.52526219932323,0,0,2,2
+1186,185,1186,2.884302571393505,0,65.38940851018249,0,0,2,2
+1187,186,1187,2.9905069048410624,0,9.415811648531163,0,0,2,2
+1188,187,1188,2.969341452422632,0,-47.24680485115514,0,0,2,2
+1189,188,1189,3.0383640323914287,0,14.488012480148827,0,0,2,2
+1190,189,1190,3.055186812134341,0,59.44320827366907,0,0,2,2
+1191,190,1191,3.066040854811578,0,92.13157067248235,0,0,2,2
+1192,191,1192,3.0813008039480883,0,65.53101789883758,0,0,2,2
+1193,192,1193,3.0910158947236637,0,102.9326460349796,0,0,2,2
+1194,193,1194,3.10475182392835,0,72.80177300701585,0,0,2,2
+1195,194,1195,3.0928347571304533,0,-83.91326632292699,0,0,2,2
+1196,195,1196,3.093569046175925,0,1361.8615260122626,0,0,2,2
+1197,196,1197,3.093684135325397,0,8688.916414664875,0,0,2,2
+1198,197,1198,3.1063991293121975,0,78.64730420148996,0,0,2,2
+1199,198,1199,3.1272933523549296,0,47.86011894076368,0,0,2,2
+1200,199,1200,3.142608033171954,0,65.29682282952635,0,0,2,2
+1201,200,1201,3.1296736144563315,0,-77.31309941220373,0,0,2,2
+1202,201,1202,3.1295315835431703,0,-7040.7207680544525,0,0,2,2
+1203,202,1203,3.129958005160909,0,2345.0968675165896,0,0,2,2
+1204,203,1204,3.13035122004071,0,2543.1387553419513,0,0,2,2
+1205,204,1205,3.130323608536339,0,-36216.78799352977,0,0,2,2
+1206,205,1206,3.1305842969634368,0,3835.996906852997,0,0,2,2
+1207,206,1207,3.130267219147295,0,-3153.799947810142,0,0,2,2
+1208,207,1208,3.1302595648893865,0,-130646.23794614259,0,0,2,2
+1209,208,1209,3.1301063347354496,0,-6526.130623167103,0,0,2,2
+1210,209,1210,3.130327684273659,0,4517.741523609919,0,0,2,2
+1211,210,1211,3.1304550560171522,0,7851.034872993834,0,0,2,2
+1212,211,1212,3.130259118536645,0,-5103.668769301195,0,0,2,2
+1213,212,1213,3.130053746211815,0,-4869.205238964958,0,0,2,2
+1214,213,1214,3.1300164498391663,0,-26812.258913566046,0,0,2,2
+1215,214,1215,3.129816693563907,0,-5006.100552799646,0,0,2,2
+1216,215,1216,3.129775533359499,0,-24295.31180383525,0,0,2,2
+1217,216,1217,3.1297243109074175,0,-19522.68896470701,0,0,2,2
+1218,217,1218,3.1297590645461977,0,28773.965406220937,0,0,2,2
+1219,218,1219,3.1296879225255916,0,-14056.390182348943,0,0,2,2
+1220,219,1220,3.1297771537246764,0,11206.842564672676,0,0,2,2
+1221,220,1221,3.1295961985796756,0,-5526.231376267536,0,0,2,2
+1222,221,1222,3.129895503753974,0,3341.0715412600025,0,0,2,2
+1223,222,1223,3.1304526609293504,0,1794.8256689405623,0,0,2,2
+1224,223,1224,3.1308640539574997,0,2430.765549184491,0,0,2,2
+1225,224,1225,3.1314005324764267,0,1864.0075319325813,0,0,2,2
+1226,225,1226,3.131838500950461,0,2283.2693659181946,0,0,2,2
+1227,226,1227,3.132310552198498,0,2118.414058130501,0,0,2,2
+1228,227,1228,3.13283405273258,0,1910.2177264318536,0,0,2,2
+1229,228,1229,3.132860414377954,0,37933.89926183089,0,0,2,2
+1230,229,1230,3.1327026639426103,0,-6339.126721396533,0,0,2,2
+1231,230,1231,3.1322941179223944,0,-2447.7046661023764,0,0,2,2
+1232,231,1232,3.132958998066535,0,1504.0304764890313,0,0,2,2
+1233,232,1233,3.1329847186775854,0,38879.32514679646,0,0,2,2
+1234,233,1234,3.132205186132164,0,-1282.820077074694,0,0,2,2
+1235,234,1235,3.1322812556392696,0,13145.871953777567,0,0,2,2
+1236,235,1236,3.1326404704421495,0,2783.8496408909646,0,0,2,2
+1237,236,1237,3.1324687366627106,0,-5822.966240347186,0,0,2,2
+1238,237,1238,3.1327176882712133,0,4016.844904174198,0,0,2,2
+1239,238,1239,3.1325963662922494,0,-8242.529577406198,0,0,2,2
+1240,239,1240,3.1326772233996776,0,12367.496585104844,0,0,2,2
+1241,240,1241,3.1324966080699,0,-5536.628597534322,0,0,2,2
+1242,241,1242,3.1327668588296307,0,3700.2671185690515,0,0,2,2
+1243,242,1243,3.132810249389559,0,23046.487569068846,0,0,2,2
+1244,243,1244,3.1324986288528094,0,-3209.031119806755,0,0,2,2
+1245,244,1245,3.132822786442226,0,3084.9192881761173,0,0,2,2
+1246,245,1246,3.1325701209629964,0,-3957.8022413229055,0,0,2,2
+1247,246,1247,3.132860171494029,0,3447.6751221216164,0,0,2,2
+1248,247,1248,3.1330470784909092,0,5350.254493905768,0,0,2,2
+1249,248,1249,3.13374161105399,0,1439.8173003782692,0,0,2,2
+1250,249,1250,3.134233784801914,0,2031.8028017911413,0,0,2,2
+1251,250,1251,3.133616456325006,0,-1619.8831536311138,0,0,2,2
+1252,251,1252,3.134066252385871,0,2223.2297856879172,0,0,2,2
+1253,252,1253,3.134784454988094,0,1392.3647685274047,0,0,2,2
+1254,253,1254,3.1345994496722183,0,-5405.250088441288,0,0,2,2
+1255,254,1255,3.1362019499703973,0,624.024844885431,0,0,2,2
+1256,255,1256,3.136262484925358,0,16519.38125086305,0,0,2,2
+1257,256,1257,3.1371188806098034,0,1167.6845390079225,0,0,2,2
+1258,257,1258,3.1369988499566253,0,-8331.205184029783,0,0,2,2
+1259,258,1259,3.137867554501641,0,1151.1393669315573,0,0,2,2
+1260,259,1260,3.1708550527699093,0,30.314514664542813,0,0,2,2
+1261,260,1261,3.2356533213820318,0,15.432511105905075,0,0,2,2
+1262,261,1262,3.2018151731438587,0,-29.55244456527002,0,0,2,2
+1263,262,1263,3.2823370968133814,0,12.418978017765573,0,0,2,2
+1264,263,1264,3.36429786440126,0,12.20095942766515,0,0,2,2
+1265,264,1265,3.4485758303486875,0,11.86549756817575,0,0,2,2
+1266,265,1266,3.4777834180506386,0,34.23767858559573,0,0,2,2
+1267,266,1267,3.5907094795636847,0,8.855351781523634,0,0,2,2
+1268,267,1268,3.7473026705560817,0,6.385973704620098,0,0,2,2
+1269,268,1269,3.757926075093773,0,94.13178199625767,0,0,2,2
+1270,269,1270,3.8646434248411556,0,9.370547547958806,0,0,2,2
+1271,270,1271,4.046477554099296,0,5.499517632250184,0,0,2,2
+1272,271,1272,4.26175711340075,0,4.645122849771844,0,0,2,2
+1273,272,1273,4.378686232163503,0,8.552189656273617,0,0,2,2
+1274,273,1274,4.4388107350157435,0,16.63215415614419,0,0,2,2
+1275,274,1275,4.567287183832628,0,7.783527714291725,0,0,2,2
+1276,275,1276,4.619480219254632,0,19.15964442218305,0,0,2,2
+1277,276,1277,4.709966137584271,0,11.051443345659726,0,0,2,2
+1278,277,1278,4.703095070861943,0,-145.53780954425937,0,0,2,2
+1279,278,1279,4.659772006824833,0,-23.082393229237343,0,0,2,2
+1280,279,1280,4.665973477910038,0,161.25206201247974,0,0,2,2
+1281,280,1281,4.636774477273602,0,-34.247747464074145,0,0,2,2
+1282,281,1282,4.652195224894353,0,64.84769899575784,0,0,2,2
+1283,282,1283,4.651902783891486,0,-3419.4931291989383,0,0,2,2
+1284,283,1284,4.687824121303961,0,27.838607135287646,0,0,2,2
+1285,284,1285,4.6736324526187385,0,-70.46387723533208,0,0,2,2
+1286,285,1286,4.684287629155659,0,93.85109636944345,0,0,2,2
+1287,286,1287,4.678651899887421,0,-177.43932548991228,0,0,2,2
+1288,287,1288,4.679214309388232,0,1778.0638459293787,0,0,2,2
+1289,288,1289,4.680302077949644,0,919.3132027110769,0,0,2,2
+1290,289,1290,4.6973518372767735,0,58.65185430558062,0,0,2,2
+1291,290,1291,4.690461092985631,0,-145.12220418415166,0,0,2,2
+1292,291,1292,4.690476707440746,0,64043.22101537215,0,0,2,2
+1293,292,1293,4.690302884709805,0,-5752.987509656575,0,0,2,2
+1294,293,1294,4.689835859312202,0,-2141.2111742384086,0,0,2,2
+1295,294,1295,4.689787908948702,0,-20854.899254200915,0,0,2,2
+1296,295,1296,4.689607694324616,0,-5548.939244371828,0,0,2,2
+1297,296,1297,4.689493209900328,0,-8734.812671833417,0,0,2,2
+1298,297,1298,4.689519331655459,0,38282.26682969883,0,0,2,2
+1299,298,1299,4.689388854307946,0,-7664.165612349602,0,0,2,2
+1300,299,1300,4.68987371253164,0,2062.4585726961486,0,0,2,2
+1301,300,1301,4.689776043791079,0,-10238.690437254516,0,0,2,2
+1302,301,1302,4.689782693126787,0,150390.96293843692,0,0,2,2
+1303,302,1303,4.689816670283746,0,29431.538406950272,0,0,2,2
+1304,303,1304,4.689796642458525,0,-49930.533592485204,0,0,2,2
+1305,304,1305,4.689747163960407,0,-20210.79939837668,0,0,2,2
+1306,305,1306,4.689876308889746,0,7743.238585658182,0,0,2,2
+1307,306,1307,4.689927338270111,0,19596.55384472955,0,0,2,2
+1308,307,1308,4.689980639334232,0,18761.351513307854,0,0,2,2
+1309,308,1309,4.6900762065868395,0,10463.835390423541,0,0,2,2
+1310,309,1310,4.6899051812441055,0,-5847.086659870083,0,0,2,2
+1311,310,1311,4.689963031378174,0,17286.044640952976,0,0,2,2
+1312,311,1312,4.690015400141843,0,19095.35245718582,0,0,2,2
+1313,312,1313,4.6899442234759725,0,-14049.548230019285,0,0,2,2
+1314,313,1314,4.689947326133968,0,322304.2956969203,0,0,2,2
+1315,314,1315,4.689964483759958,0,58283.12148612662,0,0,2,2
+1316,315,1316,4.689997295188629,0,30477.18555715479,0,0,2,2
+1317,316,1317,4.6899609756369145,0,-27533.379482733104,0,0,2,2
+1318,317,1318,4.6898658552838945,0,-10512.997147837103,0,0,2,2
+1319,318,1319,4.6899787594934255,0,8857.06568563471,0,0,2,2
+1320,319,1320,4.689935576462696,0,-23157.24448047331,0,0,2,2
+1321,320,1321,4.6899566101192764,0,47542.850963445235,0,0,2,2
+1322,321,1322,4.689960611514752,0,249912.81320388868,0,0,2,2
+1323,322,1323,4.690077962834115,0,8521.421023858762,0,0,2,2
+1324,323,1324,4.690117658235953,0,25191.834663670186,0,0,2,2
+1325,324,1325,4.690256418250002,0,7206.687076595954,0,0,2,2
+1326,325,1326,4.690478577472271,0,4501.276110846042,0,0,2,2
+1327,326,1327,4.690415949147022,0,-15967.216048522549,0,0,2,2
+1328,327,1328,4.690444340102213,0,35222.48523389742,0,0,2,2
+1329,328,1329,4.684591006921866,0,-170.84282906660815,0,0,2,2
+1330,329,1330,4.687177878574332,0,386.567303811469,0,0,2,2
+1331,330,1331,4.686995455840435,0,-5481.772905385729,0,0,2,2
+1332,331,1332,4.68699624737735,0,1263364.956066549,0,0,2,2
+1333,332,1333,4.68706285354148,0,15013.625436374265,0,0,2,2
+1334,333,1334,4.686911557450675,0,-6609.556100751335,0,0,2,2
+1335,334,1335,4.686865680707907,0,-21797.53704511855,0,0,2,2
+1336,335,1336,4.686820773354471,0,-22268.067999775332,0,0,2,2
+1337,336,1337,4.686725954049043,0,-10546.37550326547,0,0,2,2
+1338,337,1338,4.68673837550253,0,80505.8764747865,0,0,2,2
+1339,338,1339,4.686702762127034,0,-28079.337779137055,0,0,2,2
+1340,339,1340,4.686710912254904,0,122697.46142708715,0,0,2,2
+1341,340,1341,4.689136663573433,0,412.2434119118499,0,0,2,2
+1342,341,1342,4.714259546731511,0,39.80434863736831,0,0,2,2
+1343,342,1343,4.699984756063116,0,-70.05356668480162,0,0,2,2
+1344,343,1344,4.6832033701032545,0,-59.58983378320814,0,0,2,2
+1345,344,1345,4.68680269049533,0,277.83022656209147,0,0,2,2
+1346,345,1346,4.687649092174702,0,1181.4721359512878,0,0,2,2
+1347,346,1347,4.6876939836328555,0,22275.952734298215,0,0,2,2
+1348,347,1348,4.687761437827265,0,14824.87499482366,0,0,2,2
+1349,348,1349,4.687696404440299,0,-15376.717200942252,0,0,2,2
+1350,349,1350,4.687715356450129,0,52764.85232825735,0,0,2,2
+1351,350,1351,4.687582616571676,0,-7533.531080922668,0,0,2,2
+1352,351,1352,4.687707352096387,0,8016.96230738979,0,0,2,2
+1353,352,1353,4.6876684299335345,0,-25692.30296332078,0,0,2,2
+1354,353,1354,4.687624266364536,0,-22643.09752771501,0,0,2,2
+1355,354,1355,4.687720608966777,0,10379.624140726419,0,0,2,2
+1356,355,1356,4.687689292770485,0,-31932.358281027387,0,0,2,2
+1357,356,1357,4.687689884193492,0,1690837.1628970453,0,0,2,2
+1358,357,1358,4.687685465756854,0,-226324.39519933815,0,0,2,2
+1359,358,1359,4.687704008898185,0,53928.295217550374,0,0,2,2
+1360,359,1360,4.6877304231527805,0,37858.34638632972,0,0,2,2
+1361,360,1361,4.687677524239683,0,-18903.98009027763,0,0,2,2
+1362,361,1362,4.687695679221867,0,55081.298888102705,0,0,2,2
+1363,362,1363,4.687701035673197,0,186690.76563969703,0,0,2,2
+1364,363,1364,4.687689154113219,0,-84164.0324843629,0,0,2,2
+1365,364,1365,4.68770174166216,0,79443.5838693156,0,0,2,2
+1366,365,1366,4.6876837324304255,0,-55527.0771541704,0,0,2,2
+1367,366,1367,4.687671490794284,0,-81688.42697691922,0,0,2,2
+1368,367,1368,4.687716206797148,0,22363.35843882257,0,0,2,2
+1369,368,1369,4.6876577767597425,0,-17114.485021718556,0,0,2,2
+1370,369,1370,4.687655188490774,0,-386358.6095590637,0,0,2,2
+1371,370,1371,4.687665058282576,0,101319.25981922718,0,0,2,2
+1372,371,1372,4.687655931651204,0,-109569.45221008976,0,0,2,2
+1373,372,1373,4.687645642127423,0,-97186.2275966764,0,0,2,2
+1374,373,1374,4.687697758904408,0,19187.679243744642,0,0,2,2
+1375,374,1375,4.687658475344309,0,-25455.94130159902,0,0,2,2
+1376,375,1376,4.687625811077754,0,-30614.494231055636,0,0,2,2
+1377,376,1377,4.687660540456129,0,28794.065623782833,0,0,2,2
+1378,377,1378,4.687679585735443,0,52506.44968163131,0,0,2,2
+1379,378,1379,4.687632079348528,0,-21049.801193884927,0,0,2,2
+1380,379,1380,4.687728014737616,0,10423.682120927764,0,0,2,2
+1381,380,1381,4.687692515367519,0,-28169.513916645406,0,0,2,2
+1382,381,1382,4.687670701408633,0,-45842.20614199843,0,0,2,2
+1383,382,1383,4.6876685132754075,0,-457010.5641853212,0,0,2,2
+1384,383,1384,4.687649360193957,0,-52210.919823960416,0,0,2,2
+1385,384,1385,4.687743133627109,0,10664.001160973716,0,0,2,2
+1386,385,1386,4.6876840688895305,0,-16930.5755176804,0,0,2,2
+1387,386,1387,4.687666218497961,0,-56021.18004620228,0,0,2,2
+1388,387,1388,4.687661596380574,0,-216351.06081571136,0,0,2,2
+1389,388,1389,4.687720567316303,0,16957.506060121174,0,0,2,2
+1390,389,1390,4.687640571279501,0,-12500.61928038758,0,0,2,2
+1391,390,1391,4.687642280815317,0,584954.1089631345,0,0,2,2
+1392,391,1392,4.6876491204650765,0,146206.3168685606,0,0,2,2
+1393,392,1393,4.687699726107593,0,19760.64229743995,0,0,2,2
+1394,393,1394,4.687957029171137,0,3886.467522868451,0,0,2,2
+1395,394,1395,4.688037445066655,0,12435.352408410188,0,0,2,2
+1396,395,1396,4.688290751141772,0,3947.793196581492,0,0,2,2
+1397,396,1397,4.6895811702077905,0,774.9420528058007,0,0,2,2
+1398,397,1398,4.688626779234222,0,-1047.7886188101368,0,0,2,2
+1399,398,1399,4.689114982797937,0,2048.325891745624,0,0,2,2
+1400,399,1400,4.689167404357547,0,19076.120730615625,0,0,2,2
+1401,400,1401,4.68819941339195,0,-1033.067492921702,0,0,2,2
+1402,401,1402,4.68774385311159,0,-2195.0991846980946,0,0,2,2
+1403,402,1403,4.68775815043782,0,69943.1476822489,0,0,2,2
+1404,403,1404,4.687759731188219,0,632610.9426376662,0,0,2,2
+1405,404,1405,4.699177624229751,0,87.58183286203027,0,0,2,2
+1406,405,1406,4.695531633832969,0,-274.2738984947965,0,0,2,2
+1407,406,1407,4.695247715644894,0,-3522.1413843853197,0,0,2,2
+1408,407,1408,4.754735647601379,0,16.810132191710583,0,0,2,2
+1409,408,1409,4.724397302964348,0,-32.961587455216275,0,0,2,2
+1410,409,1410,4.759533786047175,0,28.460446586036806,0,0,2,2
+1411,410,1411,4.743882767713707,0,-63.89360607044966,0,0,2,2
+1412,411,1412,4.744282268105393,0,2503.126456971458,0,0,2,2
+1413,412,1413,4.745442432748384,0,861.9466263181986,0,0,2,2
+1414,413,1414,4.813486442426927,0,14.696370844755526,0,0,2,2
+1415,414,1415,4.776165988226798,0,-26.79495792407948,0,0,2,2
+1416,415,1416,4.819133963999859,0,23.273146616949663,0,0,2,2
+1417,416,1417,4.815819733304253,0,-301.72914677476547,0,0,2,2
+1418,417,1418,4.82338051411354,0,132.26147209183162,0,0,2,2
+1419,418,1419,4.883073471345198,0,16.752395029101688,0,0,2,2
+1420,419,1420,4.884712249460814,0,610.2107359567673,0,0,2,2
+1421,420,1421,4.961541038870127,0,13.015954145423295,0,0,2,2
+1422,421,1422,4.999906861541805,0,26.064865298410677,0,0,2,2
+1423,422,1423,4.993773766242908,0,-163.04980621772725,0,0,2,2
+1424,423,1424,5.027284605822452,0,29.841090600739474,0,0,2,2
+1425,424,1425,5.049561561017202,0,44.8894380429352,0,0,2,2
+1426,425,1426,5.068208436003781,0,53.62828896100618,0,0,2,2
+1427,426,1427,5.050902273466685,0,-57.78288501893302,0,0,2,2
+1428,427,1428,5.071232352781532,0,49.18819963824127,0,0,2,2
+1429,428,1429,5.099616439035059,0,35.23100906148571,0,0,2,2
+1430,429,1430,5.0834811075303215,0,-61.975795149072646,0,0,2,2
+1431,430,1431,5.198289675352311,0,8.710151332525102,0,0,2,2
+1432,431,1432,5.157266264870209,0,-24.37632532859001,0,0,2,2
+1433,432,1433,5.2054644475826946,0,20.747670217469807,0,0,2,2
+1434,433,1434,5.211499235975039,0,165.7058930630452,0,0,2,2
+1435,434,1435,5.201878966277255,0,-103.9471897789248,0,0,2,2
+1436,435,1436,5.278536273467066,0,13.045070804847205,0,0,2,2
+1437,436,1437,5.299850103443958,0,46.91789326855794,0,0,2,2
+1438,437,1438,5.292430313476906,0,-134.7747044647677,0,0,2,2
+1439,438,1439,5.335799600526837,0,23.057792000332128,0,0,2,2
+1440,439,1440,5.38607544957814,0,19.890265781082434,0,0,2,2
+1441,440,1441,5.354411048658623,0,-31.581207000939052,0,0,2,2
+1442,441,1442,5.354553509767046,0,7019.459634075851,0,0,2,2
+1443,442,1443,5.418189239753329,0,15.71444218861867,0,0,2,2
+1444,443,1444,5.478070005053282,0,16.69985336678365,0,0,2,2
+1445,444,1445,5.457578047651754,0,-48.79963296846651,0,0,2,2
+1446,445,1446,5.44743639717364,0,-98.60327982689128,0,0,2,2
+1447,446,1447,5.447582422152287,0,6848.143442770463,0,0,2,2
+1448,447,1448,5.552216302498086,0,9.557133852774605,0,0,2,2
+1449,448,1449,5.502154534883066,0,-19.975323438239375,0,0,2,2
+1450,449,1450,5.594327596757653,0,10.849156789004411,0,0,2,2
+1451,450,1451,5.703767870137007,0,9.13740407549686,0,0,2,2
+1452,451,1452,5.790683385369762,0,11.505425669077031,0,0,2,2
+1453,452,1453,5.851272330781886,0,16.50466092779857,0,0,2,2
+1454,453,1454,5.784744718309785,0,-15.031352589413242,0,0,2,2
+1455,454,1455,5.866744337751296,0,12.195178548520975,0,0,2,2
+1456,455,1456,5.83407175340783,0,-30.606700390995794,0,0,2,2
+1457,456,1457,5.83298837237763,0,-923.0362837492128,0,0,2,2
+1458,457,1458,5.909277584800559,0,13.108013154680902,0,0,2,2
+1459,458,1459,5.885308774937403,0,-41.720886673524866,0,0,2,2
+1460,459,1460,5.919908712922938,0,28.901785905456688,0,0,2,2
+1461,460,1461,5.965351169098282,0,22.00585276775979,0,0,2,2
+1462,461,1462,6.016863401140053,0,19.412864874290744,0,0,2,2
+1463,462,1463,6.060070083297614,0,23.144568156224413,0,0,2,2
+1464,463,1464,6.124157266259176,0,15.603744052843995,0,0,2,2
+1465,464,1465,6.119357144906505,0,-208.3280664235001,0,0,2,2
+1466,465,1466,6.130669368179921,0,88.39995249652004,0,0,2,2
+1467,466,1467,6.180864201080793,0,19.922369339786027,0,0,2,2
+1468,467,1468,6.230570579446633,0,20.11814243717314,0,0,2,2
+1469,468,1469,6.263350807100293,0,30.506194483013054,0,0,2,2
+1470,469,1470,6.193143317898733,0,-14.243494695118356,0,0,2,2
+1471,470,1471,6.195834121923333,0,371.6361321217441,0,0,2,2
+1472,471,1472,6.208803875641413,0,77.1024663796037,0,0,2,2
+1473,472,1473,6.221901699465272,0,76.34856090966778,0,0,2,2
+1474,473,1474,6.256501196726211,0,28.9021540532308,0,0,2,2
+1475,474,1475,6.23629215057456,0,-49.48279065205987,0,0,2,2
+1476,475,1476,6.235907617862001,0,-2600.5589832538453,0,0,2,2
+1477,476,1477,6.237021459432858,0,897.7937492762666,0,0,2,2
+1478,477,1478,6.237655749087532,0,1576.5667824346535,0,0,2,2
+1479,478,1479,6.244179133136743,0,153.29466921710022,0,0,2,2
+1480,479,1480,6.270392764015719,0,38.1480919074826,0,0,2,2
+1481,480,1481,6.256087261027579,0,-69.903169488625,0,0,2,2
+1482,481,1482,6.257661319758907,0,635.3003100185017,0,0,2,2
+1483,482,1483,6.267836945548009,0,98.27405416883752,0,0,2,2
+1484,483,1484,6.269723566488711,0,530.0481821365647,0,0,2,2
+1485,484,1485,6.267054253560375,0,-374.6282383697463,0,0,2,2
+1486,485,1486,0.013744254559647902,0,-0.15991530887798605,0,0,2,2
+1487,486,1487,0.0010119061751368357,0,-78.54010664807936,0,0,2,2
+1488,487,1488,0.0012243999420399345,0,4706.0203909699585,0,0,2,2
+1489,488,1489,0.0018815871511780942,0,1521.6364318949657,0,0,2,2
+1490,489,1490,0.002008029429596202,0,7908.747078198724,0,0,2,2
+1491,490,1491,0.0018352499454211354,0,-5787.724189445794,0,0,2,2
+1492,491,1492,0.0011422250006752636,0,-1442.9495035949885,0,0,2,2
+1493,492,1493,0.001342209822478479,0,5000.379483719008,0,0,2,2
+1494,493,1494,6.27518733217875,0,0.1593918849600848,0,0,2,2
+1495,494,1495,6.277509724011835,0,430.5905600226852,0,0,2,2
+1496,495,1496,6.278068934614515,0,1788.2350499208055,0,0,2,2
+1497,496,1497,6.27766295708146,0,-2463.1904935097405,0,0,2,2
+1498,497,1498,6.277751071486938,0,11348.882110444736,0,0,2,2
+1499,498,1499,6.277640581367299,0,-9050.58301386519,0,0,2,2
+1500,499,1500,6.2774103081305626,0,-4342.667060107362,0,0,2,2
+1501,500,1501,6.267469573203085,0,-100.59618401410951,0,0,2,2
+1502,501,1502,6.268776934010963,0,764.899784339935,0,0,2,2
+1503,502,1503,6.269099769240327,0,3097.5553751321304,0,0,2,2
+1504,503,1504,6.269088227498937,0,-86642.03833671357,0,0,2,2
+1505,504,1505,6.269037911255563,0,-19874.297700944935,0,0,2,2
+1506,505,1506,6.26957021841891,0,1878.6145835655457,0,0,2,2
+1507,506,1507,6.268937417154099,0,-1580.2749703702154,0,0,2,2
+1508,507,1508,6.25233126629231,0,-60.21865080733453,0,0,2,2
+1509,508,1509,6.258024899264237,0,175.634784491826,0,0,2,2
+1510,509,1510,6.230735642842278,0,-36.644457604030876,0,0,2,2
+1511,510,1511,6.240254079440697,0,105.05926993999557,0,0,2,2
+1512,511,1512,6.239790331359758,0,-2156.343155047787,0,0,2,2
+1513,512,1513,6.239202869296887,0,-1702.2375795858684,0,0,2,2
+1514,513,1514,6.238970234292401,0,-4298.579236645538,0,0,2,2
+1515,514,1515,6.238925065169713,0,-22139.017552296125,0,0,2,2
+1516,515,1516,6.238863666976653,0,-16287.124264657097,0,0,2,2
+1517,516,1517,6.238732914238007,0,-7648.023363449819,0,0,2,2
+1518,517,1518,6.264384233905774,0,38.98434906865983,0,0,2,2
+1519,518,1519,6.254531263325934,0,-101.49223443801617,0,0,2,2
+1520,519,1520,0.010624767483427618,0,-0.1601561459425839,0,0,2,2
+1521,520,1521,6.272689144792406,0,0.1596917469618436,0,0,2,2
+1522,521,1522,6.272761513075674,0,13818.208127091919,0,0,2,2
+1523,522,1523,6.272970784747892,0,4778.477609518447,0,0,2,2
+1524,523,1524,6.27296208880721,0,-114996.18460447,0,0,2,2
+1525,524,1525,6.273040252520847,0,12793.660299308754,0,0,2,2
+1526,525,1526,6.273027181376255,0,-76504.3943162592,0,0,2,2
+1527,526,1527,6.272997881943862,0,-34130.35401441668,0,0,2,2
+1528,527,1528,6.264496571896351,0,-117.62892947220149,0,0,2,2
+1529,528,1529,6.256412251422299,0,-123.69623436004015,0,0,2,2
+1530,529,1530,6.262334255024012,0,168.86176828916354,0,0,2,2
+1531,530,1531,6.262342165248228,0,126418.66686483091,0,0,2,2
+1532,531,1532,6.262333893420006,0,-120892.25902943166,0,0,2,2
+1533,532,1533,6.262315140960322,0,-53326.33781473414,0,0,2,2
+1534,533,1534,6.2621862311902685,0,-7757.363926623419,0,0,2,2
+1535,534,1535,6.262360065436234,0,5752.60642368762,0,0,2,2
+1536,535,1536,6.26219896741017,0,-6207.400701499976,0,0,2,2
+1537,536,1537,6.262701530291994,0,1989.8007516411894,0,0,2,2
+1538,537,1538,6.263282807370463,0,1720.3499622483916,0,0,2,2
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv
new file mode 100644
index 00000000..045416d0
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv
@@ -0,0 +1,538 @@
+LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,CrossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG
+1001,1001,0,1002,1001,1002,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1002,1002,1001,1003,1002,1003,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1003,1003,1002,1004,1003,1004,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1004,1004,1003,1005,1004,1005,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1005,1005,1004,1006,1005,1006,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1006,1006,1005,1007,1006,1007,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1007,1007,1006,1008,1007,1008,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1008,1008,1007,1009,1008,1009,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1009,1009,1008,1010,1009,1010,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1010,1010,1009,1011,1010,1011,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1011,1011,1010,1012,1011,1012,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1012,1012,1011,1013,1012,1013,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1013,1013,1012,1014,1013,1014,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1014,1014,1013,1015,1014,1015,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1015,1015,1014,1016,1015,1016,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1016,1016,1015,1017,1016,1017,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1017,1017,1016,1018,1017,1018,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1018,1018,1017,1019,1018,1019,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1019,1019,1018,1020,1019,1020,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1020,1020,1019,1021,1020,1021,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1021,1021,1020,1022,1021,1022,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1022,1022,1021,1023,1022,1023,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1023,1023,1022,1024,1023,1024,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1024,1024,1023,1025,1024,1025,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1025,1025,1024,1026,1025,1026,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1026,1026,1025,1027,1026,1027,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1027,1027,1026,1028,1027,1028,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1028,1028,1027,1029,1028,1029,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1029,1029,1028,1030,1029,1030,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1030,1030,1029,1031,1030,1031,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1031,1031,1030,1032,1031,1032,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1032,1032,1031,1033,1032,1033,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1033,1033,1032,1034,1033,1034,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1034,1034,1033,1035,1034,1035,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1035,1035,1034,1036,1035,1036,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1036,1036,1035,1037,1036,1037,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1037,1037,1036,1038,1037,1038,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1038,1038,1037,1039,1038,1039,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1039,1039,1038,1040,1039,1040,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1040,1040,1039,1041,1040,1041,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1041,1041,1040,1042,1041,1042,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1042,1042,1041,1043,1042,1043,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1043,1043,1042,1044,1043,1044,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1044,1044,1043,1045,1044,1045,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1045,1045,1044,1046,1045,1046,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1046,1046,1045,1047,1046,1047,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1047,1047,1046,1048,1047,1048,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1048,1048,1047,1049,1048,1049,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1049,1049,1048,1050,1049,1050,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1050,1050,1049,1051,1050,1051,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1051,1051,1050,1052,1051,1052,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1052,1052,1051,1053,1052,1053,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1053,1053,1052,1054,1053,1054,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1054,1054,1053,1055,1054,1055,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1055,1055,1054,1056,1055,1056,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1056,1056,1055,1057,1056,1057,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1057,1057,1056,1058,1057,1058,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1058,1058,1057,1059,1058,1059,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1059,1059,1058,1060,1059,1060,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1060,1060,1059,1061,1060,1061,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1061,1061,1060,1062,1061,1062,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1062,1062,1061,1063,1062,1063,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1063,1063,1062,1064,1063,1064,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1064,1064,1063,1065,1064,1065,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1065,1065,1064,1066,1065,1066,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1066,1066,1065,1067,1066,1067,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1067,1067,1066,1068,1067,1068,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1068,1068,1067,1069,1068,1069,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1069,1069,1068,1070,1069,1070,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1070,1070,1069,1071,1070,1071,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1071,1071,1070,1072,1071,1072,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1072,1072,1071,1073,1072,1073,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1073,1073,1072,1074,1073,1074,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1074,1074,1073,1075,1074,1075,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1075,1075,1074,1076,1075,1076,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1076,1076,1075,1077,1076,1077,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1077,1077,1076,1078,1077,1078,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1078,1078,1077,1079,1078,1079,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1079,1079,1078,1080,1079,1080,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1080,1080,1079,1081,1080,1081,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1081,1081,1080,1082,1081,1082,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1082,1082,1081,1083,1082,1083,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1083,1083,1082,1084,1083,1084,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1084,1084,1083,1085,1084,1085,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1085,1085,1084,1086,1085,1086,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1086,1086,1085,1087,1086,1087,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1087,1087,1086,1088,1087,1088,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1088,1088,1087,1089,1088,1089,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1089,1089,1088,1090,1089,1090,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1090,1090,1089,1091,1090,1091,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1091,1091,1090,1092,1091,1092,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1092,1092,1091,1093,1092,1093,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1093,1093,1092,1094,1093,1094,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1094,1094,1093,1095,1094,1095,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1095,1095,1094,1096,1095,1096,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1096,1096,1095,1097,1096,1097,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1097,1097,1096,1098,1097,1098,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1098,1098,1097,1099,1098,1099,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1099,1099,1098,1100,1099,1100,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1100,1100,1099,1101,1100,1101,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1101,1101,1100,1102,1101,1102,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1102,1102,1101,1103,1102,1103,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1103,1103,1102,1104,1103,1104,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1104,1104,1103,1105,1104,1105,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1105,1105,1104,1106,1105,1106,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1106,1106,1105,1107,1106,1107,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1107,1107,1106,1108,1107,1108,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1108,1108,1107,1109,1108,1109,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1109,1109,1108,1110,1109,1110,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1110,1110,1109,1111,1110,1111,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1111,1111,1110,1112,1111,1112,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1112,1112,1111,1113,1112,1113,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1113,1113,1112,1114,1113,1114,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1114,1114,1113,1115,1114,1115,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1115,1115,1114,1116,1115,1116,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1116,1116,1115,1117,1116,1117,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1117,1117,1116,1118,1117,1118,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1118,1118,1117,1119,1118,1119,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1119,1119,1118,1120,1119,1120,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1120,1120,1119,1121,1120,1121,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1121,1121,1120,1122,1121,1122,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1122,1122,1121,1123,1122,1123,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1123,1123,1122,1124,1123,1124,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1124,1124,1123,1125,1124,1125,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1125,1125,1124,1126,1125,1126,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1126,1126,1125,1127,1126,1127,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1127,1127,1126,1128,1127,1128,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1128,1128,1127,1129,1128,1129,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1129,1129,1128,1130,1129,1130,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1130,1130,1129,1131,1130,1131,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1131,1131,1130,1132,1131,1132,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1132,1132,1131,1133,1132,1133,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1133,1133,1132,1134,1133,1134,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1134,1134,1133,1135,1134,1135,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1135,1135,1134,1136,1135,1136,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1136,1136,1135,1137,1136,1137,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1137,1137,1136,1138,1137,1138,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1138,1138,1137,1139,1138,1139,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1139,1139,1138,1140,1139,1140,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1140,1140,1139,1141,1140,1141,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1141,1141,1140,1142,1141,1142,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1142,1142,1141,1143,1142,1143,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1143,1143,1142,1144,1143,1144,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1144,1144,1143,1145,1144,1145,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1145,1145,1144,1146,1145,1146,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1146,1146,1145,1147,1146,1147,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1147,1147,1146,1148,1147,1148,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1148,1148,1147,1149,1148,1149,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1149,1149,1148,1150,1149,1150,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1150,1150,1149,1151,1150,1151,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1151,1151,1150,1152,1151,1152,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1152,1152,1151,1153,1152,1153,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1153,1153,1152,1154,1153,1154,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1154,1154,1153,1155,1154,1155,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1155,1155,1154,1156,1155,1156,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1156,1156,1155,1157,1156,1157,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1157,1157,1156,1158,1157,1158,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1158,1158,1157,1159,1158,1159,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1159,1159,1158,1160,1159,1160,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1160,1160,1159,1161,1160,1161,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1161,1161,1160,1162,1161,1162,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1162,1162,1161,1163,1162,1163,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1163,1163,1162,1164,1163,1164,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1164,1164,1163,1165,1164,1165,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1165,1165,1164,1166,1165,1166,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1166,1166,1165,1167,1166,1167,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1167,1167,1166,1168,1167,1168,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1168,1168,1167,1169,1168,1169,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1169,1169,1168,1170,1169,1170,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1170,1170,1169,1171,1170,1171,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1171,1171,1170,1172,1171,1172,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1172,1172,1171,1173,1172,1173,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1173,1173,1172,1174,1173,1174,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1174,1174,1173,1175,1174,1175,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1175,1175,1174,1176,1175,1176,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1176,1176,1175,1177,1176,1177,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1177,1177,1176,1178,1177,1178,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1178,1178,1177,1179,1178,1179,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1179,1179,1178,1180,1179,1180,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1180,1180,1179,1181,1180,1181,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1181,1181,1180,1182,1181,1182,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1182,1182,1181,1183,1182,1183,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1183,1183,1182,1184,1183,1184,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1184,1184,1183,1185,1184,1185,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1185,1185,1184,1186,1185,1186,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1186,1186,1185,1187,1186,1187,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1187,1187,1186,1188,1187,1188,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1188,1188,1187,1189,1188,1189,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1189,1189,1188,1190,1189,1190,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1190,1190,1189,1191,1190,1191,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1191,1191,1190,1192,1191,1192,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1192,1192,1191,1193,1192,1193,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1193,1193,1192,1194,1193,1194,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1194,1194,1193,1195,1194,1195,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1195,1195,1194,1196,1195,1196,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1196,1196,1195,1197,1196,1197,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1197,1197,1196,1198,1197,1198,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1198,1198,1197,1199,1198,1199,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1199,1199,1198,1200,1199,1200,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1200,1200,1199,1201,1200,1201,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1201,1201,1200,1202,1201,1202,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1202,1202,1201,1203,1202,1203,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1203,1203,1202,1204,1203,1204,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1204,1204,1203,1205,1204,1205,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1205,1205,1204,1206,1205,1206,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1206,1206,1205,1207,1206,1207,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1207,1207,1206,1208,1207,1208,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1208,1208,1207,1209,1208,1209,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1209,1209,1208,1210,1209,1210,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1210,1210,1209,1211,1210,1211,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1211,1211,1210,1212,1211,1212,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1212,1212,1211,1213,1212,1213,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1213,1213,1212,1214,1213,1214,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1214,1214,1213,1215,1214,1215,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1215,1215,1214,1216,1215,1216,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1216,1216,1215,1217,1216,1217,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1217,1217,1216,1218,1217,1218,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1218,1218,1217,1219,1218,1219,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1219,1219,1218,1220,1219,1220,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1220,1220,1219,1221,1220,1221,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1221,1221,1220,1222,1221,1222,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1222,1222,1221,1223,1222,1223,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1223,1223,1222,1224,1223,1224,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1224,1224,1223,1225,1224,1225,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1225,1225,1224,1226,1225,1226,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1226,1226,1225,1227,1226,1227,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1227,1227,1226,1228,1227,1228,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1228,1228,1227,1229,1228,1229,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1229,1229,1228,1230,1229,1230,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1230,1230,1229,1231,1230,1231,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1231,1231,1230,1232,1231,1232,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1232,1232,1231,1233,1232,1233,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1233,1233,1232,1234,1233,1234,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1234,1234,1233,1235,1234,1235,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1235,1235,1234,1236,1235,1236,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1236,1236,1235,1237,1236,1237,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1237,1237,1236,1238,1237,1238,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1238,1238,1237,1239,1238,1239,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1239,1239,1238,1240,1239,1240,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1240,1240,1239,1241,1240,1241,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1241,1241,1240,1242,1241,1242,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1242,1242,1241,1243,1242,1243,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1243,1243,1242,1244,1243,1244,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1244,1244,1243,1245,1244,1245,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1245,1245,1244,1246,1245,1246,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1246,1246,1245,1247,1246,1247,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1247,1247,1246,1248,1247,1248,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1248,1248,1247,1249,1248,1249,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1249,1249,1248,1250,1249,1250,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1250,1250,1249,1251,1250,1251,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1251,1251,1250,1252,1251,1252,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1252,1252,1251,1253,1252,1253,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1253,1253,1252,1254,1253,1254,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1254,1254,1253,1255,1254,1255,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1255,1255,1254,1256,1255,1256,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1256,1256,1255,1257,1256,1257,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1257,1257,1256,1258,1257,1258,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1258,1258,1257,1259,1258,1259,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1259,1259,1258,1260,1259,1260,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1260,1260,1259,1261,1260,1261,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1261,1261,1260,1262,1261,1262,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1262,1262,1261,1263,1262,1263,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1263,1263,1262,1264,1263,1264,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1264,1264,1263,1265,1264,1265,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1265,1265,1264,1266,1265,1266,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1266,1266,1265,1267,1266,1267,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1267,1267,1266,1268,1267,1268,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1268,1268,1267,1269,1268,1269,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1269,1269,1268,1270,1269,1270,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1270,1270,1269,1271,1270,1271,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1271,1271,1270,1272,1271,1272,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1272,1272,1271,1273,1272,1273,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1273,1273,1272,1274,1273,1274,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1274,1274,1273,1275,1274,1275,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1275,1275,1274,1276,1275,1276,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1276,1276,1275,1277,1276,1277,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1277,1277,1276,1278,1277,1278,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1278,1278,1277,1279,1278,1279,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1279,1279,1278,1280,1279,1280,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1280,1280,1279,1281,1280,1281,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1281,1281,1280,1282,1281,1282,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1282,1282,1281,1283,1282,1283,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1283,1283,1282,1284,1283,1284,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1284,1284,1283,1285,1284,1285,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1285,1285,1284,1286,1285,1286,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1286,1286,1285,1287,1286,1287,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1287,1287,1286,1288,1287,1288,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1288,1288,1287,1289,1288,1289,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1289,1289,1288,1290,1289,1290,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1290,1290,1289,1291,1290,1291,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1291,1291,1290,1292,1291,1292,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1292,1292,1291,1293,1292,1293,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1293,1293,1292,1294,1293,1294,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1294,1294,1293,1295,1294,1295,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1295,1295,1294,1296,1295,1296,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1296,1296,1295,1297,1296,1297,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1297,1297,1296,1298,1297,1298,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1298,1298,1297,1299,1298,1299,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1299,1299,1298,1300,1299,1300,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1300,1300,1299,1301,1300,1301,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1301,1301,1300,1302,1301,1302,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1302,1302,1301,1303,1302,1303,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1303,1303,1302,1304,1303,1304,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1304,1304,1303,1305,1304,1305,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1305,1305,1304,1306,1305,1306,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1306,1306,1305,1307,1306,1307,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1307,1307,1306,1308,1307,1308,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1308,1308,1307,1309,1308,1309,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1309,1309,1308,1310,1309,1310,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1310,1310,1309,1311,1310,1311,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1311,1311,1310,1312,1311,1312,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1312,1312,1311,1313,1312,1313,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1313,1313,1312,1314,1313,1314,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1314,1314,1313,1315,1314,1315,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1315,1315,1314,1316,1315,1316,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1316,1316,1315,1317,1316,1317,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1317,1317,1316,1318,1317,1318,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1318,1318,1317,1319,1318,1319,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1319,1319,1318,1320,1319,1320,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1320,1320,1319,1321,1320,1321,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1321,1321,1320,1322,1321,1322,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1322,1322,1321,1323,1322,1323,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1323,1323,1322,1324,1323,1324,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1324,1324,1323,1325,1324,1325,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1325,1325,1324,1326,1325,1326,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1326,1326,1325,1327,1326,1327,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1327,1327,1326,1328,1327,1328,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1328,1328,1327,1329,1328,1329,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1329,1329,1328,1330,1329,1330,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1330,1330,1329,1331,1330,1331,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1331,1331,1330,1332,1331,1332,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1332,1332,1331,1333,1332,1333,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1333,1333,1332,1334,1333,1334,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1334,1334,1333,1335,1334,1335,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1335,1335,1334,1336,1335,1336,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1336,1336,1335,1337,1336,1337,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1337,1337,1336,1338,1337,1338,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1338,1338,1337,1339,1338,1339,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1339,1339,1338,1340,1339,1340,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1340,1340,1339,1341,1340,1341,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1341,1341,1340,1342,1341,1342,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1342,1342,1341,1343,1342,1343,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1343,1343,1342,1344,1343,1344,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1344,1344,1343,1345,1344,1345,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1345,1345,1344,1346,1345,1346,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1346,1346,1345,1347,1346,1347,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1347,1347,1346,1348,1347,1348,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1348,1348,1347,1349,1348,1349,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1349,1349,1348,1350,1349,1350,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1350,1350,1349,1351,1350,1351,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1351,1351,1350,1352,1351,1352,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1352,1352,1351,1353,1352,1353,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1353,1353,1352,1354,1353,1354,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1354,1354,1353,1355,1354,1355,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1355,1355,1354,1356,1355,1356,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1356,1356,1355,1357,1356,1357,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1357,1357,1356,1358,1357,1358,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1358,1358,1357,1359,1358,1359,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1359,1359,1358,1360,1359,1360,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1360,1360,1359,1361,1360,1361,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1361,1361,1360,1362,1361,1362,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1362,1362,1361,1363,1362,1363,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1363,1363,1362,1364,1363,1364,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1364,1364,1363,1365,1364,1365,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1365,1365,1364,1366,1365,1366,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1366,1366,1365,1367,1366,1367,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1367,1367,1366,1368,1367,1368,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1368,1368,1367,1369,1368,1369,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1369,1369,1368,1370,1369,1370,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1370,1370,1369,1371,1370,1371,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1371,1371,1370,1372,1371,1372,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1372,1372,1371,1373,1372,1373,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1373,1373,1372,1374,1373,1374,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1374,1374,1373,1375,1374,1375,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1375,1375,1374,1376,1375,1376,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1376,1376,1375,1377,1376,1377,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1377,1377,1376,1378,1377,1378,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1378,1378,1377,1379,1378,1379,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1379,1379,1378,1380,1379,1380,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1380,1380,1379,1381,1380,1381,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1381,1381,1380,1382,1381,1382,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1382,1382,1381,1383,1382,1383,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1383,1383,1382,1384,1383,1384,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1384,1384,1383,1385,1384,1385,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1385,1385,1384,1386,1385,1386,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1386,1386,1385,1387,1386,1387,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1387,1387,1386,1388,1387,1388,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1388,1388,1387,1389,1388,1389,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1389,1389,1388,1390,1389,1390,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1390,1390,1389,1391,1390,1391,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1391,1391,1390,1392,1391,1392,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1392,1392,1391,1393,1392,1393,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1393,1393,1392,1394,1393,1394,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1394,1394,1393,1395,1394,1395,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1395,1395,1394,1396,1395,1396,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1396,1396,1395,1397,1396,1397,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1397,1397,1396,1398,1397,1398,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1398,1398,1397,1399,1398,1399,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1399,1399,1398,1400,1399,1400,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1400,1400,1399,1401,1400,1401,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1401,1401,1400,1402,1401,1402,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1402,1402,1401,1403,1402,1403,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1403,1403,1402,1404,1403,1404,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1404,1404,1403,1405,1404,1405,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1405,1405,1404,1406,1405,1406,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1406,1406,1405,1407,1406,1407,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1407,1407,1406,1408,1407,1408,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1408,1408,1407,1409,1408,1409,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1409,1409,1408,1410,1409,1410,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1410,1410,1409,1411,1410,1411,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1411,1411,1410,1412,1411,1412,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1412,1412,1411,1413,1412,1413,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1413,1413,1412,1414,1413,1414,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1414,1414,1413,1415,1414,1415,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1415,1415,1414,1416,1415,1416,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1416,1416,1415,1417,1416,1417,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1417,1417,1416,1418,1417,1418,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1418,1418,1417,1419,1418,1419,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1419,1419,1418,1420,1419,1420,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1420,1420,1419,1421,1420,1421,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1421,1421,1420,1422,1421,1422,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1422,1422,1421,1423,1422,1423,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1423,1423,1422,1424,1423,1424,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1424,1424,1423,1425,1424,1425,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1425,1425,1424,1426,1425,1426,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1426,1426,1425,1427,1426,1427,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1427,1427,1426,1428,1427,1428,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1428,1428,1427,1429,1428,1429,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1429,1429,1428,1430,1429,1430,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1430,1430,1429,1431,1430,1431,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1431,1431,1430,1432,1431,1432,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1432,1432,1431,1433,1432,1433,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1433,1433,1432,1434,1433,1434,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1434,1434,1433,1435,1434,1435,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1435,1435,1434,1436,1435,1436,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1436,1436,1435,1437,1436,1437,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1437,1437,1436,1438,1437,1438,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1438,1438,1437,1439,1438,1439,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1439,1439,1438,1440,1439,1440,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1440,1440,1439,1441,1440,1441,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1441,1441,1440,1442,1441,1442,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1442,1442,1441,1443,1442,1443,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1443,1443,1442,1444,1443,1444,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1444,1444,1443,1445,1444,1445,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1445,1445,1444,1446,1445,1446,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1446,1446,1445,1447,1446,1447,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1447,1447,1446,1448,1447,1448,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1448,1448,1447,1449,1448,1449,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1449,1449,1448,1450,1449,1450,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1450,1450,1449,1451,1450,1451,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1451,1451,1450,1452,1451,1452,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1452,1452,1451,1453,1452,1453,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1453,1453,1452,1454,1453,1454,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1454,1454,1453,1455,1454,1455,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1455,1455,1454,1456,1455,1456,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1456,1456,1455,1457,1456,1457,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1457,1457,1456,1458,1457,1458,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1458,1458,1457,1459,1458,1459,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1459,1459,1458,1460,1459,1460,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1460,1460,1459,1461,1460,1461,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1461,1461,1460,1462,1461,1462,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1462,1462,1461,1463,1462,1463,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1463,1463,1462,1464,1463,1464,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1464,1464,1463,1465,1464,1465,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1465,1465,1464,1466,1465,1466,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1466,1466,1465,1467,1466,1467,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1467,1467,1466,1468,1467,1468,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1468,1468,1467,1469,1468,1469,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1469,1469,1468,1470,1469,1470,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1470,1470,1469,1471,1470,1471,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1471,1471,1470,1472,1471,1472,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1472,1472,1471,1473,1472,1473,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1473,1473,1472,1474,1473,1474,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1474,1474,1473,1475,1474,1475,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1475,1475,1474,1476,1475,1476,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1476,1476,1475,1477,1476,1477,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1477,1477,1476,1478,1477,1478,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1478,1478,1477,1479,1478,1479,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1479,1479,1478,1480,1479,1480,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1480,1480,1479,1481,1480,1481,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1481,1481,1480,1482,1481,1482,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1482,1482,1481,1483,1482,1483,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1483,1483,1482,1484,1483,1484,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1484,1484,1483,1485,1484,1485,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1485,1485,1484,1486,1485,1486,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1486,1486,1485,1487,1486,1487,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1487,1487,1486,1488,1487,1488,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1488,1488,1487,1489,1488,1489,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1489,1489,1488,1490,1489,1490,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1490,1490,1489,1491,1490,1491,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1491,1491,1490,1492,1491,1492,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1492,1492,1491,1493,1492,1493,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1493,1493,1492,1494,1493,1494,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1494,1494,1493,1495,1494,1495,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1495,1495,1494,1496,1495,1496,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1496,1496,1495,1497,1496,1497,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1497,1497,1496,1498,1497,1498,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1498,1498,1497,1499,1498,1499,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1499,1499,1498,1500,1499,1500,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1500,1500,1499,1501,1500,1501,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1501,1501,1500,1502,1501,1502,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1502,1502,1501,1503,1502,1503,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1503,1503,1502,1504,1503,1504,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1504,1504,1503,1505,1504,1505,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1505,1505,1504,1506,1505,1506,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1506,1506,1505,1507,1506,1507,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1507,1507,1506,1508,1507,1508,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1508,1508,1507,1509,1508,1509,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1509,1509,1508,1510,1509,1510,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1510,1510,1509,1511,1510,1511,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1511,1511,1510,1512,1511,1512,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1512,1512,1511,1513,1512,1513,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1513,1513,1512,1514,1513,1514,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1514,1514,1513,1515,1514,1515,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1515,1515,1514,1516,1515,1516,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1516,1516,1515,1517,1516,1517,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1517,1517,1516,1518,1517,1518,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1518,1518,1517,1519,1518,1519,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1519,1519,1518,1520,1519,1520,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1520,1520,1519,1521,1520,1521,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1521,1521,1520,1522,1521,1522,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1522,1522,1521,1523,1522,1523,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1523,1523,1522,1524,1523,1524,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1524,1524,1523,1525,1524,1525,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1525,1525,1524,1526,1525,1526,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1526,1526,1525,1527,1526,1527,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1527,1527,1526,1528,1527,1528,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1528,1528,1527,1529,1528,1529,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1529,1529,1528,1530,1529,1530,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1530,1530,1529,1531,1530,1531,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1531,1531,1530,1532,1531,1532,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1532,1532,1531,1533,1532,1533,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1533,1533,1532,1534,1533,1534,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1534,1534,1533,1535,1534,1535,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1535,1535,1534,1536,1535,1536,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1536,1536,1535,1537,1536,1537,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1537,1537,1536,0,1537,1538,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv
new file mode 100644
index 00000000..6aac5521
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv
@@ -0,0 +1,539 @@
+NID,PID
+1001,1001
+1002,1002
+1003,1003
+1004,1004
+1005,1005
+1006,1006
+1007,1007
+1008,1008
+1009,1009
+1010,1010
+1011,1011
+1012,1012
+1013,1013
+1014,1014
+1015,1015
+1016,1016
+1017,1017
+1018,1018
+1019,1019
+1020,1020
+1021,1021
+1022,1022
+1023,1023
+1024,1024
+1025,1025
+1026,1026
+1027,1027
+1028,1028
+1029,1029
+1030,1030
+1031,1031
+1032,1032
+1033,1033
+1034,1034
+1035,1035
+1036,1036
+1037,1037
+1038,1038
+1039,1039
+1040,1040
+1041,1041
+1042,1042
+1043,1043
+1044,1044
+1045,1045
+1046,1046
+1047,1047
+1048,1048
+1049,1049
+1050,1050
+1051,1051
+1052,1052
+1053,1053
+1054,1054
+1055,1055
+1056,1056
+1057,1057
+1058,1058
+1059,1059
+1060,1060
+1061,1061
+1062,1062
+1063,1063
+1064,1064
+1065,1065
+1066,1066
+1067,1067
+1068,1068
+1069,1069
+1070,1070
+1071,1071
+1072,1072
+1073,1073
+1074,1074
+1075,1075
+1076,1076
+1077,1077
+1078,1078
+1079,1079
+1080,1080
+1081,1081
+1082,1082
+1083,1083
+1084,1084
+1085,1085
+1086,1086
+1087,1087
+1088,1088
+1089,1089
+1090,1090
+1091,1091
+1092,1092
+1093,1093
+1094,1094
+1095,1095
+1096,1096
+1097,1097
+1098,1098
+1099,1099
+1100,1100
+1101,1101
+1102,1102
+1103,1103
+1104,1104
+1105,1105
+1106,1106
+1107,1107
+1108,1108
+1109,1109
+1110,1110
+1111,1111
+1112,1112
+1113,1113
+1114,1114
+1115,1115
+1116,1116
+1117,1117
+1118,1118
+1119,1119
+1120,1120
+1121,1121
+1122,1122
+1123,1123
+1124,1124
+1125,1125
+1126,1126
+1127,1127
+1128,1128
+1129,1129
+1130,1130
+1131,1131
+1132,1132
+1133,1133
+1134,1134
+1135,1135
+1136,1136
+1137,1137
+1138,1138
+1139,1139
+1140,1140
+1141,1141
+1142,1142
+1143,1143
+1144,1144
+1145,1145
+1146,1146
+1147,1147
+1148,1148
+1149,1149
+1150,1150
+1151,1151
+1152,1152
+1153,1153
+1154,1154
+1155,1155
+1156,1156
+1157,1157
+1158,1158
+1159,1159
+1160,1160
+1161,1161
+1162,1162
+1163,1163
+1164,1164
+1165,1165
+1166,1166
+1167,1167
+1168,1168
+1169,1169
+1170,1170
+1171,1171
+1172,1172
+1173,1173
+1174,1174
+1175,1175
+1176,1176
+1177,1177
+1178,1178
+1179,1179
+1180,1180
+1181,1181
+1182,1182
+1183,1183
+1184,1184
+1185,1185
+1186,1186
+1187,1187
+1188,1188
+1189,1189
+1190,1190
+1191,1191
+1192,1192
+1193,1193
+1194,1194
+1195,1195
+1196,1196
+1197,1197
+1198,1198
+1199,1199
+1200,1200
+1201,1201
+1202,1202
+1203,1203
+1204,1204
+1205,1205
+1206,1206
+1207,1207
+1208,1208
+1209,1209
+1210,1210
+1211,1211
+1212,1212
+1213,1213
+1214,1214
+1215,1215
+1216,1216
+1217,1217
+1218,1218
+1219,1219
+1220,1220
+1221,1221
+1222,1222
+1223,1223
+1224,1224
+1225,1225
+1226,1226
+1227,1227
+1228,1228
+1229,1229
+1230,1230
+1231,1231
+1232,1232
+1233,1233
+1234,1234
+1235,1235
+1236,1236
+1237,1237
+1238,1238
+1239,1239
+1240,1240
+1241,1241
+1242,1242
+1243,1243
+1244,1244
+1245,1245
+1246,1246
+1247,1247
+1248,1248
+1249,1249
+1250,1250
+1251,1251
+1252,1252
+1253,1253
+1254,1254
+1255,1255
+1256,1256
+1257,1257
+1258,1258
+1259,1259
+1260,1260
+1261,1261
+1262,1262
+1263,1263
+1264,1264
+1265,1265
+1266,1266
+1267,1267
+1268,1268
+1269,1269
+1270,1270
+1271,1271
+1272,1272
+1273,1273
+1274,1274
+1275,1275
+1276,1276
+1277,1277
+1278,1278
+1279,1279
+1280,1280
+1281,1281
+1282,1282
+1283,1283
+1284,1284
+1285,1285
+1286,1286
+1287,1287
+1288,1288
+1289,1289
+1290,1290
+1291,1291
+1292,1292
+1293,1293
+1294,1294
+1295,1295
+1296,1296
+1297,1297
+1298,1298
+1299,1299
+1300,1300
+1301,1301
+1302,1302
+1303,1303
+1304,1304
+1305,1305
+1306,1306
+1307,1307
+1308,1308
+1309,1309
+1310,1310
+1311,1311
+1312,1312
+1313,1313
+1314,1314
+1315,1315
+1316,1316
+1317,1317
+1318,1318
+1319,1319
+1320,1320
+1321,1321
+1322,1322
+1323,1323
+1324,1324
+1325,1325
+1326,1326
+1327,1327
+1328,1328
+1329,1329
+1330,1330
+1331,1331
+1332,1332
+1333,1333
+1334,1334
+1335,1335
+1336,1336
+1337,1337
+1338,1338
+1339,1339
+1340,1340
+1341,1341
+1342,1342
+1343,1343
+1344,1344
+1345,1345
+1346,1346
+1347,1347
+1348,1348
+1349,1349
+1350,1350
+1351,1351
+1352,1352
+1353,1353
+1354,1354
+1355,1355
+1356,1356
+1357,1357
+1358,1358
+1359,1359
+1360,1360
+1361,1361
+1362,1362
+1363,1363
+1364,1364
+1365,1365
+1366,1366
+1367,1367
+1368,1368
+1369,1369
+1370,1370
+1371,1371
+1372,1372
+1373,1373
+1374,1374
+1375,1375
+1376,1376
+1377,1377
+1378,1378
+1379,1379
+1380,1380
+1381,1381
+1382,1382
+1383,1383
+1384,1384
+1385,1385
+1386,1386
+1387,1387
+1388,1388
+1389,1389
+1390,1390
+1391,1391
+1392,1392
+1393,1393
+1394,1394
+1395,1395
+1396,1396
+1397,1397
+1398,1398
+1399,1399
+1400,1400
+1401,1401
+1402,1402
+1403,1403
+1404,1404
+1405,1405
+1406,1406
+1407,1407
+1408,1408
+1409,1409
+1410,1410
+1411,1411
+1412,1412
+1413,1413
+1414,1414
+1415,1415
+1416,1416
+1417,1417
+1418,1418
+1419,1419
+1420,1420
+1421,1421
+1422,1422
+1423,1423
+1424,1424
+1425,1425
+1426,1426
+1427,1427
+1428,1428
+1429,1429
+1430,1430
+1431,1431
+1432,1432
+1433,1433
+1434,1434
+1435,1435
+1436,1436
+1437,1437
+1438,1438
+1439,1439
+1440,1440
+1441,1441
+1442,1442
+1443,1443
+1444,1444
+1445,1445
+1446,1446
+1447,1447
+1448,1448
+1449,1449
+1450,1450
+1451,1451
+1452,1452
+1453,1453
+1454,1454
+1455,1455
+1456,1456
+1457,1457
+1458,1458
+1459,1459
+1460,1460
+1461,1461
+1462,1462
+1463,1463
+1464,1464
+1465,1465
+1466,1466
+1467,1467
+1468,1468
+1469,1469
+1470,1470
+1471,1471
+1472,1472
+1473,1473
+1474,1474
+1475,1475
+1476,1476
+1477,1477
+1478,1478
+1479,1479
+1480,1480
+1481,1481
+1482,1482
+1483,1483
+1484,1484
+1485,1485
+1486,1486
+1487,1487
+1488,1488
+1489,1489
+1490,1490
+1491,1491
+1492,1492
+1493,1493
+1494,1494
+1495,1495
+1496,1496
+1497,1497
+1498,1498
+1499,1499
+1500,1500
+1501,1501
+1502,1502
+1503,1503
+1504,1504
+1505,1505
+1506,1506
+1507,1507
+1508,1508
+1509,1509
+1510,1510
+1511,1511
+1512,1512
+1513,1513
+1514,1514
+1515,1515
+1516,1516
+1517,1517
+1518,1518
+1519,1519
+1520,1520
+1521,1521
+1522,1522
+1523,1523
+1524,1524
+1525,1525
+1526,1526
+1527,1527
+1528,1528
+1529,1529
+1530,1530
+1531,1531
+1532,1532
+1533,1533
+1534,1534
+1535,1535
+1536,1536
+1537,1537
+1538,1538
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv
new file mode 100644
index 00000000..49c7cdc7
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv
@@ -0,0 +1,539 @@
+PID,B(Lat),L(Long),H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3
+1001,0,0,0.041,129.675,314.065,0,0,0,0
+1002,0,0,0.04,130.691,314.065,0,0,0,0
+1003,0,0,0.04,131.708,314.064,0,0,0,0
+1004,0,0,0.04,132.718,314.064,0,0,0,0
+1005,0,0,0.04,133.745,314.064,0,0,0,0
+1006,0,0,0.04,134.801,314.066,0,0,0,0
+1007,0,0,0.04,135.862,314.068,0,0,0,0
+1008,0,0,0.04,136.865,314.07,0,0,0,0
+1009,0,0,0.04,137.897,314.072,0,0,0,0
+1010,0,0,0.04,138.902,314.075,0,0,0,0
+1011,0,0,0.04,139.921,314.077,0,0,0,0
+1012,0,0,0.04,140.939,314.08,0,0,0,0
+1013,0,0,0.04,141.951,314.082,0,0,0,0
+1014,0,0,0.04,142.983,314.084,0,0,0,0
+1015,0,0,0.04,144.003,314.086,0,0,0,0
+1016,0,0,0.04,145.06,314.106,0,0,0,0
+1017,0,0,0.04,146.06,314.118,0,0,0,0
+1018,0,0,0.04,147.107,314.129,0,0,0,0
+1019,0,0,0.04,148.109,314.14,0,0,0,0
+1020,0,0,0.04,149.118,314.15,0,0,0,0
+1021,0,0,0.04,150.136,314.161,0,0,0,0
+1022,0,0,0.04,151.154,314.172,0,0,0,0
+1023,0,0,0.04,152.171,314.183,0,0,0,0
+1024,0,0,0.04,153.198,314.193,0,0,0,0
+1025,0,0,0.04,154.224,314.204,0,0,0,0
+1026,0,0,0.04,155.287,314.215,0,0,0,0
+1027,0,0,0.04,156.302,314.226,0,0,0,0
+1028,0,0,0.04,157.312,314.237,0,0,0,0
+1029,0,0,0.04,158.379,314.248,0,0,0,0
+1030,0,0,0.04,159.455,314.259,0,0,0,0
+1031,0,0,0.04,160.51,314.271,0,0,0,0
+1032,0,0,0.044,161.521,314.282,0,0,0,0
+1033,0,0,0.057,162.574,314.296,0,0,0,0
+1034,0,0,0.073,163.634,314.311,0,0,0,0
+1035,0,0,0.081,164.666,314.326,0,0,0,0
+1036,0,0,0.097,165.729,314.344,0,0,0,0
+1037,0,0,0.107,166.753,314.363,0,0,0,0
+1038,0,0,0.106,167.858,314.382,0,0,0,0
+1039,0,0,0.106,168.978,314.402,0,0,0,0
+1040,0,0,0.106,170.06,314.42,0,0,0,0
+1041,0,0,0.106,171.126,314.438,0,0,0,0
+1042,0,0,0.105,172.155,314.455,0,0,0,0
+1043,0,0,0.105,173.164,314.473,0,0,0,0
+1044,0,0,0.105,174.225,314.49,0,0,0,0
+1045,0,0,0.099,175.296,314.508,0,0,0,0
+1046,0,0,0.083,176.319,314.524,0,0,0,0
+1047,0,0,0.076,177.46,314.542,0,0,0,0
+1048,0,0,0.059,178.598,314.558,0,0,0,0
+1049,0,0,0.047,179.602,314.571,0,0,0,0
+1050,0,0,0.043,180.621,314.584,0,0,0,0
+1051,0,0,0.04,181.718,314.598,0,0,0,0
+1052,0,0,0.04,182.79,314.611,0,0,0,0
+1053,0,0,0.04,183.825,314.624,0,0,0,0
+1054,0,0,0.04,184.87,314.637,0,0,0,0
+1055,0,0,0.04,185.896,314.649,0,0,0,0
+1056,0,0,0.04,187.05,314.664,0,0,0,0
+1057,0,0,0.04,188.124,314.677,0,0,0,0
+1058,0,0,0.04,189.203,314.69,0,0,0,0
+1059,0,0,0.04,190.271,314.703,0,0,0,0
+1060,0,0,0.04,191.384,314.717,0,0,0,0
+1061,0,0,0.04,192.396,314.729,0,0,0,0
+1062,0,0,0.04,193.474,314.743,0,0,0,0
+1063,0,0,0.04,194.501,314.756,0,0,0,0
+1064,0,0,0.04,195.517,314.768,0,0,0,0
+1065,0,0,0.04,196.577,314.781,0,0,0,0
+1066,0,0,0.04,197.587,314.794,0,0,0,0
+1067,0,0,0.04,198.625,314.806,0,0,0,0
+1068,0,0,0.04,199.67,314.819,0,0,0,0
+1069,0,0,0.04,200.715,314.832,0,0,0,0
+1070,0,0,0.04,201.723,314.844,0,0,0,0
+1071,0,0,0.04,202.797,314.858,0,0,0,0
+1072,0,0,0.04,203.836,314.871,0,0,0,0
+1073,0,0,0.04,204.877,314.884,0,0,0,0
+1074,0,0,0.04,205.9,314.896,0,0,0,0
+1075,0,0,0.04,206.914,314.908,0,0,0,0
+1076,0,0,0.04,207.922,314.92,0,0,0,0
+1077,0,0,0.04,208.936,314.931,0,0,0,0
+1078,0,0,0.04,209.952,314.943,0,0,0,0
+1079,0,0,0.04,210.974,314.955,0,0,0,0
+1080,0,0,0.04,211.985,314.966,0,0,0,0
+1081,0,0,0.04,213.011,314.978,0,0,0,0
+1082,0,0,0.04,214.037,314.99,0,0,0,0
+1083,0,0,0.04,215.109,315.002,0,0,0,0
+1084,0,0,0.04,216.203,315.014,0,0,0,0
+1085,0,0,0.04,217.212,315.026,0,0,0,0
+1086,0,0,0.04,218.226,315.038,0,0,0,0
+1087,0,0,0.04,219.321,315.05,0,0,0,0
+1088,0,0,0.04,220.33,315.062,0,0,0,0
+1089,0,0,0.04,221.382,315.074,0,0,0,0
+1090,0,0,0.04,222.454,315.086,0,0,0,0
+1091,0,0,0.04,223.615,315.099,0,0,0,0
+1092,0,0,0.04,224.672,315.111,0,0,0,0
+1093,0,0,0.04,225.705,315.123,0,0,0,0
+1094,0,0,0.04,226.826,315.136,0,0,0,0
+1095,0,0,0.04,227.872,315.148,0,0,0,0
+1096,0,0,0.04,228.885,315.16,0,0,0,0
+1097,0,0,0.04,230.0,315.173,0,0,0,0
+1098,0,0,0.04,231.02,315.184,0,0,0,0
+1099,0,0,0.04,232.043,315.189,0,0,0,0
+1100,0,0,0.04,233.113,315.197,0,0,0,0
+1101,0,0,0.04,234.143,315.205,0,0,0,0
+1102,0,0,0.04,235.147,315.214,0,0,0,0
+1103,0,0,0.04,236.19,315.223,0,0,0,0
+1104,0,0,0.04,237.216,315.232,0,0,0,0
+1105,0,0,0.04,238.337,315.243,0,0,0,0
+1106,0,0,0.04,239.383,315.253,0,0,0,0
+1107,0,0,0.04,240.532,315.263,0,0,0,0
+1108,0,0,0.04,241.549,315.273,0,0,0,0
+1109,0,0,0.04,242.63,315.283,0,0,0,0
+1110,0,0,0.04,243.672,315.293,0,0,0,0
+1111,0,0,0.04,244.725,315.303,0,0,0,0
+1112,0,0,0.04,245.757,315.312,0,0,0,0
+1113,0,0,0.04,246.832,315.322,0,0,0,0
+1114,0,0,0.04,247.849,315.332,0,0,0,0
+1115,0,0,0.04,248.861,315.341,0,0,0,0
+1116,0,0,0.04,249.983,315.351,0,0,0,0
+1117,0,0,0.04,251.03,315.36,0,0,0,0
+1118,0,0,0.04,252.044,315.369,0,0,0,0
+1119,0,0,0.04,253.082,315.379,0,0,0,0
+1120,0,0,0.04,254.086,315.388,0,0,0,0
+1121,0,0,0.04,255.094,315.396,0,0,0,0
+1122,0,0,0.04,256.112,315.406,0,0,0,0
+1123,0,0,0.04,257.132,315.415,0,0,0,0
+1124,0,0,0.04,258.155,315.424,0,0,0,0
+1125,0,0,0.04,259.207,315.434,0,0,0,0
+1126,0,0,0.04,260.216,315.443,0,0,0,0
+1127,0,0,0.04,261.256,315.451,0,0,0,0
+1128,0,0,0.04,262.261,315.46,0,0,0,0
+1129,0,0,0.04,263.277,315.468,0,0,0,0
+1130,0,0,0.04,264.326,315.477,0,0,0,0
+1131,0,0,0.04,265.345,315.486,0,0,0,0
+1132,0,0,0.04,266.411,315.495,0,0,0,0
+1133,0,0,0.04,267.451,315.489,0,0,0,0
+1134,0,0,0.04,268.476,315.468,0,0,0,0
+1135,0,0,0.04,269.535,315.413,0,0,0,0
+1136,0,0,0.04,270.531,315.265,0,0,0,0
+1137,0,0,0.04,271.547,315.158,0,0,0,0
+1138,0,0,0.04,272.559,315.05,0,0,0,0
+1139,0,0,0.04,273.578,314.942,0,0,0,0
+1140,0,0,0.04,274.575,314.835,0,0,0,0
+1141,0,0,0.04,275.579,314.715,0,0,0,0
+1142,0,0,0.04,276.575,314.54,0,0,0,0
+1143,0,0,0.04,277.614,314.381,0,0,0,0
+1144,0,0,0.04,278.601,314.204,0,0,0,0
+1145,0,0,0.04,279.581,313.96,0,0,0,0
+1146,0,0,0.04,280.56,313.714,0,0,0,0
+1147,0,0,0.04,281.543,313.481,0,0,0,0
+1148,0,0,0.04,282.527,313.248,0,0,0,0
+1149,0,0,0.04,283.519,313.013,0,0,0,0
+1150,0,0,0.04,284.52,312.746,0,0,0,0
+1151,0,0,0.04,285.475,312.445,0,0,0,0
+1152,0,0,0.04,286.412,312.09,0,0,0,0
+1153,0,0,0.04,287.373,311.745,0,0,0,0
+1154,0,0,0.04,288.343,311.409,0,0,0,0
+1155,0,0,0.04,289.274,311.0,0,0,0,0
+1156,0,0,0.04,290.205,310.624,0,0,0,0
+1157,0,0,0.04,291.159,310.24,0,0,0,0
+1158,0,0,0.04,292.084,309.796,0,0,0,0
+1159,0,0,0.04,293.016,309.367,0,0,0,0
+1160,0,0,0.04,293.971,308.908,0,0,0,0
+1161,0,0,0.04,294.88,308.483,0,0,0,0
+1162,0,0,0.04,295.784,308.018,0,0,0,0
+1163,0,0,0.04,296.743,307.478,0,0,0,0
+1164,0,0,0.04,297.636,306.93,0,0,0,0
+1165,0,0,0.04,298.498,306.347,0,0,0,0
+1166,0,0,0.04,299.335,305.787,0,0,0,0
+1167,0,0,0.04,300.13,305.165,0,0,0,0
+1168,0,0,0.04,300.874,304.497,0,0,0,0
+1169,0,0,0.04,301.67,303.856,0,0,0,0
+1170,0,0,0.04,302.439,303.194,0,0,0,0
+1171,0,0,0.04,303.161,302.486,0,0,0,0
+1172,0,0,0.04,303.779,301.7,0,0,0,0
+1173,0,0,0.04,304.384,300.861,0,0,0,0
+1174,0,0,0.04,304.956,300.033,0,0,0,0
+1175,0,0,0.04,305.533,299.183,0,0,0,0
+1176,0,0,0.04,306.09,298.344,0,0,0,0
+1177,0,0,0.04,306.609,297.475,0,0,0,0
+1178,0,0,0.04,307.14,296.601,0,0,0,0
+1179,0,0,0.04,307.616,295.704,0,0,0,0
+1180,0,0,0.04,307.944,294.752,0,0,0,0
+1181,0,0,0.04,308.317,293.808,0,0,0,0
+1182,0,0,0.04,308.686,292.858,0,0,0,0
+1183,0,0,0.04,308.984,291.883,0,0,0,0
+1184,0,0,0.04,309.315,290.926,0,0,0,0
+1185,0,0,0.04,309.589,289.947,0,0,0,0
+1186,0,0,0.04,309.848,288.964,0,0,0,0
+1187,0,0,0.04,309.999,287.97,0,0,0,0
+1188,0,0,0.04,310.177,286.946,0,0,0,0
+1189,0,0,0.04,310.283,285.926,0,0,0,0
+1190,0,0,0.04,310.371,284.912,0,0,0,0
+1191,0,0,0.04,310.447,283.908,0,0,0,0
+1192,0,0,0.04,310.508,282.891,0,0,0,0
+1193,0,0,0.04,310.559,281.88,0,0,0,0
+1194,0,0,0.04,310.597,280.847,0,0,0,0
+1195,0,0,0.04,310.647,279.829,0,0,0,0
+1196,0,0,0.04,310.697,278.789,0,0,0,0
+1197,0,0,0.04,310.745,277.778,0,0,0,0
+1198,0,0,0.04,310.781,276.772,0,0,0,0
+1199,0,0,0.04,310.795,275.754,0,0,0,0
+1200,0,0,0.04,310.794,274.732,0,0,0,0
+1201,0,0,0.04,310.806,273.729,0,0,0,0
+1202,0,0,0.04,310.819,272.663,0,0,0,0
+1203,0,0,0.04,310.831,271.638,0,0,0,0
+1204,0,0,0.04,310.843,270.601,0,0,0,0
+1205,0,0,0.04,310.854,269.569,0,0,0,0
+1206,0,0,0.04,310.866,268.519,0,0,0,0
+1207,0,0,0.04,310.878,267.47,0,0,0,0
+1208,0,0,0.04,310.889,266.455,0,0,0,0
+1209,0,0,0.04,310.901,265.416,0,0,0,0
+1210,0,0,0.04,310.913,264.382,0,0,0,0
+1211,0,0,0.04,310.924,263.362,0,0,0,0
+1212,0,0,0.041,310.936,262.355,0,0,0,0
+1213,0,0,0.041,310.948,261.253,0,0,0,0
+1214,0,0,0.041,310.961,260.185,0,0,0,0
+1215,0,0,0.042,310.973,259.12,0,0,0,0
+1216,0,0,0.042,310.985,258.089,0,0,0,0
+1217,0,0,0.042,310.997,257.076,0,0,0,0
+1218,0,0,0.042,311.009,256.063,0,0,0,0
+1219,0,0,0.042,311.022,255.038,0,0,0,0
+1220,0,0,0.042,311.034,254.01,0,0,0,0
+1221,0,0,0.041,311.046,252.982,0,0,0,0
+1222,0,0,0.041,311.058,251.978,0,0,0,0
+1223,0,0,0.04,311.07,250.92,0,0,0,0
+1224,0,0,0.04,311.081,249.888,0,0,0,0
+1225,0,0,0.04,311.091,248.882,0,0,0,0
+1226,0,0,0.04,311.101,247.881,0,0,0,0
+1227,0,0,0.04,311.11,246.835,0,0,0,0
+1228,0,0,0.04,311.12,245.793,0,0,0,0
+1229,0,0,0.04,311.129,244.762,0,0,0,0
+1230,0,0,0.04,311.138,243.746,0,0,0,0
+1231,0,0,0.04,311.147,242.739,0,0,0,0
+1232,0,0,0.04,311.156,241.721,0,0,0,0
+1233,0,0,0.04,311.164,240.714,0,0,0,0
+1234,0,0,0.04,311.174,239.674,0,0,0,0
+1235,0,0,0.04,311.184,238.671,0,0,0,0
+1236,0,0,0.04,311.193,237.614,0,0,0,0
+1237,0,0,0.04,311.202,236.601,0,0,0,0
+1238,0,0,0.04,311.211,235.6,0,0,0,0
+1239,0,0,0.04,311.221,234.531,0,0,0,0
+1240,0,0,0.04,311.23,233.529,0,0,0,0
+1241,0,0,0.04,311.239,232.499,0,0,0,0
+1242,0,0,0.04,311.248,231.437,0,0,0,0
+1243,0,0,0.04,311.257,230.436,0,0,0,0
+1244,0,0,0.04,311.267,229.406,0,0,0,0
+1245,0,0,0.04,311.276,228.387,0,0,0,0
+1246,0,0,0.04,311.285,227.348,0,0,0,0
+1247,0,0,0.04,311.294,226.335,0,0,0,0
+1248,0,0,0.04,311.303,225.271,0,0,0,0
+1249,0,0,0.04,311.311,224.217,0,0,0,0
+1250,0,0,0.04,311.319,223.18,0,0,0,0
+1251,0,0,0.04,311.327,222.147,0,0,0,0
+1252,0,0,0.04,311.335,221.081,0,0,0,0
+1253,0,0,0.04,311.342,220.077,0,0,0,0
+1254,0,0,0.04,311.349,219.047,0,0,0,0
+1255,0,0,0.04,311.355,218.017,0,0,0,0
+1256,0,0,0.04,311.36,216.998,0,0,0,0
+1257,0,0,0.04,311.365,215.981,0,0,0,0
+1258,0,0,0.04,311.369,214.965,0,0,0,0
+1259,0,0,0.04,311.373,213.941,0,0,0,0
+1260,0,0,0.04,311.343,212.913,0,0,0,0
+1261,0,0,0.042,311.248,211.906,0,0,0,0
+1262,0,0,0.043,311.187,210.897,0,0,0,0
+1263,0,0,0.044,311.046,209.899,0,0,0,0
+1264,0,0,0.048,310.824,208.922,0,0,0,0
+1265,0,0,0.05,310.516,207.949,0,0,0,0
+1266,0,0,0.052,310.185,207.001,0,0,0,0
+1267,0,0,0.055,309.748,206.095,0,0,0,0
+1268,0,0,0.057,309.177,205.27,0,0,0,0
+1269,0,0,0.059,308.593,204.446,0,0,0,0
+1270,0,0,0.06,307.926,203.69,0,0,0,0
+1271,0,0,0.06,307.133,203.067,0,0,0,0
+1272,0,0,0.058,306.226,202.628,0,0,0,0
+1273,0,0,0.057,305.273,202.298,0,0,0,0
+1274,0,0,0.056,304.284,202.02,0,0,0,0
+1275,0,0,0.056,303.281,201.874,0,0,0,0
+1276,0,0,0.055,302.284,201.781,0,0,0,0
+1277,0,0,0.055,301.277,201.779,0,0,0,0
+1278,0,0,0.055,300.249,201.769,0,0,0,0
+1279,0,0,0.055,299.234,201.716,0,0,0,0
+1280,0,0,0.055,298.159,201.666,0,0,0,0
+1281,0,0,0.055,297.145,201.589,0,0,0,0
+1282,0,0,0.055,296.147,201.529,0,0,0,0
+1283,0,0,0.055,295.121,201.467,0,0,0,0
+1284,0,0,0.055,294.114,201.442,0,0,0,0
+1285,0,0,0.055,293.11,201.403,0,0,0,0
+1286,0,0,0.055,292.083,201.374,0,0,0,0
+1287,0,0,0.055,291.079,201.34,0,0,0,0
+1288,0,0,0.055,290.072,201.307,0,0,0,0
+1289,0,0,0.055,289.048,201.274,0,0,0,0
+1290,0,0,0.055,288.04,201.259,0,0,0,0
+1291,0,0,0.055,287.022,201.236,0,0,0,0
+1292,0,0,0.055,286.012,201.214,0,0,0,0
+1293,0,0,0.055,285.001,201.192,0,0,0,0
+1294,0,0,0.055,283.967,201.169,0,0,0,0
+1295,0,0,0.055,282.912,201.145,0,0,0,0
+1296,0,0,0.055,281.895,201.122,0,0,0,0
+1297,0,0,0.055,280.838,201.097,0,0,0,0
+1298,0,0,0.055,279.786,201.073,0,0,0,0
+1299,0,0,0.055,278.779,201.05,0,0,0,0
+1300,0,0,0.055,277.703,201.026,0,0,0,0
+1301,0,0,0.055,276.592,201.001,0,0,0,0
+1302,0,0,0.055,275.544,200.977,0,0,0,0
+1303,0,0,0.055,274.516,200.954,0,0,0,0
+1304,0,0,0.055,273.489,200.931,0,0,0,0
+1305,0,0,0.055,272.469,200.908,0,0,0,0
+1306,0,0,0.055,271.469,200.885,0,0,0,0
+1307,0,0,0.055,270.465,200.862,0,0,0,0
+1308,0,0,0.055,269.413,200.839,0,0,0,0
+1309,0,0,0.055,268.395,200.816,0,0,0,0
+1310,0,0,0.055,267.356,200.793,0,0,0,0
+1311,0,0,0.055,266.344,200.77,0,0,0,0
+1312,0,0,0.055,265.306,200.747,0,0,0,0
+1313,0,0,0.055,264.206,200.722,0,0,0,0
+1314,0,0,0.055,263.138,200.698,0,0,0,0
+1315,0,0,0.055,262.101,200.675,0,0,0,0
+1316,0,0,0.055,261.081,200.652,0,0,0,0
+1317,0,0,0.055,259.992,200.628,0,0,0,0
+1318,0,0,0.055,258.808,200.601,0,0,0,0
+1319,0,0,0.055,257.639,200.575,0,0,0,0
+1320,0,0,0.055,256.545,200.55,0,0,0,0
+1321,0,0,0.055,255.51,200.527,0,0,0,0
+1322,0,0,0.057,254.446,200.503,0,0,0,0
+1323,0,0,0.057,253.43,200.48,0,0,0,0
+1324,0,0,0.057,252.377,200.457,0,0,0,0
+1325,0,0,0.058,251.305,200.433,0,0,0,0
+1326,0,0,0.059,250.233,200.41,0,0,0,0
+1327,0,0,0.06,249.195,200.387,0,0,0,0
+1328,0,0,0.06,248.111,200.363,0,0,0,0
+1329,0,0,0.06,247.042,200.333,0,0,0,0
+1330,0,0,0.061,245.893,200.304,0,0,0,0
+1331,0,0,0.06,244.748,200.275,0,0,0,0
+1332,0,0,0.06,243.614,200.247,0,0,0,0
+1333,0,0,0.059,242.603,200.221,0,0,0,0
+1334,0,0,0.058,241.58,200.195,0,0,0,0
+1335,0,0,0.057,240.545,200.168,0,0,0,0
+1336,0,0,0.056,239.492,200.142,0,0,0,0
+1337,0,0,0.055,238.416,200.114,0,0,0,0
+1338,0,0,0.055,237.335,200.086,0,0,0,0
+1339,0,0,0.055,236.265,200.059,0,0,0,0
+1340,0,0,0.055,235.193,200.031,0,0,0,0
+1341,0,0,0.055,234.171,200.007,0,0,0,0
+1342,0,0,0.055,233.159,200.009,0,0,0,0
+1343,0,0,0.055,232.016,199.995,0,0,0,0
+1344,0,0,0.055,231.001,199.965,0,0,0,0
+1345,0,0,0.055,229.934,199.938,0,0,0,0
+1346,0,0,0.055,228.932,199.913,0,0,0,0
+1347,0,0,0.055,227.91,199.888,0,0,0,0
+1348,0,0,0.055,226.885,199.863,0,0,0,0
+1349,0,0,0.055,225.858,199.838,0,0,0,0
+1350,0,0,0.055,224.852,199.813,0,0,0,0
+1351,0,0,0.055,223.798,199.787,0,0,0,0
+1352,0,0,0.055,222.759,199.761,0,0,0,0
+1353,0,0,0.055,221.686,199.734,0,0,0,0
+1354,0,0,0.055,220.682,199.709,0,0,0,0
+1355,0,0,0.055,219.634,199.684,0,0,0,0
+1356,0,0,0.055,218.576,199.658,0,0,0,0
+1357,0,0,0.055,217.558,199.632,0,0,0,0
+1358,0,0,0.055,216.538,199.607,0,0,0,0
+1359,0,0,0.055,215.439,199.58,0,0,0,0
+1360,0,0,0.055,214.308,199.552,0,0,0,0
+1361,0,0,0.055,213.153,199.524,0,0,0,0
+1362,0,0,0.055,212.125,199.498,0,0,0,0
+1363,0,0,0.055,211.068,199.472,0,0,0,0
+1364,0,0,0.055,210.007,199.446,0,0,0,0
+1365,0,0,0.055,208.957,199.42,0,0,0,0
+1366,0,0,0.055,207.931,199.395,0,0,0,0
+1367,0,0,0.055,206.88,199.369,0,0,0,0
+1368,0,0,0.055,205.839,199.343,0,0,0,0
+1369,0,0,0.055,204.773,199.317,0,0,0,0
+1370,0,0,0.055,203.682,199.29,0,0,0,0
+1371,0,0,0.055,202.545,199.261,0,0,0,0
+1372,0,0,0.055,201.505,199.236,0,0,0,0
+1373,0,0,0.055,200.451,199.21,0,0,0,0
+1374,0,0,0.055,199.332,199.182,0,0,0,0
+1375,0,0,0.055,198.283,199.156,0,0,0,0
+1376,0,0,0.055,197.186,199.129,0,0,0,0
+1377,0,0,0.055,196.108,199.102,0,0,0,0
+1378,0,0,0.055,195.103,199.077,0,0,0,0
+1379,0,0,0.055,194.052,199.051,0,0,0,0
+1380,0,0,0.055,193.036,199.026,0,0,0,0
+1381,0,0,0.055,191.994,199.001,0,0,0,0
+1382,0,0,0.055,190.904,198.974,0,0,0,0
+1383,0,0,0.055,189.806,198.946,0,0,0,0
+1384,0,0,0.055,188.77,198.921,0,0,0,0
+1385,0,0,0.055,187.678,198.894,0,0,0,0
+1386,0,0,0.055,186.563,198.866,0,0,0,0
+1387,0,0,0.055,185.384,198.837,0,0,0,0
+1388,0,0,0.055,184.288,198.81,0,0,0,0
+1389,0,0,0.055,183.269,198.785,0,0,0,0
+1390,0,0,0.055,182.198,198.758,0,0,0,0
+1391,0,0,0.055,181.154,198.733,0,0,0,0
+1392,0,0,0.055,180.045,198.705,0,0,0,0
+1393,0,0,0.061,179.011,198.68,0,0,0,0
+1394,0,0,0.084,177.98,198.654,0,0,0,0
+1395,0,0,0.081,176.959,198.63,0,0,0,0
+1396,0,0,0.09,175.942,198.605,0,0,0,0
+1397,0,0,0.11,174.916,198.582,0,0,0,0
+1398,0,0,0.105,173.805,198.555,0,0,0,0
+1399,0,0,0.103,172.759,198.531,0,0,0,0
+1400,0,0,0.1,171.697,198.506,0,0,0,0
+1401,0,0,0.098,170.696,198.482,0,0,0,0
+1402,0,0,0.09,169.636,198.456,0,0,0,0
+1403,0,0,0.082,168.595,198.43,0,0,0,0
+1404,0,0,0.075,167.576,198.405,0,0,0,0
+1405,0,0,0.066,166.532,198.391,0,0,0,0
+1406,0,0,0.063,165.489,198.374,0,0,0,0
+1407,0,0,0.059,164.481,198.356,0,0,0,0
+1408,0,0,0.055,163.456,198.4,0,0,0,0
+1409,0,0,0.055,162.455,198.412,0,0,0,0
+1410,0,0,0.055,161.443,198.46,0,0,0,0
+1411,0,0,0.055,160.439,198.491,0,0,0,0
+1412,0,0,0.055,159.431,198.523,0,0,0,0
+1413,0,0,0.055,158.375,198.558,0,0,0,0
+1414,0,0,0.055,157.338,198.664,0,0,0,0
+1415,0,0,0.055,156.273,198.732,0,0,0,0
+1416,0,0,0.055,155.231,198.843,0,0,0,0
+1417,0,0,0.055,154.229,198.947,0,0,0,0
+1418,0,0,0.055,153.232,199.058,0,0,0,0
+1419,0,0,0.055,152.225,199.232,0,0,0,0
+1420,0,0,0.055,151.238,199.404,0,0,0,0
+1421,0,0,0.055,150.264,199.651,0,0,0,0
+1422,0,0,0.055,149.296,199.938,0,0,0,0
+1423,0,0,0.055,148.332,200.217,0,0,0,0
+1424,0,0,0.055,147.376,200.528,0,0,0,0
+1425,0,0,0.055,146.404,200.869,0,0,0,0
+1426,0,0,0.055,145.442,201.226,0,0,0,0
+1427,0,0,0.055,144.477,201.566,0,0,0,0
+1428,0,0,0.055,143.488,201.937,0,0,0,0
+1429,0,0,0.055,142.551,202.319,0,0,0,0
+1430,0,0,0.055,141.609,202.686,0,0,0,0
+1431,0,0,0.055,140.712,203.159,0,0,0,0
+1432,0,0,0.055,139.799,203.594,0,0,0,0
+1433,0,0,0.055,138.893,204.081,0,0,0,0
+1434,0,0,0.055,137.998,204.569,0,0,0,0
+1435,0,0,0.055,137.096,205.05,0,0,0,0
+1436,0,0,0.055,136.24,205.594,0,0,0,0
+1437,0,0,0.055,135.408,206.148,0,0,0,0
+1438,0,0,0.055,134.558,206.705,0,0,0,0
+1439,0,0,0.055,133.735,207.296,0,0,0,0
+1440,0,0,0.055,132.948,207.925,0,0,0,0
+1441,0,0,0.055,132.133,208.534,0,0,0,0
+1442,0,0,0.055,131.312,209.148,0,0,0,0
+1443,0,0,0.055,130.509,209.833,0,0,0,0
+1444,0,0,0.055,129.781,210.533,0,0,0,0
+1445,0,0,0.055,129.016,211.238,0,0,0,0
+1446,0,0,0.055,128.248,211.933,0,0,0,0
+1447,0,0,0.055,127.482,212.625,0,0,0,0
+1448,0,0,0.055,126.806,213.379,0,0,0,0
+1449,0,0,0.055,126.041,214.151,0,0,0,0
+1450,0,0,0.055,125.374,214.961,0,0,0,0
+1451,0,0,0.055,124.817,215.813,0,0,0,0
+1452,0,0,0.055,124.342,216.698,0,0,0,0
+1453,0,0,0.055,123.913,217.627,0,0,0,0
+1454,0,0,0.055,123.429,218.518,0,0,0,0
+1455,0,0,0.055,123.013,219.457,0,0,0,0
+1456,0,0,0.055,122.572,220.373,0,0,0,0
+1457,0,0,0.055,122.12,221.308,0,0,0,0
+1458,0,0,0.055,121.754,222.24,0,0,0,0
+1459,0,0,0.055,121.354,223.191,0,0,0,0
+1460,0,0,0.055,120.995,224.137,0,0,0,0
+1461,0,0,0.055,120.681,225.09,0,0,0,0
+1462,0,0,0.055,120.406,226.097,0,0,0,0
+1463,0,0,0.055,120.176,227.112,0,0,0,0
+1464,0,0,0.055,120.014,228.123,0,0,0,0
+1465,0,0,0.055,119.842,229.165,0,0,0,0
+1466,0,0,0.055,119.679,230.222,0,0,0,0
+1467,0,0,0.055,119.573,231.253,0,0,0,0
+1468,0,0,0.055,119.518,232.302,0,0,0,0
+1469,0,0,0.055,119.498,233.305,0,0,0,0
+1470,0,0,0.055,119.405,234.333,0,0,0,0
+1471,0,0,0.055,119.316,235.356,0,0,0,0
+1472,0,0,0.055,119.239,236.385,0,0,0,0
+1473,0,0,0.055,119.177,237.395,0,0,0,0
+1474,0,0,0.055,119.15,238.4,0,0,0,0
+1475,0,0,0.055,119.103,239.416,0,0,0,0
+1476,0,0,0.055,119.054,240.444,0,0,0,0
+1477,0,0,0.055,119.007,241.464,0,0,0,0
+1478,0,0,0.055,118.96,242.486,0,0,0,0
+1479,0,0,0.055,118.92,243.524,0,0,0,0
+1480,0,0,0.055,118.907,244.542,0,0,0,0
+1481,0,0,0.055,118.879,245.583,0,0,0,0
+1482,0,0,0.055,118.852,246.618,0,0,0,0
+1483,0,0,0.055,118.837,247.64,0,0,0,0
+1484,0,0,0.055,118.823,248.651,0,0,0,0
+1485,0,0,0.055,118.807,249.659,0,0,0,0
+1486,0,0,0.055,118.821,250.691,0,0,0,0
+1487,0,0,0.055,118.822,251.724,0,0,0,0
+1488,0,0,0.055,118.823,252.77,0,0,0,0
+1489,0,0,0.055,118.825,253.772,0,0,0,0
+1490,0,0,0.055,118.827,254.798,0,0,0,0
+1491,0,0,0.055,118.829,255.804,0,0,0,0
+1492,0,0,0.055,118.83,256.839,0,0,0,0
+1493,0,0,0.055,118.832,257.845,0,0,0,0
+1494,0,0,0.055,118.823,258.898,0,0,0,0
+1495,0,0,0.055,118.817,259.937,0,0,0,0
+1496,0,0,0.055,118.812,260.966,0,0,0,0
+1497,0,0,0.055,118.806,261.979,0,0,0,0
+1498,0,0,0.055,118.801,262.987,0,0,0,0
+1499,0,0,0.055,118.795,263.994,0,0,0,0
+1500,0,0,0.055,118.789,265.071,0,0,0,0
+1501,0,0,0.055,118.771,266.232,0,0,0,0
+1502,0,0,0.055,118.756,267.27,0,0,0,0
+1503,0,0,0.055,118.741,268.31,0,0,0,0
+1504,0,0,0.055,118.726,269.376,0,0,0,0
+1505,0,0,0.055,118.711,270.427,0,0,0,0
+1506,0,0,0.055,118.697,271.486,0,0,0,0
+1507,0,0,0.055,118.682,272.544,0,0,0,0
+1508,0,0,0.055,118.65,273.584,0,0,0,0
+1509,0,0,0.055,118.624,274.625,0,0,0,0
+1510,0,0,0.055,118.57,275.636,0,0,0,0
+1511,0,0,0.055,118.526,276.675,0,0,0,0
+1512,0,0,0.055,118.482,277.694,0,0,0,0
+1513,0,0,0.055,118.437,278.698,0,0,0,0
+1514,0,0,0.055,118.393,279.707,0,0,0,0
+1515,0,0,0.055,118.347,280.73,0,0,0,0
+1516,0,0,0.055,118.303,281.738,0,0,0,0
+1517,0,0,0.055,118.257,282.757,0,0,0,0
+1518,0,0,0.055,118.239,283.759,0,0,0,0
+1519,0,0,0.055,118.21,284.764,0,0,0,0
+1520,0,0,0.055,118.221,285.79,0,0,0,0
+1521,0,0,0.055,118.21,286.829,0,0,0,0
+1522,0,0,0.055,118.199,287.845,0,0,0,0
+1523,0,0,0.055,118.189,288.869,0,0,0,0
+1524,0,0,0.055,118.178,289.948,0,0,0,0
+1525,0,0,0.055,118.167,290.972,0,0,0,0
+1526,0,0,0.055,118.156,292.054,0,0,0,0
+1527,0,0,0.055,118.145,293.133,0,0,0,0
+1528,0,0,0.055,118.126,294.186,0,0,0,0
+1529,0,0,0.055,118.099,295.187,0,0,0,0
+1530,0,0,0.055,118.078,296.198,0,0,0,0
+1531,0,0,0.055,118.056,297.239,0,0,0,0
+1532,0,0,0.055,118.035,298.264,0,0,0,0
+1533,0,0,0.055,118.014,299.268,0,0,0,0
+1534,0,0,0.055,117.991,300.327,0,0,0,0
+1535,0,0,0.055,117.971,301.328,0,0,0,0
+1536,0,0,0.055,117.948,302.415,0,0,0,0
+1537,0,0,0.055,117.927,303.433,0,0,0,0
+1538,0,0,0.055,117.907,304.449,0,0,0,0
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv
new file mode 100644
index 00000000..bdbe3cb0
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv
@@ -0,0 +1,160 @@
+DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW
+1001,0,1001,1.5769552072461928,0,90000000000,0,0,2,2
+1002,1,1002,1.5769552072461928,0,90000000000,0,0,2,2
+1003,2,1003,1.5737316285471363,0,-310.21423497205427,0,0,2,2
+1004,3,1004,1.5725114416586194,0,-819.5465870113417,0,0,2,2
+1005,4,1005,1.57844374331971,0,168.5686361094724,0,0,2,2
+1006,5,1006,1.5663694826442065,0,-82.8208059172365,0,0,2,2
+1007,6,1007,1.5733395387307152,0,143.47086846770284,0,0,2,2
+1008,7,1008,1.5792896863938737,0,168.06305601316365,0,0,2,2
+1009,8,1009,1.5777890561648253,0,-666.3866825035068,0,0,2,2
+1010,9,1010,1.5750140106955042,0,-360.3544558297345,0,0,2,2
+1011,10,1011,1.5746689519147377,0,-2898.0569564947527,0,0,2,2
+1012,11,1012,1.5687257363219358,0,-168.259082038207,0,0,2,2
+1013,12,1013,1.5777541674329927,0,110.76121506596262,0,0,2,2
+1014,13,1014,1.5729688000176911,0,-208.97037013342555,0,0,2,2
+1015,14,1015,1.5804808525982976,0,133.11940901234524,0,0,2,2
+1016,15,1016,1.5711309651106926,0,-106.95315866909446,0,0,2,2
+1017,16,1017,1.5737926208556858,0,375.7059874783122,0,0,2,2
+1018,17,1018,1.5761274035452846,0,428.30538553113007,0,0,2,2
+1019,18,1019,1.5702416354892954,0,-169.90136044902954,0,0,2,2
+1020,19,1020,1.578198355029532,0,125.67993567488075,0,0,2,2
+1021,20,1021,1.5720181705458571,0,-161.80746750222914,0,0,2,2
+1022,21,1022,1.5706055651130575,0,-707.9117613317552,0,0,2,2
+1023,22,1023,1.568668187450491,0,-516.1616236843148,0,0,2,2
+1024,23,1024,1.5738116929901291,0,194.4199325330892,0,0,2,2
+1025,24,1025,1.5681610697806125,0,-176.97163001699232,0,0,2,2
+1026,25,1026,1.576233551996162,0,123.87763432587761,0,0,2,2
+1027,26,1027,1.5771205911255413,0,1127.3459838236336,0,0,2,2
+1028,27,1028,1.572915733582594,0,-237.82018529433887,0,0,2,2
+1029,28,1029,1.5762353397531932,0,301.2405534297222,0,0,2,2
+1030,29,1030,1.5753996131209225,0,-1196.5635189618679,0,0,2,2
+1031,30,1031,1.583323885143584,0,126.19455732214087,0,0,2,2
+1032,31,1032,1.5763200772449486,0,-142.77947289143017,0,0,2,2
+1033,32,1033,1.581582891357367,0,190.01241135238598,0,0,2,2
+1034,33,1034,1.587602910539133,0,166.11242751998466,0,0,2,2
+1035,34,1035,1.567492964011488,0,-49.7266364495457,0,0,2,2
+1036,35,1036,1.580190068162111,0,78.75811587722826,0,0,2,2
+1037,36,1037,1.5753588285894533,0,-206.98621646905497,0,0,2,2
+1038,37,1038,1.5770779342184096,0,581.6978219116953,0,0,2,2
+1039,38,1039,1.5778070212815072,0,1371.578307467697,0,0,2,2
+1040,39,1040,1.57688054066635,0,-1079.3533978369474,0,0,2,2
+1041,40,1041,1.5693145277312603,0,-132.17000930069833,0,0,2,2
+1042,41,1042,1.5765282755232228,0,138.62419769016512,0,0,2,2
+1043,42,1043,1.5620408548588791,0,-69.02539956344258,0,0,2,2
+1044,43,1044,1.574392985650641,0,80.95769198517125,0,0,2,2
+1045,44,1045,1.5781552304956135,0,265.7987561166538,0,0,2,2
+1046,45,1046,1.5636488564992264,0,-68.9352142891846,0,0,2,2
+1047,46,1047,1.576282511813594,0,79.15365546365315,0,0,2,2
+1048,47,1048,1.5706338534883697,0,-177.0331895513095,0,0,2,2
+1049,48,1049,1.5697332764760084,0,-1110.3992065908515,0,0,2,2
+1050,49,1050,1.572074578284328,0,427.1128123877896,0,0,2,2
+1051,50,1051,1.5709788389795292,0,-912.6258368395728,0,0,2,2
+1052,51,1052,1.5707963267,0,-5479.083394168603,0,0,2,2
+1053,52,1053,1.5750698586722753,0,233.99848333592325,0,0,2,2
+1054,53,1054,1.5720016133095074,0,-325.9191758699087,0,0,2,2
+1055,54,1055,1.5710102507774575,0,-1008.7127238228784,0,0,2,2
+1056,55,1056,1.5741634461732454,0,317.1386084528101,0,0,2,2
+1057,56,1057,1.5697313502942065,0,-225.6268878860186,0,0,2,2
+1058,57,1058,1.5783787159974525,0,115.64215442219911,0,0,2,2
+1059,58,1059,1.5701001710798361,0,-120.7941745743319,0,0,2,2
+1060,59,1060,1.5760334581575526,0,168.54063976706658,0,0,2,2
+1061,60,1061,1.5701780200451096,0,-170.78141392613784,0,0,2,2
+1062,61,1062,1.5739481064805068,0,265.2459080542718,0,0,2,2
+1063,62,1063,1.5760566970430587,0,474.25043901824483,0,0,2,2
+1064,63,1064,1.5740497522036763,0,-498.2697981413879,0,0,2,2
+1065,64,1065,1.5734512687362876,0,-1670.8899317858372,0,0,2,2
+1066,65,1066,1.5766194230917199,0,315.641186574559,0,0,2,2
+1067,66,1067,1.5694254082809356,0,-139.00443998265538,0,0,2,2
+1068,67,1068,1.5794433316084266,0,99.82108739600862,0,0,2,2
+1069,68,1069,1.5725889467252774,0,-145.8920117629209,0,0,2,2
+1070,69,1070,1.5807873561845662,0,121.97487878176102,0,0,2,2
+1071,70,1071,1.5739130500618594,0,-145.46922731544663,0,0,2,2
+1072,71,1072,1.5718091127823781,0,-475.29933983893517,0,0,2,2
+1073,72,1073,1.5707963267,0,-987.3753375954086,0,0,2,2
+1074,73,1074,1.5787771988715407,0,125.29958862966704,0,0,2,2
+1075,74,1075,1.5676375831346219,0,-89.76970333777409,0,0,2,2
+1076,75,1076,1.5799596823472497,0,81.15500311628622,0,0,2,2
+1077,76,1077,1.5718318856079825,0,-123.03457284663308,0,0,2,2
+1078,77,1078,1.573141750227899,0,763.4376750046916,0,0,2,2
+1079,78,1079,1.5728167843087149,0,-3077.245769373193,0,0,2,2
+1080,79,1080,1.5785502637358915,0,174.41416031947614,0,0,2,2
+1081,80,1081,1.569674339102679,0,-112.66431851597149,0,0,2,2
+1082,81,1082,1.5762914026465802,0,151.12443659721072,0,0,2,2
+1083,82,1083,1.5779954635538054,0,586.8334845075173,0,0,2,2
+1084,83,1084,1.5718615508136478,0,-163.02807724230902,0,0,2,2
+1085,84,1085,1.5801819377192137,0,120.18671864057802,0,0,2,2
+1086,85,1086,1.5735396017275092,0,-150.54944544342285,0,0,2,2
+1087,86,1087,1.5747067556354672,0,856.7850333890341,0,0,2,2
+1088,87,1088,1.5794144265235939,0,212.4192671416631,0,0,2,2
+1089,88,1089,1.5679123444827223,0,-86.94078136867701,0,0,2,2
+1090,89,1090,1.5776885650677432,0,102.28901765291539,0,0,2,2
+1091,90,1091,1.5772802126750645,0,-2448.8652887277844,0,0,2,2
+1092,91,1092,1.5627397146614799,0,-68.77343534353095,0,0,2,2
+1093,92,1093,1.585936009663618,0,43.110332917727945,0,0,2,2
+1094,93,1094,1.5619935809487495,0,-41.7668571517556,0,0,2,2
+1095,94,1095,1.5876906095116257,0,38.91500519420658,0,0,2,2
+1096,95,1096,1.5454624617424062,0,-23.680887105564942,0,0,2,2
+1097,96,1097,1.5873645175601516,0,23.86517750703063,0,0,2,2
+1098,97,1098,1.570944021915078,0,-60.89950155067454,0,0,2,2
+1099,98,1099,1.5807948187074246,0,101.51463085472905,0,0,2,2
+1100,99,1100,1.571359730727884,0,-105.9873529709988,0,0,2,2
+1101,100,1101,1.5698360710217596,0,-656.3145274371365,0,0,2,2
+1102,101,1102,1.5778075823117197,0,125.4467269286155,0,0,2,2
+1103,102,1103,1.5842673129254203,0,154.8052170904881,0,0,2,2
+1104,103,1104,1.577986173407375,0,-159.20678041413706,0,0,2,2
+1105,104,1105,1.5728062349336525,0,-193.0524860619341,0,0,2,2
+1106,105,1106,1.5681396089673467,0,-214.28758319613829,0,0,2,2
+1107,106,1107,1.5668894971276457,0,-799.9284289949252,0,0,2,2
+1108,107,1108,1.576207785892031,0,107.31584149034198,0,0,2,2
+1109,108,1109,1.5583652788716873,0,-56.04593563336265,0,0,2,2
+1110,109,1110,1.577050748676428,0,53.51751978675366,0,0,2,2
+1111,110,1111,1.5845470828584913,0,133.39853529912384,0,0,2,2
+1112,111,1112,1.574603990145371,0,-100.57232984265129,0,0,2,2
+1113,112,1113,1.5714870138239672,0,-320.8237397034875,0,0,2,2
+1114,113,1114,1.5780119468725389,0,153.25827752652526,0,0,2,2
+1115,114,1115,1.5843865999601219,0,156.87128166203587,0,0,2,2
+1116,115,1116,1.5557032253732601,0,-34.863401339744854,0,0,2,2
+1117,116,1117,1.576884531537484,0,47.211441647968,0,0,2,2
+1118,117,1118,1.5766306482410994,0,-3938.817615182425,0,0,2,2
+1119,118,1119,1.5734950747595073,0,-318.9209265452281,0,0,2,2
+1120,119,1120,1.5775238109552248,0,248.21679837538343,0,0,2,2
+1121,120,1121,1.5735982325932878,0,-254.73953333759928,0,0,2,2
+1122,121,1122,1.5701436241046975,0,-289.46840236824585,0,0,2,2
+1123,122,1123,1.5758215401952662,0,176.1209542460578,0,0,2,2
+1124,123,1124,1.558982377070755,0,-59.385374000230954,0,0,2,2
+1125,124,1125,1.5763367431657738,0,57.62238704224561,0,0,2,2
+1126,125,1126,1.564720140088975,0,-86.0836849971439,0,0,2,2
+1127,126,1127,1.576122881314287,0,87.69821047768663,0,0,2,2
+1128,127,1128,1.579527990768055,0,293.6763160119224,0,0,2,2
+1129,128,1129,1.566197129455978,0,-75.01390769807594,0,0,2,2
+1130,129,1130,1.5793387649752906,0,76.09402943267055,0,0,2,2
+1131,130,1131,1.5738126512839616,0,-180.95899864838228,0,0,2,2
+1132,131,1132,1.5740190032649894,0,4846.088683128463,0,0,2,2
+1133,132,1133,1.5713930387454167,0,-380.8124567359731,0,0,2,2
+1134,133,1134,1.578585527369067,0,139.0339355854949,0,0,2,2
+1135,134,1135,1.5764902367863745,0,-477.2607714940234,0,0,2,2
+1136,135,1136,1.573288419766056,0,-312.32265730804284,0,0,2,2
+1137,136,1137,1.579542170941775,0,159.90402750314496,0,0,2,2
+1138,137,1138,1.573978822436062,0,-179.74786209656133,0,0,2,2
+1139,138,1139,1.5810142195432424,0,142.13838746634403,0,0,2,2
+1140,139,1140,1.5716636063774267,0,-106.94485829612124,0,0,2,2
+1141,140,1141,1.5687473345739384,0,-342.9035657114844,0,0,2,2
+1142,141,1142,1.5755972265830727,0,145.98770296911493,0,0,2,2
+1143,142,1143,1.576111637781805,0,1943.9701205268652,0,0,2,2
+1144,143,1144,1.5846480915453838,0,117.14466307620009,0,0,2,2
+1145,144,1145,1.5696541500161478,0,-66.69360408336591,0,0,2,2
+1146,145,1146,1.5665878699312612,0,-326.12806798990545,0,0,2,2
+1147,146,1147,1.5736245608080908,0,142.11225382840146,0,0,2,2
+1148,147,1148,1.5818404302554914,0,121.71566337588133,0,0,2,2
+1149,148,1149,1.5635649606831077,0,-54.71815627167872,0,0,2,2
+1150,149,1150,1.564744372765147,0,847.8800711205324,0,0,2,2
+1151,150,1151,1.573462502288806,0,114.70350346209469,0,0,2,2
+1152,151,1152,1.578323109330659,0,205.73561931449584,0,0,2,2
+1153,152,1153,1.5844978766933193,0,161.94942113076553,0,0,2,2
+1154,153,1154,1.5712725171401973,0,-75.61231102892319,0,0,2,2
+1155,154,1155,1.5788883820139301,0,131.3048506741517,0,0,2,2
+1156,155,1156,1.572986141831219,0,-169.42719527565234,0,0,2,2
+1157,156,1157,1.5740141579748792,0,972.7473699387008,0,0,2,2
+1158,157,1158,1.568363598139526,0,-176.97361485200645,0,0,2,2
+1159,158,1159,1.5809705392166384,0,79.32138287022534,0,0,2,2
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv
new file mode 100644
index 00000000..88652956
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv
@@ -0,0 +1,159 @@
+LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,CrossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG
+1001,1001,0,1002,1001,1002,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1002,1002,1001,1003,1002,1003,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1003,1003,1002,1004,1003,1004,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1004,1004,1003,1005,1004,1005,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1005,1005,1004,1006,1005,1006,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1006,1006,1005,1007,1006,1007,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1007,1007,1006,1008,1007,1008,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1008,1008,1007,1009,1008,1009,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1009,1009,1008,1010,1009,1010,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1010,1010,1009,1011,1010,1011,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1011,1011,1010,1012,1011,1012,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1012,1012,1011,1013,1012,1013,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1013,1013,1012,1014,1013,1014,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1014,1014,1013,1015,1014,1015,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1015,1015,1014,1016,1015,1016,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1016,1016,1015,1017,1016,1017,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1017,1017,1016,1018,1017,1018,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1018,1018,1017,1019,1018,1019,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1019,1019,1018,1020,1019,1020,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1020,1020,1019,1021,1020,1021,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1021,1021,1020,1022,1021,1022,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1022,1022,1021,1023,1022,1023,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1023,1023,1022,1024,1023,1024,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1024,1024,1023,1025,1024,1025,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1025,1025,1024,1026,1025,1026,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1026,1026,1025,1027,1026,1027,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1027,1027,1026,1028,1027,1028,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1028,1028,1027,1029,1028,1029,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1029,1029,1028,1030,1029,1030,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1030,1030,1029,1031,1030,1031,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1031,1031,1030,1032,1031,1032,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1032,1032,1031,1033,1032,1033,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1033,1033,1032,1034,1033,1034,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1034,1034,1033,1035,1034,1035,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1035,1035,1034,1036,1035,1036,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1036,1036,1035,1037,1036,1037,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1037,1037,1036,1038,1037,1038,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1038,1038,1037,1039,1038,1039,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1039,1039,1038,1040,1039,1040,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1040,1040,1039,1041,1040,1041,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1041,1041,1040,1042,1041,1042,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1042,1042,1041,1043,1042,1043,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1043,1043,1042,1044,1043,1044,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1044,1044,1043,1045,1044,1045,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1045,1045,1044,1046,1045,1046,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1046,1046,1045,1047,1046,1047,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1047,1047,1046,1048,1047,1048,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1048,1048,1047,1049,1048,1049,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1049,1049,1048,1050,1049,1050,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1050,1050,1049,1051,1050,1051,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1051,1051,1050,1052,1051,1052,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1052,1052,1051,1053,1052,1053,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1053,1053,1052,1054,1053,1054,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1054,1054,1053,1055,1054,1055,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1055,1055,1054,1056,1055,1056,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1056,1056,1055,1057,1056,1057,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1057,1057,1056,1058,1057,1058,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1058,1058,1057,1059,1058,1059,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1059,1059,1058,1060,1059,1060,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1060,1060,1059,1061,1060,1061,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1061,1061,1060,1062,1061,1062,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1062,1062,1061,1063,1062,1063,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1063,1063,1062,1064,1063,1064,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1064,1064,1063,1065,1064,1065,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1065,1065,1064,1066,1065,1066,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1066,1066,1065,1067,1066,1067,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1067,1067,1066,1068,1067,1068,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1068,1068,1067,1069,1068,1069,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1069,1069,1068,1070,1069,1070,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1070,1070,1069,1071,1070,1071,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1071,1071,1070,1072,1071,1072,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1072,1072,1071,1073,1072,1073,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1073,1073,1072,1074,1073,1074,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1074,1074,1073,1075,1074,1075,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1075,1075,1074,1076,1075,1076,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1076,1076,1075,1077,1076,1077,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1077,1077,1076,1078,1077,1078,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1078,1078,1077,1079,1078,1079,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1079,1079,1078,1080,1079,1080,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1080,1080,1079,1081,1080,1081,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1081,1081,1080,1082,1081,1082,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1082,1082,1081,1083,1082,1083,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1083,1083,1082,1084,1083,1084,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1084,1084,1083,1085,1084,1085,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1085,1085,1084,1086,1085,1086,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1086,1086,1085,1087,1086,1087,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1087,1087,1086,1088,1087,1088,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1088,1088,1087,1089,1088,1089,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1089,1089,1088,1090,1089,1090,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1090,1090,1089,1091,1090,1091,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1091,1091,1090,1092,1091,1092,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1092,1092,1091,1093,1092,1093,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1093,1093,1092,1094,1093,1094,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1094,1094,1093,1095,1094,1095,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1095,1095,1094,1096,1095,1096,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1096,1096,1095,1097,1096,1097,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1097,1097,1096,1098,1097,1098,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1098,1098,1097,1099,1098,1099,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1099,1099,1098,1100,1099,1100,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1100,1100,1099,1101,1100,1101,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1101,1101,1100,1102,1101,1102,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1102,1102,1101,1103,1102,1103,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1103,1103,1102,1104,1103,1104,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1104,1104,1103,1105,1104,1105,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1105,1105,1104,1106,1105,1106,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1106,1106,1105,1107,1106,1107,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1107,1107,1106,1108,1107,1108,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1108,1108,1107,1109,1108,1109,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1109,1109,1108,1110,1109,1110,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1110,1110,1109,1111,1110,1111,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1111,1111,1110,1112,1111,1112,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1112,1112,1111,1113,1112,1113,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1113,1113,1112,1114,1113,1114,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1114,1114,1113,1115,1114,1115,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1115,1115,1114,1116,1115,1116,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1116,1116,1115,1117,1116,1117,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1117,1117,1116,1118,1117,1118,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1118,1118,1117,1119,1118,1119,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1119,1119,1118,1120,1119,1120,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1120,1120,1119,1121,1120,1121,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1121,1121,1120,1122,1121,1122,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1122,1122,1121,1123,1122,1123,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1123,1123,1122,1124,1123,1124,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1124,1124,1123,1125,1124,1125,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1125,1125,1124,1126,1125,1126,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1126,1126,1125,1127,1126,1127,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1127,1127,1126,1128,1127,1128,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1128,1128,1127,1129,1128,1129,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1129,1129,1128,1130,1129,1130,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1130,1130,1129,1131,1130,1131,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1131,1131,1130,1132,1131,1132,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1132,1132,1131,1133,1132,1133,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1133,1133,1132,1134,1133,1134,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1134,1134,1133,1135,1134,1135,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1135,1135,1134,1136,1135,1136,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1136,1136,1135,1137,1136,1137,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1137,1137,1136,1138,1137,1138,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1138,1138,1137,1139,1138,1139,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1139,1139,1138,1140,1139,1140,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1140,1140,1139,1141,1140,1141,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1141,1141,1140,1142,1141,1142,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1142,1142,1141,1143,1142,1143,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1143,1143,1142,1144,1143,1144,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1144,1144,1143,1145,1144,1145,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1145,1145,1144,1146,1145,1146,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1146,1146,1145,1147,1146,1147,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1147,1147,1146,1148,1147,1148,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1148,1148,1147,1149,1148,1149,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1149,1149,1148,1150,1149,1150,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1150,1150,1149,1151,1150,1151,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1151,1151,1150,1152,1151,1152,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1152,1152,1151,1153,1152,1153,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1153,1153,1152,1154,1153,1154,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1154,1154,1153,1155,1154,1155,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1155,1155,1154,1156,1155,1156,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1156,1156,1155,1157,1156,1157,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1157,1157,1156,1158,1157,1158,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1158,1158,1157,0,1158,1159,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv
new file mode 100644
index 00000000..de7375dd
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv
@@ -0,0 +1,160 @@
+NID,PID
+1001,1001
+1002,1002
+1003,1003
+1004,1004
+1005,1005
+1006,1006
+1007,1007
+1008,1008
+1009,1009
+1010,1010
+1011,1011
+1012,1012
+1013,1013
+1014,1014
+1015,1015
+1016,1016
+1017,1017
+1018,1018
+1019,1019
+1020,1020
+1021,1021
+1022,1022
+1023,1023
+1024,1024
+1025,1025
+1026,1026
+1027,1027
+1028,1028
+1029,1029
+1030,1030
+1031,1031
+1032,1032
+1033,1033
+1034,1034
+1035,1035
+1036,1036
+1037,1037
+1038,1038
+1039,1039
+1040,1040
+1041,1041
+1042,1042
+1043,1043
+1044,1044
+1045,1045
+1046,1046
+1047,1047
+1048,1048
+1049,1049
+1050,1050
+1051,1051
+1052,1052
+1053,1053
+1054,1054
+1055,1055
+1056,1056
+1057,1057
+1058,1058
+1059,1059
+1060,1060
+1061,1061
+1062,1062
+1063,1063
+1064,1064
+1065,1065
+1066,1066
+1067,1067
+1068,1068
+1069,1069
+1070,1070
+1071,1071
+1072,1072
+1073,1073
+1074,1074
+1075,1075
+1076,1076
+1077,1077
+1078,1078
+1079,1079
+1080,1080
+1081,1081
+1082,1082
+1083,1083
+1084,1084
+1085,1085
+1086,1086
+1087,1087
+1088,1088
+1089,1089
+1090,1090
+1091,1091
+1092,1092
+1093,1093
+1094,1094
+1095,1095
+1096,1096
+1097,1097
+1098,1098
+1099,1099
+1100,1100
+1101,1101
+1102,1102
+1103,1103
+1104,1104
+1105,1105
+1106,1106
+1107,1107
+1108,1108
+1109,1109
+1110,1110
+1111,1111
+1112,1112
+1113,1113
+1114,1114
+1115,1115
+1116,1116
+1117,1117
+1118,1118
+1119,1119
+1120,1120
+1121,1121
+1122,1122
+1123,1123
+1124,1124
+1125,1125
+1126,1126
+1127,1127
+1128,1128
+1129,1129
+1130,1130
+1131,1131
+1132,1132
+1133,1133
+1134,1134
+1135,1135
+1136,1136
+1137,1137
+1138,1138
+1139,1139
+1140,1140
+1141,1141
+1142,1142
+1143,1143
+1144,1144
+1145,1145
+1146,1146
+1147,1147
+1148,1148
+1149,1149
+1150,1150
+1151,1151
+1152,1152
+1153,1153
+1154,1154
+1155,1155
+1156,1156
+1157,1157
+1158,1158
+1159,1159
diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv
new file mode 100644
index 00000000..0f778cae
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv
@@ -0,0 +1,160 @@
+PID,B(Lat),L(Long),H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3
+1001,0,0,2.458,127.057,258.58,0,0,0,0
+1002,0,0,2.459,128.123,258.573,0,0,0,0
+1003,0,0,2.459,129.339,258.569,0,0,0,0
+1004,0,0,2.459,130.567,258.567,0,0,0,0
+1005,0,0,2.46,131.7,258.559,0,0,0,0
+1006,0,0,2.462,132.796,258.564,0,0,0,0
+1007,0,0,2.462,134.128,258.56,0,0,0,0
+1008,0,0,2.461,135.17,258.551,0,0,0,0
+1009,0,0,2.463,136.475,258.542,0,0,0,0
+1010,0,0,2.463,137.611,258.537,0,0,0,0
+1011,0,0,2.463,138.785,258.533,0,0,0,0
+1012,0,0,2.464,139.891,258.535,0,0,0,0
+1013,0,0,2.462,141.114,258.527,0,0,0,0
+1014,0,0,2.461,142.477,258.524,0,0,0,0
+1015,0,0,2.461,143.766,258.511,0,0,0,0
+1016,0,0,2.461,145.134,258.511,0,0,0,0
+1017,0,0,2.461,146.549,258.506,0,0,0,0
+1018,0,0,2.461,147.877,258.499,0,0,0,0
+1019,0,0,2.46,149.143,258.5,0,0,0,0
+1020,0,0,2.459,150.371,258.491,0,0,0,0
+1021,0,0,2.457,151.52,258.49,0,0,0,0
+1022,0,0,2.454,152.64,258.49,0,0,0,0
+1023,0,0,2.457,153.716,258.492,0,0,0,0
+1024,0,0,2.46,154.738,258.489,0,0,0,0
+1025,0,0,2.457,156.035,258.492,0,0,0,0
+1026,0,0,2.455,157.264,258.486,0,0,0,0
+1027,0,0,2.455,158.48,258.478,0,0,0,0
+1028,0,0,2.454,159.704,258.475,0,0,0,0
+1029,0,0,2.455,160.865,258.469,0,0,0,0
+1030,0,0,2.456,161.939,258.464,0,0,0,0
+1031,0,0,2.48,163.143,258.449,0,0,0,0
+1032,0,0,2.51,164.181,258.443,0,0,0,0
+1033,0,0,2.518,165.358,258.431,0,0,0,0
+1034,0,0,2.548,166.424,258.413,0,0,0,0
+1035,0,0,2.557,167.449,258.416,0,0,0,0
+1036,0,0,2.556,168.57,258.406,0,0,0,0
+1037,0,0,2.557,169.594,258.401,0,0,0,0
+1038,0,0,2.556,170.677,258.394,0,0,0,0
+1039,0,0,2.56,171.822,258.386,0,0,0,0
+1040,0,0,2.557,173.021,258.379,0,0,0,0
+1041,0,0,2.559,174.297,258.381,0,0,0,0
+1042,0,0,2.543,175.655,258.373,0,0,0,0
+1043,0,0,2.508,176.851,258.383,0,0,0,0
+1044,0,0,2.501,177.954,258.379,0,0,0,0
+1045,0,0,2.47,179.003,258.372,0,0,0,0
+1046,0,0,2.452,180.062,258.379,0,0,0,0
+1047,0,0,2.454,181.096,258.374,0,0,0,0
+1048,0,0,2.455,182.411,258.374,0,0,0,0
+1049,0,0,2.455,183.703,258.375,0,0,0,0
+1050,0,0,2.454,184.753,258.374,0,0,0,0
+1051,0,0,2.451,185.757,258.374,0,0,0,0
+1052,0,0,2.455,187.03,258.374,0,0,0,0
+1053,0,0,2.454,188.244,258.368,0,0,0,0
+1054,0,0,2.456,189.383,258.367,0,0,0,0
+1055,0,0,2.457,190.667,258.367,0,0,0,0
+1056,0,0,2.458,191.863,258.363,0,0,0,0
+1057,0,0,2.459,193.009,258.364,0,0,0,0
+1058,0,0,2.46,194.084,258.356,0,0,0,0
+1059,0,0,2.458,195.18,258.357,0,0,0,0
+1060,0,0,2.458,196.497,258.35,0,0,0,0
+1061,0,0,2.459,197.731,258.351,0,0,0,0
+1062,0,0,2.459,198.941,258.347,0,0,0,0
+1063,0,0,2.462,200.026,258.341,0,0,0,0
+1064,0,0,2.458,201.086,258.338,0,0,0,0
+1065,0,0,2.459,202.086,258.335,0,0,0,0
+1066,0,0,2.458,203.396,258.327,0,0,0,0
+1067,0,0,2.455,204.465,258.329,0,0,0,0
+1068,0,0,2.456,205.481,258.32,0,0,0,0
+1069,0,0,2.458,206.775,258.318,0,0,0,0
+1070,0,0,2.458,207.835,258.307,0,0,0,0
+1071,0,0,2.457,209.0,258.303,0,0,0,0
+1072,0,0,2.457,210.085,258.302,0,0,0,0
+1073,0,0,2.46,211.148,258.302,0,0,0,0
+1074,0,0,2.457,212.268,258.293,0,0,0,0
+1075,0,0,2.456,213.408,258.297,0,0,0,0
+1076,0,0,2.457,214.507,258.287,0,0,0,0
+1077,0,0,2.455,215.509,258.286,0,0,0,0
+1078,0,0,2.458,216.589,258.283,0,0,0,0
+1079,0,0,2.458,217.782,258.281,0,0,0,0
+1080,0,0,2.457,218.88,258.272,0,0,0,0
+1081,0,0,2.456,219.968,258.274,0,0,0,0
+1082,0,0,2.457,221.04,258.268,0,0,0,0
+1083,0,0,2.457,222.66,258.256,0,0,0,0
+1084,0,0,2.457,223.777,258.255,0,0,0,0
+1085,0,0,2.456,224.837,258.245,0,0,0,0
+1086,0,0,2.458,225.871,258.242,0,0,0,0
+1087,0,0,2.456,227.518,258.236,0,0,0,0
+1088,0,0,2.455,228.57,258.227,0,0,0,0
+1089,0,0,2.457,229.861,258.23,0,0,0,0
+1090,0,0,2.455,231.087,258.222,0,0,0,0
+1091,0,0,2.455,232.231,258.214,0,0,0,0
+1092,0,0,2.455,233.329,258.223,0,0,0,0
+1093,0,0,2.454,234.347,258.208,0,0,0,0
+1094,0,0,2.453,235.55,258.218,0,0,0,0
+1095,0,0,2.453,236.672,258.2,0,0,0,0
+1096,0,0,2.454,237.704,258.226,0,0,0,0
+1097,0,0,2.452,238.879,258.206,0,0,0,0
+1098,0,0,2.451,239.912,258.206,0,0,0,0
+1099,0,0,2.448,241.081,258.194,0,0,0,0
+1100,0,0,2.446,242.165,258.194,0,0,0,0
+1101,0,0,2.438,243.309,258.195,0,0,0,0
+1102,0,0,2.443,244.471,258.187,0,0,0,0
+1103,0,0,2.443,245.583,258.172,0,0,0,0
+1104,0,0,2.442,246.585,258.165,0,0,0,0
+1105,0,0,2.442,247.906,258.162,0,0,0,0
+1106,0,0,2.442,248.917,258.165,0,0,0,0
+1107,0,0,2.442,250.01,258.169,0,0,0,0
+1108,0,0,2.44,251.104,258.163,0,0,0,0
+1109,0,0,2.44,252.201,258.177,0,0,0,0
+1110,0,0,2.442,253.26,258.17,0,0,0,0
+1111,0,0,2.443,254.412,258.154,0,0,0,0
+1112,0,0,2.446,255.422,258.15,0,0,0,0
+1113,0,0,2.448,256.615,258.149,0,0,0,0
+1114,0,0,2.45,257.748,258.141,0,0,0,0
+1115,0,0,2.45,259.226,258.121,0,0,0,0
+1116,0,0,2.451,260.477,258.14,0,0,0,0
+1117,0,0,2.449,261.941,258.131,0,0,0,0
+1118,0,0,2.451,263.243,258.124,0,0,0,0
+1119,0,0,2.45,264.476,258.12,0,0,0,0
+1120,0,0,2.449,265.628,258.112,0,0,0,0
+1121,0,0,2.453,266.706,258.109,0,0,0,0
+1122,0,0,2.451,267.735,258.11,0,0,0,0
+1123,0,0,2.449,268.913,258.104,0,0,0,0
+1124,0,0,2.45,270.088,258.118,0,0,0,0
+1125,0,0,2.45,271.19,258.112,0,0,0,0
+1126,0,0,2.451,272.23,258.118,0,0,0,0
+1127,0,0,2.451,273.427,258.112,0,0,0,0
+1128,0,0,2.452,274.577,258.102,0,0,0,0
+1129,0,0,2.449,275.678,258.107,0,0,0,0
+1130,0,0,2.45,276.814,258.097,0,0,0,0
+1131,0,0,2.449,277.887,258.094,0,0,0,0
+1132,0,0,2.445,279.118,258.09,0,0,0,0
+1133,0,0,2.448,280.141,258.089,0,0,0,0
+1134,0,0,2.449,281.253,258.081,0,0,0,0
+1135,0,0,2.449,282.266,258.075,0,0,0,0
+1136,0,0,2.447,283.271,258.072,0,0,0,0
+1137,0,0,2.448,284.813,258.059,0,0,0,0
+1138,0,0,2.45,286.002,258.055,0,0,0,0
+1139,0,0,2.451,287.101,258.044,0,0,0,0
+1140,0,0,2.45,288.262,258.043,0,0,0,0
+1141,0,0,2.452,289.275,258.045,0,0,0,0
+1142,0,0,2.451,290.578,258.039,0,0,0,0
+1143,0,0,2.452,291.778,258.032,0,0,0,0
+1144,0,0,2.451,292.913,258.017,0,0,0,0
+1145,0,0,2.451,294.195,258.018,0,0,0,0
+1146,0,0,2.453,295.515,258.024,0,0,0,0
+1147,0,0,2.449,296.713,258.02,0,0,0,0
+1148,0,0,2.453,297.854,258.008,0,0,0,0
+1149,0,0,2.454,298.938,258.016,0,0,0,0
+1150,0,0,2.453,300.189,258.023,0,0,0,0
+1151,0,0,2.452,301.196,258.02,0,0,0,0
+1152,0,0,2.451,302.721,258.009,0,0,0,0
+1153,0,0,2.45,304.159,257.989,0,0,0,0
+1154,0,0,2.444,305.249,257.989,0,0,0,0
+1155,0,0,2.444,306.501,257.979,0,0,0,0
+1156,0,0,2.446,307.685,257.976,0,0,0,0
+1157,0,0,2.444,308.786,257.972,0,0,0,0
+1158,0,0,2.44,309.827,257.975,0,0,0,0
+1159,0,0,2.442,311.051,257.963,0,0,0,0
diff --git a/autoware.ai/autoware_files/vector_map/cubetown_straight/dtlane.csv b/autoware.ai/autoware_files/vector_map/cubetown_straight/dtlane.csv
new file mode 100644
index 00000000..357cc87f
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/cubetown_straight/dtlane.csv
@@ -0,0 +1,95 @@
+DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW
+1001,0,1001,3.141840755356629,0,90000000000,0,0,2,2
+1002,1,1002,3.141840755356629,0,90000000000,0,0,2,2
+1003,2,1003,3.1415039391096657,0,-2968.9779190168465,0,0,2,2
+1004,3,1004,3.141290603752599,0,-4687.455533626855,0,0,2,2
+1005,4,1005,3.1412035506037146,0,-11487.235244366804,0,0,2,2
+1006,5,1006,3.1411593667508333,0,-22632.70255508454,0,0,2,2
+1007,6,1007,3.1409284471703254,0,-4330.511937534554,0,0,2,2
+1008,7,1008,3.1406249057059217,0,-3294.442826664472,0,0,2,2
+1009,8,1009,3.140379015416139,0,-4066.854371854934,0,0,2,2
+1010,9,1010,3.140388570087818,0,104660.84378169649,0,0,2,2
+1011,10,1011,3.1403950790189157,0,153635.0569695795,0,0,2,2
+1012,11,1012,3.1404826513196578,0,11419.135862893965,0,0,2,2
+1013,12,1013,3.1404416027677957,0,-24361.39533888552,0,0,2,2
+1014,13,1014,3.140125084123769,0,-3159.371553215576,0,0,2,2
+1015,14,1015,3.1402281204636235,0,9705.313692362022,0,0,2,2
+1016,15,1016,3.1401389842209273,0,-11218.781157377234,0,0,2,2
+1017,16,1017,3.139716141716801,0,-2364.946736057318,0,0,2,2
+1018,17,1018,3.1398037978705347,0,11408.212172274369,0,0,2,2
+1019,18,1019,3.1396230935279563,0,-5533.901320420821,0,0,2,2
+1020,19,1020,3.1397755776317453,0,6558.060644694116,0,0,2,2
+1021,20,1021,3.139774841552806,0,-1358549.9419073837,0,0,2,2
+1022,21,1022,3.139496443886718,0,-3591.9841356819943,0,0,2,2
+1023,22,1023,3.139477418909205,0,-52562.479998724964,0,0,2,2
+1024,23,1024,3.1394923637210748,0,66912.85301624147,0,0,2,2
+1025,24,1025,3.1395033644942663,0,90902.70134586656,0,0,2,2
+1026,25,1026,3.1395176493492736,0,70004.21071741311,0,0,2,2
+1027,26,1027,3.139530024556282,0,80806.72907654608,0,0,2,2
+1028,27,1028,3.1397245377792924,0,5141.038663201027,0,0,2,2
+1029,28,1029,3.1392954272772746,0,-2330.402064964111,0,0,2,2
+1030,29,1030,3.139196418825459,0,-10100.14783243779,0,0,2,2
+1031,30,1031,3.1392070069672044,0,94445.27888155554,0,0,2,2
+1032,31,1032,3.139223153056225,0,61934.503069647064,0,0,2,2
+1033,32,1033,3.1393395568464104,0,8590.785561245712,0,0,2,2
+1034,33,1034,3.139200075877934,0,-7169.436883918307,0,0,2,2
+1035,34,1035,3.1390769452122096,0,-8121.45369406833,0,0,2,2
+1036,35,1036,3.139057603754864,0,-51702.412188325434,0,0,2,2
+1037,36,1037,3.139069089854514,0,87061.75555370716,0,0,2,2
+1038,37,1038,3.139145955250907,0,13009.755324627917,0,0,2,2
+1039,38,1039,3.139311426970444,0,6043.328750054524,0,0,2,2
+1040,39,1040,3.138902401570358,0,-2444.835943660098,0,0,2,2
+1041,40,1041,3.138773122528401,0,-7735.205837383889,0,0,2,2
+1042,41,1042,3.1387475934494664,0,-39171.017590187184,0,0,2,2
+1043,42,1043,3.1387480626973945,0,2131069.6119920393,0,0,2,2
+1044,43,1044,3.1388895484743546,0,7067.848242311916,0,0,2,2
+1045,44,1045,3.1389562332098473,0,14995.935615733462,0,0,2,2
+1046,45,1046,3.1385653400160685,0,-2558.2435711730704,0,0,2,2
+1047,46,1047,3.1384299905746076,0,-7388.283166939177,0,0,2,2
+1048,47,1048,3.138399842034186,0,-33169.10158898101,0,0,2,2
+1049,48,1049,3.138398209228352,0,-612442.6916652169,0,0,2,2
+1050,49,1050,3.1384078787704386,0,103417.51357461484,0,0,2,2
+1051,50,1051,3.1386576422646226,0,4003.787676285226,0,0,2,2
+1052,51,1052,3.138409620878691,0,-4031.9103783869114,0,0,2,2
+1053,52,1053,3.1381816807382634,0,-4387.116714610132,0,0,2,2
+1054,53,1054,3.1381186342535505,0,-15861.312562530806,0,0,2,2
+1055,54,1055,3.138107281667687,0,-88085.65837197383,0,0,2,2
+1056,55,1056,3.1381139495508767,0,149972.63322800846,0,0,2,2
+1057,56,1057,3.138123768288998,0,101846.08120245696,0,0,2,2
+1058,57,1058,3.1381326263475833,0,112891.55409957237,0,0,2,2
+1059,58,1059,3.1381439258570403,0,88499.41705927694,0,0,2,2
+1060,59,1060,3.138154738664567,0,92482.91875465706,0,0,2,2
+1061,60,1061,3.138164351277002,0,104029.9925508711,0,0,2,2
+1062,61,1062,3.138174032352458,0,103294.30904130894,0,0,2,2
+1063,62,1063,3.1381852940876627,0,88796.26290373625,0,0,2,2
+1064,63,1064,3.1384424816471945,0,3888.2129517471153,0,0,2,2
+1065,64,1065,3.1382124925718102,0,-4348.032611240637,0,0,2,2
+1066,65,1066,3.1379154882371667,0,-3366.954227116705,0,0,2,2
+1067,66,1067,3.137822154845505,0,-10714.279018453317,0,0,2,2
+1068,67,1068,3.137802285633065,0,-50329.121146989644,0,0,2,2
+1069,68,1069,3.137808284895846,0,166687.14748258528,0,0,2,2
+1070,69,1070,3.1378163512995583,0,123970.98331451902,0,0,2,2
+1071,70,1071,3.137828450162874,0,82652.3925355782,0,0,2,2
+1072,71,1072,3.137836664694623,0,121735.48420661429,0,0,2,2
+1073,72,1073,3.1378522173967727,0,64297.50858561852,0,0,2,2
+1074,73,1074,3.1378603608899267,0,122797.42625002169,0,0,2,2
+1075,74,1075,3.137872972936972,0,79289.2697276673,0,0,2,2
+1076,75,1076,3.1378812529317686,0,120773.0227592231,0,0,2,2
+1077,76,1077,3.1378909513716606,0,103109.36718983989,0,0,2,2
+1078,77,1078,3.1379025772157574,0,86015.25976762181,0,0,2,2
+1079,78,1079,3.1379123614143296,0,102205.61169297676,0,0,2,2
+1080,79,1080,3.1379211334192068,0,113999.02462497579,0,0,2,2
+1081,80,1081,3.1379340927458688,0,77164.50291599584,0,0,2,2
+1082,81,1082,3.1379461179072976,0,83158.96679786622,0,0,2,2
+1083,82,1083,3.1379528362470825,0,148846.2971524249,0,0,2,2
+1084,83,1084,3.137965297720555,0,80247.33208261685,0,0,2,2
+1085,84,1085,3.1379718860215577,0,151784.2004510639,0,0,2,2
+1086,85,1086,3.1379847559380174,0,77700.58206163858,0,0,2,2
+1087,86,1087,3.1379945893453316,0,101694.15016057012,0,0,2,2
+1088,87,1088,3.1380013890106024,0,147066.0628381838,0,0,2,2
+1089,88,1089,3.1380142274392324,0,77891.15232255404,0,0,2,2
+1090,89,1090,3.1380245517508505,0,96858.75794853928,0,0,2,2
+1091,90,1091,3.1380115077573896,0,-76663.63855489486,0,0,2,2
+1092,91,1092,3.137590633002616,0,-2376.003760401227,0,0,2,2
+1093,92,1093,3.1378851847800737,0,3394.988849264264,0,0,2,2
+1094,93,1094,3.146261608250373,0,119.38269400367994,0,0,2,2
diff --git a/autoware.ai/autoware_files/vector_map/cubetown_straight/idx.csv b/autoware.ai/autoware_files/vector_map/cubetown_straight/idx.csv
new file mode 100644
index 00000000..45175d3d
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/cubetown_straight/idx.csv
@@ -0,0 +1,15 @@
+ID,KIND,fname
+1,K001,poledata.csv
+2,K002,utilitypole.csv
+3,K003,roadsign.csv
+4,K004,signaldata.csv
+5,K005,streetlight.csv
+6,P001,whiteline.csv
+7,P002,stopline.csv
+8,P003,zebrazone.csv
+9,P004,crosswalk.csv
+10,P005,road_surface_mark.csv
+11,R001,roadedge.csv
+12,R002,gutter.csv
+13,R003,curb.csv
+14,S001,guardrail.csv
diff --git a/autoware.ai/autoware_files/vector_map/cubetown_straight/lane.csv b/autoware.ai/autoware_files/vector_map/cubetown_straight/lane.csv
new file mode 100644
index 00000000..1bdc152d
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/cubetown_straight/lane.csv
@@ -0,0 +1,94 @@
+LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,CrossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG
+1001,1001,0,1002,1001,1002,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1002,1002,1001,1003,1002,1003,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1003,1003,1002,1004,1003,1004,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1004,1004,1003,1005,1004,1005,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1005,1005,1004,1006,1005,1006,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1006,1006,1005,1007,1006,1007,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1007,1007,1006,1008,1007,1008,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1008,1008,1007,1009,1008,1009,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1009,1009,1008,1010,1009,1010,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1010,1010,1009,1011,1010,1011,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1011,1011,1010,1012,1011,1012,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1012,1012,1011,1013,1012,1013,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1013,1013,1012,1014,1013,1014,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1014,1014,1013,1015,1014,1015,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1015,1015,1014,1016,1015,1016,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1016,1016,1015,1017,1016,1017,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1017,1017,1016,1018,1017,1018,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1018,1018,1017,1019,1018,1019,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1019,1019,1018,1020,1019,1020,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1020,1020,1019,1021,1020,1021,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1021,1021,1020,1022,1021,1022,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1022,1022,1021,1023,1022,1023,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1023,1023,1022,1024,1023,1024,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1024,1024,1023,1025,1024,1025,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1025,1025,1024,1026,1025,1026,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1026,1026,1025,1027,1026,1027,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1027,1027,1026,1028,1027,1028,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1028,1028,1027,1029,1028,1029,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1029,1029,1028,1030,1029,1030,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1030,1030,1029,1031,1030,1031,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1031,1031,1030,1032,1031,1032,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1032,1032,1031,1033,1032,1033,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1033,1033,1032,1034,1033,1034,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1034,1034,1033,1035,1034,1035,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1035,1035,1034,1036,1035,1036,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1036,1036,1035,1037,1036,1037,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1037,1037,1036,1038,1037,1038,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1038,1038,1037,1039,1038,1039,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1039,1039,1038,1040,1039,1040,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1040,1040,1039,1041,1040,1041,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1041,1041,1040,1042,1041,1042,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1042,1042,1041,1043,1042,1043,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1043,1043,1042,1044,1043,1044,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1044,1044,1043,1045,1044,1045,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1045,1045,1044,1046,1045,1046,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1046,1046,1045,1047,1046,1047,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1047,1047,1046,1048,1047,1048,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1048,1048,1047,1049,1048,1049,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1049,1049,1048,1050,1049,1050,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1050,1050,1049,1051,1050,1051,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1051,1051,1050,1052,1051,1052,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1052,1052,1051,1053,1052,1053,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1053,1053,1052,1054,1053,1054,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1054,1054,1053,1055,1054,1055,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1055,1055,1054,1056,1055,1056,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1056,1056,1055,1057,1056,1057,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1057,1057,1056,1058,1057,1058,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1058,1058,1057,1059,1058,1059,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1059,1059,1058,1060,1059,1060,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1060,1060,1059,1061,1060,1061,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1061,1061,1060,1062,1061,1062,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1062,1062,1061,1063,1062,1063,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1063,1063,1062,1064,1063,1064,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1064,1064,1063,1065,1064,1065,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1065,1065,1064,1066,1065,1066,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1066,1066,1065,1067,1066,1067,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1067,1067,1066,1068,1067,1068,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1068,1068,1067,1069,1068,1069,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1069,1069,1068,1070,1069,1070,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1070,1070,1069,1071,1070,1071,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1071,1071,1070,1072,1071,1072,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1072,1072,1071,1073,1072,1073,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1073,1073,1072,1074,1073,1074,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1074,1074,1073,1075,1074,1075,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1075,1075,1074,1076,1075,1076,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1076,1076,1075,1077,1076,1077,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1077,1077,1076,1078,1077,1078,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1078,1078,1077,1079,1078,1079,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1079,1079,1078,1080,1079,1080,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1080,1080,1079,1081,1080,1081,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1081,1081,1080,1082,1081,1082,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1082,1082,1081,1083,1082,1083,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1083,1083,1082,1084,1083,1084,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1084,1084,1083,1085,1084,1085,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1085,1085,1084,1086,1085,1086,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1086,1086,1085,1087,1086,1087,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1087,1087,1086,1088,1087,1088,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1088,1088,1087,1089,1088,1089,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1089,1089,1088,1090,1089,1090,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1090,1090,1089,1091,1090,1091,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1091,1091,1090,1092,1091,1092,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1092,1092,1091,1093,1092,1093,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
+1093,1093,1092,0,1093,1094,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0
diff --git a/autoware.ai/autoware_files/vector_map/cubetown_straight/node.csv b/autoware.ai/autoware_files/vector_map/cubetown_straight/node.csv
new file mode 100644
index 00000000..f1dc62b1
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/cubetown_straight/node.csv
@@ -0,0 +1,95 @@
+NID,PID
+1001,1001
+1002,1002
+1003,1003
+1004,1004
+1005,1005
+1006,1006
+1007,1007
+1008,1008
+1009,1009
+1010,1010
+1011,1011
+1012,1012
+1013,1013
+1014,1014
+1015,1015
+1016,1016
+1017,1017
+1018,1018
+1019,1019
+1020,1020
+1021,1021
+1022,1022
+1023,1023
+1024,1024
+1025,1025
+1026,1026
+1027,1027
+1028,1028
+1029,1029
+1030,1030
+1031,1031
+1032,1032
+1033,1033
+1034,1034
+1035,1035
+1036,1036
+1037,1037
+1038,1038
+1039,1039
+1040,1040
+1041,1041
+1042,1042
+1043,1043
+1044,1044
+1045,1045
+1046,1046
+1047,1047
+1048,1048
+1049,1049
+1050,1050
+1051,1051
+1052,1052
+1053,1053
+1054,1054
+1055,1055
+1056,1056
+1057,1057
+1058,1058
+1059,1059
+1060,1060
+1061,1061
+1062,1062
+1063,1063
+1064,1064
+1065,1065
+1066,1066
+1067,1067
+1068,1068
+1069,1069
+1070,1070
+1071,1071
+1072,1072
+1073,1073
+1074,1074
+1075,1075
+1076,1076
+1077,1077
+1078,1078
+1079,1079
+1080,1080
+1081,1081
+1082,1082
+1083,1083
+1084,1084
+1085,1085
+1086,1086
+1087,1087
+1088,1088
+1089,1089
+1090,1090
+1091,1091
+1092,1092
+1093,1093
+1094,1094
diff --git a/autoware.ai/autoware_files/vector_map/cubetown_straight/point.csv b/autoware.ai/autoware_files/vector_map/cubetown_straight/point.csv
new file mode 100644
index 00000000..3c3ebb2f
--- /dev/null
+++ b/autoware.ai/autoware_files/vector_map/cubetown_straight/point.csv
@@ -0,0 +1,95 @@
+PID,B(Lat),L(Long),H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3
+1001,0,0,0.37,1.53,59.701,0,0,0,0
+1002,0,0,0.368,1.53,58.502,0,0,0,0
+1003,0,0,0.368,1.53,57.169,0,0,0,0
+1004,0,0,0.368,1.53,55.894,0,0,0,0
+1005,0,0,0.368,1.531,54.511,0,0,0,0
+1006,0,0,0.366,1.531,53.332,0,0,0,0
+1007,0,0,0.366,1.532,52.051,0,0,0,0
+1008,0,0,0.367,1.534,50.651,0,0,0,0
+1009,0,0,0.369,1.535,49.256,0,0,0,0
+1010,0,0,0.37,1.537,47.868,0,0,0,0
+1011,0,0,0.37,1.539,46.486,0,0,0,0
+1012,0,0,0.368,1.54,45.094,0,0,0,0
+1013,0,0,0.367,1.542,43.56,0,0,0,0
+1014,0,0,0.369,1.544,42.026,0,0,0,0
+1015,0,0,0.367,1.546,40.469,0,0,0,0
+1016,0,0,0.367,1.549,38.732,0,0,0,0
+1017,0,0,0.369,1.552,36.991,0,0,0,0
+1018,0,0,0.369,1.555,35.26,0,0,0,0
+1019,0,0,0.37,1.559,33.536,0,0,0,0
+1020,0,0,0.37,1.562,31.821,0,0,0,0
+1021,0,0,0.366,1.565,30.011,0,0,0,0
+1022,0,0,0.368,1.569,28.149,0,0,0,0
+1023,0,0,0.369,1.573,26.296,0,0,0,0
+1024,0,0,0.369,1.577,24.452,0,0,0,0
+1025,0,0,0.37,1.581,22.616,0,0,0,0
+1026,0,0,0.37,1.584,20.789,0,0,0,0
+1027,0,0,0.37,1.588,18.97,0,0,0,0
+1028,0,0,0.366,1.592,17.066,0,0,0,0
+1029,0,0,0.368,1.596,15.087,0,0,0,0
+1030,0,0,0.369,1.601,13.119,0,0,0,0
+1031,0,0,0.369,1.606,11.159,0,0,0,0
+1032,0,0,0.369,1.61,9.209,0,0,0,0
+1033,0,0,0.367,1.614,7.309,0,0,0,0
+1034,0,0,0.368,1.617,6.253,0,0,0,0
+1035,0,0,0.369,1.62,5.2,0,0,0,0
+1036,0,0,0.369,1.625,3.182,0,0,0,0
+1037,0,0,0.369,1.63,1.173,0,0,0,0
+1038,0,0,0.368,1.635,-0.846,0,0,0,0
+1039,0,0,0.366,1.637,-1.879,0,0,0,0
+1040,0,0,0.367,1.64,-2.998,0,0,0,0
+1041,0,0,0.368,1.643,-4.029,0,0,0,0
+1042,0,0,0.369,1.646,-5.143,0,0,0,0
+1043,0,0,0.369,1.649,-6.169,0,0,0,0
+1044,0,0,0.367,1.652,-7.304,0,0,0,0
+1045,0,0,0.366,1.655,-8.403,0,0,0,0
+1046,0,0,0.367,1.658,-9.502,0,0,0,0
+1047,0,0,0.368,1.662,-10.689,0,0,0,0
+1048,0,0,0.369,1.666,-11.874,0,0,0,0
+1049,0,0,0.369,1.669,-12.964,0,0,0,0
+1050,0,0,0.369,1.673,-14.143,0,0,0,0
+1051,0,0,0.366,1.677,-15.276,0,0,0,0
+1052,0,0,0.367,1.681,-16.522,0,0,0,0
+1053,0,0,0.368,1.684,-17.669,0,0,0,0
+1054,0,0,0.368,1.689,-18.909,0,0,0,0
+1055,0,0,0.369,1.693,-20.051,0,0,0,0
+1056,0,0,0.369,1.697,-21.284,0,0,0,0
+1057,0,0,0.369,1.701,-22.42,0,0,0,0
+1058,0,0,0.369,1.705,-23.648,0,0,0,0
+1059,0,0,0.369,1.709,-24.778,0,0,0,0
+1060,0,0,0.369,1.713,-26.0,0,0,0,0
+1061,0,0,0.369,1.717,-27.125,0,0,0,0
+1062,0,0,0.369,1.721,-28.341,0,0,0,0
+1063,0,0,0.369,1.725,-29.461,0,0,0,0
+1064,0,0,0.366,1.729,-30.728,0,0,0,0
+1065,0,0,0.366,1.733,-31.923,0,0,0,0
+1066,0,0,0.367,1.738,-33.215,0,0,0,0
+1067,0,0,0.368,1.742,-34.404,0,0,0,0
+1068,0,0,0.369,1.747,-35.689,0,0,0,0
+1069,0,0,0.369,1.752,-36.872,0,0,0,0
+1070,0,0,0.369,1.757,-38.151,0,0,0,0
+1071,0,0,0.369,1.761,-39.329,0,0,0,0
+1072,0,0,0.369,1.766,-40.602,0,0,0,0
+1073,0,0,0.369,1.77,-41.773,0,0,0,0
+1074,0,0,0.369,1.775,-43.04,0,0,0,0
+1075,0,0,0.369,1.779,-44.206,0,0,0,0
+1076,0,0,0.369,1.784,-45.466,0,0,0,0
+1077,0,0,0.369,1.788,-46.626,0,0,0,0
+1078,0,0,0.369,1.793,-47.88,0,0,0,0
+1079,0,0,0.369,1.797,-49.035,0,0,0,0
+1080,0,0,0.369,1.802,-50.283,0,0,0,0
+1081,0,0,0.369,1.806,-51.432,0,0,0,0
+1082,0,0,0.369,1.81,-52.673,0,0,0,0
+1083,0,0,0.369,1.815,-53.817,0,0,0,0
+1084,0,0,0.369,1.819,-55.053,0,0,0,0
+1085,0,0,0.369,1.823,-56.19,0,0,0,0
+1086,0,0,0.369,1.828,-57.42,0,0,0,0
+1087,0,0,0.369,1.832,-58.552,0,0,0,0
+1088,0,0,0.369,1.836,-59.776,0,0,0,0
+1089,0,0,0.369,1.84,-60.903,0,0,0,0
+1090,0,0,0.369,1.844,-62.12,0,0,0,0
+1091,0,0,0.369,1.848,-63.239,0,0,0,0
+1092,0,0,0.374,1.853,-64.36,0,0,0,0
+1093,0,0,0.344,1.859,-65.989,0,0,0,0
+1094,0,0,0.34,1.856,-66.6,0,0,0,0
diff --git a/autoware.ai/src/CMakeLists.txt b/autoware.ai/src/CMakeLists.txt
new file mode 120000
index 00000000..66dd650a
--- /dev/null
+++ b/autoware.ai/src/CMakeLists.txt
@@ -0,0 +1 @@
+/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/amathutils_lib/CMakeLists.txt b/autoware.ai/src/autoware/common/amathutils_lib/CMakeLists.txt
index 80160794..68333385 100644
--- a/autoware.ai/src/autoware/common/amathutils_lib/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/amathutils_lib/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.11)
 project(amathutils_lib) # autoware math utility
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(Eigen3 REQUIRED)
 
@@ -72,25 +72,4 @@ file(GLOB_RECURSE ROSLINT_FILES
 )
 
 list(APPEND ROSLINT_CPP_OPTS "--extensions=cc,h,hpp,cpp,cu,cuh" "--filter=-build/c++11,-runtime/references")
-roslint_cpp(${ROSLINT_FILES})
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(amathutils-test test/test_amathutils_lib.test test/src/test_amathutils_lib.cpp)
-  target_link_libraries(amathutils-test ${catkin_LIBRARIES} amathutils_lib)
-  add_rostest_gtest(test-kalman_filter 
-    test/test_kalman_filter.test
-    test/src/test_kalman_filter.cpp
-    src/kalman_filter.cpp
-    src/time_delay_kalman_filter.cpp
-  )
-  target_link_libraries(test-kalman_filter ${catkin_LIBRARIES})
-
-  add_rostest_gtest(test-butterworth_filter 
-    test/test_butterworth_filter.test
-    test/src/test_butterworth_filter.cpp
-    src/butterworth_filter.cpp
-  )
-  target_link_libraries(test-butterworth_filter ${catkin_LIBRARIES})
-  roslint_add_test()
-endif()
+roslint_cpp(${ROSLINT_FILES})
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/amathutils_lib/package.xml b/autoware.ai/src/autoware/common/amathutils_lib/package.xml
index 1266baba..ee6561fe 100644
--- a/autoware.ai/src/autoware/common/amathutils_lib/package.xml
+++ b/autoware.ai/src/autoware/common/amathutils_lib/package.xml
@@ -7,7 +7,6 @@
   <author email="yusuke.fujii@tier4.jp">Yusuke FUJII</author>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
   <test_depend>rostest</test_depend>
 
@@ -17,4 +16,5 @@
   <depend>tf2</depend>
   <depend>tf</depend>
   <depend>eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/common/gnss/CMakeLists.txt b/autoware.ai/src/autoware/common/gnss/CMakeLists.txt
index 211eb56b..dd3c2e79 100644
--- a/autoware.ai/src/autoware/common/gnss/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/gnss/CMakeLists.txt
@@ -46,18 +46,4 @@ install(TARGETS gnss
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-if(CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-
-  find_package(rostest REQUIRED)
-  find_package(rosunit REQUIRED)
-
-  add_rostest_gtest(test_gnss
-    test/test_gnss.test test/test_gnss.cpp
-    src/geo_pos_conv.cpp
-  )
-  target_link_libraries(test_gnss ${catkin_LIBRARIES})
-  add_dependencies(test_gnss ${catkin_EXPORTED_TARGETS})
-endif()
+)
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/libvectormap/CMakeLists.txt b/autoware.ai/src/autoware/common/libvectormap/CMakeLists.txt
index f2bfe773..aaf3d808 100644
--- a/autoware.ai/src/autoware/common/libvectormap/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/libvectormap/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.12)
 
 project(libvectormap)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   roslint
@@ -68,8 +68,4 @@ install(DIRECTORY include/${PROJECT_NAME}/
 )
 
 set(ROSLINT_CPP_OPTS "--filter=-build/c++14,-runtime/references")
-roslint_cpp()
-
-if(CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-endif()
+roslint_cpp()
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/libvectormap/package.xml b/autoware.ai/src/autoware/common/libvectormap/package.xml
index b4a1d9fc..4f5546f8 100644
--- a/autoware.ai/src/autoware/common/libvectormap/package.xml
+++ b/autoware.ai/src/autoware/common/libvectormap/package.xml
@@ -6,7 +6,6 @@
   <maintainer email="sujiwo@ertl.jp">sujiwo</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>cmake_modules</depend>
diff --git a/autoware.ai/src/autoware/common/libwaypoint_follower/CMakeLists.txt b/autoware.ai/src/autoware/common/libwaypoint_follower/CMakeLists.txt
index 89a6c1c0..64a886c5 100644
--- a/autoware.ai/src/autoware/common/libwaypoint_follower/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/libwaypoint_follower/CMakeLists.txt
@@ -1,8 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(libwaypoint_follower)
 
-find_package(autoware_build_flags)
-
 find_package(catkin REQUIRED COMPONENTS
   amathutils_lib
   autoware_msgs
@@ -68,27 +66,3 @@ file(GLOB_RECURSE ROSLINT_FILES
 
 list(APPEND ROSLINT_CPP_OPTS "--extensions=cc,h,hpp,cpp,cu,cuh" "--filter=-build/c++11,-runtime/references")
 roslint_cpp(${ROSLINT_FILES})
-
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(test-libwaypoint_follower
-    test/test_libwaypoint_follower.test
-    test/src/test_libwaypoint_follower.cpp
-    src/libwaypoint_follower.cpp
-  )
-  add_dependencies(test-libwaypoint_follower ${catkin_EXPORTED_TARGETS})
-  target_link_libraries(test-libwaypoint_follower
-    ${catkin_LIBRARIES}
-  )
-  add_rostest_gtest(test-pure_pursuit
-    test/test_pure_pursuit.test
-    test/src/test_pure_pursuit.cpp
-    src/libwaypoint_follower.cpp
-    src/pure_pursuit.cpp
-  )  
-  add_dependencies(test-pure_pursuit ${catkin_EXPORTED_TARGETS})
-  target_link_libraries(test-pure_pursuit
-    ${catkin_LIBRARIES}
-  )
-  roslint_add_test()
-endif ()
diff --git a/autoware.ai/src/autoware/common/libwaypoint_follower/package.xml b/autoware.ai/src/autoware/common/libwaypoint_follower/package.xml
index e826881f..eb866555 100644
--- a/autoware.ai/src/autoware/common/libwaypoint_follower/package.xml
+++ b/autoware.ai/src/autoware/common/libwaypoint_follower/package.xml
@@ -6,7 +6,6 @@
   <maintainer email="h_ohta@ertl.jp">h_ohta</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
   <test_depend>rostest</test_depend>
 
@@ -18,4 +17,5 @@
   <depend>rosunit</depend>
   <depend>std_msgs</depend>
   <depend>eigen</depend>
+  <depend>tf2_eigen</depend>
 </package>
diff --git a/autoware.ai/src/autoware/common/map_file/CMakeLists.txt b/autoware.ai/src/autoware/common/map_file/CMakeLists.txt
index 24e92529..4c1fdd8f 100644
--- a/autoware.ai/src/autoware/common/map_file/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/map_file/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(map_file)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   autoware_msgs
diff --git a/autoware.ai/src/autoware/common/map_file/package.xml b/autoware.ai/src/autoware/common/map_file/package.xml
index 4907b03b..b7834b79 100644
--- a/autoware.ai/src/autoware/common/map_file/package.xml
+++ b/autoware.ai/src/autoware/common/map_file/package.xml
@@ -6,7 +6,6 @@
   <maintainer email="kondoh@axe-inc.co.jp">Masao KONDOH</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_msgs</depend>
@@ -21,6 +20,4 @@
   <depend>tf</depend>
   <depend>vector_map</depend>
   <depend>visualization_msgs</depend>
-  <depend>libboost-filesystem-dev</depend>
-  
 </package>
diff --git a/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch b/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch
index 9eb23ab3..a6d86fdf 100755
--- a/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch
+++ b/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch
@@ -10,7 +10,7 @@
   <arg name="circle_radius" default="1.7" />
 
   <!-- Launch node -->
-  <node pkg="object_map" type="grid_map_filter" name="grid_map_filter" output="screen">
+  <node pkg="object_map" type="grid_map_filter" name="grid_map_filter">
     <param name="map_frame" value="$(arg map_frame)" />
     <param name="map_topic" value="$(arg map_topic)" />
     <param name="dist_transform_distance" value="$(arg dist_transform_distance)" />
@@ -22,7 +22,7 @@
   </node>
 
 <!-- Launch the grid map visualizer -->
-  <node pkg="grid_map_visualization" type="grid_map_visualization" name="grid_map_filter_visualization" output="screen">
+  <node pkg="grid_map_visualization" type="grid_map_visualization" name="grid_map_filter_visualization">
     <rosparam command="load" file="$(find object_map)/config/grid_map_filter.yaml" />
   </node>
 
diff --git a/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch b/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch
index ee0a8583..77d3b330 100644
--- a/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch
+++ b/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch
@@ -8,7 +8,7 @@
     <arg name="scan_topic" default="/scan" />
     <arg name="sensor_frame" default="/velodyne" />
 
-  <node pkg="object_map" type="laserscan2costmap" name="laserscan2costmap" output="screen">
+  <node pkg="object_map" type="laserscan2costmap" name="laserscan2costmap">
         <param name="resolution" value="$(arg resolution)" />
         <param name="scan_size_x" value="$(arg scan_size_x)" />
         <param name="scan_size_y" value="$(arg scan_size_y)" />
diff --git a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch
index f8f6f8cf..8bd5c0c2 100755
--- a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch
+++ b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch
@@ -10,7 +10,7 @@
   <arg name="grid_position_z" default="-2" />
 
   <!-- Launch node -->
-  <node pkg="object_map" type="wayarea2grid" name="wayarea2grid" output="screen">
+  <node pkg="object_map" type="wayarea2grid" name="wayarea2grid">
     <param name="sensor_frame" value="$(arg sensor_frame)" />
     <param name="grid_frame" value="$(arg grid_frame)" />
     <param name="grid_resolution" value="$(arg grid_resolution)" />
diff --git a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch
index 6a99445d..f8cec208 100755
--- a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch
+++ b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch
@@ -10,7 +10,7 @@
   <arg name="grid_position_z" default="-2" />
 
   <!-- Launch node -->
-  <node pkg="object_map" type="wayarea2grid_lanelet2" name="wayarea2grid_lanelet2" output="screen">
+  <node pkg="object_map" type="wayarea2grid_lanelet2" name="wayarea2grid_lanelet2">
     <param name="sensor_frame" value="$(arg sensor_frame)" />
     <param name="grid_frame" value="$(arg grid_frame)" />
     <param name="grid_resolution" value="$(arg grid_resolution)" />
diff --git a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch
index eefa5062..6e9afb05 100644
--- a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch
+++ b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch
@@ -13,7 +13,7 @@
   <!-- Launch node -->
 
   <group if="$(arg use_ll2)">
-    <node pkg="object_map" type="wayarea2grid_lanelet2" name="wayarea2grid_lanelet2" output="screen">
+    <node pkg="object_map" type="wayarea2grid_lanelet2" name="wayarea2grid_lanelet2">
       <param name="sensor_frame" value="$(arg sensor_frame)" />
       <param name="grid_frame" value="$(arg grid_frame)" />
       <param name="grid_resolution" value="$(arg grid_resolution)" />
@@ -26,7 +26,7 @@
   </group>
 
   <group unless="$(arg use_ll2)">
-      <node pkg="object_map" type="wayarea2grid" name="wayarea2grid" output="screen">
+      <node pkg="object_map" type="wayarea2grid" name="wayarea2grid">
         <param name="sensor_frame" value="$(arg sensor_frame)" />
         <param name="grid_frame" value="$(arg grid_frame)" />
         <param name="grid_resolution" value="$(arg grid_resolution)" />
diff --git a/autoware.ai/src/autoware/common/op_planner/CMakeLists.txt b/autoware.ai/src/autoware/common/op_planner/CMakeLists.txt
index 3f50b0ad..41f51317 100644
--- a/autoware.ai/src/autoware/common/op_planner/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/op_planner/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(op_planner)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   cmake_modules
diff --git a/autoware.ai/src/autoware/common/op_planner/include/op_planner/DecisionMaker.h b/autoware.ai/src/autoware/common/op_planner/include/op_planner/DecisionMaker.h
index 9a7036d5..49562aec 100755
--- a/autoware.ai/src/autoware/common/op_planner/include/op_planner/DecisionMaker.h
+++ b/autoware.ai/src/autoware/common/op_planner/include/op_planner/DecisionMaker.h
@@ -53,6 +53,8 @@ class DecisionMaker
   bool m_sprintSwitch;
   double m_obstacleWaitingTimeinIntersection;
   int m_remainObstacleWaitingTime;
+  int m_prevSafeTrajectory;
+  bool m_isTargetVelocityReady = false;
 
   int m_prevTrafficLightID;
   PlannerHNS::TrafficLightState m_prevTrafficLightSignal;
diff --git a/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h b/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h
index 323aeea3..efcf4967 100755
--- a/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h
+++ b/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h
@@ -4,241 +4,257 @@
 /// \author Hatem Darweesh
 /// \date May 19, 2016
 
-
 #ifndef ROADNETWORK_H_
 #define ROADNETWORK_H_
 
-#include <string>
-#include <vector>
-#include <sstream>
 #include "op_utility/UtilityH.h"
+#include <sstream>
 #include <std_msgs/Header.h>
+#include <string>
+#include <vector>
 
 #define OPENPLANNER_ENABLE_LOGS
 
-namespace PlannerHNS
-{
-
-
-enum DIRECTION_TYPE {  FORWARD_DIR, FORWARD_LEFT_DIR, FORWARD_RIGHT_DIR,
-  BACKWARD_DIR, BACKWARD_LEFT_DIR, BACKWARD_RIGHT_DIR, STANDSTILL_DIR};
-
-enum OBSTACLE_TYPE {SIDEWALK, TREE, CAR, TRUCK, HOUSE, PEDESTRIAN, CYCLIST, GENERAL_OBSTACLE};
-
-enum DRIVABLE_TYPE {DIRT, TARMAC, PARKINGAREA, INDOOR, GENERAL_AREA};
-
-enum GLOBAL_STATE_TYPE {G_WAITING_STATE, G_PLANING_STATE, G_FORWARD_STATE, G_BRANCHING_STATE, G_FINISH_STATE};
-
-enum STATE_TYPE {INITIAL_STATE, WAITING_STATE, FORWARD_STATE, STOPPING_STATE, EMERGENCY_STATE,
-  TRAFFIC_LIGHT_STOP_STATE,TRAFFIC_LIGHT_WAIT_STATE, STOP_SIGN_STOP_STATE, STOP_SIGN_WAIT_STATE,
-  FOLLOW_STATE, LANE_CHANGE_STATE, OBSTACLE_AVOIDANCE_STATE, GOAL_STATE, FINISH_STATE, YIELDING_STATE, 
-  BRANCH_LEFT_STATE, BRANCH_RIGHT_STATE, PEDESTRIAN_STATE, INTERSECTION_STATE};
+namespace PlannerHNS {
+
+enum DIRECTION_TYPE { FORWARD_DIR, FORWARD_LEFT_DIR, FORWARD_RIGHT_DIR, BACKWARD_DIR, BACKWARD_LEFT_DIR, BACKWARD_RIGHT_DIR, STANDSTILL_DIR };
+
+enum OBSTACLE_TYPE { SIDEWALK, TREE, CAR, TRUCK, HOUSE, PEDESTRIAN, CYCLIST, GENERAL_OBSTACLE };
+
+enum DRIVABLE_TYPE { DIRT, TARMAC, PARKINGAREA, INDOOR, GENERAL_AREA };
+
+enum GLOBAL_STATE_TYPE { G_WAITING_STATE, G_PLANING_STATE, G_FORWARD_STATE, G_BRANCHING_STATE, G_FINISH_STATE };
+
+enum STATE_TYPE {
+    INITIAL_STATE,
+    WAITING_STATE,
+    FORWARD_STATE,
+    STOPPING_STATE,
+    EMERGENCY_STATE,
+    TRAFFIC_LIGHT_STOP_STATE,
+    TRAFFIC_LIGHT_WAIT_STATE,
+    STOP_SIGN_STOP_STATE,
+    STOP_SIGN_WAIT_STATE,
+    FOLLOW_STATE,
+    LANE_CHANGE_STATE,
+    OBSTACLE_AVOIDANCE_STATE,
+    GOAL_STATE,
+    FINISH_STATE,
+    YIELDING_STATE,
+    BRANCH_LEFT_STATE,
+    BRANCH_RIGHT_STATE,
+    PEDESTRIAN_STATE,
+    INTERSECTION_STATE
+};
 
-enum LIGHT_INDICATOR {INDICATOR_LEFT, INDICATOR_RIGHT, INDICATOR_BOTH , INDICATOR_NONE};
+enum LIGHT_INDICATOR { INDICATOR_LEFT, INDICATOR_RIGHT, INDICATOR_BOTH, INDICATOR_NONE };
 
-enum SHIFT_POS {SHIFT_POS_PP = 0x60, SHIFT_POS_RR = 0x40, SHIFT_POS_NN = 0x20,
-  SHIFT_POS_DD = 0x10, SHIFT_POS_BB = 0xA0, SHIFT_POS_SS = 0x0f, SHIFT_POS_UU = 0xff };
+enum SHIFT_POS {
+    SHIFT_POS_PP = 0x60,
+    SHIFT_POS_RR = 0x40,
+    SHIFT_POS_NN = 0x20,
+    SHIFT_POS_DD = 0x10,
+    SHIFT_POS_BB = 0xA0,
+    SHIFT_POS_SS = 0x0f,
+    SHIFT_POS_UU = 0xff
+};
 
-enum ACTION_TYPE {FORWARD_ACTION, BACKWARD_ACTION, STOP_ACTION, LEFT_TURN_ACTION,
-  RIGHT_TURN_ACTION, U_TURN_ACTION, SWERVE_ACTION, OVERTACK_ACTION, START_ACTION, SLOWDOWN_ACTION, CHANGE_DESTINATION, WAITING_ACTION, DESTINATION_REACHED,  UNKOWN_ACTION};
+enum ACTION_TYPE {
+    FORWARD_ACTION,
+    BACKWARD_ACTION,
+    STOP_ACTION,
+    LEFT_TURN_ACTION,
+    RIGHT_TURN_ACTION,
+    U_TURN_ACTION,
+    SWERVE_ACTION,
+    OVERTACK_ACTION,
+    START_ACTION,
+    SLOWDOWN_ACTION,
+    CHANGE_DESTINATION,
+    WAITING_ACTION,
+    DESTINATION_REACHED,
+    UNKOWN_ACTION
+};
 
-enum BEH_STATE_TYPE {BEH_FORWARD_STATE=0,BEH_STOPPING_STATE=1, BEH_BRANCH_LEFT_STATE=2, BEH_BRANCH_RIGHT_STATE=3, BEH_YIELDING_STATE=4, BEH_ACCELERATING_STATE=5, BEH_SLOWDOWN_STATE=6};
+enum BEH_STATE_TYPE {
+    BEH_FORWARD_STATE = 0,
+    BEH_STOPPING_STATE = 1,
+    BEH_BRANCH_LEFT_STATE = 2,
+    BEH_BRANCH_RIGHT_STATE = 3,
+    BEH_YIELDING_STATE = 4,
+    BEH_ACCELERATING_STATE = 5,
+    BEH_SLOWDOWN_STATE = 6
+};
 
-enum SEGMENT_TYPE {NORMAL_ROAD_SEG, INTERSECTION_ROAD_SEG, UTURN_ROAD_SEG, EXIT_ROAD_SEG, MERGE_ROAD_SEG, HIGHWAY_ROAD_SEG};
-enum RoadSegmentType {NORMAL_ROAD, INTERSECTION_ROAD, UTURN_ROAD, EXIT_ROAD, MERGE_ROAD, HIGHWAY_ROAD};
+enum SEGMENT_TYPE { NORMAL_ROAD_SEG, INTERSECTION_ROAD_SEG, UTURN_ROAD_SEG, EXIT_ROAD_SEG, MERGE_ROAD_SEG, HIGHWAY_ROAD_SEG };
+enum RoadSegmentType { NORMAL_ROAD, INTERSECTION_ROAD, UTURN_ROAD, EXIT_ROAD, MERGE_ROAD, HIGHWAY_ROAD };
 
-enum MARKING_TYPE {UNKNOWN_MARK, TEXT_MARK, AF_MARK, AL_MARK, AR_MARK, AFL_MARK, AFR_MARK, ALR_MARK, UTURN_MARK, NOUTURN_MARK};
+enum MARKING_TYPE { UNKNOWN_MARK, TEXT_MARK, AF_MARK, AL_MARK, AR_MARK, AFL_MARK, AFR_MARK, ALR_MARK, UTURN_MARK, NOUTURN_MARK };
 
-enum TrafficSignTypes {UNKNOWN_SIGN, STOP_SIGN, MAX_SPEED_SIGN, MIN_SPEED_SIGN};
+enum TrafficSignTypes { UNKNOWN_SIGN, STOP_SIGN, MAX_SPEED_SIGN, MIN_SPEED_SIGN };
 
 class Lane;
 class TrafficLight;
 class RoadSegment;
 
-class ObjTimeStamp
-{
-public:
-  timespec tStamp;
-
-  ObjTimeStamp()
-  {
-    tStamp.tv_nsec = 0;
-    tStamp.tv_sec = 0;
-  }
+class ObjTimeStamp {
+  public:
+    timespec tStamp;
+
+    ObjTimeStamp() {
+        tStamp.tv_nsec = 0;
+        tStamp.tv_sec = 0;
+    }
 };
 
-//class POINT2D
+// class POINT2D
 //{
-//public:
-//    double x;
-//    double y;
-//    double z;
-//    POINT2D()
-//    {
-//      x=0;y=0;z=0;
-//    }
-//    POINT2D(double px, double py, double pz = 0)
-//    {
-//      x = px;
-//      y = py;
-//      z = pz;
-//    }
-//};
-
-class GPSPoint
-{
-public:
-  double lat, x;
-  double lon, y;
-  double alt, z;
-  double dir, a;
-
-  GPSPoint()
-  {
-    lat = x = 0;
-    lon = y = 0;
-    alt = z = 0;
-    dir = a = 0;
-  }
-
-  GPSPoint(const double& x, const double& y, const double& z, const double& a)
-  {
-    this->x = x;
-    this->y = y;
-    this->z = z;
-    this->a = a;
-
-    lat = 0;
-    lon = 0;
-    alt = 0;
-    dir = 0;
-  }
-
-  std::string ToString()
-  {
-    std::stringstream str;
-    str.precision(12);
-    str << "X:" << x << ", Y:" << y << ", Z:" << z << ", A:" << a << std::endl;
-    str << "Lon:" << lon << ", Lat:" << lat << ", Alt:" << alt << ", Dir:" << dir << std::endl;
-    return str.str();
-  }
+// public:
+//     double x;
+//     double y;
+//     double z;
+//     POINT2D()
+//     {
+//       x=0;y=0;z=0;
+//     }
+//     POINT2D(double px, double py, double pz = 0)
+//     {
+//       x = px;
+//       y = py;
+//       z = pz;
+//     }
+// };
+
+class GPSPoint {
+  public:
+    double lat, x;
+    double lon, y;
+    double alt, z;
+    double dir, a;
+
+    GPSPoint() {
+        lat = x = 0;
+        lon = y = 0;
+        alt = z = 0;
+        dir = a = 0;
+    }
+
+    GPSPoint(const double &x, const double &y, const double &z, const double &a) {
+        this->x = x;
+        this->y = y;
+        this->z = z;
+        this->a = a;
+
+        lat = 0;
+        lon = 0;
+        alt = 0;
+        dir = 0;
+    }
+
+    std::string ToString() {
+        std::stringstream str;
+        str.precision(12);
+        str << "X:" << x << ", Y:" << y << ", Z:" << z << ", A:" << a << std::endl;
+        str << "Lon:" << lon << ", Lat:" << lat << ", Alt:" << alt << ", Dir:" << dir << std::endl;
+        return str.str();
+    }
 };
 
 class RECTANGLE
 
 {
-public:
-  GPSPoint bottom_left;
-  GPSPoint top_right;
-  double width;
-  double length;
-  bool bObstacle;
-
-
-  inline bool PointInRect(GPSPoint p)
-  {
-    return p.x >= bottom_left.x && p.x <= top_right.x && p.y >= bottom_left.y && p.y <= top_right.y;
-  }
-
-  inline bool PointInsideRect(GPSPoint p)
-  {
-    return p.x > bottom_left.x && p.x < top_right.x && p.y > bottom_left.y && p.y < top_right.y;
-  }
-
-  inline bool HitTest(GPSPoint p)
-  {
-    return PointInRect(p) && bObstacle;
-  }
-
-  RECTANGLE()
-  {
-    width=0;
-    length = 0;
-    bObstacle = true;
-  }
-
-  virtual ~RECTANGLE(){}
+  public:
+    GPSPoint bottom_left;
+    GPSPoint top_right;
+    double width;
+    double length;
+    bool bObstacle;
+
+    inline bool PointInRect(GPSPoint p) { return p.x >= bottom_left.x && p.x <= top_right.x && p.y >= bottom_left.y && p.y <= top_right.y; }
+
+    inline bool PointInsideRect(GPSPoint p) { return p.x > bottom_left.x && p.x < top_right.x && p.y > bottom_left.y && p.y < top_right.y; }
+
+    inline bool HitTest(GPSPoint p) { return PointInRect(p) && bObstacle; }
+
+    RECTANGLE() {
+        width = 0;
+        length = 0;
+        bObstacle = true;
+    }
+
+    virtual ~RECTANGLE() {}
 };
 
-class PolygonShape
-{
-public:
-  std::vector<GPSPoint> points;
-
-  inline int PointInsidePolygon(const PolygonShape& polygon,const GPSPoint& p)
-  {
-    int counter = 0;
-      int i;
-      double xinters;
-      GPSPoint p1,p2;
-      int N = polygon.points.size();
-      if(N <=0 ) return -1;
-
-      p1 = polygon.points.at(0);
-      for (i=1;i<=N;i++)
-      {
-        p2 = polygon.points.at(i % N);
-
-        if (p.y > MIN(p1.y,p2.y))
-        {
-          if (p.y <= MAX(p1.y,p2.y))
-          {
-            if (p.x <= MAX(p1.x,p2.x))
-            {
-              if (p1.y != p2.y)
-              {
-                xinters = (p.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x;
-                if (p1.x == p2.x || p.x <= xinters)
-                  counter++;
-              }
+class PolygonShape {
+  public:
+    std::vector<GPSPoint> points;
+
+    inline int PointInsidePolygon(const PolygonShape &polygon, const GPSPoint &p) {
+        int counter = 0;
+        int i;
+        double xinters;
+        GPSPoint p1, p2;
+        int N = polygon.points.size();
+        if (N <= 0)
+            return -1;
+
+        p1 = polygon.points.at(0);
+        for (i = 1; i <= N; i++) {
+            p2 = polygon.points.at(i % N);
+
+            if (p.y > MIN(p1.y, p2.y)) {
+                if (p.y <= MAX(p1.y, p2.y)) {
+                    if (p.x <= MAX(p1.x, p2.x)) {
+                        if (p1.y != p2.y) {
+                            xinters = (p.y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y) + p1.x;
+                            if (p1.x == p2.x || p.x <= xinters)
+                                counter++;
+                        }
+                    }
+                }
             }
-          }
+            p1 = p2;
         }
-        p1 = p2;
-      }
-
-      if (counter % 2 == 0)
-        return 0;
-      else
-        return 1;
-  }
+
+        if (counter % 2 == 0)
+            return 0;
+        else
+            return 1;
+    }
 };
 
-class MapItem
-{
-public:
-  int id;
-  GPSPoint sp; //start point
-  GPSPoint ep; // end point
-  GPSPoint center;
-  double c; //curvature
-  double w; //width
-  double l; //length
-  std::string fileName; //
-  std::vector<GPSPoint> polygon;
-
-
-  MapItem(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length, std::string objName)
-  {
-    id = ID;
-    sp = start;
-    ep = end;
-    c = curvature;
-    w = width;
-    l = length;
-    fileName = objName;
-
-  }
-
-  MapItem()
-  {
-    id = 0; c = 0; w = 0; l = 0;
-  }
-
-  virtual ~MapItem(){}
-
-  MapItem(const MapItem & cmi)
-  {
+class MapItem {
+  public:
+    int id;
+    GPSPoint sp; // start point
+    GPSPoint ep; // end point
+    GPSPoint center;
+    double c;             // curvature
+    double w;             // width
+    double l;             // length
+    std::string fileName; //
+    std::vector<GPSPoint> polygon;
+
+    MapItem(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length, std::string objName) {
+        id = ID;
+        sp = start;
+        ep = end;
+        c = curvature;
+        w = width;
+        l = length;
+        fileName = objName;
+    }
+
+    MapItem() {
+        id = 0;
+        c = 0;
+        w = 0;
+        l = 0;
+    }
+
+    virtual ~MapItem() {}
+
+    MapItem(const MapItem &cmi) {
         id = cmi.id;
         sp = cmi.sp;
         ep = cmi.ep;
@@ -246,1160 +262,1053 @@ class MapItem
         w = cmi.w;
         l = cmi.l;
         fileName = cmi.fileName;
-  }
-  MapItem &operator=(const MapItem &cmi)
-  {
-    this->id = cmi.id;
-      this->sp = cmi.sp;
-      this->ep = cmi.ep;
-      this->c = cmi.c;
-      this->w = cmi.w;
-      this->l = cmi.l;
-      this->fileName = cmi.fileName;
-      return *this;
-  }
-
-  virtual int operator==(const MapItem &mi) const
-    {
-      return this->id == mi.id;
     }
+    MapItem &operator=(const MapItem &cmi) {
+        this->id = cmi.id;
+        this->sp = cmi.sp;
+        this->ep = cmi.ep;
+        this->c = cmi.c;
+        this->w = cmi.w;
+        this->l = cmi.l;
+        this->fileName = cmi.fileName;
+        return *this;
+    }
+
+    virtual int operator==(const MapItem &mi) const { return this->id == mi.id; }
 };
 
-class Obstacle : public MapItem
-{
+class Obstacle : public MapItem {
   public:
     OBSTACLE_TYPE t;
 
-    Obstacle(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length,OBSTACLE_TYPE type, std::string fileName ) : MapItem(ID, start, end, curvature, width, length, fileName)
-  {
-      t = type;
-  }
-    virtual ~Obstacle()
-    {
+    Obstacle(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length, OBSTACLE_TYPE type, std::string fileName)
+        : MapItem(ID, start, end, curvature, width, length, fileName) {
+        t = type;
+    }
+    virtual ~Obstacle() {}
+
+    Obstacle() : MapItem() { t = SIDEWALK; }
+
+    Obstacle(const Obstacle &ob) : MapItem(ob) { t = ob.t; }
+
+    Obstacle &operator=(const Obstacle &ob) {
+        this->id = ob.id;
+        this->sp = ob.sp;
+        this->ep = ob.ep;
+        this->c = ob.c;
+        this->w = ob.w;
+        this->l = ob.l;
+        this->t = ob.t;
+        this->fileName = ob.fileName;
+        return *this;
     }
 
-    Obstacle() : MapItem()
-       {
-      t = SIDEWALK;
-       }
-
-    Obstacle(const Obstacle& ob) : MapItem(ob)
-      {
-        t = ob.t;
-      }
-
-    Obstacle& operator=(const Obstacle& ob)
-      {
-      this->id = ob.id;
-      this->sp = ob.sp;
-      this->ep = ob.ep;
-      this->c = ob.c;
-      this->w = ob.w;
-      this->l = ob.l;
-      this->t = ob.t;
-      this->fileName = ob.fileName;
-      return *this;
-      }
-
-      virtual int operator==(const Obstacle &ob) const
-          {
-            return this->id == ob.id && this->t == ob.t;
-          }
+    virtual int operator==(const Obstacle &ob) const { return this->id == ob.id && this->t == ob.t; }
 };
 
-class DrivableArea : public MapItem
-{
-public:
-  DRIVABLE_TYPE t; // drivable area type
-
-  DrivableArea(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length,DRIVABLE_TYPE type, std::string fileName ) : MapItem( ID, start, end, curvature, width, length, fileName)
-  {
-    t = type;
-  }
+class DrivableArea : public MapItem {
+  public:
+    DRIVABLE_TYPE t; // drivable area type
 
-  virtual ~DrivableArea()
-  {
+    DrivableArea(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length, DRIVABLE_TYPE type, std::string fileName)
+        : MapItem(ID, start, end, curvature, width, length, fileName) {
+        t = type;
+    }
 
-  }
+    virtual ~DrivableArea() {}
 
-  DrivableArea() : MapItem()
-    {
-      t = PARKINGAREA;
-    }
+    DrivableArea() : MapItem() { t = PARKINGAREA; }
 
-  DrivableArea(const DrivableArea& da) : MapItem(da)
-  {
-    t = da.t;
-  }
-
-  DrivableArea& operator=(const DrivableArea& da)
-  {
-    this->id = da.id;
-    this->sp = da.sp;
-    this->ep = da.ep;
-    this->c = da.c;
-    this->w = da.w;
-    this->l = da.l;
-    this->t = da.t;
-    this->fileName = da.fileName;
-    return *this;
-  }
-
-  virtual int operator==(const DrivableArea &da) const
-      {
-        return this->id == da.id && this->t == da.t;
-      }
+    DrivableArea(const DrivableArea &da) : MapItem(da) { t = da.t; }
 
-};
+    DrivableArea &operator=(const DrivableArea &da) {
+        this->id = da.id;
+        this->sp = da.sp;
+        this->ep = da.ep;
+        this->c = da.c;
+        this->w = da.w;
+        this->l = da.l;
+        this->t = da.t;
+        this->fileName = da.fileName;
+        return *this;
+    }
 
-class Rotation
-{
-public:
-  double x;
-  double y;
-  double z;
-  double w;
-
-  Rotation()
-  {
-    x = 0;
-    y = 0;
-    z = 0;
-    w = 0;
-  }
+    virtual int operator==(const DrivableArea &da) const { return this->id == da.id && this->t == da.t; }
 };
 
-class WayPoint
-{
-public:
-  GPSPoint  pos;
-  Rotation   rot;
-  double    v;
-  double    cost;
-  double    timeCost;
-  double    totalReward;
-  double    collisionCost;
-  double     laneChangeCost;
-  int     laneId;
-  int     id;
-  int     LeftPointId;
-  int     RightPointId;
-  int     LeftLnId;
-  int     RightLnId;
-  int     stopLineID;
-  DIRECTION_TYPE bDir;
-  STATE_TYPE  state;
-  BEH_STATE_TYPE beh_state;
-  int     iOriginalIndex;
-
-  Lane* pLane;
-  WayPoint* pLeft;
-  WayPoint* pRight;
-  std::vector<int>   toIds;
-  std::vector<int>   fromIds;
-  std::vector<WayPoint*> pFronts;
-  std::vector<WayPoint*> pBacks;
-  std::vector<std::pair<ACTION_TYPE, double> > actionCost;
-
-  int      originalMapID;
-  int      gid;
-
-  WayPoint()
-  {
-    id = 0;
-    v = 0;
-    cost = 0;
-    laneId = -1;
-    pLane  = 0;
-    pLeft = 0;
-    pRight = 0;
-    bDir = FORWARD_DIR;
-    LeftPointId = 0;
-    RightPointId = 0;
-    LeftLnId = 0;
-    RightLnId = 0;
-    timeCost = 0;
-    totalReward = 0;
-    collisionCost = 0;
-    laneChangeCost = 0;
-    stopLineID = -1;
-    state = INITIAL_STATE;
-    beh_state = BEH_STOPPING_STATE;
-    iOriginalIndex = 0;
-
-    gid = 0;
-    originalMapID = -1;
-  }
-
-  WayPoint(const double& x, const double& y, const double& z, const double& a)
-  {
-    pos.x = x;
-    pos.y = y;
-    pos.z = z;
-    pos.a = a;
-
-    id = 0;
-    v = 0;
-    cost = 0;
-    laneId = -1;
-    pLane  = 0;
-    pLeft = 0;
-    pRight = 0;
-    bDir = FORWARD_DIR;
-    LeftPointId = 0;
-    RightPointId = 0;
-    LeftLnId = 0;
-    RightLnId = 0;
-    timeCost = 0;
-    totalReward = 0;
-    collisionCost = 0;
-    laneChangeCost = 0;
-    stopLineID = -1;
-    iOriginalIndex = 0;
-    state = INITIAL_STATE;
-    beh_state = BEH_STOPPING_STATE;
-
-    gid = 0;
-    originalMapID = -1;
-  }
+class Rotation {
+  public:
+    double x;
+    double y;
+    double z;
+    double w;
+
+    Rotation() {
+        x = 0;
+        y = 0;
+        z = 0;
+        w = 0;
+    }
 };
 
-class RelativeInfo
-{
-public:
-  double perp_distance;
-  double to_front_distance; //negative
-  double from_back_distance;
-  int iFront;
-  int iBack;
-  int iGlobalPath;
-  WayPoint perp_point;
-  double angle_diff; // degrees
-  bool bBefore;
-  bool bAfter;
-  double after_angle;
-
-  RelativeInfo()
-  {
-    after_angle = 0;
-    bBefore = false;
-    bAfter = false;
-    perp_distance = 0;
-    to_front_distance = 0;
-    from_back_distance = 0;
-    iFront = 0;
-    iBack = 0;
-    iGlobalPath = 0;
-    angle_diff = 0;
-  }
-};
+class WayPoint {
+  public:
+    GPSPoint pos;
+    Rotation rot;
+    double v;
+    double cost;
+    double timeCost;
+    double totalReward;
+    double collisionCost;
+    double laneChangeCost;
+    int laneId;
+    int id;
+    int LeftPointId;
+    int RightPointId;
+    int LeftLnId;
+    int RightLnId;
+    int stopLineID;
+    DIRECTION_TYPE bDir;
+    STATE_TYPE state;
+    BEH_STATE_TYPE beh_state;
+    int iOriginalIndex;
+
+    Lane *pLane;
+    WayPoint *pLeft;
+    WayPoint *pRight;
+    std::vector<int> toIds;
+    std::vector<int> fromIds;
+    std::vector<WayPoint *> pFronts;
+    std::vector<WayPoint *> pBacks;
+    std::vector<std::pair<ACTION_TYPE, double>> actionCost;
+
+    int originalMapID;
+    int gid;
+
+    WayPoint() {
+        id = 0;
+        v = 0;
+        cost = 0;
+        laneId = -1;
+        pLane = 0;
+        pLeft = 0;
+        pRight = 0;
+        bDir = FORWARD_DIR;
+        LeftPointId = 0;
+        RightPointId = 0;
+        LeftLnId = 0;
+        RightLnId = 0;
+        timeCost = 0;
+        totalReward = 0;
+        collisionCost = 0;
+        laneChangeCost = 0;
+        stopLineID = -1;
+        state = INITIAL_STATE;
+        beh_state = BEH_STOPPING_STATE;
+        iOriginalIndex = 0;
+
+        gid = 0;
+        originalMapID = -1;
+    }
 
-class Boundary //represent wayarea in vector map
-{
-public:
-  int id;
-  int roadId;
-  std::vector<GPSPoint> points;
-  RoadSegment* pSegment;
-
-  Boundary()
-  {
-    id    = 0;
-    roadId =0;
-    pSegment = nullptr;
-  }
+    WayPoint(const double &x, const double &y, const double &z, const double &a) {
+        pos.x = x;
+        pos.y = y;
+        pos.z = z;
+        pos.a = a;
+
+        id = 0;
+        v = 0;
+        cost = 0;
+        laneId = -1;
+        pLane = 0;
+        pLeft = 0;
+        pRight = 0;
+        bDir = FORWARD_DIR;
+        LeftPointId = 0;
+        RightPointId = 0;
+        LeftLnId = 0;
+        RightLnId = 0;
+        timeCost = 0;
+        totalReward = 0;
+        collisionCost = 0;
+        laneChangeCost = 0;
+        stopLineID = -1;
+        iOriginalIndex = 0;
+        state = INITIAL_STATE;
+        beh_state = BEH_STOPPING_STATE;
+
+        gid = 0;
+        originalMapID = -1;
+    }
 };
 
-class Curb
-{
-public:
-  int id;
-  int laneId;
-  int roadId;
-  std::vector<GPSPoint> points;
-  Lane* pLane;
-
-  Curb()
-  {
-    id    = 0;
-    laneId =0;
-    roadId =0;
-    pLane = 0;
-  }
+class RelativeInfo {
+  public:
+    double perp_distance;
+    double to_front_distance; // negative
+    double from_back_distance;
+    int iFront;
+    int iBack;
+    int iGlobalPath;
+    WayPoint perp_point;
+    double angle_diff; // degrees
+    bool bBefore;
+    bool bAfter;
+    double after_angle;
+
+    RelativeInfo() {
+        after_angle = 0;
+        bBefore = false;
+        bAfter = false;
+        perp_distance = 0;
+        to_front_distance = 0;
+        from_back_distance = 0;
+        iFront = 0;
+        iBack = 0;
+        iGlobalPath = 0;
+        angle_diff = 0;
+    }
 };
 
-class Crossing
+class Boundary // represent wayarea in vector map
 {
-public:
-  int id;
-  int roadId;
-  GPSPoint pos;
-  PolygonShape intersection_area;
-  std::vector<PolygonShape> risky_area;
-  std::vector<GPSPoint> points;
-  RoadSegment* pSegment;
-
-  Crossing()
-  {
-    id    = 0;
-    roadId = 0;
-    pSegment = nullptr;
-  }
+  public:
+    int id;
+    int roadId;
+    std::vector<GPSPoint> points;
+    RoadSegment *pSegment;
+
+    Boundary() {
+        id = 0;
+        roadId = 0;
+        pSegment = nullptr;
+    }
 };
 
-class StopLine
-{
-public:
-  int id;
-  int laneId;
-  int roadId;
-  int trafficLightID;
-  int stopSignID;
-  int length;
-  std::vector<GPSPoint> points;
-  Lane* pLane;
-  int linkID;
-
-  StopLine()
-  {
-    id    = 0;
-    laneId =0;
-    roadId =0;
-    pLane = 0;
-    trafficLightID = -1;
-    stopSignID = -1;
-    linkID = 0;
-    length = 0;
-  }
+class Curb {
+  public:
+    int id;
+    int laneId;
+    int roadId;
+    std::vector<GPSPoint> points;
+    Lane *pLane;
+
+    Curb() {
+        id = 0;
+        laneId = 0;
+        roadId = 0;
+        pLane = 0;
+    }
 };
 
-class WaitingLine
-{
-public:
-  int id;
-  int laneId;
-  int roadId;
-  std::vector<GPSPoint> points;
-  Lane* pLane;
-
-  WaitingLine()
-  {
-    id    = 0;
-    laneId =0;
-    roadId =0;
-    pLane = 0;
-  }
+class Crossing {
+  public:
+    int id;
+    int roadId;
+    GPSPoint pos;
+    PolygonShape intersection_area;
+    std::vector<PolygonShape> risky_area;
+    std::vector<GPSPoint> points;
+    RoadSegment *pSegment;
+
+    Crossing() {
+        id = 0;
+        roadId = 0;
+        pSegment = nullptr;
+    }
 };
 
-class TrafficSign
-{
-public:
-  int id;
-  int laneId;
-  int roadId;
-
-  GPSPoint pos;
-  TrafficSignTypes signType;
-  double value;
-  double fromValue;
-  double toValue;
-  std::string strValue;
-  timespec timeValue;
-  timespec fromTimeValue;
-  timespec toTimeValue;
-
-  Lane* pLane;
-
-  TrafficSign()
-  {
-    id        = 0;
-    laneId     = 0;
-    roadId    = 0;
-    signType    = UNKNOWN_SIGN;
-    value    = 0;
-    fromValue  = 0;
-    toValue    = 0;
-//    timeValue  = 0;
-//    fromTimeValue = 0;
-//    toTimeValue  = 0;
-    pLane     = 0;
-  }
+class StopLine {
+  public:
+    int id;
+    int laneId;
+    int roadId;
+    int trafficLightID;
+    int stopSignID;
+    int length;
+    std::vector<GPSPoint> points;
+    Lane *pLane;
+    int linkID;
+
+    StopLine() {
+        id = 0;
+        laneId = 0;
+        roadId = 0;
+        pLane = 0;
+        trafficLightID = -1;
+        stopSignID = -1;
+        linkID = 0;
+        length = 0;
+    }
 };
 
-enum TrafficLightState {UNKNOWN_LIGHT, RED_LIGHT, GREEN_LIGHT, YELLOW_LIGHT, LEFT_GREEN, FORWARD_GREEN, RIGHT_GREEN, FLASH_YELLOW, FLASH_RED};
-
-class TrafficLight
-{
-public:
-  int id;
-  GPSPoint pos;
-  TrafficLightState lightState;
-  double stoppingDistance;
-  std::vector<int> laneIds;
-  std::vector<Lane*> pLanes;
-  int linkID;
-  double remainTime;
-  std::vector<double> routine;
-
-  TrafficLight()
-  {
-    stoppingDistance = 2;
-    id       = 0;
-    lightState  = GREEN_LIGHT;
-    linkID     = 0;
-    remainTime = 0;
-  }
-
-  bool CheckLane(const int& laneId)
-  {
-    for(unsigned int i=0; i < laneIds.size(); i++)
-    {
-      if(laneId == laneIds.at(i))
-        return true;
+class WaitingLine {
+  public:
+    int id;
+    int laneId;
+    int roadId;
+    std::vector<GPSPoint> points;
+    Lane *pLane;
+
+    WaitingLine() {
+        id = 0;
+        laneId = 0;
+        roadId = 0;
+        pLane = 0;
     }
-    return false;
-  }
 };
 
-class Marking
-{
-public:
-  int id;
-  int laneId;
-  int roadId;
-  MARKING_TYPE  mark_type;
-  GPSPoint center;
-  std::vector<GPSPoint> points;
-  Lane* pLane;
-
-  Marking()
-  {
-    id = 0;
-    laneId = 0;
-    roadId = 0;
-    mark_type = UNKNOWN_MARK;
-    pLane = nullptr;
-  }
+class TrafficSign {
+  public:
+    int id;
+    int laneId;
+    int roadId;
+
+    GPSPoint pos;
+    TrafficSignTypes signType;
+    double value;
+    double fromValue;
+    double toValue;
+    std::string strValue;
+    timespec timeValue;
+    timespec fromTimeValue;
+    timespec toTimeValue;
+
+    Lane *pLane;
+
+    TrafficSign() {
+        id = 0;
+        laneId = 0;
+        roadId = 0;
+        signType = UNKNOWN_SIGN;
+        value = 0;
+        fromValue = 0;
+        toValue = 0;
+        //    timeValue  = 0;
+        //    fromTimeValue = 0;
+        //    toTimeValue  = 0;
+        pLane = 0;
+    }
 };
 
-class RoadSegment
-{
-public:
-  int id;
-
-  SEGMENT_TYPE   roadType;
-  Boundary  boundary;
-  Crossing  start_crossing;
-  Crossing  finish_crossing;
-  double     avgWidth;
-  std::vector<int> fromIds;
-  std::vector<int> toIds;
-  std::vector<Lane> Lanes;
-
-
-  std::vector<RoadSegment*> fromLanes;
-  std::vector<RoadSegment*> toLanes;
-
-  RoadSegment()
-  {
-    id = 0;
-    avgWidth = 0;
-    roadType = NORMAL_ROAD_SEG;
-  }
+enum TrafficLightState { UNKNOWN_LIGHT, RED_LIGHT, GREEN_LIGHT, YELLOW_LIGHT, LEFT_GREEN, FORWARD_GREEN, RIGHT_GREEN, FLASH_YELLOW, FLASH_RED };
 
+class TrafficLight {
+  public:
+    int id;
+    GPSPoint pos;
+    TrafficLightState lightState;
+    double stoppingDistance;
+    std::vector<int> laneIds;
+    std::vector<Lane *> pLanes;
+    int linkID;
+    double remainTime;
+    std::vector<double> routine;
+
+    TrafficLight() {
+        stoppingDistance = 2;
+        id = 0;
+        lightState = GREEN_LIGHT;
+        linkID = 0;
+        remainTime = 0;
+    }
 
+    bool CheckLane(const int &laneId) {
+        for (unsigned int i = 0; i < laneIds.size(); i++) {
+            if (laneId == laneIds.at(i))
+                return true;
+        }
+        return false;
+    }
 };
 
-enum LaneType{NORMAL_LANE, MERGE_LANE, EXIT_LANE, BUS_LANE, BUS_STOP_LANE, EMERGENCY_LANE};
-
-class Lane
-{
-public:
-  int id;
-  int roadId;
-  int areaId;
-  int fromAreaId;
-  int toAreaId;
-  std::vector<int> fromIds;
-  std::vector<int> toIds;
-  int num; //lane number in the road segment from left to right
-  double speed;
-  double length;
-  double dir;
-  LaneType type;
-  double width;
-  std::vector<WayPoint> points;
-  std::vector<TrafficLight> trafficlights;
-  std::vector<StopLine> stopLines;
-  WaitingLine waitingLine;
-
-  std::vector<Lane*> fromLanes;
-  std::vector<Lane*> toLanes;
-  Lane* pLeftLane;
-  Lane* pRightLane;
-
-  RoadSegment * pRoad;
-
-  Lane()
-  {
-    id     = 0;
-    num    = 0;
-    speed   = 0;
-    length   = 0;
-    dir    = 0;
-    type   = NORMAL_LANE;
-    width   = 0;
-    pLeftLane = 0;
-    pRightLane = 0;
-    pRoad  = 0;
-    roadId = 0;
-    areaId = 0;
-    fromAreaId = 0;
-    toAreaId = 0;
-  }
-
+class Marking {
+  public:
+    int id;
+    int laneId;
+    int roadId;
+    MARKING_TYPE mark_type;
+    GPSPoint center;
+    std::vector<GPSPoint> points;
+    Lane *pLane;
+
+    Marking() {
+        id = 0;
+        laneId = 0;
+        roadId = 0;
+        mark_type = UNKNOWN_MARK;
+        pLane = nullptr;
+    }
 };
 
-class RoadNetwork
-{
-public:
-  std::vector<RoadSegment> roadSegments;
-  std::vector<TrafficLight> trafficLights;
-  std::vector<StopLine> stopLines;
-  std::vector<Curb> curbs;
-  std::vector<Boundary> boundaries;
-  std::vector<Crossing> crossings;
-  std::vector<Marking> markings;
-  std::vector<TrafficSign> signs;
+class RoadSegment {
+  public:
+    int id;
+
+    SEGMENT_TYPE roadType;
+    Boundary boundary;
+    Crossing start_crossing;
+    Crossing finish_crossing;
+    double avgWidth;
+    std::vector<int> fromIds;
+    std::vector<int> toIds;
+    std::vector<Lane> Lanes;
+
+    std::vector<RoadSegment *> fromLanes;
+    std::vector<RoadSegment *> toLanes;
+
+    RoadSegment() {
+        id = 0;
+        avgWidth = 0;
+        roadType = NORMAL_ROAD_SEG;
+    }
 };
 
-class VehicleState : public ObjTimeStamp
-{
-public:
-  double speed;
-  double steer;
-  SHIFT_POS shift;
-
-  VehicleState()
-  {
-    speed = 0;
-    steer = 0;
-    shift = SHIFT_POS_NN;
-  }
+enum LaneType { NORMAL_LANE, MERGE_LANE, EXIT_LANE, BUS_LANE, BUS_STOP_LANE, EMERGENCY_LANE };
 
+class Lane {
+  public:
+    int id;
+    int roadId;
+    int areaId;
+    int fromAreaId;
+    int toAreaId;
+    std::vector<int> fromIds;
+    std::vector<int> toIds;
+    int num; // lane number in the road segment from left to right
+    double speed;
+    double length;
+    double dir;
+    LaneType type;
+    double width;
+    std::vector<WayPoint> points;
+    std::vector<TrafficLight> trafficlights;
+    std::vector<StopLine> stopLines;
+    WaitingLine waitingLine;
+
+    std::vector<Lane *> fromLanes;
+    std::vector<Lane *> toLanes;
+    Lane *pLeftLane;
+    Lane *pRightLane;
+
+    RoadSegment *pRoad;
+
+    Lane() {
+        id = 0;
+        num = 0;
+        speed = 0;
+        length = 0;
+        dir = 0;
+        type = NORMAL_LANE;
+        width = 0;
+        pLeftLane = 0;
+        pRightLane = 0;
+        pRoad = 0;
+        roadId = 0;
+        areaId = 0;
+        fromAreaId = 0;
+        toAreaId = 0;
+    }
 };
 
-class BehaviorState
-{
-public:
-  STATE_TYPE state;
-  double maxVelocity;
-  double minVelocity;
-  double stopDistance;
-  double followVelocity;
-  double followDistance;
-  LIGHT_INDICATOR indicator;
-  bool bNewPlan;
-  int iTrajectory;
-  int currTrajectory;
-
-
-  BehaviorState()
-  {
-    state = INITIAL_STATE;
-    maxVelocity = 0;
-    minVelocity = 0;
-    stopDistance = 0;
-    followVelocity = 0;
-    followDistance = 0;
-    indicator  = INDICATOR_NONE;
-    bNewPlan = false;
-    iTrajectory = -1;
-    currTrajectory = -1;
-  }
-
+class RoadNetwork {
+  public:
+    std::vector<RoadSegment> roadSegments;
+    std::vector<TrafficLight> trafficLights;
+    std::vector<StopLine> stopLines;
+    std::vector<Curb> curbs;
+    std::vector<Boundary> boundaries;
+    std::vector<Crossing> crossings;
+    std::vector<Marking> markings;
+    std::vector<TrafficSign> signs;
 };
 
-class DetectedObject
-{
-public:
-  std_msgs::Header header;
-  int id;
-  std::string label;
-  OBSTACLE_TYPE t;
-  WayPoint center;
-  WayPoint predicted_center;
-  WayPoint noisy_center;
-  STATE_TYPE predicted_behavior;
-  std::vector<WayPoint> centers_list;
-  std::vector<GPSPoint> contour;
-  std::vector<std::vector<WayPoint> > predTrajectories;
-  std::vector<WayPoint*> pClosestWaypoints;
-  double w;
-  double l;
-  double h;
-  double distance_to_center;
-  double image_width;
-  double image_height;
-  double image_x;
-  double image_y;
-
-  double actual_speed;
-  double actual_yaw;
-
-  bool bDirection;
-  bool bVelocity;
-  int acceleration;
-
-  int acceleration_desc;
-  double acceleration_raw;
-
-  LIGHT_INDICATOR indicator_state;
-
-  int originalID;
-  BEH_STATE_TYPE behavior_state;
-
-  DetectedObject()
-  {
-    bDirection = false;
-    bVelocity = false;
-    acceleration = 0;
-    acceleration_desc = 0;
-    acceleration_raw = 0.0;
-    id = 0;
-    w = 0;
-    l = 0;
-    h = 0;
-    t = GENERAL_OBSTACLE;
-    distance_to_center = 0;
-    predicted_behavior = INITIAL_STATE;
-    actual_speed = 0;
-    actual_yaw = 0;
-
-    acceleration_desc = 0;
-    acceleration_raw = 0.0;
-    indicator_state = INDICATOR_NONE;
-
-    originalID = -1;
-    behavior_state = BEH_STOPPING_STATE;
-  }
-
+class VehicleState : public ObjTimeStamp {
+  public:
+    double speed;
+    double steer;
+    SHIFT_POS shift;
+
+    VehicleState() {
+        speed = 0;
+        steer = 0;
+        shift = SHIFT_POS_NN;
+    }
 };
 
-class PlanningParams
-{
-public:
-  double   maxSpeed;
-  double   minSpeed;
-  double   planningDistance;
-  double   microPlanDistance;
-  double   carTipMargin;
-  double   rollInMargin;
-  double   rollInSpeedFactor;
-  double   pathDensity;
-  double   rollOutDensity;
-  int   rollOutNumber;
-  double   horizonDistance;
-  double   minFollowingDistance; //should be bigger than Distance to follow
-  double   minDistanceToAvoid; // should be smaller than minFollowingDistance and larger than maxDistanceToAvoid
-  double  maxDistanceToAvoid; // should be smaller than minDistanceToAvoid
-  double   speedProfileFactor;
-  double   smoothingDataWeight;
-  double   smoothingSmoothWeight;
-  double   smoothingToleranceError;
-
-  // Added by HJW for make traj eval parameter
-  double weightPriority;
-  double weightTransition;
-  double weightLong;
-  double weightLat;
-  double LateralSkipDistance;
-  
-  // Added by HJW for traffic signal parameter
-  double stopLineMargin;
-  double stopLineDetectionDistance;
-
-  // Added by HJW for handle intersection
-  bool isInsideIntersection;
-  int closestIntersectionID;
-  double closestIntersectionDistance;
-  int riskyAreaIdx;
-  bool obstacleinRiskyArea;
-
-  bool enableSlowDownOnCurve;
-  double curveVelocityRatio;
-
-  // Added by PHY
-  bool pedestrianAppearence;
-  bool turnLeft;
-  bool turnRight;
-
-  double stopSignStopTime;
-
-  double additionalBrakingDistance;
-  double verticalSafetyDistance;
-  double horizontalSafetyDistancel;
-
-  double giveUpDistance;
-
-  int nReliableCount;
-
-  bool   enableLaneChange;
-  bool   enableSwerving;
-  bool   enableFollowing;
-  bool   enableHeadingSmoothing;
-  bool   enableTrafficLightBehavior;
-  bool   enableStopSignBehavior;
-
-  bool   enabTrajectoryVelocities;
-  double minIndicationDistance;
-
-  PlanningParams()
-  {
-    maxSpeed             = 3;
-    minSpeed             = 0;
-    planningDistance         = 10000;
-    microPlanDistance         = 30;
-    carTipMargin          = 4.0;
-    rollInMargin          = 12.0;
-    rollInSpeedFactor        = 0.25;
-    pathDensity            = 0.25;
-    rollOutDensity          = 0.5;
-    rollOutNumber          = 4;
-    horizonDistance          = 120;
-    minFollowingDistance      = 35;
-    minDistanceToAvoid        = 15;
-    maxDistanceToAvoid        = 5;
-    speedProfileFactor        = 1.0;
-    smoothingDataWeight        = 0.47;
-    smoothingSmoothWeight      = 0.2;
-    smoothingToleranceError      = 0.05;
-
-    double weightPriority     = 1;
-    double weightTransition   = 1;
-    double weightLong         = 1.2;
-    double weightLat          = 1;
-    double LateralSkipDistance = 5;
-
-    stopSignStopTime         = 2.0;
-
-    additionalBrakingDistance    = 10.0;
-    verticalSafetyDistance       = 0.0;
-    horizontalSafetyDistancel    = 0.0;
-
-    giveUpDistance          = -4;
-    nReliableCount          = 2;
-
-    enableSlowDownOnCurve = false;
-    curveVelocityRatio = 0.5;
-
-    enableHeadingSmoothing      = false;
-    enableSwerving           = false;
-    enableFollowing          = false;
-    enableTrafficLightBehavior    = false;
-    enableLaneChange         = false;
-    enableStopSignBehavior      = false;
-    enabTrajectoryVelocities     = false;
-    minIndicationDistance      = 15;
-    pedestrianAppearence       = false;
-
-    turnRight                  = false;
-    turnLeft                   = false;
-
-    isInsideIntersection       = false;
-    closestIntersectionID      = -1;
-    riskyAreaIdx               = -1;
-    obstacleinRiskyArea        = false;
-  }
+class BehaviorState {
+  public:
+    STATE_TYPE state;
+    double maxVelocity;
+    double minVelocity;
+    double stopDistance;
+    double followVelocity;
+    double followDistance;
+    LIGHT_INDICATOR indicator;
+    bool bNewPlan;
+    int iTrajectory;
+    int currTrajectory;
+
+    BehaviorState() {
+        state = INITIAL_STATE;
+        maxVelocity = 0;
+        minVelocity = 0;
+        stopDistance = 0;
+        followVelocity = 0;
+        followDistance = 0;
+        indicator = INDICATOR_NONE;
+        bNewPlan = false;
+        iTrajectory = -1;
+        currTrajectory = -1;
+    }
 };
 
-class HMIPreCalculatedConditions
-{
-public:
+class DetectedObject {
+  public:
+    std_msgs::Header header;
+    int id;
+    std::string label;
+    OBSTACLE_TYPE t;
+    WayPoint center;
+    WayPoint predicted_center;
+    WayPoint noisy_center;
+    STATE_TYPE predicted_behavior;
+    std::vector<WayPoint> centers_list;
+    std::vector<GPSPoint> contour;
+    std::vector<std::vector<WayPoint>> predTrajectories;
+    std::vector<WayPoint *> pClosestWaypoints;
+    double w;
+    double l;
+    double h;
+    double distance_to_center;
+    double image_width;
+    double image_height;
+    double image_x;
+    double image_y;
+
+    double actual_speed;
+    double actual_yaw;
+
+    bool bDirection;
+    bool bVelocity;
+    int acceleration;
+
+    int acceleration_desc;
+    double acceleration_raw;
+
+    LIGHT_INDICATOR indicator_state;
+
+    int originalID;
+    BEH_STATE_TYPE behavior_state;
+
+    DetectedObject() {
+        bDirection = false;
+        bVelocity = false;
+        acceleration = 0;
+        acceleration_desc = 0;
+        acceleration_raw = 0.0;
+        id = 0;
+        w = 0;
+        l = 0;
+        h = 0;
+        t = GENERAL_OBSTACLE;
+        distance_to_center = 0;
+        predicted_behavior = INITIAL_STATE;
+        actual_speed = 0;
+        actual_yaw = 0;
+
+        acceleration_desc = 0;
+        acceleration_raw = 0.0;
+        indicator_state = INDICATOR_NONE;
+
+        originalID = -1;
+        behavior_state = BEH_STOPPING_STATE;
+    }
+};
 
-  HMIPreCalculatedConditions()
-  {
+class PlanningParams {
+  public:
+    double maxSpeed;
+    double minSpeed;
+    double planningDistance;
+    double microPlanDistance;
+    double carTipMargin;
+    double rollInMargin;
+    double rollInSpeedFactor;
+    double pathDensity;
+    double rollOutDensity;
+    int rollOutNumber;
+    int blockIdx;
+    double horizonDistance;
+    double minFollowingDistance; // should be bigger than Distance to follow
+    double minDistanceToAvoid;   // should be smaller than minFollowingDistance and larger than maxDistanceToAvoid
+    double maxDistanceToAvoid;   // should be smaller than minDistanceToAvoid
+    double speedProfileFactor;
+    double smoothingDataWeight;
+    double smoothingSmoothWeight;
+    double smoothingToleranceError;
+
+    // Added by HYP
+    int enableDebug;
+
+    // Added by HYP for blocking the trajectories
+    double lateralBlockingThreshold;
+    double frontLongitudinalBlockingThreshold;
+    double rearLongitudinalBlockingThreshold;
+
+    // Added by HJW and HYP for make traj eval parameter
+    double weightPriority;
+    double weightTransition;
+    double weightLong;
+    double weightLat;
+    double blockThreshold;
+    double LateralSkipDistance;
+
+    // Added by HJW for traffic signal parameter
+    double stopLineMargin;
+    double stopLineDetectionDistance;
+
+    // Added by HJW for handle intersection
+    bool isInsideIntersection;
+    int closestIntersectionID;
+    double closestIntersectionDistance;
+    int riskyAreaIdx;
+    bool obstacleinRiskyArea;
+
+    bool enableSlowDownOnCurve;
+    double curveVelocityRatio;
+
+    // Added by PHY
+    bool pedestrianAppearence;
+    bool turnLeft;
+    bool turnRight;
+
+    double stopSignStopTime;
+
+    double additionalBrakingDistance;
+    double verticalSafetyDistance;
+    double horizontalSafetyDistancel;
+
+    double giveUpDistance;
+
+    int nReliableCount;
+
+    bool enableLaneChange;
+    bool enableSwerving;
+    bool enableFollowing;
+    bool enableHeadingSmoothing;
+    bool enableTrafficLightBehavior;
+    bool enableStopSignBehavior;
+    bool enableStop;
+
+    bool enabTrajectoryVelocities;
+    double minIndicationDistance;
+
+    PlanningParams() {
+        maxSpeed = 3;
+        minSpeed = 0;
+        planningDistance = 10000;
+        microPlanDistance = 30;
+        carTipMargin = 4.0;
+        rollInMargin = 12.0;
+        rollInSpeedFactor = 0.25;
+        pathDensity = 0.25;
+        rollOutDensity = 0.5;
+        rollOutNumber = 4;
+        horizonDistance = 120;
+        minFollowingDistance = 35;
+        minDistanceToAvoid = 15;
+        maxDistanceToAvoid = 5;
+        speedProfileFactor = 1.0;
+        smoothingDataWeight = 0.47;
+        smoothingSmoothWeight = 0.2;
+        smoothingToleranceError = 0.05;
+
+        enableDebug = 0;
+
+        lateralBlockingThreshold = 1.5;
+        frontLongitudinalBlockingThreshold = 30.0;
+        rearLongitudinalBlockingThreshold = -5.0;
+
+        weightPriority = 1;
+        weightTransition = 1;
+        weightLong = 1.2;
+        weightLat = 1;
+        LateralSkipDistance = 5;
+
+        stopSignStopTime = 2.0;
+
+        additionalBrakingDistance = 10.0;
+        verticalSafetyDistance = 0.0;
+        horizontalSafetyDistancel = 0.0;
+
+        giveUpDistance = -4;
+        nReliableCount = 2;
+
+        enableSlowDownOnCurve = false;
+        curveVelocityRatio = 0.5;
+
+        enableHeadingSmoothing = false;
+        enableSwerving = false;
+        enableFollowing = false;
+        enableTrafficLightBehavior = false;
+        enableLaneChange = false;
+        enableStopSignBehavior = false;
+        enabTrajectoryVelocities = false;
+        minIndicationDistance = 15;
+        pedestrianAppearence = false;
+        enableStop = false;
+
+        turnRight = false;
+        turnLeft = false;
+
+        isInsideIntersection = false;
+        closestIntersectionID = -1;
+        riskyAreaIdx = -1;
+        obstacleinRiskyArea = false;
+    }
+};
 
-  }
+class HMIPreCalculatedConditions {
+  public:
+    HMIPreCalculatedConditions() {}
 };
 
-class PreCalculatedConditions
-{
-public:
-  //-------------------------------------------//
-  //Global Goals
-  int         currentGoalID;
-  int         prevGoalID;
-  //-------------------------------------------//
-  //Following
-  double         distanceToNext;
-  double        velocityOfNext;
-  //-------------------------------------------//
-  //For Lane Change
-  int         iPrevSafeLane;
-  int         iCurrSafeLane;
-  double        distanceToGoBack;
-  double         timeToGoBack;
-  double         distanceToChangeLane;
-  double        timeToChangeLane;
-  int         currentLaneID;
-  int         originalLaneID;
-  int         targetLaneID;
-  bool         bUpcomingLeft;
-  bool         bUpcomingRight;
-  bool        bCanChangeLane;
-  bool        bTargetLaneSafe;
-  //-------------------------------------------//
-  //Traffic Lights & Stop Sign
-  int         currentStopSignID;
-  int         prevStopSignID;
-  int         currentTrafficLightID;
-  int         prevTrafficLightID;
-  bool         bTrafficIsRed; //On , off status
-  //-------------------------------------------//
-  //Swerving
-  int         iPrevSafeTrajectory;
-  int         iCurrSafeTrajectory;
-  int         iCentralTrajectory;
-  bool        bFullyBlock;
-  LIGHT_INDICATOR   indicator;
-
-  //-------------------------------------------//
-  //General
-  bool         bNewGlobalPath;
-  bool         bRePlan;
-  double         currentVelocity;
-  double        minStoppingDistance; //comfortably
-  int         bOutsideControl; // 0 waiting, 1 start, 2 Green Traffic Light, 3 Red Traffic Light, 5 Emergency Stop
-  bool        bGreenOutsideControl;
-  std::vector<double> stoppingDistances;
-
-
-  double distanceToStop()
-  {
-    if(stoppingDistances.size()==0) return 0;
-    double minS = stoppingDistances.at(0);
-    for(unsigned int i=0; i< stoppingDistances.size(); i++)
-    {
-      if(stoppingDistances.at(i) < minS)
-        minS = stoppingDistances.at(i);
+class PreCalculatedConditions {
+  public:
+    //-------------------------------------------//
+    // Global Goals
+    int currentGoalID;
+    int prevGoalID;
+    //-------------------------------------------//
+    // Following
+    double distanceToNext;
+    double velocityOfNext;
+    //-------------------------------------------//
+    // For Lane Change
+    int iPrevSafeLane;
+    int iCurrSafeLane;
+    double distanceToGoBack;
+    double timeToGoBack;
+    double distanceToChangeLane;
+    double timeToChangeLane;
+    int currentLaneID;
+    int originalLaneID;
+    int targetLaneID;
+    bool bUpcomingLeft;
+    bool bUpcomingRight;
+    bool bCanChangeLane;
+    bool bTargetLaneSafe;
+    //-------------------------------------------//
+    // Traffic Lights & Stop Sign
+    int currentStopSignID;
+    int prevStopSignID;
+    int currentTrafficLightID;
+    int prevTrafficLightID;
+    bool bTrafficIsRed; // On , off status
+    //-------------------------------------------//
+    // Swerving
+    int iPrevSafeTrajectory;
+    int iCurrSafeTrajectory;
+    int iCentralTrajectory;
+    bool bFullyBlock;
+    LIGHT_INDICATOR indicator;
+
+    //-------------------------------------------//
+    // General
+    bool bNewGlobalPath;
+    bool bRePlan;
+    double currentVelocity;
+    double minStoppingDistance; // comfortably
+    int bOutsideControl;        // 0 waiting, 1 start, 2 Green Traffic Light, 3 Red Traffic Light, 5 Emergency Stop
+    bool bGreenOutsideControl;
+    std::vector<double> stoppingDistances;
+
+    double distanceToStop() {
+        if (stoppingDistances.size() == 0)
+            return 0;
+        double minS = stoppingDistances.at(0);
+        for (unsigned int i = 0; i < stoppingDistances.size(); i++) {
+            if (stoppingDistances.at(i) < minS)
+                minS = stoppingDistances.at(i);
+        }
+        return minS;
     }
-    return minS;
-  }
-
-  PreCalculatedConditions()
-  {
-    currentGoalID       = 0;
-    prevGoalID        = -1;
-    currentVelocity     = 0;
-    minStoppingDistance    = 1;
-    bOutsideControl      = 0;
-    bGreenOutsideControl  = false;
-    //distance to stop
-    distanceToNext      = -1;
-    velocityOfNext      = 0;
-    currentStopSignID    = -1;
-    prevStopSignID      = -1;
-    currentTrafficLightID  = -1;
-    prevTrafficLightID    = -1;
-    bTrafficIsRed      = false;
-    iCurrSafeTrajectory    = -1;
-    bFullyBlock        = false;
-
-    iPrevSafeTrajectory    = -1;
-    iCentralTrajectory    = -1;
-    bRePlan          = false;
-    bNewGlobalPath      = false;
-
-    bCanChangeLane      = false;
-    distanceToGoBack    = 0;
-    timeToGoBack      = 0;
-    distanceToChangeLane  = 0;
-    timeToChangeLane    = 0;
-    bTargetLaneSafe      = true;
-    bUpcomingLeft      = false;
-    bUpcomingRight      = false;
-    targetLaneID      = -1;
-    currentLaneID      = -1;
-    originalLaneID      = -1;
-    iCurrSafeLane       = -1;
-    iPrevSafeLane      = -1;
-
-    indicator         = INDICATOR_NONE;
-  }
-
-  virtual ~PreCalculatedConditions(){}
-
-  std::string ToStringHeader()
-  {
-    return "Time:General>>:currentVelocity:distanceToStop:minStoppingDistance:bStartBehaviorGenerator:bGoalReached:"
-        "Following>>:velocityOfNext:distanceToNext:"
-        "TrafficLight>>:currentTrafficLightID:bTrafficIsRed:"
-        "Swerving>>:iSafeTrajectory:bFullyBlock:";
-  }
-
-  std::string ToString(STATE_TYPE beh)
-  {
-    std::string str = "Unknown";
-    switch(beh)
-    {
-    case PlannerHNS::INITIAL_STATE:
-      str = "Init";
-      break;
-    case PlannerHNS::WAITING_STATE:
-      str = "Waiting";
-      break;
-    case PlannerHNS::FORWARD_STATE:
-      str = "Forward";
-      break;
-    case PlannerHNS::STOPPING_STATE:
-      str = "Stop";
-      break;
-    case PlannerHNS::FINISH_STATE:
-      str = "End";
-      break;
-    case PlannerHNS::FOLLOW_STATE:
-      str = "Follow";
-      break;
-    case PlannerHNS::OBSTACLE_AVOIDANCE_STATE:
-      str = "Swerving";
-      break;
-    case PlannerHNS::TRAFFIC_LIGHT_STOP_STATE:
-      str = "Light Stop";
-      break;
-    case PlannerHNS::TRAFFIC_LIGHT_WAIT_STATE:
-      str = "Light Wait";
-      break;
-    case PlannerHNS::STOP_SIGN_STOP_STATE:
-      str = "Sign Stop";
-      break;
-    case PlannerHNS::STOP_SIGN_WAIT_STATE:
-      str = "Sign Wait";
-      break;
-    default:
-      str = "Unknown";
-      break;
+
+    PreCalculatedConditions() {
+        currentGoalID = 0;
+        prevGoalID = -1;
+        currentVelocity = 0;
+        minStoppingDistance = 1;
+        bOutsideControl = 0;
+        bGreenOutsideControl = false;
+        // distance to stop
+        distanceToNext = -1;
+        velocityOfNext = 0;
+        currentStopSignID = -1;
+        prevStopSignID = -1;
+        currentTrafficLightID = -1;
+        prevTrafficLightID = -1;
+        bTrafficIsRed = false;
+        iCurrSafeTrajectory = -1;
+        bFullyBlock = false;
+
+        iPrevSafeTrajectory = -1;
+        iCentralTrajectory = -1;
+        bRePlan = false;
+        bNewGlobalPath = false;
+
+        bCanChangeLane = false;
+        distanceToGoBack = 0;
+        timeToGoBack = 0;
+        distanceToChangeLane = 0;
+        timeToChangeLane = 0;
+        bTargetLaneSafe = true;
+        bUpcomingLeft = false;
+        bUpcomingRight = false;
+        targetLaneID = -1;
+        currentLaneID = -1;
+        originalLaneID = -1;
+        iCurrSafeLane = -1;
+        iPrevSafeLane = -1;
+
+        indicator = INDICATOR_NONE;
     }
 
-    return str;
-  }
-};
+    virtual ~PreCalculatedConditions() {}
 
-class TrajectoryCost
-{
-public:
-  int index;
-  int relative_index;
-  double closest_obj_velocity;
-  double distance_from_center;
-  double priority_cost; //0 to 1
-  double transition_cost; // 0 to 1
-  double closest_obj_cost; // 0 to 1
-  double cost;
-  double closest_obj_distance;
-
-  int lane_index;
-  double lane_change_cost;
-  double lateral_cost;
-  double longitudinal_cost;
-  bool bBlocked;
-  std::vector<std::pair<int, double> > lateral_costs;
-
-
-  TrajectoryCost()
-  {
-    lane_index = -1;
-    index = -1;
-    relative_index = -100;
-    closest_obj_velocity = 0;
-    priority_cost = 0;
-    transition_cost = 0;
-    closest_obj_cost = 0;
-    distance_from_center = 0;
-    cost = 0;
-    closest_obj_distance = -1;
-    lane_change_cost = 0;
-    lateral_cost = 0;
-    longitudinal_cost = 0;
-    bBlocked = false;
-  }
-
-  std::string ToString()
-  {
-    std::ostringstream str;
-    str.precision(4);
-    str << "LI   : " << lane_index;
-    str << ", In : " << relative_index;
-    str << ", Co : " << cost;
-    str << ", Pr : " << priority_cost;
-    str << ", Tr : " << transition_cost;
-    str << ", La : " << lateral_cost;
-    str << ", Lo : " << longitudinal_cost;
-    str << ", Ln : " << lane_change_cost;
-    str << ", Bl : " << bBlocked;
-    str << "\n";
-    for (unsigned int i=0; i<lateral_costs.size(); i++ )
-    {
-      str << " - (" << lateral_costs.at(i).first << ", " << lateral_costs.at(i).second << ")";
+    std::string ToStringHeader() {
+        return "Time:General>>:currentVelocity:distanceToStop:minStoppingDistance:bStartBehaviorGenerator:bGoalReached:"
+               "Following>>:velocityOfNext:distanceToNext:"
+               "TrafficLight>>:currentTrafficLightID:bTrafficIsRed:"
+               "Swerving>>:iSafeTrajectory:bFullyBlock:";
     }
 
-    return str.str();
+    std::string ToString(STATE_TYPE beh) {
+        std::string str = "Unknown";
+        switch (beh) {
+        case PlannerHNS::INITIAL_STATE:
+            str = "Init";
+            break;
+        case PlannerHNS::WAITING_STATE:
+            str = "Waiting";
+            break;
+        case PlannerHNS::FORWARD_STATE:
+            str = "Forward";
+            break;
+        case PlannerHNS::STOPPING_STATE:
+            str = "Stop";
+            break;
+        case PlannerHNS::FINISH_STATE:
+            str = "End";
+            break;
+        case PlannerHNS::FOLLOW_STATE:
+            str = "Follow";
+            break;
+        case PlannerHNS::OBSTACLE_AVOIDANCE_STATE:
+            str = "Swerving";
+            break;
+        case PlannerHNS::TRAFFIC_LIGHT_STOP_STATE:
+            str = "Light Stop";
+            break;
+        case PlannerHNS::TRAFFIC_LIGHT_WAIT_STATE:
+            str = "Light Wait";
+            break;
+        case PlannerHNS::STOP_SIGN_STOP_STATE:
+            str = "Sign Stop";
+            break;
+        case PlannerHNS::STOP_SIGN_WAIT_STATE:
+            str = "Sign Wait";
+            break;
+        default:
+            str = "Unknown";
+            break;
+        }
 
-  }
+        return str;
+    }
 };
 
-class OccupancyToGridMap
-{
-public:
-  int width;
-  int length;
-  double res;
-  WayPoint center;
-
-  OccupancyToGridMap(const int& _width, const int& _length, const double& _res, const WayPoint& _center)
-  {
-    width = _width;
-    length = _length;
-    res = _res;
-    center = _center;
-  }
-
-  OccupancyToGridMap()
-  {
-    width = 0;
-    length  = 0;
-    res = 0.0;
-  }
-
-  bool GetCellIndexFromPoint(const GPSPoint& p, const std::vector<int>& data, int& _cell)
-  {
-    int col = floor(p.x / res);
-    int row = floor(p.y / res);
-
-    int index = -1;
-    if(row >= 0 && row < length && col >=0 && col < width)
-    {
-      index = get2dIndex(row,col);
-
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        //printf("Cell Info: P(%f,%f) , D(%f,%f), G(%d,%d), index = %d \n", p.x, p.y, p.x-center.pos.x, p.y-center.pos.y, col, row , index);
-        return true;
-      }
+class TrajectoryCost {
+  public:
+    int index;
+    int relative_index;
+    double closest_obj_velocity;
+    double distance_from_center;
+    double priority_cost;    // 0 to 1
+    double transition_cost;  // 0 to 1
+    double closest_obj_cost; // 0 to 1
+    double cost;
+    double closest_obj_distance;
+
+    int lane_index;
+    double lane_change_cost;
+    double lateral_cost;
+    double longitudinal_cost;
+    bool bBlocked;
+    std::vector<std::pair<int, double>> lateral_costs;
+
+    TrajectoryCost() {
+        lane_index = -1;
+        index = -1;
+        relative_index = -100;
+        closest_obj_velocity = 0;
+        priority_cost = 0;
+        transition_cost = 0;
+        closest_obj_cost = 0;
+        distance_from_center = 0;
+        cost = 0;
+        closest_obj_distance = -1;
+        lane_change_cost = 0;
+        lateral_cost = 0;
+        longitudinal_cost = 0;
+        bBlocked = false;
     }
 
-    if(row+1 >= 0 && row+1 < length && col >=0 && col < width)
-    {
-      index = get2dIndex(row+1,col);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
-    }
+    std::string ToString() {
+        std::ostringstream str;
+        str.precision(4);
+        str << "LI   : " << lane_index;
+        str << ", In : " << relative_index;
+        str << ", Co : " << cost;
+        str << ", Pr : " << priority_cost;
+        str << ", Tr : " << transition_cost;
+        str << ", La : " << lateral_cost;
+        str << ", Lo : " << longitudinal_cost;
+        str << ", Ln : " << lane_change_cost;
+        str << ", Bl : " << bBlocked;
+        str << "\n";
+        for (unsigned int i = 0; i < lateral_costs.size(); i++) {
+            str << " - (" << lateral_costs.at(i).first << ", " << lateral_costs.at(i).second << ")";
+        }
 
-    if(row >= 0 && row < length && col+1 >=0 && col+1 < width)
-    {
-      index = get2dIndex(row,col+1);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
+        return str.str();
     }
+};
 
-    if(row-1 >= 0 && row-1 < length && col >=0 && col < width)
-    {
-      index = get2dIndex(row-1,col);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
+class OccupancyToGridMap {
+  public:
+    int width;
+    int length;
+    double res;
+    WayPoint center;
+
+    OccupancyToGridMap(const int &_width, const int &_length, const double &_res, const WayPoint &_center) {
+        width = _width;
+        length = _length;
+        res = _res;
+        center = _center;
     }
 
-    if(row >= 0 && row < length && col-1 >=0 && col-1 < width)
-    {
-      index = get2dIndex(row,col-1);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
+    OccupancyToGridMap() {
+        width = 0;
+        length = 0;
+        res = 0.0;
     }
 
-    if(row+1 >= 0 && row+1 < length && col+1 >=0 && col+1 < width)
-    {
-      index = get2dIndex(row+1,col+1);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
-    }
+    bool GetCellIndexFromPoint(const GPSPoint &p, const std::vector<int> &data, int &_cell) {
+        int col = floor(p.x / res);
+        int row = floor(p.y / res);
 
-    if(row-1 >= 0 && row-1 < length && col-1 >=0 && col-1 < width)
-    {
-      index = get2dIndex(row-1,col-1);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
-    }
+        int index = -1;
+        if (row >= 0 && row < length && col >= 0 && col < width) {
+            index = get2dIndex(row, col);
 
-    if(row-1 >= 0 && row-1 < length && col+1 >=0 && col+1 < width)
-    {
-      index = get2dIndex(row-1,col+1);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
-    }
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                // printf("Cell Info: P(%f,%f) , D(%f,%f), G(%d,%d), index = %d \n", p.x, p.y, p.x-center.pos.x, p.y-center.pos.y, col, row , index);
+                return true;
+            }
+        }
 
-    if(row+1 >= 0 && row+1 < length && col-1 >=0 && col-1 < width)
-    {
-      index = get2dIndex(row+1,col-1);
-      if(index >= 0 && index < (int)data.size())
-      {
-        _cell = data.at((unsigned int)index);
-        return true;
-      }
-    }
+        if (row + 1 >= 0 && row + 1 < length && col >= 0 && col < width) {
+            index = get2dIndex(row + 1, col);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
+
+        if (row >= 0 && row < length && col + 1 >= 0 && col + 1 < width) {
+            index = get2dIndex(row, col + 1);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
+
+        if (row - 1 >= 0 && row - 1 < length && col >= 0 && col < width) {
+            index = get2dIndex(row - 1, col);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
 
-    //printf("Error Getting Cell with Info: P(%f,%f) , C(%d,%d), index = %d \n", p.x, p.y, row, col, index);
-    return false;
-  }
-private:
+        if (row >= 0 && row < length && col - 1 >= 0 && col - 1 < width) {
+            index = get2dIndex(row, col - 1);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
 
-  int get2dIndex(const int& r,const int& c)
-  {
-    return ((r*width) + c);
-  }
+        if (row + 1 >= 0 && row + 1 < length && col + 1 >= 0 && col + 1 < width) {
+            index = get2dIndex(row + 1, col + 1);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
 
-};
+        if (row - 1 >= 0 && row - 1 < length && col - 1 >= 0 && col - 1 < width) {
+            index = get2dIndex(row - 1, col - 1);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
 
-class ParticleInfo
-{
-public:
-  double vel;
-  int acl; //slow down -1 braking , 0 cruising , 1 accelerating
-  PlannerHNS::LIGHT_INDICATOR indicator;
-  PlannerHNS::STATE_TYPE state;
-
-  ParticleInfo()
-  {
-    vel = 0;
-    acl = 0;
-    indicator = PlannerHNS::INDICATOR_NONE;
-    state = PlannerHNS::FORWARD_STATE;
-  }
+        if (row - 1 >= 0 && row - 1 < length && col + 1 >= 0 && col + 1 < width) {
+            index = get2dIndex(row - 1, col + 1);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
+
+        if (row + 1 >= 0 && row + 1 < length && col - 1 >= 0 && col - 1 < width) {
+            index = get2dIndex(row + 1, col - 1);
+            if (index >= 0 && index < (int)data.size()) {
+                _cell = data.at((unsigned int)index);
+                return true;
+            }
+        }
+
+        // printf("Error Getting Cell with Info: P(%f,%f) , C(%d,%d), index = %d \n", p.x, p.y, row, col, index);
+        return false;
+    }
+
+  private:
+    int get2dIndex(const int &r, const int &c) { return ((r * width) + c); }
 };
 
-}
+class ParticleInfo {
+  public:
+    double vel;
+    int acl; // slow down -1 braking , 0 cruising , 1 accelerating
+    PlannerHNS::LIGHT_INDICATOR indicator;
+    PlannerHNS::STATE_TYPE state;
+
+    ParticleInfo() {
+        vel = 0;
+        acl = 0;
+        indicator = PlannerHNS::INDICATOR_NONE;
+        state = PlannerHNS::FORWARD_STATE;
+    }
+};
 
+} // namespace PlannerHNS
 
 #endif /* ROADNETWORK_H_ */
-
diff --git a/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h b/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h
index 05d60262..608d6f91 100755
--- a/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h
+++ b/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h
@@ -4,77 +4,86 @@
 /// \author Hatem Darweesh
 /// \date Jan 14, 2018
 
-
 #ifndef TRAJECTORYDYNAMICCOSTS_H_
 #define TRAJECTORYDYNAMICCOSTS_H_
 
-#include "RoadNetwork.h"
 #include "PlannerCommonDef.h"
 #include "PlanningHelpers.h"
+#include "RoadNetwork.h"
 
 using namespace std;
 
-namespace PlannerHNS
-{
-
-class TrajectoryDynamicCosts
-{
-public:
-  TrajectoryDynamicCosts();
-  virtual ~TrajectoryDynamicCosts();
-
-  TrajectoryCost DoOneStep(const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths,
-      const WayPoint& currState, const int& currTrajectoryIndex, const int& currLaneIndex, const PlanningParams& params,
-      const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const std::vector<PlannerHNS::DetectedObject>& obj_list);
-
-  TrajectoryCost DoOneStepStatic(const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
-      const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
-      const std::vector<PlannerHNS::DetectedObject>& obj_list, const PlannerHNS::STATE_TYPE& state, const int& iCurrentIndex = -1);
-
-  TrajectoryCost DoOneStepDynamic(const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
-      const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
-      const std::vector<PlannerHNS::DetectedObject>& obj_list, const int& iCurrentIndex = -1);
-
-public:
-  int m_PrevCostIndex;
-  int m_PrevIndex;
-  int m_PrevSelectedIndex;
-  vector<TrajectoryCost> m_TrajectoryCosts;
-  PlanningParams m_Params;
-  PolygonShape m_SafetyBorder;
-  vector<WayPoint> m_AllContourPoints;
-  vector<WayPoint> m_CollisionPoints;
-  double m_WeightPriority;
-  double m_WeightTransition;
-  double m_WeightLong;
-  double m_WeightLat;
-  double m_WeightLaneChange;
-  double m_LateralSkipDistance;
-  double m_CollisionTimeDiff;
-
-  int m_startTrajIdx;
-  int m_endTrajIdx;
-
-private:
-  bool ValidateRollOutsInput(const vector<vector<vector<WayPoint> > >& rollOuts);
-  vector<TrajectoryCost> CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint> >& laneRollOuts, const int& lane_index, const PlanningParams& params);
-  void NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts);
-  void CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts, const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths, const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState);
-  void CalculateLateralAndLongitudinalCostsStatic(vector<TrajectoryCost>& trajectoryCosts, const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths, const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState);
-  void CalculateTransitionCosts(vector<TrajectoryCost>& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params);
-  
-  void CalculateIntersectionVelocities(const std::vector<WayPoint>& path, const DetectedObject& obj, const WayPoint& currPose, const CAR_BASIC_INFO& carInfo, const double& c_lateral_d, WayPoint& collisionPoint, TrajectoryCost& trajectoryCosts);
-  int GetCurrentRollOutIndex(const std::vector<WayPoint>& path, const WayPoint& currState, const PlanningParams& params);
-  void InitializeCosts(const vector<vector<WayPoint> >& rollOuts, const PlanningParams& params);
-  void InitializeSafetyPolygon(const WayPoint& currState, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d);
-  void CalculateLateralAndLongitudinalCostsDynamic(const std::vector<PlannerHNS::DetectedObject>& obj_list, const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
-      const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo,
-      const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d );
-
-  double CalculateTurnAngle(const std::vector<WayPoint>& path, const WayPoint& currState, int distance);
-
+namespace PlannerHNS {
+
+class TrajectoryDynamicCosts {
+  public:
+    TrajectoryDynamicCosts();
+    virtual ~TrajectoryDynamicCosts();
+
+    TrajectoryCost DoOneStep(const vector<vector<vector<WayPoint>>> &rollOuts, const vector<vector<WayPoint>> &totalPaths, const WayPoint &currState,
+                             const int &currTrajectoryIndex, const int &currLaneIndex, PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                             const VehicleState &vehicleState, const std::vector<PlannerHNS::DetectedObject> &obj_list);
+
+    TrajectoryCost DoOneStepStatic(const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths, const WayPoint &currState,
+                                   PlanningParams &params, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
+                                   const std::vector<PlannerHNS::DetectedObject> &obj_list, const PlannerHNS::STATE_TYPE &state,
+                                   const int &iCurrentIndex = -1);
+
+    TrajectoryCost DoOneStepDynamic(const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths, const WayPoint &currState,
+                                    PlanningParams &params, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
+                                    const std::vector<PlannerHNS::DetectedObject> &obj_list, const int &iCurrentIndex = -1);
+
+  public:
+    int m_PrevCostIndex;
+    int m_PrevIndex;
+    int m_PrevSelectedIndex;
+    vector<TrajectoryCost> m_TrajectoryCosts;
+    PlanningParams m_Params;
+    PolygonShape m_SafetyBorder;
+    vector<WayPoint> m_AllContourPoints;
+    vector<WayPoint> m_CollisionPoints;
+    double m_WeightPriority;
+    double m_WeightTransition;
+    double m_WeightLong;
+    double m_WeightLat;
+    double m_WeightLaneChange;
+    double m_LateralSkipDistance;
+    double m_CollisionTimeDiff;
+
+    int m_startTrajIdx;
+    int m_endTrajIdx;
+
+  private:
+    bool ValidateRollOutsInput(const vector<vector<vector<WayPoint>>> &rollOuts);
+    vector<TrajectoryCost> CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint>> &laneRollOuts, const int &lane_index,
+                                                               PlanningParams &params);
+    void NormalizeCosts(vector<TrajectoryCost> &trajectoryCosts, int enableDebug);
+    void CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost> &trajectoryCosts, const vector<vector<vector<WayPoint>>> &rollOuts,
+                                              const vector<vector<WayPoint>> &totalPaths, const WayPoint &currState,
+                                              const vector<WayPoint> &contourPoints, PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                                              const VehicleState &vehicleState);
+    void CalculateLateralAndLongitudinalCostsStatic(vector<TrajectoryCost> &trajectoryCosts, const vector<vector<WayPoint>> &rollOuts,
+                                                    const vector<WayPoint> &totalPaths, const WayPoint &currState,
+                                                    const vector<WayPoint> &contourPoints, PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                                                    const VehicleState &vehicleState);
+    void CalculateTransitionCosts(vector<TrajectoryCost> &trajectoryCosts, const int &currTrajectoryIndex, PlanningParams &params);
+
+    void CalculateIntersectionVelocities(const std::vector<WayPoint> &path, const DetectedObject &obj, const WayPoint &currPose,
+                                         const CAR_BASIC_INFO &carInfo, const double &c_lateral_d, WayPoint &collisionPoint,
+                                         TrajectoryCost &trajectoryCosts);
+    int GetCurrentRollOutIndex(const std::vector<WayPoint> &path, const WayPoint &currState, PlanningParams &params);
+    void InitializeCosts(const vector<vector<WayPoint>> &rollOuts, PlanningParams &params);
+    void InitializeSafetyPolygon(const WayPoint &currState, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
+                                 const double &c_lateral_d, const double &c_long_front_d, const double &c_long_back_d);
+    void CalculateLateralAndLongitudinalCostsDynamic(const std::vector<PlannerHNS::DetectedObject> &obj_list,
+                                                     const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths,
+                                                     const WayPoint &currState, PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                                                     const VehicleState &vehicleState, const double &c_lateral_d, const double &c_long_front_d,
+                                                     const double &c_long_back_d);
+
+    double CalculateTurnAngle(const std::vector<WayPoint> &path, const WayPoint &currState, int distance);
 };
 
-}
+} // namespace PlannerHNS
 
 #endif /* TRAJECTORYDYNAMICCOSTS_H_ */
diff --git a/autoware.ai/src/autoware/common/op_planner/package.xml b/autoware.ai/src/autoware/common/op_planner/package.xml
index 29bb890f..9bc3f29c 100644
--- a/autoware.ai/src/autoware/common/op_planner/package.xml
+++ b/autoware.ai/src/autoware/common/op_planner/package.xml
@@ -7,7 +7,6 @@
   <author email="hatem.darweesh@g.sp.m.is.nagoya-u.ac.jp">Hatem Darweesh</author>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>cmake_modules</depend>
diff --git a/autoware.ai/src/autoware/common/op_planner/src/BehaviorStateMachine.cpp b/autoware.ai/src/autoware/common/op_planner/src/BehaviorStateMachine.cpp
index 3b3b5d02..da033a72 100755
--- a/autoware.ai/src/autoware/common/op_planner/src/BehaviorStateMachine.cpp
+++ b/autoware.ai/src/autoware/common/op_planner/src/BehaviorStateMachine.cpp
@@ -10,551 +10,463 @@
 
 using namespace UtilityHNS;
 
+namespace PlannerHNS {
 
-namespace PlannerHNS
-{
+BehaviorStateMachine::BehaviorStateMachine(PlanningParams *pParams, PreCalculatedConditions *pPreCalcVal, BehaviorStateMachine *nextState) {
+    m_Behavior = INITIAL_STATE;
 
-BehaviorStateMachine::BehaviorStateMachine(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* nextState)
-{
-  m_Behavior = INITIAL_STATE;
+    m_currentStopSignID = -1;
+    m_currentTrafficLightID = -1;
+    decisionMakingTime = 0.0;
+    decisionMakingCount = 1;
+    m_zero_velocity = 0.1;
 
-  m_currentStopSignID    = -1;
-  m_currentTrafficLightID  = -1;
-  decisionMakingTime    = 0.0;
-  decisionMakingCount    = 1;
-  m_zero_velocity     = 0.1;
+    if (!pPreCalcVal)
+        m_pCalculatedValues = new PreCalculatedConditions();
+    else
+        m_pCalculatedValues = pPreCalcVal;
 
-  if(!pPreCalcVal)
-    m_pCalculatedValues = new PreCalculatedConditions();
-  else
-    m_pCalculatedValues = pPreCalcVal;
+    if (!pParams)
+        m_pParams = new PlanningParams;
+    else
+        m_pParams = pParams;
 
-  if(!pParams)
-    m_pParams = new PlanningParams;
-  else
-    m_pParams = pParams;
+    if (nextState)
+        pNextStates.push_back(nextState);
 
-  if(nextState)
-    pNextStates.push_back(nextState);
+    pNextStates.push_back(this);
 
-  pNextStates.push_back(this);
-
-  Init();
+    Init();
 }
 
-void BehaviorStateMachine::InsertNextState(BehaviorStateMachine* nextState)
-{
-  if(nextState)
-    pNextStates.push_back(nextState);
+void BehaviorStateMachine::InsertNextState(BehaviorStateMachine *nextState) {
+    if (nextState)
+        pNextStates.push_back(nextState);
 }
 
-void BehaviorStateMachine::UpdateLogCount(BehaviorStateMachine* pState)
-{
-  if(!pState) return;
-
-  bool bFound = false;
-  for(unsigned int i = 0; i < m_BehaviorsLog.size(); i++)
-  {
-    if(m_BehaviorsLog.at(i).first->m_Behavior == pState->m_Behavior)
-    {
-      m_BehaviorsLog.at(i).second++;
-      bFound = true;
-      break;
+void BehaviorStateMachine::UpdateLogCount(BehaviorStateMachine *pState) {
+    if (!pState)
+        return;
+
+    bool bFound = false;
+    for (unsigned int i = 0; i < m_BehaviorsLog.size(); i++) {
+        if (m_BehaviorsLog.at(i).first->m_Behavior == pState->m_Behavior) {
+            m_BehaviorsLog.at(i).second++;
+            bFound = true;
+            break;
+        }
     }
-  }
 
-  if(!bFound)
-  {
-    m_BehaviorsLog.push_back(std::make_pair(pState, 1));
-  }
+    if (!bFound) {
+        m_BehaviorsLog.push_back(std::make_pair(pState, 1));
+    }
 }
 
-BehaviorStateMachine* BehaviorStateMachine::FindBestState(int nMinCount)
-{
-  for(unsigned int i = 0; i < m_BehaviorsLog.size(); i++)
-  {
-    if(m_BehaviorsLog.at(i).second >= nMinCount)
-    {
-      //std::cout << "Found Next Beh: " << m_BehaviorsLog.at(i).first->m_Behavior << ", Count: " << m_BehaviorsLog.at(i).second  << ", LogSize: " << m_BehaviorsLog.size() << std::endl;
-      return m_BehaviorsLog.at(i).first;
+BehaviorStateMachine *BehaviorStateMachine::FindBestState(int nMinCount) {
+    for (unsigned int i = 0; i < m_BehaviorsLog.size(); i++) {
+        if (m_BehaviorsLog.at(i).second >= nMinCount) {
+            // std::cout << "Found Next Beh: " << m_BehaviorsLog.at(i).first->m_Behavior << ", Count: " << m_BehaviorsLog.at(i).second  << ", LogSize:
+            // " << m_BehaviorsLog.size() << std::endl;
+            return m_BehaviorsLog.at(i).first;
+        }
     }
-  }
 
-  return nullptr;
+    return nullptr;
 }
 
-BehaviorStateMachine* BehaviorStateMachine::FindBehaviorState(const STATE_TYPE& behavior)
-{
-  for(unsigned int i = 0 ; i < pNextStates.size(); i++)
-  {
-    BehaviorStateMachine* pState = pNextStates.at(i);
-    if(pState && behavior == pState->m_Behavior )
-    {
-      //UpdateLogCount(pState);
-      //pState = FindBestState(decisionMakingCount);
-
-      if(pState == 0) return this;
-
-      m_BehaviorsLog.clear();
-      pState->ResetTimer();
-      return pState;
+BehaviorStateMachine *BehaviorStateMachine::FindBehaviorState(const STATE_TYPE &behavior) {
+    for (unsigned int i = 0; i < pNextStates.size(); i++) {
+        BehaviorStateMachine *pState = pNextStates.at(i);
+        if (pState && behavior == pState->m_Behavior) {
+            // UpdateLogCount(pState);
+            // pState = FindBestState(decisionMakingCount);
+
+            if (pState == 0)
+                return this;
+
+            m_BehaviorsLog.clear();
+            pState->ResetTimer();
+            return pState;
+        }
     }
-  }
-  return nullptr;
+    return nullptr;
 }
 
-void BehaviorStateMachine::Init()
-{
-  UtilityH::GetTickCount(m_StateTimer);
-}
+void BehaviorStateMachine::Init() { UtilityH::GetTickCount(m_StateTimer); }
 
-void BehaviorStateMachine::ResetTimer()
-{
-  UtilityH::GetTickCount(m_StateTimer);
-}
+void BehaviorStateMachine::ResetTimer() { UtilityH::GetTickCount(m_StateTimer); }
 
-BehaviorStateMachine::~BehaviorStateMachine()
-{
-}
+BehaviorStateMachine::~BehaviorStateMachine() {}
 
-BehaviorStateMachine* ForwardState::GetNextState()
-{
-  //  std::cout<<m_pParams->pedestrianAppearence<<std::endl;
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this; //return this behavior only , without reset
-
-  PreCalculatedConditions* pCParams = GetCalcParams();
-
-  if(pCParams->currentGoalID != pCParams->prevGoalID)
-    return FindBehaviorState(GOAL_STATE);
-
-  else if(m_pParams->enableSwerving
-      && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid
-      && !pCParams->bFullyBlock
-      && (pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory || pCParams->iCurrSafeLane != pCParams->iPrevSafeLane)
-      )
-    return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
-
-  else if(m_pParams->enableTrafficLightBehavior
-      && pCParams->currentTrafficLightID > 0
-      && pCParams->bTrafficIsRed
-      && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
-    return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
-
-  else if(m_pParams->enableStopSignBehavior
-      && pCParams->currentStopSignID > 0
-      && pCParams->currentStopSignID != pCParams->prevStopSignID)
-      return FindBehaviorState(STOP_SIGN_STOP_STATE);
-
-  else if(m_pParams->enableFollowing
-      && pCParams->bFullyBlock)
-      return FindBehaviorState(FOLLOW_STATE);
-
-//  else if(pCParams->distanceToNext <= m_pParams->maxDistanceToAvoid)
-//    return FindBehaviorState(STOPPING_STATE);
-
-  else
-  {
-    if(pCParams->iCurrSafeTrajectory == pCParams->iCentralTrajectory && pCParams->iPrevSafeTrajectory != pCParams->iCurrSafeTrajectory){
-      pCParams->bRePlan = true;
-    }
+BehaviorStateMachine *ForwardState::GetNextState() {
+    //  std::cout<<m_pParams->pedestrianAppearence<<std::endl;
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this; // return this behavior only , without reset
 
-    return FindBehaviorState(this->m_Behavior); // return and reset
-  }
-}
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-BehaviorStateMachine* MissionAccomplishedState::GetNextState()
-{
-  return FindBehaviorState(this->m_Behavior); // return and reset
-}
+    if (pCParams->currentGoalID != pCParams->prevGoalID)
+        return FindBehaviorState(GOAL_STATE);
 
-BehaviorStateMachine* StopState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+    else if (m_pParams->enableSwerving && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid && !pCParams->bFullyBlock &&
+             (pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory || pCParams->iCurrSafeLane != pCParams->iPrevSafeLane))
+        return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    else if (m_pParams->enableTrafficLightBehavior && pCParams->currentTrafficLightID > 0 && pCParams->bTrafficIsRed &&
+             pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
+        return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
 
-  if(pCParams->distanceToNext > m_pParams->maxDistanceToAvoid)
-    return FindBehaviorState(FORWARD_STATE);
+    else if (m_pParams->enableStopSignBehavior && pCParams->currentStopSignID > 0 && pCParams->currentStopSignID != pCParams->prevStopSignID)
+        return FindBehaviorState(STOP_SIGN_STOP_STATE);
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
-}
+    else if (m_pParams->enableFollowing && pCParams->bFullyBlock)
+        return FindBehaviorState(FOLLOW_STATE);
 
-BehaviorStateMachine* TrafficLightStopState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+    //  else if(pCParams->distanceToNext <= m_pParams->maxDistanceToAvoid)
+    //    return FindBehaviorState(STOPPING_STATE);
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    else {
+        if (pCParams->iCurrSafeTrajectory == pCParams->iCentralTrajectory && pCParams->iPrevSafeTrajectory != pCParams->iCurrSafeTrajectory) {
+            pCParams->bRePlan = true;
+        }
 
-  if(!pCParams->bTrafficIsRed)
-  {
-    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
-    return FindBehaviorState(FORWARD_STATE);
-  }
+        return FindBehaviorState(this->m_Behavior); // return and reset
+    }
+}
 
-  else if(pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity)
-      return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE);
-  else
+BehaviorStateMachine *MissionAccomplishedState::GetNextState() {
     return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* TrafficLightWaitState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+BehaviorStateMachine *StopState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  if(!pCParams->bTrafficIsRed)
-  {
-    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
-    return FindBehaviorState(FORWARD_STATE);
-  }
+    if (pCParams->distanceToNext > m_pParams->maxDistanceToAvoid)
+        return FindBehaviorState(FORWARD_STATE);
 
-  else if(pCParams->currentVelocity > m_zero_velocity)
-    return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
+}
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+BehaviorStateMachine *TrafficLightStopState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
+
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
+    if (!pCParams->bTrafficIsRed) {
+        pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+        return FindBehaviorState(FORWARD_STATE);
+    }
+
+    else if (pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity)
+        return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE);
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* StopSignStopState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+BehaviorStateMachine *TrafficLightWaitState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  //std::cout << "From Stop Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " << pCParams->currentStopSignID << std::endl;
+    if (!pCParams->bTrafficIsRed) {
+        pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+        return FindBehaviorState(FORWARD_STATE);
+    }
 
-  if(pCParams->currentVelocity < m_zero_velocity)
-    return FindBehaviorState(STOP_SIGN_WAIT_STATE);
+    else if (pCParams->currentVelocity > m_zero_velocity)
+        return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* StopSignWaitState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+BehaviorStateMachine *StopSignStopState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  //std::cout << "From Wait Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " << pCParams->currentStopSignID << std::endl;
+    // std::cout << "From Stop Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " <<
+    // pCParams->currentStopSignID << std::endl;
 
-  pCParams->prevStopSignID = pCParams->currentStopSignID;
+    if (pCParams->currentVelocity < m_zero_velocity)
+        return FindBehaviorState(STOP_SIGN_WAIT_STATE);
 
-  return FindBehaviorState(FORWARD_STATE);
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* WaitState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+BehaviorStateMachine *StopSignWaitState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  //PreCalculatedConditions* pCParams = GetCalcParams();
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  return FindBehaviorState(FORWARD_STATE);
+    // std::cout << "From Wait Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " <<
+    // pCParams->currentStopSignID << std::endl;
+
+    pCParams->prevStopSignID = pCParams->currentStopSignID;
+
+    return FindBehaviorState(FORWARD_STATE);
 }
 
-BehaviorStateMachine* InitState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+BehaviorStateMachine *WaitState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    // PreCalculatedConditions* pCParams = GetCalcParams();
 
-  if(pCParams->bOutsideControl == 1)
-  {
-    pCParams->prevGoalID = pCParams->currentGoalID;
     return FindBehaviorState(FORWARD_STATE);
-  }
+}
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+BehaviorStateMachine *InitState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
+
+    PreCalculatedConditions *pCParams = GetCalcParams();
+
+    if (pCParams->bOutsideControl == 1) {
+        pCParams->prevGoalID = pCParams->currentGoalID;
+        return FindBehaviorState(FORWARD_STATE);
+    }
+
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* FollowState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-      return this;
+BehaviorStateMachine *FollowState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  //std::cout << "Following State >> followDistance: " << pCParams->distanceToNext << ", followSpeed: " << pCParams->velocityOfNext << std::endl;
+    // std::cout << "Following State >> followDistance: " << pCParams->distanceToNext << ", followSpeed: " << pCParams->velocityOfNext << std::endl;
 
-  if(m_pParams->enableTrafficLightBehavior
-        && pCParams->currentTrafficLightID > 0
-        && pCParams->bTrafficIsRed
-        && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
-      return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+    if (m_pParams->enableTrafficLightBehavior && pCParams->currentTrafficLightID > 0 && pCParams->bTrafficIsRed &&
+        pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
+        return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
 
-  else if(m_pParams->enableStopSignBehavior
-      && pCParams->currentStopSignID > 0
-      && pCParams->currentStopSignID != pCParams->prevStopSignID)
-      return FindBehaviorState(STOP_SIGN_STOP_STATE);
+    else if (m_pParams->enableStopSignBehavior && pCParams->currentStopSignID > 0 && pCParams->currentStopSignID != pCParams->prevStopSignID)
+        return FindBehaviorState(STOP_SIGN_STOP_STATE);
 
-  else if(pCParams->currentGoalID != pCParams->prevGoalID
-      || !pCParams->bFullyBlock)
-    return FindBehaviorState(FORWARD_STATE);
+    else if (pCParams->currentGoalID != pCParams->prevGoalID || !pCParams->bFullyBlock)
+        return FindBehaviorState(FORWARD_STATE);
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* SwerveState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+BehaviorStateMachine *SwerveState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  if(pCParams->distanceToNext > 0
-        && pCParams->distanceToNext < m_pParams->minDistanceToAvoid
-        && !pCParams->bFullyBlock
-        && pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
-    return FindBehaviorState(this->m_Behavior);
+    if (pCParams->distanceToNext > 0 && pCParams->distanceToNext < m_pParams->minDistanceToAvoid && !pCParams->bFullyBlock &&
+        pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
+        return FindBehaviorState(this->m_Behavior);
 
-  else
-    return FindBehaviorState(FORWARD_STATE);
+    else
+        return FindBehaviorState(FORWARD_STATE);
 }
 
-BehaviorStateMachine* GoalState::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
+BehaviorStateMachine *GoalState::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
 
-  PreCalculatedConditions* pCParams = GetCalcParams();
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  if(pCParams->currentGoalID == -1)
-    return FindBehaviorState(FINISH_STATE);
+    if (pCParams->currentGoalID == -1)
+        return FindBehaviorState(FINISH_STATE);
 
-  else if(pCParams->currentGoalID != pCParams->prevGoalID)
-  {
-    pCParams->prevGoalID = pCParams->currentGoalID;
-    return FindBehaviorState(FORWARD_STATE);
-  }
+    else if (pCParams->currentGoalID != pCParams->prevGoalID) {
+        pCParams->prevGoalID = pCParams->currentGoalID;
+        return FindBehaviorState(FORWARD_STATE);
+    }
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* ForwardStateII::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
-
-  if(pCParams->currentGoalID != pCParams->prevGoalID)
-    return FindBehaviorState(GOAL_STATE);
-  else if(m_pParams->pedestrianAppearence){
-    return FindBehaviorState(PEDESTRIAN_STATE);
-  }
-  // hjw added
-  else if(m_pParams->enableTrafficLightBehavior
-        && pCParams->currentTrafficLightID > 0
-        && pCParams->bTrafficIsRed)
+BehaviorStateMachine *ForwardStateII::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
+
+    if (pCParams->currentGoalID != pCParams->prevGoalID)
+        return FindBehaviorState(GOAL_STATE);
+    // else if (m_pParams->pedestrianAppearence) {
+    //     return FindBehaviorState(PEDESTRIAN_STATE);
+    // }
+    // hjw added
+    else if (m_pParams->enableTrafficLightBehavior && pCParams->currentTrafficLightID > 0 && pCParams->bTrafficIsRed)
         // && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
-      return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
-  else if(m_pParams->isInsideIntersection
-    && (m_pParams->turnLeft || m_pParams->turnRight)){
-    return FindBehaviorState(INTERSECTION_STATE);
-  }
-  else if(m_pParams->enableStopSignBehavior
-      && pCParams->currentStopSignID > 0
-      && pCParams->currentStopSignID != pCParams->prevStopSignID)
-    return FindBehaviorState(STOP_SIGN_STOP_STATE);
-
-  // else if(m_pParams->enableFollowing && pCParams->bFullyBlock)
-  //   return FindBehaviorState(FOLLOW_STATE);
-
-  else if(m_pParams->enableSwerving
-      && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid
-      && !pCParams->bFullyBlock
-      && pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
-    return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
-
-  else
-    return FindBehaviorState(this->m_Behavior);
+        return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+    else if (m_pParams->isInsideIntersection && (m_pParams->turnLeft || m_pParams->turnRight)) {
+        return FindBehaviorState(INTERSECTION_STATE);
+    } else if (m_pParams->enableStopSignBehavior && pCParams->currentStopSignID > 0 && pCParams->currentStopSignID != pCParams->prevStopSignID)
+        return FindBehaviorState(STOP_SIGN_STOP_STATE);
+
+    // else if(m_pParams->enableFollowing && pCParams->bFullyBlock)
+    //   return FindBehaviorState(FOLLOW_STATE);
+
+    else if (m_pParams->enableSwerving && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid && !pCParams->bFullyBlock &&
+             pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
+        return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
+
+    else
+        return FindBehaviorState(this->m_Behavior);
 }
 
-BehaviorStateMachine* FollowStateII::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
-
-  if(pCParams->currentGoalID != pCParams->prevGoalID)
-    return FindBehaviorState(GOAL_STATE);
-  else if(m_pParams->pedestrianAppearence)
-    return FindBehaviorState(PEDESTRIAN_STATE);
-  else if(m_pParams->enableTrafficLightBehavior
-        && pCParams->currentTrafficLightID > 0
-        && pCParams->bTrafficIsRed)
+BehaviorStateMachine *FollowStateII::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
+
+    if (pCParams->currentGoalID != pCParams->prevGoalID)
+        return FindBehaviorState(GOAL_STATE);
+    else if (m_pParams->pedestrianAppearence)
+        return FindBehaviorState(PEDESTRIAN_STATE);
+    else if (m_pParams->enableTrafficLightBehavior && pCParams->currentTrafficLightID > 0 && pCParams->bTrafficIsRed)
         // && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
-      return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+        return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
 
-  else if(m_pParams->enableStopSignBehavior
-      && pCParams->currentStopSignID > 0
-      && pCParams->currentStopSignID != pCParams->prevStopSignID)
-    return FindBehaviorState(STOP_SIGN_STOP_STATE);
+    else if (m_pParams->enableStopSignBehavior && pCParams->currentStopSignID > 0 && pCParams->currentStopSignID != pCParams->prevStopSignID)
+        return FindBehaviorState(STOP_SIGN_STOP_STATE);
 
-  else if(m_pParams->enableSwerving
-      && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid
-      && !pCParams->bFullyBlock
-      && pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
-    return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
+    else if (m_pParams->enableSwerving && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid && !pCParams->bFullyBlock &&
+             pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
+        return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
 
-  else if(!pCParams->bFullyBlock)
-    return FindBehaviorState(FORWARD_STATE);
+    else if (!pCParams->bFullyBlock)
+        return FindBehaviorState(FORWARD_STATE);
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* SwerveStateII::GetNextState()
-{
-  if(m_pParams->pedestrianAppearence)
-    return FindBehaviorState(PEDESTRIAN_STATE);
-  PreCalculatedConditions* pCParams = GetCalcParams();
+BehaviorStateMachine *SwerveStateII::GetNextState() {
+    if (m_pParams->pedestrianAppearence)
+        return FindBehaviorState(PEDESTRIAN_STATE);
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  pCParams->iPrevSafeTrajectory = pCParams->iCurrSafeTrajectory;
-  pCParams->bRePlan = true;
+    pCParams->iPrevSafeTrajectory = pCParams->iCurrSafeTrajectory;
+    pCParams->bRePlan = true;
 
-  // if(pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
-  //   return FindBehaviorState(FORWARD_STATE);
-  // else
-  //   return FindBehaviorState(this->m_Behavior);
+    // if(pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
+    //   return FindBehaviorState(FORWARD_STATE);
+    // else
+    //   return FindBehaviorState(this->m_Behavior);
 
-  return FindBehaviorState(FORWARD_STATE);
+    return FindBehaviorState(FORWARD_STATE);
 }
 
-BehaviorStateMachine* InitStateII::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
+BehaviorStateMachine *InitStateII::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  if(pCParams->currentGoalID > 0)
-    return FindBehaviorState(FORWARD_STATE);
-  else
-    return FindBehaviorState(this->m_Behavior);
+    if (pCParams->currentGoalID > 0)
+        return FindBehaviorState(FORWARD_STATE);
+    else
+        return FindBehaviorState(this->m_Behavior);
 }
 
-BehaviorStateMachine* GoalStateII::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
+BehaviorStateMachine *GoalStateII::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  if(pCParams->currentGoalID == -1)
-    return FindBehaviorState(FINISH_STATE);
+    if (pCParams->currentGoalID == -1)
+        return FindBehaviorState(FINISH_STATE);
 
-  else
-  {
-    pCParams->prevGoalID = pCParams->currentGoalID;
-    return FindBehaviorState(FORWARD_STATE);
-  }
+    else {
+        pCParams->prevGoalID = pCParams->currentGoalID;
+        return FindBehaviorState(FORWARD_STATE);
+    }
 }
 
-BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
-{
-  return FindBehaviorState(this->m_Behavior);
-}
+BehaviorStateMachine *MissionAccomplishedStateII::GetNextState() { return FindBehaviorState(this->m_Behavior); }
 
-BehaviorStateMachine* StopSignStopStateII::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
-  
-  if(pCParams->currentGoalID != pCParams->prevGoalID)
-    return FindBehaviorState(GOAL_STATE);
-  else if(m_pParams->pedestrianAppearence)
-    return FindBehaviorState(PEDESTRIAN_STATE);
-  else if(pCParams->currentVelocity < m_zero_velocity)
-    return FindBehaviorState(STOP_SIGN_WAIT_STATE);
-
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+BehaviorStateMachine *StopSignStopStateII::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
+
+    if (pCParams->currentGoalID != pCParams->prevGoalID)
+        return FindBehaviorState(GOAL_STATE);
+    else if (m_pParams->pedestrianAppearence)
+        return FindBehaviorState(PEDESTRIAN_STATE);
+    else if (pCParams->currentVelocity < m_zero_velocity)
+        return FindBehaviorState(STOP_SIGN_WAIT_STATE);
+
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* StopSignWaitStateII::GetNextState()
-{
-  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
-    return this;
-  else if(m_pParams->pedestrianAppearence)
-    return FindBehaviorState(PEDESTRIAN_STATE);
-  PreCalculatedConditions* pCParams = GetCalcParams();
+BehaviorStateMachine *StopSignWaitStateII::GetNextState() {
+    if (UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+        return this;
+    else if (m_pParams->pedestrianAppearence)
+        return FindBehaviorState(PEDESTRIAN_STATE);
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-  pCParams->prevStopSignID = pCParams->currentStopSignID;
+    pCParams->prevStopSignID = pCParams->currentStopSignID;
 
-  return FindBehaviorState(FORWARD_STATE);
+    return FindBehaviorState(FORWARD_STATE);
 }
 
-BehaviorStateMachine* TrafficLightStopStateII::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
-  if(m_pParams->pedestrianAppearence)
-    return FindBehaviorState(PEDESTRIAN_STATE);
-  //std::cout << "Stopping for trafficLight "  << std::endl;
-  else if(!pCParams->bTrafficIsRed)
-  {
-    //std::cout << "Color Changed Stopping for trafficLight "  << std::endl;
-    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
-    return FindBehaviorState(FORWARD_STATE);
-  }
+BehaviorStateMachine *TrafficLightStopStateII::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
+    if (m_pParams->pedestrianAppearence)
+        return FindBehaviorState(PEDESTRIAN_STATE);
+    // std::cout << "Stopping for trafficLight "  << std::endl;
+    else if (!pCParams->bTrafficIsRed) {
+        // std::cout << "Color Changed Stopping for trafficLight "  << std::endl;
+        pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+        return FindBehaviorState(FORWARD_STATE);
+    }
 
-  else if(pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity)
-  {
-    //std::cout << "Velocity Changed Stopping for trafficLight ("  <<pCParams->currentVelocity << ", " << m_zero_velocity << ")" <<  std::endl;
-    return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE);
-  }
+    else if (pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity) {
+        // std::cout << "Velocity Changed Stopping for trafficLight ("  <<pCParams->currentVelocity << ", " << m_zero_velocity << ")" <<  std::endl;
+        return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE);
+    }
 
-  else
-  {
-    return FindBehaviorState(this->m_Behavior); // return and reset
-  }
+    else {
+        return FindBehaviorState(this->m_Behavior); // return and reset
+    }
 }
 
-BehaviorStateMachine* TrafficLightWaitStateII::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
-
-  //std::cout << "Wait for trafficLight "  << std::endl;
-  if(m_pParams->pedestrianAppearence)
-    return FindBehaviorState(PEDESTRIAN_STATE);
-  else if(!pCParams->bTrafficIsRed)
-  {
-    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
-    return FindBehaviorState(FORWARD_STATE);
-  }
+BehaviorStateMachine *TrafficLightWaitStateII::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
 
-//  else if(pCParams->currentVelocity > m_zero_velocity)
-//    return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+    // std::cout << "Wait for trafficLight "  << std::endl;
+    if (m_pParams->pedestrianAppearence)
+        return FindBehaviorState(PEDESTRIAN_STATE);
+    else if (!pCParams->bTrafficIsRed) {
+        pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+        return FindBehaviorState(FORWARD_STATE);
+    }
 
-  else
-    return FindBehaviorState(this->m_Behavior); // return and reset
+    //  else if(pCParams->currentVelocity > m_zero_velocity)
+    //    return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
 
+    else
+        return FindBehaviorState(this->m_Behavior); // return and reset
 }
 
-BehaviorStateMachine* PedestrianState::GetNextState()
-{
-  if(m_pParams->pedestrianAppearence){
-    return FindBehaviorState(this->m_Behavior);
-  }
-  else
-    return FindBehaviorState(FORWARD_STATE);
+BehaviorStateMachine *PedestrianState::GetNextState() {
+    if (m_pParams->pedestrianAppearence) {
+        return FindBehaviorState(this->m_Behavior);
+    } else
+        return FindBehaviorState(FORWARD_STATE);
 }
 
-BehaviorStateMachine* IntersectionState::GetNextState()
-{
-  PreCalculatedConditions* pCParams = GetCalcParams();
-  if(m_pParams->closestIntersectionDistance < 35){
-    return FindBehaviorState(this->m_Behavior);
-  }
-  else if(m_pParams->enableTrafficLightBehavior
-        && pCParams->currentTrafficLightID > 0
-        && pCParams->bTrafficIsRed)
+BehaviorStateMachine *IntersectionState::GetNextState() {
+    PreCalculatedConditions *pCParams = GetCalcParams();
+    if (m_pParams->closestIntersectionDistance < 35) {
+        return FindBehaviorState(this->m_Behavior);
+    } else if (m_pParams->enableTrafficLightBehavior && pCParams->currentTrafficLightID > 0 && pCParams->bTrafficIsRed)
         // && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
-      return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
-  else
-    return FindBehaviorState(FORWARD_STATE);
+        return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+    else
+        return FindBehaviorState(FORWARD_STATE);
 }
 
 } /* namespace PlannerHNS */
diff --git a/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp b/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp
index 9edfca2f..f4fc1a2c 100755
--- a/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp
+++ b/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp
@@ -4,66 +4,61 @@
 /// \author Hatem Darweesh
 /// \date Dec 14, 2016
 
-
 #include "op_planner/DecisionMaker.h"
-#include "op_utility/UtilityH.h"
-#include "op_planner/PlanningHelpers.h"
 #include "op_planner/MappingHelpers.h"
 #include "op_planner/MatrixOperations.h"
+#include "op_planner/PlanningHelpers.h"
+#include "op_utility/UtilityH.h"
 
+namespace PlannerHNS {
 
-namespace PlannerHNS
-{
-
-DecisionMaker::DecisionMaker()
-{
-  m_iCurrentTotalPathId = 0;
-  pLane = 0;
-  m_pCurrentBehaviorState = 0;
-  m_pGoToGoalState = 0;
-  m_pStopState= 0;
-  m_pWaitState= 0;
-  m_pMissionCompleteState= 0;
-  m_pAvoidObstacleState = 0;
-  m_pTrafficLightStopState = 0;
-  m_pTrafficLightWaitState = 0;
-  m_pStopSignStopState = 0;
-  m_pStopSignWaitState = 0;
-  m_pFollowState = 0;
-  m_MaxLaneSearchDistance = 3.0;
-  m_pStopState = 0;
-  m_pMissionCompleteState = 0;
-  m_pGoalState = 0;
-  m_pGoToGoalState = 0;
-  m_pWaitState = 0;
-  m_pInitState = 0;
-  m_pFollowState = 0;
-  m_pAvoidObstacleState = 0;
-  m_pPedestrianState = 0;
-  m_sprintSpeed = -1;
-  m_remainObstacleWaitingTime = 0;
-  curveSlowDownCount = 400;
+DecisionMaker::DecisionMaker() {
+    m_iCurrentTotalPathId = 0;
+    pLane = 0;
+    m_pCurrentBehaviorState = 0;
+    m_pGoToGoalState = 0;
+    m_pStopState = 0;
+    m_pWaitState = 0;
+    m_pMissionCompleteState = 0;
+    m_pAvoidObstacleState = 0;
+    m_pTrafficLightStopState = 0;
+    m_pTrafficLightWaitState = 0;
+    m_pStopSignStopState = 0;
+    m_pStopSignWaitState = 0;
+    m_pFollowState = 0;
+    m_MaxLaneSearchDistance = 3.0;
+    m_pStopState = 0;
+    m_pMissionCompleteState = 0;
+    m_pGoalState = 0;
+    m_pGoToGoalState = 0;
+    m_pWaitState = 0;
+    m_pInitState = 0;
+    m_pFollowState = 0;
+    m_pAvoidObstacleState = 0;
+    m_pPedestrianState = 0;
+    m_sprintSpeed = -1;
+    m_remainObstacleWaitingTime = 0;
+    curveSlowDownCount = 400;
 }
 
-DecisionMaker::~DecisionMaker()
-{
-  delete m_pStopState;
-  delete m_pMissionCompleteState;
-  delete m_pGoalState;
-  delete m_pGoToGoalState;
-  delete m_pWaitState;
-  delete m_pInitState;
-  delete m_pFollowState;
-  delete m_pAvoidObstacleState;
-  delete m_pTrafficLightStopState;
-  delete m_pTrafficLightWaitState;
-  delete m_pStopSignWaitState;
-  delete m_pStopSignStopState;
-  delete m_pPedestrianState;
+DecisionMaker::~DecisionMaker() {
+    delete m_pStopState;
+    delete m_pMissionCompleteState;
+    delete m_pGoalState;
+    delete m_pGoToGoalState;
+    delete m_pWaitState;
+    delete m_pInitState;
+    delete m_pFollowState;
+    delete m_pAvoidObstacleState;
+    delete m_pTrafficLightStopState;
+    delete m_pTrafficLightWaitState;
+    delete m_pStopSignWaitState;
+    delete m_pStopSignStopState;
+    delete m_pPedestrianState;
 }
 
-void DecisionMaker::Init(const ControllerParams& ctrlParams, const PlannerHNS::PlanningParams& params,const CAR_BASIC_INFO& carInfo, const double sprintSpeed)
-   {
+void DecisionMaker::Init(const ControllerParams &ctrlParams, const PlannerHNS::PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                         const double sprintSpeed) {
     m_CarInfo = carInfo;
     m_ControlParams = ctrlParams;
     m_params = params;
@@ -76,7 +71,7 @@ void DecisionMaker::Init(const ControllerParams& ctrlParams, const PlannerHNS::P
     m_pidSprintVelocity.Setlimit(sprintSpeed, 0);
 
     m_pidIntersectionVelocity.Init(0.01, 0.004, 0.01);
-    m_pidIntersectionVelocity.Setlimit(m_params.maxSpeed*0.7, 0);
+    m_pidIntersectionVelocity.Setlimit(m_params.maxSpeed * 0.7, 0);
 
     m_pidStopping.Init(0.05, 0.05, 0.1);
     m_pidStopping.Setlimit(m_params.horizonDistance, 0);
@@ -90,771 +85,732 @@ void DecisionMaker::Init(const ControllerParams& ctrlParams, const PlannerHNS::P
 
     InitBehaviorStates();
 
-    if(m_pCurrentBehaviorState)
-      m_pCurrentBehaviorState->SetBehaviorsParams(&m_params);
-   }
+    if (m_pCurrentBehaviorState)
+        m_pCurrentBehaviorState->SetBehaviorsParams(&m_params);
+}
 
-void DecisionMaker::InitBehaviorStates()
-{
+void DecisionMaker::InitBehaviorStates() {
 
-  m_pStopState         = new StopState(&m_params, 0, 0);
-  m_pMissionCompleteState   = new MissionAccomplishedStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), 0);
-  m_pGoalState        = new GoalStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pMissionCompleteState);
-  m_pGoToGoalState       = new ForwardStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoalState);
-  m_pInitState         = new InitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    m_pStopState = new StopState(&m_params, 0, 0);
+    m_pMissionCompleteState = new MissionAccomplishedStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), 0);
+    m_pGoalState = new GoalStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pMissionCompleteState);
+    m_pGoToGoalState = new ForwardStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoalState);
+    m_pInitState = new InitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
 
-  m_pFollowState        = new FollowStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
-  m_pAvoidObstacleState    = new SwerveStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
-  m_pStopSignWaitState    = new StopSignWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
-  m_pStopSignStopState    = new StopSignStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pStopSignWaitState);
+    m_pFollowState = new FollowStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    m_pAvoidObstacleState = new SwerveStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    m_pStopSignWaitState = new StopSignWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    m_pStopSignStopState = new StopSignStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pStopSignWaitState);
 
-  m_pTrafficLightWaitState  = new TrafficLightWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
-  m_pTrafficLightStopState  = new TrafficLightStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    m_pTrafficLightWaitState = new TrafficLightWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    m_pTrafficLightStopState = new TrafficLightStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
 
-  // Added by PHY
-  m_pPedestrianState         = new PedestrianState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    // Added by PHY
+    m_pPedestrianState = new PedestrianState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
 
-  m_pIntersectionState         = new IntersectionState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+    m_pIntersectionState = new IntersectionState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
 
-  m_pGoToGoalState->InsertNextState(m_pAvoidObstacleState);
-  m_pGoToGoalState->InsertNextState(m_pStopSignStopState);
-  m_pGoToGoalState->InsertNextState(m_pTrafficLightStopState);
-  m_pGoToGoalState->InsertNextState(m_pFollowState);
-  m_pGoToGoalState->InsertNextState(m_pPedestrianState);
-  m_pGoToGoalState->InsertNextState(m_pIntersectionState);
-  m_pGoToGoalState->decisionMakingCount = 0;//m_params.nReliableCount;
+    m_pGoToGoalState->InsertNextState(m_pAvoidObstacleState);
+    m_pGoToGoalState->InsertNextState(m_pStopSignStopState);
+    m_pGoToGoalState->InsertNextState(m_pTrafficLightStopState);
+    m_pGoToGoalState->InsertNextState(m_pFollowState);
+    m_pGoToGoalState->InsertNextState(m_pPedestrianState);
+    m_pGoToGoalState->InsertNextState(m_pIntersectionState);
+    m_pGoToGoalState->decisionMakingCount = 0; // m_params.nReliableCount;
 
-  m_pGoalState->InsertNextState(m_pGoToGoalState);
+    m_pGoalState->InsertNextState(m_pGoToGoalState);
 
-  m_pStopSignWaitState->decisionMakingTime = m_params.stopSignStopTime;
-  m_pStopSignWaitState->InsertNextState(m_pStopSignStopState);
-  m_pStopSignWaitState->InsertNextState(m_pGoalState);
-  m_pStopSignWaitState->InsertNextState(m_pPedestrianState);
+    m_pStopSignWaitState->decisionMakingTime = m_params.stopSignStopTime;
+    m_pStopSignWaitState->InsertNextState(m_pStopSignStopState);
+    m_pStopSignWaitState->InsertNextState(m_pGoalState);
+    m_pStopSignWaitState->InsertNextState(m_pPedestrianState);
 
-  m_pStopSignStopState->InsertNextState(m_pPedestrianState);
+    m_pStopSignStopState->InsertNextState(m_pPedestrianState);
 
-  m_pTrafficLightStopState->InsertNextState(m_pTrafficLightWaitState);
-  m_pTrafficLightStopState->InsertNextState(m_pPedestrianState);
+    m_pTrafficLightStopState->InsertNextState(m_pTrafficLightWaitState);
+    m_pTrafficLightStopState->InsertNextState(m_pPedestrianState);
 
-  m_pTrafficLightWaitState->InsertNextState(m_pTrafficLightStopState);
-  m_pTrafficLightWaitState->InsertNextState(m_pGoalState);
-  m_pTrafficLightWaitState->InsertNextState(m_pPedestrianState);
+    m_pTrafficLightWaitState->InsertNextState(m_pTrafficLightStopState);
+    m_pTrafficLightWaitState->InsertNextState(m_pGoalState);
+    m_pTrafficLightWaitState->InsertNextState(m_pPedestrianState);
 
-  m_pFollowState->InsertNextState(m_pAvoidObstacleState);
-  m_pFollowState->InsertNextState(m_pStopSignStopState);
-  m_pFollowState->InsertNextState(m_pTrafficLightStopState);
-  m_pFollowState->InsertNextState(m_pGoalState);
-  m_pFollowState->InsertNextState(m_pPedestrianState);
-  m_pFollowState->decisionMakingCount = 0;//m_params.nReliableCount;
+    m_pFollowState->InsertNextState(m_pAvoidObstacleState);
+    m_pFollowState->InsertNextState(m_pStopSignStopState);
+    m_pFollowState->InsertNextState(m_pTrafficLightStopState);
+    m_pFollowState->InsertNextState(m_pGoalState);
+    m_pFollowState->InsertNextState(m_pPedestrianState);
+    m_pFollowState->decisionMakingCount = 0; // m_params.nReliableCount;
 
-  m_pAvoidObstacleState->InsertNextState(m_pPedestrianState);
+    m_pAvoidObstacleState->InsertNextState(m_pPedestrianState);
 
-  m_pInitState->decisionMakingCount = 0;//m_params.nReliableCount;
+    m_pInitState->decisionMakingCount = 0; // m_params.nReliableCount;
 
-  m_pCurrentBehaviorState = m_pInitState;
+    m_pCurrentBehaviorState = m_pInitState;
 }
 
- bool DecisionMaker::GetNextTrafficLight(const int& prevTrafficLightId, const std::vector<PlannerHNS::TrafficLight>& trafficLights, PlannerHNS::TrafficLight& trafficL)
- {
-   for(unsigned int i = 0; i < trafficLights.size(); i++)
-   {
-     double d = hypot(trafficLights.at(i).pos.y - state.pos.y, trafficLights.at(i).pos.x - state.pos.x);
-     if(d <= trafficLights.at(i).stoppingDistance)
-     {
-       double a_diff = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(UtilityHNS::UtilityH::FixNegativeAngle(trafficLights.at(i).pos.a) , UtilityHNS::UtilityH::FixNegativeAngle(state.pos.a));
-
-       if(a_diff < M_PI_2 && trafficLights.at(i).id != prevTrafficLightId)
-       {
-         //std::cout << "Detected Light, ID = " << trafficLights.at(i).id << ", Distance = " << d << ", Angle = " << trafficLights.at(i).pos.a*RAD2DEG << ", Car Heading = " << state.pos.a*RAD2DEG << ", Diff = " << a_diff*RAD2DEG << std::endl;
-         trafficL = trafficLights.at(i);
-         return true;
-       }
-     }
-   }
-
-   return false;
- }
-
- void DecisionMaker::CalculateImportantParameterForDecisionMaking(const PlannerHNS::VehicleState& car_state,
-     const int& goalID, const bool& bEmergencyStop, const std::vector<TrafficLight>& detectedLights,
-     const TrajectoryCost& bestTrajectory)
- {
-   if(m_TotalPath.size() == 0) return;
-
-   PreCalculatedConditions* pValues = m_pCurrentBehaviorState->GetCalcParams();
-
-   if(m_CarInfo.max_deceleration != 0)
-     pValues->minStoppingDistance = -pow(car_state.speed, 2)/(m_CarInfo.max_deceleration);
-
-   pValues->iCentralTrajectory    = m_pCurrentBehaviorState->m_pParams->rollOutNumber/2;
-
-  if(pValues->iPrevSafeTrajectory < 0)
-    pValues->iPrevSafeTrajectory = pValues->iCentralTrajectory;
-
-   pValues->stoppingDistances.clear();
-   pValues->currentVelocity     = car_state.speed;
-   pValues->bTrafficIsRed       = false;
-   pValues->currentTrafficLightID   = -1;
-   pValues->bFullyBlock       = false;
-
-   pValues->distanceToNext = bestTrajectory.closest_obj_distance;
-   pValues->velocityOfNext = bestTrajectory.closest_obj_velocity;
-
-   if(bestTrajectory.index >=0 &&  bestTrajectory.index < (int)m_RollOuts.size())
-     pValues->iCurrSafeTrajectory = bestTrajectory.index;
-   else
-     pValues->iCurrSafeTrajectory = pValues->iCentralTrajectory;
-
-  pValues->bFullyBlock = bestTrajectory.bBlocked;
-
-   if(bestTrajectory.lane_index >=0)
-     pValues->iCurrSafeLane = bestTrajectory.lane_index;
-   else
-   {
-     PlannerHNS::RelativeInfo info;
-     PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_TotalPath, state, m_params.rollOutDensity*m_params.rollOutNumber/2.0 + 0.1, info);
-     pValues->iCurrSafeLane = info.iGlobalPath;
-   }
-
-   double critical_long_front_distance =  m_CarInfo.wheel_base/2.0 + m_CarInfo.length/2.0 + m_params.verticalSafetyDistance;
-
-  // HJW modified
-  // ISSUE: Stop distnace should be calculated dynamically on real vehicle
-  //if(ReachEndOfGlobalPath(pValues->minStoppingDistance + critical_long_front_distance, pValues->iCurrSafeLane))
-  if(ReachEndOfGlobalPath(0.3, pValues->iCurrSafeLane))
-    pValues->currentGoalID = -1;
-  else
-    pValues->currentGoalID = goalID;
-
-  m_iCurrentTotalPathId = pValues->iCurrSafeLane;
-
-  // For Intersection
-  m_params.isInsideIntersection = m_isInsideIntersection;
-  m_params.obstacleinRiskyArea = (m_riskyLeft || m_riskyRight);
-  m_params.closestIntersectionDistance = m_closestIntersectionDistance;
-
-  // std::cout << "isIn : " << m_params.isInsideIntersection << " left : " << m_params.turnLeft << " right : " << m_params.turnRight << std::endl;
-
-  // For Traffic Signal
-
-  int stopLineID = -1;
-  int stopSignID = -1;
-  double stopLineLength = -1;
-  int trafficLightID = -1;
-  double distanceToClosestStopLine = 0;
-  bool bShouldForward = true;
-  bool traffic_detected = false;
-  double remain_time = 0;
-
-  distanceToClosestStopLine = PlanningHelpers::CalculateStopLineDistance_RUBIS(m_TotalPath.at(pValues->iCurrSafeLane), state, m_Map.stopLines, stopLineID, stopLineLength, trafficLightID) - critical_long_front_distance;
-
-  // std::cout << "StopLineID : " << stopLineID << ", TrafficLightID : " << trafficLightID << ", Distance: " << distanceToClosestStopLine << ", MinStopDistance: " << pValues->minStoppingDistance << std::endl;
-  // std::cout << "detected Lights # : " << detectedLights.size() << std::endl;
-
-  if(m_pCurrentBehaviorState->m_pParams->enableTrafficLightBehavior){
-
-    for(unsigned int i=0; i< detectedLights.size(); i++)
-    {
-      if(detectedLights.at(i).id == trafficLightID){
-        traffic_detected = true;
-
-        ////// V2X without remain time
-        remain_time = m_remainTrafficLightWaitingTime;
-        // new light case
-        if(detectedLights.at(i).id != m_prevTrafficLightID){
-          if(detectedLights.at(i).lightState == GREEN_LIGHT){
-            remain_time = detectedLights.at(i).routine.at(1); // Add yellow light
-          }
-          else{ // For yellow, red case
-            remain_time = 0;
-          }
+bool DecisionMaker::GetNextTrafficLight(const int &prevTrafficLightId, const std::vector<PlannerHNS::TrafficLight> &trafficLights,
+                                        PlannerHNS::TrafficLight &trafficL) {
+    for (unsigned int i = 0; i < trafficLights.size(); i++) {
+        double d = hypot(trafficLights.at(i).pos.y - state.pos.y, trafficLights.at(i).pos.x - state.pos.x);
+        if (d <= trafficLights.at(i).stoppingDistance) {
+            double a_diff = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(UtilityHNS::UtilityH::FixNegativeAngle(trafficLights.at(i).pos.a),
+                                                                                UtilityHNS::UtilityH::FixNegativeAngle(state.pos.a));
+
+            if (a_diff < M_PI_2 && trafficLights.at(i).id != prevTrafficLightId) {
+                // std::cout << "Detected Light, ID = " << trafficLights.at(i).id << ", Distance = " << d << ", Angle = " <<
+                // trafficLights.at(i).pos.a*RAD2DEG << ", Car Heading = " << state.pos.a*RAD2DEG << ", Diff = " << a_diff*RAD2DEG << std::endl;
+                trafficL = trafficLights.at(i);
+                return true;
+            }
         }
-        else if(detectedLights.at(i).lightState != m_prevTrafficLightSignal){
-          if(detectedLights.at(i).lightState == GREEN_LIGHT){ // R -> G
-            // G Time + Y Time
-            remain_time = detectedLights.at(i).routine.at(0) + detectedLights.at(i).routine.at(1);
-          }
-          else if(detectedLights.at(i).lightState == YELLOW_LIGHT){ // G -> Y
-            remain_time = detectedLights.at(i).routine.at(1);
-          }
-          else{ // Y -> R
-            remain_time = detectedLights.at(i).routine.at(2);
-          }
-        }
-        else{
-          remain_time -= 0.01; // spin period;
-          if(remain_time < 0) remain_time = 0.0;
-        }
-
-        if(distanceToClosestStopLine < m_params.stopLineDetectionDistance && distanceToClosestStopLine > 0){
-          bool bGreenTrafficLight = !(detectedLights.at(i).lightState == RED_LIGHT);
-          double reachableDistance = m_params.maxSpeed * detectedLights.at(i).remainTime / 2;
-          bShouldForward = (bGreenTrafficLight && reachableDistance > distanceToClosestStopLine) ||
-                        (!bGreenTrafficLight && reachableDistance < distanceToClosestStopLine);
+    }
 
-          pValues->currentTrafficLightID = trafficLightID;
-          pValues->stoppingDistances.push_back(distanceToClosestStopLine);
-        }
+    return false;
+}
 
-        m_prevTrafficLightID = trafficLightID;
-        m_prevTrafficLightSignal = detectedLights.at(i).lightState;
+void DecisionMaker::CalculateImportantParameterForDecisionMaking(const PlannerHNS::VehicleState &car_state, const int &goalID,
+                                                                 const bool &bEmergencyStop, const std::vector<TrafficLight> &detectedLights,
+                                                                 const TrajectoryCost &bestTrajectory) {
+    if (m_TotalPath.size() == 0)
+        return;
 
-        ////// V2X with remain time and stop line length
-        // double remain_time = detectedLights.at(i).remainTime;
-        // if(detectedLights.at(i).lightState == GREEN_LIGHT){ // Add time for yellow lights time
-        //   remain_time += detectedLights.at(i).routine.at(1); // Add yellow light
-        // }
-        // double reachableDistance = m_params.maxSpeed * detectedLights.at(i).remainTime / 2;
-        // bool bGreenTrafficLight = !(detectedLights.at(i).lightState == RED_LIGHT);
-        // bShouldForward = (bGreenTrafficLight && reachableDistance > distanceToClosestStopLine + stopLineLength) ||
-        //               (!bGreenTrafficLight && reachableDistance < distanceToClosestStopLine);
-
-        ////// V2X with remain time without stop line length
-        // double remain_time = detectedLights.at(i).remainTime;
-        // if(detectedLights.at(i).lightState == GREEN_LIGHT){ // Add time for yellow lights time
-        //   remain_time += detectedLights.at(i).routine.at(1); // Add yellow light
-        // }
-        // double reachableDistance = m_params.maxSpeed * detectedLights.at(i).remainTime / 2;
-        // bool bGreenTrafficLight = !(detectedLights.at(i).lightState == RED_LIGHT);
-        // bShouldForward = (bGreenTrafficLight && reachableDistance > distanceToClosestStopLine) ||
-        //               (!bGreenTrafficLight && reachableDistance < distanceToClosestStopLine);
-      }
-    }
+    PreCalculatedConditions *pValues = m_pCurrentBehaviorState->GetCalcParams();
 
-    if(!traffic_detected){
-      m_prevTrafficLightID = -1;
-      m_prevTrafficLightSignal = UNKNOWN_LIGHT;
-    }
+    if (m_CarInfo.max_deceleration != 0)
+        pValues->minStoppingDistance = -pow(car_state.speed, 2) / (m_CarInfo.max_deceleration);
 
-    m_remainTrafficLightWaitingTime = remain_time;
-  }
-
-   pValues->bTrafficIsRed = !bShouldForward;
-
-  if(bEmergencyStop)
-  {
-    pValues->bFullyBlock = true;
-    pValues->distanceToNext = 1;
-    pValues->velocityOfNext = 0;
-  }
-   //cout << "Distances: " << pValues->stoppingDistances.size() << ", Distance To Stop : " << pValues->distanceToStop << endl;
- }
-
- void DecisionMaker::UpdateCurrentLane(const double& search_distance)
- {
-   PlannerHNS::Lane* pMapLane = 0;
-  PlannerHNS::Lane* pPathLane = 0;
-  pPathLane = MappingHelpers::GetLaneFromPath(state, m_TotalPath.at(m_iCurrentTotalPathId));
-  if(!pPathLane)
-  {
-    std::cout << "Performance Alert: Can't Find Lane Information in Global Path, Searching the Map :( " << std::endl;
-    pMapLane  = MappingHelpers::GetClosestLaneFromMap(state, m_Map, search_distance);
-  }
-
-  if(pPathLane)
-    pLane = pPathLane;
-  else if(pMapLane)
-    pLane = pMapLane;
-  else
-    pLane = 0;
- }
-
- bool DecisionMaker::ReachEndOfGlobalPath(const double& min_distance, const int& iGlobalPathIndex)
- {
-   if(m_TotalPath.size()==0) return false;
-
-   PlannerHNS::RelativeInfo info;
-   PlanningHelpers::GetRelativeInfo(m_TotalPath.at(iGlobalPathIndex), state, info);
-
-   double d = 0;
-   for(unsigned int i = info.iFront; i < m_TotalPath.at(iGlobalPathIndex).size()-1; i++)
-   {
-     d+= hypot(m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.y - m_TotalPath.at(iGlobalPathIndex).at(i).pos.y, m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.x - m_TotalPath.at(iGlobalPathIndex).at(i).pos.x);
-     if(d > min_distance)
-       return false;
-   }
-
-   return true;
- }
-
-void DecisionMaker::SetNewGlobalPath(const std::vector<std::vector<WayPoint> >& globalPath)
-{
-  if(m_pCurrentBehaviorState)
-  {
-    m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath = true;
-    m_TotalOriginalPath = globalPath;
-  }
- }
-
-bool DecisionMaker::SelectSafeTrajectory()
-{
-  bool bNewTrajectory = false;
-  PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
-
-  if(!preCalcPrams || m_RollOuts.size() == 0) return bNewTrajectory;
-
-  int currIndex = PlannerHNS::PlanningHelpers::GetClosestNextPointIndexFast(m_Path, state);
-  int index_limit = 0;
-  if(index_limit<=0)
-    index_limit =  m_Path.size()/2.0;
-  if(currIndex > index_limit
-      || preCalcPrams->bRePlan
-      || preCalcPrams->bNewGlobalPath)
-  {
-    std::cout << "New Local Plan !! " << currIndex << ", "<< preCalcPrams->bRePlan << ", " << preCalcPrams->bNewGlobalPath  << ", " <<  m_TotalOriginalPath.at(0).size() << ", PrevLocal: " << m_Path.size();
-    m_Path = m_RollOuts.at(preCalcPrams->iCurrSafeTrajectory);
-    std::cout << ", NewLocal: " << m_Path.size() << std::endl;
+    pValues->iCentralTrajectory = m_pCurrentBehaviorState->m_pParams->rollOutNumber / 2;
 
-    preCalcPrams->bNewGlobalPath = false;
-    preCalcPrams->bRePlan = false;
-    bNewTrajectory = true;
-  }
+    if (pValues->iPrevSafeTrajectory < 0)
+        pValues->iPrevSafeTrajectory = pValues->iCentralTrajectory;
 
-  return bNewTrajectory;
- }
+    pValues->stoppingDistances.clear();
+    pValues->currentVelocity = car_state.speed;
+    pValues->bTrafficIsRed = false;
+    pValues->currentTrafficLightID = -1;
+    pValues->bFullyBlock = false;
 
- PlannerHNS::BehaviorState DecisionMaker::GenerateBehaviorState(const PlannerHNS::VehicleState& vehicleState)
- {
-  PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
+    pValues->distanceToNext = bestTrajectory.closest_obj_distance;
+    pValues->velocityOfNext = bestTrajectory.closest_obj_velocity;
 
-  m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();
-  if(m_pCurrentBehaviorState==0)
-    m_pCurrentBehaviorState = m_pInitState;
+    if (bestTrajectory.index >= 0 && bestTrajectory.index < (int)m_RollOuts.size()) {
+        pValues->iCurrSafeTrajectory = bestTrajectory.index;
+    } else {
+        pValues->iCurrSafeTrajectory = pValues->iCentralTrajectory;
+    }
 
-  PlannerHNS::BehaviorState currentBehavior;
+    if (pValues->bFullyBlock == false)
+        pValues->bFullyBlock = bestTrajectory.bBlocked;
 
-  currentBehavior.state = m_pCurrentBehaviorState->m_Behavior;
-  currentBehavior.followDistance = preCalcPrams->distanceToNext;
+    if (bestTrajectory.lane_index >= 0)
+        pValues->iCurrSafeLane = bestTrajectory.lane_index;
+    else {
+        PlannerHNS::RelativeInfo info;
+        PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_TotalPath, state, m_params.rollOutDensity * m_params.rollOutNumber / 2.0 + 0.1, info);
+        pValues->iCurrSafeLane = info.iGlobalPath;
+    }
 
-  currentBehavior.minVelocity    = 0;
-  currentBehavior.stopDistance   = preCalcPrams->distanceToStop();
-  currentBehavior.followVelocity   = preCalcPrams->velocityOfNext;
-  if(preCalcPrams->iPrevSafeTrajectory<0 || preCalcPrams->iPrevSafeTrajectory >= m_RollOuts.size())
-    currentBehavior.iTrajectory    = preCalcPrams->iCurrSafeTrajectory;
-  else
-    currentBehavior.iTrajectory    = preCalcPrams->iPrevSafeTrajectory;
+    double critical_long_front_distance = m_CarInfo.wheel_base / 2.0 + m_CarInfo.length / 2.0 + m_params.verticalSafetyDistance;
 
-  double average_braking_distance = -pow(vehicleState.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
+    // HJW modified
+    // ISSUE: Stop distnace should be calculated dynamically on real vehicle
+    // if(ReachEndOfGlobalPath(pValues->minStoppingDistance + critical_long_front_distance, pValues->iCurrSafeLane))
+    if (ReachEndOfGlobalPath(0.3, pValues->iCurrSafeLane))
+        pValues->currentGoalID = -1;
+    else
+        pValues->currentGoalID = goalID;
+
+    m_iCurrentTotalPathId = pValues->iCurrSafeLane;
+
+    // For Intersection
+    m_params.isInsideIntersection = m_isInsideIntersection;
+    m_params.obstacleinRiskyArea = (m_riskyLeft || m_riskyRight);
+    m_params.closestIntersectionDistance = m_closestIntersectionDistance;
+
+    // std::cout << "isIn : " << m_params.isInsideIntersection << " left : " << m_params.turnLeft << " right : " << m_params.turnRight << std::endl;
+
+    // For Traffic Signal
+
+    int stopLineID = -1;
+    int stopSignID = -1;
+    double stopLineLength = -1;
+    int trafficLightID = -1;
+    double distanceToClosestStopLine = 0;
+    bool bShouldForward = true;
+    bool traffic_detected = false;
+    double remain_time = 0;
+
+    distanceToClosestStopLine = PlanningHelpers::CalculateStopLineDistance_RUBIS(m_TotalPath.at(pValues->iCurrSafeLane), state, m_Map.stopLines,
+                                                                                 stopLineID, stopLineLength, trafficLightID) -
+                                critical_long_front_distance;
+
+    // std::cout << "StopLineID : " << stopLineID << ", TrafficLightID : " << trafficLightID << ", Distance: " << distanceToClosestStopLine << ",
+    // MinStopDistance: " << pValues->minStoppingDistance << std::endl; std::cout << "detected Lights # : " << detectedLights.size() << std::endl;
+
+    if (m_pCurrentBehaviorState->m_pParams->enableTrafficLightBehavior) {
+
+        for (unsigned int i = 0; i < detectedLights.size(); i++) {
+            if (detectedLights.at(i).id == trafficLightID) {
+                traffic_detected = true;
+
+                ////// V2X without remain time
+                remain_time = m_remainTrafficLightWaitingTime;
+                // new light case
+                if (detectedLights.at(i).id != m_prevTrafficLightID) {
+                    if (detectedLights.at(i).lightState == GREEN_LIGHT) {
+                        remain_time = detectedLights.at(i).routine.at(1); // Add yellow light
+                    } else {                                              // For yellow, red case
+                        remain_time = 0;
+                    }
+                } else if (detectedLights.at(i).lightState != m_prevTrafficLightSignal) {
+                    if (detectedLights.at(i).lightState == GREEN_LIGHT) { // R -> G
+                        // G Time + Y Time
+                        remain_time = detectedLights.at(i).routine.at(0) + detectedLights.at(i).routine.at(1);
+                    } else if (detectedLights.at(i).lightState == YELLOW_LIGHT) { // G -> Y
+                        remain_time = detectedLights.at(i).routine.at(1);
+                    } else { // Y -> R
+                        remain_time = detectedLights.at(i).routine.at(2);
+                    }
+                } else {
+                    remain_time -= 0.01; // spin period;
+                    if (remain_time < 0)
+                        remain_time = 0.0;
+                }
+
+                if (distanceToClosestStopLine < m_params.stopLineDetectionDistance && distanceToClosestStopLine > 0) {
+                    bool bGreenTrafficLight = !(detectedLights.at(i).lightState == RED_LIGHT);
+                    double reachableDistance = m_params.maxSpeed * detectedLights.at(i).remainTime / 2;
+                    bShouldForward = (bGreenTrafficLight && reachableDistance > distanceToClosestStopLine) ||
+                                     (!bGreenTrafficLight && reachableDistance < distanceToClosestStopLine);
+
+                    pValues->currentTrafficLightID = trafficLightID;
+                    pValues->stoppingDistances.push_back(distanceToClosestStopLine);
+                }
+
+                m_prevTrafficLightID = trafficLightID;
+                m_prevTrafficLightSignal = detectedLights.at(i).lightState;
+
+                ////// V2X with remain time and stop line length
+                // double remain_time = detectedLights.at(i).remainTime;
+                // if(detectedLights.at(i).lightState == GREEN_LIGHT){ // Add time for yellow lights time
+                //   remain_time += detectedLights.at(i).routine.at(1); // Add yellow light
+                // }
+                // double reachableDistance = m_params.maxSpeed * detectedLights.at(i).remainTime / 2;
+                // bool bGreenTrafficLight = !(detectedLights.at(i).lightState == RED_LIGHT);
+                // bShouldForward = (bGreenTrafficLight && reachableDistance > distanceToClosestStopLine + stopLineLength) ||
+                //               (!bGreenTrafficLight && reachableDistance < distanceToClosestStopLine);
+
+                ////// V2X with remain time without stop line length
+                // double remain_time = detectedLights.at(i).remainTime;
+                // if(detectedLights.at(i).lightState == GREEN_LIGHT){ // Add time for yellow lights time
+                //   remain_time += detectedLights.at(i).routine.at(1); // Add yellow light
+                // }
+                // double reachableDistance = m_params.maxSpeed * detectedLights.at(i).remainTime / 2;
+                // bool bGreenTrafficLight = !(detectedLights.at(i).lightState == RED_LIGHT);
+                // bShouldForward = (bGreenTrafficLight && reachableDistance > distanceToClosestStopLine) ||
+                //               (!bGreenTrafficLight && reachableDistance < distanceToClosestStopLine);
+            }
+        }
 
-  if(average_braking_distance  < m_params.minIndicationDistance)
-    average_braking_distance = m_params.minIndicationDistance;
+        if (!traffic_detected) {
+            m_prevTrafficLightID = -1;
+            m_prevTrafficLightSignal = UNKNOWN_LIGHT;
+        }
 
-  double minDistanceToRollOut = 0;
-  for(int i=0; i<m_RollOuts.size(); i++){
-    const PlannerHNS::WayPoint rollout_start_waypoint = m_RollOuts.at(i).at(std::min(10, int(m_RollOuts.at(i).size()))-1);
+        m_remainTrafficLightWaitingTime = remain_time;
+    }
 
-    double direct_distance = hypot(rollout_start_waypoint.pos.y - state.pos.y, rollout_start_waypoint.pos.x - state.pos.x);
+    pValues->bTrafficIsRed = !bShouldForward;
 
-    if(minDistanceToRollOut == 0 || minDistanceToRollOut > direct_distance){
-      minDistanceToRollOut = direct_distance;
-      currentBehavior.currTrajectory = i;
+    if (bEmergencyStop) {
+        pValues->bFullyBlock = true;
+        pValues->distanceToNext = 1;
+        pValues->velocityOfNext = 0;
     }
-  }
-
-  // Check turn
-  // Detects whether or not to turning 50m ahead
-  m_turnWaypoint = m_RollOuts.at(currentBehavior.currTrajectory).at(std::min(100, int(m_RollOuts.at(currentBehavior.currTrajectory).size()))-1);
-  
-  currentBehavior.indicator = PlanningHelpers::GetIndicatorsFromPath(m_Path, state, average_braking_distance );
-
-  return currentBehavior;
- }
-
- double DecisionMaker::UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt)
- {
-  if(m_TotalOriginalPath.size() ==0 ) return 0;
-
-  RelativeInfo info, total_info;
-  PlanningHelpers::GetRelativeInfo(m_TotalOriginalPath.at(m_iCurrentTotalPathId), state, total_info);
-  PlanningHelpers::GetRelativeInfo(m_Path, state, info);
-  double average_braking_distance = -pow(CurrStatus.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
-  double max_velocity  = PlannerHNS::PlanningHelpers::GetVelocityAhead(m_TotalOriginalPath.at(m_iCurrentTotalPathId), total_info, total_info.iBack, average_braking_distance);
-
-  unsigned int point_index = 0;
-  double critical_long_front_distance = m_CarInfo.length/2.0;
-
-  if(beh.state == PEDESTRIAN_STATE){
-    double desiredVelocity = -1;
-    return desiredVelocity;
-  }
-  else if(beh.state == INTERSECTION_STATE){
-    
-    max_velocity = m_params.maxSpeed*0.7;
-    double target_velocity = max_velocity;
-
-    double e = target_velocity - CurrStatus.speed;
-
-    m_pidSprintVelocity.getPID(e);
-    m_pidVelocity.getPID(e);
-    double desiredVelocity = m_pidIntersectionVelocity.getPID(e);
-    
-    if(desiredVelocity > max_velocity)
-      desiredVelocity = max_velocity;
-    else if(desiredVelocity < m_params.minSpeed)
-      desiredVelocity = 0;
-
-    // std::cout << "o_a : " << m_params.obstacleinRiskyArea << "f_d : " << beh.followDistance << std::endl;
-    // std::cout << "remain_time : " << m_remainObstacleWaitingTime << std::endl;
-
-    // Wait for predetermined time
-    if(m_remainObstacleWaitingTime > 0){
-      m_remainObstacleWaitingTime--;
-      desiredVelocity = 0;
-    }
-    else if(m_params.obstacleinRiskyArea){
-      // double desiredAcceleration = m_params.maxSpeed * m_params.maxSpeed / 2 / std::max(beh.stopDistance - m_params.stopLineMargin, 0.1);
-      // double desiredVelocity = m_params.maxSpeed - desiredAcceleration * 0.1; // 0.1 stands for delta t.
-      // if(desiredVelocity < 0.5)
-      //   desiredVelocity = 0;
-      desiredVelocity = 0;
-      m_remainObstacleWaitingTime = int(m_obstacleWaitingTimeinIntersection * 100);
+    // cout << "Distances: " << pValues->stoppingDistances.size() << ", Distance To Stop : " << pValues->distanceToStop << endl;
+}
+
+void DecisionMaker::UpdateCurrentLane(const double &search_distance) {
+    PlannerHNS::Lane *pMapLane = 0;
+    PlannerHNS::Lane *pPathLane = 0;
+    pPathLane = MappingHelpers::GetLaneFromPath(state, m_TotalPath.at(m_iCurrentTotalPathId));
+    if (!pPathLane) {
+        std::cout << "Performance Alert: Can't Find Lane Information in Global Path, Searching the Map :( " << std::endl;
+        pMapLane = MappingHelpers::GetClosestLaneFromMap(state, m_Map, search_distance);
     }
-    // else if(beh.followDistance < 30){
-    //   desiredVelocity = 0;
-    //   m_remainObstacleWaitingTime = int(m_obstacleWaitingTimeinIntersection * 100);
-    // }
-
-    for(unsigned int i = 0; i < m_Path.size(); i++)
-      m_Path.at(i).v = desiredVelocity;
-
-    return desiredVelocity;
-  }
-  else if(beh.state == TRAFFIC_LIGHT_STOP_STATE || beh.state == TRAFFIC_LIGHT_WAIT_STATE)
-  {
-    double desiredAcceleration = m_params.maxSpeed * m_params.maxSpeed / 2 / std::max(beh.stopDistance - m_params.stopLineMargin, 0.1);
-    double desiredVelocity = m_params.maxSpeed - desiredAcceleration * 0.1; // 0.1 stands for delta t.
-    
-    double e = max_velocity - CurrStatus.speed;
-    m_pidSprintVelocity.getPID(e);
-    m_pidVelocity.getPID(e);
-    m_pidIntersectionVelocity.getPID(e);
-
-    if(desiredVelocity < 0.5)
-      desiredVelocity = 0;
-
-    for(unsigned int i = 0; i < m_Path.size(); i++)
-      m_Path.at(i).v = desiredVelocity;
-    return desiredVelocity;
-  }
-  else if(beh.state == STOP_SIGN_STOP_STATE)
-  {
-    PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.stopDistance - critical_long_front_distance, point_index);
-
-    double e = -beh.stopDistance;
-    double desiredVelocity = m_pidStopping.getPID(e);
-
-    double e2 = max_velocity - CurrStatus.speed;
-    m_pidSprintVelocity.getPID(e2);
-    m_pidVelocity.getPID(e2);
-    m_pidIntersectionVelocity.getPID(e2);
-
-//    std::cout << "Stopping : e=" << e << ", desiredPID=" << desiredVelocity << ", PID: " << m_pidStopping.ToString() << std::endl;
-
-    if(desiredVelocity > max_velocity)
-      desiredVelocity = max_velocity;
-    else if(desiredVelocity < m_params.minSpeed)
-      desiredVelocity = 0;
-
-    for(unsigned int i =  0; i < m_Path.size(); i++)
-      m_Path.at(i).v = desiredVelocity;
-
-    return desiredVelocity;
-  }
-  else if(beh.state == STOP_SIGN_WAIT_STATE)
-  {
-    double target_velocity = 0;
-    for(unsigned int i = 0; i < m_Path.size(); i++)
-      m_Path.at(i).v = target_velocity;
-
-    return target_velocity;
-  }
-  else if(beh.state == FOLLOW_STATE)
-  {
-
-    double deceleration_critical = 0;
-    double inv_time = 2.0*((beh.followDistance- (critical_long_front_distance+m_params.additionalBrakingDistance))-CurrStatus.speed);
-    if(inv_time <= 0)
-      deceleration_critical = m_CarInfo.max_deceleration;
+
+    if (pPathLane)
+        pLane = pPathLane;
+    else if (pMapLane)
+        pLane = pMapLane;
     else
-      deceleration_critical = CurrStatus.speed*CurrStatus.speed/inv_time;
+        pLane = 0;
+}
 
-    if(deceleration_critical > 0) deceleration_critical = -deceleration_critical;
-    if(deceleration_critical < - m_CarInfo.max_acceleration) deceleration_critical = - m_CarInfo.max_acceleration;
+bool DecisionMaker::ReachEndOfGlobalPath(const double &min_distance, const int &iGlobalPathIndex) {
+    if (m_TotalPath.size() == 0)
+        return false;
 
-    double desiredVelocity = (deceleration_critical * dt) + CurrStatus.speed;
+    PlannerHNS::RelativeInfo info;
+    PlanningHelpers::GetRelativeInfo(m_TotalPath.at(iGlobalPathIndex), state, info);
 
-    if(desiredVelocity > m_params.maxSpeed)
-      desiredVelocity = m_params.maxSpeed;
+    double d = 0;
+    for (unsigned int i = info.iFront; i < m_TotalPath.at(iGlobalPathIndex).size() - 1; i++) {
+        d += hypot(m_TotalPath.at(iGlobalPathIndex).at(i + 1).pos.y - m_TotalPath.at(iGlobalPathIndex).at(i).pos.y,
+                   m_TotalPath.at(iGlobalPathIndex).at(i + 1).pos.x - m_TotalPath.at(iGlobalPathIndex).at(i).pos.x);
+        if (d > min_distance)
+            return false;
+    }
 
-    if((desiredVelocity < 0.1 && desiredVelocity > -0.1) || beh.followDistance <= 0) //use only effective velocities
-      desiredVelocity = 0;
+    return true;
+}
 
-    //std::cout << "Acc: V: " << desiredVelocity << ", Accel: " << deceleration_critical<< std::endl;
+void DecisionMaker::SetNewGlobalPath(const std::vector<std::vector<WayPoint>> &globalPath) {
+    if (m_pCurrentBehaviorState) {
+        m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath = true;
+        m_TotalOriginalPath = globalPath;
+    }
+}
 
-    for(unsigned int i = 0; i < m_Path.size(); i++)
-      m_Path.at(i).v = desiredVelocity;
+bool DecisionMaker::SelectSafeTrajectory() {
+    bool bNewTrajectory = false;
+    PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
 
-    return desiredVelocity;
+    if (!preCalcPrams || m_RollOuts.size() == 0)
+        return bNewTrajectory;
 
-  }
-  else if(beh.state == FORWARD_STATE)
-  {
-    if(m_sprintSwitch == true){
-      max_velocity = m_sprintSpeed;
-    }
+    // int currIndex = PlannerHNS::PlanningHelpers::GetClosestNextPointIndexFast(m_Path, state);
+    // int index_limit = 0;
+    // if(index_limit<=0)
+    //   index_limit =  m_Path.size()/2.0;
 
-    double target_velocity = max_velocity;
-    bool bSlowBecauseChange=false;
+    if (preCalcPrams->iCurrSafeTrajectory != m_prevSafeTrajectory || preCalcPrams->bRePlan || preCalcPrams->bNewGlobalPath) {
+        // m_Path = m_RollOuts.at(preCalcPrams->iCurrSafeTrajectory);
 
-    // std::cout << "curr Traj : " << beh.currTrajectory << ", curr Safe Traj : " << m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << std::endl;
-    // if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
-    if(beh.currTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory)
-    {
-      // target_velocity /= fabs(beh.currTrajectory - m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory);
-      target_velocity *= 0.5;
-      bSlowBecauseChange = true;
+        preCalcPrams->bNewGlobalPath = false;
+        preCalcPrams->bRePlan = false;
+        bNewTrajectory = true;
     }
-    
-    double e = target_velocity - CurrStatus.speed;
-    double desiredVelocity = 0; 
 
-    double sprint_pid_vel, forward_pid_vel;
-    sprint_pid_vel = m_pidSprintVelocity.getPID(e);
-    forward_pid_vel = m_pidVelocity.getPID(e);
-    m_pidIntersectionVelocity.getPID(e);
+    m_Path = m_RollOuts.at(preCalcPrams->iCurrSafeTrajectory);
+    m_prevSafeTrajectory = preCalcPrams->iCurrSafeTrajectory;
 
-    if(m_sprintSwitch == true)
-      desiredVelocity = sprint_pid_vel;
+    return bNewTrajectory;
+}
+
+PlannerHNS::BehaviorState DecisionMaker::GenerateBehaviorState(const PlannerHNS::VehicleState &vehicleState) {
+    PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
+
+    m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();
+    if (m_pCurrentBehaviorState == 0)
+        m_pCurrentBehaviorState = m_pInitState;
+
+    PlannerHNS::BehaviorState currentBehavior;
+
+    currentBehavior.state = m_pCurrentBehaviorState->m_Behavior;
+    currentBehavior.followDistance = preCalcPrams->distanceToNext;
+
+    currentBehavior.minVelocity = 0;
+    currentBehavior.stopDistance = preCalcPrams->distanceToStop();
+    currentBehavior.followVelocity = preCalcPrams->velocityOfNext;
+    if (preCalcPrams->iPrevSafeTrajectory < 0 || preCalcPrams->iPrevSafeTrajectory >= m_RollOuts.size())
+        currentBehavior.iTrajectory = preCalcPrams->iCurrSafeTrajectory;
     else
-      desiredVelocity = forward_pid_vel;
-
-    if(m_params.enableSlowDownOnCurve){
-      GPSPoint curr_point = m_Path.at(info.iFront).pos; // current waypoint (p1)
-      GPSPoint near_point = m_Path.at(std::min(info.iFront + 3, int(m_Path.size())-1)).pos; // waypoint after 1.5m (p2)
-      GPSPoint far_point = m_Path.at(std::min(info.iFront + 100, int(m_Path.size())-1)).pos; // waypoint afeter 50m (p3)
-
-      double deg_1 = atan2((near_point.y - curr_point.y), (near_point.x - curr_point.x)) / 3.14 * 180;
-      double deg_2 = atan2((far_point.y - curr_point.y), (far_point.x - curr_point.x)) / 3.14 * 180;
-      double angle_diff = std::abs(deg_1 - deg_2); // angle between p1p2 and p1p3
-      if (angle_diff > 180){
-        angle_diff = 360 - angle_diff;
-      }
-
-      // std::cout << "curvature : " << angle_diff << std::endl;
-
-      if (angle_diff > 7){ // Slow down when angle is large
-        desiredVelocity = m_params.maxSpeed * 40 / (angle_diff + 33);
-        // desiredVelocity = m_params.maxSpeed * 17 / (angle_diff + 10);
-        if(desiredVelocity > previous_velocity){
-          desiredVelocity = previous_velocity;
+        currentBehavior.iTrajectory = preCalcPrams->iPrevSafeTrajectory;
+
+    double average_braking_distance = -pow(vehicleState.speed, 2) / (m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
+
+    if (average_braking_distance < m_params.minIndicationDistance)
+        average_braking_distance = m_params.minIndicationDistance;
+
+    double minDistanceToRollOut = 0;
+    for (int i = 0; i < m_RollOuts.size(); i++) {
+        const PlannerHNS::WayPoint rollout_start_waypoint = m_RollOuts.at(i).at(std::min(10, int(m_RollOuts.at(i).size())) - 1);
+
+        double direct_distance = hypot(rollout_start_waypoint.pos.y - state.pos.y, rollout_start_waypoint.pos.x - state.pos.x);
+
+        if (minDistanceToRollOut == 0 || minDistanceToRollOut > direct_distance) {
+            minDistanceToRollOut = direct_distance;
+            currentBehavior.currTrajectory = i;
         }
-        curveSlowDownCount = 0;
-      }
-      // if (angle_diff > 30){
-      //   desiredVelocity = m_params.maxSpeed * 0.8;
-      //   curveSlowDownCount = 0;
-      //   // desiredVelocity = max_velocity * 100 / (angle_diff + 90);
-      // }
-      // else if(angle_diff > 10){
-      //   desiredVelocity = m_params.maxSpeed * 0.6;
-      //   curveSlowDownCount = 0;
-      // }
-      else if(curveSlowDownCount < 400){ // wait 4 sec when angle become less than 7 // TODO: Check its feasibility when pure pursuit is sufficiently tuned
-        desiredVelocity = previous_velocity + (m_params.maxSpeed - previous_velocity) / 100;
-        curveSlowDownCount += 1;
-      }
-      else{
-        desiredVelocity = m_params.maxSpeed;
-      }
-
-      if(desiredVelocity < m_params.maxSpeed * m_params.curveVelocityRatio){ // minimum of target velocity is max_speed / 2
-        desiredVelocity = m_params.maxSpeed * m_params.curveVelocityRatio; 
-      }
-      previous_velocity = desiredVelocity;
     }
 
-    for(auto it = m_Path.begin(); it != m_Path.end(); ++it){
-      (*it).v = desiredVelocity;
-    }
+    // Check turn
+    // Detects whether or not to turning 50m ahead
+    m_turnWaypoint = m_RollOuts.at(currentBehavior.currTrajectory).at(std::min(100, int(m_RollOuts.at(currentBehavior.currTrajectory).size())) - 1);
+
+    currentBehavior.indicator = PlanningHelpers::GetIndicatorsFromPath(m_Path, state, average_braking_distance);
+
+    return currentBehavior;
+}
+
+double DecisionMaker::UpdateVelocityDirectlyToTrajectory(const BehaviorState &beh, const VehicleState &CurrStatus, const double &dt) {
+    if (m_TotalOriginalPath.size() == 0)
+        return 0;
+
+    RelativeInfo info, total_info;
+    PlanningHelpers::GetRelativeInfo(m_TotalOriginalPath.at(m_iCurrentTotalPathId), state, total_info);
+    PlanningHelpers::GetRelativeInfo(m_Path, state, info);
+    double average_braking_distance = -pow(CurrStatus.speed, 2) / (m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
+    double max_velocity = PlannerHNS::PlanningHelpers::GetVelocityAhead(m_TotalOriginalPath.at(m_iCurrentTotalPathId), total_info, total_info.iBack,
+                                                                        average_braking_distance);
+
+    unsigned int point_index = 0;
+    double critical_long_front_distance = m_CarInfo.length / 2.0;
+
+    if (beh.state == PEDESTRIAN_STATE) {
+        double desiredVelocity = -1;
+        return desiredVelocity;
+    } else if (beh.state == INTERSECTION_STATE) {
+
+        max_velocity = m_params.maxSpeed * 0.7;
+        double target_velocity = max_velocity;
+
+        double e = target_velocity - CurrStatus.speed;
+
+        m_pidSprintVelocity.getPID(e);
+        m_pidVelocity.getPID(e);
+        double desiredVelocity = m_pidIntersectionVelocity.getPID(e);
+
+        if (desiredVelocity > max_velocity)
+            desiredVelocity = max_velocity;
+        else if (desiredVelocity < m_params.minSpeed)
+            desiredVelocity = 0;
+
+        // std::cout << "o_a : " << m_params.obstacleinRiskyArea << "f_d : " << beh.followDistance << std::endl;
+        // std::cout << "remain_time : " << m_remainObstacleWaitingTime << std::endl;
+
+        // Wait for predetermined time
+        if (m_remainObstacleWaitingTime > 0) {
+            m_remainObstacleWaitingTime--;
+            desiredVelocity = 0;
+        } else if (m_params.obstacleinRiskyArea) {
+            // double desiredAcceleration = m_params.maxSpeed * m_params.maxSpeed / 2 / std::max(beh.stopDistance - m_params.stopLineMargin, 0.1);
+            // double desiredVelocity = m_params.maxSpeed - desiredAcceleration * 0.1; // 0.1 stands for delta t.
+            // if(desiredVelocity < 0.5)
+            //   desiredVelocity = 0;
+            desiredVelocity = 0;
+            m_remainObstacleWaitingTime = int(m_obstacleWaitingTimeinIntersection * 100);
+        }
+        // else if(beh.followDistance < 30){
+        //   desiredVelocity = 0;
+        //   m_remainObstacleWaitingTime = int(m_obstacleWaitingTimeinIntersection * 100);
+        // }
+
+        for (unsigned int i = 0; i < m_Path.size(); i++)
+            m_Path.at(i).v = desiredVelocity;
+
+        return desiredVelocity;
+    } else if (beh.state == TRAFFIC_LIGHT_STOP_STATE || beh.state == TRAFFIC_LIGHT_WAIT_STATE) {
+        double desiredAcceleration = m_params.maxSpeed * m_params.maxSpeed / 2 / std::max(beh.stopDistance - m_params.stopLineMargin, 0.1);
+        double desiredVelocity = m_params.maxSpeed - desiredAcceleration * 0.1; // 0.1 stands for delta t.
+
+        double e = max_velocity - CurrStatus.speed;
+        m_pidSprintVelocity.getPID(e);
+        m_pidVelocity.getPID(e);
+        m_pidIntersectionVelocity.getPID(e);
 
-    // std::cout << "desiredVelocity : " << desiredVelocity << std::endl;
-
-    // std::cout << "Target Velocity: " << target_velocity << ", desired : " << desiredVelocity << ", Change Slowdown: " << bSlowBecauseChange  << std::endl;
-
-    if(desiredVelocity>max_velocity)
-      desiredVelocity = max_velocity;
-    else if(desiredVelocity < m_params.minSpeed)
-      desiredVelocity = 0;
-
-    return desiredVelocity;
-  }
-  else if(beh.state == OBSTACLE_AVOIDANCE_STATE )
-  {
-    double target_velocity = max_velocity;
-    bool bSlowBecauseChange=false;
-    // std::cout << "curr Traj : " << beh.currTrajectory << ", curr Safe Traj : " << m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << std::endl;
-    // if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
-    if(beh.currTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory)
-    {
-      // target_velocity /= fabs(beh.currTrajectory - m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory);
-      target_velocity *= 0.5;
-      bSlowBecauseChange = true;
+        if (desiredVelocity < 0.5)
+            desiredVelocity = 0;
+
+        for (unsigned int i = 0; i < m_Path.size(); i++)
+            m_Path.at(i).v = desiredVelocity;
+        return desiredVelocity;
+    } else if (beh.state == STOP_SIGN_STOP_STATE) {
+        PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.stopDistance - critical_long_front_distance, point_index);
+
+        double e = -beh.stopDistance;
+        double desiredVelocity = m_pidStopping.getPID(e);
+
+        double e2 = max_velocity - CurrStatus.speed;
+        m_pidSprintVelocity.getPID(e2);
+        m_pidVelocity.getPID(e2);
+        m_pidIntersectionVelocity.getPID(e2);
+
+        //    std::cout << "Stopping : e=" << e << ", desiredPID=" << desiredVelocity << ", PID: " << m_pidStopping.ToString() << std::endl;
+
+        if (desiredVelocity > max_velocity)
+            desiredVelocity = max_velocity;
+        else if (desiredVelocity < m_params.minSpeed)
+            desiredVelocity = 0;
+
+        for (unsigned int i = 0; i < m_Path.size(); i++)
+            m_Path.at(i).v = desiredVelocity;
+
+        return desiredVelocity;
+    } else if (beh.state == STOP_SIGN_WAIT_STATE) {
+        double target_velocity = 0;
+        for (unsigned int i = 0; i < m_Path.size(); i++)
+            m_Path.at(i).v = target_velocity;
+
+        return target_velocity;
+    } else if (beh.state == FOLLOW_STATE) {
+
+        double deceleration_critical = 0;
+        double inv_time = 2.0 * ((beh.followDistance - (critical_long_front_distance + m_params.additionalBrakingDistance)) - CurrStatus.speed);
+        if (inv_time <= 0)
+            deceleration_critical = m_CarInfo.max_deceleration;
+        else
+            deceleration_critical = CurrStatus.speed * CurrStatus.speed / inv_time;
+
+        if (deceleration_critical > 0)
+            deceleration_critical = -deceleration_critical;
+        if (deceleration_critical < -m_CarInfo.max_acceleration)
+            deceleration_critical = -m_CarInfo.max_acceleration;
+
+        double desiredVelocity = (deceleration_critical * dt) + CurrStatus.speed;
+
+        if (desiredVelocity > m_params.maxSpeed)
+            desiredVelocity = m_params.maxSpeed;
+
+        if ((desiredVelocity < 0.1 && desiredVelocity > -0.1) || beh.followDistance <= 0) // use only effective velocities
+            desiredVelocity = 0;
+
+        // std::cout << "Acc: V: " << desiredVelocity << ", Accel: " << deceleration_critical<< std::endl;
+
+        for (unsigned int i = 0; i < m_Path.size(); i++)
+            m_Path.at(i).v = desiredVelocity;
+
+        return desiredVelocity;
+
+    } else if (beh.state == FORWARD_STATE) {
+        if (m_sprintSwitch == true) {
+            max_velocity = m_sprintSpeed;
+        }
+
+        double target_velocity = max_velocity;
+        bool bSlowBecauseChange = false;
+
+        if (beh.currTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory) {
+            // target_velocity /= fabs(beh.currTrajectory - m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory);
+            target_velocity *= 0.5;
+            bSlowBecauseChange = true;
+        }
+
+        double e = target_velocity - CurrStatus.speed;
+        double desiredVelocity = 0;
+
+        double sprint_pid_vel, forward_pid_vel;
+        sprint_pid_vel = m_pidSprintVelocity.getPID(e);
+        forward_pid_vel = m_pidVelocity.getPID(e);
+        m_pidIntersectionVelocity.getPID(e);
+
+        if (m_sprintSwitch == true)
+            desiredVelocity = sprint_pid_vel;
+        else
+            desiredVelocity = forward_pid_vel;
+
+        if (m_params.enableSlowDownOnCurve) {
+            GPSPoint curr_point = m_Path.at(info.iFront).pos;                                        // current waypoint (p1)
+            GPSPoint near_point = m_Path.at(std::min(info.iFront + 3, int(m_Path.size()) - 1)).pos;  // waypoint after 1.5m (p2)
+            GPSPoint far_point = m_Path.at(std::min(info.iFront + 100, int(m_Path.size()) - 1)).pos; // waypoint afeter 50m (p3)
+
+            double deg_1 = atan2((near_point.y - curr_point.y), (near_point.x - curr_point.x)) / 3.14 * 180;
+            double deg_2 = atan2((far_point.y - curr_point.y), (far_point.x - curr_point.x)) / 3.14 * 180;
+            double angle_diff = std::abs(deg_1 - deg_2); // angle between p1p2 and p1p3
+            if (angle_diff > 180) {
+                angle_diff = 360 - angle_diff;
+            }
+            
+            if (angle_diff > 40) { // Slow down when angle is large
+                desiredVelocity = m_params.maxSpeed * (90-angle_diff)/90;
+
+                curveSlowDownCount = 0;
+            }
+            else if (curveSlowDownCount <
+                     400) { // wait 4 sec when angle become less than 7 // TODO: Check its feasibility when pure pursuit is sufficiently tuned
+                desiredVelocity = previous_velocity + (m_params.maxSpeed - previous_velocity) / 100;
+                curveSlowDownCount += 1;
+            } else {
+                desiredVelocity = m_params.maxSpeed;
+            }
+
+            if (desiredVelocity < m_params.maxSpeed * m_params.curveVelocityRatio) { // minimum of target velocity is max_speed * curveVelocityRatio
+                desiredVelocity = m_params.maxSpeed * m_params.curveVelocityRatio;
+            }
+            previous_velocity = desiredVelocity;
+        }
+
+        for (auto it = m_Path.begin(); it != m_Path.end(); ++it) {
+            (*it).v = desiredVelocity;
+        }
+
+        // std::cout << "desiredVelocity : " << desiredVelocity << std::endl;
+
+        // std::cout << "Target Velocity: " << target_velocity << ", desired : " << desiredVelocity << ", Change Slowdown: " << bSlowBecauseChange  <<
+        // std::endl;
+
+        if (desiredVelocity > max_velocity)
+            desiredVelocity = max_velocity;
+        else if (desiredVelocity < m_params.minSpeed)
+            desiredVelocity = 0;
+            
+        return desiredVelocity;
+    } else if (beh.state == OBSTACLE_AVOIDANCE_STATE) {
+        double target_velocity = max_velocity;
+        bool bSlowBecauseChange = false;
+        // std::cout << "curr Traj : " << beh.currTrajectory << ", curr Safe Traj : " << m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory
+        // << std::endl; if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory !=
+        // m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+        if (beh.currTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory) {
+            // target_velocity /= fabs(beh.currTrajectory - m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory);
+            target_velocity *= 0.5;
+            bSlowBecauseChange = true;
+        }
+
+        double e = target_velocity - CurrStatus.speed;
+        double desiredVelocity = m_pidVelocity.getPID(e);
+        m_pidSprintVelocity.getPID(e);
+        m_pidIntersectionVelocity.getPID(e);
+
+        if (desiredVelocity > max_velocity)
+            desiredVelocity = max_velocity;
+        else if (desiredVelocity < m_params.minSpeed)
+            desiredVelocity = 0;
+
+        for (unsigned int i = 0; i < m_Path.size(); i++)
+            m_Path.at(i).v = desiredVelocity;
+
+        // std::cout << "Target Velocity: " << desiredVelocity << ", Change Slowdown: " << bSlowBecauseChange  << std::endl;
+
+        return desiredVelocity;
+    } else {
+        double target_velocity = 0;
+        for (unsigned int i = 0; i < m_Path.size(); i++)
+            m_Path.at(i).v = target_velocity;
+
+        return target_velocity;
     }
 
-    double e = target_velocity - CurrStatus.speed;
-    double desiredVelocity = m_pidVelocity.getPID(e);
-    m_pidSprintVelocity.getPID(e);    
-    m_pidIntersectionVelocity.getPID(e);
-    
-    if(desiredVelocity>max_velocity)
-      desiredVelocity = max_velocity;
-    else if(desiredVelocity < m_params.minSpeed)
-      desiredVelocity = 0;
-
-    for(unsigned int i = 0; i < m_Path.size(); i++)
-      m_Path.at(i).v = desiredVelocity;
-
-    // std::cout << "Target Velocity: " << desiredVelocity << ", Change Slowdown: " << bSlowBecauseChange  << std::endl;
-
-    return desiredVelocity;
-  }
-  else
-  {
-    double target_velocity = 0;
-    for(unsigned int i = 0; i < m_Path.size(); i++)
-      m_Path.at(i).v = target_velocity;
-
-    return target_velocity;
-  }
-
-  return max_velocity;
- }
-
- PlannerHNS::BehaviorState DecisionMaker::DoOneStep(
-    const double& dt,
-    const PlannerHNS::WayPoint currPose,
-    const PlannerHNS::VehicleState& vehicleState,
-    const int& goalID,
-    const std::vector<TrafficLight>& trafficLight,
-    const TrajectoryCost& tc,
-    const bool& bEmergencyStop)
-{
-
-   PlannerHNS::BehaviorState beh;
-   state = currPose;
-   m_TotalPath.clear();
-  for(unsigned int i = 0; i < m_TotalOriginalPath.size(); i++)
-  {
-    t_centerTrajectorySmoothed.clear();
-    PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_TotalOriginalPath.at(i), state, m_params.horizonDistance ,  m_params.pathDensity , t_centerTrajectorySmoothed);
-    m_TotalPath.push_back(t_centerTrajectorySmoothed);
-  }
-
-  if(m_TotalPath.size()==0) return beh;
-
-  UpdateCurrentLane(m_MaxLaneSearchDistance);
-
-  CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc);
-
-  // Enable if left turn scenario needed
-  // CheckTurn();
-
-  // PrintTurn();
-
-  beh = GenerateBehaviorState(vehicleState);
-
-  beh.bNewPlan = SelectSafeTrajectory();
-
-  beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
-  
-  //std::cout << "Eval_i: " << tc.index << ", Curr_i: " <<  m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ", Prev_i: " << m_pCurrentBehaviorState->GetCalcParams()->iPrevSafeTrajectory << std::endl;
-
-  return beh;
- }
-
- // Added by PHY
-  void DecisionMaker::UpdatePedestrianAppearence(const bool pedestrianAppearence){
-    m_params.pedestrianAppearence = pedestrianAppearence;
-  }
-
-  void DecisionMaker::printPedestrianAppearence(){
-    if(m_params.pedestrianAppearence == true){
-      std::cout<<"Pedestrian Appearence: True"<<std::endl;  
+    return max_velocity;
+}
+
+PlannerHNS::BehaviorState DecisionMaker::DoOneStep(const double &dt, const PlannerHNS::WayPoint currPose,
+                                                   const PlannerHNS::VehicleState &vehicleState, const int &goalID,
+                                                   const std::vector<TrafficLight> &trafficLight, const TrajectoryCost &tc,
+                                                   const bool &bEmergencyStop) {
+    static double prev_max_velocity = 0.0;
+
+    PlannerHNS::BehaviorState beh;
+    state = currPose;
+    m_TotalPath.clear();
+    for (unsigned int i = 0; i < m_TotalOriginalPath.size(); i++) {
+        t_centerTrajectorySmoothed.clear();
+        PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_TotalOriginalPath.at(i), state, m_params.horizonDistance,
+                                                                                 m_params.pathDensity, t_centerTrajectorySmoothed);
+        m_TotalPath.push_back(t_centerTrajectorySmoothed);
     }
-    else{
-        std::cout<<"Pedestrian Appearence: False"<<std::endl;  
+
+    if (m_TotalPath.size() == 0)
+        return beh;
+
+    UpdateCurrentLane(m_MaxLaneSearchDistance);
+
+    CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc);
+
+    // Enable if left turn scenario needed
+    // CheckTurn();
+
+    // PrintTurn();
+
+    beh = GenerateBehaviorState(vehicleState);
+
+    beh.bNewPlan = SelectSafeTrajectory();
+
+    // ADDED
+    if(!m_isTargetVelocityReady){
+        while(1){
+            beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
+            if (beh.maxVelocity > m_params.maxSpeed - 1) {
+                m_isTargetVelocityReady = true;
+                break;
+            }
+        }
     }
 
-  }
+    beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
 
-  void DecisionMaker::CheckTurn(){
-    if(abs(m_turnAngle) > m_turnThreshold){
-      if(m_turnAngle > 0){
-        m_params.turnLeft = true;
-        m_params.turnRight = false;
-      }
-      else{
-        m_params.turnLeft = false;
-        m_params.turnRight = true;
-      }
+    // std::cout << "Eval_i: " << tc.index << ", Curr_i: " <<  m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ", Prev_i: " <<
+    // m_pCurrentBehaviorState->GetCalcParams()->iPrevSafeTrajectory << std::endl;
+
+    return beh;
+}
+
+// Added by PHY
+void DecisionMaker::UpdatePedestrianAppearence(const bool pedestrianAppearence) { m_params.pedestrianAppearence = pedestrianAppearence; }
+
+void DecisionMaker::printPedestrianAppearence() {
+    if (m_params.pedestrianAppearence == true) {
+        std::cout << "Pedestrian Appearence: True" << std::endl;
+    } else {
+        std::cout << "Pedestrian Appearence: False" << std::endl;
     }
-    else{
-      m_params.turnLeft = false;
-      m_params.turnRight = false;
+}
+
+void DecisionMaker::CheckTurn() {
+    if (abs(m_turnAngle) > m_turnThreshold) {
+        if (m_turnAngle > 0) {
+            m_params.turnLeft = true;
+            m_params.turnRight = false;
+        } else {
+            m_params.turnLeft = false;
+            m_params.turnRight = true;
+        }
+    } else {
+        m_params.turnLeft = false;
+        m_params.turnRight = false;
     }
-  }
+}
 
-  void DecisionMaker::PrintTurn(){
-    std::cout<<"LEFT:"<<m_params.turnLeft<<std::endl;
-    std::cout<<"RIGHT:"<<m_params.turnRight<<std::endl;
-  }
+void DecisionMaker::PrintTurn() {
+    std::cout << "LEFT:" << m_params.turnLeft << std::endl;
+    std::cout << "RIGHT:" << m_params.turnRight << std::endl;
+}
 
-  std::string DecisionMaker::ToString(STATE_TYPE beh)
-  {
+std::string DecisionMaker::ToString(STATE_TYPE beh) {
     std::string str = "Unknown";
-    switch(beh)
-    {
+    switch (beh) {
     case PlannerHNS::INITIAL_STATE:
-      str = "Init";
-      break;
+        str = "Init";
+        break;
     case PlannerHNS::EMERGENCY_STATE:
-      str = "Emergency";
-      break;
+        str = "Emergency";
+        break;
     case PlannerHNS::FORWARD_STATE:
-      str = "Forward";
-      break;
+        str = "Forward";
+        break;
     case PlannerHNS::STOPPING_STATE:
-      str = "Stop";
-      break;
+        str = "Stop";
+        break;
     case PlannerHNS::FINISH_STATE:
-      str = "End";
-      break;
+        str = "End";
+        break;
     case PlannerHNS::FOLLOW_STATE:
-      str = "Follow";
-      break;
+        str = "Follow";
+        break;
     case PlannerHNS::OBSTACLE_AVOIDANCE_STATE:
-      str = "Swerving";
-      break;
+        str = "Swerving";
+        break;
     case PlannerHNS::TRAFFIC_LIGHT_STOP_STATE:
-      str = "Light Stop";
-      break;
+        str = "Light Stop";
+        break;
     case PlannerHNS::TRAFFIC_LIGHT_WAIT_STATE:
-      str = "Light Wait";
-      break;
+        str = "Light Wait";
+        break;
     case PlannerHNS::STOP_SIGN_STOP_STATE:
-      str = "Sign Stop";
-      break;
+        str = "Sign Stop";
+        break;
     case PlannerHNS::STOP_SIGN_WAIT_STATE:
-      str = "Sign Wait";
-      break;
+        str = "Sign Wait";
+        break;
     case PlannerHNS::INTERSECTION_STATE:
-      str = "Intersection";
-      break;
+        str = "Intersection";
+        break;
     default:
-      str = "Unknown";
-      break;
+        str = "Unknown";
+        break;
     }
 
     return str;
-  }
+}
 } /* namespace PlannerHNS */
diff --git a/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp b/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp
index 3985a922..8db6358f 100755
--- a/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp
+++ b/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp
@@ -147,13 +147,13 @@ double PlannerH::PlanUsingDP(const WayPoint& start,
   {
     GPSPoint sp = start.pos;
     GPSPoint gp = goalPos.pos;
-    cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map for Start (" <<  sp.ToString() << ") and Goal (" << gp.ToString() << ")" << endl;
+    // cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map for Start (" <<  sp.ToString() << ") and Goal (" << gp.ToString() << ")" << endl;
     return 0;
   }
 
   if(!pStart->pLane || !pGoal->pLane)
   {
-    cout << endl << "Error: PlannerH -> Null Lane, Start (" << pStart->pLane << ") and Goal (" << pGoal->pLane << ")" << endl;
+    // cout << endl << "Error: PlannerH -> Null Lane, Start (" << pStart->pLane << ") and Goal (" << pGoal->pLane << ")" << endl;
     return 0;
   }
 
diff --git a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp
index d4a1ca2d..34cd18e0 100755
--- a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp
+++ b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp
@@ -31,6 +31,7 @@ TrajectoryCost TrajectoryCosts::DoOneStep(const vector<vector<vector<WayPoint> >
     const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
     const std::vector<PlannerHNS::DetectedObject>& obj_list)
 {
+  /*
   TrajectoryCost bestTrajectory;
   bestTrajectory.bBlocked = true;
   bestTrajectory.closest_obj_distance = params.horizonDistance;
@@ -53,6 +54,11 @@ TrajectoryCost TrajectoryCosts::DoOneStep(const vector<vector<vector<WayPoint> >
     }
   }
 
+  std::cout<<"## Initail trajecotry cost"<<std::endl;
+  for(auto it = m_TrajectoryCosts.begin(); it != m_TrajectoryCosts.end(); it++){
+    std::cout<<it->ToString()<<std::endl;
+  }
+
   CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
 
 
@@ -123,6 +129,7 @@ TrajectoryCost TrajectoryCosts::DoOneStep(const vector<vector<vector<WayPoint> >
   m_PrevCostIndex = smallestIndex;
 
   return bestTrajectory;
+  */
 }
 
 void TrajectoryCosts::CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts,
@@ -303,6 +310,8 @@ void TrajectoryCosts::NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts)
       trajectoryCosts.at(ic).longitudinal_cost = trajectoryCosts.at(ic).longitudinal_cost / totalLongitudinalCosts;
     else
       trajectoryCosts.at(ic).longitudinal_cost = 0;
+    
+    std::cout<<"Costs("<<ic<<"): "<<trajectoryCosts.at(ic).priority_cost<<" "<<trajectoryCosts.at(ic).transition_cost<<" "<<trajectoryCosts.at(ic).lane_change_cost<<" "<<trajectoryCosts.at(ic).longitudinal_cost<<std::endl;
 
     trajectoryCosts.at(ic).priority_cost = m_WeightPriority*trajectoryCosts.at(ic).priority_cost;
     trajectoryCosts.at(ic).transition_cost = m_WeightTransition*trajectoryCosts.at(ic).transition_cost;
@@ -310,6 +319,7 @@ void TrajectoryCosts::NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts)
     trajectoryCosts.at(ic).lateral_cost = m_WeightLat*trajectoryCosts.at(ic).lateral_cost;
     trajectoryCosts.at(ic).longitudinal_cost = m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost;
 
+    std::cout<<"Weighted Costs("<<ic<<"): "<<trajectoryCosts.at(ic).priority_cost<<" "<<trajectoryCosts.at(ic).transition_cost<<" "<<trajectoryCosts.at(ic).lane_change_cost<<" "<<trajectoryCosts.at(ic).longitudinal_cost<<std::endl;
 
     trajectoryCosts.at(ic).cost = m_WeightPriority*trajectoryCosts.at(ic).priority_cost/5.0 +
         m_WeightLaneChange*trajectoryCosts.at(ic).lane_change_cost/5.0 +
diff --git a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp
index 3a72dfa8..1f239693 100755
--- a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp
+++ b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp
@@ -5,846 +5,755 @@
 /// \date Jan 14, 2018
 
 #include "op_planner/TrajectoryDynamicCosts.h"
-#include "op_planner/MatrixOperations.h"
 #include "float.h"
+#include "op_planner/MatrixOperations.h"
 
-// #define DEBUG_ENABLE
-
-namespace PlannerHNS
-{
+#define DEBUG_ENABLE
 
+namespace PlannerHNS {
 
-TrajectoryDynamicCosts::TrajectoryDynamicCosts()
-{
-  m_PrevCostIndex = -1;
-  m_PrevSelectedIndex = -1;
-  m_WeightPriority = 0.9;
-  m_WeightTransition = 0.9;
-  m_WeightLong = 1.0;
-  m_WeightLat = 1.2;
-  m_WeightLaneChange = 0.0;
-  m_LateralSkipDistance = 5;
+TrajectoryDynamicCosts::TrajectoryDynamicCosts() {
+    m_PrevCostIndex = -1;
+    m_PrevSelectedIndex = -1;
+    m_WeightPriority = 0.9;
+    m_WeightTransition = 0.9;
+    m_WeightLong = 1.0;
+    m_WeightLat = 1.2;
+    m_WeightLaneChange = 0.0;
+    m_LateralSkipDistance = 5;
 
-  m_startTrajIdx = 0;
-  m_endTrajIdx = 0;
+    m_startTrajIdx = 0;
+    m_endTrajIdx = 0;
 
-  m_CollisionTimeDiff = 6.0; //seconds
-  m_PrevIndex = -1;
+    m_CollisionTimeDiff = 6.0; // seconds
+    m_PrevIndex = -1;
 }
 
-TrajectoryDynamicCosts::~TrajectoryDynamicCosts()
-{
-}
+TrajectoryDynamicCosts::~TrajectoryDynamicCosts() {}
 
-TrajectoryCost TrajectoryDynamicCosts::DoOneStepDynamic(const vector<vector<WayPoint> >& rollOuts,
-    const vector<WayPoint>& totalPaths, const WayPoint& currState,
-    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
-    const std::vector<PlannerHNS::DetectedObject>& obj_list, const int& iCurrentIndex)
-{
-  TrajectoryCost bestTrajectory;
-  bestTrajectory.bBlocked = true;
-  bestTrajectory.closest_obj_distance = params.horizonDistance;
-  bestTrajectory.closest_obj_velocity = 0;
-  bestTrajectory.index = -1;
-
-  double critical_lateral_distance   =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
-  double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
-  double critical_long_back_distance   =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
-
-  int currIndex = -1;
-  if(iCurrentIndex >=0 && iCurrentIndex < rollOuts.size())
-    currIndex  = iCurrentIndex;
-  else
-    currIndex = GetCurrentRollOutIndex(totalPaths, currState, params);
+TrajectoryCost TrajectoryDynamicCosts::DoOneStepDynamic(const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths,
+                                                        const WayPoint &currState, PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                                                        const VehicleState &vehicleState, const std::vector<PlannerHNS::DetectedObject> &obj_list,
+                                                        const int &iCurrentIndex) {
+    /*
+    TrajectoryCost bestTrajectory;
+    bestTrajectory.bBlocked = true;
+    bestTrajectory.closest_obj_distance = params.horizonDistance;
+    bestTrajectory.closest_obj_velocity = 0;
+    bestTrajectory.index = -1;
 
-  InitializeCosts(rollOuts, params);
+    double critical_lateral_distance   =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
+    double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
+    double critical_long_back_distance   =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
 
-  InitializeSafetyPolygon(currState, carInfo, vehicleState, critical_lateral_distance, critical_long_front_distance, critical_long_back_distance);
+    int currIndex = -1;
+    if(iCurrentIndex >=0 && iCurrentIndex < rollOuts.size())
+      currIndex  = iCurrentIndex;
+    else
+      currIndex = GetCurrentRollOutIndex(totalPaths, currState, params);
 
-  CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
+    InitializeCosts(rollOuts, params);
 
-  CalculateLateralAndLongitudinalCostsDynamic(obj_list, rollOuts, totalPaths,  currState, params, carInfo, vehicleState, critical_lateral_distance, critical_long_front_distance, critical_long_back_distance);
+    InitializeSafetyPolygon(currState, carInfo, vehicleState, critical_lateral_distance, critical_long_front_distance, critical_long_back_distance);
 
-  NormalizeCosts(m_TrajectoryCosts);
+    CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
 
-  int smallestIndex = -1;
-  double smallestCost = DBL_MAX;
-  double smallestDistance = DBL_MAX;
-  double velo_of_next = 0;
-  bool bAllFree = true;
+    CalculateLateralAndLongitudinalCostsDynamic(obj_list, rollOuts, totalPaths,  currState, params, carInfo, vehicleState, critical_lateral_distance,
+    critical_long_front_distance, critical_long_back_distance);
 
-  //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
-  for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
-  {
-    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
-    {
-      smallestCost = m_TrajectoryCosts.at(ic).cost;
-      smallestIndex = ic;
-    }
+    NormalizeCosts(m_TrajectoryCosts);
+
+    int smallestIndex = -1;
+    double smallestCost = DBL_MAX;
+    double smallestDistance = DBL_MAX;
+    double velo_of_next = 0;
+    bool bAllFree = true;
 
-    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
+    //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
+    for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
     {
-      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
-      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
-    }
+      if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
+      {
+        smallestCost = m_TrajectoryCosts.at(ic).cost;
+        smallestIndex = ic;
+      }
 
-    if(m_TrajectoryCosts.at(ic).bBlocked)
-      bAllFree = false;
-  }
+      if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
+      {
+        smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
+        velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+      }
 
-  if(bAllFree && smallestIndex >=0)
-    smallestIndex = params.rollOutNumber/2;
+      if(m_TrajectoryCosts.at(ic).bBlocked)
+        bAllFree = false;
+    }
 
+    if(bAllFree && smallestIndex >=0)
+      smallestIndex = params.rollOutNumber/2;
 
-  if(smallestIndex == -1)
-  {
-    bestTrajectory.bBlocked = true;
-    bestTrajectory.lane_index = 0;
-    bestTrajectory.index = m_PrevCostIndex;
-    bestTrajectory.closest_obj_distance = smallestDistance;
-    bestTrajectory.closest_obj_velocity = velo_of_next;
-  }
-  else if(smallestIndex >= 0)
-  {
-    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
-  }
-
-  m_PrevIndex = currIndex;
-
-  //std::cout << "Current Selected Index : " << bestTrajectory.index << std::endl;
-  return bestTrajectory;
-}
 
-TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector<vector<WayPoint> >& rollOuts,
-    const vector<WayPoint>& totalPaths, const WayPoint& currState,
-    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
-    const std::vector<PlannerHNS::DetectedObject>& obj_list, const PlannerHNS::STATE_TYPE& state, const int& iCurrentIndex)
-{
-  TrajectoryCost bestTrajectory;
-  bestTrajectory.bBlocked = true;
-  bestTrajectory.closest_obj_distance = params.horizonDistance;
-  bestTrajectory.closest_obj_velocity = 0;
-  bestTrajectory.index = -1;
-
-  // get parameter
-  m_WeightPriority = params.weightPriority;
-  m_WeightTransition = params.weightTransition;
-  m_WeightLong = params.weightLong;
-  m_WeightLat = params.weightLat;
-  m_LateralSkipDistance = params.LateralSkipDistance;
-
-  RelativeInfo car_info;
-  PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
-
-  if(car_info.perp_point.LeftLnId == 100){
-    m_startTrajIdx = rollOuts.size() / 2;
-    m_endTrajIdx = rollOuts.size() / 2;
-  }
-  else if(car_info.perp_point.LeftLnId > car_info.perp_point.RightLnId){
-    m_endTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1);
-    m_startTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1);
-  }
-  else{
-    m_startTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1);
-    m_endTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1);
-  }
-
-  double minDistanceToRollOut = 0;
-  int currIndex = 0;
-
-  for(int i=0; i<rollOuts.size(); i++){
-    const PlannerHNS::WayPoint rollout_start_waypoint = rollOuts.at(i).at(std::min(3, int(rollOuts.at(i).size()))-1);
-
-    double direct_distance = hypot(rollout_start_waypoint.pos.y - currState.pos.y, rollout_start_waypoint.pos.x - currState.pos.x);
-
-    if(minDistanceToRollOut == 0 || minDistanceToRollOut > direct_distance){
-      minDistanceToRollOut = direct_distance;
-      currIndex = i;
+    if(smallestIndex == -1)
+    {
+      bestTrajectory.bBlocked = true;
+      bestTrajectory.lane_index = 0;
+      bestTrajectory.index = m_PrevCostIndex;
+      bestTrajectory.closest_obj_distance = smallestDistance;
+      bestTrajectory.closest_obj_velocity = velo_of_next;
     }
-  }
-  
-  //std::cout <<  "Current Index: " << currIndex << std::endl;
-
-  // Calculate lane change cost: Scoring the cost by the distance between current path and candidate path
-  m_TrajectoryCosts.clear();
-  if(rollOuts.size()>0)
-  {
-    TrajectoryCost tc;
-    int centralIndex = params.rollOutNumber/2;
-    tc.lane_index = 0;
-    for(unsigned int it=0; it< rollOuts.size(); it++)
+    else if(smallestIndex >= 0)
     {
-      tc.index = it;
-      tc.relative_index = it - centralIndex; // index based on center (central index is 0)
-      tc.distance_from_center = params.rollOutDensity*tc.relative_index;
-      tc.priority_cost = fabs(tc.distance_from_center); // priority_cost = distance from center
-      tc.closest_obj_distance = params.horizonDistance;
-      if(rollOuts.at(it).size() > 0)
-          tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
-      m_TrajectoryCosts.push_back(tc);
+      bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
     }
-  }
 
-  // std::cout << m_PrevSelectedIndex << std::endl;
+    m_PrevIndex = currIndex;
 
-  if(m_PrevSelectedIndex == -1){
-    CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
-  }
-  else{
-    CalculateTransitionCosts(m_TrajectoryCosts, m_PrevSelectedIndex, params);
-  }
-  //end
-
-  // Calculate trajectory cost with object
-  WayPoint p;
-  m_AllContourPoints.clear();
-  for(unsigned int io=0; io<obj_list.size(); io++) // Object in object list
-  {
-    // for(unsigned int icon=0; icon < obj_list.at(io).contour.size(); icon++) // Point in contour
-    // {
-    //   p.pos = obj_list.at(io).contour.at(icon);
-    //   p.v = obj_list.at(io).center.v;
-    //   p.id = io;
-    //   p.cost = sqrt(obj_list.at(io).w*obj_list.at(io).w + obj_list.at(io).l*obj_list.at(io).l);
-    //   m_AllContourPoints.push_back(p);
-    // }
-
-    // Only add centroid
-    p.pos = obj_list.at(io).center.pos;
-    p.v = obj_list.at(io).center.v;
-    p.id = io;
-    m_AllContourPoints.push_back(p);
-  }
+    //std::cout << "Current Selected Index : " << bestTrajectory.index << std::endl;
+    return bestTrajectory;
+    */
+}
 
-  CalculateLateralAndLongitudinalCostsStatic(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);
+TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths,
+                                                       const WayPoint &currState, PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                                                       const VehicleState &vehicleState, const std::vector<PlannerHNS::DetectedObject> &obj_list,
+                                                       const PlannerHNS::STATE_TYPE &state, const int &iCurrentIndex) {
+    TrajectoryCost bestTrajectory;
+    bestTrajectory.bBlocked = false;
+    bestTrajectory.closest_obj_distance = params.horizonDistance;
+    bestTrajectory.closest_obj_velocity = 0;
+    bestTrajectory.index = -1;
+
+    // get parameter
+    m_WeightPriority = params.weightPriority;
+    m_WeightTransition = params.weightTransition;
+    m_WeightLong = params.weightLong;
+    m_WeightLat = params.weightLat;
+    m_LateralSkipDistance = params.LateralSkipDistance;
 
-  NormalizeCosts(m_TrajectoryCosts);
+    RelativeInfo car_info;
+    PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
 
-  int smallestIndex = -1;
-  double smallestCost = DBL_MAX;
-  double smallestDistance = DBL_MAX;
-  double velo_of_next = 0;
+    // M
+    // if(car_info.perp_point.LeftLnId == 100){
+    //   m_startTrajIdx = rollOuts.size() / 2;
+    //   m_endTrajIdx = rollOuts.size() / 2;
+    // }
+    // else if(car_info.perp_point.LeftLnId > car_info.perp_point.RightLnId){
+    //   m_endTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1);
+    //   m_startTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1);
+    // }
+    // else{
+    //   m_startTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1);
+    //   m_endTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1);
+    // }
 
-  bool bAllFree = true;
+    double minDistanceToRollOut = 0;
+    int currIndex = 0;
+    currIndex = GetCurrentRollOutIndex(totalPaths, currState, params);
 
-  for(int ic = 0; ic < m_startTrajIdx; ic++) m_TrajectoryCosts.at(ic).bBlocked = true;
-  for(int ic = rollOuts.size() - 1; ic > m_endTrajIdx; ic--) m_TrajectoryCosts.at(ic).bBlocked = true;
+    // Calculate lane change cost: Scoring the cost by the distance between current path and candidate path
+    int centralIndex = params.rollOutNumber / 2;
+    if (rollOuts.size() % 2 == 0)
+        centralIndex--;
+
+    m_TrajectoryCosts.clear();
+    for (unsigned int it = 0; it < rollOuts.size(); it++) {
+        TrajectoryCost tc;
+        tc.lane_index = it;
+        tc.index = it;
+        tc.relative_index = it - centralIndex; // index based on center (central index is 0)
+        tc.distance_from_center = params.rollOutDensity * tc.relative_index;
+        tc.priority_cost = fabs(tc.distance_from_center); // priority_cost = distance from center
+        tc.closest_obj_distance = params.horizonDistance;
+
+        if (it == params.blockIdx) {
+            tc.cost = 99999.0;
+            tc.bBlocked = true;
+        }
 
-  // for(unsigned int ic = std::max(currIndex - 1, m_startTrajIdx); ic <= std::min(currIndex + 1, m_endTrajIdx); ic++)
-  for(unsigned int ic = m_startTrajIdx; ic <= m_endTrajIdx; ic++)
-  {
-    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
-    {
-      #ifdef DEBUG_ENABLE
-      std::cout << "smallestIndex is Updated" << std::endl;
-      #endif
-      smallestCost = m_TrajectoryCosts.at(ic).cost;
-      smallestIndex = ic;
+        if (rollOuts.at(it).size() > 0)
+            tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
+        m_TrajectoryCosts.push_back(tc);
     }
 
-    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
-    {
-      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
-      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+    if (m_PrevSelectedIndex == -1) {
+        CalculateTransitionCosts(m_TrajectoryCosts, centralIndex, params);
+    } else {
+        CalculateTransitionCosts(m_TrajectoryCosts, m_PrevSelectedIndex, params);
     }
 
-    if(m_TrajectoryCosts.at(ic).lateral_cost > 0){
-      bAllFree = false;
+    // Calculate trajectory cost with object
+    WayPoint p;
+    m_AllContourPoints.clear();
+    for (unsigned int io = 0; io < obj_list.size(); io++) // Object in object list
+    {
+        // Only add centroid
+        p.pos = obj_list.at(io).center.pos;
+        p.v = obj_list.at(io).center.v;
+        p.id = io;
+        m_AllContourPoints.push_back(p);
     }
-  }
-
-  // Enable when needed
-  // if(bAllFree && smallestIndex >= 0){
-  //   smallestIndex = params.rollOutNumber/2;
-  // }
-
-  ////// Change Lane when turn left / right
-  // // Calculate Turn Angle
-  // double turn_angle = 0;
-
-  // if(m_PrevSelectedIndex != -1)
-  //   turn_angle = CalculateTurnAngle(rollOuts.at(m_PrevSelectedIndex), currState, 50);
-
-  // // std::cout << "b_all_free : " << bAllFree << " , t_a : " << turn_angle << std::endl;
-
-  // // Keep state when Intersection state
-  // if(state == PlannerHNS::INTERSECTION_STATE){
-  //   smallestIndex = m_PrevSelectedIndex;
-  // }
-  // // For Left Turn
-  // else if(bAllFree && turn_angle > 45){
-  //   smallestIndex = 0;
-  // }
-  // // For Right Turn
-  // else if(bAllFree && turn_angle < -45 && params.rollOutNumber > 0){
-  //   smallestIndex = params.rollOutNumber - 1;
-  // }
-
-  // if(smallestIndex >= 0 && m_TrajectoryCosts.at(currIndex).bBlocked){
-  //   int left_block_idx = -1;
-
-  //   for(int ic = currIndex - 1; ic >= m_startTrajIdx; ic--)
-  //   {
-  //     if(m_TrajectoryCosts.at(ic).bBlocked){
-  //       left_block_idx = ic;
-  //       break;
-  //     }
-  //   }
-
-  //   if(left_block_idx != -1 && smallestIndex < left_block_idx){
-  //     smallestIndex = -1;
-  //   }
-
-  //   int right_block_idx = -1;
-
-  //   for(int ic = currIndex + 1; ic <= m_endTrajIdx; ic++)
-  //   {
-  //     if(m_TrajectoryCosts.at(ic).bBlocked){
-  //       right_block_idx = ic;
-  //       break;
-  //     }
-  //   }
-
-  //   if(right_block_idx != -1 && smallestIndex > right_block_idx){
-  //     smallestIndex = -1;
-  //   }
-  // }
-  // Hyundai challenge legacy
-
-  if(smallestIndex == -1)
-  {
-    bestTrajectory.bBlocked = true;
-    bestTrajectory.lane_index = m_PrevSelectedIndex;
-    bestTrajectory.index = m_PrevSelectedIndex;
-    bestTrajectory.closest_obj_distance = smallestDistance;
-    bestTrajectory.closest_obj_velocity = velo_of_next;
-  }
-  else if(smallestIndex >= 0)
-  {
-    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
-    m_PrevSelectedIndex = smallestIndex;
-  }
-
-  m_PrevIndex = currIndex;
-
-  #ifdef DEBUG_ENABLE
-  for(unsigned int ic=0; ic<rollOuts.size(); ic++){
-    std::cout << "Index: " << ic
-           << ", Priority: " << m_TrajectoryCosts.at(ic).priority_cost
-           << ", Transition: " << m_TrajectoryCosts.at(ic).transition_cost
-           << ", Lat: " << m_TrajectoryCosts.at(ic).lateral_cost
-           << ", Long: " << m_TrajectoryCosts.at(ic).longitudinal_cost
-           << ", Change: " << m_TrajectoryCosts.at(ic).lane_change_cost
-           << ", Avg: " << m_TrajectoryCosts.at(ic).cost
-           << ", Blocked : " << m_TrajectoryCosts.at(ic).bBlocked
-           << std::endl;
-  }
-  std::cout << "---------------------------------------" << std::endl;
-  std::cout << "leftLnId : " << car_info.perp_point.LeftLnId << ", RightLnId : " << car_info.perp_point.RightLnId << std::endl;
-  std::cout << "start_idx : " << m_startTrajIdx << ", end_idx : " << m_endTrajIdx << std::endl;
-  std::cout << "current_idx : " << currIndex << ", selected one : " << smallestIndex << std::endl;
-  std::cout << "---------------------------------------" << std::endl;
-
-  #endif
-
-  return bestTrajectory;
-}
 
-TrajectoryCost TrajectoryDynamicCosts::DoOneStep(const vector<vector<vector<WayPoint> > >& rollOuts,
-    const vector<vector<WayPoint> >& totalPaths, const WayPoint& currState, const int& currIndex,
-    const int& currLaneIndex,
-    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
-    const std::vector<PlannerHNS::DetectedObject>& obj_list)
-{
-  TrajectoryCost bestTrajectory;
-  bestTrajectory.bBlocked = true;
-  bestTrajectory.closest_obj_distance = params.horizonDistance;
-  bestTrajectory.closest_obj_velocity = 0;
-  bestTrajectory.index = -1;
+    CalculateLateralAndLongitudinalCostsStatic(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);
+    NormalizeCosts(m_TrajectoryCosts, params.enableDebug);
 
-  if(!ValidateRollOutsInput(rollOuts) || rollOuts.size() != totalPaths.size()) return bestTrajectory;
+    int smallestIndex = -1;
+    double smallestCost = DBL_MAX;
+    double smallestDistance = DBL_MAX;
+    double velo_of_next = 0;
 
-  if(m_PrevCostIndex == -1)
-    m_PrevCostIndex = params.rollOutNumber/2;
+    bool bAllFree = true;
 
-  m_TrajectoryCosts.clear();
+    // M
+    // for(int ic = 0; ic < m_startTrajIdx; ic++) m_TrajectoryCosts.at(ic).bBlocked = true;
+    // for(int ic = rollOuts.size() - 1; ic > m_endTrajIdx; ic--) m_TrajectoryCosts.at(ic).bBlocked = true;
 
-  for(unsigned int il = 0; il < rollOuts.size(); il++)
-  {
-    if(rollOuts.at(il).size()>0 && rollOuts.at(il).at(0).size()>0)
-    {
-      vector<TrajectoryCost> costs = CalculatePriorityAndLaneChangeCosts(rollOuts.at(il), il, params);
-      m_TrajectoryCosts.insert(m_TrajectoryCosts.end(), costs.begin(), costs.end());
-    }
-  }
+    // for(unsigned int ic = std::max(currIndex - 1, m_startTrajIdx); ic <= std::min(currIndex + 1, m_endTrajIdx); ic++)
+    // for(unsigned int ic = m_startTrajIdx; ic <= m_endTrajIdx; ic++)
 
-  CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
+    bool is_current_path_blocked = false;
 
+    for (unsigned int ic = 0; ic < rollOuts.size(); ic++) {
+        if (!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost) {
+            if (params.enableDebug)
+                std::cout << "smallestIndex is Updated" << std::endl;
+            smallestCost = m_TrajectoryCosts.at(ic).cost;
+            smallestIndex = ic;
+        }
 
-  WayPoint p;
-  m_AllContourPoints.clear();
-  for(unsigned int io=0; io<obj_list.size(); io++)
-  {
-    for(unsigned int icon=0; icon < obj_list.at(io).contour.size(); icon++)
-    {
-      p.pos = obj_list.at(io).contour.at(icon);
-      p.v = obj_list.at(io).center.v;
-      p.id = io;
-      p.cost = sqrt(obj_list.at(io).w*obj_list.at(io).w + obj_list.at(io).l*obj_list.at(io).l);
-      m_AllContourPoints.push_back(p);
-    }
-  }
-
-  CalculateLateralAndLongitudinalCosts(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);
-
-  NormalizeCosts(m_TrajectoryCosts);
+        if (m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance) {
+            smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
+            velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+        }
 
-  int smallestIndex = -1;
-  double smallestCost = DBL_MAX;
-  double smallestDistance = DBL_MAX;
-  double velo_of_next = 0;
+        if (ic == currIndex && m_TrajectoryCosts.at(ic).bBlocked == true)
+            is_current_path_blocked = true;
 
-  //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
-  for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
-  {
-    //cout << m_TrajectoryCosts.at(ic).ToString();
-    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
-    {
-      smallestCost = m_TrajectoryCosts.at(ic).cost;
-      smallestIndex = ic;
+        if (m_TrajectoryCosts.at(ic).lateral_cost > 0) {
+            bAllFree = false;
+        }
     }
 
-    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
-    {
-      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
-      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+    static int path_keeping_cnt = 80, cur_path_blocking_cnt = 80;
+    if (m_PrevIndex == currIndex) {
+        path_keeping_cnt--;
+        if (path_keeping_cnt < 0)
+            path_keeping_cnt = 0;
+    } else
+        path_keeping_cnt = 80;
+
+    for (unsigned int ic = 0; ic < rollOuts.size(); ic++) {
+        if (m_TrajectoryCosts.at(ic).cost > params.blockThreshold) {
+            m_TrajectoryCosts.at(ic).bBlocked = true;
+        }
     }
-  }
-
-  //cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;
-
-  //All is blocked !
-  if(smallestIndex == -1 && m_PrevCostIndex < (int)m_TrajectoryCosts.size())
-  {
-    bestTrajectory.bBlocked = true;
-    bestTrajectory.lane_index = currLaneIndex;
-    bestTrajectory.index = currIndex;
-    bestTrajectory.closest_obj_distance = smallestDistance;
-    bestTrajectory.closest_obj_velocity = velo_of_next;
-    //bestTrajectory.index = smallestIndex;
-  }
-  else if(smallestIndex >= 0)
-  {
-    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
-    //bestTrajectory.index = smallestIndex;
-  }
-
-//  cout << "smallestI: " <<  smallestIndex << ", C_Size: " << m_TrajectoryCosts.size()
-//      << ", LaneI: " << bestTrajectory.lane_index << "TrajI: " << bestTrajectory.index
-//      << ", prevSmalI: " << m_PrevCostIndex << ", distance: " << bestTrajectory.closest_obj_distance
-//      << ", Blocked: " << bestTrajectory.bBlocked
-//      << endl;
-
-  m_PrevCostIndex = smallestIndex;
-
-  return bestTrajectory;
-}
 
-void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector<TrajectoryCost>& trajectoryCosts,
-    const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
-    const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params,
-    const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState)
-{
-  double critical_lateral_distance =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
-  double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
-  double critical_long_back_distance =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
+    // Change lane if current path is blocked or current path is not center and the ego keeps lane 1 sceonds(When rate is 20)
+    if ((is_current_path_blocked) || (smallestIndex != params.rollOutNumber / 2 && path_keeping_cnt == 0)) {
+        if (smallestIndex == -1) {
+            bestTrajectory.bBlocked = true;
+            bestTrajectory.lane_index = m_PrevSelectedIndex;
+            bestTrajectory.index = m_PrevSelectedIndex;
+            bestTrajectory.closest_obj_distance = smallestDistance;
+            bestTrajectory.closest_obj_velocity = velo_of_next;
+        } else if (smallestIndex >= 0) {
+            bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
+            bestTrajectory.index = smallestIndex;
+            bestTrajectory.lane_index = smallestIndex;
+            m_PrevSelectedIndex = smallestIndex;
+        }
+    } else {
+        bestTrajectory.index = smallestIndex;
+        bestTrajectory.lane_index = smallestIndex;
+    }
 
-  PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2);
-  PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
+    m_PrevIndex = currIndex;
 
-  double corner_slide_distance = critical_lateral_distance/2.0;
-  double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle;
-  double slide_distance = vehicleState.steer * ratio_to_angle;
+    if (params.enableDebug) {
+        for (unsigned int ic = 0; ic < rollOuts.size(); ic++) {
+            std::cout << "Index: " << ic << ", Priority: " << m_TrajectoryCosts.at(ic).priority_cost
+                      << ", Transition: " << m_TrajectoryCosts.at(ic).transition_cost << ", Lat: " << m_TrajectoryCosts.at(ic).lateral_cost
+                      << ", Long: " << m_TrajectoryCosts.at(ic).longitudinal_cost << ", Change: " << m_TrajectoryCosts.at(ic).lane_change_cost
+                      << ", Avg: " << m_TrajectoryCosts.at(ic).cost << ", Blocked : " << m_TrajectoryCosts.at(ic).bBlocked << std::endl;
+        }
+        std::cout << "---------------------------------------" << std::endl;
+        std::cout << "leftLnId : " << car_info.perp_point.LeftLnId << ", RightLnId : " << car_info.perp_point.RightLnId << std::endl;
+        std::cout << "start_idx : " << m_startTrajIdx << ", end_idx : " << m_endTrajIdx << std::endl;
+        std::cout << "Path keeping cnt: " << path_keeping_cnt << ", prev_idx : " << m_PrevIndex << ", current_idx : " << currIndex
+                  << ", selected one : " << smallestIndex << std::endl;
+        std::cout << "---------------------------------------" << std::endl;
+    }
 
-  GPSPoint bottom_left(-critical_lateral_distance ,-critical_long_back_distance,  currState.pos.z, 0);
-  GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance,  currState.pos.z, 0);
+    return bestTrajectory;
+}
 
-  GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0,  currState.pos.z, 0);
-  GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0);
+TrajectoryCost TrajectoryDynamicCosts::DoOneStep(const vector<vector<vector<WayPoint>>> &rollOuts, const vector<vector<WayPoint>> &totalPaths,
+                                                 const WayPoint &currState, const int &currIndex, const int &currLaneIndex, PlanningParams &params,
+                                                 const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
+                                                 const std::vector<PlannerHNS::DetectedObject> &obj_list) {
+    /*
+    TrajectoryCost bestTrajectory;
+    bestTrajectory.bBlocked = true;
+    bestTrajectory.closest_obj_distance = params.horizonDistance;
+    bestTrajectory.closest_obj_velocity = 0;
+    bestTrajectory.index = -1;
 
-  GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance,  currState.pos.z, 0);
-  GPSPoint top_left(-critical_lateral_distance - slide_distance , critical_long_front_distance, currState.pos.z, 0);
+    if(!ValidateRollOutsInput(rollOuts) || rollOuts.size() != totalPaths.size()) return bestTrajectory;
 
-  bottom_left = invRotationMat*bottom_left;
-  bottom_left = invTranslationMat*bottom_left;
+    if(m_PrevCostIndex == -1)
+      m_PrevCostIndex = params.rollOutNumber/2;
 
-  top_right = invRotationMat*top_right;
-  top_right = invTranslationMat*top_right;
+    m_TrajectoryCosts.clear();
 
-  bottom_right = invRotationMat*bottom_right;
-  bottom_right = invTranslationMat*bottom_right;
+    for(unsigned int il = 0; il < rollOuts.size(); il++)
+    {
+      if(rollOuts.at(il).size()>0 && rollOuts.at(il).at(0).size()>0)
+      {
+        vector<TrajectoryCost> costs = CalculatePriorityAndLaneChangeCosts(rollOuts.at(il), il, params);
+        m_TrajectoryCosts.insert(m_TrajectoryCosts.end(), costs.begin(), costs.end());
+      }
+    }
 
-  top_left = invRotationMat*top_left;
-  top_left = invTranslationMat*top_left;
+    CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
 
-  top_right_car = invRotationMat*top_right_car;
-  top_right_car = invTranslationMat*top_right_car;
 
-  top_left_car = invRotationMat*top_left_car;
-  top_left_car = invTranslationMat*top_left_car;
+    WayPoint p;
+    m_AllContourPoints.clear();
+    for(unsigned int io=0; io<obj_list.size(); io++)
+    {
+      for(unsigned int icon=0; icon < obj_list.at(io).contour.size(); icon++)
+      {
+        p.pos = obj_list.at(io).contour.at(icon);
+        p.v = obj_list.at(io).center.v;
+        p.id = io;
+        p.cost = sqrt(obj_list.at(io).w*obj_list.at(io).w + obj_list.at(io).l*obj_list.at(io).l);
+        m_AllContourPoints.push_back(p);
+      }
+    }
 
-  m_SafetyBorder.points.clear();
-  m_SafetyBorder.points.push_back(bottom_left) ;
-  m_SafetyBorder.points.push_back(bottom_right) ;
-  m_SafetyBorder.points.push_back(top_right_car) ;
-  m_SafetyBorder.points.push_back(top_right) ;
-  m_SafetyBorder.points.push_back(top_left) ;
-  m_SafetyBorder.points.push_back(top_left_car);
+    CalculateLateralAndLongitudinalCosts(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);
 
-  #ifdef DEBUG_ENABLE
-    std::cout << "points num : " << contourPoints.size() << std::endl;
-  #endif
+    NormalizeCosts(m_TrajectoryCosts, params.enableDebug);
 
-  int iCostIndex = 0;
-  if(rollOuts.size() > 0 && rollOuts.at(0).size()>0)
-  {
-    RelativeInfo car_info;
-    PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
+    int smallestIndex = -1;
+    double smallestCost = DBL_MAX;
+    double smallestDistance = DBL_MAX;
+    double velo_of_next = 0;
 
-    for(unsigned int it=0; it< rollOuts.size(); it++)
+    for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
     {
-      #ifdef DEBUG_ENABLE
-      int unskipped = 0;
-      #endif
-
-      int skip_id = -1;
-      for(unsigned int icon = 0; icon < contourPoints.size(); icon++)
+      if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
       {
-        // if(skip_id == contourPoints.at(icon).id)
-        //   continue;
-
-        RelativeInfo obj_info;
-        PlanningHelpers::GetRelativeInfo(totalPaths, contourPoints.at(icon), obj_info);
-        double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, obj_info); // Caculate cumulated distance between car and obj in path
-        if(obj_info.iFront == 0 && longitudinalDist > 0)
-          longitudinalDist = -longitudinalDist;
-
-        // double direct_distance = hypot(obj_info.perp_point.pos.y-contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x-contourPoints.at(icon).pos.x); // Caculate direct distance
+        smallestCost = m_TrajectoryCosts.at(ic).cost;
+        smallestIndex = ic;
+      }
 
-        // Original
-        // if(contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance+contourPoints.at(icon).cost))
-        // {
-        //   skip_id = contourPoints.at(icon).id;
-        //   continue;
-        // }
+      if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
+      {
+        smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
+        velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+      }
+    }
 
-        // std::cout << obj_info.perp_distance << std::endl;
-        // std::cout << rollOuts.size() << " " << rollOuts.size() /2 << std::endl;
-        // std::cout << ((rollOuts.size() / 2) * params.rollOutDensity) * (-1) << std::endl;
+    //All is blocked !
+    if(smallestIndex == -1 && m_PrevCostIndex < (int)m_TrajectoryCosts.size())
+    {
+      bestTrajectory.bBlocked = true;
+      bestTrajectory.lane_index = currLaneIndex;
+      bestTrajectory.index = currIndex;
+      bestTrajectory.closest_obj_distance = smallestDistance;
+      bestTrajectory.closest_obj_velocity = velo_of_next;
+    }
+    else if(smallestIndex >= 0)
+    {
+      bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
+    }
 
-        double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center; // Disetance between center and trajectory
+    m_PrevCostIndex = smallestIndex;
 
-        double lateralDist = fabs(obj_info.perp_distance - distance_from_center);
+    return bestTrajectory;
+    */
+}
 
-        // std::cout << "long : " << longitudinalDist << ", lat : " << lateralDist << std::endl;
+void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector<TrajectoryCost> &trajectoryCosts,
+                                                                        const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths,
+                                                                        const WayPoint &currState, const vector<WayPoint> &contourPoints,
+                                                                        PlanningParams &params, const CAR_BASIC_INFO &carInfo,
+                                                                        const VehicleState &vehicleState) {
+    double critical_lateral_distance = carInfo.width / 2.0 + params.horizontalSafetyDistancel;
+    double critical_long_front_distance = carInfo.wheel_base / 2.0 + carInfo.length / 2.0 + params.verticalSafetyDistance;
+    double critical_long_back_distance = carInfo.length / 2.0 + params.verticalSafetyDistance - carInfo.wheel_base / 2.0;
 
-        if(longitudinalDist < 0 ||
-          longitudinalDist > 30 ||
-          obj_info.perp_distance < (((rollOuts.size() - 1) / 2) * params.rollOutDensity + critical_lateral_distance / 2) * (-1) ||
-          obj_info.perp_distance > ((rollOuts.size() - 1) / 2 + 1) * params.rollOutDensity + critical_lateral_distance / 2)
-          // obj_info.perp_distance < (((rollOuts.size() - 1) / 2) * params.rollOutDensity) * (-1) ||
-          // obj_info.perp_distance > ((rollOuts.size() - 1) / 2 + 1) * params.rollOutDensity)
-        {
-          continue;
+    PlannerHNS::Mat3 invRotationMat(currState.pos.a - M_PI_2);
+    PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
+
+    double corner_slide_distance = critical_lateral_distance / 2.0;
+    double ratio_to_angle = corner_slide_distance / carInfo.max_steer_angle;
+    double slide_distance = vehicleState.steer * ratio_to_angle;
+
+    GPSPoint bottom_left(-critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);
+    GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);
+
+    GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
+    GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
+
+    GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);
+    GPSPoint top_left(-critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);
+
+    bottom_left = invRotationMat * bottom_left;
+    bottom_left = invTranslationMat * bottom_left;
+
+    top_right = invRotationMat * top_right;
+    top_right = invTranslationMat * top_right;
+
+    bottom_right = invRotationMat * bottom_right;
+    bottom_right = invTranslationMat * bottom_right;
+
+    top_left = invRotationMat * top_left;
+    top_left = invTranslationMat * top_left;
+
+    top_right_car = invRotationMat * top_right_car;
+    top_right_car = invTranslationMat * top_right_car;
+
+    top_left_car = invRotationMat * top_left_car;
+    top_left_car = invTranslationMat * top_left_car;
+
+    m_SafetyBorder.points.clear();
+    m_SafetyBorder.points.push_back(bottom_left);
+    m_SafetyBorder.points.push_back(bottom_right);
+    m_SafetyBorder.points.push_back(top_right_car);
+    m_SafetyBorder.points.push_back(top_right);
+    m_SafetyBorder.points.push_back(top_left);
+    m_SafetyBorder.points.push_back(top_left_car);
+
+    if (params.enableDebug)
+        std::cout << "points num : " << contourPoints.size() << std::endl;
+
+    int iCostIndex = 0;
+    int blockCnt = 0;
+    if (rollOuts.size() > 0 && rollOuts.at(0).size() > 0) {
+        RelativeInfo car_info;
+        PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
+
+        for (unsigned int it = 0; it < rollOuts.size(); it++) {
+            // int unskipped = 0;
+
+            int skip_id = -1;
+            for (unsigned int icon = 0; icon < contourPoints.size(); icon++) {
+                // if(skip_id == contourPoints.at(icon).id)
+                //   continue;
+
+                RelativeInfo obj_info;
+                PlanningHelpers::GetRelativeInfo(totalPaths, contourPoints.at(icon), obj_info);
+                double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(
+                    totalPaths, car_info, obj_info); // Caculate cumulated distance between car and obj in path
+                if (obj_info.iFront == 0 && longitudinalDist > 0)
+                    longitudinalDist = -longitudinalDist;
+
+                // double direct_distance = hypot(obj_info.perp_point.pos.y-contourPoints.at(icon).pos.y,
+                // obj_info.perp_point.pos.x-contourPoints.at(icon).pos.x); // Caculate direct distance
+
+                // Original
+                // if(contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance+contourPoints.at(icon).cost))
+                // {
+                //   skip_id = contourPoints.at(icon).id;
+                //   continue;
+                // }
+
+                // std::cout << obj_info.perp_distance << std::endl;
+                // std::cout << rollOuts.size() << " " << rollOuts.size() /2 << std::endl;
+                // std::cout << ((rollOuts.size() / 2) * params.rollOutDensity) * (-1) << std::endl;
+
+                double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center; // Disetance between center and trajectory
+
+                double lateralDist = fabs(obj_info.perp_distance - distance_from_center);
+
+                // std::cout << "long : " << longitudinalDist << ", lat : " << lateralDist << std::endl;
+
+                // *** Debug *** //
+                /*
+                if (longitudinalDist < 0 || longitudinalDist > 30 ||
+                    obj_info.perp_distance < (((rollOuts.size() - 1) / 2) * params.rollOutDensity + critical_lateral_distance / 2) * (-1) ||
+                    obj_info.perp_distance > ((rollOuts.size() - 1) / 2 + 1) * params.rollOutDensity + critical_lateral_distance / 2)
+                // obj_info.perp_distance < (((rollOuts.size() - 1) / 2) * params.rollOutDensity) * (-1) ||
+                // obj_info.perp_distance > ((rollOuts.size() - 1) / 2 + 1) * params.rollOutDensity)
+                {
+                    continue;
+                }
+                */
+
+                // if(params.enableDebug) unskipped++;
+
+                // Original
+                // if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
+                // {
+                //   continue;
+                // }
+
+                if (m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true) {
+                    trajectoryCosts.at(iCostIndex).bBlocked = true;
+                }
+
+                if (lateralDist <= params.lateralBlockingThreshold && longitudinalDist < params.frontLongitudinalBlockingThreshold &&
+                    longitudinalDist >= params.rearLongitudinalBlockingThreshold) {
+                    trajectoryCosts.at(it).bBlocked = true;
+                }
+
+                // Original
+                // if(lateralDist <= critical_lateral_distance
+                //     && longitudinalDist >= -carInfo.length/1.5
+                //     && longitudinalDist < params.minFollowingDistance)
+                //   trajectoryCosts.at(iCostIndex).bBlocked = true;
+
+                if (lateralDist != 0)
+                    trajectoryCosts.at(iCostIndex).lateral_cost += 1.0 / lateralDist;
+
+                if (longitudinalDist != 0)
+                    trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0 / fabs(longitudinalDist);
+
+                if (longitudinalDist > 0 && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance) {
+                    trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
+                    trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
+                }
+            }
+
+            // if(params.enableDebug){
+            //   std::cout << trajectoryCosts.at(iCostIndex).longitudinal_cost << " " << trajectoryCosts.at(iCostIndex).lateral_cost << ", ";
+            //   std::cout << unskipped << " ";
+            // }
+
+            // Calculate lateral/logitudinal cost, disdtance and velocity
+            if (trajectoryCosts.at(iCostIndex).bBlocked) {
+                blockCnt++;
+            }
+            iCostIndex++;
         }
 
-        #ifdef DEBUG_ENABLE
-        unskipped++;
-        #endif
-
-        // Original
-        // if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
-        // {
-        //   continue;
-        // }
-
-        if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
-          trajectoryCosts.at(iCostIndex).bBlocked = true;
-
-        // Disabled bj hjw
-        if(lateralDist <= 1.5
-            && longitudinalDist >= -5
-            && longitudinalDist < 30)
-          trajectoryCosts.at(it).bBlocked = true;
-
-        // Original
-        // if(lateralDist <= critical_lateral_distance
-        //     && longitudinalDist >= -carInfo.length/1.5
-        //     && longitudinalDist < params.minFollowingDistance)
-        //   trajectoryCosts.at(iCostIndex).bBlocked = true;
-
-        if(lateralDist != 0)
-          trajectoryCosts.at(iCostIndex).lateral_cost += 1.0/lateralDist;
-
-        if(longitudinalDist != 0)
-          trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0/fabs(longitudinalDist);
-
-        if(longitudinalDist > 0 && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance)
-        {
-          trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
-          trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
+        if (blockCnt == rollOuts.size()) {
+            params.enableStop = true;
         }
-      }
 
-      #ifdef DEBUG_ENABLE
-      // std::cout << trajectoryCosts.at(iCostIndex).longitudinal_cost << " " << trajectoryCosts.at(iCostIndex).lateral_cost << ", ";
-      std::cout << unskipped << " ";
-      #endif
-
-      // Calculate lateral/logitudinal cost, disdtance and velocity
-      iCostIndex++;
+        if (params.enableDebug)
+            std::cout << std::endl;
     }
-    #ifdef DEBUG_ENABLE
-    std::cout << std::endl;
-    #endif
-  }
 }
 
-void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts,
-    const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths,
-    const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params,
-    const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState)
-{
-  double critical_lateral_distance =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
-  double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
-  double critical_long_back_distance =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
-  int iCostIndex = 0;
-
-  PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2);
-  PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
+void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost> &trajectoryCosts,
+                                                                  const vector<vector<vector<WayPoint>>> &rollOuts,
+                                                                  const vector<vector<WayPoint>> &totalPaths, const WayPoint &currState,
+                                                                  const vector<WayPoint> &contourPoints, PlanningParams &params,
+                                                                  const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState) {
+    double critical_lateral_distance = carInfo.width / 2.0 + params.horizontalSafetyDistancel;
+    double critical_long_front_distance = carInfo.wheel_base / 2.0 + carInfo.length / 2.0 + params.verticalSafetyDistance;
+    double critical_long_back_distance = carInfo.length / 2.0 + params.verticalSafetyDistance - carInfo.wheel_base / 2.0;
+    int iCostIndex = 0;
 
-  double corner_slide_distance = critical_lateral_distance/2.0;
-  double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle;
-  double slide_distance = vehicleState.steer * ratio_to_angle;
+    PlannerHNS::Mat3 invRotationMat(currState.pos.a - M_PI_2);
+    PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
 
-  GPSPoint bottom_left(-critical_lateral_distance ,-critical_long_back_distance,  currState.pos.z, 0);
-  GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance,  currState.pos.z, 0);
+    double corner_slide_distance = critical_lateral_distance / 2.0;
+    double ratio_to_angle = corner_slide_distance / carInfo.max_steer_angle;
+    double slide_distance = vehicleState.steer * ratio_to_angle;
 
-  GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0,  currState.pos.z, 0);
-  GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0);
+    GPSPoint bottom_left(-critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);
+    GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0);
 
-  GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance,  currState.pos.z, 0);
-  GPSPoint top_left(-critical_lateral_distance - slide_distance , critical_long_front_distance, currState.pos.z, 0);
+    GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
+    GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
 
-  bottom_left = invRotationMat*bottom_left;
-  bottom_left = invTranslationMat*bottom_left;
+    GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);
+    GPSPoint top_left(-critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0);
 
-  top_right = invRotationMat*top_right;
-  top_right = invTranslationMat*top_right;
+    bottom_left = invRotationMat * bottom_left;
+    bottom_left = invTranslationMat * bottom_left;
 
-  bottom_right = invRotationMat*bottom_right;
-  bottom_right = invTranslationMat*bottom_right;
+    top_right = invRotationMat * top_right;
+    top_right = invTranslationMat * top_right;
 
-  top_left = invRotationMat*top_left;
-  top_left = invTranslationMat*top_left;
+    bottom_right = invRotationMat * bottom_right;
+    bottom_right = invTranslationMat * bottom_right;
 
-  top_right_car = invRotationMat*top_right_car;
-  top_right_car = invTranslationMat*top_right_car;
+    top_left = invRotationMat * top_left;
+    top_left = invTranslationMat * top_left;
 
-  top_left_car = invRotationMat*top_left_car;
-  top_left_car = invTranslationMat*top_left_car;
+    top_right_car = invRotationMat * top_right_car;
+    top_right_car = invTranslationMat * top_right_car;
 
-  m_SafetyBorder.points.clear();
-  m_SafetyBorder.points.push_back(bottom_left) ;
-  m_SafetyBorder.points.push_back(bottom_right) ;
-  m_SafetyBorder.points.push_back(top_right_car) ;
-  m_SafetyBorder.points.push_back(top_right) ;
-  m_SafetyBorder.points.push_back(top_left) ;
-  m_SafetyBorder.points.push_back(top_left_car) ;
+    top_left_car = invRotationMat * top_left_car;
+    top_left_car = invTranslationMat * top_left_car;
 
-  for(unsigned int il=0; il < rollOuts.size(); il++)
-  {
-    if(rollOuts.at(il).size() > 0 && rollOuts.at(il).at(0).size()>0)
-    {
-      RelativeInfo car_info;
-      PlanningHelpers::GetRelativeInfo(totalPaths.at(il), currState, car_info);
+    m_SafetyBorder.points.clear();
+    m_SafetyBorder.points.push_back(bottom_left);
+    m_SafetyBorder.points.push_back(bottom_right);
+    m_SafetyBorder.points.push_back(top_right_car);
+    m_SafetyBorder.points.push_back(top_right);
+    m_SafetyBorder.points.push_back(top_left);
+    m_SafetyBorder.points.push_back(top_left_car);
 
+    for (unsigned int il = 0; il < rollOuts.size(); il++) {
+        if (rollOuts.at(il).size() > 0 && rollOuts.at(il).at(0).size() > 0) {
+            RelativeInfo car_info;
+            PlanningHelpers::GetRelativeInfo(totalPaths.at(il), currState, car_info);
 
-      for(unsigned int it=0; it< rollOuts.at(il).size(); it++)
-      {
-        int skip_id = -1;
-        for(unsigned int icon = 0; icon < contourPoints.size(); icon++)
-        {
-          if(skip_id == contourPoints.at(icon).id)
-            continue;
-
-          RelativeInfo obj_info;
-          PlanningHelpers::GetRelativeInfo(totalPaths.at(il), contourPoints.at(icon), obj_info);
-          double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths.at(il), car_info, obj_info);
-          if(obj_info.iFront == 0 && longitudinalDist > 0)
-            longitudinalDist = -longitudinalDist;
+            for (unsigned int it = 0; it < rollOuts.at(il).size(); it++) {
+                int skip_id = -1;
+                for (unsigned int icon = 0; icon < contourPoints.size(); icon++) {
+                    if (skip_id == contourPoints.at(icon).id)
+                        continue;
 
-          double direct_distance = hypot(obj_info.perp_point.pos.y-contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x-contourPoints.at(icon).pos.x);
-          if(contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance+contourPoints.at(icon).cost))
-          {
-            skip_id = contourPoints.at(icon).id;
-            continue;
-          }
+                    RelativeInfo obj_info;
+                    PlanningHelpers::GetRelativeInfo(totalPaths.at(il), contourPoints.at(icon), obj_info);
+                    double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths.at(il), car_info, obj_info);
+                    if (obj_info.iFront == 0 && longitudinalDist > 0)
+                        longitudinalDist = -longitudinalDist;
 
-          double close_in_percentage = 1;
-//          close_in_percentage = ((longitudinalDist- critical_long_front_distance)/params.rollInMargin)*4.0;
-//
-//          if(close_in_percentage <= 0 || close_in_percentage > 1) close_in_percentage = 1;
+                    double direct_distance =
+                        hypot(obj_info.perp_point.pos.y - contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x - contourPoints.at(icon).pos.x);
+                    if (contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance + contourPoints.at(icon).cost)) {
+                        skip_id = contourPoints.at(icon).id;
+                        continue;
+                    }
 
-          double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center;
+                    double close_in_percentage = 1;
+                    //          close_in_percentage = ((longitudinalDist- critical_long_front_distance)/params.rollInMargin)*4.0;
+                    //
+                    //          if(close_in_percentage <= 0 || close_in_percentage > 1) close_in_percentage = 1;
 
-          if(close_in_percentage < 1)
-            distance_from_center = distance_from_center - distance_from_center * (1.0-close_in_percentage);
+                    double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center;
 
-          double lateralDist = fabs(obj_info.perp_distance - distance_from_center);
+                    if (close_in_percentage < 1)
+                        distance_from_center = distance_from_center - distance_from_center * (1.0 - close_in_percentage);
 
-          if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
-          {
-            continue;
-          }
+                    double lateralDist = fabs(obj_info.perp_distance - distance_from_center);
 
-          longitudinalDist = longitudinalDist - critical_long_front_distance;
+                    if (longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance) {
+                        continue;
+                    }
 
-          if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
-            trajectoryCosts.at(iCostIndex).bBlocked = true;
+                    longitudinalDist = longitudinalDist - critical_long_front_distance;
 
-          if(lateralDist <= critical_lateral_distance
-              && longitudinalDist >= -carInfo.length/1.5
-              && longitudinalDist < params.minFollowingDistance)
-            trajectoryCosts.at(iCostIndex).bBlocked = true;
+                    if (m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true) {
+                        trajectoryCosts.at(iCostIndex).bBlocked = true;
+                    }
 
+                    if (lateralDist <= critical_lateral_distance && longitudinalDist >= -carInfo.length / 1.5 &&
+                        longitudinalDist < params.minFollowingDistance) {
+                        trajectoryCosts.at(iCostIndex).bBlocked = true;
+                    }
 
-          if(lateralDist != 0)
-            trajectoryCosts.at(iCostIndex).lateral_cost += 1.0/lateralDist;
+                    if (lateralDist != 0)
+                        trajectoryCosts.at(iCostIndex).lateral_cost += 1.0 / lateralDist;
 
-          if(longitudinalDist != 0)
-            trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0/fabs(longitudinalDist);
+                    if (longitudinalDist != 0)
+                        trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0 / fabs(longitudinalDist);
 
+                    if (longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance) {
+                        trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
+                        trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
+                    }
+                }
 
-          if(longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance)
-          {
-            trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
-            trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
-          }
+                iCostIndex++;
+            }
         }
-
-        iCostIndex++;
-      }
     }
-  }
 }
 
-void TrajectoryDynamicCosts::NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts)
-{
-  //Normalize costs
-  double totalPriorities = 0;
-  double totalChange = 0;
-  double totalLateralCosts = 0;
-  double totalLongitudinalCosts = 0;
-  double transitionCosts = 0;
-
-  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
-  {
-    totalPriorities += trajectoryCosts.at(ic).priority_cost;
-    transitionCosts += trajectoryCosts.at(ic).transition_cost;
-  }
-
-  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
-  {
-    totalChange += trajectoryCosts.at(ic).lane_change_cost;
-    totalLateralCosts += trajectoryCosts.at(ic).lateral_cost;
-    totalLongitudinalCosts += trajectoryCosts.at(ic).longitudinal_cost;
-  }
-
-//  cout << "------ Normalizing Step " << endl;
-  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
-  {
-    if(totalPriorities != 0)
-      trajectoryCosts.at(ic).priority_cost = trajectoryCosts.at(ic).priority_cost / totalPriorities;
-    else
-      trajectoryCosts.at(ic).priority_cost = 0;
-
-    if(transitionCosts != 0)
-      trajectoryCosts.at(ic).transition_cost = trajectoryCosts.at(ic).transition_cost / transitionCosts;
-    else
-      trajectoryCosts.at(ic).transition_cost = 0;
-
-    if(totalChange != 0)
-      trajectoryCosts.at(ic).lane_change_cost = trajectoryCosts.at(ic).lane_change_cost / totalChange;
-    else
-      trajectoryCosts.at(ic).lane_change_cost = 0;
+void TrajectoryDynamicCosts::NormalizeCosts(vector<TrajectoryCost> &trajectoryCosts, int enableDebug) {
+    // Normalize costs
+    double totalPriorities = 0;
+    double totalChange = 0;
+    double totalLateralCosts = 0;
+    double totalLongitudinalCosts = 0;
+    double transitionCosts = 0;
+
+    for (unsigned int ic = 0; ic < trajectoryCosts.size(); ic++) {
+        totalPriorities += trajectoryCosts.at(ic).priority_cost;
+        transitionCosts += trajectoryCosts.at(ic).transition_cost;
+    }
 
-    if(totalLateralCosts != 0)
-      trajectoryCosts.at(ic).lateral_cost = trajectoryCosts.at(ic).lateral_cost / totalLateralCosts;
-    else
-      trajectoryCosts.at(ic).lateral_cost = 0;
+    for (unsigned int ic = 0; ic < trajectoryCosts.size(); ic++) {
+        totalChange += trajectoryCosts.at(ic).lane_change_cost;
+        totalLateralCosts += trajectoryCosts.at(ic).lateral_cost;
+        totalLongitudinalCosts += trajectoryCosts.at(ic).longitudinal_cost;
+    }
 
-    if(totalLongitudinalCosts != 0)
-      trajectoryCosts.at(ic).longitudinal_cost = trajectoryCosts.at(ic).longitudinal_cost / totalLongitudinalCosts;
-    else
-      trajectoryCosts.at(ic).longitudinal_cost = 0;
-
-    trajectoryCosts.at(ic).cost = (m_WeightPriority*trajectoryCosts.at(ic).priority_cost + m_WeightTransition*trajectoryCosts.at(ic).transition_cost + m_WeightLat*trajectoryCosts.at(ic).lateral_cost + m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost)/4.0;
-
-  #ifdef DEBUG_ENABLE
-   std::cout << "Index: " << ic
-           << ", Priority: " << trajectoryCosts.at(ic).priority_cost
-           << ", Transition: " << trajectoryCosts.at(ic).transition_cost
-           << ", Lat: " << trajectoryCosts.at(ic).lateral_cost
-           << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost
-           << ", Change: " << trajectoryCosts.at(ic).lane_change_cost
-           << ", Avg: " << trajectoryCosts.at(ic).cost
-           << ", Blocked : " << trajectoryCosts.at(ic).bBlocked
-           << std::endl;
-  #endif
-  }
-
-  #ifdef DEBUG_ENABLE
-  std::cout << "------------------------ " << std::endl;
-  #endif
+    //  cout << "------ Normalizing Step " << endl;
+    for (unsigned int ic = 0; ic < trajectoryCosts.size(); ic++) {
+        if (totalPriorities != 0)
+            trajectoryCosts.at(ic).priority_cost = trajectoryCosts.at(ic).priority_cost / totalPriorities;
+        else
+            trajectoryCosts.at(ic).priority_cost = 0;
+
+        if (transitionCosts != 0)
+            trajectoryCosts.at(ic).transition_cost = trajectoryCosts.at(ic).transition_cost / transitionCosts;
+        else
+            trajectoryCosts.at(ic).transition_cost = 0;
+
+        if (totalChange != 0)
+            trajectoryCosts.at(ic).lane_change_cost = trajectoryCosts.at(ic).lane_change_cost / totalChange;
+        else
+            trajectoryCosts.at(ic).lane_change_cost = 0;
+
+        if (totalLateralCosts != 0)
+            trajectoryCosts.at(ic).lateral_cost = trajectoryCosts.at(ic).lateral_cost / totalLateralCosts;
+        else
+            trajectoryCosts.at(ic).lateral_cost = 0;
+
+        if (totalLongitudinalCosts != 0)
+            trajectoryCosts.at(ic).longitudinal_cost = trajectoryCosts.at(ic).longitudinal_cost / totalLongitudinalCosts;
+        else
+            trajectoryCosts.at(ic).longitudinal_cost = 0;
+
+        trajectoryCosts.at(ic).cost =
+            (m_WeightPriority * trajectoryCosts.at(ic).priority_cost + m_WeightTransition * trajectoryCosts.at(ic).transition_cost +
+             m_WeightLat * trajectoryCosts.at(ic).lateral_cost + m_WeightLong * trajectoryCosts.at(ic).longitudinal_cost) /
+            4.0;
+
+        if (enableDebug) {
+            std::cout << "Index: " << ic << ", Priority: " << trajectoryCosts.at(ic).priority_cost
+                      << ", Transition: " << trajectoryCosts.at(ic).transition_cost << ", Lat: " << trajectoryCosts.at(ic).lateral_cost
+                      << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost << ", Change: " << trajectoryCosts.at(ic).lane_change_cost
+                      << ", Avg: " << trajectoryCosts.at(ic).cost << ", Blocked : " << trajectoryCosts.at(ic).bBlocked << std::endl;
+        }
+    }
+    if (enableDebug)
+        std::cout << "------------------------ " << std::endl;
 }
 
-vector<TrajectoryCost> TrajectoryDynamicCosts::CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint> >& laneRollOuts,
-    const int& lane_index, const PlanningParams& params)
-{
-  vector<TrajectoryCost> costs;
-  TrajectoryCost tc;
-  int centralIndex = params.rollOutNumber/2;
-
-  tc.lane_index = lane_index;
-  for(unsigned int it=0; it< laneRollOuts.size(); it++)
-  {
-    tc.index = it;
-    tc.relative_index = it - centralIndex;
-    tc.distance_from_center = params.rollOutDensity*tc.relative_index;
-    tc.priority_cost = fabs(tc.distance_from_center);
-    tc.closest_obj_distance = params.horizonDistance;
-    tc.lane_change_cost = laneRollOuts.at(it).at(0).laneChangeCost;
-
-//    if(laneRollOuts.at(it).at(0).bDir == FORWARD_LEFT_DIR || laneRollOuts.at(it).at(0).bDir == FORWARD_RIGHT_DIR)
-//      tc.lane_change_cost = 1;
-//    else if(laneRollOuts.at(it).at(0).bDir == BACKWARD_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_RIGHT_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_LEFT_DIR)
-//      tc.lane_change_cost = 2;
-//    else
-//      tc.lane_change_cost = 0;
-
-    costs.push_back(tc);
-  }
-
-  return costs;
+vector<TrajectoryCost> TrajectoryDynamicCosts::CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint>> &laneRollOuts,
+                                                                                   const int &lane_index, PlanningParams &params) {
+    vector<TrajectoryCost> costs;
+    TrajectoryCost tc;
+    int centralIndex = params.rollOutNumber / 2;
+
+    tc.lane_index = lane_index;
+    for (unsigned int it = 0; it < laneRollOuts.size(); it++) {
+        tc.index = it;
+        tc.relative_index = it - centralIndex;
+        tc.distance_from_center = params.rollOutDensity * tc.relative_index;
+        tc.priority_cost = fabs(tc.distance_from_center);
+        tc.closest_obj_distance = params.horizonDistance;
+        tc.lane_change_cost = laneRollOuts.at(it).at(0).laneChangeCost;
+
+        //    if(laneRollOuts.at(it).at(0).bDir == FORWARD_LEFT_DIR || laneRollOuts.at(it).at(0).bDir == FORWARD_RIGHT_DIR)
+        //      tc.lane_change_cost = 1;
+        //    else if(laneRollOuts.at(it).at(0).bDir == BACKWARD_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_RIGHT_DIR ||
+        //    laneRollOuts.at(it).at(0).bDir == BACKWARD_LEFT_DIR)
+        //      tc.lane_change_cost = 2;
+        //    else
+        //      tc.lane_change_cost = 0;
+
+        costs.push_back(tc);
+    }
+
+    return costs;
 }
 
-void TrajectoryDynamicCosts::CalculateTransitionCosts(vector<TrajectoryCost>& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params)
-{
-  for(int ic = 0; ic< trajectoryCosts.size(); ic++)
-  {
-    trajectoryCosts.at(ic).transition_cost = fabs(params.rollOutDensity * (ic - currTrajectoryIndex));
-  }
+void TrajectoryDynamicCosts::CalculateTransitionCosts(vector<TrajectoryCost> &trajectoryCosts, const int &currTrajectoryIndex,
+                                                      PlanningParams &params) {
+    for (int ic = 0; ic < trajectoryCosts.size(); ic++) {
+        trajectoryCosts.at(ic).transition_cost = fabs(params.rollOutDensity * (ic - currTrajectoryIndex));
+    }
 }
 
 /**
@@ -852,265 +761,252 @@ void TrajectoryDynamicCosts::CalculateTransitionCosts(vector<TrajectoryCost>& tr
  * @param rollOuts
  * @return true if data isvalid for cost calculation
  */
-bool TrajectoryDynamicCosts::ValidateRollOutsInput(const vector<vector<vector<WayPoint> > >& rollOuts)
-{
-  if(rollOuts.size()==0)
-    return false;
-
-  for(unsigned int il = 0; il < rollOuts.size(); il++)
-  {
-    if(rollOuts.at(il).size() == 0)
-      return false;
-  }
-
-  return true;
-}
+bool TrajectoryDynamicCosts::ValidateRollOutsInput(const vector<vector<vector<WayPoint>>> &rollOuts) {
+    if (rollOuts.size() == 0)
+        return false;
 
-void TrajectoryDynamicCosts::CalculateIntersectionVelocities(const std::vector<PlannerHNS::WayPoint>& path, const PlannerHNS::DetectedObject& obj, const WayPoint& currPose, const CAR_BASIC_INFO& carInfo, const double& c_lateral_d, WayPoint& collisionPoint, TrajectoryCost& trajectoryCosts)
-{
-  trajectoryCosts.bBlocked = false;
-  int closest_path_i = path.size();
-  for(unsigned int k = 0; k < obj.predTrajectories.size(); k++)
-  {
-    for(unsigned int j = 0; j < obj.predTrajectories.at(k).size(); j++)
-    {
-      for(unsigned int i = 0; i < path.size(); i++)
-      {
-        //if(path.at(i).timeCost > -1)
-        {
-          double collision_distance = hypot(path.at(i).pos.x-obj.predTrajectories.at(k).at(j).pos.x, path.at(i).pos.y-obj.predTrajectories.at(k).at(j).pos.y);
-          double collision_t = fabs(path.at(i).timeCost - obj.predTrajectories.at(k).at(j).timeCost);
-
-          //if(collision_distance <= c_lateral_d && i < closest_path_i && collision_t < m_CollisionTimeDiff)
-          if(collision_distance <= c_lateral_d && i < closest_path_i)
-          {
-
-            closest_path_i = i;
-            double a = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(path.at(i).pos.a, obj.predTrajectories.at(k).at(j).pos.a)/M_PI;
-            if(a < 0.25 && (currPose.v - obj.center.v) > 0)
-              trajectoryCosts.closest_obj_velocity = (currPose.v - obj.center.v);
-            else
-              trajectoryCosts.closest_obj_velocity = currPose.v;
-
-            collisionPoint = path.at(i);
-            collisionPoint.collisionCost = collision_t;
-            collisionPoint.cost = collision_distance;
-            trajectoryCosts.bBlocked = true;
-          }
-        }
-      }
+    for (unsigned int il = 0; il < rollOuts.size(); il++) {
+        if (rollOuts.at(il).size() == 0)
+            return false;
     }
-  }
-}
 
-int TrajectoryDynamicCosts::GetCurrentRollOutIndex(const std::vector<WayPoint>& path, const WayPoint& currState, const PlanningParams& params)
-{
-  RelativeInfo obj_info;
-  PlanningHelpers::GetRelativeInfo(path, currState, obj_info);
-  int currIndex = params.rollOutNumber/2 + floor(obj_info.perp_distance/params.rollOutDensity);
-  if(currIndex < 0)
-    currIndex = 0;
-  else if(currIndex > params.rollOutNumber)
-    currIndex = params.rollOutNumber;
-
-  return currIndex;
+    return true;
 }
 
-void TrajectoryDynamicCosts::InitializeCosts(const vector<vector<WayPoint> >& rollOuts, const PlanningParams& params)
-{
-  m_TrajectoryCosts.clear();
-  if(rollOuts.size()>0)
-  {
-    TrajectoryCost tc;
-    int centralIndex = params.rollOutNumber/2;
-    tc.lane_index = 0;
-    for(unsigned int it=0; it< rollOuts.size(); it++)
-    {
-      tc.index = it;
-      tc.relative_index = it - centralIndex;
-      tc.distance_from_center = params.rollOutDensity*tc.relative_index;
-      tc.priority_cost = fabs(tc.distance_from_center);
-      tc.closest_obj_distance = params.horizonDistance;
-      if(rollOuts.at(it).size() > 0)
-        tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
-      m_TrajectoryCosts.push_back(tc);
+void TrajectoryDynamicCosts::CalculateIntersectionVelocities(const std::vector<PlannerHNS::WayPoint> &path, const PlannerHNS::DetectedObject &obj,
+                                                             const WayPoint &currPose, const CAR_BASIC_INFO &carInfo, const double &c_lateral_d,
+                                                             WayPoint &collisionPoint, TrajectoryCost &trajectoryCosts) {
+    trajectoryCosts.bBlocked = false;
+    int closest_path_i = path.size();
+    for (unsigned int k = 0; k < obj.predTrajectories.size(); k++) {
+        for (unsigned int j = 0; j < obj.predTrajectories.at(k).size(); j++) {
+            for (unsigned int i = 0; i < path.size(); i++) {
+                // if(path.at(i).timeCost > -1)
+                {
+                    double collision_distance =
+                        hypot(path.at(i).pos.x - obj.predTrajectories.at(k).at(j).pos.x, path.at(i).pos.y - obj.predTrajectories.at(k).at(j).pos.y);
+                    double collision_t = fabs(path.at(i).timeCost - obj.predTrajectories.at(k).at(j).timeCost);
+
+                    // if(collision_distance <= c_lateral_d && i < closest_path_i && collision_t < m_CollisionTimeDiff)
+                    if (collision_distance <= c_lateral_d && i < closest_path_i) {
+
+                        closest_path_i = i;
+                        double a =
+                            UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(path.at(i).pos.a, obj.predTrajectories.at(k).at(j).pos.a) / M_PI;
+                        if (a < 0.25 && (currPose.v - obj.center.v) > 0)
+                            trajectoryCosts.closest_obj_velocity = (currPose.v - obj.center.v);
+                        else
+                            trajectoryCosts.closest_obj_velocity = currPose.v;
+
+                        collisionPoint = path.at(i);
+                        collisionPoint.collisionCost = collision_t;
+                        collisionPoint.cost = collision_distance;
+                        trajectoryCosts.bBlocked = true;
+                    }
+                }
+            }
+        }
     }
-  }
 }
 
-void TrajectoryDynamicCosts::InitializeSafetyPolygon(const WayPoint& currState, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d)
-{
-  PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2);
-  PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
-
-  double corner_slide_distance = c_lateral_d/2.0;
-  double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle;
-  double slide_distance = vehicleState.steer * ratio_to_angle;
-
-  GPSPoint bottom_left(-c_lateral_d ,-c_long_back_d,  currState.pos.z, 0);
-  GPSPoint bottom_right(c_lateral_d, -c_long_back_d,  currState.pos.z, 0);
-
-  GPSPoint top_right_car(c_lateral_d, carInfo.wheel_base/3.0 + carInfo.length/3.0,  currState.pos.z, 0);
-  GPSPoint top_left_car(-c_lateral_d, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0);
-
-  GPSPoint top_right(c_lateral_d - slide_distance, c_long_front_d,  currState.pos.z, 0);
-  GPSPoint top_left(-c_lateral_d - slide_distance , c_long_front_d, currState.pos.z, 0);
-
-  bottom_left = invRotationMat*bottom_left;
-  bottom_left = invTranslationMat*bottom_left;
-
-  top_right = invRotationMat*top_right;
-  top_right = invTranslationMat*top_right;
-
-  bottom_right = invRotationMat*bottom_right;
-  bottom_right = invTranslationMat*bottom_right;
+int TrajectoryDynamicCosts::GetCurrentRollOutIndex(const std::vector<WayPoint> &path, const WayPoint &currState, PlanningParams &params) {
+    RelativeInfo cur_pose_to_center_line;
+    PlanningHelpers::GetRelativeInfo(path, currState, cur_pose_to_center_line);
 
-  top_left = invRotationMat*top_left;
-  top_left = invTranslationMat*top_left;
+    int currIndex = floor(double(params.rollOutNumber) / 2.0                              // Center index
+                          + cur_pose_to_center_line.perp_distance / params.rollOutDensity // Normalized distance
+                          + 0.5);
 
-  top_right_car = invRotationMat*top_right_car;
-  top_right_car = invTranslationMat*top_right_car;
+    if (currIndex < 0)
+        currIndex = 0;
+    else if (currIndex > params.rollOutNumber)
+        currIndex = params.rollOutNumber;
 
-  top_left_car = invRotationMat*top_left_car;
-  top_left_car = invTranslationMat*top_left_car;
-
-  m_SafetyBorder.points.clear();
-  m_SafetyBorder.points.push_back(bottom_left) ;
-  m_SafetyBorder.points.push_back(bottom_right) ;
-  m_SafetyBorder.points.push_back(top_right_car) ;
-  m_SafetyBorder.points.push_back(top_right) ;
-  m_SafetyBorder.points.push_back(top_left) ;
-  m_SafetyBorder.points.push_back(top_left_car) ;
+    return currIndex;
 }
 
-void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsDynamic(const std::vector<PlannerHNS::DetectedObject>& obj_list, const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
-    const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo,
-    const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d )
-{
-
-  RelativeInfo car_info;
-  PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
-  m_CollisionPoints.clear();
-
-  for(unsigned int i=0; i < obj_list.size(); i++)
-  {
-    if(obj_list.at(i).label.compare("curb") == 0)
-    {
-      double d = hypot(obj_list.at(i).center.pos.y - currState.pos.y ,  obj_list.at(i).center.pos.x - currState.pos.x);
-      if(d > params.minFollowingDistance + c_lateral_d)
-        continue;
+void TrajectoryDynamicCosts::InitializeCosts(const vector<vector<WayPoint>> &rollOuts, PlanningParams &params) {
+    m_TrajectoryCosts.clear();
+    if (rollOuts.size() > 0) {
+        TrajectoryCost tc;
+        int centralIndex = params.rollOutNumber / 2;
+        tc.lane_index = 0;
+        for (unsigned int it = 0; it < rollOuts.size(); it++) {
+            tc.index = it;
+            tc.relative_index = it - centralIndex;
+            tc.distance_from_center = params.rollOutDensity * tc.relative_index;
+            tc.priority_cost = fabs(tc.distance_from_center);
+            tc.closest_obj_distance = params.horizonDistance;
+            if (rollOuts.at(it).size() > 0)
+                tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
+            m_TrajectoryCosts.push_back(tc);
+        }
     }
+}
 
-    if(obj_list.at(i).bVelocity && obj_list.at(i).predTrajectories.size() > 0) // dynamic
-    {
-
-      for(unsigned int ir=0; ir < rollOuts.size(); ir++)
-      {
-        WayPoint collisionPoint;
-        TrajectoryCost trajectoryCosts;
-        CalculateIntersectionVelocities(rollOuts.at(ir), obj_list.at(i), currState, carInfo, c_lateral_d, collisionPoint,trajectoryCosts);
-        if(trajectoryCosts.bBlocked)
-        {
-          RelativeInfo col_info;
-          PlanningHelpers::GetRelativeInfo(totalPaths, collisionPoint, col_info);
-          double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, col_info);
-
-          if(col_info.iFront == 0 && longitudinalDist > 0)
-            longitudinalDist = -longitudinalDist;
+void TrajectoryDynamicCosts::InitializeSafetyPolygon(const WayPoint &currState, const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
+                                                     const double &c_lateral_d, const double &c_long_front_d, const double &c_long_back_d) {
+    PlannerHNS::Mat3 invRotationMat(currState.pos.a - M_PI_2);
+    PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
 
-          if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || fabs(longitudinalDist) < carInfo.width/2.0)
-            continue;
+    double corner_slide_distance = c_lateral_d / 2.0;
+    double ratio_to_angle = corner_slide_distance / carInfo.max_steer_angle;
+    double slide_distance = vehicleState.steer * ratio_to_angle;
 
-          //std::cout << "LongDistance: " << longitudinalDist << std::endl;
+    GPSPoint bottom_left(-c_lateral_d, -c_long_back_d, currState.pos.z, 0);
+    GPSPoint bottom_right(c_lateral_d, -c_long_back_d, currState.pos.z, 0);
 
-          if(longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(ir).closest_obj_distance)
-            m_TrajectoryCosts.at(ir).closest_obj_distance = longitudinalDist;
+    GPSPoint top_right_car(c_lateral_d, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
+    GPSPoint top_left_car(-c_lateral_d, carInfo.wheel_base / 3.0 + carInfo.length / 3.0, currState.pos.z, 0);
 
-          m_TrajectoryCosts.at(ir).closest_obj_velocity = trajectoryCosts.closest_obj_velocity;
-          m_TrajectoryCosts.at(ir).bBlocked = true;
+    GPSPoint top_right(c_lateral_d - slide_distance, c_long_front_d, currState.pos.z, 0);
+    GPSPoint top_left(-c_lateral_d - slide_distance, c_long_front_d, currState.pos.z, 0);
 
-          m_CollisionPoints.push_back(collisionPoint);
-        }
-      }
-    }
-    else
-    {
-      RelativeInfo obj_info;
-      WayPoint corner_p;
-      for(unsigned int icon = 0; icon < obj_list.at(i).contour.size(); icon++)
-      {
-        if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, obj_list.at(i).contour.at(icon)) == true)
-        {
-          for(unsigned int it=0; it< rollOuts.size(); it++)
-            m_TrajectoryCosts.at(it).bBlocked = true;
+    bottom_left = invRotationMat * bottom_left;
+    bottom_left = invTranslationMat * bottom_left;
 
-          return;
-        }
+    top_right = invRotationMat * top_right;
+    top_right = invTranslationMat * top_right;
 
-        corner_p.pos = obj_list.at(i).contour.at(icon);
-        PlanningHelpers::GetRelativeInfo(totalPaths, corner_p, obj_info);
-        double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, obj_info);
-        if(obj_info.iFront == 0 && longitudinalDist > 0)
-          longitudinalDist = -longitudinalDist;
+    bottom_right = invRotationMat * bottom_right;
+    bottom_right = invTranslationMat * bottom_right;
 
+    top_left = invRotationMat * top_left;
+    top_left = invTranslationMat * top_left;
 
-        if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance)
-          continue;
+    top_right_car = invRotationMat * top_right_car;
+    top_right_car = invTranslationMat * top_right_car;
 
-        longitudinalDist = longitudinalDist - c_long_front_d;
+    top_left_car = invRotationMat * top_left_car;
+    top_left_car = invTranslationMat * top_left_car;
 
-        for(unsigned int it=0; it< rollOuts.size(); it++)
-        {
-          double lateralDist = fabs(obj_info.perp_distance - m_TrajectoryCosts.at(it).distance_from_center);
+    m_SafetyBorder.points.clear();
+    m_SafetyBorder.points.push_back(bottom_left);
+    m_SafetyBorder.points.push_back(bottom_right);
+    m_SafetyBorder.points.push_back(top_right_car);
+    m_SafetyBorder.points.push_back(top_right);
+    m_SafetyBorder.points.push_back(top_left);
+    m_SafetyBorder.points.push_back(top_left_car);
+}
 
-          if(lateralDist > m_LateralSkipDistance)
-            continue;
+void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsDynamic(const std::vector<PlannerHNS::DetectedObject> &obj_list,
+                                                                         const vector<vector<WayPoint>> &rollOuts, const vector<WayPoint> &totalPaths,
+                                                                         const WayPoint &currState, PlanningParams &params,
+                                                                         const CAR_BASIC_INFO &carInfo, const VehicleState &vehicleState,
+                                                                         const double &c_lateral_d, const double &c_long_front_d,
+                                                                         const double &c_long_back_d) {
 
-          if(lateralDist <= c_lateral_d && longitudinalDist > -carInfo.length && longitudinalDist < params.minFollowingDistance)
-          {
-            m_TrajectoryCosts.at(it).bBlocked = true;
-            m_CollisionPoints.push_back(obj_info.perp_point);
-          }
+    RelativeInfo car_info;
+    PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
+    m_CollisionPoints.clear();
 
-          if(lateralDist != 0)
-            m_TrajectoryCosts.at(it).lateral_cost += 1.0/lateralDist;
+    for (unsigned int i = 0; i < obj_list.size(); i++) {
+        if (obj_list.at(i).label.compare("curb") == 0) {
+            double d = hypot(obj_list.at(i).center.pos.y - currState.pos.y, obj_list.at(i).center.pos.x - currState.pos.x);
+            if (d > params.minFollowingDistance + c_lateral_d)
+                continue;
+        }
 
-          if(longitudinalDist != 0)
-            m_TrajectoryCosts.at(it).longitudinal_cost += 1.0/fabs(longitudinalDist);
+        if (obj_list.at(i).bVelocity && obj_list.at(i).predTrajectories.size() > 0) // dynamic
+        {
 
-          if(longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(it).closest_obj_distance)
-          {
-            m_TrajectoryCosts.at(it).closest_obj_distance = longitudinalDist;
-            m_TrajectoryCosts.at(it).closest_obj_velocity = obj_list.at(i).center.v;
-          }
+            for (unsigned int ir = 0; ir < rollOuts.size(); ir++) {
+                WayPoint collisionPoint;
+                TrajectoryCost trajectoryCosts;
+                CalculateIntersectionVelocities(rollOuts.at(ir), obj_list.at(i), currState, carInfo, c_lateral_d, collisionPoint, trajectoryCosts);
+                if (trajectoryCosts.bBlocked) {
+                    RelativeInfo col_info;
+                    PlanningHelpers::GetRelativeInfo(totalPaths, collisionPoint, col_info);
+                    double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, col_info);
+
+                    if (col_info.iFront == 0 && longitudinalDist > 0)
+                        longitudinalDist = -longitudinalDist;
+
+                    if (longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance ||
+                        fabs(longitudinalDist) < carInfo.width / 2.0)
+                        continue;
+
+                    // std::cout << "LongDistance: " << longitudinalDist << std::endl;
+
+                    if (longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(ir).closest_obj_distance)
+                        m_TrajectoryCosts.at(ir).closest_obj_distance = longitudinalDist;
+
+                    m_TrajectoryCosts.at(ir).closest_obj_velocity = trajectoryCosts.closest_obj_velocity;
+                    m_TrajectoryCosts.at(ir).bBlocked = true;
+
+                    m_CollisionPoints.push_back(collisionPoint);
+                }
+            }
+        } else {
+            RelativeInfo obj_info;
+            WayPoint corner_p;
+            for (unsigned int icon = 0; icon < obj_list.at(i).contour.size(); icon++) {
+                if (m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, obj_list.at(i).contour.at(icon)) == true) {
+                    for (unsigned int it = 0; it < rollOuts.size(); it++) {
+                        m_TrajectoryCosts.at(it).bBlocked = true;
+                    }
+
+                    return;
+                }
+
+                corner_p.pos = obj_list.at(i).contour.at(icon);
+                PlanningHelpers::GetRelativeInfo(totalPaths, corner_p, obj_info);
+                double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, obj_info);
+                if (obj_info.iFront == 0 && longitudinalDist > 0)
+                    longitudinalDist = -longitudinalDist;
+
+                if (longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance)
+                    continue;
+
+                longitudinalDist = longitudinalDist - c_long_front_d;
+
+                for (unsigned int it = 0; it < rollOuts.size(); it++) {
+                    double lateralDist = fabs(obj_info.perp_distance - m_TrajectoryCosts.at(it).distance_from_center);
+
+                    if (lateralDist > m_LateralSkipDistance)
+                        continue;
+
+                    if (lateralDist <= c_lateral_d && longitudinalDist > -carInfo.length && longitudinalDist < params.minFollowingDistance) {
+                        m_TrajectoryCosts.at(it).bBlocked = true;
+                        m_CollisionPoints.push_back(obj_info.perp_point);
+                    }
+
+                    if (lateralDist != 0)
+                        m_TrajectoryCosts.at(it).lateral_cost += 1.0 / lateralDist;
+
+                    if (longitudinalDist != 0)
+                        m_TrajectoryCosts.at(it).longitudinal_cost += 1.0 / fabs(longitudinalDist);
+
+                    if (longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(it).closest_obj_distance) {
+                        m_TrajectoryCosts.at(it).closest_obj_distance = longitudinalDist;
+                        m_TrajectoryCosts.at(it).closest_obj_velocity = obj_list.at(i).center.v;
+                    }
+                }
+            }
         }
-      }
-
     }
-  }
 }
 
-double TrajectoryDynamicCosts::CalculateTurnAngle(const std::vector<WayPoint>& path, const WayPoint& currState, int distance)
-{
-  RelativeInfo car_info;
-  PlanningHelpers::GetRelativeInfo(path, currState, car_info);
-  GPSPoint rollout_start_pose = path.at(std::min(car_info.iFront + 3, int(path.size())-1)).pos;
-  GPSPoint turn_pose = path.at(std::min(car_info.iFront + distance, int(path.size())-1)).pos;
+double TrajectoryDynamicCosts::CalculateTurnAngle(const std::vector<WayPoint> &path, const WayPoint &currState, int distance) {
+    RelativeInfo car_info;
+    PlanningHelpers::GetRelativeInfo(path, currState, car_info);
+    GPSPoint rollout_start_pose = path.at(std::min(car_info.iFront + 3, int(path.size()) - 1)).pos;
+    GPSPoint turn_pose = path.at(std::min(car_info.iFront + distance, int(path.size()) - 1)).pos;
 
-  // std::cout << "ego x: " << rollout_start_pose.x << ", y: " << rollout_start_pose.y << std::endl;
-  // std::cout << "turn x: " << turn_pose.x << ", y: " << turn_pose.y << std::endl;
+    // std::cout << "ego x: " << rollout_start_pose.x << ", y: " << rollout_start_pose.y << std::endl;
+    // std::cout << "turn x: " << turn_pose.x << ", y: " << turn_pose.y << std::endl;
 
-  double m_turnAngle = 0;
-  double hypot_length = hypot((rollout_start_pose.x - turn_pose.x), (rollout_start_pose.y - turn_pose.y));
+    double m_turnAngle = 0;
+    double hypot_length = hypot((rollout_start_pose.x - turn_pose.x), (rollout_start_pose.y - turn_pose.y));
 
-  if(hypot_length <= 0.1)
-    m_turnAngle = 0;
-  else
-    m_turnAngle = acos(abs(turn_pose.x - rollout_start_pose.x)/hypot_length)*180.0/3.14;
-  if((turn_pose.y - rollout_start_pose.y) < 0)
-    m_turnAngle = -1 * m_turnAngle;
+    if (hypot_length <= 0.1)
+        m_turnAngle = 0;
+    else
+        m_turnAngle = acos(abs(turn_pose.x - rollout_start_pose.x) / hypot_length) * 180.0 / 3.14;
+    if ((turn_pose.y - rollout_start_pose.y) < 0)
+        m_turnAngle = -1 * m_turnAngle;
 
     return m_turnAngle;
 }
 
-}
+} // namespace PlannerHNS
diff --git a/autoware.ai/src/autoware/common/op_ros_helpers/CMakeLists.txt b/autoware.ai/src/autoware/common/op_ros_helpers/CMakeLists.txt
index 43cb6def..fc7c18e5 100644
--- a/autoware.ai/src/autoware/common/op_ros_helpers/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/op_ros_helpers/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(op_ros_helpers)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   autoware_msgs
@@ -11,7 +11,6 @@ find_package(catkin REQUIRED COMPONENTS
   libwaypoint_follower
   map_file
   op_planner
-  op_simu
   op_utility
   pcl_conversions
   pcl_ros
diff --git a/autoware.ai/src/autoware/common/op_ros_helpers/package.xml b/autoware.ai/src/autoware/common/op_ros_helpers/package.xml
index 37fa3e62..e987fdc6 100644
--- a/autoware.ai/src/autoware/common/op_ros_helpers/package.xml
+++ b/autoware.ai/src/autoware/common/op_ros_helpers/package.xml
@@ -7,7 +7,6 @@
   <author email="hatem.darweesh@g.sp.m.is.nagoya-u.ac.jp">Hatem Darweesh</author>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_msgs</depend>
@@ -15,7 +14,6 @@
   <depend>jsk_recognition_msgs</depend>
   <depend>map_file</depend>
   <depend>op_planner</depend>
-  <depend>op_simu</depend>
   <depend>op_utility</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
diff --git a/autoware.ai/src/autoware/common/op_ros_helpers/src/op_ROSHelpers.cpp b/autoware.ai/src/autoware/common/op_ros_helpers/src/op_ROSHelpers.cpp
index e0523a5d..6c4979d5 100644
--- a/autoware.ai/src/autoware/common/op_ros_helpers/src/op_ROSHelpers.cpp
+++ b/autoware.ai/src/autoware/common/op_ros_helpers/src/op_ROSHelpers.cpp
@@ -5,1865 +5,1720 @@
 
 #include "op_ros_helpers/op_ROSHelpers.h"
 
-#include <iostream>
-#include <sstream>
-#include <fstream>
-#include <math.h>
-#include "op_ros_helpers/PolygonGenerator.h"
 #include "op_planner/MappingHelpers.h"
 #include "op_planner/MatrixOperations.h"
+#include "op_ros_helpers/PolygonGenerator.h"
+#include <fstream>
+#include <iostream>
+#include <math.h>
+#include <sstream>
 
+namespace PlannerHNS {
 
-namespace PlannerHNS
-{
-
-ROSHelpers::ROSHelpers() {
+ROSHelpers::ROSHelpers() {}
 
-}
+ROSHelpers::~ROSHelpers() {}
 
-ROSHelpers::~ROSHelpers() {
-}
+void ROSHelpers::GetTransformFromTF(const std::string parent_frame, const std::string child_frame, tf::StampedTransform &transform) {
+    static tf::TransformListener listener;
 
-void ROSHelpers::GetTransformFromTF(const std::string parent_frame, const std::string child_frame, tf::StampedTransform &transform)
-{
-  static tf::TransformListener listener;
-
-  int nFailedCounter = 0;
-  while (1)
-  {
-    try
-    {
-      listener.lookupTransform(parent_frame, child_frame, ros::Time(0), transform);
-      break;
-    }
-    catch (tf::TransformException& ex)
-    {
-      if(nFailedCounter > 2)
-      {
-        ROS_ERROR("%s", ex.what());
-      }
-      ros::Duration(1.0).sleep();
-      nFailedCounter ++;
+    int nFailedCounter = 0;
+    while (1) {
+        try {
+            listener.lookupTransform(parent_frame, child_frame, ros::Time(0), transform);
+            break;
+        } catch (tf::TransformException &ex) {
+            if (nFailedCounter > 2) {
+                ROS_ERROR("%s", ex.what());
+            }
+            ros::Duration(1.0).sleep();
+            nFailedCounter++;
+        }
     }
-  }
-}
-
-visualization_msgs::Marker ROSHelpers::CreateGenMarker(const double& x, const double& y, const double& z,const double& a,
-    const double& r, const double& g, const double& b, const double& scale, const int& id, const std::string& ns, const int& type)
-{
-  visualization_msgs::Marker mkr;
-  mkr.header.frame_id = "map";
-  mkr.header.stamp = ros::Time();
-  mkr.ns = ns;
-  mkr.type = type;
-  mkr.action = visualization_msgs::Marker::ADD;
-  mkr.scale.x = scale;
-  mkr.scale.y = scale;
-  mkr.scale.z = scale;
-  mkr.color.a = 0.8;
-  mkr.color.r = r;
-  mkr.color.g = g;
-  mkr.color.b = b;
-  mkr.pose.position.x = x;
-  mkr.pose.position.y = y;
-  mkr.pose.position.z = z;
-  mkr.pose.orientation = tf::createQuaternionMsgFromYaw(a);
-  mkr.id = id;
-  return mkr;
-}
-
-void ROSHelpers::InitMarkers(const int& nMarkers,
-    visualization_msgs::MarkerArray& centers,
-    visualization_msgs::MarkerArray& dirs,
-    visualization_msgs::MarkerArray& text_info,
-    visualization_msgs::MarkerArray& polygons,
-    visualization_msgs::MarkerArray& trajectories)
-{
-  centers.markers.clear();
-  dirs.markers.clear();
-  text_info.markers.clear();
-  polygons.markers.clear();
-  trajectories.markers.clear();
-
-  for(int i=0; i<nMarkers; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"CenterMarker", visualization_msgs::Marker::SPHERE);
-    centers.markers.push_back(mkr);
-  }
-
-  for(int i=nMarkers; i<nMarkers*2; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"Directions", visualization_msgs::Marker::ARROW);
-    dirs.markers.push_back(mkr);
-  }
-
-  for(int i=nMarkers*2; i<nMarkers*3; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"InfoText", visualization_msgs::Marker::TEXT_VIEW_FACING);
-    text_info.markers.push_back(mkr);
-  }
-
-  for(int i=nMarkers*3; i<nMarkers*4; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"detected_polygons", visualization_msgs::Marker::LINE_STRIP);
-    polygons.markers.push_back(mkr);
-  }
-
-  for(int i=nMarkers*4; i<nMarkers*5; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"tracked_trajectories", visualization_msgs::Marker::LINE_STRIP);
-    trajectories.markers.push_back(mkr);
-  }
-}
-
-void ROSHelpers::InitMatchingMarkers(const int& nMarkers, visualization_msgs::MarkerArray& connections)
-{
-  connections.markers.clear();
-  for(int i=0; i<nMarkers; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"matching_connections", visualization_msgs::Marker::LINE_STRIP);
-    connections.markers.push_back(mkr);
-  }
 }
 
-void ROSHelpers::ConvertMatchingMarkers(const std::vector<std::pair<PlannerHNS::WayPoint, PlannerHNS::WayPoint> >& match_list,
-    visualization_msgs::MarkerArray& tracked_traj_d, visualization_msgs::MarkerArray& tracked_traj, int start_id)
-{
-
-  tracked_traj = tracked_traj_d;
-
-  for(unsigned int i = 0; i < match_list.size(); i++)
-  {
-    visualization_msgs::Marker match_mkr = CreateGenMarker(0,0,0,0,1,0,0,0.2, start_id+i,"matching_connections", visualization_msgs::Marker::LINE_STRIP);
-    geometry_msgs::Point point;
-    point.x = match_list.at(i).first.pos.x;
-    point.y = match_list.at(i).first.pos.y;
-    point.z = match_list.at(i).first.pos.z;
-    match_mkr.points.push_back(point);
-
-    point.x = match_list.at(i).second.pos.x;
-    point.y = match_list.at(i).second.pos.y;
-    point.z = match_list.at(i).second.pos.z;
-    match_mkr.points.push_back(point);
-
-    if(i < tracked_traj.markers.size())
-      tracked_traj.markers.at(i) = match_mkr;
-    else
-      tracked_traj.markers.push_back(match_mkr);
-  }
+visualization_msgs::Marker ROSHelpers::CreateGenMarker(const double &x, const double &y, const double &z, const double &a, const double &r,
+                                                       const double &g, const double &b, const double &scale, const int &id, const std::string &ns,
+                                                       const int &type) {
+    visualization_msgs::Marker mkr;
+    mkr.header.frame_id = "map";
+    mkr.header.stamp = ros::Time();
+    mkr.ns = ns;
+    mkr.type = type;
+    mkr.action = visualization_msgs::Marker::ADD;
+    mkr.scale.x = scale;
+    mkr.scale.y = scale;
+    mkr.scale.z = scale;
+    mkr.color.a = 0.8;
+    mkr.color.r = r;
+    mkr.color.g = g;
+    mkr.color.b = b;
+    mkr.pose.position.x = x;
+    mkr.pose.position.y = y;
+    mkr.pose.position.z = z;
+    mkr.pose.orientation = tf::createQuaternionMsgFromYaw(a);
+    mkr.id = id;
+    return mkr;
 }
 
-int ROSHelpers::ConvertTrackedObjectsMarkers(const PlannerHNS::WayPoint& currState, const std::vector<PlannerHNS::DetectedObject>& trackedObstacles,
-    visualization_msgs::MarkerArray& centers_d,
-    visualization_msgs::MarkerArray& dirs_d,
-    visualization_msgs::MarkerArray& text_info_d,
-    visualization_msgs::MarkerArray& polygons_d,
-    visualization_msgs::MarkerArray& tracked_traj_d,
-    visualization_msgs::MarkerArray& centers,
-    visualization_msgs::MarkerArray& dirs,
-    visualization_msgs::MarkerArray& text_info,
-    visualization_msgs::MarkerArray& polygons,
-    visualization_msgs::MarkerArray& tracked_traj)
-{
-
-  int i_next_id = 0;
-  centers = centers_d;
-  dirs = dirs_d;
-  text_info = text_info_d;
-  polygons = polygons_d;
-  tracked_traj = tracked_traj_d;
-
-  for(unsigned int i =0; i < trackedObstacles.size(); i++)
-  {
-    int speed = (trackedObstacles.at(i).center.v*3.6);
-
-    //Update Stage
-    visualization_msgs::Marker center_mkr = CreateGenMarker(trackedObstacles.at(i).center.pos.x,trackedObstacles.at(i).center.pos.y,trackedObstacles.at(i).center.pos.z,
-        trackedObstacles.at(i).center.pos.a,1,0,0,0.5,i,"CenterMarker", visualization_msgs::Marker::SPHERE);
-    if(i < centers.markers.size())
-    {
-      center_mkr.id = centers.markers.at(i).id;
-      centers.markers.at(i) = center_mkr;
-    }
-    else
-      centers.markers.push_back(center_mkr);
-
-    //Directions
-    if(trackedObstacles.at(i).bDirection)
-    {
-      visualization_msgs::Marker dir_mkr = CreateGenMarker(trackedObstacles.at(i).center.pos.x,trackedObstacles.at(i).center.pos.y,trackedObstacles.at(i).center.pos.z+0.5,
-          trackedObstacles.at(i).center.pos.a,0,1,0,0.3,centers.markers.size()+i,"Directions", visualization_msgs::Marker::ARROW);
-      dir_mkr.scale.x = 0.4;
-      if(i < dirs.markers.size())
-      {
-        dir_mkr.id = dirs.markers.at(i).id;
-        dirs.markers.at(i) = dir_mkr;
-      }
-      else
-        dirs.markers.push_back(dir_mkr);
+void ROSHelpers::InitMarkers(const int &nMarkers, visualization_msgs::MarkerArray &centers, visualization_msgs::MarkerArray &dirs,
+                             visualization_msgs::MarkerArray &text_info, visualization_msgs::MarkerArray &polygons,
+                             visualization_msgs::MarkerArray &trajectories) {
+    centers.markers.clear();
+    dirs.markers.clear();
+    text_info.markers.clear();
+    polygons.markers.clear();
+    trajectories.markers.clear();
+
+    for (int i = 0; i < nMarkers; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "CenterMarker", visualization_msgs::Marker::SPHERE);
+        centers.markers.push_back(mkr);
     }
 
-
-    //Text
-    visualization_msgs::Marker text_mkr;
-//    if(speed > 3.0)
-//      text_mkr = CreateGenMarker(trackedObstacles.at(i).center.pos.x+0.5,trackedObstacles.at(i).center.pos.y+0.5,trackedObstacles.at(i).center.pos.z+1,
-//          trackedObstacles.at(i).center.pos.a,1,0,0,0.75,centers.markers.size()*2+i,"InfoText", visualization_msgs::Marker::TEXT_VIEW_FACING);
-//    else
-    text_mkr = CreateGenMarker(trackedObstacles.at(i).center.pos.x+0.5,trackedObstacles.at(i).center.pos.y+0.5,trackedObstacles.at(i).center.pos.z+1,
-              trackedObstacles.at(i).center.pos.a,1,1,1,1.2,centers.markers.size()*2+i,"InfoText", visualization_msgs::Marker::TEXT_VIEW_FACING);
-
-    std::ostringstream str_out;
-    //str_out << trackedObstacles.at(i).id << " ( " << speed << " )" << " (" << trackedObstacles.at(i).distance_to_center << ")";
-    str_out << trackedObstacles.at(i).id << " (" << speed << ")";
-    text_mkr.text = str_out.str();
-
-    if(i < text_info.markers.size())
-    {
-      text_mkr.id = text_info.markers.at(i).id;
-      text_info.markers.at(i) = text_mkr;
+    for (int i = nMarkers; i < nMarkers * 2; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "Directions", visualization_msgs::Marker::ARROW);
+        dirs.markers.push_back(mkr);
     }
-    else
-      text_info.markers.push_back(text_mkr);
-
 
-    //Polygons
-    visualization_msgs::Marker poly_mkr = CreateGenMarker(0,0,0,0, 1,0.25,0.25,0.1,centers.markers.size()*3+i,"detected_polygons", visualization_msgs::Marker::LINE_STRIP);
+    for (int i = nMarkers * 2; i < nMarkers * 3; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "InfoText", visualization_msgs::Marker::TEXT_VIEW_FACING);
+        text_info.markers.push_back(mkr);
+    }
 
-    for(unsigned int p = 0; p < trackedObstacles.at(i).contour.size(); p++)
-    {
-      geometry_msgs::Point point;
-      point.x = trackedObstacles.at(i).contour.at(p).x;
-      point.y = trackedObstacles.at(i).contour.at(p).y;
-      point.z = trackedObstacles.at(i).contour.at(p).z;
-      poly_mkr.points.push_back(point);
+    for (int i = nMarkers * 3; i < nMarkers * 4; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "detected_polygons", visualization_msgs::Marker::LINE_STRIP);
+        polygons.markers.push_back(mkr);
     }
 
-    if(trackedObstacles.at(i).contour.size()>0)
-    {
-      geometry_msgs::Point point;
-      point.x = trackedObstacles.at(i).contour.at(0).x;
-      point.y = trackedObstacles.at(i).contour.at(0).y;
-      point.z = trackedObstacles.at(i).contour.at(0).z;
-      poly_mkr.points.push_back(point);
+    for (int i = nMarkers * 4; i < nMarkers * 5; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "tracked_trajectories", visualization_msgs::Marker::LINE_STRIP);
+        trajectories.markers.push_back(mkr);
     }
+}
 
-    if(i < polygons.markers.size())
-    {
-      poly_mkr.id =  polygons.markers.at(i).id;
-      polygons.markers.at(i) = poly_mkr;
+void ROSHelpers::InitMatchingMarkers(const int &nMarkers, visualization_msgs::MarkerArray &connections) {
+    connections.markers.clear();
+    for (int i = 0; i < nMarkers; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "matching_connections", visualization_msgs::Marker::LINE_STRIP);
+        connections.markers.push_back(mkr);
     }
-    else
-      polygons.markers.push_back(poly_mkr);
+}
 
+void ROSHelpers::ConvertMatchingMarkers(const std::vector<std::pair<PlannerHNS::WayPoint, PlannerHNS::WayPoint>> &match_list,
+                                        visualization_msgs::MarkerArray &tracked_traj_d, visualization_msgs::MarkerArray &tracked_traj,
+                                        int start_id) {
 
-    //Trajectories
-    visualization_msgs::Marker traj_mkr = CreateGenMarker(0,0,0,0,1,1,0,0.1,centers.markers.size()*4+i,"tracked_trajectories", visualization_msgs::Marker::LINE_STRIP);
+    tracked_traj = tracked_traj_d;
 
-    for(unsigned int p = 0; p < trackedObstacles.at(i).centers_list.size(); p++)
-    {
-      geometry_msgs::Point point;
-      point.x = trackedObstacles.at(i).centers_list.at(p).pos.x;
-      point.y = trackedObstacles.at(i).centers_list.at(p).pos.y;
-      point.z = trackedObstacles.at(i).centers_list.at(p).pos.z;
-      traj_mkr.points.push_back(point);
+    for (unsigned int i = 0; i < match_list.size(); i++) {
+        visualization_msgs::Marker match_mkr =
+            CreateGenMarker(0, 0, 0, 0, 1, 0, 0, 0.2, start_id + i, "matching_connections", visualization_msgs::Marker::LINE_STRIP);
+        geometry_msgs::Point point;
+        point.x = match_list.at(i).first.pos.x;
+        point.y = match_list.at(i).first.pos.y;
+        point.z = match_list.at(i).first.pos.z;
+        match_mkr.points.push_back(point);
+
+        point.x = match_list.at(i).second.pos.x;
+        point.y = match_list.at(i).second.pos.y;
+        point.z = match_list.at(i).second.pos.z;
+        match_mkr.points.push_back(point);
+
+        if (i < tracked_traj.markers.size())
+            tracked_traj.markers.at(i) = match_mkr;
+        else
+            tracked_traj.markers.push_back(match_mkr);
     }
+}
 
+int ROSHelpers::ConvertTrackedObjectsMarkers(const PlannerHNS::WayPoint &currState, const std::vector<PlannerHNS::DetectedObject> &trackedObstacles,
+                                             visualization_msgs::MarkerArray &centers_d, visualization_msgs::MarkerArray &dirs_d,
+                                             visualization_msgs::MarkerArray &text_info_d, visualization_msgs::MarkerArray &polygons_d,
+                                             visualization_msgs::MarkerArray &tracked_traj_d, visualization_msgs::MarkerArray &centers,
+                                             visualization_msgs::MarkerArray &dirs, visualization_msgs::MarkerArray &text_info,
+                                             visualization_msgs::MarkerArray &polygons, visualization_msgs::MarkerArray &tracked_traj) {
+
+    int i_next_id = 0;
+    centers = centers_d;
+    dirs = dirs_d;
+    text_info = text_info_d;
+    polygons = polygons_d;
+    tracked_traj = tracked_traj_d;
+
+    for (unsigned int i = 0; i < trackedObstacles.size(); i++) {
+        int speed = (trackedObstacles.at(i).center.v * 3.6);
+
+        // Update Stage
+        visualization_msgs::Marker center_mkr =
+            CreateGenMarker(trackedObstacles.at(i).center.pos.x, trackedObstacles.at(i).center.pos.y, trackedObstacles.at(i).center.pos.z,
+                            trackedObstacles.at(i).center.pos.a, 1, 0, 0, 0.5, i, "CenterMarker", visualization_msgs::Marker::SPHERE);
+        if (i < centers.markers.size()) {
+            center_mkr.id = centers.markers.at(i).id;
+            centers.markers.at(i) = center_mkr;
+        } else
+            centers.markers.push_back(center_mkr);
+
+        // Directions
+        if (trackedObstacles.at(i).bDirection) {
+            visualization_msgs::Marker dir_mkr = CreateGenMarker(trackedObstacles.at(i).center.pos.x, trackedObstacles.at(i).center.pos.y,
+                                                                 trackedObstacles.at(i).center.pos.z + 0.5, trackedObstacles.at(i).center.pos.a, 0, 1,
+                                                                 0, 0.3, centers.markers.size() + i, "Directions", visualization_msgs::Marker::ARROW);
+            dir_mkr.scale.x = 0.4;
+            if (i < dirs.markers.size()) {
+                dir_mkr.id = dirs.markers.at(i).id;
+                dirs.markers.at(i) = dir_mkr;
+            } else
+                dirs.markers.push_back(dir_mkr);
+        }
 
-    if(i < tracked_traj.markers.size())
-    {
-      traj_mkr.id = tracked_traj.markers.at(i).id ;
-      tracked_traj.markers.at(i) = traj_mkr;
-    }
-    else
-      tracked_traj.markers.push_back(traj_mkr);
+        // Text
+        visualization_msgs::Marker text_mkr;
+        //    if(speed > 3.0)
+        //      text_mkr =
+        //      CreateGenMarker(trackedObstacles.at(i).center.pos.x+0.5,trackedObstacles.at(i).center.pos.y+0.5,trackedObstacles.at(i).center.pos.z+1,
+        //          trackedObstacles.at(i).center.pos.a,1,0,0,0.75,centers.markers.size()*2+i,"InfoText",
+        //          visualization_msgs::Marker::TEXT_VIEW_FACING);
+        //    else
+        text_mkr = CreateGenMarker(trackedObstacles.at(i).center.pos.x + 0.5, trackedObstacles.at(i).center.pos.y + 0.5,
+                                   trackedObstacles.at(i).center.pos.z + 1, trackedObstacles.at(i).center.pos.a, 1, 1, 1, 1.2,
+                                   centers.markers.size() * 2 + i, "InfoText", visualization_msgs::Marker::TEXT_VIEW_FACING);
+
+        std::ostringstream str_out;
+        // str_out << trackedObstacles.at(i).id << " ( " << speed << " )" << " (" << trackedObstacles.at(i).distance_to_center << ")";
+        str_out << trackedObstacles.at(i).id << " (" << speed << ")";
+        text_mkr.text = str_out.str();
+
+        if (i < text_info.markers.size()) {
+            text_mkr.id = text_info.markers.at(i).id;
+            text_info.markers.at(i) = text_mkr;
+        } else
+            text_info.markers.push_back(text_mkr);
+
+        // Polygons
+        visualization_msgs::Marker poly_mkr = CreateGenMarker(0, 0, 0, 0, 1, 0.25, 0.25, 0.1, centers.markers.size() * 3 + i, "detected_polygons",
+                                                              visualization_msgs::Marker::LINE_STRIP);
+
+        for (unsigned int p = 0; p < trackedObstacles.at(i).contour.size(); p++) {
+            geometry_msgs::Point point;
+            point.x = trackedObstacles.at(i).contour.at(p).x;
+            point.y = trackedObstacles.at(i).contour.at(p).y;
+            point.z = trackedObstacles.at(i).contour.at(p).z;
+            poly_mkr.points.push_back(point);
+        }
 
-    i_next_id = traj_mkr.id;
+        if (trackedObstacles.at(i).contour.size() > 0) {
+            geometry_msgs::Point point;
+            point.x = trackedObstacles.at(i).contour.at(0).x;
+            point.y = trackedObstacles.at(i).contour.at(0).y;
+            point.z = trackedObstacles.at(i).contour.at(0).z;
+            poly_mkr.points.push_back(point);
+        }
 
-  }
+        if (i < polygons.markers.size()) {
+            poly_mkr.id = polygons.markers.at(i).id;
+            polygons.markers.at(i) = poly_mkr;
+        } else
+            polygons.markers.push_back(poly_mkr);
+
+        // Trajectories
+        visualization_msgs::Marker traj_mkr =
+            CreateGenMarker(0, 0, 0, 0, 1, 1, 0, 0.1, centers.markers.size() * 4 + i, "tracked_trajectories", visualization_msgs::Marker::LINE_STRIP);
+
+        for (unsigned int p = 0; p < trackedObstacles.at(i).centers_list.size(); p++) {
+            geometry_msgs::Point point;
+            point.x = trackedObstacles.at(i).centers_list.at(p).pos.x;
+            point.y = trackedObstacles.at(i).centers_list.at(p).pos.y;
+            point.z = trackedObstacles.at(i).centers_list.at(p).pos.z;
+            traj_mkr.points.push_back(point);
+        }
 
-  return i_next_id +1;
-}
+        if (i < tracked_traj.markers.size()) {
+            traj_mkr.id = tracked_traj.markers.at(i).id;
+            tracked_traj.markers.at(i) = traj_mkr;
+        } else
+            tracked_traj.markers.push_back(traj_mkr);
 
-void ROSHelpers::CreateCircleMarker(const PlannerHNS::WayPoint& _center, const double& radius, const int& start_id, visualization_msgs::Marker& circle_points)
-{
-  circle_points = CreateGenMarker(0,0,0,0,1,1,1,0.2,start_id,"Detection_Circles", visualization_msgs::Marker::LINE_STRIP);
-  for (float i = 0; i < M_PI*2.0+0.05; i+=0.05)
-  {
-    geometry_msgs::Point point;
-    point.x = _center.pos.x + (radius * cos(i));
-    point.y = _center.pos.y + (radius * sin(i));
-    point.z = _center.pos.z;
-    circle_points.points.push_back(point);
-  }
-}
+        i_next_id = traj_mkr.id;
+    }
 
-void ROSHelpers::InitPredMarkers(const int& nMarkers, visualization_msgs::MarkerArray& paths)
-{
-  paths.markers.clear();
-  for(int i=0; i<nMarkers; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"Predicted_Trajectories", visualization_msgs::Marker::LINE_STRIP);
-    paths.markers.push_back(mkr);
-  }
+    return i_next_id + 1;
 }
 
-void ROSHelpers::InitCurbsMarkers(const int& nMarkers, visualization_msgs::MarkerArray& curbs)
-{
-  curbs.markers.clear();
-  for(int i=0; i<nMarkers; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"map_detected_curbs", visualization_msgs::Marker::SPHERE);
-    curbs.markers.push_back(mkr);
-  }
+void ROSHelpers::CreateCircleMarker(const PlannerHNS::WayPoint &_center, const double &radius, const int &start_id,
+                                    visualization_msgs::Marker &circle_points) {
+    circle_points = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 0.2, start_id, "Detection_Circles", visualization_msgs::Marker::LINE_STRIP);
+    for (float i = 0; i < M_PI * 2.0 + 0.05; i += 0.05) {
+        geometry_msgs::Point point;
+        point.x = _center.pos.x + (radius * cos(i));
+        point.y = _center.pos.y + (radius * sin(i));
+        point.z = _center.pos.z;
+        circle_points.points.push_back(point);
+    }
 }
 
-void ROSHelpers::ConvertPredictedTrqajectoryMarkers(std::vector<std::vector<PlannerHNS::WayPoint> >& paths,visualization_msgs::MarkerArray& path_markers, visualization_msgs::MarkerArray& path_markers_d)
-{
-
-  path_markers = path_markers_d;
-  for(unsigned int i = 0; i < paths.size(); i++)
-  {
-    double additional_z = 0;
-    double basic_color = 0.5;
-    double prop = 1.0;
-    bool bCurrent = false;
-//    if(paths.at(i).size()>0)
-//    {
-//
-//      prop = paths.at(i).at(0).collisionCost;
-//      if(prop < 0.5)
-//        continue;
-//
-//      if(prop > 0.5)
-//      {
-//        additional_z = prop;
-//        bCurrent = true;
-//      }
-//    }
-
-
-//    double r = 0, g = 0, b = 0;
-//    if(bCurrent == true)
-//    {
-//      r = basic_color+additional_z;
-//      g = basic_color+additional_z;
-//      b = basic_color+additional_z;
-//    }
-//    else if(i == 0)
-//    {
-//      r = basic_color+additional_z;
-//    }
-//    else if(i == 1)
-//    {
-//      g = basic_color+additional_z;
-//    }
-//    else if(i == 2)
-//    {
-//      b = basic_color+additional_z;
-//    }
-//    else if(i == 3)
-//    {
-//      r = basic_color+additional_z;
-//      b = basic_color+additional_z;
-//    }
-//    else
-//    {
-//      g = basic_color+additional_z;
-//      b = basic_color+additional_z;
-//    }
-//
-//    visualization_msgs::Marker path_mkr = CreateGenMarker(0,0,0,0,r,g,b,0.1,i,"Predicted_Trajectories", visualization_msgs::Marker::LINE_STRIP);
-
-    visualization_msgs::Marker path_mkr = CreateGenMarker(0,0,0,0,1.0*prop,0.1*prop,0.1*prop,0.1,i,"Predicted_Trajectories", visualization_msgs::Marker::LINE_STRIP);
-
-
-    for(unsigned int p = 0; p < paths.at(i).size(); p++)
-    {
-      geometry_msgs::Point point;
-      point.x = paths.at(i).at(p).pos.x;
-      point.y = paths.at(i).at(p).pos.y;
-      point.z = paths.at(i).at(p).pos.z + additional_z;
-      path_mkr.points.push_back(point);
+void ROSHelpers::InitPredMarkers(const int &nMarkers, visualization_msgs::MarkerArray &paths) {
+    paths.markers.clear();
+    for (int i = 0; i < nMarkers; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "Predicted_Trajectories", visualization_msgs::Marker::LINE_STRIP);
+        paths.markers.push_back(mkr);
     }
-
-    if(i < path_markers.markers.size())
-      path_markers.markers.at(i) = path_mkr;
-    else
-      path_markers.markers.push_back(path_mkr);
-  }
 }
 
-void ROSHelpers::ConvertCurbsMarkers(const std::vector<PlannerHNS::DetectedObject>& curbs, visualization_msgs::MarkerArray& curbs_markers, visualization_msgs::MarkerArray& curbs_markers_d)
-{
-
-  curbs_markers = curbs_markers_d;
-  for(unsigned int i = 0; i < curbs.size(); i++)
-  {
-    if(curbs.at(i).contour.size() > 0)
-    {
-      visualization_msgs::Marker curb_mkr = CreateGenMarker(curbs.at(i).contour.at(0).x,curbs.at(i).contour.at(0).y,curbs.at(i).contour.at(0).z,0,1,0.54,0,0.2,i,"map_detected_curbs", visualization_msgs::Marker::SPHERE);
-
-      if(i < curbs_markers.markers.size())
-        curbs_markers.markers.at(i) = curb_mkr;
-      else
-        curbs_markers.markers.push_back(curb_mkr);
+void ROSHelpers::InitCurbsMarkers(const int &nMarkers, visualization_msgs::MarkerArray &curbs) {
+    curbs.markers.clear();
+    for (int i = 0; i < nMarkers; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "map_detected_curbs", visualization_msgs::Marker::SPHERE);
+        curbs.markers.push_back(mkr);
     }
-  }
 }
 
-void ROSHelpers::InitCollisionPointsMarkers(const int& nMarkers, visualization_msgs::MarkerArray& col_points)
-{
-  col_points.markers.clear();
-  for(int i=0; i<nMarkers; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,1,i,"collision_points_rviz", visualization_msgs::Marker::SPHERE);
-    col_points.markers.push_back(mkr);
-  }
+void ROSHelpers::ConvertPredictedTrqajectoryMarkers(std::vector<std::vector<PlannerHNS::WayPoint>> &paths,
+                                                    visualization_msgs::MarkerArray &path_markers, visualization_msgs::MarkerArray &path_markers_d) {
+
+    path_markers = path_markers_d;
+    for (unsigned int i = 0; i < paths.size(); i++) {
+        double additional_z = 0;
+        double basic_color = 0.5;
+        double prop = 1.0;
+        bool bCurrent = false;
+        //    if(paths.at(i).size()>0)
+        //    {
+        //
+        //      prop = paths.at(i).at(0).collisionCost;
+        //      if(prop < 0.5)
+        //        continue;
+        //
+        //      if(prop > 0.5)
+        //      {
+        //        additional_z = prop;
+        //        bCurrent = true;
+        //      }
+        //    }
+
+        //    double r = 0, g = 0, b = 0;
+        //    if(bCurrent == true)
+        //    {
+        //      r = basic_color+additional_z;
+        //      g = basic_color+additional_z;
+        //      b = basic_color+additional_z;
+        //    }
+        //    else if(i == 0)
+        //    {
+        //      r = basic_color+additional_z;
+        //    }
+        //    else if(i == 1)
+        //    {
+        //      g = basic_color+additional_z;
+        //    }
+        //    else if(i == 2)
+        //    {
+        //      b = basic_color+additional_z;
+        //    }
+        //    else if(i == 3)
+        //    {
+        //      r = basic_color+additional_z;
+        //      b = basic_color+additional_z;
+        //    }
+        //    else
+        //    {
+        //      g = basic_color+additional_z;
+        //      b = basic_color+additional_z;
+        //    }
+        //
+        //    visualization_msgs::Marker path_mkr = CreateGenMarker(0,0,0,0,r,g,b,0.1,i,"Predicted_Trajectories",
+        //    visualization_msgs::Marker::LINE_STRIP);
+
+        visualization_msgs::Marker path_mkr =
+            CreateGenMarker(0, 0, 0, 0, 1.0 * prop, 0.1 * prop, 0.1 * prop, 0.1, i, "Predicted_Trajectories", visualization_msgs::Marker::LINE_STRIP);
+
+        for (unsigned int p = 0; p < paths.at(i).size(); p++) {
+            geometry_msgs::Point point;
+            point.x = paths.at(i).at(p).pos.x;
+            point.y = paths.at(i).at(p).pos.y;
+            point.z = paths.at(i).at(p).pos.z + additional_z;
+            path_mkr.points.push_back(point);
+        }
+
+        if (i < path_markers.markers.size())
+            path_markers.markers.at(i) = path_mkr;
+        else
+            path_markers.markers.push_back(path_mkr);
+    }
 }
 
-void ROSHelpers::ConvertCollisionPointsMarkers(const std::vector<PlannerHNS::WayPoint>& col_points, visualization_msgs::MarkerArray& collision_markers, visualization_msgs::MarkerArray& collision_markers_d)
-{
-  collision_markers = collision_markers_d;
-  for(unsigned int i = 0; i < col_points.size(); i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(col_points.at(i).pos.x, col_points.at(i).pos.y, col_points.at(i).pos.z,0,1,0,0,0.5,i,"collision_points_rviz", visualization_msgs::Marker::SPHERE);
+void ROSHelpers::ConvertCurbsMarkers(const std::vector<PlannerHNS::DetectedObject> &curbs, visualization_msgs::MarkerArray &curbs_markers,
+                                     visualization_msgs::MarkerArray &curbs_markers_d) {
 
-    if(i < collision_markers.markers.size())
-      collision_markers.markers.at(i) = mkr;
-    else
-      collision_markers.markers.push_back(mkr);
+    curbs_markers = curbs_markers_d;
+    for (unsigned int i = 0; i < curbs.size(); i++) {
+        if (curbs.at(i).contour.size() > 0) {
+            visualization_msgs::Marker curb_mkr =
+                CreateGenMarker(curbs.at(i).contour.at(0).x, curbs.at(i).contour.at(0).y, curbs.at(i).contour.at(0).z, 0, 1, 0.54, 0, 0.2, i,
+                                "map_detected_curbs", visualization_msgs::Marker::SPHERE);
 
-  }
+            if (i < curbs_markers.markers.size())
+                curbs_markers.markers.at(i) = curb_mkr;
+            else
+                curbs_markers.markers.push_back(curb_mkr);
+        }
+    }
 }
 
-void ROSHelpers::ConvertFromPlannerHToAutowarePathFormat(const std::vector<PlannerHNS::WayPoint>& path, const int& iStart,
-    autoware_msgs::Lane& trajectory)
-{
-  trajectory.waypoints.clear();
-  for(unsigned int i=iStart; i < path.size(); i++)
-  {
-    autoware_msgs::Waypoint wp;
-    wp.pose.pose.position.x = path.at(i).pos.x;
-    wp.pose.pose.position.y = path.at(i).pos.y;
-    wp.pose.pose.position.z = path.at(i).pos.z;
-    wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(path.at(i).pos.a));
-    wp.twist.twist.linear.x = path.at(i).v;
-    if(path.at(i).bDir == FORWARD_DIR)
-      wp.dtlane.dir = 0;
-    else if(path.at(i).bDir == FORWARD_LEFT_DIR)
-      wp.dtlane.dir = 1;
-    else if(path.at(i).bDir == FORWARD_RIGHT_DIR)
-      wp.dtlane.dir = 2;
-    //PlannerHNS::GPSPoint p = path.at(i).pos;
-    //std::cout << p.ToString() << std::endl;
-    trajectory.waypoints.push_back(wp);
-  }
+void ROSHelpers::InitCollisionPointsMarkers(const int &nMarkers, visualization_msgs::MarkerArray &col_points) {
+    col_points.markers.clear();
+    for (int i = 0; i < nMarkers; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 1, i, "collision_points_rviz", visualization_msgs::Marker::SPHERE);
+        col_points.markers.push_back(mkr);
+    }
 }
 
-void ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(const PlannerHNS::RoadNetwork& map,  visualization_msgs::MarkerArray& markerArray, double scale)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "road_network_vector_map";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 0.25 * scale;
-  std_msgs::ColorRGBA roll_color, total_color, curr_color;
-  roll_color.r = 1;
-  roll_color.g = 1;
-  roll_color.b = 1;
-  roll_color.a = 0.5;
-
-  lane_waypoint_marker.color = roll_color;
-  lane_waypoint_marker.frame_locked = false;
-
-  markerArray.markers.clear();
-
-  for(unsigned int i = 0; i< map.roadSegments.size(); i++)
-  {
-    for(unsigned int j = 0; j < map.roadSegments.at(i).Lanes.size(); j++)
-    {
-      lane_waypoint_marker.points.clear();
-
-      lane_waypoint_marker.id = map.roadSegments.at(i).Lanes.at(j).id;
-      for(unsigned int p = 0; p < map.roadSegments.at(i).Lanes.at(j).points.size(); p++)
-      {
-        geometry_msgs::Point point;
+void ROSHelpers::ConvertCollisionPointsMarkers(const std::vector<PlannerHNS::WayPoint> &col_points,
+                                               visualization_msgs::MarkerArray &collision_markers,
+                                               visualization_msgs::MarkerArray &collision_markers_d) {
+    collision_markers = collision_markers_d;
+    for (unsigned int i = 0; i < col_points.size(); i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(col_points.at(i).pos.x, col_points.at(i).pos.y, col_points.at(i).pos.z, 0, 1, 0, 0, 0.5, i,
+                                                         "collision_points_rviz", visualization_msgs::Marker::SPHERE);
 
-
-
-          point.x = map.roadSegments.at(i).Lanes.at(j).points.at(p).pos.x;
-          point.y = map.roadSegments.at(i).Lanes.at(j).points.at(p).pos.y;
-          point.z = map.roadSegments.at(i).Lanes.at(j).points.at(p).pos.z;
-
-          lane_waypoint_marker.points.push_back(point);
-      }
-
-      markerArray.markers.push_back(lane_waypoint_marker);
+        if (i < collision_markers.markers.size())
+            collision_markers.markers.at(i) = mkr;
+        else
+            collision_markers.markers.push_back(mkr);
     }
-  }
 }
 
-void ROSHelpers::InitPredParticlesMarkers(const int& nMarkers, visualization_msgs::MarkerArray& paths)
-{
-  paths.markers.clear();
-  for(int i=0; i<nMarkers; i++)
-  {
-    visualization_msgs::Marker mkr = CreateGenMarker(0,0,0,0,1,1,1,0.05,i,"Particles", visualization_msgs::Marker::ARROW);
-    mkr.scale.x = 0.3;
-    paths.markers.push_back(mkr);
-  }
+void ROSHelpers::ConvertFromPlannerHToAutowarePathFormat(const std::vector<PlannerHNS::WayPoint> &path, const int &iStart,
+                                                         autoware_msgs::Lane &trajectory) {
+    trajectory.waypoints.clear();
+    for (unsigned int i = iStart; i < path.size(); i++) {
+        autoware_msgs::Waypoint wp;
+        wp.pose.pose.position.x = path.at(i).pos.x;
+        wp.pose.pose.position.y = path.at(i).pos.y;
+        wp.pose.pose.position.z = path.at(i).pos.z;
+        wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(path.at(i).pos.a));
+        wp.twist.twist.linear.x = path.at(i).v;
+        if (path.at(i).bDir == FORWARD_DIR)
+            wp.dtlane.dir = 0;
+        else if (path.at(i).bDir == FORWARD_LEFT_DIR)
+            wp.dtlane.dir = 1;
+        else if (path.at(i).bDir == FORWARD_RIGHT_DIR)
+            wp.dtlane.dir = 2;
+        // PlannerHNS::GPSPoint p = path.at(i).pos;
+        // std::cout << p.ToString() << std::endl;
+        trajectory.waypoints.push_back(wp);
+    }
 }
 
-void ROSHelpers::ConvertParticles(std::vector<PlannerHNS::WayPoint>& points, visualization_msgs::MarkerArray& part_mkrs, visualization_msgs::MarkerArray& part_markers_d)
-{
-  part_mkrs = part_markers_d;
-  for(unsigned int i = 0; i < points.size(); i++)
-  {
-    visualization_msgs::Marker mkr;
-    if(points.at(i).bDir == PlannerHNS::STANDSTILL_DIR)
-      mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y,points.at(i).pos.z,points.at(i).pos.a,1,0,0,0.05,i,"Particles", visualization_msgs::Marker::ARROW);
-    else if(points.at(i).bDir == PlannerHNS::FORWARD_DIR)
-      mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y,points.at(i).pos.z,points.at(i).pos.a,1,1,1,0.05,i,"Particles", visualization_msgs::Marker::ARROW);
-    else if(points.at(i).bDir == PlannerHNS::FORWARD_RIGHT_DIR)
-      mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y,points.at(i).pos.z,points.at(i).pos.a,0,1,0,0.05,i,"Particles", visualization_msgs::Marker::ARROW);
-    else if(points.at(i).bDir == PlannerHNS::FORWARD_LEFT_DIR)
-      mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y,points.at(i).pos.z,points.at(i).pos.a,0,0,1,0.05,i,"Particles", visualization_msgs::Marker::ARROW);
-    else if(points.at(i).bDir == PlannerHNS::BACKWARD_DIR)
-      mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y,points.at(i).pos.z,points.at(i).pos.a,1,0,1,0.05,i,"Particles", visualization_msgs::Marker::ARROW);
-    else
-      mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y,points.at(i).pos.z,points.at(i).pos.a,1,1,0,0.05,i,"Particles", visualization_msgs::Marker::ARROW);
-
-    mkr.scale.x = 0.3;
-    if(i < part_mkrs.markers.size())
-      part_mkrs.markers.at(i) = mkr;
-    else
-      part_mkrs.markers.push_back(mkr);
-  }
+void ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(const PlannerHNS::RoadNetwork &map, visualization_msgs::MarkerArray &markerArray,
+                                                                    double scale) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "road_network_vector_map";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 0.25 * scale;
+    std_msgs::ColorRGBA roll_color, total_color, curr_color;
+    roll_color.r = 1;
+    roll_color.g = 1;
+    roll_color.b = 1;
+    roll_color.a = 0.5;
+
+    lane_waypoint_marker.color = roll_color;
+    lane_waypoint_marker.frame_locked = false;
+
+    markerArray.markers.clear();
+
+    for (unsigned int i = 0; i < map.roadSegments.size(); i++) {
+        for (unsigned int j = 0; j < map.roadSegments.at(i).Lanes.size(); j++) {
+            lane_waypoint_marker.points.clear();
+
+            lane_waypoint_marker.id = map.roadSegments.at(i).Lanes.at(j).id;
+            for (unsigned int p = 0; p < map.roadSegments.at(i).Lanes.at(j).points.size(); p++) {
+                geometry_msgs::Point point;
+
+                point.x = map.roadSegments.at(i).Lanes.at(j).points.at(p).pos.x;
+                point.y = map.roadSegments.at(i).Lanes.at(j).points.at(p).pos.y;
+                point.z = map.roadSegments.at(i).Lanes.at(j).points.at(p).pos.z;
+
+                lane_waypoint_marker.points.push_back(point);
+            }
+
+            markerArray.markers.push_back(lane_waypoint_marker);
+        }
+    }
 }
 
-void ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(const std::vector<PlannerHNS::GPSPoint>& safety_rect,
-    visualization_msgs::Marker& marker)
-{
-  //if(safety_rect.size() != 4) return;
-
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "global_lane_array_marker";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 0.2;
-  lane_waypoint_marker.scale.y = 0.2;
-  //lane_waypoint_marker.scale.z = 0.1;
-  lane_waypoint_marker.frame_locked = false;
-  lane_waypoint_marker.color.r = 0.0;
-  lane_waypoint_marker.color.g = 1.0;
-  lane_waypoint_marker.color.b = 0.0;
-  lane_waypoint_marker.color.a = 0.6;
-
-  for(unsigned int i = 0; i < safety_rect.size(); i++)
-  {
-    geometry_msgs::Point p;
-    p.x = safety_rect.at(i).x;
-    p.y = safety_rect.at(i).y;
-    p.z = safety_rect.at(i).z;
-
-    lane_waypoint_marker.points.push_back(p);
-  }
-  if(safety_rect.size() > 0)
-  {
-    geometry_msgs::Point p;
-    p.x = safety_rect.at(0).x;
-    p.y = safety_rect.at(0).y;
-    p.z = safety_rect.at(0).z;
-    lane_waypoint_marker.points.push_back(p);
-  }
-
-//  geometry_msgs::Point p1, p2,p3,p4;
-//  p1.x = safety_rect.at(0).x;
-//  p1.y = safety_rect.at(0).y;
-//  p1.z = safety_rect.at(0).z;
-//
-//  p2.x = safety_rect.at(1).x;
-//  p2.y = safety_rect.at(1).y;
-//  p2.z = safety_rect.at(1).z;
-//
-//  p3.x = safety_rect.at(2).x;
-//  p3.y = safety_rect.at(2).y;
-//  p3.z = safety_rect.at(2).z;
-//
-//  p4.x = safety_rect.at(3).x;
-//  p4.y = safety_rect.at(3).y;
-//  p4.z = safety_rect.at(3).z;
-//
-//  lane_waypoint_marker.points.push_back(p1);
-//  lane_waypoint_marker.points.push_back(p2);
-//  lane_waypoint_marker.points.push_back(p3);
-//  lane_waypoint_marker.points.push_back(p4);
-//  lane_waypoint_marker.points.push_back(p1);
-
-   marker = lane_waypoint_marker;
-
+void ROSHelpers::InitPredParticlesMarkers(const int &nMarkers, visualization_msgs::MarkerArray &paths) {
+    paths.markers.clear();
+    for (int i = 0; i < nMarkers; i++) {
+        visualization_msgs::Marker mkr = CreateGenMarker(0, 0, 0, 0, 1, 1, 1, 0.05, i, "Particles", visualization_msgs::Marker::ARROW);
+        mkr.scale.x = 0.3;
+        paths.markers.push_back(mkr);
+    }
 }
 
-void ROSHelpers::TrajectoriesToMarkers(const std::vector<std::vector<std::vector<PlannerHNS::WayPoint> > >& paths, visualization_msgs::MarkerArray& markerArray)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "global_lane_array_marker";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 0.1;
-  lane_waypoint_marker.scale.y = 0.1;
-  //lane_waypoint_marker.scale.z = 0.1;
-  lane_waypoint_marker.frame_locked = false;
-  std_msgs::ColorRGBA  total_color, curr_color;
-
-  int count = 0;
-  for (unsigned int il = 0; il < paths.size(); il++)
-  {
-    for (unsigned int i = 0; i < paths.at(il).size(); i++)
-    {
-      lane_waypoint_marker.points.clear();
-      lane_waypoint_marker.id = count;
-
-      for (unsigned int j=0; j < paths.at(il).at(i).size(); j++)
-      {
-        geometry_msgs::Point point;
-
-        point.x = paths.at(il).at(i).at(j).pos.x;
-        point.y = paths.at(il).at(i).at(j).pos.y;
-        point.z = paths.at(il).at(i).at(j).pos.z;
-
-        lane_waypoint_marker.points.push_back(point);
-      }
-
-      lane_waypoint_marker.color.a = 0.9;
-
-      lane_waypoint_marker.color.r = 0.0;
-      lane_waypoint_marker.color.g = 1.0;
-      lane_waypoint_marker.color.b = 0.0;
+void ROSHelpers::ConvertParticles(std::vector<PlannerHNS::WayPoint> &points, visualization_msgs::MarkerArray &part_mkrs,
+                                  visualization_msgs::MarkerArray &part_markers_d) {
+    part_mkrs = part_markers_d;
+    for (unsigned int i = 0; i < points.size(); i++) {
+        visualization_msgs::Marker mkr;
+        if (points.at(i).bDir == PlannerHNS::STANDSTILL_DIR)
+            mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y, points.at(i).pos.z, points.at(i).pos.a, 1, 0, 0, 0.05, i, "Particles",
+                                  visualization_msgs::Marker::ARROW);
+        else if (points.at(i).bDir == PlannerHNS::FORWARD_DIR)
+            mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y, points.at(i).pos.z, points.at(i).pos.a, 1, 1, 1, 0.05, i, "Particles",
+                                  visualization_msgs::Marker::ARROW);
+        else if (points.at(i).bDir == PlannerHNS::FORWARD_RIGHT_DIR)
+            mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y, points.at(i).pos.z, points.at(i).pos.a, 0, 1, 0, 0.05, i, "Particles",
+                                  visualization_msgs::Marker::ARROW);
+        else if (points.at(i).bDir == PlannerHNS::FORWARD_LEFT_DIR)
+            mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y, points.at(i).pos.z, points.at(i).pos.a, 0, 0, 1, 0.05, i, "Particles",
+                                  visualization_msgs::Marker::ARROW);
+        else if (points.at(i).bDir == PlannerHNS::BACKWARD_DIR)
+            mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y, points.at(i).pos.z, points.at(i).pos.a, 1, 0, 1, 0.05, i, "Particles",
+                                  visualization_msgs::Marker::ARROW);
+        else
+            mkr = CreateGenMarker(points.at(i).pos.x, points.at(i).pos.y, points.at(i).pos.z, points.at(i).pos.a, 1, 1, 0, 0.05, i, "Particles",
+                                  visualization_msgs::Marker::ARROW);
 
-      markerArray.markers.push_back(lane_waypoint_marker);
-      count++;
+        mkr.scale.x = 0.3;
+        if (i < part_mkrs.markers.size())
+            part_mkrs.markers.at(i) = mkr;
+        else
+            part_mkrs.markers.push_back(mkr);
     }
-  }
 }
 
-void ROSHelpers::TrajectoriesToColoredMarkers(const std::vector<std::vector<PlannerHNS::WayPoint> >& paths, const std::vector<PlannerHNS::TrajectoryCost>& traj_costs,const int& iClosest, visualization_msgs::MarkerArray& markerArray)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "local_lane_array_marker_colored";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 0.1;
-  lane_waypoint_marker.scale.y = 0.1;
-  //lane_waypoint_marker.scale.z = 0.1;
-  lane_waypoint_marker.color.a = 0.9;
-  lane_waypoint_marker.color.r = 1.0;
-  lane_waypoint_marker.color.g = 1.0;
-  lane_waypoint_marker.color.b = 1.0;
-  lane_waypoint_marker.frame_locked = false;
-
-  int count = 0;
-  for (unsigned int i = 0; i < paths.size(); i++)
-  {
-    lane_waypoint_marker.points.clear();
-    lane_waypoint_marker.id = count;
-
-    for (unsigned int j=0; j < paths.at(i).size(); j++)
-    {
-      geometry_msgs::Point point;
-
-      point.x = paths.at(i).at(j).pos.x;
-      point.y = paths.at(i).at(j).pos.y;
-      point.z = paths.at(i).at(j).pos.z;
-
-      lane_waypoint_marker.points.push_back(point);
+void ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(const std::vector<PlannerHNS::GPSPoint> &safety_rect,
+                                                            visualization_msgs::Marker &marker) {
+    // if(safety_rect.size() != 4) return;
+
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "global_lane_array_marker";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 0.2;
+    lane_waypoint_marker.scale.y = 0.2;
+    // lane_waypoint_marker.scale.z = 0.1;
+    lane_waypoint_marker.frame_locked = false;
+    lane_waypoint_marker.color.r = 0.0;
+    lane_waypoint_marker.color.g = 1.0;
+    lane_waypoint_marker.color.b = 0.0;
+    lane_waypoint_marker.color.a = 0.6;
+
+    for (unsigned int i = 0; i < safety_rect.size(); i++) {
+        geometry_msgs::Point p;
+        p.x = safety_rect.at(i).x;
+        p.y = safety_rect.at(i).y;
+        p.z = safety_rect.at(i).z;
+
+        lane_waypoint_marker.points.push_back(p);
     }
-
-    lane_waypoint_marker.color.b = 0;
-
-    if(traj_costs.size() == paths.size())
-    {
-      float norm_cost = traj_costs.at(i).cost * paths.size();
-      if(norm_cost <= 1.0)
-      {
-        lane_waypoint_marker.color.r = norm_cost;
-        lane_waypoint_marker.color.g = 1.0;
-      }
-      else if(norm_cost > 1.0)
-      {
-        lane_waypoint_marker.color.r = 1.0;
-        lane_waypoint_marker.color.g = 2.0 - norm_cost;
-      }
-    }
-    else
-    {
-      lane_waypoint_marker.color.r = 1.0;
-      lane_waypoint_marker.color.g = 0.0;
+    if (safety_rect.size() > 0) {
+        geometry_msgs::Point p;
+        p.x = safety_rect.at(0).x;
+        p.y = safety_rect.at(0).y;
+        p.z = safety_rect.at(0).z;
+        lane_waypoint_marker.points.push_back(p);
     }
 
-    if(traj_costs.at(i).bBlocked)
-    {
-      lane_waypoint_marker.color.r = 1.0;
-      lane_waypoint_marker.color.g = 0.0;
-      lane_waypoint_marker.color.b = 0.0;
-    }
+    //  geometry_msgs::Point p1, p2,p3,p4;
+    //  p1.x = safety_rect.at(0).x;
+    //  p1.y = safety_rect.at(0).y;
+    //  p1.z = safety_rect.at(0).z;
+    //
+    //  p2.x = safety_rect.at(1).x;
+    //  p2.y = safety_rect.at(1).y;
+    //  p2.z = safety_rect.at(1).z;
+    //
+    //  p3.x = safety_rect.at(2).x;
+    //  p3.y = safety_rect.at(2).y;
+    //  p3.z = safety_rect.at(2).z;
+    //
+    //  p4.x = safety_rect.at(3).x;
+    //  p4.y = safety_rect.at(3).y;
+    //  p4.z = safety_rect.at(3).z;
+    //
+    //  lane_waypoint_marker.points.push_back(p1);
+    //  lane_waypoint_marker.points.push_back(p2);
+    //  lane_waypoint_marker.points.push_back(p3);
+    //  lane_waypoint_marker.points.push_back(p4);
+    //  lane_waypoint_marker.points.push_back(p1);
+
+    marker = lane_waypoint_marker;
+}
 
-    if(i == iClosest)
-    {
-      lane_waypoint_marker.color.r = 1.0;
-      lane_waypoint_marker.color.g = 0.0;
-      lane_waypoint_marker.color.b = 1.0;
+void ROSHelpers::TrajectoriesToMarkers(const std::vector<std::vector<std::vector<PlannerHNS::WayPoint>>> &paths,
+                                       visualization_msgs::MarkerArray &markerArray) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "global_lane_array_marker";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 0.1;
+    lane_waypoint_marker.scale.y = 0.1;
+    // lane_waypoint_marker.scale.z = 0.1;
+    lane_waypoint_marker.frame_locked = false;
+    std_msgs::ColorRGBA total_color, curr_color;
+
+    int count = 0;
+    for (unsigned int il = 0; il < paths.size(); il++) {
+        for (unsigned int i = 0; i < paths.at(il).size(); i++) {
+            lane_waypoint_marker.points.clear();
+            lane_waypoint_marker.id = count;
+
+            for (unsigned int j = 0; j < paths.at(il).at(i).size(); j++) {
+                geometry_msgs::Point point;
+
+                point.x = paths.at(il).at(i).at(j).pos.x;
+                point.y = paths.at(il).at(i).at(j).pos.y;
+                point.z = paths.at(il).at(i).at(j).pos.z;
+
+                lane_waypoint_marker.points.push_back(point);
+            }
+
+            lane_waypoint_marker.color.a = 0.9;
+
+            lane_waypoint_marker.color.r = 0.0;
+            lane_waypoint_marker.color.g = 1.0;
+            lane_waypoint_marker.color.b = 0.0;
+
+            markerArray.markers.push_back(lane_waypoint_marker);
+            count++;
+        }
     }
-
-    markerArray.markers.push_back(lane_waypoint_marker);
-    count++;
-  }
 }
 
-void ROSHelpers::ConvertFromPlannerHToAutowareVisualizePathFormat(const std::vector<PlannerHNS::WayPoint>& curr_path,
-    const std::vector<std::vector<std::vector<PlannerHNS::WayPoint> > >& paths, const PlannerHNS::LocalPlannerH& localPlanner,
-      visualization_msgs::MarkerArray& markerArray)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "global_lane_array_marker";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 0.1;
-  lane_waypoint_marker.scale.y = 0.1;
-  //lane_waypoint_marker.scale.z = 0.1;
-  lane_waypoint_marker.frame_locked = false;
-  std_msgs::ColorRGBA  total_color, curr_color;
-
-
-  int count = 0;
-  for (unsigned int il = 0; il < paths.size(); il++)
-  {
-    for (unsigned int i = 0; i < paths.at(il).size(); i++)
-    {
-      lane_waypoint_marker.points.clear();
-      lane_waypoint_marker.id = count;
-
-      for (unsigned int j=0; j < paths.at(il).at(i).size(); j++)
-      {
-        geometry_msgs::Point point;
+void ROSHelpers::TrajectoriesToColoredMarkers(const std::vector<std::vector<PlannerHNS::WayPoint>> &paths,
+                                              const std::vector<PlannerHNS::TrajectoryCost> &traj_costs, const int &iClosest,
+                                              visualization_msgs::MarkerArray &markerArray) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "local_lane_array_marker_colored";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 0.1;
+    lane_waypoint_marker.scale.y = 0.1;
+    // lane_waypoint_marker.scale.z = 0.1;
+    lane_waypoint_marker.color.a = 0.9;
+    lane_waypoint_marker.color.r = 1.0;
+    lane_waypoint_marker.color.g = 1.0;
+    lane_waypoint_marker.color.b = 1.0;
+    lane_waypoint_marker.frame_locked = false;
+    lane_waypoint_marker.scale.x = 1.0;
+    lane_waypoint_marker.scale.y = 1.0;
+    lane_waypoint_marker.scale.z = 1.0;
+
+    int count = 0;
+    for (unsigned int i = 0; i < paths.size(); i++) {
+        lane_waypoint_marker.points.clear();
+        lane_waypoint_marker.id = count;
+
+        for (unsigned int j = 0; j < paths.at(i).size(); j++) {
+            geometry_msgs::Point point;
+
+            point.x = paths.at(i).at(j).pos.x;
+            point.y = paths.at(i).at(j).pos.y;
+            point.z = paths.at(i).at(j).pos.z;
+
+            lane_waypoint_marker.points.push_back(point);
+        }
 
-        point.x = paths.at(il).at(i).at(j).pos.x;
-        point.y = paths.at(il).at(i).at(j).pos.y;
-        point.z = paths.at(il).at(i).at(j).pos.z;
+        lane_waypoint_marker.color.b = 0;
 
-        lane_waypoint_marker.points.push_back(point);
-      }
+        if (traj_costs.size() == paths.size()) {
+            float norm_cost = traj_costs.at(i).cost * paths.size();
+            if (norm_cost <= 1.0) {
+                lane_waypoint_marker.color.r = norm_cost;
+                lane_waypoint_marker.color.g = 1.0;
+            } else if (norm_cost > 1.0) {
+                lane_waypoint_marker.color.r = 1.0;
+                lane_waypoint_marker.color.g = 2.0 - norm_cost;
+            }
+        } else {
+            lane_waypoint_marker.color.r = 1.0;
+            lane_waypoint_marker.color.g = 0.0;
+        }
 
-      lane_waypoint_marker.color.a = 0.9;
-      if(localPlanner.m_TrajectoryCostsCalculatotor.m_TrajectoryCosts.size() == paths.size())
-      {
-        float norm_cost = localPlanner.m_TrajectoryCostsCalculatotor.m_TrajectoryCosts.at(i).cost * paths.size();
-        if(norm_cost <= 1.0)
-        {
-          lane_waypoint_marker.color.r = norm_cost;
-          lane_waypoint_marker.color.g = 1.0;
+        if (traj_costs.at(i).bBlocked) {
+            lane_waypoint_marker.color.r = 1.0;
+            lane_waypoint_marker.color.g = 0.0;
+            lane_waypoint_marker.color.b = 0.0;
         }
-        else if(norm_cost > 1.0)
-        {
-          lane_waypoint_marker.color.r = 1.0;
-          lane_waypoint_marker.color.g = 2.0 - norm_cost;
+
+        if (i == iClosest) {
+            lane_waypoint_marker.color.r = 1.0;
+            lane_waypoint_marker.color.g = 0.0;
+            lane_waypoint_marker.color.b = 1.0;
         }
-      }
-      else
-      {
-        lane_waypoint_marker.color.r = 0.0;
-        lane_waypoint_marker.color.g = 1.0;
-      }
-
-      if((int)i == localPlanner.m_iSafeTrajectory && (int)il == localPlanner.m_iCurrentTotalPathId)
-      {
-        lane_waypoint_marker.color.r = 1.0;
-        lane_waypoint_marker.color.g = 0.0;
-        lane_waypoint_marker.color.b = 1.0;
-      }
-      else
-      {
-        lane_waypoint_marker.color.b = 0;
-      }
 
-      markerArray.markers.push_back(lane_waypoint_marker);
-      count++;
+        markerArray.markers.push_back(lane_waypoint_marker);
+        count++;
     }
-  }
 }
 
-void ROSHelpers::ConvertFromPlannerHToAutowareVisualizePathFormat(const std::vector<std::vector<PlannerHNS::WayPoint> >& globalPaths, visualization_msgs::MarkerArray& markerArray)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "global_lane_array_marker";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-
-
-  std_msgs::ColorRGBA roll_color, total_color, curr_color;
-  lane_waypoint_marker.points.clear();
-  lane_waypoint_marker.id = 1;
-  lane_waypoint_marker.scale.x = 0.1;
-  lane_waypoint_marker.scale.y = 0.1;
-  total_color.r = 1;
-  total_color.g = 0;
-  total_color.b = 0;
-  total_color.a = 0.5;
-  lane_waypoint_marker.color = total_color;
-  lane_waypoint_marker.frame_locked = false;
-
-  int count = 0;
-  for (unsigned int i = 0; i < globalPaths.size(); i++)
-  {
-    lane_waypoint_marker.points.clear();
-    lane_waypoint_marker.id = count;
-
-    for (unsigned int j=0; j < globalPaths.at(i).size(); j++)
-    {
-      geometry_msgs::Point point;
-
-      point.x = globalPaths.at(i).at(j).pos.x;
-      point.y = globalPaths.at(i).at(j).pos.y;
-      point.z = globalPaths.at(i).at(j).pos.z;
-
-      lane_waypoint_marker.points.push_back(point);
+void ROSHelpers::ConvertFromPlannerHToAutowareVisualizePathFormat(const std::vector<PlannerHNS::WayPoint> &curr_path,
+                                                                  const std::vector<std::vector<std::vector<PlannerHNS::WayPoint>>> &paths,
+                                                                  const PlannerHNS::LocalPlannerH &localPlanner,
+                                                                  visualization_msgs::MarkerArray &markerArray) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "global_lane_array_marker";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 0.1;
+    lane_waypoint_marker.scale.y = 0.1;
+    // lane_waypoint_marker.scale.z = 0.1;
+    lane_waypoint_marker.frame_locked = false;
+    std_msgs::ColorRGBA total_color, curr_color;
+
+    int count = 0;
+    for (unsigned int il = 0; il < paths.size(); il++) {
+        for (unsigned int i = 0; i < paths.at(il).size(); i++) {
+            lane_waypoint_marker.points.clear();
+            lane_waypoint_marker.id = count;
+
+            for (unsigned int j = 0; j < paths.at(il).at(i).size(); j++) {
+                geometry_msgs::Point point;
+
+                point.x = paths.at(il).at(i).at(j).pos.x;
+                point.y = paths.at(il).at(i).at(j).pos.y;
+                point.z = paths.at(il).at(i).at(j).pos.z;
+
+                lane_waypoint_marker.points.push_back(point);
+            }
+
+            lane_waypoint_marker.color.a = 0.9;
+            if (localPlanner.m_TrajectoryCostsCalculatotor.m_TrajectoryCosts.size() == paths.size()) {
+                float norm_cost = localPlanner.m_TrajectoryCostsCalculatotor.m_TrajectoryCosts.at(i).cost * paths.size();
+                if (norm_cost <= 1.0) {
+                    lane_waypoint_marker.color.r = norm_cost;
+                    lane_waypoint_marker.color.g = 1.0;
+                } else if (norm_cost > 1.0) {
+                    lane_waypoint_marker.color.r = 1.0;
+                    lane_waypoint_marker.color.g = 2.0 - norm_cost;
+                }
+            } else {
+                lane_waypoint_marker.color.r = 0.0;
+                lane_waypoint_marker.color.g = 1.0;
+            }
+
+            if ((int)i == localPlanner.m_iSafeTrajectory && (int)il == localPlanner.m_iCurrentTotalPathId) {
+                lane_waypoint_marker.color.r = 1.0;
+                lane_waypoint_marker.color.g = 0.0;
+                lane_waypoint_marker.color.b = 1.0;
+            } else {
+                lane_waypoint_marker.color.b = 0;
+            }
+
+            markerArray.markers.push_back(lane_waypoint_marker);
+            count++;
+        }
     }
-
-    markerArray.markers.push_back(lane_waypoint_marker);
-    count++;
-  }
 }
 
-void ROSHelpers::ConvertFromPlannerObstaclesToAutoware(const PlannerHNS::WayPoint& currState, const std::vector<PlannerHNS::DetectedObject>& trackedObstacles,
-    visualization_msgs::MarkerArray& detectedPolygons)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "detected_polygons";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = .1;
-  lane_waypoint_marker.scale.y = .1;
-  //lane_waypoint_marker.scale.z = .05;
-  lane_waypoint_marker.color.a = 0.8;
-  lane_waypoint_marker.frame_locked = false;
-
-  visualization_msgs::Marker corner_marker;
-  corner_marker.header.frame_id = "map";
-  corner_marker.header.stamp = ros::Time();
-  corner_marker.ns = "Polygon_Corners";
-  corner_marker.type = visualization_msgs::Marker::SPHERE;
-  corner_marker.action = visualization_msgs::Marker::ADD;
-  corner_marker.scale.x = .1;
-  corner_marker.scale.y = .1;
-  corner_marker.scale.z = .1;
-  corner_marker.color.a = 0.8;
-  corner_marker.frame_locked = false;
-
-
-  visualization_msgs::Marker quarters_marker;
-  quarters_marker.header.frame_id = "map";
-  quarters_marker.header.stamp = ros::Time();
-  quarters_marker.ns = "Quarters_Lines";
-  quarters_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  quarters_marker.action = visualization_msgs::Marker::ADD;
-  quarters_marker.scale.x = .03;
-  quarters_marker.scale.y = .03;
-  quarters_marker.scale.z = .03;
-  quarters_marker.color.a = 0.8;
-  quarters_marker.color.r = 0.6;
-  quarters_marker.color.g = 0.5;
-  quarters_marker.color.b = 0;
-  quarters_marker.frame_locked = false;
-
-  visualization_msgs::Marker direction_marker;
-  direction_marker.header.frame_id = "map";
-  direction_marker.header.stamp = ros::Time();
-  direction_marker.ns = "Object_Direction";
-  direction_marker.type = visualization_msgs::Marker::ARROW;
-  direction_marker.action = visualization_msgs::Marker::ADD;
-  direction_marker.scale.x = .9;
-  direction_marker.scale.y = .4;
-  direction_marker.scale.z = .4;
-  direction_marker.color.a = 0.8;
-  direction_marker.color.r = 0;
-  direction_marker.color.g = 1;
-  direction_marker.color.b = 0;
-  direction_marker.frame_locked = false;
-
-
-  visualization_msgs::Marker velocity_marker;
-  velocity_marker.header.frame_id = "map";
-  velocity_marker.header.stamp = ros::Time();
-  velocity_marker.ns = "detected_polygons_velocity";
-  velocity_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-  //velocity_marker.action = visualization_msgs::Marker::ADD;
-  velocity_marker.scale.z = 0.9;
-  velocity_marker.scale.x = 0.9;
-  velocity_marker.scale.y = 0.9;
-  velocity_marker.color.a = 0.5;
-
-  velocity_marker.frame_locked = false;
-  detectedPolygons.markers.clear();
-
-  int pointID = 0;
-  int quartersIds = 0;
-  for(unsigned int i =0; i < trackedObstacles.size(); i++)
-  {
-    //double distance = hypot(currState.pos.y-trackedObstacles.at(i).center.pos.y, currState.pos.x-trackedObstacles.at(i).center.pos.x);
-
-    lane_waypoint_marker.color.g = 0;
-    lane_waypoint_marker.color.r = 0;
-    lane_waypoint_marker.color.b = 1;
-
-    velocity_marker.color.r = 1;//trackedObstacles.at(i).center.v/16.0;
-    velocity_marker.color.g = 1;// - trackedObstacles.at(i).center.v/16.0;
-    velocity_marker.color.b = 1;
+void ROSHelpers::ConvertFromPlannerHToAutowareVisualizePathFormat(const std::vector<std::vector<PlannerHNS::WayPoint>> &globalPaths,
+                                                                  visualization_msgs::MarkerArray &markerArray) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "global_lane_array_marker";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
 
+    std_msgs::ColorRGBA roll_color, total_color, curr_color;
     lane_waypoint_marker.points.clear();
-    lane_waypoint_marker.id = i;
-    velocity_marker.id = i;
+    lane_waypoint_marker.id = 1;
+    lane_waypoint_marker.scale.x = 0.1;
+    lane_waypoint_marker.scale.y = 0.1;
+    total_color.r = 1;
+    total_color.g = 0;
+    total_color.b = 0;
+    total_color.a = 0.5;
+    lane_waypoint_marker.color = total_color;
+    lane_waypoint_marker.frame_locked = false;
+
+    int count = 0;
+    for (unsigned int i = 0; i < globalPaths.size(); i++) {
+        lane_waypoint_marker.points.clear();
+        lane_waypoint_marker.id = count;
+
+        for (unsigned int j = 0; j < globalPaths.at(i).size(); j++) {
+            geometry_msgs::Point point;
+
+            point.x = globalPaths.at(i).at(j).pos.x;
+            point.y = globalPaths.at(i).at(j).pos.y;
+            point.z = globalPaths.at(i).at(j).pos.z;
+
+            lane_waypoint_marker.points.push_back(point);
+        }
+
+        markerArray.markers.push_back(lane_waypoint_marker);
+        count++;
+    }
+}
 
-    //std::cout << " Distance : " << distance << ", Of Object" << trackedObstacles.at(i).id << std::endl;
+void ROSHelpers::ConvertFromPlannerObstaclesToAutoware(const PlannerHNS::WayPoint &currState,
+                                                       const std::vector<PlannerHNS::DetectedObject> &trackedObstacles,
+                                                       visualization_msgs::MarkerArray &detectedPolygons) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "detected_polygons";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = .1;
+    lane_waypoint_marker.scale.y = .1;
+    // lane_waypoint_marker.scale.z = .05;
+    lane_waypoint_marker.color.a = 0.8;
+    lane_waypoint_marker.frame_locked = false;
+
+    visualization_msgs::Marker corner_marker;
+    corner_marker.header.frame_id = "map";
+    corner_marker.header.stamp = ros::Time();
+    corner_marker.ns = "Polygon_Corners";
+    corner_marker.type = visualization_msgs::Marker::SPHERE;
+    corner_marker.action = visualization_msgs::Marker::ADD;
+    corner_marker.scale.x = .1;
+    corner_marker.scale.y = .1;
+    corner_marker.scale.z = .1;
+    corner_marker.color.a = 0.8;
+    corner_marker.frame_locked = false;
+
+    visualization_msgs::Marker quarters_marker;
+    quarters_marker.header.frame_id = "map";
+    quarters_marker.header.stamp = ros::Time();
+    quarters_marker.ns = "Quarters_Lines";
+    quarters_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    quarters_marker.action = visualization_msgs::Marker::ADD;
+    quarters_marker.scale.x = .03;
+    quarters_marker.scale.y = .03;
+    quarters_marker.scale.z = .03;
+    quarters_marker.color.a = 0.8;
+    quarters_marker.color.r = 0.6;
+    quarters_marker.color.g = 0.5;
+    quarters_marker.color.b = 0;
+    quarters_marker.frame_locked = false;
+
+    visualization_msgs::Marker direction_marker;
+    direction_marker.header.frame_id = "map";
+    direction_marker.header.stamp = ros::Time();
+    direction_marker.ns = "Object_Direction";
+    direction_marker.type = visualization_msgs::Marker::ARROW;
+    direction_marker.action = visualization_msgs::Marker::ADD;
+    direction_marker.scale.x = .9;
+    direction_marker.scale.y = .4;
+    direction_marker.scale.z = .4;
+    direction_marker.color.a = 0.8;
+    direction_marker.color.r = 0;
+    direction_marker.color.g = 1;
+    direction_marker.color.b = 0;
+    direction_marker.frame_locked = false;
+
+    visualization_msgs::Marker velocity_marker;
+    velocity_marker.header.frame_id = "map";
+    velocity_marker.header.stamp = ros::Time();
+    velocity_marker.ns = "detected_polygons_velocity";
+    velocity_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    // velocity_marker.action = visualization_msgs::Marker::ADD;
+    velocity_marker.scale.z = 0.9;
+    velocity_marker.scale.x = 0.9;
+    velocity_marker.scale.y = 0.9;
+    velocity_marker.color.a = 0.5;
+
+    velocity_marker.frame_locked = false;
+    detectedPolygons.markers.clear();
+
+    int pointID = 0;
+    int quartersIds = 0;
+    for (unsigned int i = 0; i < trackedObstacles.size(); i++) {
+        // double distance = hypot(currState.pos.y-trackedObstacles.at(i).center.pos.y, currState.pos.x-trackedObstacles.at(i).center.pos.x);
+
+        lane_waypoint_marker.color.g = 0;
+        lane_waypoint_marker.color.r = 0;
+        lane_waypoint_marker.color.b = 1;
+
+        velocity_marker.color.r = 1; // trackedObstacles.at(i).center.v/16.0;
+        velocity_marker.color.g = 1; // - trackedObstacles.at(i).center.v/16.0;
+        velocity_marker.color.b = 1;
+
+        lane_waypoint_marker.points.clear();
+        lane_waypoint_marker.id = i;
+        velocity_marker.id = i;
+
+        // std::cout << " Distance : " << distance << ", Of Object" << trackedObstacles.at(i).id << std::endl;
+
+        for (unsigned int p = 0; p < trackedObstacles.at(i).contour.size(); p++) {
+
+            geometry_msgs::Point point;
+
+            point.x = trackedObstacles.at(i).contour.at(p).x;
+            point.y = trackedObstacles.at(i).contour.at(p).y;
+            point.z = trackedObstacles.at(i).contour.at(p).z;
+
+            lane_waypoint_marker.points.push_back(point);
+
+            corner_marker.pose.position = point;
+            corner_marker.color.r = 0.8;
+            corner_marker.color.g = 0;
+            corner_marker.color.b = 0.7;
+            corner_marker.color.a = 0.5;
+            corner_marker.id = pointID;
+            pointID++;
+
+            detectedPolygons.markers.push_back(corner_marker);
+        }
 
-    for(unsigned int p = 0; p < trackedObstacles.at(i).contour.size(); p++)
-    {
+        if (trackedObstacles.at(i).contour.size() > 0) {
+            geometry_msgs::Point point;
 
-      geometry_msgs::Point point;
+            point.x = trackedObstacles.at(i).contour.at(0).x;
+            point.y = trackedObstacles.at(i).contour.at(0).y;
+            point.z = trackedObstacles.at(i).contour.at(0).z + 1;
 
-        point.x = trackedObstacles.at(i).contour.at(p).x;
-        point.y = trackedObstacles.at(i).contour.at(p).y;
-        point.z = trackedObstacles.at(i).contour.at(p).z;
+            lane_waypoint_marker.points.push_back(point);
+        }
 
-        lane_waypoint_marker.points.push_back(point);
+        geometry_msgs::Point point;
 
-        corner_marker.pose.position = point;
-        corner_marker.color.r = 0.8;
-        corner_marker.color.g = 0;
-        corner_marker.color.b = 0.7;
-        corner_marker.color.a = 0.5;
-        corner_marker.id = pointID;
-        pointID++;
+        point.x = trackedObstacles.at(i).center.pos.x;
+        point.y = trackedObstacles.at(i).center.pos.y;
+        point.z = trackedObstacles.at(i).center.pos.z + 1;
+
+        //    geometry_msgs::Point relative_p;
+        // relative_p.y = 0.5;
+        //    velocity_marker.pose.position = calcAbsoluteCoordinate(relative_p, point);
+        velocity_marker.pose.position = point;
+        velocity_marker.pose.position.z += 0.5;
+
+        direction_marker.id = i;
+        direction_marker.pose.position = point;
+        direction_marker.pose.position.z += 0.5;
+        direction_marker.pose.orientation =
+            tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(trackedObstacles.at(i).center.pos.a));
+
+        for (unsigned int iq = 0; iq < 8; iq++) {
+            quarters_marker.points.clear();
+            quarters_marker.id = quartersIds;
+            quarters_marker.points.push_back(point);
+            geometry_msgs::Point point2 = point;
+            double a_q = UtilityHNS::UtilityH::SplitPositiveAngle(trackedObstacles.at(i).center.pos.a + (iq * M_PI_4));
+            point2.x += 2.0 * cos(a_q);
+            point2.y += 1.5 * sin(a_q);
+            quarters_marker.points.push_back(point2);
+
+            quartersIds++;
+            detectedPolygons.markers.push_back(quarters_marker);
+        }
 
-        detectedPolygons.markers.push_back(corner_marker);
+        int speed = (trackedObstacles.at(i).center.v * 3.6);
+
+        // double to string
+        std::ostringstream str_out;
+        //  if(trackedObstacles.at(i).center.v > 0.75)
+        str_out << "(" << trackedObstacles.at(i).id << " , " << speed << ")";
+        //    else
+        //      str_out << trackedObstacles.at(i).id;
+        // std::string vel = str_out.str();
+        velocity_marker.text = str_out.str(); // vel.erase(vel.find_first_of(".") + 2);
+        // if(speed > 0.5)
+
+        detectedPolygons.markers.push_back(velocity_marker);
+        detectedPolygons.markers.push_back(lane_waypoint_marker);
+        detectedPolygons.markers.push_back(direction_marker);
     }
+}
 
-    if(trackedObstacles.at(i).contour.size()>0)
-    {
-    geometry_msgs::Point point;
-
-      point.x = trackedObstacles.at(i).contour.at(0).x;
-      point.y = trackedObstacles.at(i).contour.at(0).y;
-      point.z = trackedObstacles.at(i).contour.at(0).z+1;
+std::string ROSHelpers::GetBehaviorNameFromCode(const PlannerHNS::STATE_TYPE &behState) {
+    std::string str = "Unknown";
+    switch (behState) {
+    case PlannerHNS::INITIAL_STATE:
+        str = "Init";
+        break;
+    case PlannerHNS::WAITING_STATE:
+        str = "Waiting";
+        break;
+    case PlannerHNS::FORWARD_STATE:
+        str = "Forward";
+        break;
+    case PlannerHNS::STOPPING_STATE:
+        str = "Stop";
+        break;
+    case PlannerHNS::FINISH_STATE:
+        str = "End";
+        break;
+    case PlannerHNS::FOLLOW_STATE:
+        str = "Follow";
+        break;
+    case PlannerHNS::OBSTACLE_AVOIDANCE_STATE:
+        str = "Swerving";
+        break;
+    case PlannerHNS::TRAFFIC_LIGHT_STOP_STATE:
+        str = "Light Stop";
+        break;
+    case PlannerHNS::TRAFFIC_LIGHT_WAIT_STATE:
+        str = "Light Wait";
+        break;
+    case PlannerHNS::STOP_SIGN_STOP_STATE:
+        str = "Sign Stop";
+        break;
+    case PlannerHNS::STOP_SIGN_WAIT_STATE:
+        str = "Sign Wait";
+        break;
+    case PlannerHNS::PEDESTRIAN_STATE:
+        str = "Pedestrian";
+        break;
+    case PlannerHNS::INTERSECTION_STATE:
+        str = "Intersection";
+        break;
+    default:
+        str = "Unknown";
+        break;
+    }
 
-      lane_waypoint_marker.points.push_back(point);
+    return str;
+}
 
+void ROSHelpers::VisualizeBehaviorState(const PlannerHNS::WayPoint &currState, const PlannerHNS::BehaviorState &beh, const bool &bGreenLight,
+                                        const int &avoidDirection, visualization_msgs::Marker &behaviorMarker, std::string ns, double size_factor) {
+    behaviorMarker.header.frame_id = "map";
+    behaviorMarker.header.stamp = ros::Time();
+    behaviorMarker.ns = ns;
+    behaviorMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    behaviorMarker.scale.z = 1.0 * size_factor;
+    behaviorMarker.scale.x = 1.0 * size_factor;
+    behaviorMarker.scale.y = 1.0 * size_factor;
+    behaviorMarker.color.a = 0.9;
+    behaviorMarker.frame_locked = false;
+    if (bGreenLight) {
+        behaviorMarker.color.r = 0.1; // trackedObstacles.at(i).center.v/16.0;
+        behaviorMarker.color.g = 1;   // - trackedObstacles.at(i).center.v/16.0;
+        behaviorMarker.color.b = 0.1;
+    } else {
+        behaviorMarker.color.r = 1;   // trackedObstacles.at(i).center.v/16.0;
+        behaviorMarker.color.g = 0.1; // - trackedObstacles.at(i).center.v/16.0;
+        behaviorMarker.color.b = 0.1;
     }
 
+    behaviorMarker.id = 0;
 
     geometry_msgs::Point point;
 
-    point.x = trackedObstacles.at(i).center.pos.x;
-    point.y = trackedObstacles.at(i).center.pos.y;
-    point.z = trackedObstacles.at(i).center.pos.z+1;
-
-//    geometry_msgs::Point relative_p;
-    //relative_p.y = 0.5;
-//    velocity_marker.pose.position = calcAbsoluteCoordinate(relative_p, point);
-    velocity_marker.pose.position = point;
-      velocity_marker.pose.position.z += 0.5;
-
-      direction_marker.id = i;
-      direction_marker.pose.position = point;
-      direction_marker.pose.position.z += 0.5;
-      direction_marker.pose.orientation = tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(trackedObstacles.at(i).center.pos.a));
-
-
-    for(unsigned int iq = 0; iq < 8; iq++)
-    {
-      quarters_marker.points.clear();
-      quarters_marker.id = quartersIds;
-      quarters_marker.points.push_back(point);
-      geometry_msgs::Point point2 = point;
-      double a_q = UtilityHNS::UtilityH::SplitPositiveAngle(trackedObstacles.at(i).center.pos.a+(iq*M_PI_4));
-      point2.x += 2.0*cos(a_q);
-      point2.y += 1.5*sin(a_q);
-      quarters_marker.points.push_back(point2);
-
-      quartersIds++;
-      detectedPolygons.markers.push_back(quarters_marker);
-    }
+    point.x = currState.pos.x;
+    point.y = currState.pos.y;
+    point.z = currState.pos.z + 3.0;
 
-    int speed = (trackedObstacles.at(i).center.v*3.6);
+    behaviorMarker.pose.position = point;
 
-    // double to string
     std::ostringstream str_out;
-  //  if(trackedObstacles.at(i).center.v > 0.75)
-    str_out << "(" << trackedObstacles.at(i).id << " , " << speed << ")";
-//    else
-//      str_out << trackedObstacles.at(i).id;
-    //std::string vel = str_out.str();
-    velocity_marker.text = str_out.str();//vel.erase(vel.find_first_of(".") + 2);
-    //if(speed > 0.5)
-
-    detectedPolygons.markers.push_back(velocity_marker);
-    detectedPolygons.markers.push_back(lane_waypoint_marker);
-    detectedPolygons.markers.push_back(direction_marker);
-
-  }
-}
 
-std::string ROSHelpers::GetBehaviorNameFromCode(const PlannerHNS::STATE_TYPE& behState)
-{
-  std::string str = "Unknown";
-  switch(behState)
-  {
-  case PlannerHNS::INITIAL_STATE:
-    str = "Init";
-    break;
-  case PlannerHNS::WAITING_STATE:
-    str = "Waiting";
-    break;
-  case PlannerHNS::FORWARD_STATE:
-    str = "Forward";
-    break;
-  case PlannerHNS::STOPPING_STATE:
-    str = "Stop";
-    break;
-  case PlannerHNS::FINISH_STATE:
-    str = "End";
-    break;
-  case PlannerHNS::FOLLOW_STATE:
-    str = "Follow";
-    break;
-  case PlannerHNS::OBSTACLE_AVOIDANCE_STATE:
-    str = "Swerving";
-    break;
-  case PlannerHNS::TRAFFIC_LIGHT_STOP_STATE:
-    str = "Light Stop";
-    break;
-  case PlannerHNS::TRAFFIC_LIGHT_WAIT_STATE:
-    str = "Light Wait";
-    break;
-  case PlannerHNS::STOP_SIGN_STOP_STATE:
-    str = "Sign Stop";
-    break;
-  case PlannerHNS::STOP_SIGN_WAIT_STATE:
-    str = "Sign Wait";
-    break;
-  case PlannerHNS::PEDESTRIAN_STATE:
-    str = "Pedestrian";
-    break;
-  case PlannerHNS::INTERSECTION_STATE:
-    str = "Intersection";
-    break;
-  default:
-    str = "Unknown";
-    break;
-  }
-
-  return str;
-}
+    // str_out << "(" << (int)(beh.followDistance * 100) / 100 <<")";
+    str_out << "(" << (int)(beh.stopDistance * 100.0) / 100.0 << ")";
 
-void ROSHelpers::VisualizeBehaviorState(const PlannerHNS::WayPoint& currState, const PlannerHNS::BehaviorState& beh, const bool& bGreenLight, const int& avoidDirection, visualization_msgs::Marker& behaviorMarker, std::string ns,double size_factor)
-{
-  behaviorMarker.header.frame_id = "map";
-  behaviorMarker.header.stamp = ros::Time();
-  behaviorMarker.ns = ns;
-  behaviorMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-  behaviorMarker.scale.z = 1.0*size_factor;
-  behaviorMarker.scale.x = 1.0*size_factor;
-  behaviorMarker.scale.y = 1.0*size_factor;
-  behaviorMarker.color.a = 0.9;
-  behaviorMarker.frame_locked = false;
-  if(bGreenLight)
-  {
-    behaviorMarker.color.r = 0.1;//trackedObstacles.at(i).center.v/16.0;
-    behaviorMarker.color.g = 1;// - trackedObstacles.at(i).center.v/16.0;
-    behaviorMarker.color.b = 0.1;
-  }
-  else
-  {
-    behaviorMarker.color.r = 1;//trackedObstacles.at(i).center.v/16.0;
-    behaviorMarker.color.g = 0.1;// - trackedObstacles.at(i).center.v/16.0;
-    behaviorMarker.color.b = 0.1;
-  }
-
-  behaviorMarker.id = 0;
-
-  geometry_msgs::Point point;
-
-  point.x = currState.pos.x;
-  point.y = currState.pos.y;
-  point.z = currState.pos.z+3.0;
-
-  behaviorMarker.pose.position = point;
-
-  std::ostringstream str_out;
-
-  //str_out << "(" << (int)(beh.followDistance * 100) / 100 <<")";
-  str_out << "(" << (int)(beh.stopDistance * 100.0) / 100.0 <<")";
-
-  if(avoidDirection == -1)
-    str_out << "<< ";
-
-  str_out << GetBehaviorNameFromCode(beh.state);
-  if(avoidDirection == 1)
-    str_out << " >>";
-  behaviorMarker.text = str_out.str();
+    if (avoidDirection == -1)
+        str_out << "<< ";
+
+    str_out << GetBehaviorNameFromCode(beh.state);
+    if (avoidDirection == 1)
+        str_out << " >>";
+    behaviorMarker.text = str_out.str();
 }
 
-void ROSHelpers::ConvertFromAutowareBoundingBoxObstaclesToPlannerH(const jsk_recognition_msgs::BoundingBoxArray& detectedObstacles,
-    std::vector<PlannerHNS::DetectedObject>& obstacles_list)
-{
-  obstacles_list.clear();
-  for(unsigned int i =0; i < detectedObstacles.boxes.size(); i++)
-  {
-    PlannerHNS::DetectedObject obj;
-    obj.center = PlannerHNS::WayPoint(detectedObstacles.boxes.at(i).pose.position.x,
-        detectedObstacles.boxes.at(i).pose.position.y,
-        0,
-        0);
-    obj.w = detectedObstacles.boxes.at(i).dimensions.y;
-    obj.l = detectedObstacles.boxes.at(i).dimensions.x;
-    obj.h = detectedObstacles.boxes.at(i).dimensions.z;
-    //double objSize = obj.w*obj.l;
-    //double d = hypot(m_State.state.pos.y - obj.center.pos.y, m_State.state.pos.x - obj.center.pos.x);
-    //std::cout << ", Distance of  : " << d;
-    //if(d < 7)
-    {
-
-      double l2 = obj.l/2.0;
-      double w2 = obj.w/2.0;
-
-      obj.contour.push_back(PlannerHNS::GPSPoint(-w2, -l2, 0,0));
-      obj.contour.push_back(PlannerHNS::GPSPoint(w2, -l2, 0,0));
-      obj.contour.push_back(PlannerHNS::GPSPoint(w2, l2, 0,0));
-      obj.contour.push_back(PlannerHNS::GPSPoint(-w2, l2, 0,0));
-      obstacles_list.push_back(obj);
+void ROSHelpers::ConvertFromAutowareBoundingBoxObstaclesToPlannerH(const jsk_recognition_msgs::BoundingBoxArray &detectedObstacles,
+                                                                   std::vector<PlannerHNS::DetectedObject> &obstacles_list) {
+    obstacles_list.clear();
+    for (unsigned int i = 0; i < detectedObstacles.boxes.size(); i++) {
+        PlannerHNS::DetectedObject obj;
+        obj.center = PlannerHNS::WayPoint(detectedObstacles.boxes.at(i).pose.position.x, detectedObstacles.boxes.at(i).pose.position.y, 0, 0);
+        obj.w = detectedObstacles.boxes.at(i).dimensions.y;
+        obj.l = detectedObstacles.boxes.at(i).dimensions.x;
+        obj.h = detectedObstacles.boxes.at(i).dimensions.z;
+        // double objSize = obj.w*obj.l;
+        // double d = hypot(m_State.state.pos.y - obj.center.pos.y, m_State.state.pos.x - obj.center.pos.x);
+        // std::cout << ", Distance of  : " << d;
+        // if(d < 7)
+        {
+
+            double l2 = obj.l / 2.0;
+            double w2 = obj.w / 2.0;
+
+            obj.contour.push_back(PlannerHNS::GPSPoint(-w2, -l2, 0, 0));
+            obj.contour.push_back(PlannerHNS::GPSPoint(w2, -l2, 0, 0));
+            obj.contour.push_back(PlannerHNS::GPSPoint(w2, l2, 0, 0));
+            obj.contour.push_back(PlannerHNS::GPSPoint(-w2, l2, 0, 0));
+            obstacles_list.push_back(obj);
+        }
     }
-  }
 }
 
-void ROSHelpers::ConvertFromAutowareCloudClusterObstaclesToPlannerH(const PlannerHNS::WayPoint& currState, const double& car_width,
-    const double& car_length, const autoware_msgs::CloudClusterArray& clusters, vector<PlannerHNS::DetectedObject>& obstacles_list,
-    const double max_obj_size, const double& min_obj_size, const double& detection_radius,
-    const int& n_poly_quarters,const double& poly_resolution, int& nOriginalPoints, int& nContourPoints)
-{
-  PlannerHNS::Mat3 rotationMat(-currState.pos.a);
-  PlannerHNS::Mat3 translationMat(-currState.pos.x, -currState.pos.y);
-
-  int nPoints = 0;
-  int nOrPoints = 0;
-  double object_size = 0;
-  PlannerHNS::GPSPoint relative_point;
-  PlannerHNS::GPSPoint avg_center;
-  PolygonGenerator polyGen(n_poly_quarters);
-  PlannerHNS::DetectedObject obj;
+void ROSHelpers::ConvertFromAutowareCloudClusterObstaclesToPlannerH(const PlannerHNS::WayPoint &currState, const double &car_width,
+                                                                    const double &car_length, const autoware_msgs::CloudClusterArray &clusters,
+                                                                    vector<PlannerHNS::DetectedObject> &obstacles_list, const double max_obj_size,
+                                                                    const double &min_obj_size, const double &detection_radius,
+                                                                    const int &n_poly_quarters, const double &poly_resolution, int &nOriginalPoints,
+                                                                    int &nContourPoints) {
+    PlannerHNS::Mat3 rotationMat(-currState.pos.a);
+    PlannerHNS::Mat3 translationMat(-currState.pos.x, -currState.pos.y);
+
+    int nPoints = 0;
+    int nOrPoints = 0;
+    double object_size = 0;
+    PlannerHNS::GPSPoint relative_point;
+    PlannerHNS::GPSPoint avg_center;
+    PolygonGenerator polyGen(n_poly_quarters);
+    PlannerHNS::DetectedObject obj;
 
-  for(unsigned int i =0; i < clusters.clusters.size(); i++)
-  {
-    obj.id = clusters.clusters.at(i).id;
-    obj.label = clusters.clusters.at(i).label;
+    for (unsigned int i = 0; i < clusters.clusters.size(); i++) {
+        obj.id = clusters.clusters.at(i).id;
+        obj.label = clusters.clusters.at(i).label;
 
-    obj.center.pos.x = clusters.clusters.at(i).centroid_point.point.x;
-    obj.center.pos.y = clusters.clusters.at(i).centroid_point.point.y;
-    obj.center.pos.z = clusters.clusters.at(i).centroid_point.point.z;
-    obj.center.pos.a = 0;
-    obj.center.v = 0;
-    obj.actual_yaw = clusters.clusters.at(i).estimated_angle;
+        obj.center.pos.x = clusters.clusters.at(i).centroid_point.point.x;
+        obj.center.pos.y = clusters.clusters.at(i).centroid_point.point.y;
+        obj.center.pos.z = clusters.clusters.at(i).centroid_point.point.z;
+        obj.center.pos.a = 0;
+        obj.center.v = 0;
+        obj.actual_yaw = clusters.clusters.at(i).estimated_angle;
 
-    obj.w = clusters.clusters.at(i).dimensions.x;
-    obj.l = clusters.clusters.at(i).dimensions.y;
-    obj.h = clusters.clusters.at(i).dimensions.z;
+        obj.w = clusters.clusters.at(i).dimensions.x;
+        obj.l = clusters.clusters.at(i).dimensions.y;
+        obj.h = clusters.clusters.at(i).dimensions.z;
 
-    pcl::PointCloud<pcl::PointXYZ> point_cloud;
-    pcl::fromROSMsg(clusters.clusters.at(i).cloud, point_cloud);
+        pcl::PointCloud<pcl::PointXYZ> point_cloud;
+        pcl::fromROSMsg(clusters.clusters.at(i).cloud, point_cloud);
 
+        obj.contour = polyGen.EstimateClusterPolygon(point_cloud, obj.center.pos, avg_center, poly_resolution);
 
-    obj.contour = polyGen.EstimateClusterPolygon(point_cloud ,obj.center.pos,avg_center, poly_resolution);
+        obj.distance_to_center = hypot(obj.center.pos.y - currState.pos.y, obj.center.pos.x - currState.pos.x);
 
-    obj.distance_to_center = hypot(obj.center.pos.y-currState.pos.y, obj.center.pos.x-currState.pos.x);
+        object_size = hypot(obj.w, obj.l);
 
-    object_size = hypot(obj.w, obj.l);
+        if (obj.distance_to_center > detection_radius || object_size < min_obj_size || object_size > max_obj_size)
+            continue;
 
-    if(obj.distance_to_center > detection_radius || object_size < min_obj_size || object_size > max_obj_size)
-      continue;
+        relative_point = translationMat * obj.center.pos;
+        relative_point = rotationMat * relative_point;
 
-    relative_point = translationMat*obj.center.pos;
-    relative_point = rotationMat*relative_point;
+        double distance_x = fabs(relative_point.x - car_length / 3.0);
+        double distance_y = fabs(relative_point.y);
 
-    double distance_x = fabs(relative_point.x - car_length/3.0);
-    double distance_y = fabs(relative_point.y);
+        if (distance_x <= car_length * 0.5 && distance_y <= car_width * 0.5) // don't detect yourself
+            continue;
 
-    if(distance_x  <= car_length*0.5 && distance_y <= car_width*0.5) // don't detect yourself
-      continue;
+        // obj.center.pos = avg_center;
+        nOrPoints += point_cloud.points.size();
+        nPoints += obj.contour.size();
+        // std::cout << " Distance_X: " << distance_x << ", " << " Distance_Y: " << distance_y << ", " << " Size: " << object_size << std::endl;
+        obstacles_list.push_back(obj);
+    }
 
-    //obj.center.pos = avg_center;
-    nOrPoints += point_cloud.points.size();
-    nPoints += obj.contour.size();
-    //std::cout << " Distance_X: " << distance_x << ", " << " Distance_Y: " << distance_y << ", " << " Size: " << object_size << std::endl;
-    obstacles_list.push_back(obj);
-  }
+    nOriginalPoints = nOrPoints;
+    nContourPoints = nPoints;
+}
 
-  nOriginalPoints = nOrPoints;
-  nContourPoints =  nPoints;
+PlannerHNS::SHIFT_POS ROSHelpers::ConvertShiftFromAutowareToPlannerH(const PlannerHNS::AUTOWARE_SHIFT_POS &shift) {
+    if (shift == PlannerHNS::AW_SHIFT_POS_DD)
+        return PlannerHNS::SHIFT_POS_DD;
+    else if (shift == PlannerHNS::AW_SHIFT_POS_RR)
+        return PlannerHNS::SHIFT_POS_RR;
+    else if (shift == PlannerHNS::AW_SHIFT_POS_NN)
+        return PlannerHNS::SHIFT_POS_NN;
+    else if (shift == PlannerHNS::AW_SHIFT_POS_PP)
+        return PlannerHNS::SHIFT_POS_PP;
+    else if (shift == PlannerHNS::AW_SHIFT_POS_BB)
+        return PlannerHNS::SHIFT_POS_BB;
+    else if (shift == PlannerHNS::AW_SHIFT_POS_SS)
+        return PlannerHNS::SHIFT_POS_SS;
+    else
+        return PlannerHNS::SHIFT_POS_UU;
 }
 
-PlannerHNS::SHIFT_POS ROSHelpers::ConvertShiftFromAutowareToPlannerH(const PlannerHNS::AUTOWARE_SHIFT_POS& shift)
-{
-  if(shift == PlannerHNS::AW_SHIFT_POS_DD)
-    return PlannerHNS::SHIFT_POS_DD;
-  else if(shift == PlannerHNS::AW_SHIFT_POS_RR)
-    return PlannerHNS::SHIFT_POS_RR;
-  else if(shift == PlannerHNS::AW_SHIFT_POS_NN)
-    return PlannerHNS::SHIFT_POS_NN;
-  else if(shift == PlannerHNS::AW_SHIFT_POS_PP)
-    return PlannerHNS::SHIFT_POS_PP;
-  else if(shift == PlannerHNS::AW_SHIFT_POS_BB)
-    return PlannerHNS::SHIFT_POS_BB;
-  else if(shift == PlannerHNS::AW_SHIFT_POS_SS)
-    return PlannerHNS::SHIFT_POS_SS;
-  else
-    return PlannerHNS::SHIFT_POS_UU;
+PlannerHNS::AUTOWARE_SHIFT_POS ROSHelpers::ConvertShiftFromPlannerHToAutoware(const PlannerHNS::SHIFT_POS &shift) {
+    if (shift == PlannerHNS::SHIFT_POS_DD)
+        return PlannerHNS::AW_SHIFT_POS_DD;
+    else if (shift == PlannerHNS::SHIFT_POS_RR)
+        return PlannerHNS::AW_SHIFT_POS_RR;
+    else if (shift == PlannerHNS::SHIFT_POS_NN)
+        return PlannerHNS::AW_SHIFT_POS_NN;
+    else if (shift == PlannerHNS::SHIFT_POS_PP)
+        return PlannerHNS::AW_SHIFT_POS_PP;
+    else if (shift == PlannerHNS::SHIFT_POS_BB)
+        return PlannerHNS::AW_SHIFT_POS_BB;
+    else if (shift == PlannerHNS::SHIFT_POS_SS)
+        return PlannerHNS::AW_SHIFT_POS_SS;
+    else
+        return PlannerHNS::AW_SHIFT_POS_UU;
 }
 
-PlannerHNS::AUTOWARE_SHIFT_POS ROSHelpers::ConvertShiftFromPlannerHToAutoware(const PlannerHNS::SHIFT_POS& shift)
-{
-  if(shift == PlannerHNS::SHIFT_POS_DD)
-    return PlannerHNS::AW_SHIFT_POS_DD;
-  else if(shift == PlannerHNS::SHIFT_POS_RR)
-    return PlannerHNS::AW_SHIFT_POS_RR;
-  else if(shift == PlannerHNS::SHIFT_POS_NN)
-    return PlannerHNS::AW_SHIFT_POS_NN;
-  else if(shift == PlannerHNS::SHIFT_POS_PP)
-    return PlannerHNS::AW_SHIFT_POS_PP;
-  else if(shift == PlannerHNS::SHIFT_POS_BB)
-    return PlannerHNS::AW_SHIFT_POS_BB;
-  else if(shift == PlannerHNS::SHIFT_POS_SS)
-    return PlannerHNS::AW_SHIFT_POS_SS;
-  else
-    return PlannerHNS::AW_SHIFT_POS_UU;
+PlannerHNS::AutowareBehaviorState ROSHelpers::ConvertBehaviorStateFromPlannerHToAutoware(const PlannerHNS::BehaviorState &beh) {
+    PlannerHNS::AutowareBehaviorState arw_state;
+    arw_state.followDistance = beh.followDistance;
+    arw_state.followVelocity = beh.followVelocity;
+    arw_state.maxVelocity = beh.maxVelocity;
+    arw_state.minVelocity = beh.minVelocity;
+    arw_state.stopDistance = beh.stopDistance;
+
+    if (beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_LEFT)
+        arw_state.indicator = PlannerHNS::AW_INDICATOR_LEFT;
+    else if (beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_RIGHT)
+        arw_state.indicator = PlannerHNS::AW_INDICATOR_RIGHT;
+    else if (beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_BOTH)
+        arw_state.indicator = PlannerHNS::AW_INDICATOR_BOTH;
+    else if (beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_NONE)
+        arw_state.indicator = PlannerHNS::AW_INDICATOR_NONE;
+
+    if (beh.state == PlannerHNS::INITIAL_STATE)
+        arw_state.state = PlannerHNS::AW_INITIAL_STATE;
+    else if (beh.state == PlannerHNS::WAITING_STATE)
+        arw_state.state = PlannerHNS::AW_WAITING_STATE;
+    else if (beh.state == PlannerHNS::FORWARD_STATE)
+        arw_state.state = PlannerHNS::AW_FORWARD_STATE;
+    else if (beh.state == PlannerHNS::STOPPING_STATE)
+        arw_state.state = PlannerHNS::AW_STOPPING_STATE;
+    else if (beh.state == PlannerHNS::EMERGENCY_STATE)
+        arw_state.state = PlannerHNS::AW_EMERGENCY_STATE;
+    else if (beh.state == PlannerHNS::TRAFFIC_LIGHT_STOP_STATE)
+        arw_state.state = PlannerHNS::AW_TRAFFIC_LIGHT_STOP_STATE;
+    else if (beh.state == PlannerHNS::STOP_SIGN_STOP_STATE)
+        arw_state.state = PlannerHNS::AW_STOP_SIGN_STOP_STATE;
+    else if (beh.state == PlannerHNS::FOLLOW_STATE)
+        arw_state.state = PlannerHNS::AW_FOLLOW_STATE;
+    else if (beh.state == PlannerHNS::LANE_CHANGE_STATE)
+        arw_state.state = PlannerHNS::AW_LANE_CHANGE_STATE;
+    else if (beh.state == PlannerHNS::OBSTACLE_AVOIDANCE_STATE)
+        arw_state.state = PlannerHNS::AW_OBSTACLE_AVOIDANCE_STATE;
+    else if (beh.state == PlannerHNS::FINISH_STATE)
+        arw_state.state = PlannerHNS::AW_FINISH_STATE;
+
+    return arw_state;
 }
 
-PlannerHNS::AutowareBehaviorState ROSHelpers::ConvertBehaviorStateFromPlannerHToAutoware(const PlannerHNS::BehaviorState& beh)
-{
-  PlannerHNS::AutowareBehaviorState arw_state;
-  arw_state.followDistance = beh.followDistance;
-  arw_state.followVelocity = beh.followVelocity;
-  arw_state.maxVelocity = beh.maxVelocity;
-  arw_state.minVelocity = beh.minVelocity;
-  arw_state.stopDistance = beh.stopDistance;
-
-  if(beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_LEFT)
-    arw_state.indicator = PlannerHNS::AW_INDICATOR_LEFT;
-  else if(beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_RIGHT)
-    arw_state.indicator = PlannerHNS::AW_INDICATOR_RIGHT;
-  else if(beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_BOTH)
-    arw_state.indicator = PlannerHNS::AW_INDICATOR_BOTH;
-  else if(beh.indicator == PlannerHNS::LIGHT_INDICATOR::INDICATOR_NONE)
-    arw_state.indicator = PlannerHNS::AW_INDICATOR_NONE;
-
-  if(beh.state == PlannerHNS::INITIAL_STATE)
-    arw_state.state = PlannerHNS::AW_INITIAL_STATE;
-  else if(beh.state == PlannerHNS::WAITING_STATE)
-    arw_state.state = PlannerHNS::AW_WAITING_STATE;
-  else if(beh.state == PlannerHNS::FORWARD_STATE)
-    arw_state.state = PlannerHNS::AW_FORWARD_STATE;
-  else if(beh.state == PlannerHNS::STOPPING_STATE)
-    arw_state.state = PlannerHNS::AW_STOPPING_STATE;
-  else if(beh.state == PlannerHNS::EMERGENCY_STATE)
-    arw_state.state = PlannerHNS::AW_EMERGENCY_STATE;
-  else if(beh.state == PlannerHNS::TRAFFIC_LIGHT_STOP_STATE)
-    arw_state.state = PlannerHNS::AW_TRAFFIC_LIGHT_STOP_STATE;
-  else if(beh.state == PlannerHNS::STOP_SIGN_STOP_STATE)
-    arw_state.state = PlannerHNS::AW_STOP_SIGN_STOP_STATE;
-  else if(beh.state == PlannerHNS::FOLLOW_STATE)
-    arw_state.state = PlannerHNS::AW_FOLLOW_STATE;
-  else if(beh.state == PlannerHNS::LANE_CHANGE_STATE)
-    arw_state.state = PlannerHNS::AW_LANE_CHANGE_STATE;
-  else if(beh.state == PlannerHNS::OBSTACLE_AVOIDANCE_STATE)
-    arw_state.state = PlannerHNS::AW_OBSTACLE_AVOIDANCE_STATE;
-  else if(beh.state == PlannerHNS::FINISH_STATE)
-    arw_state.state = PlannerHNS::AW_FINISH_STATE;
-
-
-  return arw_state;
+void ROSHelpers::ConvertFromLocalLaneToAutowareLane(const std::vector<PlannerHNS::WayPoint> &path, autoware_msgs::Lane &trajectory,
+                                                    const unsigned int &iStart) {
+    trajectory.waypoints.clear();
 
-}
+    for (unsigned int i = iStart; i < path.size(); i++) {
+        autoware_msgs::Waypoint wp;
+        wp.pose.pose.position.x = path.at(i).pos.x;
+        wp.pose.pose.position.y = path.at(i).pos.y;
+        wp.pose.pose.position.z = path.at(i).pos.z;
+        wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(path.at(i).pos.a));
 
-void ROSHelpers::ConvertFromLocalLaneToAutowareLane(const std::vector<PlannerHNS::WayPoint>& path, autoware_msgs::Lane& trajectory , const unsigned int& iStart)
-{
-  trajectory.waypoints.clear();
-
-  for(unsigned int i = iStart; i < path.size(); i++)
-  {
-    autoware_msgs::Waypoint wp;
-    wp.pose.pose.position.x = path.at(i).pos.x;
-    wp.pose.pose.position.y = path.at(i).pos.y;
-    wp.pose.pose.position.z = path.at(i).pos.z;
-    wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(path.at(i).pos.a));
-
-    wp.twist.twist.linear.x = path.at(i).v;
-    wp.lane_id = path.at(i).laneId;
-    wp.stop_line_id = path.at(i).stopLineID;
-    wp.left_lane_id = path.at(i).LeftLnId;
-    wp.right_lane_id = path.at(i).RightLnId;
-    wp.time_cost = path.at(i).timeCost;
-
-    wp.gid = path.at(i).gid;
-
-    //wp.cost = path.at(i).cost;
-    wp.cost = 0;
-
-    if(path.at(i).actionCost.size()>0)
-    {
-      wp.direction = path.at(i).actionCost.at(0).first;
-      wp.cost += path.at(i).actionCost.at(0).second;
-    }
+        wp.twist.twist.linear.x = path.at(i).v;
+        wp.lane_id = path.at(i).laneId;
+        wp.stop_line_id = path.at(i).stopLineID;
+        wp.left_lane_id = path.at(i).LeftLnId;
+        wp.right_lane_id = path.at(i).RightLnId;
+        wp.time_cost = path.at(i).timeCost;
 
-    trajectory.waypoints.push_back(wp);
-  }
+        wp.gid = path.at(i).gid;
+
+        // wp.cost = path.at(i).cost;
+        wp.cost = 0;
+
+        if (path.at(i).actionCost.size() > 0) {
+            wp.direction = path.at(i).actionCost.at(0).first;
+            wp.cost += path.at(i).actionCost.at(0).second;
+        }
+
+        trajectory.waypoints.push_back(wp);
+    }
 }
 
-void ROSHelpers::ConvertFromLocalLaneToAutowareLane(const std::vector<PlannerHNS::GPSPoint>& path, autoware_msgs::Lane& trajectory)
-{
-  trajectory.waypoints.clear();
+void ROSHelpers::ConvertFromLocalLaneToAutowareLane(const std::vector<PlannerHNS::GPSPoint> &path, autoware_msgs::Lane &trajectory) {
+    trajectory.waypoints.clear();
 
-  for(unsigned int i=0; i < path.size(); i++)
-  {
-    autoware_msgs::Waypoint wp;
-    wp.pose.pose.position.x = path.at(i).x;
-    wp.pose.pose.position.y = path.at(i).y;
-    wp.pose.pose.position.z = path.at(i).z;
-    wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(path.at(i).a));
+    for (unsigned int i = 0; i < path.size(); i++) {
+        autoware_msgs::Waypoint wp;
+        wp.pose.pose.position.x = path.at(i).x;
+        wp.pose.pose.position.y = path.at(i).y;
+        wp.pose.pose.position.z = path.at(i).z;
+        wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(UtilityHNS::UtilityH::SplitPositiveAngle(path.at(i).a));
 
-    trajectory.waypoints.push_back(wp);
-  }
+        trajectory.waypoints.push_back(wp);
+    }
 }
 
-void ROSHelpers::ConvertFromAutowareLaneToLocalLane(const autoware_msgs::Lane& trajectory, std::vector<PlannerHNS::WayPoint>& path)
-{
-  path.clear();
-
-  for(unsigned int i=0; i < trajectory.waypoints.size(); i++)
-  {
-    PlannerHNS::WayPoint wp;
-    wp.pos.x = trajectory.waypoints.at(i).pose.pose.position.x;
-    wp.pos.y = trajectory.waypoints.at(i).pose.pose.position.y;
-    wp.pos.z = trajectory.waypoints.at(i).pose.pose.position.z;
-    wp.pos.a = tf::getYaw(trajectory.waypoints.at(i).pose.pose.orientation);
-
-    wp.v = trajectory.waypoints.at(i).twist.twist.linear.x;
-
-    wp.gid = trajectory.waypoints.at(i).gid;
-    wp.laneId = trajectory.waypoints.at(i).lane_id;
-    wp.stopLineID = trajectory.waypoints.at(i).stop_line_id;
-    wp.LeftLnId = trajectory.waypoints.at(i).left_lane_id;
-    wp.RightLnId = trajectory.waypoints.at(i).right_lane_id;
-    wp.timeCost = trajectory.waypoints.at(i).time_cost;
-
-    if(trajectory.waypoints.at(i).direction == 0)
-      wp.bDir = PlannerHNS::FORWARD_DIR;
-    else if(trajectory.waypoints.at(i).direction == 1)
-      wp.bDir = PlannerHNS::FORWARD_LEFT_DIR;
-    else if(trajectory.waypoints.at(i).direction == 2)
-      wp.bDir = PlannerHNS::FORWARD_RIGHT_DIR;
-    else if(trajectory.waypoints.at(i).direction == 3)
-      wp.bDir = PlannerHNS::BACKWARD_DIR;
-    else if(trajectory.waypoints.at(i).direction == 4)
-      wp.bDir = PlannerHNS::BACKWARD_LEFT_DIR;
-    else if(trajectory.waypoints.at(i).direction == 5)
-      wp.bDir = PlannerHNS::BACKWARD_RIGHT_DIR;
-    else if(trajectory.waypoints.at(i).direction == 6)
-      wp.bDir = PlannerHNS::STANDSTILL_DIR;
-
-    wp.cost = trajectory.waypoints.at(i).cost;
-
-    path.push_back(wp);
-  }
+void ROSHelpers::ConvertFromAutowareLaneToLocalLane(const autoware_msgs::Lane &trajectory, std::vector<PlannerHNS::WayPoint> &path) {
+    path.clear();
+
+    for (unsigned int i = 0; i < trajectory.waypoints.size(); i++) {
+        PlannerHNS::WayPoint wp;
+        wp.pos.x = trajectory.waypoints.at(i).pose.pose.position.x;
+        wp.pos.y = trajectory.waypoints.at(i).pose.pose.position.y;
+        wp.pos.z = trajectory.waypoints.at(i).pose.pose.position.z;
+        wp.pos.a = tf::getYaw(trajectory.waypoints.at(i).pose.pose.orientation);
+
+        wp.v = trajectory.waypoints.at(i).twist.twist.linear.x;
+
+        wp.gid = trajectory.waypoints.at(i).gid;
+        wp.laneId = trajectory.waypoints.at(i).lane_id;
+        wp.stopLineID = trajectory.waypoints.at(i).stop_line_id;
+        wp.LeftLnId = trajectory.waypoints.at(i).left_lane_id;
+        wp.RightLnId = trajectory.waypoints.at(i).right_lane_id;
+        wp.timeCost = trajectory.waypoints.at(i).time_cost;
+
+        if (trajectory.waypoints.at(i).direction == 0)
+            wp.bDir = PlannerHNS::FORWARD_DIR;
+        else if (trajectory.waypoints.at(i).direction == 1)
+            wp.bDir = PlannerHNS::FORWARD_LEFT_DIR;
+        else if (trajectory.waypoints.at(i).direction == 2)
+            wp.bDir = PlannerHNS::FORWARD_RIGHT_DIR;
+        else if (trajectory.waypoints.at(i).direction == 3)
+            wp.bDir = PlannerHNS::BACKWARD_DIR;
+        else if (trajectory.waypoints.at(i).direction == 4)
+            wp.bDir = PlannerHNS::BACKWARD_LEFT_DIR;
+        else if (trajectory.waypoints.at(i).direction == 5)
+            wp.bDir = PlannerHNS::BACKWARD_RIGHT_DIR;
+        else if (trajectory.waypoints.at(i).direction == 6)
+            wp.bDir = PlannerHNS::STANDSTILL_DIR;
+
+        wp.cost = trajectory.waypoints.at(i).cost;
+
+        path.push_back(wp);
+    }
 }
 
-void ROSHelpers::createGlobalLaneArrayMarker(std_msgs::ColorRGBA color,
-    const autoware_msgs::LaneArray &lane_waypoints_array, visualization_msgs::MarkerArray& markerArray, double scale)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "global_lane_array_marker";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 0.75*scale;
-  lane_waypoint_marker.scale.y = 0.75*scale;
-  lane_waypoint_marker.color = color;
-  lane_waypoint_marker.frame_locked = false;
-
-  int count = 0;
-  for (unsigned int i=0; i<  lane_waypoints_array.lanes.size(); i++)
-  {
-    lane_waypoint_marker.points.clear();
-    lane_waypoint_marker.id = count;
+void ROSHelpers::createGlobalLaneArrayMarker(std_msgs::ColorRGBA color, const autoware_msgs::LaneArray &lane_waypoints_array,
+                                             visualization_msgs::MarkerArray &markerArray, double scale) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "global_lane_array_marker";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 0.75 * scale;
+    lane_waypoint_marker.scale.y = 0.75 * scale;
+    lane_waypoint_marker.color = color;
+    lane_waypoint_marker.frame_locked = false;
+
+    int count = 0;
+    for (unsigned int i = 0; i < lane_waypoints_array.lanes.size(); i++) {
+        lane_waypoint_marker.points.clear();
+        lane_waypoint_marker.id = count;
+
+        for (unsigned int j = 0; j < lane_waypoints_array.lanes.at(i).waypoints.size(); j++) {
+            geometry_msgs::Point point;
+            point = lane_waypoints_array.lanes.at(i).waypoints.at(j).pose.pose.position;
+            lane_waypoint_marker.points.push_back(point);
+        }
+        markerArray.markers.push_back(lane_waypoint_marker);
+        count++;
+    }
+}
 
-    for (unsigned int j=0; j < lane_waypoints_array.lanes.at(i).waypoints.size(); j++)
-    {
-      geometry_msgs::Point point;
-      point = lane_waypoints_array.lanes.at(i).waypoints.at(j).pose.pose.position;
-      lane_waypoint_marker.points.push_back(point);
+void ROSHelpers::createGlobalLaneArrayVelocityMarker(const autoware_msgs::LaneArray &lane_waypoints_array,
+                                                     visualization_msgs::MarkerArray &markerArray, double scale) {
+    visualization_msgs::MarkerArray tmp_marker_array;
+    // display by markers the velocity of each waypoint.
+    visualization_msgs::Marker velocity_marker;
+    velocity_marker.header.frame_id = "map";
+    velocity_marker.header.stamp = ros::Time();
+    velocity_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    velocity_marker.action = visualization_msgs::Marker::ADD;
+    // velocity_marker.scale.z = 0.4;
+    velocity_marker.color.a = 0.9;
+    velocity_marker.color.r = 1;
+    velocity_marker.color.g = 1;
+    velocity_marker.color.b = 1;
+    velocity_marker.frame_locked = false;
+
+    int count = 1;
+    for (unsigned int i = 0; i < lane_waypoints_array.lanes.size(); i++) {
+
+        std::ostringstream str_count;
+        str_count << count;
+        velocity_marker.ns = "global_velocity_lane_" + str_count.str();
+        for (unsigned int j = 0; j < lane_waypoints_array.lanes.at(i).waypoints.size(); j++) {
+            // std::cout << _waypoints[i].GetX() << " " << _waypoints[i].GetY() << " " << _waypoints[i].GetZ() << " " <<
+            // _waypoints[i].GetVelocity_kmh() << std::endl;
+            velocity_marker.id = j;
+            geometry_msgs::Point relative_p;
+            relative_p.y = 0.5;
+            velocity_marker.pose.position = calcAbsoluteCoordinate(relative_p, lane_waypoints_array.lanes.at(i).waypoints.at(j).pose.pose);
+            velocity_marker.pose.position.z += 0.2;
+
+            // double to string
+            std::ostringstream str_out;
+            str_out << lane_waypoints_array.lanes.at(i).waypoints.at(j).twist.twist.linear.x;
+            // std::string vel = str_out.str();
+            velocity_marker.text = str_out.str(); // vel.erase(vel.find_first_of(".") + 2);
+
+            tmp_marker_array.markers.push_back(velocity_marker);
+        }
+        count++;
     }
-    markerArray.markers.push_back(lane_waypoint_marker);
-    count++;
-  }
 
+    markerArray.markers.insert(markerArray.markers.end(), tmp_marker_array.markers.begin(), tmp_marker_array.markers.end());
 }
 
-void ROSHelpers::createGlobalLaneArrayVelocityMarker(const autoware_msgs::LaneArray &lane_waypoints_array
-    , visualization_msgs::MarkerArray& markerArray, double scale)
-{
-  visualization_msgs::MarkerArray tmp_marker_array;
-  // display by markers the velocity of each waypoint.
-  visualization_msgs::Marker velocity_marker;
-  velocity_marker.header.frame_id = "map";
-  velocity_marker.header.stamp = ros::Time();
-  velocity_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-  velocity_marker.action = visualization_msgs::Marker::ADD;
-  //velocity_marker.scale.z = 0.4;
-  velocity_marker.color.a = 0.9;
-  velocity_marker.color.r = 1;
-  velocity_marker.color.g = 1;
-  velocity_marker.color.b = 1;
-  velocity_marker.frame_locked = false;
-
-  int count = 1;
-  for (unsigned int i=0; i<  lane_waypoints_array.lanes.size(); i++)
-  {
-
-    std::ostringstream str_count;
-    str_count << count;
-    velocity_marker.ns = "global_velocity_lane_" + str_count.str();
-    for (unsigned int j=0; j < lane_waypoints_array.lanes.at(i).waypoints.size(); j++)
-    {
-      //std::cout << _waypoints[i].GetX() << " " << _waypoints[i].GetY() << " " << _waypoints[i].GetZ() << " " << _waypoints[i].GetVelocity_kmh() << std::endl;
-      velocity_marker.id = j;
-      geometry_msgs::Point relative_p;
-      relative_p.y = 0.5;
-      velocity_marker.pose.position = calcAbsoluteCoordinate(relative_p, lane_waypoints_array.lanes.at(i).waypoints.at(j).pose.pose);
-      velocity_marker.pose.position.z += 0.2;
-
-      // double to string
-      std::ostringstream str_out;
-      str_out << lane_waypoints_array.lanes.at(i).waypoints.at(j).twist.twist.linear.x;
-      //std::string vel = str_out.str();
-      velocity_marker.text = str_out.str();//vel.erase(vel.find_first_of(".") + 2);
-
-      tmp_marker_array.markers.push_back(velocity_marker);
+void ROSHelpers::createGlobalLaneArrayOrientationMarker(const autoware_msgs::LaneArray &lane_waypoints_array,
+                                                        visualization_msgs::MarkerArray &markerArray, double scale) {
+    visualization_msgs::MarkerArray tmp_marker_array;
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.type = visualization_msgs::Marker::ARROW;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 0.6 * scale;
+    lane_waypoint_marker.scale.y = 0.2 * scale;
+    lane_waypoint_marker.scale.z = 0.1 * scale;
+    lane_waypoint_marker.color.r = 1.0;
+    lane_waypoint_marker.color.a = 1.0;
+    // lane_waypoint_marker.frame_locked = false;
+
+    lane_waypoint_marker.ns = "global_lane_waypoint_orientation_marker";
+
+    int count = 1;
+    for (unsigned int i = 0; i < lane_waypoints_array.lanes.size(); i++) {
+        //    std::ostringstream str_ns;
+        //    str_ns << "global_lane_waypoint_orientation_marker_";
+        //    str_ns << i;
+        //   lane_waypoint_marker.ns = str_ns.str();
+
+        for (unsigned int j = 0; j < lane_waypoints_array.lanes.at(i).waypoints.size(); j++) {
+            lane_waypoint_marker.id = count;
+            lane_waypoint_marker.pose = lane_waypoints_array.lanes.at(i).waypoints.at(j).pose.pose;
+
+            if (lane_waypoints_array.lanes.at(i).waypoints.at(j).dtlane.dir == 1) {
+                lane_waypoint_marker.color.r = 0.0;
+                lane_waypoint_marker.color.g = 1.0;
+                lane_waypoint_marker.color.b = 0.0;
+                tmp_marker_array.markers.push_back(lane_waypoint_marker);
+            } else if (lane_waypoints_array.lanes.at(i).waypoints.at(j).dtlane.dir == 2) {
+                lane_waypoint_marker.color.r = 0.0;
+                lane_waypoint_marker.color.g = 0.0;
+                lane_waypoint_marker.color.b = 1.0;
+                tmp_marker_array.markers.push_back(lane_waypoint_marker);
+            } else {
+
+                if (lane_waypoints_array.lanes.at(i).waypoints.at(j).cost >= 100) {
+                    lane_waypoint_marker.color.r = 1.0;
+                    lane_waypoint_marker.color.g = 0.0;
+                    lane_waypoint_marker.color.b = 0.0;
+                    tmp_marker_array.markers.push_back(lane_waypoint_marker);
+                } else {
+                    lane_waypoint_marker.color.r = 0.0;
+                    lane_waypoint_marker.color.g = 0.8;
+                    lane_waypoint_marker.color.b = 0.0;
+                    tmp_marker_array.markers.push_back(lane_waypoint_marker);
+                }
+            }
+
+            count++;
+        }
     }
-    count++;
-  }
 
-  markerArray.markers.insert(markerArray.markers.end(), tmp_marker_array.markers.begin(),
-                                       tmp_marker_array.markers.end());
+    markerArray.markers.insert(markerArray.markers.end(), tmp_marker_array.markers.begin(), tmp_marker_array.markers.end());
 }
 
-void ROSHelpers::createGlobalLaneArrayOrientationMarker(const autoware_msgs::LaneArray &lane_waypoints_array
-    , visualization_msgs::MarkerArray& markerArray, double scale)
-{
-  visualization_msgs::MarkerArray tmp_marker_array;
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.type = visualization_msgs::Marker::ARROW;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 0.6 * scale;
-  lane_waypoint_marker.scale.y = 0.2 * scale;
-  lane_waypoint_marker.scale.z = 0.1 * scale;
-  lane_waypoint_marker.color.r = 1.0;
-  lane_waypoint_marker.color.a = 1.0;
-  //lane_waypoint_marker.frame_locked = false;
-
-  lane_waypoint_marker.ns = "global_lane_waypoint_orientation_marker";
-
-  int count = 1;
-  for (unsigned int i=0; i<  lane_waypoints_array.lanes.size(); i++)
-  {
-//    std::ostringstream str_ns;
-//    str_ns << "global_lane_waypoint_orientation_marker_";
-//    str_ns << i;
-//   lane_waypoint_marker.ns = str_ns.str();
-
-    for (unsigned int j=0; j < lane_waypoints_array.lanes.at(i).waypoints.size(); j++)
-    {
-      lane_waypoint_marker.id = count;
-      lane_waypoint_marker.pose = lane_waypoints_array.lanes.at(i).waypoints.at(j).pose.pose;
-
-      if(lane_waypoints_array.lanes.at(i).waypoints.at(j).dtlane.dir == 1)
-      {
-        lane_waypoint_marker.color.r = 0.0;
-        lane_waypoint_marker.color.g = 1.0;
-        lane_waypoint_marker.color.b = 0.0;
-        tmp_marker_array.markers.push_back(lane_waypoint_marker);
-      }
-      else if(lane_waypoints_array.lanes.at(i).waypoints.at(j).dtlane.dir == 2)
-      {
-        lane_waypoint_marker.color.r = 0.0;
-      lane_waypoint_marker.color.g = 0.0;
-      lane_waypoint_marker.color.b = 1.0;
-      tmp_marker_array.markers.push_back(lane_waypoint_marker);
-      }
-      else
-      {
-
-        if(lane_waypoints_array.lanes.at(i).waypoints.at(j).cost >= 100)
-        {
-          lane_waypoint_marker.color.r = 1.0;
-        lane_waypoint_marker.color.g = 0.0;
-        lane_waypoint_marker.color.b = 0.0;
-        tmp_marker_array.markers.push_back(lane_waypoint_marker);
-        }
-        else
-        {
-        lane_waypoint_marker.color.r = 0.0;
-        lane_waypoint_marker.color.g = 0.8;
-        lane_waypoint_marker.color.b = 0.0;
-        tmp_marker_array.markers.push_back(lane_waypoint_marker);
+void ROSHelpers::GetTrafficLightForVisualization(std::vector<PlannerHNS::TrafficLight> &lights, visualization_msgs::MarkerArray &markerArray) {
+    markerArray.markers.clear();
+    for (unsigned int i = 0; i < lights.size(); i++) {
+        if (lights.at(i).lightState == RED_LIGHT) {
+            visualization_msgs::Marker mkr = CreateGenMarker(lights.at(i).pos.x, lights.at(i).pos.y, lights.at(i).pos.z, 0, 1, 0, 0, 3, i,
+                                                             "traffic_light_visualize", visualization_msgs::Marker::SPHERE);
+            markerArray.markers.push_back(mkr);
+        } else if (lights.at(i).lightState == GREEN_LIGHT) {
+            visualization_msgs::Marker mkr = CreateGenMarker(lights.at(i).pos.x, lights.at(i).pos.y, lights.at(i).pos.z, 0, 0, 1, 0, 3, i,
+                                                             "traffic_light_visualize", visualization_msgs::Marker::SPHERE);
+            markerArray.markers.push_back(mkr);
         }
-      }
-
+    }
+}
 
-      count++;
+void ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(const autoware_msgs::DetectedObject &det_obj,
+                                                                              PlannerHNS::DetectedObject &obj) {
+    obj.header = det_obj.header;
+    obj.id = det_obj.id;
+    obj.label = det_obj.label;
+    obj.l = det_obj.dimensions.x;
+    obj.w = det_obj.dimensions.y;
+    obj.h = det_obj.dimensions.z;
+
+    obj.center.pos.x = det_obj.pose.position.x;
+    obj.center.pos.y = det_obj.pose.position.y;
+    obj.center.pos.z = det_obj.pose.position.z;
+    obj.center.pos.a = tf::getYaw(det_obj.pose.orientation);
+
+    obj.center.v = det_obj.velocity.linear.x;
+    obj.acceleration_raw = det_obj.velocity.linear.y;
+    obj.acceleration_desc = det_obj.velocity.linear.z;
+    obj.bVelocity = det_obj.velocity_reliable;
+    obj.bDirection = det_obj.pose_reliable;
+
+    obj.image_x = det_obj.x;
+    obj.image_y = det_obj.y;
+    obj.image_width = det_obj.width;
+    obj.image_height = det_obj.height;
+
+    if (det_obj.indicator_state == 0)
+        obj.indicator_state = PlannerHNS::INDICATOR_LEFT;
+    else if (det_obj.indicator_state == 1)
+        obj.indicator_state = PlannerHNS::INDICATOR_RIGHT;
+    else if (det_obj.indicator_state == 2)
+        obj.indicator_state = PlannerHNS::INDICATOR_BOTH;
+    else if (det_obj.indicator_state == 3)
+        obj.indicator_state = PlannerHNS::INDICATOR_NONE;
+
+    PlannerHNS::GPSPoint p;
+    obj.contour.clear();
+
+    for (unsigned int j = 0; j < det_obj.convex_hull.polygon.points.size(); j++) {
+
+        p.x = det_obj.convex_hull.polygon.points.at(j).x;
+        p.y = det_obj.convex_hull.polygon.points.at(j).y;
+        p.z = det_obj.convex_hull.polygon.points.at(j).z;
+        obj.contour.push_back(p);
     }
-  }
 
-  markerArray.markers.insert(markerArray.markers.end(), tmp_marker_array.markers.begin(),
-                       tmp_marker_array.markers.end());
+    obj.predTrajectories.clear();
+
+    for (unsigned int j = 0; j < det_obj.candidate_trajectories.lanes.size(); j++) {
+        std::vector<PlannerHNS::WayPoint> _traj;
+        PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(det_obj.candidate_trajectories.lanes.at(j), _traj);
+        for (unsigned int k = 0; k < _traj.size(); k++)
+            _traj.at(k).collisionCost = det_obj.candidate_trajectories.lanes.at(j).cost;
+
+        obj.predTrajectories.push_back(_traj);
+    }
 }
 
-void ROSHelpers::GetTrafficLightForVisualization(std::vector<PlannerHNS::TrafficLight>& lights, visualization_msgs::MarkerArray& markerArray)
-{
-  markerArray.markers.clear();
-  for(unsigned int i=0; i<lights.size(); i++)
-  {
-    if(lights.at(i).lightState == RED_LIGHT)
-    {
-      visualization_msgs::Marker mkr = CreateGenMarker(lights.at(i).pos.x,lights.at(i).pos.y,lights.at(i).pos.z,0,1,0,0,3,i,"traffic_light_visualize", visualization_msgs::Marker::SPHERE);
-      markerArray.markers.push_back(mkr);
+void ROSHelpers::ConvertFromOpenPlannerDetectedObjectToAutowareDetectedObject(const PlannerHNS::DetectedObject &det_obj, const bool &bSimulationMode,
+                                                                              autoware_msgs::DetectedObject &obj) {
+    if (bSimulationMode)
+        obj.id = det_obj.originalID;
+    else
+        obj.id = det_obj.id;
+
+    obj.header = det_obj.header;
+    obj.label = det_obj.label;
+    obj.indicator_state = det_obj.indicator_state;
+    obj.dimensions.x = det_obj.l;
+    obj.dimensions.y = det_obj.w;
+    obj.dimensions.z = det_obj.h;
+
+    obj.pose.position.x = det_obj.center.pos.x;
+    obj.pose.position.y = det_obj.center.pos.y;
+    obj.pose.position.z = det_obj.center.pos.z;
+    obj.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(det_obj.center.pos.a));
+
+    obj.velocity.linear.x = det_obj.center.v;
+    obj.velocity.linear.y = det_obj.acceleration_raw;
+    obj.velocity.linear.z = det_obj.acceleration_desc;
+    obj.velocity_reliable = det_obj.bVelocity;
+    obj.pose_reliable = det_obj.bDirection;
+
+    obj.x = det_obj.image_x;
+    obj.y = det_obj.image_y;
+    obj.width = det_obj.image_width;
+    obj.height = det_obj.image_height;
+
+    geometry_msgs::Point32 p;
+    obj.convex_hull.polygon.points.clear();
+
+    for (unsigned int j = 0; j < det_obj.contour.size(); j++) {
+        p.x = det_obj.contour.at(j).x;
+        p.y = det_obj.contour.at(j).y;
+        p.z = det_obj.contour.at(j).z;
+        obj.convex_hull.polygon.points.push_back(p);
     }
-    else if(lights.at(i).lightState == GREEN_LIGHT)
-    {
-      visualization_msgs::Marker mkr = CreateGenMarker(lights.at(i).pos.x,lights.at(i).pos.y,lights.at(i).pos.z,0,0,1,0,3,i,"traffic_light_visualize", visualization_msgs::Marker::SPHERE);
-      markerArray.markers.push_back(mkr);
+
+    obj.candidate_trajectories.lanes.clear();
+    for (unsigned int j = 0; j < det_obj.predTrajectories.size(); j++) {
+        autoware_msgs::Lane pred_traj;
+        PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(det_obj.predTrajectories.at(j), pred_traj);
+        if (det_obj.predTrajectories.at(j).size() > 0) {
+            pred_traj.cost = det_obj.predTrajectories.at(j).at(0).collisionCost;
+        }
+        pred_traj.lane_index = 0;
+        obj.candidate_trajectories.lanes.push_back(pred_traj);
     }
-  }
 }
 
-void ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(const autoware_msgs::DetectedObject& det_obj, PlannerHNS::DetectedObject& obj)
-{
-  obj.header = det_obj.header;
-  obj.id = det_obj.id;
-  obj.label = det_obj.label;
-  obj.l = det_obj.dimensions.x;
-  obj.w = det_obj.dimensions.y;
-  obj.h = det_obj.dimensions.z;
-
-  obj.center.pos.x = det_obj.pose.position.x;
-  obj.center.pos.y = det_obj.pose.position.y;
-  obj.center.pos.z = det_obj.pose.position.z;
-  obj.center.pos.a = tf::getYaw(det_obj.pose.orientation);
-
-  obj.center.v = det_obj.velocity.linear.x;
-  obj.acceleration_raw = det_obj.velocity.linear.y;
-  obj.acceleration_desc = det_obj.velocity.linear.z;
-  obj.bVelocity = det_obj.velocity_reliable;
-  obj.bDirection = det_obj.pose_reliable;
-
-  obj.image_x = det_obj.x;
-  obj.image_y = det_obj.y;
-  obj.image_width = det_obj.width;
-  obj.image_height = det_obj.height;
-
-  if(det_obj.indicator_state == 0)
-    obj.indicator_state = PlannerHNS::INDICATOR_LEFT;
-  else if(det_obj.indicator_state == 1)
-    obj.indicator_state = PlannerHNS::INDICATOR_RIGHT;
-  else if(det_obj.indicator_state == 2)
-    obj.indicator_state = PlannerHNS::INDICATOR_BOTH;
-  else if(det_obj.indicator_state == 3)
-    obj.indicator_state = PlannerHNS::INDICATOR_NONE;
-
-  PlannerHNS::GPSPoint p;
-  obj.contour.clear();
-
-  for(unsigned int j=0; j < det_obj.convex_hull.polygon.points.size(); j++)
-  {
-
-    p.x = det_obj.convex_hull.polygon.points.at(j).x;
-    p.y = det_obj.convex_hull.polygon.points.at(j).y;
-    p.z = det_obj.convex_hull.polygon.points.at(j).z;
-    obj.contour.push_back(p);
-  }
-
-  obj.predTrajectories.clear();
-
-  for(unsigned int j = 0 ; j < det_obj.candidate_trajectories.lanes.size(); j++)
-  {
-    std::vector<PlannerHNS::WayPoint> _traj;
-    PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(det_obj.candidate_trajectories.lanes.at(j), _traj);
-    for(unsigned int k=0; k < _traj.size(); k++)
-      _traj.at(k).collisionCost = det_obj.candidate_trajectories.lanes.at(j).cost;
-
-    obj.predTrajectories.push_back(_traj);
-  }
-}
+void ROSHelpers::UpdateRoadMap(const AutowareRoadNetwork &src_map, PlannerHNS::RoadNetwork &out_map) {
+    std::vector<UtilityHNS::AisanLanesFileReader::AisanLane> lanes;
+    for (unsigned int i = 0; i < src_map.lanes.data.size(); i++) {
+        UtilityHNS::AisanLanesFileReader::AisanLane l;
+        l.BLID = src_map.lanes.data.at(i).blid;
+        l.BLID2 = src_map.lanes.data.at(i).blid2;
+        l.BLID3 = src_map.lanes.data.at(i).blid3;
+        l.BLID4 = src_map.lanes.data.at(i).blid4;
+        l.BNID = src_map.lanes.data.at(i).bnid;
+        l.ClossID = src_map.lanes.data.at(i).clossid;
+        l.DID = src_map.lanes.data.at(i).did;
+        l.FLID = src_map.lanes.data.at(i).flid;
+        l.FLID2 = src_map.lanes.data.at(i).flid2;
+        l.FLID3 = src_map.lanes.data.at(i).flid3;
+        l.FLID4 = src_map.lanes.data.at(i).flid4;
+        l.FNID = src_map.lanes.data.at(i).fnid;
+        l.JCT = src_map.lanes.data.at(i).jct;
+        l.LCnt = src_map.lanes.data.at(i).lcnt;
+        l.LnID = src_map.lanes.data.at(i).lnid;
+        l.Lno = src_map.lanes.data.at(i).lno;
+        l.Span = src_map.lanes.data.at(i).span;
+        l.RefVel = src_map.lanes.data.at(i).refvel;
+        l.LimitVel = src_map.lanes.data.at(i).limitvel;
+
+        //    l.LaneChgFG =  src_map.lanes.at(i).;
+        //    l.LaneType   =  src_map.lanes.at(i).blid;
+        //    l.LimitVel   =  src_map.lanes.at(i).;
+        //    l.LinkWAID   =  src_map.lanes.at(i).blid;
+        //    l.RefVel   =  src_map.lanes.at(i).blid;
+        //    l.RoadSecID =  src_map.lanes.at(i).;
+
+        lanes.push_back(l);
+    }
 
-void ROSHelpers::ConvertFromOpenPlannerDetectedObjectToAutowareDetectedObject(const PlannerHNS::DetectedObject& det_obj, const bool& bSimulationMode, autoware_msgs::DetectedObject& obj)
-{
-  if(bSimulationMode)
-    obj.id = det_obj.originalID;
-  else
-    obj.id = det_obj.id;
+    std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints> points;
+
+    for (unsigned int i = 0; i < src_map.points.data.size(); i++) {
+        UtilityHNS::AisanPointsFileReader::AisanPoints p;
+        double integ_part = src_map.points.data.at(i).l;
+        double deg = trunc(src_map.points.data.at(i).l);
+        double min = trunc((src_map.points.data.at(i).l - deg) * 100.0) / 60.0;
+        double sec = modf((src_map.points.data.at(i).l - deg) * 100.0, &integ_part) / 36.0;
+        double L = deg + min + sec;
+
+        deg = trunc(src_map.points.data.at(i).b);
+        min = trunc((src_map.points.data.at(i).b - deg) * 100.0) / 60.0;
+        sec = modf((src_map.points.data.at(i).b - deg) * 100.0, &integ_part) / 36.0;
+        double B = deg + min + sec;
+
+        p.B = B;
+        p.Bx = src_map.points.data.at(i).bx;
+        p.H = src_map.points.data.at(i).h;
+        p.L = L;
+        p.Ly = src_map.points.data.at(i).ly;
+        p.MCODE1 = src_map.points.data.at(i).mcode1;
+        p.MCODE2 = src_map.points.data.at(i).mcode2;
+        p.MCODE3 = src_map.points.data.at(i).mcode3;
+        p.PID = src_map.points.data.at(i).pid;
+        p.Ref = src_map.points.data.at(i).ref;
+
+        points.push_back(p);
+    }
 
-  obj.header = det_obj.header;
-  obj.label = det_obj.label;
-  obj.indicator_state = det_obj.indicator_state;
-  obj.dimensions.x = det_obj.l;
-  obj.dimensions.y = det_obj.w;
-  obj.dimensions.z = det_obj.h;
-
-  obj.pose.position.x = det_obj.center.pos.x;
-  obj.pose.position.y = det_obj.center.pos.y;
-  obj.pose.position.z = det_obj.center.pos.z;
-  obj.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(det_obj.center.pos.a));
-
-  obj.velocity.linear.x = det_obj.center.v;
-  obj.velocity.linear.y = det_obj.acceleration_raw;
-  obj.velocity.linear.z = det_obj.acceleration_desc;
-  obj.velocity_reliable = det_obj.bVelocity;
-  obj.pose_reliable = det_obj.bDirection;
-
-  obj.x = det_obj.image_x;
-  obj.y = det_obj.image_y;
-  obj.width = det_obj.image_width;
-  obj.height = det_obj.image_height;
-
-  geometry_msgs::Point32 p;
-  obj.convex_hull.polygon.points.clear();
-
-  for(unsigned int j=0; j < det_obj.contour.size(); j++)
-  {
-    p.x = det_obj.contour.at(j).x;
-    p.y = det_obj.contour.at(j).y;
-    p.z = det_obj.contour.at(j).z;
-    obj.convex_hull.polygon.points.push_back(p);
-  }
-
-
-  obj.candidate_trajectories.lanes.clear();
-  for(unsigned int j = 0 ; j < det_obj.predTrajectories.size(); j++)
-  {
-    autoware_msgs::Lane pred_traj;
-    PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(det_obj.predTrajectories.at(j), pred_traj);
-    if(det_obj.predTrajectories.at(j).size() > 0)
-    {
-      pred_traj.cost = det_obj.predTrajectories.at(j).at(0).collisionCost;
+    std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine> dts;
+    for (unsigned int i = 0; i < src_map.dtlanes.data.size(); i++) {
+        UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine dt;
+
+        dt.Apara = src_map.dtlanes.data.at(i).apara;
+        dt.DID = src_map.dtlanes.data.at(i).did;
+        dt.Dir = src_map.dtlanes.data.at(i).dir;
+        dt.Dist = src_map.dtlanes.data.at(i).dist;
+        dt.LW = src_map.dtlanes.data.at(i).lw;
+        dt.PID = src_map.dtlanes.data.at(i).pid;
+        dt.RW = src_map.dtlanes.data.at(i).rw;
+        dt.cant = src_map.dtlanes.data.at(i).cant;
+        dt.r = src_map.dtlanes.data.at(i).r;
+        dt.slope = src_map.dtlanes.data.at(i).slope;
+
+        dts.push_back(dt);
     }
-    pred_traj.lane_index = 0;
-    obj.candidate_trajectories.lanes.push_back(pred_traj);
-  }
-}
 
-void ROSHelpers::UpdateRoadMap(const AutowareRoadNetwork& src_map, PlannerHNS::RoadNetwork& out_map)
-{
-  std::vector<UtilityHNS::AisanLanesFileReader::AisanLane> lanes;
-  for(unsigned int i=0; i < src_map.lanes.data.size();i++)
-  {
-    UtilityHNS::AisanLanesFileReader::AisanLane l;
-    l.BLID     =  src_map.lanes.data.at(i).blid;
-    l.BLID2   =  src_map.lanes.data.at(i).blid2;
-    l.BLID3   =  src_map.lanes.data.at(i).blid3;
-    l.BLID4   =  src_map.lanes.data.at(i).blid4;
-    l.BNID     =  src_map.lanes.data.at(i).bnid;
-    l.ClossID   =  src_map.lanes.data.at(i).clossid;
-    l.DID     =  src_map.lanes.data.at(i).did;
-    l.FLID     =  src_map.lanes.data.at(i).flid;
-    l.FLID2   =  src_map.lanes.data.at(i).flid2;
-    l.FLID3   =  src_map.lanes.data.at(i).flid3;
-    l.FLID4   =  src_map.lanes.data.at(i).flid4;
-    l.FNID     =  src_map.lanes.data.at(i).fnid;
-    l.JCT     =  src_map.lanes.data.at(i).jct;
-    l.LCnt     =  src_map.lanes.data.at(i).lcnt;
-    l.LnID     =  src_map.lanes.data.at(i).lnid;
-    l.Lno     =  src_map.lanes.data.at(i).lno;
-    l.Span     =  src_map.lanes.data.at(i).span;
-    l.RefVel  =  src_map.lanes.data.at(i).refvel;
-    l.LimitVel  =  src_map.lanes.data.at(i).limitvel;
-
-//    l.LaneChgFG =  src_map.lanes.at(i).;
-//    l.LaneType   =  src_map.lanes.at(i).blid;
-//    l.LimitVel   =  src_map.lanes.at(i).;
-//    l.LinkWAID   =  src_map.lanes.at(i).blid;
-//    l.RefVel   =  src_map.lanes.at(i).blid;
-//    l.RoadSecID =  src_map.lanes.at(i).;
-
-    lanes.push_back(l);
-  }
-
-  std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints> points;
-
-  for(unsigned int i=0; i < src_map.points.data.size();i++)
-  {
-    UtilityHNS::AisanPointsFileReader::AisanPoints p;
-    double integ_part = src_map.points.data.at(i).l;
-    double deg = trunc(src_map.points.data.at(i).l);
-    double min = trunc((src_map.points.data.at(i).l - deg) * 100.0) / 60.0;
-    double sec = modf((src_map.points.data.at(i).l - deg) * 100.0, &integ_part)/36.0;
-    double L =  deg + min + sec;
-
-    deg = trunc(src_map.points.data.at(i).b);
-    min = trunc((src_map.points.data.at(i).b - deg) * 100.0) / 60.0;
-    sec = modf((src_map.points.data.at(i).b - deg) * 100.0, &integ_part)/36.0;
-    double B =  deg + min + sec;
-
-    p.B     = B;
-    p.Bx     = src_map.points.data.at(i).bx;
-    p.H     = src_map.points.data.at(i).h;
-    p.L     = L;
-    p.Ly     = src_map.points.data.at(i).ly;
-    p.MCODE1   = src_map.points.data.at(i).mcode1;
-    p.MCODE2   = src_map.points.data.at(i).mcode2;
-    p.MCODE3   = src_map.points.data.at(i).mcode3;
-    p.PID     = src_map.points.data.at(i).pid;
-    p.Ref     = src_map.points.data.at(i).ref;
-
-    points.push_back(p);
-  }
-
-
-  std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine> dts;
-  for(unsigned int i=0; i < src_map.dtlanes.data.size();i++)
-  {
-    UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine dt;
-
-    dt.Apara   = src_map.dtlanes.data.at(i).apara;
-    dt.DID     = src_map.dtlanes.data.at(i).did;
-    dt.Dir     = src_map.dtlanes.data.at(i).dir;
-    dt.Dist   = src_map.dtlanes.data.at(i).dist;
-    dt.LW     = src_map.dtlanes.data.at(i).lw;
-    dt.PID     = src_map.dtlanes.data.at(i).pid;
-    dt.RW     = src_map.dtlanes.data.at(i).rw;
-    dt.cant   = src_map.dtlanes.data.at(i).cant;
-    dt.r     = src_map.dtlanes.data.at(i).r;
-    dt.slope   = src_map.dtlanes.data.at(i).slope;
-
-    dts.push_back(dt);
-  }
-
-  std::vector<UtilityHNS::AisanAreasFileReader::AisanArea> areas;
-  std::vector<UtilityHNS::AisanIntersectionFileReader::AisanIntersection> inters;
-  std::vector<UtilityHNS::AisanLinesFileReader::AisanLine> line_data;
-  std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine> stop_line_data;
-  std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal> signal_data;
-  std::vector<UtilityHNS::AisanVectorFileReader::AisanVector> vector_data;
-  std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb> curb_data;
-  std::vector<UtilityHNS::AisanRoadEdgeFileReader::AisanRoadEdge> roadedge_data;
-  std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea> way_area;
-  std::vector<UtilityHNS::AisanCrossWalkFileReader::AisanCrossWalk> crossing;
-  std::vector<UtilityHNS::AisanNodesFileReader::AisanNode > nodes_data;
-  std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;
-
-  PlannerHNS::GPSPoint origin;//(m_OriginPos.position.x, m_OriginPos.position.y, m_OriginPos.position.z, 0);
-  PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(lanes, points, dts, inters, areas, line_data, stop_line_data, signal_data, vector_data, curb_data, roadedge_data,way_area, crossing, nodes_data,  conn_data, origin, out_map);
+    std::vector<UtilityHNS::AisanAreasFileReader::AisanArea> areas;
+    std::vector<UtilityHNS::AisanIntersectionFileReader::AisanIntersection> inters;
+    std::vector<UtilityHNS::AisanLinesFileReader::AisanLine> line_data;
+    std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine> stop_line_data;
+    std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal> signal_data;
+    std::vector<UtilityHNS::AisanVectorFileReader::AisanVector> vector_data;
+    std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb> curb_data;
+    std::vector<UtilityHNS::AisanRoadEdgeFileReader::AisanRoadEdge> roadedge_data;
+    std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea> way_area;
+    std::vector<UtilityHNS::AisanCrossWalkFileReader::AisanCrossWalk> crossing;
+    std::vector<UtilityHNS::AisanNodesFileReader::AisanNode> nodes_data;
+    std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;
+
+    PlannerHNS::GPSPoint origin; //(m_OriginPos.position.x, m_OriginPos.position.y, m_OriginPos.position.z, 0);
+    PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(lanes, points, dts, inters, areas, line_data, stop_line_data, signal_data,
+                                                                   vector_data, curb_data, roadedge_data, way_area, crossing, nodes_data, conn_data,
+                                                                   origin, out_map);
 }
 
-void ROSHelpers::GetIndicatorArrows(const PlannerHNS::WayPoint& center, const double& width,const double& length, const PlannerHNS::LIGHT_INDICATOR& indicator, const int& id, visualization_msgs::MarkerArray& markerArray)
-{
-  double critical_lateral_distance =  width/2.0 + 0.2;
-  //double critical_long_front_distance =  carInfo.length/2.0 ;
-  PlannerHNS::GPSPoint top_right(critical_lateral_distance, length, center.pos.z, 0);
-  PlannerHNS::GPSPoint top_left(-critical_lateral_distance, length, center.pos.z, 0);
-
-  PlannerHNS::Mat3 invRotationMat(center.pos.a-M_PI_2);
-  PlannerHNS::Mat3 invTranslationMat(center.pos.x, center.pos.y);
-
-  top_right = invRotationMat*top_right;
-  top_right = invTranslationMat*top_right;
-  top_left = invRotationMat*top_left;
-  top_left = invTranslationMat*top_left;
-
-  top_right.a = center.pos.a - M_PI_2;
-  top_left.a = center.pos.a + M_PI_2;
-
-  std_msgs::ColorRGBA color_l, color_r;
-  color_l.r = 1; color_l.g = 1;color_l.b = 1;
-  color_r.r = 1; color_r.g = 1;color_r.b = 1;
-
-  if(indicator == PlannerHNS::INDICATOR_LEFT)
-  {
-    color_l.b = 0;
-  }
-  else if(indicator == PlannerHNS::INDICATOR_RIGHT )
-  {
-    color_r.b = 0;
-  }
-  else if(indicator == PlannerHNS::INDICATOR_BOTH)
-  {
-    color_l.b = 0;
-    color_r.b = 0;
-  }
-
-  visualization_msgs::Marker mkr_l = PlannerHNS::ROSHelpers::CreateGenMarker(top_left.x,top_left.y,top_left.z,top_left.a,color_l.r,color_l.g,color_l.b,1.0, id,"simu_car_indicator_left", visualization_msgs::Marker::ARROW);
-  mkr_l.scale.y = 0.4;
-  mkr_l.scale.z = 0.4;
-  visualization_msgs::Marker mkr_r = PlannerHNS::ROSHelpers::CreateGenMarker(top_right.x,top_right.y,top_right.z,top_right.a,color_r.r,color_r.g,color_r.b,1.0, id,"simu_car_indicator_right", visualization_msgs::Marker::ARROW);
-  mkr_r.scale.y = 0.4;
-  mkr_r.scale.z = 0.4;
-  markerArray.markers.push_back(mkr_l);
-  markerArray.markers.push_back(mkr_r);
+void ROSHelpers::GetIndicatorArrows(const PlannerHNS::WayPoint &center, const double &width, const double &length,
+                                    const PlannerHNS::LIGHT_INDICATOR &indicator, const int &id, visualization_msgs::MarkerArray &markerArray) {
+    double critical_lateral_distance = width / 2.0 + 0.2;
+    // double critical_long_front_distance =  carInfo.length/2.0 ;
+    PlannerHNS::GPSPoint top_right(critical_lateral_distance, length, center.pos.z, 0);
+    PlannerHNS::GPSPoint top_left(-critical_lateral_distance, length, center.pos.z, 0);
+
+    PlannerHNS::Mat3 invRotationMat(center.pos.a - M_PI_2);
+    PlannerHNS::Mat3 invTranslationMat(center.pos.x, center.pos.y);
+
+    top_right = invRotationMat * top_right;
+    top_right = invTranslationMat * top_right;
+    top_left = invRotationMat * top_left;
+    top_left = invTranslationMat * top_left;
+
+    top_right.a = center.pos.a - M_PI_2;
+    top_left.a = center.pos.a + M_PI_2;
+
+    std_msgs::ColorRGBA color_l, color_r;
+    color_l.r = 1;
+    color_l.g = 1;
+    color_l.b = 1;
+    color_r.r = 1;
+    color_r.g = 1;
+    color_r.b = 1;
+
+    if (indicator == PlannerHNS::INDICATOR_LEFT) {
+        color_l.b = 0;
+    } else if (indicator == PlannerHNS::INDICATOR_RIGHT) {
+        color_r.b = 0;
+    } else if (indicator == PlannerHNS::INDICATOR_BOTH) {
+        color_l.b = 0;
+        color_r.b = 0;
+    }
+
+    visualization_msgs::Marker mkr_l =
+        PlannerHNS::ROSHelpers::CreateGenMarker(top_left.x, top_left.y, top_left.z, top_left.a, color_l.r, color_l.g, color_l.b, 1.0, id,
+                                                "simu_car_indicator_left", visualization_msgs::Marker::ARROW);
+    mkr_l.scale.y = 0.4;
+    mkr_l.scale.z = 0.4;
+    visualization_msgs::Marker mkr_r =
+        PlannerHNS::ROSHelpers::CreateGenMarker(top_right.x, top_right.y, top_right.z, top_right.a, color_r.r, color_r.g, color_r.b, 1.0, id,
+                                                "simu_car_indicator_right", visualization_msgs::Marker::ARROW);
+    mkr_r.scale.y = 0.4;
+    mkr_r.scale.z = 0.4;
+    markerArray.markers.push_back(mkr_l);
+    markerArray.markers.push_back(mkr_r);
 }
 
-void ROSHelpers::TTC_PathRviz(const std::vector<PlannerHNS::WayPoint>& path, visualization_msgs::MarkerArray& markerArray)
-{
-  visualization_msgs::Marker lane_waypoint_marker;
-  lane_waypoint_marker.header.frame_id = "map";
-  lane_waypoint_marker.header.stamp = ros::Time();
-  lane_waypoint_marker.ns = "ttc_path";
-  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
-  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
-  lane_waypoint_marker.scale.x = 1;
-  lane_waypoint_marker.scale.y = 1;
-  lane_waypoint_marker.scale.z = 1;
-  lane_waypoint_marker.frame_locked = false;
-  std_msgs::ColorRGBA  total_color, curr_color;
-
-  lane_waypoint_marker.color.a = 0.9;
-  lane_waypoint_marker.color.r = 0.5;
-  lane_waypoint_marker.color.g = 1.0;
-  lane_waypoint_marker.color.b = 0.0;
-
-  lane_waypoint_marker.id = 1;
-  for (unsigned int i = 0; i < path.size(); i++)
-  {
-    geometry_msgs::Point point;
-    point.x = path.at(i).pos.x;
-    point.y = path.at(i).pos.y;
-    point.z = path.at(i).pos.z;
+void ROSHelpers::TTC_PathRviz(const std::vector<PlannerHNS::WayPoint> &path, visualization_msgs::MarkerArray &markerArray) {
+    visualization_msgs::Marker lane_waypoint_marker;
+    lane_waypoint_marker.header.frame_id = "map";
+    lane_waypoint_marker.header.stamp = ros::Time();
+    lane_waypoint_marker.ns = "ttc_path";
+    lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+    lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+    lane_waypoint_marker.scale.x = 1;
+    lane_waypoint_marker.scale.y = 1;
+    lane_waypoint_marker.scale.z = 1;
+    lane_waypoint_marker.frame_locked = false;
+    std_msgs::ColorRGBA total_color, curr_color;
+
+    lane_waypoint_marker.color.a = 0.9;
+    lane_waypoint_marker.color.r = 0.5;
+    lane_waypoint_marker.color.g = 1.0;
+    lane_waypoint_marker.color.b = 0.0;
+
+    lane_waypoint_marker.id = 1;
+    for (unsigned int i = 0; i < path.size(); i++) {
+        geometry_msgs::Point point;
+        point.x = path.at(i).pos.x;
+        point.y = path.at(i).pos.y;
+        point.z = path.at(i).pos.z;
 
-    lane_waypoint_marker.points.push_back(point);
+        lane_waypoint_marker.points.push_back(point);
 
-    markerArray.markers.push_back(lane_waypoint_marker);
-  }
+        markerArray.markers.push_back(lane_waypoint_marker);
+    }
 }
 
-}
+} // namespace PlannerHNS
diff --git a/autoware.ai/src/autoware/common/op_utility/CMakeLists.txt b/autoware.ai/src/autoware/common/op_utility/CMakeLists.txt
index 769e43ac..a6976cac 100644
--- a/autoware.ai/src/autoware/common/op_utility/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/op_utility/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(op_utility)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   cmake_modules
@@ -50,14 +50,4 @@ install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(test-op_utility
-    test/test_op_utility.test
-    test/src/test_op_utility.cpp
-  )
-  add_dependencies(test-op_utility ${catkin_EXPORTED_TARGETS})
-  target_link_libraries(test-op_utility ${catkin_LIBRARIES} ${PROJECT_NAME})
-endif ()
+)
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/op_utility/package.xml b/autoware.ai/src/autoware/common/op_utility/package.xml
index 29d44069..0bcb34c2 100644
--- a/autoware.ai/src/autoware/common/op_utility/package.xml
+++ b/autoware.ai/src/autoware/common/op_utility/package.xml
@@ -7,7 +7,6 @@
   <author email="hatem.darweesh@g.sp.m.is.nagoya-u.ac.jp">Hatem Darweesh</author>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
   <buildtool_depend>cmake_modules</buildtool_depend>
   <test_depend>rostest</test_depend>
diff --git a/autoware.ai/src/autoware/common/ros_observer/CMakeLists.txt b/autoware.ai/src/autoware/common/ros_observer/CMakeLists.txt
index 107ce212..ed767273 100644
--- a/autoware.ai/src/autoware/common/ros_observer/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/ros_observer/CMakeLists.txt
@@ -74,16 +74,3 @@ install(TARGETS ros_observer lib_ros_observer
 install(DIRECTORY launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
 )
-
-if (CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(test_ros_observer
-    test/test_ros_observer.test
-    test/src/test_ros_observer.cpp
-  )
-  target_link_libraries(test_ros_observer
-    lib_ros_observer
-    ${catkin_LIBRARIES}
-  )
-endif()
diff --git a/autoware.ai/src/autoware/common/rubis_lib/CMakeLists.txt b/autoware.ai/src/autoware/common/rubis_lib/CMakeLists.txt
index 03792c36..e9e8e19d 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/rubis_lib/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(rubis_lib)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(PCL REQUIRED)
 
diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common.hpp b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common.hpp
deleted file mode 100644
index dd7b197b..00000000
--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common.hpp
+++ /dev/null
@@ -1,8 +0,0 @@
-// Task state
-#define TASK_STATE_READY 0
-#define TASK_STATE_RUNNING 1
-#define TASK_STATE_DONE 2
-#define GPU_SEG_LOOP_START 0
-#define GPU_SEG_LOOP_MID 1
-#define GPU_SEG_LOOP_END 2
-#define RUBIS_NO_INSTANCE 0
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common_c.h b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common_c.h
deleted file mode 100644
index 52711361..00000000
--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common_c.h
+++ /dev/null
@@ -1,10 +0,0 @@
-// Task state
-#define TASK_STATE_READY 0
-#define TASK_STATE_RUNNING 1
-#define TASK_STATE_DONE 2
-
-#define GPU_SEG_LOOP_START 0
-#define GPU_SEG_LOOP_MID 1
-#define GPU_SEG_LOOP_END 2
-
-#define RUBIS_NO_INSTANCE 0
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp
index 496d3899..c9b069ec 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp
+++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp
@@ -55,7 +55,6 @@
 #define TASK_READY 1
 
 namespace rubis {
-namespace sched {
 
 // contains thread-specific arguments
 struct thr_arg {
@@ -91,61 +90,31 @@ struct sched_attr {
 	__u64 sched_period;
 };
 
-// GPU 
-typedef struct  gpuSchedInfo{
-    int pid;
-    unsigned long deadline;
-    int state; // NONE = 0, WAIT = 1, RUN = 2
-    int scheduling_flag;
-} GPUSchedInfo;
-
 extern int key_id_;
 extern int is_scheduled_;
-extern int gpu_scheduling_flag_;
-extern GPUSchedInfo* gpu_sched_info_;
-extern int gpu_scheduler_pid_;
 extern std::string task_filename_;
-extern std::string gpu_deadline_filename_;
-// extern unsigned long gpu_deadline_list_[1024];
-extern unsigned long* gpu_deadline_list_;
-extern unsigned int max_gpu_id_;
-extern unsigned int gpu_seg_id_;
-extern unsigned int cpu_seg_id_;
-extern int task_state_;
-extern int is_task_ready_;
-extern int was_in_loop_;
-extern int loop_cnt_;
-extern int gpu_seg_cnt_in_loop_;
-
-// Task scheduling
+
 int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags);
 int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigned int flags);
-bool set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period);
-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline);
+
+bool set_sched_deadline(int pid, unsigned int exec_time, unsigned int deadline, unsigned int period);
+bool set_sched_fifo(int pid, int priority);
+bool set_sched_fifo(int pid, int priority, int child_priority);
+bool set_sched_rr(int pid, int priority);
+bool set_sched_rr(int pid, int priority, int child_priority);
+
+bool init_task_scheduling(std::string policy, struct sched_attr attr);
 void yield_task_scheduling();
-void init_task();
-void disable_task();
+struct sched_attr create_sched_attr(int priority, int exec_time, int deadline, int period);
 
-// GPU scheduling
-void init_gpu_scheduling(std::string task_filename, std::string gpu_deadline_filename, int key_id);
-void get_deadline_list();
 void sig_handler(int signum);
 void termination();
 unsigned long get_current_time_us();
 
+std::string get_cmd_output(const char* cmd);
+std::vector<int> get_child_pids(int pid);
+std::vector<std::string> tokenize_string(std::string s, std::string delimiter);
 
-void start_job();
-void finish_job();
-
-void request_gpu();
-void request_gpu_in_loop(int flag);
-
-void yield_gpu(std::string remark = "");
-void yield_gpu_in_loop(int flag, std::string remark = "");
-void print_loop_info(std::string tag);
-void print_gpu_deadline_list();
-
-} // namespace sched
 } // namespace rubis
 
 #endif
diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h
index 986c680c..60af8e3f 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h
+++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h
@@ -89,58 +89,18 @@ struct sched_attr {
 	__u64 sched_period;
 };
 
-// GPU 
-typedef struct  gpuSchedInfo{
-    int pid;
-    unsigned long deadline;
-    int state; // NONE = 0, WAIT = 1, RUN = 2
-    int scheduling_flag;
-} GPUSchedInfo;
-
 extern int key_id_;
 extern int is_scheduled_;
-extern int gpu_scheduling_flag_;
-extern GPUSchedInfo* gpu_sched_info_;
-extern int gpu_scheduler_pid_;
 extern char* task_filename_;
-extern char* gpu_deadline_filename_;
-// extern unsigned long gpu_deadline_list_[1024];
-extern unsigned long* gpu_deadline_list_;
-extern unsigned int max_gpu_id_;
-extern unsigned int gpu_seg_id_;
-extern unsigned int cpu_seg_id_;
-extern int task_state_;
-extern int is_task_ready_;
-extern int was_in_loop_;
-extern int loop_cnt_;
-extern int gpu_seg_cnt_in_loop_;
 
 // Task scheduling
 // int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags);
 // int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigned int flags);
 int set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period);
-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline);
 void yield_task_scheduling();
-void init_task();
-void disable_task();
 
-// GPU scheduling
-void init_gpu_scheduling(char* task_filename, char* gpu_deadline_filename, int key_id);
-void get_deadline_list();
 void sig_handler(int signum);
 void termination();
 unsigned long get_current_time_us();
 
-void start_job();
-void finish_job();
-
-void request_gpu();
-void yield_gpu();
-void yield_gpu_with_remark(const char* remark);
-
-void request_gpu_in_loop(int flag);
-void yield_gpu_in_loop(int flag);
-void yield_gpu_with_remark_in_loop(int flag, const char* remark);
-void print_loop_info(const char* tag);
-void print_gpu_deadline_list();
 #endif
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp
index f428b32a..96294833 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp
+++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp
@@ -1,52 +1,32 @@
 #ifndef __RUBIS_LIB_PROF_H__
 #define __RUBIS_LIB_PROF_H__
 
-#include <stdio.h>
+#include <cstdlib>
+#include <iostream>
 #include <signal.h>
+#include <stdio.h>
+#include <string.h>
 #include <sys/stat.h>
 #include <sys/types.h>
 #include <unistd.h>
-#include <string.h>
-#include <iostream>
-#include <cstdlib>
-#include <rubis_lib/common.hpp>
 
 namespace rubis {
-  extern int instance_mode_;
-  extern unsigned long instance_;
-
-namespace sched {
-  extern int task_profiling_flag_;
-  extern int gpu_profiling_flag_;
-
-  extern FILE* task_response_time_fp_;
-  extern FILE* seg_execution_time_fp_;
-  extern FILE* seg_response_time_fp_;
-
-  extern int is_gpu_profiling_started_;
-
-  extern struct timespec task_start_time_;
-  extern struct timespec task_end_time_;
-  extern unsigned long gpu_seg_response_time_;
-  extern unsigned long gpu_seg_execution_time_;
-  extern unsigned long cpu_seg_response_time_;
-  extern int is_gpu_profiling_ready_;
-
-  void init_task_profiling(std::string task_reponse_time_filename);
-  void start_task_profiling();
-  void stop_task_profiling(unsigned long instance, int state);
-  void init_gpu_profiling(std::string execution_time_filename, std::string response_time_filename);
-  void start_profiling_cpu_seg_response_time();
-  void stop_profiling_cpu_seg_response_time(unsigned int cpu_seg_id, unsigned int iter);
-  void start_profiling_gpu_seg_response_time();
-  void start_profiling_gpu_seg_execution_time();
-  void stop_profiling_gpu_seg_time(unsigned int gpu_seg_id, unsigned int iter, std::string remark = " ");
-  unsigned long get_current_time_ns();
-  void start_job_profiling();
-  void finish_job_profiling(unsigned int cpu_seg_id);
-  void start_gpu_profiling();
-
-}
-}
+extern unsigned long instance_;
+extern unsigned long lidar_instance_;
+extern unsigned long vision_instance_;
+
+extern FILE *task_response_time_fp_;
+
+extern struct timespec task_start_time_;
+extern struct timespec task_end_time_;
+
+void init_task_profiling(std::string task_reponse_time_filename);
+void start_task_profiling();
+void start_task_profiling_at_initial_node(long long tp_time_sec,
+                                          long long tp_time_nsec);
+void stop_task_profiling(unsigned long instance, unsigned long lidar_instance_,
+                         unsigned long vision_instance_);
+unsigned long get_current_time_ns();
+} // namespace rubis
 
 #endif
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h
index 4925accf..23338237 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h
+++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h
@@ -1,60 +1,38 @@
 #ifndef __RUBIS_LIB_PROF_H__
 #define __RUBIS_LIB_PROF_H__
 
-
+#include <signal.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <signal.h>
+#include <string.h>
 #include <sys/stat.h>
 #include <sys/types.h>
-#include <unistd.h>
-#include <string.h>
 #include <time.h>
-#include <rubis_lib/common_c.h>
+#include <unistd.h>
 
 #define rubis_lib_BUFFER_SIZE 1024
 
-extern int instance_mode_;
 extern unsigned long instance_;
+extern unsigned long lidar_instance_;
+extern unsigned long vision_instance_;
 
-extern int task_profiling_flag_;
-extern int gpu_profiling_flag_;
-
-extern FILE* task_response_time_fp_;
-extern FILE* seg_execution_time_fp_;
-extern FILE* seg_response_time_fp_;
-
-extern int is_gpu_profiling_started_;
+extern FILE *task_response_time_fp_;
 
 extern struct timespec task_start_time_;
 extern struct timespec task_end_time_;
-extern unsigned long gpu_seg_response_time_;
-extern unsigned long gpu_seg_execution_time_;
-extern unsigned long cpu_seg_response_time_;
-extern int is_gpu_profiling_ready_;
 
 #ifdef __cplusplus
-extern "C"{
+extern "C" {
 #endif
 
-void init_task_profiling(char* _task_response_time_filename);
+void init_task_profiling(char *_task_response_time_filename);
 void start_task_profiling();
-void stop_task_profiling(unsigned long instance, int state);
-void init_gpu_profiling(char* _execution_time_filename, char* _response_time_filename);
-void start_profiling_cpu_seg_response_time();
-void stop_profiling_cpu_seg_response_time(unsigned int cpu_seg_id, unsigned int iter);
-void start_profiling_gpu_seg_response_time();
-void start_profiling_gpu_seg_execution_time();
-void stop_profiling_gpu_seg_time(unsigned int gpu_seg_id, unsigned int iter);
-void stop_profiling_gpu_seg_time_with_remark(unsigned int gpu_seg_id, unsigned int iter, const char* remark);
+void stop_task_profiling(unsigned long instance, unsigned long lidar_instance_,
+                         unsigned long vision_instance_);
 unsigned long get_current_time_ns();
-void start_job_profiling();
-void finish_job_profiling(unsigned int cpu_seg_id);
-void start_gpu_profiling();
 
 #ifdef __cplusplus
 }
 #endif
 
 #endif
-
diff --git a/autoware.ai/src/autoware/common/rubis_lib/package.xml b/autoware.ai/src/autoware/common/rubis_lib/package.xml
index 5e9f7979..085d84de 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/package.xml
+++ b/autoware.ai/src/autoware/common/rubis_lib/package.xml
@@ -6,7 +6,6 @@
   
   <maintainer email="kite9240@gmail.com">hypark</maintainer>
   <license>MIT</license>
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>pcl_conversions</depend>
diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp b/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp
index 4d41e5c3..3eaf336c 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp
+++ b/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp
@@ -3,26 +3,21 @@
 // #define DEBUG 
 
 namespace rubis{
-namespace sched{
 
 int key_id_;
 int is_scheduled_;
-int gpu_scheduling_flag_;
-GPUSchedInfo* gpu_sched_info_;
-int gpu_scheduler_pid_;
 std::string task_filename_;
-std::string gpu_deadline_filename_;
-// unsigned long gpu_deadline_list_[1024];
-unsigned long* gpu_deadline_list_;
-unsigned int max_gpu_id_ = 0;
-unsigned int cpu_seg_id_ = 0;
-unsigned int gpu_seg_id_ = 0;
-
-int is_task_ready_ = TASK_NOT_READY;
-int task_state_ = TASK_STATE_READY;
-int was_in_loop_ = 0;
-int loop_cnt_ = 0;
-int gpu_seg_cnt_in_loop_ = 0;
+
+struct sched_attr create_sched_attr(int priority, int exec_time, int deadline, int period){
+  struct sched_attr attr;
+
+  attr.sched_priority = (__u32)priority;
+  attr.sched_runtime = (__u64)exec_time;
+  attr.sched_deadline = (__u64)deadline;
+  attr.sched_period = (__u64)period;
+
+  return attr;
+}
 
 // system call hook to call SCHED_DEADLINE
 int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags){
@@ -34,354 +29,168 @@ int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigne
 	return syscall(__NR_sched_getattr, pid, attr, size, flags);
 }
 
-bool set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period) {
-    struct sched_attr attr;
-    attr.size = sizeof(attr);
-    attr.sched_flags = 0;
-    attr.sched_nice = 0;
-    attr.sched_priority = 0;
-
-    attr.sched_policy = SCHED_DEADLINE; // 6
-    attr.sched_runtime = _exec_time;
-    attr.sched_deadline = _deadline;
-    attr.sched_period = _period;
-
-    int ret = sched_setattr(_tid, &attr, attr.sched_flags);
-    if(ret < 0) {
-        std::cerr << "[ERROR] sched_setattr failed. Are you root? (" << ret << ")" << std::endl;
-        perror("sched_setattr");
-        exit(-1);
-        return false;
-    } 
-    // else {
-    //     std::cerr << "[SCHED_DEADLINE] (" << _tid << ") exec_time: " << _exec_time << " _deadline: " << _deadline << " _period: " << _period << std::endl;
-    // }
-    return true;
-}
-
-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline){
-  sched::set_sched_deadline(gettid(), 
-    static_cast<uint64_t>(task_execution_time), 
-    static_cast<uint64_t>(task_relative_deadline), 
-    static_cast<uint64_t>(task_minimum_inter_release_time)
-  );
-}
-
-void yield_task_scheduling(){
-  sched_yield();
-}
-
-void init_gpu_scheduling(std::string task_filename, std::string gpu_deadline_filename, int key_id){
-  gpu_scheduling_flag_ = 1;
-  task_filename_ = task_filename;
-  gpu_deadline_filename_ = gpu_deadline_filename;
-  key_id_ = key_id;
-
-  // Get deadlines
-  printf("deadline filename: %s\n", gpu_deadline_filename_.c_str());
-  get_deadline_list();
-
-  // Init signal handler
-  signal(SIGINT, sig_handler);
-  signal(SIGTSTP, sig_handler);
-  signal(SIGQUIT, sig_handler);
-
-  // Create task file
-  FILE* task_fp;
-  task_fp = fopen(task_filename_.c_str(), "w");
-  if(task_fp == NULL){
-      printf("Cannot create task file at %s\n", task_filename_.c_str());
-      exit(1);
-  }
-  fprintf(task_fp, "%d\n", getpid());
-  fprintf(task_fp, "%d", key_id_);
-  fclose(task_fp);
-
-  // Get pid of scheduler
-  FILE* scheduler_fp;
-  printf("Wait the scheduler...\n");
-
-  while(1){
-      scheduler_fp = fopen("/tmp/np_edf_scheduler", "r");
-      if(scheduler_fp) break;
-  }
-  while(1){
-      fscanf(scheduler_fp, "%d", &gpu_scheduler_pid_);
-      if(gpu_scheduler_pid_ != 0) break;
+bool set_sched_fifo(int pid, int priority){
+  struct sched_param sp = {.sched_priority = (int32_t) priority};
+  int ret = sched_setscheduler(pid, SCHED_FIFO, &sp);
+  if(ret == -1){
+    perror("sched_setscheduler");
+    return false;
   }
-  printf("Scheduler pid: %d\n", gpu_scheduler_pid_);
-  fclose(scheduler_fp);
+  return true;
+}
 
+bool set_sched_fifo(int pid, int priority, int child_priority){
+  if(pid == 0) pid = getpid();
+  bool output = set_sched_fifo(pid, priority);
+  std::vector<int> child_pids = get_child_pids(pid);
 
-  // Initialize scheduling information (shared memory data)
-  FILE* sm_key_fp;
-  sm_key_fp = fopen("/tmp/sm_key", "r");
-  if(sm_key_fp == NULL){
-      printf("Cannot open /tmp/sm_key\n");
-      termination();
+  for(auto it = child_pids.begin(); it != child_pids.end(); it++){    
+    int child_pid = *it;
+    output = set_sched_fifo(child_pid, child_priority);
   }
 
-  key_t key;
-  int shmid;
-  key = ftok("/tmp/sm_key", key_id_);
-  shmid = shmget(key, sizeof(GPUSchedInfo), 0666|IPC_CREAT);
-  gpu_sched_info_ = (GPUSchedInfo*)shmat(shmid, 0, 0);
-  gpu_sched_info_->pid = getpid();
-  gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  gpu_sched_info_->scheduling_flag = 0;
-  printf("Task [%d] is ready to work\n", getpid());
-
-  gpu_seg_id_ = 0;
-  cpu_seg_id_ = 0;
-
-  return;
+  return output;
 }
 
-void sig_handler(int signum){
-  if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){
-    termination();
+bool set_sched_rr(int pid, int priority){
+  struct sched_param sp = {.sched_priority = (int32_t) priority};
+  int ret = sched_setscheduler(pid, SCHED_RR, &sp);
+  if(ret == -1){
+    perror("sched_setscheduler");
+    return false;
   }
+  return true;
 }
 
-void get_deadline_list(){
-  if(gpu_deadline_filename_.at(0) == '~'){
-    gpu_deadline_filename_.erase(0, 1);
-    std::string user_home_str(std::getenv("USER_HOME"));
-    gpu_deadline_filename_ =  user_home_str + gpu_deadline_filename_;
-  }  
+bool set_sched_rr(int pid, int priority, int child_priority){
+  if(pid == 0) pid = getpid();
+  bool output = set_sched_rr(pid, priority);
+  std::vector<int> child_pids = get_child_pids(pid);
 
-  FILE* fp;
-  fp = fopen(gpu_deadline_filename_.c_str(), "r");
-  if(fp==NULL){
-	  fprintf(stderr, "Cannot find file %s\n", gpu_deadline_filename_.c_str());
-	  exit(1);
+  for(auto it = child_pids.begin(); it != child_pids.end(); it++){    
+    int child_pid = *it;
+    output = set_sched_rr(child_pid, child_priority);
   }
 
-  char* buf;
-  int cnt = 0;
-  size_t len = 0;
-  ssize_t n;
+  return output;
+}
 
-  getline(&buf, &len, fp); // skip first line
-  while( (n = getline(&buf, &len, fp)) != -1 ){
-    cnt++;
-  }
-  max_gpu_id_ = cnt;
-  gpu_deadline_list_ = (unsigned long *)malloc(sizeof(unsigned long) * max_gpu_id_);
-  rewind(fp);
+bool set_sched_deadline(int pid, unsigned int exec_time, unsigned int deadline, unsigned int period) {
 
-  int idx = 0;
-  
-  getline(&buf, &len, fp); // skip first line
-  while((n = getline(&buf, &len, fp)) != -1){
-    unsigned long deadline;
-    sscanf(buf, "gpu_%*d,%llu", &deadline);
-    gpu_deadline_list_[idx++] = deadline;
-  }
+  struct sched_attr attr;
+  attr.size = sizeof(attr);
+  attr.sched_flags = 0;
+  attr.sched_nice = 0;
+  attr.sched_priority = 0;
 
-  // print_gpu_deadline_list();
+  attr.sched_policy = SCHED_DEADLINE; // 6
+  attr.sched_runtime = (__u64)exec_time;
+  attr.sched_deadline = (__u64)deadline;
+  attr.sched_period = (__u64)period;
 
-  free(buf); // free allocated memory at getline
-  fclose(fp);  
-  
-  return;  
+  int ret = sched_setattr(pid, &attr, attr.sched_flags);
+  if(ret < 0) {
+      std::cerr << "[ERROR] sched_setattr failed. Are you root? (" << ret << ")" << std::endl;
+      perror("sched_setattr");
+      exit(-1);
+      return false;
+  } 
+  // else {
+  //     std::cerr << "[SCHED_DEADLINE] (" << _pid << ") exec_time: " << _exec_time << " _deadline: " << _deadline << " _period: " << _period << std::endl;
+  // }
+  return true;
 }
 
-void termination(){
-  printf("TERMINATION\n");
-	if(gpu_scheduling_flag_==1){
-		gpu_sched_info_->state = SCHEDULING_STATE_STOP;
-  	shmdt(gpu_sched_info_);
-	}
-  
-  free(gpu_deadline_list_);
-  if(remove(task_filename_.c_str())){
-      printf("Cannot remove file %s\n", task_filename_);
-      exit(1);
-  }
-
-  if(task_profiling_flag_){
-    fclose(task_response_time_fp_);
+bool init_task_scheduling(std::string policy, struct sched_attr attr){
+  if(policy.compare(std::string("NONE")) == 0){
+    return true;
   }
-
-  if(gpu_profiling_flag_){
-    fclose(seg_response_time_fp_);
-    fclose(seg_execution_time_fp_);
+  else if(policy.compare(std::string("SCHED_FIFO")) == 0){
+    return set_sched_fifo(getpid(), attr.sched_priority, 99);
   }
-
-  exit(0);
-}
-
-void start_job(){
-  gpu_seg_id_ = 0;
-  cpu_seg_id_ = 0;
-  start_job_profiling();
-}
-
-void finish_job(){
-  finish_job_profiling(cpu_seg_id_);
-}
-
-void request_gpu(){
-  if(is_task_ready_ != TASK_READY) return;
-  if(was_in_loop_ == 1){
-    was_in_loop_ = 0;
-    loop_cnt_ = 0;
-    gpu_seg_cnt_in_loop_ = 0;
+  else if(policy.compare(std::string("SCHED_RR")) == 0){
+    return set_sched_rr(getpid(), attr.sched_priority, 99);
   }
-
-  stop_profiling_cpu_seg_response_time(cpu_seg_id_, 1);
-  if(gpu_scheduling_flag_==1){    
-    unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_];
-
-    if(gpu_seg_id_ > max_gpu_id_){
-      printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_.c_str());
-      printf("gpu seg id: %d / max seg id: %d\n", gpu_seg_id_);
-      relative_deadline = 1000; // 1us
-    }
-
-    gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline;
-    gpu_sched_info_->state = SCHEDULING_STATE_WAIT;
+  else if(policy.compare(std::string("SCHED_DEADLINE")) == 0){
+    return set_sched_deadline(getpid(), attr.sched_runtime, attr.sched_deadline, attr.sched_period);
   }
-
-  start_profiling_gpu_seg_response_time();
-
-  #ifdef DEBUG
-    printf("request_gpu: %d\n", gpu_seg_id_);
-  #endif
-  
-
-  if(gpu_scheduling_flag_ == 1){
-    while(1){
-      kill(gpu_scheduler_pid_, SIGUSR1);
-      if(gpu_sched_info_->scheduling_flag == 1) break;
-    }
+  else{
+    std::cout<<"[ERROR] Invalidate scheduling policy: "<<policy<<std::endl;
   }
 
-  start_profiling_gpu_seg_execution_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    gpu_sched_info_->state = SCHEDULING_STATE_RUN;
-    gpu_sched_info_->deadline = -1;
-  }
+  return true;
 }
 
-void request_gpu_in_loop(int flag){
-  if(is_task_ready_ != TASK_READY) return;
-  was_in_loop_ = 1;
+void yield_task_scheduling(){
+  sched_yield();
+}
 
-  if(flag == GPU_SEG_LOOP_START){
-      loop_cnt_++;
-    if(gpu_seg_cnt_in_loop_ == 0){
-      gpu_seg_cnt_in_loop_ = 1;
-    }
-    else{
-      gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_;
-      cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_;
-      gpu_seg_cnt_in_loop_ = 1;
-    }
+void sig_handler(int signum){
+  if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){
+    termination();
   }
+}
 
-  if(flag != GPU_SEG_LOOP_START){
-    gpu_seg_cnt_in_loop_++;
+void termination(){
+  printf("TERMINATION\n");
+  if(remove(task_filename_.c_str())){
+      printf("Cannot remove file %s\n", task_filename_);
+      exit(1);
   }
 
-  stop_profiling_cpu_seg_response_time(cpu_seg_id_, loop_cnt_);
-  if(gpu_scheduling_flag_==1){
-    unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_];
-    
-    if(gpu_seg_id_ > max_gpu_id_){
-      printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_.c_str());
-      relative_deadline = 1000; // 1us
-    }
-        
-    gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline;
-    gpu_sched_info_->state = SCHEDULING_STATE_WAIT;
-  }
+  fclose(task_response_time_fp_);
   
-  start_profiling_gpu_seg_response_time();
-
-  #ifdef DEBUG
-    printf("request_gpu: %d\n", gpu_seg_id_);
-  #endif
+  exit(0);
+}
 
-  if(gpu_scheduling_flag_ == 1){
-    while(1){
-      kill(gpu_scheduler_pid_, SIGUSR1);
-      if(gpu_sched_info_->scheduling_flag == 1) break;
+std::string get_cmd_output(const char* cmd) {
+    char buffer[128];
+    std::string result = "";
+    FILE* pipe = popen(cmd, "r");
+    if (!pipe) throw std::runtime_error("popen() failed!");
+    try {
+        while (fgets(buffer, sizeof(buffer), pipe) != NULL) {
+            result += buffer;
+        }
+    } catch (...) {
+        pclose(pipe);
+        throw;
     }
-  }
-
-  start_profiling_gpu_seg_execution_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    gpu_sched_info_->state = SCHEDULING_STATE_RUN;
-    gpu_sched_info_->deadline = -1;
-  }
+    pclose(pipe);
+    return result;
 }
 
-void yield_gpu(std::string remark){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-
-  #ifdef DEBUG
-    printf("yield_gpu: %d, %s\n", gpu_seg_id_, remark.c_str());
-  #endif
+std::vector<int> get_child_pids(int pid){
+  std::vector<int> child_pids;
+  std::string cmd = "ps -ef -T | grep " + std::to_string(pid);
+  std::string s = get_cmd_output(cmd.c_str());
 
-  stop_profiling_gpu_seg_time(gpu_seg_id_, 1, remark);
-  start_profiling_cpu_seg_response_time();
-  gpu_seg_id_++;
-  cpu_seg_id_++;
-}
+  std::vector<std::string> task_ps_info_vec = tokenize_string(s, "\n");
 
-void yield_gpu_in_loop(int flag, std::string remark){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
+  for(auto it = task_ps_info_vec.begin(); it != task_ps_info_vec.end(); ++it){
+    std::string task_ps_info = *it;
+    if(task_ps_info.find(std::string("grep")) != std::string::npos) continue;
+    if(task_ps_info.find(std::string("ps -ef -T")) != std::string::npos) continue;
+    std::vector<std::string> parsed_task_ps_info = tokenize_string(task_ps_info, " ");
+    int child_pid = std::stoi(parsed_task_ps_info[2]);
+    if(child_pid == pid) continue;
+    child_pids.push_back(child_pid);
   }
 
-  #ifdef DEBUG
-    printf("yield_gpu: %d\n", gpu_seg_id_);
-  #endif
-
-  stop_profiling_gpu_seg_time(gpu_seg_id_, loop_cnt_, remark);
-  start_profiling_cpu_seg_response_time();
-
-  gpu_seg_id_++;
-  cpu_seg_id_++;
-
-  // if(flag == GPU_SEG_LOOP_END){
-  //   gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1;
-  //   cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1;
-  // }
-}
-
-void init_task(){
-  is_task_ready_ = TASK_READY;
+  return child_pids;
 }
 
-void disable_task(){
-  is_task_ready_ = TASK_NOT_READY;
-}
+std::vector<std::string> tokenize_string(std::string s, std::string delimiter){
+  std::vector<std::string> output;
+  size_t pos = 0;
 
-void print_loop_info(std::string tag){
-  std::cout<<"tag: "<<tag<<std::endl;
-  std::cout<<"cpu_seg_id: "<<cpu_seg_id_<<std::endl;
-  std::cout<<"gpu_seg_id: "<<gpu_seg_id_<<std::endl;
-  std::cout<<"loop cnt: "<<loop_cnt_<<std::endl;
-  std::cout<<"loop seg cnt: "<<gpu_seg_cnt_in_loop_<<std::endl<<std::endl;;
-}
+  while ((pos = s.find(delimiter)) != std::string::npos) {
+      std::string token = s.substr(0, pos);
+      if(token.size() != 0) output.push_back(token);
+      s.erase(0, pos + delimiter.length());
+  }  
 
-void print_gpu_deadline_list(){
-  printf("====================================\n[GPU deadline list]\n");
-  printf("gpu_id\tdeadline\n");
-  for(int i = 0; i < max_gpu_id_; i++){
-    printf("%d\t%llu\n",i,gpu_deadline_list_[i]);
-  }
-  printf("====================================\n");
+  return output;
 }
 
-} // namespace sched
-} // namespace rubiss
\ No newline at end of file
+} // namespace rubis
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c b/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c
index 51d833bf..75882297 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c
+++ b/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c
@@ -4,22 +4,7 @@
 
 int key_id_;
 int is_scheduled_;
-int gpu_scheduling_flag_;
-GPUSchedInfo* gpu_sched_info_;
-int gpu_scheduler_pid_;
 char* task_filename_;
-char* gpu_deadline_filename_;
-// unsigned long gpu_deadline_list_[1024];
-unsigned long* gpu_deadline_list_;
-unsigned int max_gpu_id_ = 0;
-unsigned int cpu_seg_id_ = 0;
-unsigned int gpu_seg_id_ = 0;
-
-int is_task_ready_ = TASK_NOT_READY;
-int task_state_ = TASK_STATE_READY;
-int was_in_loop_ = 0;
-int loop_cnt_ = 0;
-int gpu_seg_cnt_in_loop_ = 0;
 
 // system call hook to call SCHED_DEADLINE
 int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags){
@@ -53,370 +38,25 @@ int set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _perio
     return 1;
 }
 
-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline){
-  set_sched_deadline(gettid(), 
-    (u_int64_t)(task_execution_time), 
-    (u_int64_t)(task_relative_deadline), 
-    (u_int64_t)(task_minimum_inter_release_time)
-  );
-}
-
 void yield_task_scheduling(){
   sched_yield();
 }
 
-void init_gpu_scheduling(char* task_filename, char* gpu_deadline_filename, int key_id){
-  gpu_scheduling_flag_ = 1;
-
-  task_filename_ = (char *)malloc(strlen(task_filename) * sizeof(char));
-  gpu_deadline_filename_ = (char *)malloc(strlen(gpu_deadline_filename) * sizeof(char));
-
-  strcpy(task_filename_, task_filename);
-  strcpy(gpu_deadline_filename_, gpu_deadline_filename);
-  key_id_ = key_id;
-
-  // Get deadlines
-  printf("deadline filename: %s\n", gpu_deadline_filename_);
-  get_deadline_list();
-
-  // Init signal handler
-  signal(SIGINT, sig_handler);
-  signal(SIGTSTP, sig_handler);
-  signal(SIGQUIT, sig_handler);
-
-  // Create task file
-  FILE* task_fp;
-  task_fp = fopen(task_filename_, "w");
-  if(task_fp == NULL){
-      printf("Cannot create task file at %s\n", task_filename_);
-      exit(1);
-  }
-  fprintf(task_fp, "%d\n", getpid());
-  fprintf(task_fp, "%d", key_id_);
-  fclose(task_fp);
-
-  // Get pid of scheduler
-  FILE* scheduler_fp;
-  printf("Wait the scheduler...\n");
-
-  while(1){
-      scheduler_fp = fopen("/tmp/np_edf_scheduler", "r");
-      if(scheduler_fp) break;
-  }
-  while(1){
-      fscanf(scheduler_fp, "%d", &gpu_scheduler_pid_);
-      if(gpu_scheduler_pid_ != 0) break;
-  }
-  printf("Scheduler pid: %d\n", gpu_scheduler_pid_);
-  fclose(scheduler_fp);
-
-
-  // Initialize scheduling information (shared memory data)
-  FILE* sm_key_fp;
-  sm_key_fp = fopen("/tmp/sm_key", "r");
-  if(sm_key_fp == NULL){
-      printf("Cannot open /tmp/sm_key\n");
-      termination();
-  }
-
-  key_t key;
-  int shmid;
-  key = ftok("/tmp/sm_key", key_id_);
-  shmid = shmget(key, sizeof(GPUSchedInfo), 0666|IPC_CREAT);
-  gpu_sched_info_ = (GPUSchedInfo*)shmat(shmid, 0, 0);
-  gpu_sched_info_->pid = getpid();
-  gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  gpu_sched_info_->scheduling_flag = 0;
-  printf("Task [%d] is ready to work\n", getpid());
-
-  gpu_seg_id_ = 0;
-  cpu_seg_id_ = 0;
-  
-  return;
-}
-
 void sig_handler(int signum){
   if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){
     termination();
   }
 }
 
-void get_deadline_list(){
-  char gpu_deadline_filename[rubis_lib_BUFFER_SIZE];
-  char* user_name = getenv("USER_HOME");
-
-  if(gpu_deadline_filename_[0] != '~'){
-    strcpy(gpu_deadline_filename, gpu_deadline_filename_);
-  }
-  else{
-    strcpy(gpu_deadline_filename, user_name);
-    strcat(gpu_deadline_filename, &gpu_deadline_filename_[1]);
-  }  
-
-  FILE* fp;
-  fp = fopen(gpu_deadline_filename, "r");
-  if(fp==NULL){
-	  fprintf(stderr, "Cannot find file %s\n", gpu_deadline_filename);
-	  exit(1);
-  }
-
-  char* buf;
-  int cnt = 0;
-  size_t len = 0;
-  ssize_t n;
-
-  getline(&buf, &len, fp); // skip first line
-  while( (n = getline(&buf, &len, fp)) != -1 ){
-    cnt++;
-  }
-  max_gpu_id_ = cnt;
-  gpu_deadline_list_ = (unsigned long *)malloc(sizeof(unsigned long) * max_gpu_id_);
-  rewind(fp);
-
-  int idx = 0;
-  
-  getline(&buf, &len, fp); // skip first line
-  while((n = getline(&buf, &len, fp)) != -1){
-    unsigned long deadline;
-    sscanf(buf, "gpu_%*d,%llu", &deadline);
-    gpu_deadline_list_[idx++] = deadline;
-  }
-
-  free(buf); // free allocated memory at getline
-  fclose(fp);  
-
-  // print_gpu_deadline_list();
-
-  return;  
-}
-
 void termination(){
   printf("TERMINATION\n");
-	if(gpu_scheduling_flag_==1){
-		gpu_sched_info_->state = SCHEDULING_STATE_STOP;
-  	shmdt(gpu_sched_info_);
-	}
   
-  free(gpu_deadline_list_);
   if(remove(task_filename_)){
       printf("Cannot remove file %s\n", task_filename_);
       exit(1);
   }
 
-  if(task_profiling_flag_){
-    fclose(task_response_time_fp_);
-  }
-
-  if(gpu_profiling_flag_){
-    fclose(seg_response_time_fp_);
-    fclose(seg_execution_time_fp_);
-    free(task_filename_);
-    free(gpu_deadline_filename_);
-  }
+  fclose(task_response_time_fp_);
 
   exit(0);
-}
-
-void start_job(){
-  gpu_seg_id_ = 0;
-  cpu_seg_id_ = 0;
-  start_job_profiling();
-}
-
-void finish_job(){
-  finish_job_profiling(cpu_seg_id_);
-}
-
-void request_gpu(){  
-  if(is_task_ready_ != TASK_READY) return;
-  if(was_in_loop_ == 1){
-    was_in_loop_ = 0;
-    loop_cnt_ = 0;
-    gpu_seg_cnt_in_loop_ = 0;
-  }
-
-  stop_profiling_cpu_seg_response_time(cpu_seg_id_, 1);
-  if(gpu_scheduling_flag_==1){
-    unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_];
-
-    if(gpu_seg_id_ > max_gpu_id_){
-      printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_);
-      printf("gpu seg id: %d / max seg id: %d\n", gpu_seg_id_, max_gpu_id_);
-      relative_deadline = 1000; // 1us
-    }
-
-    gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline;
-    gpu_sched_info_->state = SCHEDULING_STATE_WAIT;
-  }
-  
-  start_profiling_gpu_seg_response_time();
-
-  #ifdef DEBUG
-    printf("request_gpu: %d\n", gpu_seg_id_);
-  #endif
-
-  if(gpu_scheduling_flag_ == 1){
-    while(1){
-      kill(gpu_scheduler_pid_, SIGUSR1);
-      if(gpu_sched_info_->scheduling_flag == 1) break;
-    }
-  }
-
-  start_profiling_gpu_seg_execution_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    gpu_sched_info_->state = SCHEDULING_STATE_RUN;
-    gpu_sched_info_->deadline = -1;
-  }
-}
-
-void request_gpu_in_loop(int flag){
-  if(is_task_ready_ != TASK_READY) return;
-  was_in_loop_ = 1;
-
-  if(flag == GPU_SEG_LOOP_START){
-      loop_cnt_++;
-    if(gpu_seg_cnt_in_loop_ == 0){
-      gpu_seg_cnt_in_loop_ = 1;
-    }
-    else{
-      gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_;
-      cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_;
-      gpu_seg_cnt_in_loop_ = 1;
-    }
-  }
-
-  if(flag != GPU_SEG_LOOP_START){
-    gpu_seg_cnt_in_loop_++;
-  }
-
-  stop_profiling_cpu_seg_response_time(cpu_seg_id_, loop_cnt_);
-  if(gpu_scheduling_flag_==1){    
-    unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_];
-
-    if(gpu_seg_id_ > max_gpu_id_){
-      printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_);
-      relative_deadline = 1000; // 1us
-    }
-
-    gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline;
-    gpu_sched_info_->state = SCHEDULING_STATE_WAIT;
-  }
-  
-  start_profiling_gpu_seg_response_time();
-
-  #ifdef DEBUG
-    printf("request_gpu: %d\n", gpu_seg_id_);
-  #endif
-
-  if(gpu_scheduling_flag_ == 1){
-    while(1){
-      kill(gpu_scheduler_pid_, SIGUSR1);
-      if(gpu_sched_info_->scheduling_flag == 1) break;
-    }
-  }
-
-  start_profiling_gpu_seg_execution_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    gpu_sched_info_->state = SCHEDULING_STATE_RUN;
-    gpu_sched_info_->deadline = -1;
-  }
-}
-
-void yield_gpu(){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-
-  #ifdef DEBUG
-    printf("yield_gpu: %d\n", gpu_seg_id_);
-  #endif
-
-  stop_profiling_gpu_seg_time(gpu_seg_id_, 1);
-  start_profiling_cpu_seg_response_time(1);
-
-  gpu_seg_id_++;
-  cpu_seg_id_++;
-}
-
-void yield_gpu_with_remark(const char* remark){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-
-  #ifdef DEBUG
-    printf("yield_gpu: %d\n", gpu_seg_id_);
-  #endif
-
-  stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, 1, remark);
-  start_profiling_cpu_seg_response_time();
-
-  gpu_seg_id_++;
-  cpu_seg_id_++;
-}
-
-void yield_gpu_in_loop(int flag){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-
-  #ifdef DEBUG
-    printf("yield_gpu: %d\n", gpu_seg_id_);
-  #endif
-
-  stop_profiling_gpu_seg_time(gpu_seg_id_, loop_cnt_);
-  start_profiling_cpu_seg_response_time();
-
-  if(flag == GPU_SEG_LOOP_END){
-    gpu_seg_id_ -= gpu_seg_cnt_in_loop_;
-    cpu_seg_id_ -= gpu_seg_cnt_in_loop_;
-  }
-}
-
-void yield_gpu_with_remark_in_loop(int flag, const char* remark){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-
-  #ifdef DEBUG
-    printf("yield_gpu: %d\n", gpu_seg_id_);
-  #endif
-
-  stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, loop_cnt_, remark);
-  start_profiling_cpu_seg_response_time();
-
-  // if(flag == GPU_SEG_LOOP_END){
-  //   gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1;
-  //   cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1;
-  // }
-}
-
-void init_task(){
-  is_task_ready_ = TASK_READY;
-}
-
-void disable_task(){
-  is_task_ready_ = TASK_NOT_READY;
-}
-
-void print_loop_info(const char* tag){
-  printf("tag: %s\n",tag);
-  printf("cpu_seg_id: %d\n",cpu_seg_id_);
-  printf("gpu_seg_id: %d\n",gpu_seg_id_);
-  printf("loop cnt: %d\n",loop_cnt_);
-  printf("loop seg cnt: %d\n",gpu_seg_cnt_in_loop_);
-}
-
-void print_gpu_deadline_list(){
-  printf("====================================\n[GPU deadline list]\n");
-  printf("gpu_id\tdeadline\n");
-  for(int i = 0; i < max_gpu_id_; i++){
-    printf("%d\t%lu\n",i,gpu_deadline_list_[i]);
-  }
-  printf("====================================\n");
-}
+}
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c.backup b/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c.backup
deleted file mode 100644
index 7f5bfb1d..00000000
--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c.backup
+++ /dev/null
@@ -1,370 +0,0 @@
-#include "rubis_lib/sched_c.h"
-
-int key_id_;
-int is_scheduled_;
-int gpu_scheduling_flag_;
-GPUSchedInfo* gpu_sched_info_;
-int gpu_scheduler_pid_;
-char* task_filename_;
-char* gpu_deadline_filename_;
-// unsigned long long gpu_deadline_list_[1024];
-unsigned long long* gpu_deadline_list_;
-unsigned int max_gpu_id_ = TASK_NOT_READY;
-unsigned int cpu_seg_id_ = 0;
-unsigned int gpu_seg_id_ = 0;
-
-int task_state_ = TASK_STATE_READY;
-int is_task_ready_ = 0;
-int was_in_loop_ = 0;
-int loop_cnt_ = 0;
-int gpu_seg_cnt_in_loop_ = 0;
-
-
-// system call hook to call SCHED_DEADLINE
-int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags){
-	return syscall(__NR_sched_setattr, pid, attr, flags);
-}
-
-int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigned int flags)
-{
-	return syscall(__NR_sched_getattr, pid, attr, size, flags);
-}
-
-int set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period) {
-    struct sched_attr attr;
-    attr.size = sizeof(attr);
-    attr.sched_flags = 0;
-    attr.sched_nice = 0;
-    attr.sched_priority = 0;
-
-    attr.sched_policy = SCHED_DEADLINE; // 6
-    attr.sched_runtime = _exec_time;
-    attr.sched_deadline = _deadline;
-    attr.sched_period = _period;
-
-    int ret = sched_setattr(_tid, &attr, attr.sched_flags);
-    if(ret < 0) {
-        printf("[ERROR] sched_setattr failed. Are you root? (%d)\n", ret);
-        perror("sched_setattr");
-        exit(-1);
-        return 0;
-    } 
-    return 1;
-}
-
-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline){
-  set_sched_deadline(gettid(), 
-    (u_int64_t)(task_execution_time), 
-    (u_int64_t)(task_relative_deadline), 
-    (u_int64_t)(task_minimum_inter_release_time)
-  );
-}
-
-void yield_task_scheduling(){
-  sched_yield();
-}
-
-void init_gpu_scheduling(char* task_filename, char* gpu_deadline_filename, int key_id){
-  gpu_scheduling_flag_ = 1;
-
-  task_filename_ = (char *)malloc(strlen(task_filename) * sizeof(char));
-  gpu_deadline_filename_ = (char *)malloc(strlen(gpu_deadline_filename) * sizeof(char));
-
-  strcpy(task_filename_, task_filename);
-  strcpy(gpu_deadline_filename_, gpu_deadline_filename);
-  key_id_ = key_id;
-
-  // Get deadlines
-  printf("deadline filename: %s\n", gpu_deadline_filename_);
-  get_deadline_list();
-
-  // Init signal handler
-  signal(SIGINT, sig_handler);
-  signal(SIGTSTP, sig_handler);
-  signal(SIGQUIT, sig_handler);
-
-  // Create task file
-  FILE* task_fp;
-  task_fp = fopen(task_filename_, "w");
-  if(task_fp == NULL){
-      printf("Cannot create task file at %s\n", task_filename_);
-      exit(1);
-  }
-  fprintf(task_fp, "%d\n", getpid());
-  fprintf(task_fp, "%d", key_id_);
-  fclose(task_fp);
-
-  // Get pid of scheduler
-  FILE* scheduler_fp;
-  printf("Wait the scheduler...\n");
-
-  while(1){
-      scheduler_fp = fopen("/tmp/np_edf_scheduler", "r");
-      if(scheduler_fp) break;
-  }
-  while(1){
-      fscanf(scheduler_fp, "%d", &gpu_scheduler_pid_);
-      if(gpu_scheduler_pid_ != 0) break;
-  }
-  printf("Scheduler pid: %d\n", gpu_scheduler_pid_);
-  fclose(scheduler_fp);
-
-
-  // Initialize scheduling information (shared memory data)
-  FILE* sm_key_fp;
-  sm_key_fp = fopen("/tmp/sm_key", "r");
-  if(sm_key_fp == NULL){
-      printf("Cannot open /tmp/sm_key\n");
-      termination();
-  }
-
-  key_t key;
-  int shmid;
-  key = ftok("/tmp/sm_key", key_id_);
-  shmid = shmget(key, sizeof(GPUSchedInfo), 0666|IPC_CREAT);
-  gpu_sched_info_ = (GPUSchedInfo*)shmat(shmid, 0, 0);
-  gpu_sched_info_->pid = getpid();
-  gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  gpu_sched_info_->scheduling_flag = 0;
-  printf("Task [%d] is ready to work\n", getpid());
-
-  gpu_seg_id_ = 0;
-  cpu_seg_id_ = 0;
-  
-  return;
-}
-
-void sig_handler(int signum){
-  if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){
-    termination();
-  }
-}
-
-void get_deadline_list(){
-  char gpu_deadline_filename[rubis_lib_BUFFER_SIZE];
-  char* user_name = getenv("USER_HOME");
-
-  if(gpu_deadline_filename_[0] != '~'){
-    strcpy(gpu_deadline_filename, gpu_deadline_filename_);
-  }
-  else{
-    strcpy(gpu_deadline_filename, user_name);
-    strcat(gpu_deadline_filename, &gpu_deadline_filename_[1]);
-  }  
-
-  FILE* fp;
-  fp = fopen(gpu_deadline_filename, "r");
-  if(fp==NULL){
-	  fprintf(stderr, "Cannot find file %s\n", gpu_deadline_filename);
-	  exit(1);
-  }
-  char buf[1024];
-  
-  while(1){
-    int id;
-    if(!fgets(buf, 1024, fp)) break;
-    strtok(buf, "\n");
-    sscanf(buf, "%d, %*llu", &id);
-    if(gpu_seg_id_ > max_gpu_id_) max_gpu_id_ = id;
-  }
-
-  gpu_deadline_list_ = (unsigned long long *)malloc(sizeof(unsigned long long) * (max_gpu_id_+1));
-  printf("file read is finished\n");
-
-  rewind(fp);
-  while(1){    
-    int id;
-    long long int deadline;
-    if(!fgets(buf, 1024, fp)) break;
-    sscanf(buf, "%d, %llu", &id, &deadline);
-    gpu_deadline_list_[id] = deadline;
-  }
-  fclose(fp);
-
-  return;  
-}
-
-void termination(){
-  printf("TERMINATION\n");
-	if(gpu_scheduling_flag_==1){
-		gpu_sched_info_->state = SCHEDULING_STATE_STOP;
-  	shmdt(gpu_sched_info_);
-	}
-  
-  free(gpu_deadline_list_);
-  if(remove(task_filename_)){
-      printf("Cannot remove file %s\n", task_filename_);
-      exit(1);
-  }
-
-  if(task_profiling_flag_){
-    fclose(task_response_time_fp_);
-  }
-
-  if(gpu_profiling_flag_){
-    fclose(seg_response_time_fp_);
-    fclose(seg_execution_time_fp_);
-    free(task_filename_);
-    free(gpu_deadline_filename_);
-  }
-
-  exit(0);
-}
-
-void start_job(){
-  gpu_seg_id_ = 0;
-  cpu_seg_id_ = 0;
-  start_job_profiling();
-}
-
-void finish_job(){
-  finish_job_profiling(cpu_seg_id_);
-}
-
-void request_gpu(){  
-  if(was_in_loop_ == 1){
-    was_in_loop_ = 0;
-    loop_cnt_ = 0;
-    gpu_seg_cnt_in_loop_ = 0;
-  }
-
-  stop_profiling_cpu_seg_response_time(cpu_seg_id_, 1);
-  if(gpu_scheduling_flag_==1){
-    if(gpu_seg_id_ > max_gpu_id_){
-      printf("[ERROR] GPU segment id bigger than max segment id!\n");
-      exit(1);
-    }
-    
-    unsigned long long relative_deadline = gpu_deadline_list_[gpu_seg_id_];
-    gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline;
-    gpu_sched_info_->state = SCHEDULING_STATE_WAIT;
-  }
-  
-  start_profiling_gpu_seg_response_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    while(1){
-      kill(gpu_scheduler_pid_, SIGUSR1);
-      if(gpu_sched_info_->scheduling_flag == 1) break;
-    }
-  }
-
-  start_profiling_gpu_seg_execution_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    gpu_sched_info_->state = SCHEDULING_STATE_RUN;
-    gpu_sched_info_->deadline = -1;
-  }
-}
-
-void request_gpu_in_loop(int flag){
-  was_in_loop_ = 1;
-
-  if(flag == GPU_SEG_LOOP_START){
-      loop_cnt_++;
-    if(gpu_seg_cnt_in_loop_ == 0){
-      gpu_seg_cnt_in_loop_ = 1;
-    }
-    else{
-      gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_;
-      cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_;
-      gpu_seg_cnt_in_loop_ = 1;
-    }
-  }
-
-  if(flag != GPU_SEG_LOOP_START){
-    gpu_seg_cnt_in_loop_++;
-  }
-
-  stop_profiling_cpu_seg_response_time(cpu_seg_id_, loop_cnt_);
-  if(gpu_scheduling_flag_==1){
-    if(gpu_seg_id_ > max_gpu_id_){
-      printf("[ERROR] GPU segment id bigger than max segment id!\n");
-      exit(1);
-    }
-    
-    unsigned long long relative_deadline = gpu_deadline_list_[gpu_seg_id_];
-    gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline;
-    gpu_sched_info_->state = SCHEDULING_STATE_WAIT;
-  }
-  
-  start_profiling_gpu_seg_response_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    while(1){
-      kill(gpu_scheduler_pid_, SIGUSR1);
-      if(gpu_sched_info_->scheduling_flag == 1) break;
-    }
-  }
-
-  start_profiling_gpu_seg_execution_time();
-
-  if(gpu_scheduling_flag_ == 1){
-    gpu_sched_info_->state = SCHEDULING_STATE_RUN;
-    gpu_sched_info_->deadline = -1;
-  }
-}
-
-void yield_gpu(){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-  stop_profiling_gpu_seg_time(gpu_seg_id_, 1);
-  start_profiling_cpu_seg_response_time(1);
-
-  gpu_seg_id_++;
-  cpu_seg_id_++;
-}
-
-void yield_gpu_with_remark(const char* remark){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-  stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, 1, remark);
-  start_profiling_cpu_seg_response_time();
-
-  gpu_seg_id_++;
-  cpu_seg_id_++;
-}
-
-void yield_gpu_in_loop(int flag){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-  stop_profiling_gpu_seg_time(gpu_seg_id_, loop_cnt_);
-  start_profiling_cpu_seg_response_time();
-
-  if(flag == GPU_SEG_LOOP_END){
-    gpu_seg_id_ -= gpu_seg_cnt_in_loop_;
-    cpu_seg_id_ -= gpu_seg_cnt_in_loop_;
-  }
-}
-
-void yield_gpu_with_remark_in_loop(int flag, const char* remark){
-  if(gpu_scheduling_flag_==1){
-    gpu_sched_info_->scheduling_flag = 0;
-    gpu_sched_info_->state = SCHEDULING_STATE_NONE;
-  }
-  stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, loop_cnt_, remark);
-  start_profiling_cpu_seg_response_time();
-
-  // if(flag == GPU_SEG_LOOP_END){
-  //   gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1;
-  //   cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1;
-  // }
-}
-
-void init_task(){
-  is_task_ready_ = TASK_READY;
-}
-
-void print_loop_info(const char* tag){
-  printf("tag: %s\n",tag);
-  printf("cpu_seg_id: %d\n",cpu_seg_id_);
-  printf("gpu_seg_id: %d\n",gpu_seg_id_);
-  printf("loop cnt: %d\n",loop_cnt_);
-  printf("loop seg cnt: %d\n",gpu_seg_cnt_in_loop_);
-}
diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp b/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp
index 0be0a0b6..f4a07758 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp
+++ b/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp
@@ -1,148 +1,69 @@
 #include "rubis_lib/sched_profiling.hpp"
 
-namespace rubis{
-  int instance_mode_;
-  unsigned long instance_;
-namespace sched{
-  int task_profiling_flag_;
-  int gpu_profiling_flag_;
-  int iter_;
+namespace rubis {
+unsigned long instance_;
+unsigned long lidar_instance_;
+unsigned long vision_instance_;
 
-  FILE* task_response_time_fp_;
-  FILE* seg_execution_time_fp_;
-  FILE* seg_response_time_fp_;
+int iter_;
 
-  int is_gpu_profiling_started_;
+FILE *task_response_time_fp_;
 
-  struct timespec task_start_time_;
-  struct timespec task_end_time_;
-  unsigned long gpu_seg_response_time_;
-  unsigned long gpu_seg_execution_time_;
-  unsigned long cpu_seg_response_time_;
-  int is_gpu_profiling_ready_ = 0;
+struct timespec task_start_time_;
+struct timespec task_end_time_;
+struct timespec topic_pub_time_;
 
-  void start_gpu_profiling(){
-    is_gpu_profiling_ready_ = 1;
-  }
-
-  void init_task_profiling(std::string task_reponse_time_filename){
-    if(task_reponse_time_filename.at(0) == '~'){
-      task_reponse_time_filename.erase(0, 1);
-      std::string user_home_str(std::getenv("USER_HOME"));
-      task_reponse_time_filename =  user_home_str + task_reponse_time_filename;
+void init_task_profiling(std::string task_reponse_time_filename) {
+    if (task_reponse_time_filename.at(0) == '~') {
+        task_reponse_time_filename.erase(0, 1);
+        std::string user_home_str(std::getenv("USER_HOME"));
+        task_reponse_time_filename = user_home_str + task_reponse_time_filename;
     }
 
-    task_profiling_flag_ = 1;
     task_response_time_fp_ = fopen(task_reponse_time_filename.c_str(), "w+");
-    if(task_response_time_fp_ == NULL){
-      std::cout<<"Cannot create/open file: "<<task_reponse_time_filename<<std::endl;
-      perror("Failed: ");
-      exit(0);
+    if (task_response_time_fp_ == NULL) {
+        std::cout << "Cannot create/open file: " << task_reponse_time_filename
+                  << std::endl;
+        perror("Failed: ");
+        exit(0);
     }
 
     chmod(task_reponse_time_filename.c_str(), strtol("0777", 0, 8));
-    fprintf(task_response_time_fp_, "iter,PID,start,end,instance,activation\n");
-  }
-
-  void start_task_profiling(){
-    if(task_profiling_flag_) clock_gettime(CLOCK_MONOTONIC, &task_start_time_);
-  }
-
-  void stop_task_profiling(unsigned long instance, int state){
-    if(task_profiling_flag_){
-      int activation = 0;
-      if(state == TASK_STATE_DONE) activation = 1;
-      clock_gettime(CLOCK_MONOTONIC, &task_end_time_);
-      fprintf(task_response_time_fp_, "%d,%d,%lld.%.9ld,%lld.%.9ld,%lu,%d\n",iter_++, getpid(), (long long)task_start_time_.tv_sec, task_start_time_.tv_nsec, (long long)task_end_time_.tv_sec, task_end_time_.tv_nsec, instance, activation);
-      fflush(task_response_time_fp_);
-    }
-  }
-
-  void init_gpu_profiling(std::string execution_time_filename, std::string response_time_filename){
-    if(execution_time_filename.at(0) == '~'){
-      execution_time_filename.erase(0, 1);
-      std::string user_home_str(std::getenv("USER_HOME"));
-      execution_time_filename =  user_home_str + execution_time_filename;
-    }
-
-    if(response_time_filename.at(0) == '~'){
-      response_time_filename.erase(0, 1);
-      std::string user_home_str(std::getenv("USER_HOME"));
-      response_time_filename =  user_home_str + response_time_filename;
-    }
-
-    gpu_profiling_flag_ = 1;
-    seg_execution_time_fp_ = fopen(execution_time_filename.c_str(), "w+");
-    chmod(execution_time_filename.c_str(), strtol("0777", 0, 8));
-    fprintf(seg_execution_time_fp_, "ID,TIME,ITERATION, REMARK\n");
-
-    seg_response_time_fp_ = fopen(response_time_filename.c_str(), "w+");
-    chmod(response_time_filename.c_str(), strtol("0777", 0, 8));
-    fprintf(seg_response_time_fp_, "ID,TIME,ITERATION, REMARK\n");
-  }
-
-  void start_profiling_cpu_seg_response_time(){
-    if(gpu_profiling_flag_)
-      cpu_seg_response_time_ = get_current_time_ns();
-  }
-
-  void stop_profiling_cpu_seg_response_time(unsigned int cpu_seg_id, unsigned int iter){
-    if(gpu_profiling_flag_){
-      cpu_seg_response_time_ = get_current_time_ns() - cpu_seg_response_time_;
-      fprintf(seg_response_time_fp_, "cpu_%u,%llu,%u\n", cpu_seg_id, cpu_seg_response_time_, iter);
-    }    
-  }
-
-  void start_profiling_gpu_seg_response_time(){
-    if(gpu_profiling_flag_) gpu_seg_response_time_ = get_current_time_ns();
-  }
-
-  void start_profiling_gpu_seg_execution_time(){
-    if(gpu_profiling_flag_) gpu_seg_execution_time_ = get_current_time_ns();
-  }
-
-  void stop_profiling_gpu_seg_time(unsigned int gpu_seg_id, unsigned int iter, std::string remark){
-    if(gpu_profiling_flag_){
-      unsigned long current_time = get_current_time_ns();
-      gpu_seg_response_time_ = current_time - gpu_seg_response_time_;
-      gpu_seg_execution_time_ = current_time - gpu_seg_execution_time_;
-      
-      fprintf(seg_response_time_fp_, "gpu_%u,%llu,%u,%s\n", gpu_seg_id, gpu_seg_response_time_, iter, remark.c_str());
-      fprintf(seg_execution_time_fp_, "gpu_%u,%llu,%u,%s\n", gpu_seg_id, gpu_seg_execution_time_, iter, remark.c_str());
-    }
-  }
-
-  unsigned long get_current_time_ns(){
+    fprintf(task_response_time_fp_,
+            "iter,PID,start,end,instance,lidar_instance,topic_pub_time,vision_"
+            "instasnce\n");
+}
+
+void start_task_profiling() {
+    clock_gettime(CLOCK_REALTIME, &task_start_time_);
+}
+
+void start_task_profiling_at_initial_node(long long tp_time_sec,
+                                          long long tp_time_nsec) {
+    clock_gettime(CLOCK_REALTIME, &task_start_time_);
+    topic_pub_time_.tv_sec = tp_time_sec;
+    topic_pub_time_.tv_nsec = tp_time_nsec;
+}
+
+void stop_task_profiling(unsigned long instance, unsigned long lidar_instance_,
+                         unsigned long vision_instance_) {
+    clock_gettime(CLOCK_REALTIME, &task_end_time_);
+    fprintf(task_response_time_fp_,
+            "%d,%d,%lld.%.9ld,%lld.%.9ld,%lu,%lu,%lld.%.9ld,%lu\n", iter_++,
+            getpid(), (long long)task_start_time_.tv_sec,
+            task_start_time_.tv_nsec, (long long)task_end_time_.tv_sec,
+            task_end_time_.tv_nsec, instance, lidar_instance_,
+            (long long)topic_pub_time_.tv_sec, topic_pub_time_.tv_nsec,
+            vision_instance_);
+    fflush(task_response_time_fp_);
+}
+
+unsigned long get_current_time_ns() {
     struct timespec ts;
     unsigned long current_time;
     clock_gettime(CLOCK_REALTIME, &ts);
-    current_time = ts.tv_sec%10000 * 1000000000 + ts.tv_nsec;
+    current_time = ts.tv_sec % 10000 * 1000000000 + ts.tv_nsec;
     return current_time;
-  }
-
-  void start_job_profiling(){
-    if(!gpu_profiling_flag_) return;
-    
-    if(is_gpu_profiling_started_ == 0) is_gpu_profiling_started_ = 1;
-    else if( (is_gpu_profiling_started_ == 1) && (is_gpu_profiling_ready_ == 1) ){
-      start_profiling_cpu_seg_response_time();
-    }    
-  }
-
-  void finish_job_profiling(unsigned int cpu_seg_id){
-    if(!gpu_profiling_flag_) return;
-    
-    if(is_gpu_profiling_started_ == 0) is_gpu_profiling_started_ = 1;
-    else if( (is_gpu_profiling_started_ == 1) && (is_gpu_profiling_ready_ == 1) ){
-      stop_profiling_cpu_seg_response_time(cpu_seg_id, 1);
-      fprintf(seg_response_time_fp_, "-1, -1, -1\n");
-      fprintf(seg_execution_time_fp_, "-1, -1, -1\n");
-      
-      fflush(seg_response_time_fp_);
-      fflush(seg_execution_time_fp_);
-    }    
-  }
-
+}
 
-}// end namespace
-}
\ No newline at end of file
+} // namespace rubis
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling_c.c b/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling_c.c
index 0b1ba6e3..d2d9325f 100644
--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling_c.c
+++ b/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling_c.c
@@ -1,168 +1,58 @@
 #include "rubis_lib/sched_profiling_c.h"ß
 
-int instance_mode_;
 unsigned long instance_;
+unsigned long lidar_instance_;
+unsigned long vision_instance_;
 
-int task_profiling_flag_;
-int gpu_profiling_flag_;
 int iter_;
 
-FILE* task_response_time_fp_;
-FILE* seg_execution_time_fp_;
-FILE* seg_response_time_fp_;
-
-int is_gpu_profiling_started_;
+FILE *task_response_time_fp_;
 
 struct timespec task_start_time_;
 struct timespec task_end_time_;
-unsigned long gpu_seg_response_time_;
-unsigned long gpu_seg_execution_time_;
-unsigned long cpu_seg_response_time_;
-int is_gpu_profiling_ready_ = 0;
-
-void start_gpu_profiling(){
-  is_gpu_profiling_ready_ = 1;
-}
 
-void init_task_profiling(char* _task_response_time_filename){
-  char task_response_time_filename[rubis_lib_BUFFER_SIZE];
-  char* user_name = getenv("USER_HOME");
+void init_task_profiling(char *_task_response_time_filename) {
+    char task_response_time_filename[rubis_lib_BUFFER_SIZE];
+    char *user_name = getenv("USER_HOME");
 
-  if(_task_response_time_filename[0] != '~'){
-    strcpy(task_response_time_filename, _task_response_time_filename);
-  }
-  else{      
-    strcpy(task_response_time_filename, user_name);
-    strcat(task_response_time_filename, &_task_response_time_filename[1]);
-  }
+    if (_task_response_time_filename[0] != '~') {
+        strcpy(task_response_time_filename, _task_response_time_filename);
+    } else {
+        strcpy(task_response_time_filename, user_name);
+        strcat(task_response_time_filename, &_task_response_time_filename[1]);
+    }
 
-  task_profiling_flag_ = 1;
-  task_response_time_fp_ = fopen(task_response_time_filename, "w+");
-  if(task_response_time_fp_ == NULL){
-      printf("Cannot create/open file: %s\n", task_response_time_filename);
-      perror("Failed: ");
-      exit(0);
-  }
+    task_response_time_fp_ = fopen(task_response_time_filename, "w+");
+    if (task_response_time_fp_ == NULL) {
+        printf("Cannot create/open file: %s\n", task_response_time_filename);
+        perror("Failed: ");
+        exit(0);
+    }
 
-  chmod(task_response_time_filename, strtol("0777", 0, 8));
-  fprintf(task_response_time_fp_, "iter,PID,start,end,instance,activation\n");
+    chmod(task_response_time_filename, strtol("0777", 0, 8));
+    fprintf(task_response_time_fp_,
+            "iter,PID,start,end,instance,lidar_instance,vision_instance\n");
 }
 
-void start_task_profiling(){
-  if(task_profiling_flag_) clock_gettime(CLOCK_MONOTONIC, &task_start_time_);
+void start_task_profiling() {
+    clock_gettime(CLOCK_MONOTONIC, &task_start_time_);
 }
 
-void stop_task_profiling(unsigned long instance, int state){
-  if(task_profiling_flag_){
-    int activation = 0;
-    if(state == TASK_STATE_DONE) activation = 1;
+void stop_task_profiling(unsigned long instance, unsigned long lidar_instance_,
+                         unsigned long vision_instance_) {
     clock_gettime(CLOCK_MONOTONIC, &task_end_time_);
-    fprintf(task_response_time_fp_, "%d,%d,%lld.%.9ld,%lld.%.9ld,%lu,%d\n",iter_++, getpid(), (long long)task_start_time_.tv_sec, task_start_time_.tv_nsec, (long long)task_end_time_.tv_sec, task_end_time_.tv_nsec, instance_, activation);
+    fprintf(task_response_time_fp_, "%d,%d,%lld.%.9ld,%lld.%.9ld,%lu,%lu,%lu\n",
+            iter_++, getpid(), (long long)task_start_time_.tv_sec,
+            task_start_time_.tv_nsec, (long long)task_end_time_.tv_sec,
+            task_end_time_.tv_nsec, instance, lidar_instance_,
+            vision_instance_);
     fflush(task_response_time_fp_);
-  }
-}
-
-void init_gpu_profiling(char* _execution_time_filename, char* _response_time_filename){    
-  char execution_time_filename[rubis_lib_BUFFER_SIZE];    
-  char response_time_filename[rubis_lib_BUFFER_SIZE];    
-  char* user_name = getenv("USER_HOME");
-
-
-  if(_execution_time_filename[0] != '~'){
-    strcpy(execution_time_filename, _execution_time_filename);
-  }
-  else{      
-    strcpy(execution_time_filename, user_name);
-    strcat(execution_time_filename, &_execution_time_filename[1]);
-  }
-
-  if(_response_time_filename[0] != '~'){
-    strcpy(response_time_filename, _response_time_filename);
-  }
-  else{      
-    strcpy(response_time_filename, user_name);
-    strcat(response_time_filename, &_response_time_filename[1]);
-  }
-
-  gpu_profiling_flag_ = 1;
-  seg_execution_time_fp_ = fopen(execution_time_filename, "w+");
-  chmod(execution_time_filename, strtol("0777", 0, 8));
-  fprintf(seg_execution_time_fp_, "ID,TIME,ITERATION,REMARK\n");
-
-  seg_response_time_fp_ = fopen(response_time_filename, "w+");
-  chmod(response_time_filename, strtol("0777", 0, 8));
-  fprintf(seg_response_time_fp_, "ID,TIME,ITERATION,REMARK\n");
-}
-
-void start_profiling_cpu_seg_response_time(){
-  if(gpu_profiling_flag_)
-    cpu_seg_response_time_ = get_current_time_ns();
-}
-
-void stop_profiling_cpu_seg_response_time(unsigned int cpu_seg_id, unsigned int iter){
-  if(gpu_profiling_flag_){
-    cpu_seg_response_time_ = get_current_time_ns() - cpu_seg_response_time_;
-    fprintf(seg_response_time_fp_, "cpu_%u,%llu,%u\n", cpu_seg_id, cpu_seg_response_time_, iter);
-  }    
-}
-
-void start_profiling_gpu_seg_response_time(){
-  if(gpu_profiling_flag_) gpu_seg_response_time_ = get_current_time_ns();
-}
-
-void start_profiling_gpu_seg_execution_time(){
-  if(gpu_profiling_flag_) gpu_seg_execution_time_ = get_current_time_ns();
-}
-
-void stop_profiling_gpu_seg_time(unsigned int gpu_seg_id, unsigned int iter){
-  if(gpu_profiling_flag_){
-    unsigned long current_time = get_current_time_ns();
-    gpu_seg_response_time_ = current_time - gpu_seg_response_time_;
-    gpu_seg_execution_time_ = current_time - gpu_seg_execution_time_;
-    
-    fprintf(seg_response_time_fp_, "gpu_%u,%llu,%u\n", gpu_seg_id, gpu_seg_response_time_, iter);
-    fprintf(seg_execution_time_fp_, "gpu_%u,%llu,%u\n", gpu_seg_id, gpu_seg_execution_time_, iter);
-  }
-}
-
-void stop_profiling_gpu_seg_time_with_remark(unsigned int gpu_seg_id, unsigned int iter, const char* remark){
-  if(gpu_profiling_flag_){
-    unsigned long current_time = get_current_time_ns();
-    gpu_seg_response_time_ = current_time - gpu_seg_response_time_;
-    gpu_seg_execution_time_ = current_time - gpu_seg_execution_time_;
-    
-    fprintf(seg_response_time_fp_, "gpu_%u,%llu,%u,%s\n", gpu_seg_id, gpu_seg_response_time_, iter, remark);
-    fprintf(seg_execution_time_fp_, "gpu_%u,%llu,%u,%s\n", gpu_seg_id, gpu_seg_execution_time_, iter, remark);
-  }
-}
-
-unsigned long get_current_time_ns(){
-  struct timespec ts;
-  unsigned long current_time;
-  clock_gettime(CLOCK_REALTIME, &ts);
-  current_time = ts.tv_sec%10000 * 1000000000 + ts.tv_nsec;
-  return current_time;
-}
-
-void start_job_profiling(){
-  if(!gpu_profiling_flag_) return;
-  
-  if(is_gpu_profiling_started_ == 0) is_gpu_profiling_started_ = 1;
-  else if( (is_gpu_profiling_started_ == 1) && (is_gpu_profiling_ready_ == 1) ){
-    start_profiling_cpu_seg_response_time();
-  }
 }
 
-void finish_job_profiling(unsigned int cpu_seg_id){
-  if(!gpu_profiling_flag_) return;
-  
-  if(is_gpu_profiling_started_ == 0) is_gpu_profiling_started_ = 1;
-  else if( (is_gpu_profiling_started_ == 1) && (is_gpu_profiling_ready_ == 1) ){
-    stop_profiling_cpu_seg_response_time(cpu_seg_id, 1);
-    fprintf(seg_response_time_fp_, "-1, -1, -1\n");
-    fprintf(seg_execution_time_fp_, "-1, -1, -1\n");
-    
-    fflush(seg_response_time_fp_);
-    fflush(seg_execution_time_fp_);
-  }
+unsigned long get_current_time_ns() {
+    struct timespec ts;
+    unsigned long current_time;
+    clock_gettime(CLOCK_REALTIME, &ts);
+    current_time = ts.tv_sec % 10000 * 1000000000 + ts.tv_nsec;
+    return current_time;
 }
diff --git a/autoware.ai/src/autoware/common/vector_map/CMakeLists.txt b/autoware.ai/src/autoware/common/vector_map/CMakeLists.txt
index 2b87e783..ce63e68b 100644
--- a/autoware.ai/src/autoware/common/vector_map/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/vector_map/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(vector_map)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   tf
@@ -48,8 +48,4 @@ install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-if(CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-endif()
+)
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/vector_map/package.xml b/autoware.ai/src/autoware/common/vector_map/package.xml
index 2a137505..d5da7bf7 100644
--- a/autoware.ai/src/autoware/common/vector_map/package.xml
+++ b/autoware.ai/src/autoware/common/vector_map/package.xml
@@ -6,7 +6,6 @@
   <maintainer email="syouji@axe.bz">syouji</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>geometry_msgs</depend>
diff --git a/autoware.ai/src/autoware/common/vector_map_server/CMakeLists.txt b/autoware.ai/src/autoware/common/vector_map_server/CMakeLists.txt
index 1bfdbe95..64522e51 100644
--- a/autoware.ai/src/autoware/common/vector_map_server/CMakeLists.txt
+++ b/autoware.ai/src/autoware/common/vector_map_server/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(vector_map_server)
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   autoware_msgs
@@ -101,8 +101,4 @@ install(TARGETS ${PROJECT_NAME} vector_map_client
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-if(CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-endif()
+)
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/common/vector_map_server/package.xml b/autoware.ai/src/autoware/common/vector_map_server/package.xml
index 084652d1..fd9a90c8 100644
--- a/autoware.ai/src/autoware/common/vector_map_server/package.xml
+++ b/autoware.ai/src/autoware/common/vector_map_server/package.xml
@@ -6,7 +6,6 @@
   <maintainer email="syouji@axe.bz">syouji</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>message_generation</build_depend>
 
diff --git a/autoware.ai/src/autoware/core_perception/autoware_connector/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/autoware_connector/CMakeLists.txt
index 8c5aa4ab..4b431a89 100644
--- a/autoware.ai/src/autoware/core_perception/autoware_connector/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/autoware_connector/CMakeLists.txt
@@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 2.8.3)
 project(autoware_connector)
 
 find_package(catkin REQUIRED COMPONENTS
-  autoware_build_flags
   autoware_can_msgs
   autoware_msgs
   geometry_msgs
diff --git a/autoware.ai/src/autoware/core_perception/autoware_connector/package.xml b/autoware.ai/src/autoware/core_perception/autoware_connector/package.xml
index 4345b3f2..84eee481 100644
--- a/autoware.ai/src/autoware/core_perception/autoware_connector/package.xml
+++ b/autoware.ai/src/autoware/core_perception/autoware_connector/package.xml
@@ -6,7 +6,6 @@
   <maintainer email="h_ohta@ertl.jp">h_ohta</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_can_msgs</depend>
diff --git a/autoware.ai/src/autoware/core_perception/image_processor/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/image_processor/CMakeLists.txt
index 07288671..5a33d7c5 100644
--- a/autoware.ai/src/autoware/core_perception/image_processor/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/image_processor/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(image_processor)
 
-find_package(autoware_build_flags REQUIRED)
+
 find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   roscpp
diff --git a/autoware.ai/src/autoware/core_perception/image_processor/package.xml b/autoware.ai/src/autoware/core_perception/image_processor/package.xml
index f2b5af99..bf7ef4a8 100644
--- a/autoware.ai/src/autoware/core_perception/image_processor/package.xml
+++ b/autoware.ai/src/autoware/core_perception/image_processor/package.xml
@@ -6,7 +6,6 @@
   <maintainer email="abrahammonrroy@yahoo.com">amc-nu</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>cv_bridge</depend>
diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt
index 6bf3de1d..613c58e3 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt
@@ -5,15 +5,152 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+find_package(autoware_build_flags COMPONENTS)
+if(autoware_build_flags_FOUND)
+  find_package(autoware_build_flags REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
+  find_package(catkin REQUIRED COMPONENTS
+    autoware_msgs
+    geometry_msgs
+    grid_map_cv
+    grid_map_msgs
+    grid_map_ros
+    jsk_rviz_plugins
+    pcl_ros
+    roscpp
+    sensor_msgs
+    std_msgs
+    tf
+    vector_map_server
+    rubis_lib
+    rubis_msgs
+  )
+
+  find_package(OpenMP)
+  find_package(OpenCV REQUIRED)
+
+  set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
+
+  catkin_package(
+    INCLUDE_DIRS include
+    CATKIN_DEPENDS
+      vector_map_server
+      grid_map_ros
+      grid_map_cv
+      grid_map_msgs
+      jsk_rviz_plugins
+      rubis_lib
+  )
+
+  # Resolve system dependency on yaml-cpp, which apparently does not
+  # provide a CMake find_package() module.
+  find_package(PkgConfig REQUIRED)
+  pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+  find_path(YAML_CPP_INCLUDE_DIR NAMES yaml_cpp.h PATHS ${YAML_CPP_INCLUDE_DIRS})
+  find_library(YAML_CPP_LIBRARY NAMES YAML_CPP PATHS ${YAML_CPP_LIBRARY_DIRS})
+  link_directories(${YAML_CPP_LIBRARY_DIRS})
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+  )
+  link_directories(${OpenCV_LIBRARY_DIRS})
+
+  #Euclidean Cluster
+  add_executable(lidar_euclidean_cluster_detect
+    nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp
+    nodes/lidar_euclidean_cluster_detect/cluster.cpp
+  )
+
+  find_package(CUDA)
+  find_package(Eigen3 QUIET)
+
+  AW_CHECK_CUDA()
+  if(USE_CUDA)
+    
+    add_definitions(-DGPU_CLUSTERING)
+
+    message("-- USING ACCELERATED CLUSTERING --")
+    message("Version: " ${CUDA_VERSION})
+    message("Library: " ${CUDA_CUDA_LIBRARY})
+    message("Runtime: " ${CUDA_CUDART_LIBRARY})
+    target_compile_definitions(lidar_euclidean_cluster_detect PRIVATE
+      GPU_CLUSTERING=1
+    )
+
+    cuda_add_library(gpu_euclidean_clustering
+      include/gpu_euclidean_clustering.h
+      nodes/lidar_euclidean_cluster_detect/gpu_euclidean_clustering.cu
+    )
+
+    target_link_libraries(gpu_euclidean_clustering
+      ${catkin_LIBRARIES}
+    )
+  endif()
+
+  if(USE_CUDA)
+    target_link_libraries(lidar_euclidean_cluster_detect
+      ${OpenCV_LIBRARIES}
+      ${catkin_LIBRARIES}
+      ${YAML_CPP_LIBRARIES}
+      gpu_euclidean_clustering
+    )
+
+    install(TARGETS
+      gpu_euclidean_clustering
+      ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+      LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+      RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+    )
+  else()
+    target_link_libraries(lidar_euclidean_cluster_detect
+      ${OpenCV_LIBRARIES}
+      ${catkin_LIBRARIES}
+      ${YAML_CPP_LIBRARIES}
+    )
+
+    install(TARGETS
+      ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+      LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+      RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+    )
+  endif()
+
+  add_dependencies(lidar_euclidean_cluster_detect
+    ${catkin_EXPORTED_TARGETS}
+  )
+
+  if(OPENMP_FOUND)
+    set_target_properties(lidar_euclidean_cluster_detect PROPERTIES
+      COMPILE_FLAGS ${OpenMP_CXX_FLAGS}
+      LINK_FLAGS ${OpenMP_CXX_FLAGS}
+    )
+  endif()
+
+  install(DIRECTORY include/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  )
+
+  install(TARGETS
+    lidar_euclidean_cluster_detect
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+  install(DIRECTORY launch config
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+  find_package(catkin REQUIRED COMPONENTS
   autoware_msgs
   geometry_msgs
   grid_map_cv
   grid_map_msgs
   grid_map_ros
-  jsk_rviz_plugins
   pcl_ros
   roscpp
   sensor_msgs
@@ -21,122 +158,73 @@ find_package(catkin REQUIRED COMPONENTS
   tf
   vector_map_server
   rubis_lib
-)
+  rubis_msgs
+  )
 
-find_package(OpenMP)
-find_package(OpenCV REQUIRED)
+  find_package(OpenMP)
+  find_package(OpenCV REQUIRED)
+  find_package(Eigen3 QUIET)
 
-set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
+  set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
 
-catkin_package(
+  catkin_package(
   INCLUDE_DIRS include
   CATKIN_DEPENDS
     vector_map_server
     grid_map_ros
     grid_map_cv
     grid_map_msgs
-    jsk_rviz_plugins
     rubis_lib
-)
+  )
 
-# Resolve system dependency on yaml-cpp, which apparently does not
-# provide a CMake find_package() module.
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
-find_path(YAML_CPP_INCLUDE_DIR NAMES yaml_cpp.h PATHS ${YAML_CPP_INCLUDE_DIRS})
-find_library(YAML_CPP_LIBRARY NAMES YAML_CPP PATHS ${YAML_CPP_LIBRARY_DIRS})
-link_directories(${YAML_CPP_LIBRARY_DIRS})
+  # Resolve system dependency on yaml-cpp, which apparently does not
+  # provide a CMake find_package() module.
+  find_package(PkgConfig REQUIRED)
+  pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+  find_path(YAML_CPP_INCLUDE_DIR NAMES yaml_cpp.h PATHS ${YAML_CPP_INCLUDE_DIRS})
+  find_library(YAML_CPP_LIBRARY NAMES YAML_CPP PATHS ${YAML_CPP_LIBRARY_DIRS})
+  link_directories(${YAML_CPP_LIBRARY_DIRS})
 
-include_directories(
+  include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
-)
-link_directories(${OpenCV_LIBRARY_DIRS})
+  )
+  link_directories(${OpenCV_LIBRARY_DIRS})
 
-#Euclidean Cluster
-add_executable(lidar_euclidean_cluster_detect
+  #Euclidean Cluster
+  add_executable(lidar_euclidean_cluster_detect
   nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp
   nodes/lidar_euclidean_cluster_detect/cluster.cpp
-)
-
-find_package(CUDA)
-find_package(Eigen3 QUIET)
-
-AW_CHECK_CUDA()
-if(USE_CUDA)
-  
-  add_definitions(-DGPU_CLUSTERING)
-
-  message("-- USING ACCELERATED CLUSTERING --")
-  message("Version: " ${CUDA_VERSION})
-  message("Library: " ${CUDA_CUDA_LIBRARY})
-  message("Runtime: " ${CUDA_CUDART_LIBRARY})
-  target_compile_definitions(lidar_euclidean_cluster_detect PRIVATE
-    GPU_CLUSTERING=1
   )
 
-  cuda_add_library(gpu_euclidean_clustering
-    include/gpu_euclidean_clustering.h
-    nodes/lidar_euclidean_cluster_detect/gpu_euclidean_clustering.cu
+  target_link_libraries(lidar_euclidean_cluster_detect
+  ${OpenCV_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${YAML_CPP_LIBRARIES}
   )
 
-  target_link_libraries(gpu_euclidean_clustering
-    ${catkin_LIBRARIES}
+  add_dependencies(lidar_euclidean_cluster_detect
+  ${catkin_EXPORTED_TARGETS}
   )
-endif()
 
-if(USE_CUDA)
-  target_link_libraries(lidar_euclidean_cluster_detect
-    ${OpenCV_LIBRARIES}
-    ${catkin_LIBRARIES}
-    ${YAML_CPP_LIBRARIES}
-    gpu_euclidean_clustering
+  install(
+  TARGETS
+    lidar_euclidean_cluster_detect
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
   )
 
-  install(TARGETS
-    gpu_euclidean_clustering
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-else()
-  target_link_libraries(lidar_euclidean_cluster_detect
-    ${OpenCV_LIBRARIES}
-    ${catkin_LIBRARIES}
-    ${YAML_CPP_LIBRARIES}
+  install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
   )
 
-  install(TARGETS
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  install(DIRECTORY config/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+  PATTERN ".svn" EXCLUDE
   )
-endif()
-
-add_dependencies(lidar_euclidean_cluster_detect
-  ${catkin_EXPORTED_TARGETS}
-)
 
-if(OPENMP_FOUND)
-  set_target_properties(lidar_euclidean_cluster_detect PROPERTIES
-    COMPILE_FLAGS ${OpenMP_CXX_FLAGS}
-    LINK_FLAGS ${OpenMP_CXX_FLAGS}
-  )
 endif()
 
-install(DIRECTORY include/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-install(TARGETS
-  lidar_euclidean_cluster_detect
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(DIRECTORY launch config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  PATTERN ".svn" EXCLUDE
-)
diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h
index 26b5b2f4..6ec25fb2 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h
+++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h
@@ -43,7 +43,6 @@
 
 #include <jsk_recognition_msgs/BoundingBox.h>
 #include <jsk_recognition_msgs/PolygonArray.h>
-#include <jsk_rviz_plugins/PictogramArray.h>
 
 #include "autoware_msgs/CloudCluster.h"
 
diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch
index 88c8ff40..af531b77 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch
+++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch
@@ -62,27 +62,10 @@
     <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
   </node>
 
-  <!-- <group if="$(arg use_vector_map)">
-    <node name="object_roi_filter_clustering_$(arg label)" pkg="roi_object_filter" type="roi_object_filter"
-          output="screen" ns="/detection/lidar_detector">
-      <param name="objects_src_topic" value="/objects_$(arg label)"/>
-      <param name="wayarea_gridmap_layer" value="$(arg wayarea_gridmap_layer)"/>
-      <param name="sync_topics" value="false"/>
-      <param name="exception_list" value="[person, bicycle]"/>
-      <param name="label" value="$(arg label)" />
-    </node>
-    <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_$(arg label)"
-          output="screen" ns="/detection/lidar_detector">
-      <param name="objects_src_topic" value="/objects_filtered_$(arg label)"/>
-      <param name="label" value="$(arg label)" />
-    </node>
-  </group>
-  <group unless="$(arg use_vector_map)"> -->
   <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_$(arg label)"
         output="screen" ns="/detection/lidar_detector">
     <param name="objects_src_topic" value="/objects_$(arg label)"/>
     <param name="label" value="$(arg label)" />
   </node>
-  <!-- </group> -->
 
 </launch>
diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch
index 8ad4a0aa..882071b7 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch
+++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch
@@ -18,13 +18,13 @@
     </node>
     <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_01"
           output="screen" ns="/detection/lidar_detector">
-      <param name="objects_src_topic" value="/objects_filtered"/>
+      <param name="objects_src_topic" value="/detection/lidar_detector/objects_center"/>
     </node>
   </group>
   <group unless="$(arg use_vector_map)">
     <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_01"
           output="screen" ns="/detection/lidar_detector">
-      <param name="objects_src_topic" value="/objects"/>
+      <param name="objects_src_topic" value="/detection/lidar_detector/objects_center"/>
     </node>
   </group>
 
diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/gpu_euclidean_clustering.cu b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/gpu_euclidean_clustering.cu
index 6ba49d2e..4dfb0035 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/gpu_euclidean_clustering.cu
+++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/gpu_euclidean_clustering.cu
@@ -55,33 +55,19 @@
  void GpuEuclideanCluster::setInputPoints(float *x, float *y, float *z, int size)
  {
    size_ = size;
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&x_, size_ * sizeof(float)));
-   rubis::sched::yield_gpu("1_htod");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&y_, size_ * sizeof(float)));
-   rubis::sched::yield_gpu("2_htod");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&z_, size_ * sizeof(float)));
-   rubis::sched::yield_gpu("3_htod");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemcpy(x_, x, size_ * sizeof(float), cudaMemcpyHostToDevice));
-   rubis::sched::yield_gpu("4_htod");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemcpy(y_, y, size_ * sizeof(float), cudaMemcpyHostToDevice));
-   rubis::sched::yield_gpu("5_htod");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemcpy(z_, z, size_ * sizeof(float), cudaMemcpyHostToDevice));
-   rubis::sched::yield_gpu("6_htod");
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&cluster_indices_, size_ * sizeof(int)));
-   rubis::sched::yield_gpu("7_htod");
 
    cluster_indices_host_ = (int *) malloc(size_ * sizeof(int));
  }
@@ -363,9 +349,7 @@
  void GpuEuclideanCluster::exclusiveScan(int *input, int ele_num, int *sum)
  {
    thrust::device_ptr<int> dev_ptr(input);
-   rubis::sched::request_gpu(); 
    thrust::exclusive_scan(dev_ptr, dev_ptr + ele_num, dev_ptr);
-   rubis::sched::yield_gpu("71_exclusive_scan"); 
    checkCudaErrors(cudaDeviceSynchronize());
  
    *sum = *(dev_ptr + ele_num - 1);
@@ -408,65 +392,43 @@
    int *cluster_offset;
    int cluster_num, old_cluster_num;
 
-   rubis::sched::request_gpu(); 
    pclEuclideanInitialize << < grid_x, block_x >> > (cluster_indices_, size_);
-   rubis::sched::yield_gpu("8_pclEuclideanInitialize");
    checkCudaErrors(cudaDeviceSynchronize());
  
    old_cluster_num = cluster_num = size_;
   
-   rubis::sched::request_gpu(); 
    checkCudaErrors(cudaMalloc(&cluster_offset, (size_ + 1) * sizeof(int)));
-   rubis::sched::yield_gpu("9_cudaMalloc");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int)));
-   rubis::sched::yield_gpu("10_cudaMemset");
 
-   rubis::sched::request_gpu();
    blockLabelling << < grid_x, block_x >> > (x_, y_, z_, cluster_indices_, size_, threshold_);
-   rubis::sched::yield_gpu("11_blockLabelling");
 
-   rubis::sched::request_gpu();
    clusterMark << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_);
-   rubis::sched::yield_gpu("12_clusterMark");
 
 
    exclusiveScan(cluster_offset, size_ + 1, &cluster_num);
  
    int *cluster_list, *new_cluster_list, *tmp;
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&cluster_list, cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("13_cudaMalloc");
 
-   rubis::sched::request_gpu();
    clusterCollector << < grid_x, block_x >> > (cluster_indices_, cluster_list, cluster_offset, size_);
-   rubis::sched::yield_gpu("14_clusterCollector");
 
    checkCudaErrors(cudaDeviceSynchronize());
  
    int *cluster_matrix;
    int *new_cluster_matrix;
   
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&cluster_matrix, cluster_num * cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("15_cudaMalloc");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemset(cluster_matrix, 0, cluster_num * cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("16_cudaMemset");
 
    checkCudaErrors(cudaDeviceSynchronize());
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&new_cluster_list, cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("17_cudaMalloc");
  
-   rubis::sched::request_gpu();
    buildClusterMatrix << < grid_x, block_x >> >
                                    (x_, y_, z_, cluster_indices_, cluster_matrix, cluster_offset, size_, cluster_num, threshold_);
-   rubis::sched::yield_gpu("18_buildClusterMatrix");
    checkCudaErrors(cudaDeviceSynchronize());
  
    int block_x2 = 0, grid_x2 = 0;
@@ -479,51 +441,33 @@
      block_x2 = (cluster_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : cluster_num;
      grid_x2 = (cluster_num - 1) / block_x2 + 1;
  
-     rubis::sched::request_gpu();
      mergeClusters << < grid_x2, block_x2 >> > (cluster_matrix, cluster_list, cluster_num);
-     rubis::sched::yield_gpu("19_mergeClusters");
 
-     rubis::sched::request_gpu();
      reflexClusterChanges << < grid_x, block_x >> > (cluster_indices_, cluster_offset, cluster_list, size_);
-     rubis::sched::yield_gpu("20_reflexClusterChanges");
 
-     rubis::sched::request_gpu();
      checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int)));
-     rubis::sched::yield_gpu("21_cudaMemset");
 
-     rubis::sched::request_gpu();
      clusterMark << < grid_x2, block_x2 >> > (cluster_list, cluster_offset, cluster_num);
-     rubis::sched::yield_gpu("22_clusterMark");
 
      exclusiveScan(cluster_offset, size_ + 1, &cluster_num);
  
      if (grid_x2 == 1 && cluster_num == old_cluster_num)
        break;
     
-     rubis::sched::request_gpu();
      clusterCollector << < grid_x2, block_x2 >> > (cluster_list, new_cluster_list, cluster_offset, old_cluster_num);
-     rubis::sched::yield_gpu("23_clusterCollector");
 
      checkCudaErrors(cudaDeviceSynchronize());
  
-     rubis::sched::request_gpu();
      checkCudaErrors(cudaMalloc(&new_cluster_matrix, cluster_num * cluster_num * sizeof(int)));
-     rubis::sched::yield_gpu("24_cudaMalloc");
 
-     rubis::sched::request_gpu();
      checkCudaErrors(cudaMemset(new_cluster_matrix, 0, cluster_num * cluster_num * sizeof(int)));
-     rubis::sched::yield_gpu("25_cudaMemset");
 
-     rubis::sched::request_gpu();
      rebuildClusterMatrix << < grid_x2, block_x2 >> >
                                         (cluster_matrix, cluster_list, new_cluster_matrix, cluster_offset, old_cluster_num, cluster_num);
-     rubis::sched::yield_gpu("26_rebuildClusterMatrix");
 
      checkCudaErrors(cudaDeviceSynchronize());
     
-     rubis::sched::request_gpu();
      checkCudaErrors(cudaFree(cluster_matrix));
-     rubis::sched::yield_gpu("27_free");
      
      cluster_matrix = new_cluster_matrix;
      tmp = cluster_list;
@@ -533,32 +477,19 @@
  
    cluster_num_ = cluster_num;
  
-   rubis::sched::request_gpu();
    resetClusterIndexes << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_);
-   rubis::sched::yield_gpu("28_resetClusterIndexes");
 
    checkCudaErrors(cudaDeviceSynchronize());
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemcpy(cluster_indices_host_, cluster_indices_, size_ * sizeof(int), cudaMemcpyDeviceToHost));
-   rubis::sched::yield_gpu("29_dtoh");
  
- 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(cluster_matrix));
-   rubis::sched::yield_gpu("30_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(cluster_list));
-   rubis::sched::yield_gpu("31_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(new_cluster_list));
-   rubis::sched::yield_gpu("32_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(cluster_offset));
-   rubis::sched::yield_gpu("33_free");
  }
  
  extern "C" __global__ void mergeSelfClusters(int *cluster_matrix, int *cluster_list, int cluster_num, bool *changed)
@@ -728,80 +659,53 @@
    int *cluster_offset;
    int cluster_num, old_cluster_num;
  
-   rubis::sched::request_gpu();
    pclEuclideanInitialize << < grid_x, block_x >> > (cluster_indices_, size_);
-   rubis::sched::yield_gpu("34_pclEuclideanInitialize");
    
    checkCudaErrors(cudaDeviceSynchronize());
  
    old_cluster_num = cluster_num = size_;
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&cluster_offset, (size_ + 1) * sizeof(int)));
-   rubis::sched::yield_gpu("35_cudaMalloc");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int)));
-   rubis::sched::yield_gpu("36_cudaMemset");
 
-   rubis::sched::request_gpu();
    blockLabelling << < grid_x, block_x >> > (x_, y_, z_, cluster_indices_, size_, threshold_);
-   rubis::sched::yield_gpu("37_blockLabelling");
    
-   rubis::sched::request_gpu();
    clusterMark << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_);
-   rubis::sched::yield_gpu("38_clusterMark");
-
   
    exclusiveScan(cluster_offset, size_ + 1, &cluster_num);
  
    int *cluster_list, *new_cluster_list, *tmp;
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&cluster_list, cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("39_cudaMalloc");
 
-   rubis::sched::request_gpu();
    clusterCollector << < grid_x, block_x >> > (cluster_indices_, cluster_list, cluster_offset, size_);
-   rubis::sched::yield_gpu("40_clusterCollector");
    checkCudaErrors(cudaDeviceSynchronize());
  
    int *cluster_matrix;
    int *new_cluster_matrix;
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&cluster_matrix, cluster_num * cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("41_cudaMalloc");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemset(cluster_matrix, 0, cluster_num * cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("42_cudaMemset");
    
    checkCudaErrors(cudaDeviceSynchronize());
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMalloc(&new_cluster_list, cluster_num * sizeof(int)));
-   rubis::sched::yield_gpu("43_cudaMalloc");
  
-   rubis::sched::request_gpu();
    buildClusterMatrix << < grid_x, block_x >> >
                                    (x_, y_, z_, cluster_indices_, cluster_matrix, cluster_offset, size_, cluster_num, threshold_);
-   rubis::sched::yield_gpu("44_buildClusterMatrix");
    checkCudaErrors(cudaDeviceSynchronize());
  
    int block_x2 = 0, grid_x2 = 0;
  
    bool *changed;
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMallocHost(&changed, sizeof(bool)));
-   rubis::sched::yield_gpu("45_cudaMallocHost");
  
  #ifndef SERIAL
    int *changed_diag;
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMallocHost(&changed_diag, sizeof(int)));
-   rubis::sched::yield_gpu("46_cudaMallocHost");
  #endif
  
    int max_base_row = 0;
@@ -812,9 +716,7 @@
      block_x2 = (cluster_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : cluster_num;
      grid_x2 = (cluster_num - 1) / block_x2 + 1;
  
-     rubis::sched::request_gpu();
      mergeSelfClusters << < grid_x2, block_x2 >> > (cluster_matrix, cluster_list, cluster_num, changed);
-     rubis::sched::yield_gpu("47_mergeSelfClusters");
 
      checkCudaErrors(cudaDeviceSynchronize());
  
@@ -837,14 +739,12 @@
  #ifdef SERIAL
        //Merge clusters in each sub-matrix by moving from top to bottom of the similarity sub-matrix
        for (int shift_level = 0; !(*changed) && shift_level < sub_matrix_col; shift_level++) {
-         rubis::sched::request_gpu();
          mergeInterClusters<<<grid_x2, block_x2>>>(cluster_matrix, cluster_list,
                                  shift_level,
                                  base_row, base_column,
                                  sub_matrix_row, sub_matrix_col,
                                  sub_matrix_offset_row, sub_matrix_offset_col,
                                  cluster_num, changed);
-         rubis::sched::yield_gpu("48_mergeInterClusters");
          checkCudaErrors(cudaDeviceSynchronize());
        }
  #else
@@ -855,13 +755,11 @@
  
        *changed_diag = -1;
 
-       rubis::sched::request_gpu(); 
        clustersIntersecCheck << < grid_size, block_size >> > (cluster_matrix, changed_diag,
          base_row, base_column,
          sub_matrix_row, sub_matrix_col,
          sub_matrix_offset_row, sub_matrix_offset_col,
          cluster_num);
-       rubis::sched::yield_gpu("49_clustersIntersecCheck");
 
        checkCudaErrors(cudaDeviceSynchronize());
  
@@ -869,13 +767,11 @@
        {
          //Merge clusters in sub-matrix that stay in the changed_diag diagonal by moving from top to bottom of the matrix.
 
-         rubis::sched::request_gpu();
          mergeInterClusters << < grid_x2, block_x2 >> > (cluster_matrix, cluster_list, *changed_diag,
            base_row, base_column,
            sub_matrix_row, sub_matrix_col,
            sub_matrix_offset_row, sub_matrix_offset_col,
            cluster_num, changed);
-         rubis::sched::yield_gpu("50_mergeInterClusters");
          checkCudaErrors(cudaDeviceSynchronize());
        }
  
@@ -891,48 +787,32 @@
  
      if (*changed)
      {
-       rubis::sched::request_gpu();
        reflexClusterChanges << < grid_x, block_x >> > (cluster_indices_, cluster_offset, cluster_list, size_);
-       rubis::sched::yield_gpu("51_reflexClusterChanges");
 
-       rubis::sched::request_gpu();
        checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int)));
-       rubis::sched::yield_gpu("52_cudaMemset");
  
        block_x2 = (cluster_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : cluster_num;
        grid_x2 = (cluster_num - 1) / block_x2 + 1;
  
-       rubis::sched::request_gpu();
        clusterMark << < grid_x2, block_x2 >> > (cluster_list, cluster_offset, cluster_num);
-       rubis::sched::yield_gpu("53_clusterMark");
  
        old_cluster_num = cluster_num;
        
        exclusiveScan(cluster_offset, size_ + 1, &cluster_num);
 
-       rubis::sched::request_gpu();
        clusterCollector << < grid_x2, block_x2 >> > (cluster_list, new_cluster_list, cluster_offset, old_cluster_num);
-       rubis::sched::yield_gpu("54_clusterCollector");
 
        checkCudaErrors(cudaDeviceSynchronize());
  
-       rubis::sched::request_gpu();
        checkCudaErrors(cudaMalloc(&new_cluster_matrix, cluster_num * cluster_num * sizeof(int)));
-       rubis::sched::yield_gpu("55_cudaMalloc");
 
-       rubis::sched::request_gpu();
        checkCudaErrors(cudaMemset(new_cluster_matrix, 0, cluster_num * cluster_num * sizeof(int)));
-       rubis::sched::yield_gpu("56_cudaMemset");
 
-       rubis::sched::request_gpu();
        rebuildClusterMatrix << < grid_x2, block_x2 >> >
                                           (cluster_matrix, cluster_list, new_cluster_matrix, cluster_offset, old_cluster_num, cluster_num);
-       rubis::sched::yield_gpu("57_rebuildClusterMatrix");
        checkCudaErrors(cudaDeviceSynchronize());
 
-       rubis::sched::request_gpu(); 
        checkCudaErrors(cudaFree(cluster_matrix));
-       rubis::sched::yield_gpu("58_cudaFree");
 
        cluster_matrix = new_cluster_matrix;
        tmp = cluster_list;
@@ -945,39 +825,23 @@
  
    //Reset all cluster indexes to make them start from 0
 
-   rubis::sched::request_gpu();
    resetClusterIndexes << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_);
-   rubis::sched::yield_gpu("59_resetClusterIndexes");
 
    checkCudaErrors(cudaDeviceSynchronize());
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaMemcpy(cluster_indices_host_, cluster_indices_, size_ * sizeof(int), cudaMemcpyDeviceToHost));
-   rubis::sched::yield_gpu("60_cudaMemcpy");
  
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(cluster_matrix));
-   rubis::sched::yield_gpu("61_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(cluster_list));
-   rubis::sched::yield_gpu("62_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(new_cluster_list));
-   rubis::sched::yield_gpu("63_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(cluster_offset));
-   rubis::sched::yield_gpu("64_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFreeHost(changed));
-   rubis::sched::yield_gpu("65_free");
  #ifndef SERIAL
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFreeHost(changed_diag));
-   rubis::sched::yield_gpu("66_free");
  #endif
  }
  
@@ -1043,21 +907,13 @@
  
  GpuEuclideanCluster::~GpuEuclideanCluster()
  {
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(x_));
-   rubis::sched::yield_gpu("67_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(y_));
-   rubis::sched::yield_gpu("68_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(z_));
-   rubis::sched::yield_gpu("69_free");
 
-   rubis::sched::request_gpu();
    checkCudaErrors(cudaFree(cluster_indices_));
-   rubis::sched::yield_gpu("70_free");
 
    free(cluster_indices_host_);
  }
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp
index 40e5d6e8..489957da 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp
+++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp
@@ -1,51 +1,51 @@
 
+#include <cmath>
 #include <iostream>
-#include <vector>
-#include <string>
-#include <sstream>
 #include <limits>
-#include <cmath>
 #include <sched.h>
+#include <sstream>
+#include <string>
+#include <vector>
 
 #include <ros/ros.h>
 
-#include <pcl_conversions/pcl_conversions.h>
 #include <pcl/PCLPointCloud2.h>
 #include <pcl/conversions.h>
-#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
 
 #include <pcl/ModelCoefficients.h>
 #include <pcl/point_types.h>
 
+#include <pcl/filters/conditional_removal.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/conditional_removal.h>
 
-#include <pcl/features/normal_3d.h>
-#include <pcl/features/normal_3d_omp.h>
 #include <pcl/features/don.h>
 #include <pcl/features/fpfh_omp.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
 
 #include <pcl/kdtree/kdtree.h>
 
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/sample_consensus/model_types.h>
 
-#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/segmentation/extract_clusters.h>
 #include <pcl/segmentation/conditional_euclidean_clustering.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/segmentation/sac_segmentation.h>
 
 #include <pcl/common/common.h>
 
-#include <pcl/search/organized.h>
 #include <pcl/search/kdtree.h>
+#include <pcl/search/organized.h>
 
 #include <pcl/segmentation/extract_clusters.h>
 
 #include <std_msgs/Float32MultiArray.h>
-#include <std_msgs/MultiArrayLayout.h>
 #include <std_msgs/MultiArrayDimension.h>
+#include <std_msgs/MultiArrayLayout.h>
 
 #include "autoware_msgs/Centroids.h"
 #include "autoware_msgs/CloudCluster.h"
@@ -53,15 +53,18 @@
 #include "autoware_msgs/DetectedObject.h"
 #include "autoware_msgs/DetectedObjectArray.h"
 
+#include "rubis_msgs/DetectedObjectArray.h"
+#include "rubis_msgs/PointCloud2.h"
+
 #include <vector_map/vector_map.h>
 
 #include <tf/tf.h>
 
 #include <yaml-cpp/yaml.h>
 
-#include <opencv2/opencv.hpp>
-#include <opencv2/highgui.hpp>
 #include <opencv2/core/version.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/opencv.hpp>
 
 #include "gencolors.cpp"
 
@@ -91,6 +94,8 @@ ros::Publisher _pub_points_lanes_cloud;
 
 ros::Publisher _pub_detected_objects;
 
+ros::Publisher _pub_rubis_detected_objects;
+
 std_msgs::Header _velodyne_header;
 
 std::string _output_frame;
@@ -103,7 +108,7 @@ static int _cluster_size_min;
 static int _cluster_size_max;
 static const double _initial_quat_w = 1.0;
 
-static bool _remove_ground;  // only ground
+static bool _remove_ground; // only ground
 
 static bool _using_sensor_cloud;
 static bool _use_diffnormals;
@@ -143,53 +148,50 @@ static std::string _deadline_file_name;
 
 /* For GPU Profiling */
 #ifdef GPU_CLUSTERING
-static GpuEuclideanCluster _gecl_cluster;  
+static GpuEuclideanCluster _gecl_cluster;
 #endif
 
-tf::StampedTransform findTransform(const std::string &in_target_frame, const std::string &in_source_frame)
-{
-  tf::StampedTransform transform;
-
-  try
-  {
-    // What time should we use?
-    _vectormap_transform_listener->lookupTransform(in_target_frame, in_source_frame, ros::Time(0), transform);
-  }
-  catch (tf::TransformException ex)
-  {
-    ROS_ERROR("%s", ex.what());
-    return transform;
-  }
+tf::StampedTransform findTransform(const std::string &in_target_frame,
+                                   const std::string &in_source_frame) {
+    tf::StampedTransform transform;
+
+    try {
+        // What time should we use?
+        _vectormap_transform_listener->lookupTransform(
+            in_target_frame, in_source_frame, ros::Time(0), transform);
+    } catch (tf::TransformException ex) {
+        ROS_ERROR("[lidar_euclidean_cluster_detect] %s", ex.what());
+        return transform;
+    }
 
-  return transform;
+    return transform;
 }
 
-geometry_msgs::Point transformPoint(const geometry_msgs::Point& point, const tf::Transform& tf)
-{
-  tf::Point tf_point;
-  tf::pointMsgToTF(point, tf_point);
+geometry_msgs::Point transformPoint(const geometry_msgs::Point &point,
+                                    const tf::Transform &tf) {
+    tf::Point tf_point;
+    tf::pointMsgToTF(point, tf_point);
 
-  tf_point = tf * tf_point;
+    tf_point = tf * tf_point;
 
-  geometry_msgs::Point ros_point;
-  tf::pointTFToMsg(tf_point, ros_point);
+    geometry_msgs::Point ros_point;
+    tf::pointTFToMsg(tf_point, ros_point);
 
-  return ros_point;
+    return ros_point;
 }
 
-void transformBoundingBox(const jsk_recognition_msgs::BoundingBox &in_boundingbox,
-                          jsk_recognition_msgs::BoundingBox &out_boundingbox, const std::string &in_target_frame,
-                          const std_msgs::Header &in_header)
-{
+void transformBoundingBox(
+    const jsk_recognition_msgs::BoundingBox &in_boundingbox,
+    jsk_recognition_msgs::BoundingBox &out_boundingbox,
+    const std::string &in_target_frame, const std_msgs::Header &in_header) {
     geometry_msgs::PoseStamped pose_in, pose_out;
     pose_in.header = in_header;
     pose_in.pose = in_boundingbox.pose;
-    try
-    {
-        _transform_listener->transformPose(in_target_frame, ros::Time(), pose_in, in_header.frame_id, pose_out);
-    }
-    catch (tf::TransformException &ex)
-    {
+    try {
+        _transform_listener->transformPose(in_target_frame, ros::Time(),
+                                           pose_in, in_header.frame_id,
+                                           pose_out);
+    } catch (tf::TransformException &ex) {
         ROS_ERROR("transformBoundingBox: %s", ex.what());
     }
     out_boundingbox.pose = pose_out.pose;
@@ -200,970 +202,941 @@ void transformBoundingBox(const jsk_recognition_msgs::BoundingBox &in_boundingbo
     out_boundingbox.label = in_boundingbox.label;
 }
 
-void publishDetectedObjects(const autoware_msgs::CloudClusterArray &in_clusters)
-{
-  autoware_msgs::DetectedObjectArray detected_objects;
-  detected_objects.header = in_clusters.header;
-
-  for (size_t i = 0; i < in_clusters.clusters.size(); i++)
-  {
-    autoware_msgs::DetectedObject detected_object;
-    detected_object.header = in_clusters.header;
-    detected_object.label = "unknown";
-    detected_object.score = 1.;
-    detected_object.space_frame = in_clusters.header.frame_id;
-    detected_object.pose = in_clusters.clusters[i].bounding_box.pose;
-    detected_object.dimensions = in_clusters.clusters[i].dimensions;
-    detected_object.pointcloud = in_clusters.clusters[i].cloud;
-    detected_object.convex_hull = in_clusters.clusters[i].convex_hull;
-    detected_object.valid = true;
-    
-    detected_objects.objects.push_back(detected_object);
-  }
-  _pub_detected_objects.publish(detected_objects);
-  
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY){
-    rubis::sched::init_task();
-    if(rubis::sched::gpu_profiling_flag_) rubis::sched::start_gpu_profiling();
-  }
-  
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+void publishDetectedObjects(
+    const autoware_msgs::CloudClusterArray &in_clusters) {
+    autoware_msgs::DetectedObjectArray detected_objects;
+    rubis_msgs::DetectedObjectArray rubis_detected_objects;
+    detected_objects.header = in_clusters.header;
+
+    for (size_t i = 0; i < in_clusters.clusters.size(); i++) {
+        autoware_msgs::DetectedObject detected_object;
+        detected_object.header = in_clusters.header;
+        detected_object.label = "unknown";
+        detected_object.score = 1.;
+        detected_object.space_frame = in_clusters.header.frame_id;
+        detected_object.pose = in_clusters.clusters[i].bounding_box.pose;
+        detected_object.dimensions = in_clusters.clusters[i].dimensions;
+        detected_object.pointcloud = in_clusters.clusters[i].cloud;
+        detected_object.convex_hull = in_clusters.clusters[i].convex_hull;
+        detected_object.valid = true;
+
+        detected_objects.objects.push_back(detected_object);
+    }
+    _pub_detected_objects.publish(detected_objects);
+
+    rubis_detected_objects.header = in_clusters.header;
+    rubis_detected_objects.object_array.header = in_clusters.header;
+    rubis_detected_objects.instance = rubis::instance_;
+    rubis_detected_objects.lidar_instance = rubis::lidar_instance_;
+    rubis_detected_objects.object_array = detected_objects;
+
+    _pub_rubis_detected_objects.publish(rubis_detected_objects);
 }
 
-void publishCloudClusters(const ros::Publisher *in_publisher, const autoware_msgs::CloudClusterArray &in_clusters,
-                          const std::string &in_target_frame, const std_msgs::Header &in_header)
-{
-  if (in_target_frame != in_header.frame_id)
-  {
-    autoware_msgs::CloudClusterArray clusters_transformed;
-    clusters_transformed.header = in_header;
-    clusters_transformed.header.frame_id = in_target_frame;
-    for (auto i = in_clusters.clusters.begin(); i != in_clusters.clusters.end(); i++)
-    {
-      autoware_msgs::CloudCluster cluster_transformed;
-      cluster_transformed.header = in_header;
-      try
-      {
-        _transform_listener->lookupTransform(in_target_frame, _velodyne_header.frame_id, ros::Time(),
-                                             *_transform);
-        pcl_ros::transformPointCloud(in_target_frame, *_transform, i->cloud, cluster_transformed.cloud);
-        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->min_point, in_header.frame_id,
-                                            cluster_transformed.min_point);
-        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->max_point, in_header.frame_id,
-                                            cluster_transformed.max_point);
-        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->avg_point, in_header.frame_id,
-                                            cluster_transformed.avg_point);
-        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->centroid_point, in_header.frame_id,
-                                            cluster_transformed.centroid_point);
-
-        cluster_transformed.dimensions = i->dimensions;
-        cluster_transformed.eigen_values = i->eigen_values;
-        cluster_transformed.eigen_vectors = i->eigen_vectors;
-
-        cluster_transformed.convex_hull = i->convex_hull;
-        cluster_transformed.bounding_box.pose.position = i->bounding_box.pose.position;
-        if(_pose_estimation)
-        {
-          cluster_transformed.bounding_box.pose.orientation = i->bounding_box.pose.orientation;
+void publishCloudClusters(const ros::Publisher *in_publisher,
+                          const autoware_msgs::CloudClusterArray &in_clusters,
+                          const std::string &in_target_frame,
+                          const std_msgs::Header &in_header) {
+    if (in_target_frame != in_header.frame_id) {
+        autoware_msgs::CloudClusterArray clusters_transformed;
+        clusters_transformed.header = in_header;
+        clusters_transformed.header.frame_id = in_target_frame;
+        for (auto i = in_clusters.clusters.begin();
+             i != in_clusters.clusters.end(); i++) {
+            autoware_msgs::CloudCluster cluster_transformed;
+            cluster_transformed.header = in_header;
+            try {
+                _transform_listener->lookupTransform(in_target_frame,
+                                                     _velodyne_header.frame_id,
+                                                     ros::Time(), *_transform);
+                pcl_ros::transformPointCloud(in_target_frame, *_transform,
+                                             i->cloud,
+                                             cluster_transformed.cloud);
+                _transform_listener->transformPoint(
+                    in_target_frame, ros::Time(), i->min_point,
+                    in_header.frame_id, cluster_transformed.min_point);
+                _transform_listener->transformPoint(
+                    in_target_frame, ros::Time(), i->max_point,
+                    in_header.frame_id, cluster_transformed.max_point);
+                _transform_listener->transformPoint(
+                    in_target_frame, ros::Time(), i->avg_point,
+                    in_header.frame_id, cluster_transformed.avg_point);
+                _transform_listener->transformPoint(
+                    in_target_frame, ros::Time(), i->centroid_point,
+                    in_header.frame_id, cluster_transformed.centroid_point);
+
+                cluster_transformed.dimensions = i->dimensions;
+                cluster_transformed.eigen_values = i->eigen_values;
+                cluster_transformed.eigen_vectors = i->eigen_vectors;
+
+                cluster_transformed.convex_hull = i->convex_hull;
+                cluster_transformed.bounding_box.pose.position =
+                    i->bounding_box.pose.position;
+                if (_pose_estimation) {
+                    cluster_transformed.bounding_box.pose.orientation =
+                        i->bounding_box.pose.orientation;
+                } else {
+                    cluster_transformed.bounding_box.pose.orientation.w =
+                        _initial_quat_w;
+                }
+                clusters_transformed.clusters.push_back(cluster_transformed);
+            } catch (tf::TransformException &ex) {
+                ROS_ERROR("publishCloudClusters: %s", ex.what());
+            }
         }
-        else
-        {
-          cluster_transformed.bounding_box.pose.orientation.w = _initial_quat_w;
-        }
-        clusters_transformed.clusters.push_back(cluster_transformed);
-      }
-      catch (tf::TransformException &ex)
-      {
-        ROS_ERROR("publishCloudClusters: %s", ex.what());
-      }
+        in_publisher->publish(clusters_transformed);
+        publishDetectedObjects(clusters_transformed);
+    } else {
+        in_publisher->publish(in_clusters);
+        publishDetectedObjects(in_clusters);
     }
-    in_publisher->publish(clusters_transformed);
-    publishDetectedObjects(clusters_transformed);
-  } else
-  { 
-    in_publisher->publish(in_clusters);
-    publishDetectedObjects(in_clusters);
-  }  
 }
 
-void publishCentroids(const ros::Publisher *in_publisher, const autoware_msgs::Centroids &in_centroids,
-                      const std::string &in_target_frame, const std_msgs::Header &in_header)
-{
-  if (in_target_frame != in_header.frame_id)
-  {
-    autoware_msgs::Centroids centroids_transformed;
-    centroids_transformed.header = in_header;
-    centroids_transformed.header.frame_id = in_target_frame;
-    for (auto i = centroids_transformed.points.begin(); i != centroids_transformed.points.end(); i++)
-    {
-      geometry_msgs::PointStamped centroid_in, centroid_out;
-      centroid_in.header = in_header;
-      centroid_in.point = *i;
-      try
-      {
-        _transform_listener->transformPoint(in_target_frame, ros::Time(), centroid_in, in_header.frame_id,
-                                            centroid_out);
-
-        centroids_transformed.points.push_back(centroid_out.point);
-      }
-      catch (tf::TransformException &ex)
-      {
-        ROS_ERROR("publishCentroids: %s", ex.what());
-      }
+void publishCentroids(const ros::Publisher *in_publisher,
+                      const autoware_msgs::Centroids &in_centroids,
+                      const std::string &in_target_frame,
+                      const std_msgs::Header &in_header) {
+    if (in_target_frame != in_header.frame_id) {
+        autoware_msgs::Centroids centroids_transformed;
+        centroids_transformed.header = in_header;
+        centroids_transformed.header.frame_id = in_target_frame;
+        for (auto i = centroids_transformed.points.begin();
+             i != centroids_transformed.points.end(); i++) {
+            geometry_msgs::PointStamped centroid_in, centroid_out;
+            centroid_in.header = in_header;
+            centroid_in.point = *i;
+            try {
+                _transform_listener->transformPoint(
+                    in_target_frame, ros::Time(), centroid_in,
+                    in_header.frame_id, centroid_out);
+
+                centroids_transformed.points.push_back(centroid_out.point);
+            } catch (tf::TransformException &ex) {
+                ROS_ERROR("publishCentroids: %s", ex.what());
+            }
+        }
+        in_publisher->publish(centroids_transformed);
+    } else {
+        in_publisher->publish(in_centroids);
     }
-    in_publisher->publish(centroids_transformed);
-  } else
-  {
-    in_publisher->publish(in_centroids);
-  }
 }
 
-void publishCloud(const ros::Publisher *in_publisher, const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_to_publish_ptr)
-{
-  sensor_msgs::PointCloud2 cloud_msg;
-  pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
-  cloud_msg.header = _velodyne_header;
-  in_publisher->publish(cloud_msg);
+void publishCloud(
+    const ros::Publisher *in_publisher,
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_to_publish_ptr) {
+    sensor_msgs::PointCloud2 cloud_msg;
+    pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
+    cloud_msg.header = _velodyne_header;
+    in_publisher->publish(cloud_msg);
 }
 
-void publishColorCloud(const ros::Publisher *in_publisher,
-                       const pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud_to_publish_ptr)
-{
-  sensor_msgs::PointCloud2 cloud_msg;
-  pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
-  cloud_msg.header = _velodyne_header;
-  in_publisher->publish(cloud_msg);
+void publishColorCloud(
+    const ros::Publisher *in_publisher,
+    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud_to_publish_ptr) {
+    sensor_msgs::PointCloud2 cloud_msg;
+    pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
+    cloud_msg.header = _velodyne_header;
+    in_publisher->publish(cloud_msg);
 }
 
 void keepLanePoints(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
-                    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, float in_left_lane_threshold = 1.5,
-                    float in_right_lane_threshold = 1.5)
-{
-  pcl::PointIndices::Ptr far_indices(new pcl::PointIndices);
-  for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
-  {
-    pcl::PointXYZ current_point;
-    current_point.x = in_cloud_ptr->points[i].x;
-    current_point.y = in_cloud_ptr->points[i].y;
-    current_point.z = in_cloud_ptr->points[i].z;
-
-    if (current_point.y > (in_left_lane_threshold) || current_point.y < -1.0 * in_right_lane_threshold)
-    {
-      far_indices->indices.push_back(i);
+                    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+                    float in_left_lane_threshold = 1.5,
+                    float in_right_lane_threshold = 1.5) {
+    pcl::PointIndices::Ptr far_indices(new pcl::PointIndices);
+    for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++) {
+        pcl::PointXYZ current_point;
+        current_point.x = in_cloud_ptr->points[i].x;
+        current_point.y = in_cloud_ptr->points[i].y;
+        current_point.z = in_cloud_ptr->points[i].z;
+
+        if (current_point.y > (in_left_lane_threshold) ||
+            current_point.y < -1.0 * in_right_lane_threshold) {
+            far_indices->indices.push_back(i);
+        }
     }
-  }
-  out_cloud_ptr->points.clear();
-  pcl::ExtractIndices<pcl::PointXYZ> extract;
-  extract.setInputCloud(in_cloud_ptr);
-  extract.setIndices(far_indices);
-  extract.setNegative(true);  // true removes the indices, false leaves only the indices
-  extract.filter(*out_cloud_ptr);
+    out_cloud_ptr->points.clear();
+    pcl::ExtractIndices<pcl::PointXYZ> extract;
+    extract.setInputCloud(in_cloud_ptr);
+    extract.setIndices(far_indices);
+    extract.setNegative(
+        true); // true removes the indices, false leaves only the indices
+    extract.filter(*out_cloud_ptr);
 }
 
 #ifdef GPU_CLUSTERING
 
-std::vector<ClusterPtr> clusterAndColorGpu(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
-                                           pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud_ptr,
-                                           autoware_msgs::Centroids &in_out_centroids,
-                                           double in_max_cluster_distance = 0.5)
-{
-  std::vector<ClusterPtr> clusters;
+std::vector<ClusterPtr>
+clusterAndColorGpu(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+                   pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud_ptr,
+                   autoware_msgs::Centroids &in_out_centroids,
+                   double in_max_cluster_distance = 0.5) {
+    std::vector<ClusterPtr> clusters;
 
-  // Convert input point cloud to vectors of x, y, and z
+    // Convert input point cloud to vectors of x, y, and z
 
-  int size = in_cloud_ptr->points.size();
+    int size = in_cloud_ptr->points.size();
 
-  if (size == 0)
-    return clusters;
+    if (size == 0)
+        return clusters;
+
+    float *tmp_x, *tmp_y, *tmp_z;
+
+    tmp_x = (float *)malloc(sizeof(float) * size);
+    tmp_y = (float *)malloc(sizeof(float) * size);
+    tmp_z = (float *)malloc(sizeof(float) * size);
+
+    for (int i = 0; i < size; i++) {
+        pcl::PointXYZ tmp_point = in_cloud_ptr->at(i);
+
+        tmp_x[i] = tmp_point.x;
+        tmp_y[i] = tmp_point.y;
+        tmp_z[i] = tmp_point.z;
+    }
+
+    _gecl_cluster.setInputPoints(tmp_x, tmp_y, tmp_z, size);
+    _gecl_cluster.setThreshold(in_max_cluster_distance);
+    _gecl_cluster.setMinClusterPts(_cluster_size_min);
+    _gecl_cluster.setMaxClusterPts(_cluster_size_max);
+    _gecl_cluster.extractClusters();
+    std::vector<GpuEuclideanCluster::GClusterIndex> cluster_indices =
+        _gecl_cluster.getOutput();
+
+    unsigned int k = 0;
 
-  float *tmp_x, *tmp_y, *tmp_z;
-
-  tmp_x = (float *) malloc(sizeof(float) * size);
-  tmp_y = (float *) malloc(sizeof(float) * size);
-  tmp_z = (float *) malloc(sizeof(float) * size);
-
-  for (int i = 0; i < size; i++)
-  {
-    pcl::PointXYZ tmp_point = in_cloud_ptr->at(i);
-
-    tmp_x[i] = tmp_point.x;
-    tmp_y[i] = tmp_point.y;
-    tmp_z[i] = tmp_point.z;
-  }  
-  
-  _gecl_cluster.setInputPoints(tmp_x, tmp_y, tmp_z, size);
-  _gecl_cluster.setThreshold(in_max_cluster_distance);
-  _gecl_cluster.setMinClusterPts(_cluster_size_min);
-  _gecl_cluster.setMaxClusterPts(_cluster_size_max);
-  _gecl_cluster.extractClusters();
-  std::vector<GpuEuclideanCluster::GClusterIndex> cluster_indices = _gecl_cluster.getOutput();
-
-  unsigned int k = 0;
-
-  for (auto it = cluster_indices.begin(); it != cluster_indices.end(); it++)
-  {
-    ClusterPtr cluster(new Cluster());
-    cluster->SetCloud(in_cloud_ptr, it->points_in_cluster, _velodyne_header, k, (int) _colors[k].val[0],
-                      (int) _colors[k].val[1], (int) _colors[k].val[2], "", _pose_estimation);
-    clusters.push_back(cluster);
-
-    k++;
-  }
-
-  free(tmp_x);
-  free(tmp_y);
-  free(tmp_z);
-
-  return clusters;
+    for (auto it = cluster_indices.begin(); it != cluster_indices.end(); it++) {
+        ClusterPtr cluster(new Cluster());
+        cluster->SetCloud(in_cloud_ptr, it->points_in_cluster, _velodyne_header,
+                          k, (int)_colors[k].val[0], (int)_colors[k].val[1],
+                          (int)_colors[k].val[2], "", _pose_estimation);
+        clusters.push_back(cluster);
+
+        k++;
+    }
+
+    free(tmp_x);
+    free(tmp_y);
+    free(tmp_z);
+
+    return clusters;
 }
 
 #endif
 
-std::vector<ClusterPtr> clusterAndColor(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
-                                        pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud_ptr,
-                                        autoware_msgs::Centroids &in_out_centroids,
-                                        double in_max_cluster_distance = 0.5)
-{
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
-
-  // create 2d pc
-  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d);
-  // make it flat
-  for (size_t i = 0; i < cloud_2d->points.size(); i++)
-  {
-    cloud_2d->points[i].z = 0;
-  }
-
-  if (cloud_2d->points.size() > 0)
-    tree->setInputCloud(cloud_2d);
-
-  std::vector<pcl::PointIndices> cluster_indices;
-
-  // perform clustering on 2d cloud
-  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-  ec.setClusterTolerance(in_max_cluster_distance);  //
-  ec.setMinClusterSize(_cluster_size_min);
-  ec.setMaxClusterSize(_cluster_size_max);
-  ec.setSearchMethod(tree);
-  ec.setInputCloud(cloud_2d);
-  ec.extract(cluster_indices);
-  // use indices on 3d cloud
-
-  /////////////////////////////////
-  //---  3. Color clustered points
-  /////////////////////////////////
-  unsigned int k = 0;
-  // pcl::PointCloud<pcl::PointXYZRGB>::Ptr final_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
-
-  std::vector<ClusterPtr> clusters;
-  // pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);//coord + color
-  // cluster
-  for (auto it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
-  {
-    ClusterPtr cluster(new Cluster());
-    cluster->SetCloud(in_cloud_ptr, it->indices, _velodyne_header, k, (int) _colors[k].val[0],
-                      (int) _colors[k].val[1],
-                      (int) _colors[k].val[2], "", _pose_estimation);
-    clusters.push_back(cluster);
-
-    k++;
-  }
-  // std::cout << "Clusters: " << k << std::endl;
-  return clusters;
+std::vector<ClusterPtr>
+clusterAndColor(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+                pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud_ptr,
+                autoware_msgs::Centroids &in_out_centroids,
+                double in_max_cluster_distance = 0.5) {
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
+        new pcl::search::KdTree<pcl::PointXYZ>);
+
+    // create 2d pc
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(
+        new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d);
+    // make it flat
+    for (size_t i = 0; i < cloud_2d->points.size(); i++) {
+        cloud_2d->points[i].z = 0;
+    }
+
+    if (cloud_2d->points.size() > 0)
+        tree->setInputCloud(cloud_2d);
+
+    std::vector<pcl::PointIndices> cluster_indices;
+
+    // perform clustering on 2d cloud
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+    ec.setClusterTolerance(in_max_cluster_distance); //
+    ec.setMinClusterSize(_cluster_size_min);
+    ec.setMaxClusterSize(_cluster_size_max);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(cloud_2d);
+    ec.extract(cluster_indices);
+    // use indices on 3d cloud
+
+    /////////////////////////////////
+    //---  3. Color clustered points
+    /////////////////////////////////
+    unsigned int k = 0;
+    // pcl::PointCloud<pcl::PointXYZRGB>::Ptr final_cluster (new
+    // pcl::PointCloud<pcl::PointXYZRGB>);
+
+    std::vector<ClusterPtr> clusters;
+    // pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new
+    // pcl::PointCloud<pcl::PointXYZRGB>);//coord + color cluster
+    for (auto it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
+        ClusterPtr cluster(new Cluster());
+        cluster->SetCloud(in_cloud_ptr, it->indices, _velodyne_header, k,
+                          (int)_colors[k].val[0], (int)_colors[k].val[1],
+                          (int)_colors[k].val[2], "", _pose_estimation);
+        clusters.push_back(cluster);
+
+        k++;
+    }
+    // std::cout << "Clusters: " << k << std::endl;
+    return clusters;
 }
 
-void checkClusterMerge(size_t in_cluster_id, std::vector<ClusterPtr> &in_clusters,
-                       std::vector<bool> &in_out_visited_clusters, std::vector<size_t> &out_merge_indices,
-                       double in_merge_threshold)
-{
-  // std::cout << "checkClusterMerge" << std::endl;
-  pcl::PointXYZ point_a = in_clusters[in_cluster_id]->GetCentroid();
-  for (size_t i = 0; i < in_clusters.size(); i++)
-  {
-    if (i != in_cluster_id && !in_out_visited_clusters[i])
-    {
-      pcl::PointXYZ point_b = in_clusters[i]->GetCentroid();
-      double distance = sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2));
-      if (distance <= in_merge_threshold)
-      {
-        in_out_visited_clusters[i] = true;
-        out_merge_indices.push_back(i);
-        // std::cout << "Merging " << in_cluster_id << " with " << i << " dist:" << distance << std::endl;
-        checkClusterMerge(i, in_clusters, in_out_visited_clusters, out_merge_indices, in_merge_threshold);
-      }
+void checkClusterMerge(size_t in_cluster_id,
+                       std::vector<ClusterPtr> &in_clusters,
+                       std::vector<bool> &in_out_visited_clusters,
+                       std::vector<size_t> &out_merge_indices,
+                       double in_merge_threshold) {
+    // std::cout << "checkClusterMerge" << std::endl;
+    pcl::PointXYZ point_a = in_clusters[in_cluster_id]->GetCentroid();
+    for (size_t i = 0; i < in_clusters.size(); i++) {
+        if (i != in_cluster_id && !in_out_visited_clusters[i]) {
+            pcl::PointXYZ point_b = in_clusters[i]->GetCentroid();
+            double distance = sqrt(pow(point_b.x - point_a.x, 2) +
+                                   pow(point_b.y - point_a.y, 2));
+            if (distance <= in_merge_threshold) {
+                in_out_visited_clusters[i] = true;
+                out_merge_indices.push_back(i);
+                // std::cout << "Merging " << in_cluster_id << " with " << i <<
+                // " dist:" << distance << std::endl;
+                checkClusterMerge(i, in_clusters, in_out_visited_clusters,
+                                  out_merge_indices, in_merge_threshold);
+            }
+        }
     }
-  }
 }
 
-void mergeClusters(const std::vector<ClusterPtr> &in_clusters, std::vector<ClusterPtr> &out_clusters,
-                   std::vector<size_t> in_merge_indices, const size_t &current_index,
-                   std::vector<bool> &in_out_merged_clusters)
-{
-  // std::cout << "mergeClusters:" << in_merge_indices.size() << std::endl;
-  pcl::PointCloud<pcl::PointXYZRGB> sum_cloud;
-  pcl::PointCloud<pcl::PointXYZ> mono_cloud;
-  ClusterPtr merged_cluster(new Cluster());
-  for (size_t i = 0; i < in_merge_indices.size(); i++)
-  {
-    sum_cloud += *(in_clusters[in_merge_indices[i]]->GetCloud());
-    in_out_merged_clusters[in_merge_indices[i]] = true;
-  }
-  std::vector<int> indices(sum_cloud.points.size(), 0);
-  for (size_t i = 0; i < sum_cloud.points.size(); i++)
-  {
-    indices[i] = i;
-  }
-
-  if (sum_cloud.points.size() > 0)
-  {
-    pcl::copyPointCloud(sum_cloud, mono_cloud);
-    merged_cluster->SetCloud(mono_cloud.makeShared(), indices, _velodyne_header, current_index,
-                             (int) _colors[current_index].val[0], (int) _colors[current_index].val[1],
-                             (int) _colors[current_index].val[2], "", _pose_estimation);
-    out_clusters.push_back(merged_cluster);
-  }
+void mergeClusters(const std::vector<ClusterPtr> &in_clusters,
+                   std::vector<ClusterPtr> &out_clusters,
+                   std::vector<size_t> in_merge_indices,
+                   const size_t &current_index,
+                   std::vector<bool> &in_out_merged_clusters) {
+    // std::cout << "mergeClusters:" << in_merge_indices.size() << std::endl;
+    pcl::PointCloud<pcl::PointXYZRGB> sum_cloud;
+    pcl::PointCloud<pcl::PointXYZ> mono_cloud;
+    ClusterPtr merged_cluster(new Cluster());
+    for (size_t i = 0; i < in_merge_indices.size(); i++) {
+        sum_cloud += *(in_clusters[in_merge_indices[i]]->GetCloud());
+        in_out_merged_clusters[in_merge_indices[i]] = true;
+    }
+    std::vector<int> indices(sum_cloud.points.size(), 0);
+    for (size_t i = 0; i < sum_cloud.points.size(); i++) {
+        indices[i] = i;
+    }
+
+    if (sum_cloud.points.size() > 0) {
+        pcl::copyPointCloud(sum_cloud, mono_cloud);
+        merged_cluster->SetCloud(
+            mono_cloud.makeShared(), indices, _velodyne_header, current_index,
+            (int)_colors[current_index].val[0],
+            (int)_colors[current_index].val[1],
+            (int)_colors[current_index].val[2], "", _pose_estimation);
+        out_clusters.push_back(merged_cluster);
+    }
 }
 
-void checkAllForMerge(std::vector<ClusterPtr> &in_clusters, std::vector<ClusterPtr> &out_clusters,
-                      float in_merge_threshold)
-{
-  // std::cout << "checkAllForMerge" << std::endl;
-  std::vector<bool> visited_clusters(in_clusters.size(), false);
-  std::vector<bool> merged_clusters(in_clusters.size(), false);
-  size_t current_index = 0;
-  for (size_t i = 0; i < in_clusters.size(); i++)
-  {
-    if (!visited_clusters[i])
-    {
-      visited_clusters[i] = true;
-      std::vector<size_t> merge_indices;
-      checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, in_merge_threshold);
-      mergeClusters(in_clusters, out_clusters, merge_indices, current_index++, merged_clusters);
+void checkAllForMerge(std::vector<ClusterPtr> &in_clusters,
+                      std::vector<ClusterPtr> &out_clusters,
+                      float in_merge_threshold) {
+    // std::cout << "checkAllForMerge" << std::endl;
+    std::vector<bool> visited_clusters(in_clusters.size(), false);
+    std::vector<bool> merged_clusters(in_clusters.size(), false);
+    size_t current_index = 0;
+    for (size_t i = 0; i < in_clusters.size(); i++) {
+        if (!visited_clusters[i]) {
+            visited_clusters[i] = true;
+            std::vector<size_t> merge_indices;
+            checkClusterMerge(i, in_clusters, visited_clusters, merge_indices,
+                              in_merge_threshold);
+            mergeClusters(in_clusters, out_clusters, merge_indices,
+                          current_index++, merged_clusters);
+        }
     }
-  }
-  for (size_t i = 0; i < in_clusters.size(); i++)
-  {
-    // check for clusters not merged, add them to the output
-    if (!merged_clusters[i])
-    {
-      out_clusters.push_back(in_clusters[i]);
+    for (size_t i = 0; i < in_clusters.size(); i++) {
+        // check for clusters not merged, add them to the output
+        if (!merged_clusters[i]) {
+            out_clusters.push_back(in_clusters[i]);
+        }
     }
-  }
 
-  // ClusterPtr cluster(new Cluster());
+    // ClusterPtr cluster(new Cluster());
 }
 
 void segmentByDistance(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                        pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud_ptr,
-                       autoware_msgs::Centroids &in_out_centroids, autoware_msgs::CloudClusterArray &in_out_clusters)
-{
-  // cluster the pointcloud according to the distance of the points using different thresholds (not only one for the
-  // entire pc)
-  // in this way, the points farther in the pc will also be clustered
-
-  // 0 => 0-15m d=0.5
-  // 1 => 15-30 d=1
-  // 2 => 30-45 d=1.6
-  // 3 => 45-60 d=2.1
-  // 4 => >60   d=2.6
-
-  std::vector<ClusterPtr> all_clusters;
-
-  if (!_use_multiple_thres)
-  {
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-
-    for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
-    {
-      pcl::PointXYZ current_point;
-      current_point.x = in_cloud_ptr->points[i].x;
-      current_point.y = in_cloud_ptr->points[i].y;
-      current_point.z = in_cloud_ptr->points[i].z;
-
-      cloud_ptr->points.push_back(current_point);
-    }
+                       autoware_msgs::Centroids &in_out_centroids,
+                       autoware_msgs::CloudClusterArray &in_out_clusters) {
+    // cluster the pointcloud according to the distance of the points using
+    // different thresholds (not only one for the entire pc) in this way, the
+    // points farther in the pc will also be clustered
+
+    // 0 => 0-15m d=0.5
+    // 1 => 15-30 d=1
+    // 2 => 30-45 d=1.6
+    // 3 => 45-60 d=2.1
+    // 4 => >60   d=2.6
+
+    std::vector<ClusterPtr> all_clusters;
+
+    if (!_use_multiple_thres) {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+
+        for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++) {
+            pcl::PointXYZ current_point;
+            current_point.x = in_cloud_ptr->points[i].x;
+            current_point.y = in_cloud_ptr->points[i].y;
+            current_point.z = in_cloud_ptr->points[i].z;
+
+            cloud_ptr->points.push_back(current_point);
+        }
 #ifdef GPU_CLUSTERING
-    if (_use_gpu)
-    {
-      all_clusters = clusterAndColorGpu(cloud_ptr, out_cloud_ptr, in_out_centroids,
-                                        _clustering_distance);
-    } else
-    {
-      all_clusters =
-        clusterAndColor(cloud_ptr, out_cloud_ptr, in_out_centroids, _clustering_distance);
-    }
+        if (_use_gpu) {
+            all_clusters =
+                clusterAndColorGpu(cloud_ptr, out_cloud_ptr, in_out_centroids,
+                                   _clustering_distance);
+        } else {
+            all_clusters =
+                clusterAndColor(cloud_ptr, out_cloud_ptr, in_out_centroids,
+                                _clustering_distance);
+        }
 #else
-    all_clusters =
-        clusterAndColor(cloud_ptr, out_cloud_ptr, in_out_centroids, _clustering_distance);
+        all_clusters = clusterAndColor(cloud_ptr, out_cloud_ptr,
+                                       in_out_centroids, _clustering_distance);
 #endif
-  } else
-  {
-    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_segments_array(5);
-    for (unsigned int i = 0; i < cloud_segments_array.size(); i++)
-    {
-      pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-      cloud_segments_array[i] = tmp_cloud;
-    }
+    } else {
+        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_segments_array(
+            5);
+        for (unsigned int i = 0; i < cloud_segments_array.size(); i++) {
+            pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(
+                new pcl::PointCloud<pcl::PointXYZ>);
+            cloud_segments_array[i] = tmp_cloud;
+        }
 
-    for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
-    {
-      pcl::PointXYZ current_point;
-      current_point.x = in_cloud_ptr->points[i].x;
-      current_point.y = in_cloud_ptr->points[i].y;
-      current_point.z = in_cloud_ptr->points[i].z;
-
-      float origin_distance = sqrt(pow(current_point.x, 2) + pow(current_point.y, 2));
-
-      if (origin_distance < _clustering_ranges[0])
-      {
-        cloud_segments_array[0]->points.push_back(current_point);
-      }
-      else if (origin_distance < _clustering_ranges[1])
-      {
-        cloud_segments_array[1]->points.push_back(current_point);
-
-      }else if (origin_distance < _clustering_ranges[2])
-      {
-        cloud_segments_array[2]->points.push_back(current_point);
-
-      }else if (origin_distance < _clustering_ranges[3])
-      {
-        cloud_segments_array[3]->points.push_back(current_point);
-
-      }else
-      {
-        cloud_segments_array[4]->points.push_back(current_point);
-      }
-    }
+        for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++) {
+            pcl::PointXYZ current_point;
+            current_point.x = in_cloud_ptr->points[i].x;
+            current_point.y = in_cloud_ptr->points[i].y;
+            current_point.z = in_cloud_ptr->points[i].z;
+
+            float origin_distance =
+                sqrt(pow(current_point.x, 2) + pow(current_point.y, 2));
+
+            if (origin_distance < _clustering_ranges[0]) {
+                cloud_segments_array[0]->points.push_back(current_point);
+            } else if (origin_distance < _clustering_ranges[1]) {
+                cloud_segments_array[1]->points.push_back(current_point);
 
-    std::vector<ClusterPtr> local_clusters;
-    for (unsigned int i = 0; i < cloud_segments_array.size(); i++)
-    {
+            } else if (origin_distance < _clustering_ranges[2]) {
+                cloud_segments_array[2]->points.push_back(current_point);
+
+            } else if (origin_distance < _clustering_ranges[3]) {
+                cloud_segments_array[3]->points.push_back(current_point);
+
+            } else {
+                cloud_segments_array[4]->points.push_back(current_point);
+            }
+        }
+
+        std::vector<ClusterPtr> local_clusters;
+        for (unsigned int i = 0; i < cloud_segments_array.size(); i++) {
 #ifdef GPU_CLUSTERING
-      if (_use_gpu)
-      {
-        local_clusters = clusterAndColorGpu(cloud_segments_array[i], out_cloud_ptr,
-                                            in_out_centroids, _clustering_distances[i]);
-      } else
-      {
-        local_clusters = clusterAndColor(cloud_segments_array[i], out_cloud_ptr,
-                                         in_out_centroids, _clustering_distances[i]);
-      }
+            if (_use_gpu) {
+                local_clusters = clusterAndColorGpu(
+                    cloud_segments_array[i], out_cloud_ptr, in_out_centroids,
+                    _clustering_distances[i]);
+            } else {
+                local_clusters =
+                    clusterAndColor(cloud_segments_array[i], out_cloud_ptr,
+                                    in_out_centroids, _clustering_distances[i]);
+            }
 #else
-      local_clusters = clusterAndColor(
-          cloud_segments_array[i], out_cloud_ptr, in_out_centroids, _clustering_distances[i]);
+            local_clusters =
+                clusterAndColor(cloud_segments_array[i], out_cloud_ptr,
+                                in_out_centroids, _clustering_distances[i]);
 #endif
-      all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end());
+            all_clusters.insert(all_clusters.end(), local_clusters.begin(),
+                                local_clusters.end());
+        }
     }
-  }
 
-  // Clusters can be merged or checked in here
-  //....
-  // check for mergable clusters
-  std::vector<ClusterPtr> mid_clusters;
-  std::vector<ClusterPtr> final_clusters;
+    // Clusters can be merged or checked in here
+    //....
+    // check for mergable clusters
+    std::vector<ClusterPtr> mid_clusters;
+    std::vector<ClusterPtr> final_clusters;
 
-  if (all_clusters.size() > 0)
-    checkAllForMerge(all_clusters, mid_clusters, _cluster_merge_threshold);
-  else
-    mid_clusters = all_clusters;
+    if (all_clusters.size() > 0)
+        checkAllForMerge(all_clusters, mid_clusters, _cluster_merge_threshold);
+    else
+        mid_clusters = all_clusters;
 
-  if (mid_clusters.size() > 0)
-    checkAllForMerge(mid_clusters, final_clusters, _cluster_merge_threshold);
-  else
-    final_clusters = mid_clusters;
+    if (mid_clusters.size() > 0)
+        checkAllForMerge(mid_clusters, final_clusters,
+                         _cluster_merge_threshold);
+    else
+        final_clusters = mid_clusters;
 
     // Get final PointCloud to be published
-    for (unsigned int i = 0; i < final_clusters.size(); i++)
-    {
-      *out_cloud_ptr = *out_cloud_ptr + *(final_clusters[i]->GetCloud());
-
-      jsk_recognition_msgs::BoundingBox bounding_box = final_clusters[i]->GetBoundingBox();
-      geometry_msgs::PolygonStamped polygon = final_clusters[i]->GetPolygon();
-      jsk_rviz_plugins::Pictogram pictogram_cluster;
-      pictogram_cluster.header = _velodyne_header;
-
-      // PICTO
-      pictogram_cluster.mode = pictogram_cluster.STRING_MODE;
-      pictogram_cluster.pose.position.x = final_clusters[i]->GetMaxPoint().x;
-      pictogram_cluster.pose.position.y = final_clusters[i]->GetMaxPoint().y;
-      pictogram_cluster.pose.position.z = final_clusters[i]->GetMaxPoint().z;
-      tf::Quaternion quat(0.0, -0.7, 0.0, 0.7);
-      tf::quaternionTFToMsg(quat, pictogram_cluster.pose.orientation);
-      pictogram_cluster.size = 4;
-      std_msgs::ColorRGBA color;
-      color.a = 1;
-      color.r = 1;
-      color.g = 1;
-      color.b = 1;
-      pictogram_cluster.color = color;
-      pictogram_cluster.character = std::to_string(i);
-      // PICTO
-
-      // pcl::PointXYZ min_point = final_clusters[i]->GetMinPoint();
-      // pcl::PointXYZ max_point = final_clusters[i]->GetMaxPoint();
-      pcl::PointXYZ center_point = final_clusters[i]->GetCentroid();
-      geometry_msgs::Point centroid;
-      centroid.x = center_point.x;
-      centroid.y = center_point.y;
-      centroid.z = center_point.z;
-      bounding_box.header = _velodyne_header;
-      polygon.header = _velodyne_header;
-
-      if (final_clusters[i]->IsValid())
-      {
-
-        in_out_centroids.points.push_back(centroid);
-
-        autoware_msgs::CloudCluster cloud_cluster;
-        final_clusters[i]->ToROSMessage(_velodyne_header, cloud_cluster);
-        in_out_clusters.clusters.push_back(cloud_cluster);
-      }
+    for (unsigned int i = 0; i < final_clusters.size(); i++) {
+        *out_cloud_ptr = *out_cloud_ptr + *(final_clusters[i]->GetCloud());
+
+        jsk_recognition_msgs::BoundingBox bounding_box =
+            final_clusters[i]->GetBoundingBox();
+        geometry_msgs::PolygonStamped polygon = final_clusters[i]->GetPolygon();
+
+        // pcl::PointXYZ min_point = final_clusters[i]->GetMinPoint();
+        // pcl::PointXYZ max_point = final_clusters[i]->GetMaxPoint();
+        pcl::PointXYZ center_point = final_clusters[i]->GetCentroid();
+        geometry_msgs::Point centroid;
+        centroid.x = center_point.x;
+        centroid.y = center_point.y;
+        centroid.z = center_point.z;
+        bounding_box.header = _velodyne_header;
+        polygon.header = _velodyne_header;
+
+        if (final_clusters[i]->IsValid()) {
+
+            in_out_centroids.points.push_back(centroid);
+
+            autoware_msgs::CloudCluster cloud_cluster;
+            final_clusters[i]->ToROSMessage(_velodyne_header, cloud_cluster);
+            in_out_clusters.clusters.push_back(cloud_cluster);
+        }
     }
 }
 
 void removeFloor(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                  pcl::PointCloud<pcl::PointXYZ>::Ptr out_nofloor_cloud_ptr,
-                 pcl::PointCloud<pcl::PointXYZ>::Ptr out_onlyfloor_cloud_ptr, float in_max_height = 0.2,
-                 float in_floor_max_angle = 0.1)
-{
-
-  pcl::SACSegmentation<pcl::PointXYZ> seg;
-  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
-  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
-
-  seg.setOptimizeCoefficients(true);
-  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
-  seg.setMethodType(pcl::SAC_RANSAC);
-  seg.setMaxIterations(100);
-  seg.setAxis(Eigen::Vector3f(0, 0, 1));
-  seg.setEpsAngle(in_floor_max_angle);
-
-  seg.setDistanceThreshold(in_max_height);  // floor distance
-  seg.setOptimizeCoefficients(true);
-  seg.setInputCloud(in_cloud_ptr);
-  seg.segment(*inliers, *coefficients);
-  if (inliers->indices.size() == 0)
-  {
-    std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
-  }
-
-  // REMOVE THE FLOOR FROM THE CLOUD
-  pcl::ExtractIndices<pcl::PointXYZ> extract;
-  extract.setInputCloud(in_cloud_ptr);
-  extract.setIndices(inliers);
-  extract.setNegative(true);  // true removes the indices, false leaves only the indices
-  extract.filter(*out_nofloor_cloud_ptr);
-
-  // EXTRACT THE FLOOR FROM THE CLOUD
-  extract.setNegative(false);  // true removes the indices, false leaves only the indices
-  extract.filter(*out_onlyfloor_cloud_ptr);
+                 pcl::PointCloud<pcl::PointXYZ>::Ptr out_onlyfloor_cloud_ptr,
+                 float in_max_height = 0.2, float in_floor_max_angle = 0.1) {
+
+    pcl::SACSegmentation<pcl::PointXYZ> seg;
+    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
+    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
+
+    seg.setOptimizeCoefficients(true);
+    seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
+    seg.setMethodType(pcl::SAC_RANSAC);
+    seg.setMaxIterations(100);
+    seg.setAxis(Eigen::Vector3f(0, 0, 1));
+    seg.setEpsAngle(in_floor_max_angle);
+
+    seg.setDistanceThreshold(in_max_height); // floor distance
+    seg.setOptimizeCoefficients(true);
+    seg.setInputCloud(in_cloud_ptr);
+    seg.segment(*inliers, *coefficients);
+    if (inliers->indices.size() == 0) {
+        std::cout << "Could not estimate a planar model for the given dataset."
+                  << std::endl;
+    }
+
+    // REMOVE THE FLOOR FROM THE CLOUD
+    pcl::ExtractIndices<pcl::PointXYZ> extract;
+    extract.setInputCloud(in_cloud_ptr);
+    extract.setIndices(inliers);
+    extract.setNegative(
+        true); // true removes the indices, false leaves only the indices
+    extract.filter(*out_nofloor_cloud_ptr);
+
+    // EXTRACT THE FLOOR FROM THE CLOUD
+    extract.setNegative(
+        false); // true removes the indices, false leaves only the indices
+    extract.filter(*out_onlyfloor_cloud_ptr);
 }
 
 void downsampleCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
-                     pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, float in_leaf_size = 0.2)
-{
-  pcl::VoxelGrid<pcl::PointXYZ> sor;
-  sor.setInputCloud(in_cloud_ptr);
-  sor.setLeafSize((float) in_leaf_size, (float) in_leaf_size, (float) in_leaf_size);
-  sor.filter(*out_cloud_ptr);
+                     pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+                     float in_leaf_size = 0.2) {
+    pcl::VoxelGrid<pcl::PointXYZ> sor;
+    sor.setInputCloud(in_cloud_ptr);
+    sor.setLeafSize((float)in_leaf_size, (float)in_leaf_size,
+                    (float)in_leaf_size);
+    sor.filter(*out_cloud_ptr);
 }
 
 void clipCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
-               pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, float in_min_height = -1.3, float in_max_height = 0.5)
-{
-  out_cloud_ptr->points.clear();
-  for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
-  {
-    if (in_cloud_ptr->points[i].z >= in_min_height && in_cloud_ptr->points[i].z <= in_max_height)
-    {
-      out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
+               pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+               float in_min_height = -1.3, float in_max_height = 0.5) {
+    out_cloud_ptr->points.clear();
+    for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++) {
+        if (in_cloud_ptr->points[i].z >= in_min_height &&
+            in_cloud_ptr->points[i].z <= in_max_height) {
+            out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
+        }
     }
-  }
 }
 
-void differenceNormalsSegmentation(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
-                                   pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr)
-{
-  float small_scale = 0.5;
-  float large_scale = 2.0;
-  float angle_threshold = 0.5;
-  pcl::search::Search<pcl::PointXYZ>::Ptr tree;
-  if (in_cloud_ptr->isOrganized())
-  {
-    tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
-  } else
-  {
-    tree.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
-  }
+void differenceNormalsSegmentation(
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr) {
+    float small_scale = 0.5;
+    float large_scale = 2.0;
+    float angle_threshold = 0.5;
+    pcl::search::Search<pcl::PointXYZ>::Ptr tree;
+    if (in_cloud_ptr->isOrganized()) {
+        tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
+    } else {
+        tree.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
+    }
 
-  // Set the input pointcloud for the search tree
-  tree->setInputCloud(in_cloud_ptr);
+    // Set the input pointcloud for the search tree
+    tree->setInputCloud(in_cloud_ptr);
+
+    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::PointNormal> normal_estimation;
+    // pcl::gpu::NormalEstimation<pcl::PointXYZ, pcl::PointNormal>
+    // normal_estimation;
+    normal_estimation.setInputCloud(in_cloud_ptr);
+    normal_estimation.setSearchMethod(tree);
+
+    normal_estimation.setViewPoint(std::numeric_limits<float>::max(),
+                                   std::numeric_limits<float>::max(),
+                                   std::numeric_limits<float>::max());
+
+    pcl::PointCloud<pcl::PointNormal>::Ptr normals_small_scale(
+        new pcl::PointCloud<pcl::PointNormal>);
+    pcl::PointCloud<pcl::PointNormal>::Ptr normals_large_scale(
+        new pcl::PointCloud<pcl::PointNormal>);
+
+    normal_estimation.setRadiusSearch(small_scale);
+    normal_estimation.compute(*normals_small_scale);
+
+    normal_estimation.setRadiusSearch(large_scale);
+    normal_estimation.compute(*normals_large_scale);
+
+    pcl::PointCloud<pcl::PointNormal>::Ptr diffnormals_cloud(
+        new pcl::PointCloud<pcl::PointNormal>);
+    pcl::copyPointCloud<pcl::PointXYZ, pcl::PointNormal>(*in_cloud_ptr,
+                                                         *diffnormals_cloud);
+
+    // Create DoN operator
+    pcl::DifferenceOfNormalsEstimation<pcl::PointXYZ, pcl::PointNormal,
+                                       pcl::PointNormal>
+        diffnormals_estimator;
+    diffnormals_estimator.setInputCloud(in_cloud_ptr);
+    diffnormals_estimator.setNormalScaleLarge(normals_large_scale);
+    diffnormals_estimator.setNormalScaleSmall(normals_small_scale);
+
+    diffnormals_estimator.initCompute();
+
+    diffnormals_estimator.computeFeature(*diffnormals_cloud);
+
+    pcl::ConditionOr<pcl::PointNormal>::Ptr range_cond(
+        new pcl::ConditionOr<pcl::PointNormal>());
+    range_cond->addComparison(pcl::FieldComparison<pcl::PointNormal>::ConstPtr(
+        new pcl::FieldComparison<pcl::PointNormal>(
+            "curvature", pcl::ComparisonOps::GT, angle_threshold)));
+    // Build the filter
+    pcl::ConditionalRemoval<pcl::PointNormal> cond_removal;
+    cond_removal.setCondition(range_cond);
+    cond_removal.setInputCloud(diffnormals_cloud);
+
+    pcl::PointCloud<pcl::PointNormal>::Ptr diffnormals_cloud_filtered(
+        new pcl::PointCloud<pcl::PointNormal>);
+
+    // Apply filter
+    cond_removal.filter(*diffnormals_cloud_filtered);
+
+    pcl::copyPointCloud<pcl::PointNormal, pcl::PointXYZ>(*diffnormals_cloud,
+                                                         *out_cloud_ptr);
+}
 
-  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::PointNormal> normal_estimation;
-  // pcl::gpu::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> normal_estimation;
-  normal_estimation.setInputCloud(in_cloud_ptr);
-  normal_estimation.setSearchMethod(tree);
+void removePointsUpTo(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+                      pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+                      const double in_distance) {
+    out_cloud_ptr->points.clear();
+    for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++) {
+        float origin_distance = sqrt(pow(in_cloud_ptr->points[i].x, 2) +
+                                     pow(in_cloud_ptr->points[i].y, 2));
+        if (origin_distance > in_distance) {
+            out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
+        }
+    }
+}
 
-  normal_estimation.setViewPoint(std::numeric_limits<float>::max(), std::numeric_limits<float>::max(),
-                                 std::numeric_limits<float>::max());
+void velodyne_callback(const rubis_msgs::PointCloud2ConstPtr &in_sensor_cloud) {
+    rubis::start_task_profiling();
+    rubis::instance_ = in_sensor_cloud->instance;
+    rubis::lidar_instance_ = in_sensor_cloud->instance;
+    //_start = std::chrono::system_clock::now();
+    if (!_using_sensor_cloud) {
+        _using_sensor_cloud = true;
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr current_sensor_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr removed_points_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr inlanes_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr nofloor_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr onlyfloor_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr diffnormals_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr clipped_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_clustered_cloud_ptr(
+            new pcl::PointCloud<pcl::PointXYZRGB>);
+
+        autoware_msgs::Centroids centroids;
+        autoware_msgs::CloudClusterArray cloud_clusters;
+
+        pcl::fromROSMsg(in_sensor_cloud->msg, *current_sensor_cloud_ptr);
+
+        _velodyne_header = in_sensor_cloud->msg.header;
+
+        if (_remove_points_upto > 0.0) {
+            removePointsUpTo(current_sensor_cloud_ptr, removed_points_cloud_ptr,
+                             _remove_points_upto);
+        } else {
+            removed_points_cloud_ptr = current_sensor_cloud_ptr;
+        }
 
-  pcl::PointCloud<pcl::PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<pcl::PointNormal>);
-  pcl::PointCloud<pcl::PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<pcl::PointNormal>);
+        if (_downsample_cloud)
+            downsampleCloud(removed_points_cloud_ptr, downsampled_cloud_ptr,
+                            _leaf_size);
+        else
+            downsampled_cloud_ptr = removed_points_cloud_ptr;
 
-  normal_estimation.setRadiusSearch(small_scale);
-  normal_estimation.compute(*normals_small_scale);
+        clipCloud(downsampled_cloud_ptr, clipped_cloud_ptr, _clip_min_height,
+                  _clip_max_height);
 
-  normal_estimation.setRadiusSearch(large_scale);
-  normal_estimation.compute(*normals_large_scale);
+        if (_keep_lanes)
+            keepLanePoints(clipped_cloud_ptr, inlanes_cloud_ptr,
+                           _keep_lane_left_distance, _keep_lane_right_distance);
+        else
+            inlanes_cloud_ptr = clipped_cloud_ptr;
+
+        if (_remove_ground) {
+            removeFloor(inlanes_cloud_ptr, nofloor_cloud_ptr,
+                        onlyfloor_cloud_ptr);
+            publishCloud(&_pub_ground_cloud, onlyfloor_cloud_ptr);
+        } else {
+            nofloor_cloud_ptr = inlanes_cloud_ptr;
+        }
 
-  pcl::PointCloud<pcl::PointNormal>::Ptr diffnormals_cloud(new pcl::PointCloud<pcl::PointNormal>);
-  pcl::copyPointCloud<pcl::PointXYZ, pcl::PointNormal>(*in_cloud_ptr, *diffnormals_cloud);
+        publishCloud(&_pub_points_lanes_cloud, nofloor_cloud_ptr);
 
-  // Create DoN operator
-  pcl::DifferenceOfNormalsEstimation<pcl::PointXYZ, pcl::PointNormal, pcl::PointNormal> diffnormals_estimator;
-  diffnormals_estimator.setInputCloud(in_cloud_ptr);
-  diffnormals_estimator.setNormalScaleLarge(normals_large_scale);
-  diffnormals_estimator.setNormalScaleSmall(normals_small_scale);
+        if (_use_diffnormals)
+            differenceNormalsSegmentation(nofloor_cloud_ptr,
+                                          diffnormals_cloud_ptr);
+        else
+            diffnormals_cloud_ptr = nofloor_cloud_ptr;
 
-  diffnormals_estimator.initCompute();
+        segmentByDistance(diffnormals_cloud_ptr, colored_clustered_cloud_ptr,
+                          centroids, cloud_clusters);
 
-  diffnormals_estimator.computeFeature(*diffnormals_cloud);
+        publishColorCloud(&_pub_cluster_cloud, colored_clustered_cloud_ptr);
 
-  pcl::ConditionOr<pcl::PointNormal>::Ptr range_cond(new pcl::ConditionOr<pcl::PointNormal>());
-  range_cond->addComparison(pcl::FieldComparison<pcl::PointNormal>::ConstPtr(
-    new pcl::FieldComparison<pcl::PointNormal>("curvature", pcl::ComparisonOps::GT, angle_threshold)));
-  // Build the filter
-  pcl::ConditionalRemoval<pcl::PointNormal> cond_removal;
-  cond_removal.setCondition(range_cond);
-  cond_removal.setInputCloud(diffnormals_cloud);
+        centroids.header = _velodyne_header;
 
-  pcl::PointCloud<pcl::PointNormal>::Ptr diffnormals_cloud_filtered(new pcl::PointCloud<pcl::PointNormal>);
+        publishCentroids(&_centroid_pub, centroids, _output_frame,
+                         _velodyne_header);
 
-  // Apply filter
-  cond_removal.filter(*diffnormals_cloud_filtered);
+        cloud_clusters.header = _velodyne_header;
 
-  pcl::copyPointCloud<pcl::PointNormal, pcl::PointXYZ>(*diffnormals_cloud, *out_cloud_ptr);
-}
+        publishCloudClusters(&_pub_clusters_message, cloud_clusters,
+                             _output_frame, _velodyne_header);
 
-void removePointsUpTo(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
-                      pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, const double in_distance)
-{
-  out_cloud_ptr->points.clear();
-  for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
-  {
-    float origin_distance = sqrt(pow(in_cloud_ptr->points[i].x, 2) + pow(in_cloud_ptr->points[i].y, 2));
-    if (origin_distance > in_distance)
-    {
-      out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
+        _using_sensor_cloud = false;
     }
-  }
-}
 
-void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud)
-{
-  //_start = std::chrono::system_clock::now();  
-  if (!_using_sensor_cloud)
-  {
-    _using_sensor_cloud = true;
-
-    pcl::PointCloud<pcl::PointXYZ>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr removed_points_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr inlanes_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr nofloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr onlyfloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr diffnormals_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr clipped_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_clustered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-    autoware_msgs::Centroids centroids;
-    autoware_msgs::CloudClusterArray cloud_clusters;
-
-    pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr);
-
-    _velodyne_header = in_sensor_cloud->header;
-
-    if (_remove_points_upto > 0.0)
-    {
-      removePointsUpTo(current_sensor_cloud_ptr, removed_points_cloud_ptr, _remove_points_upto);
-    }
-    else
-    {
-      removed_points_cloud_ptr = current_sensor_cloud_ptr;
-    }
-
-    if (_downsample_cloud)
-      downsampleCloud(removed_points_cloud_ptr, downsampled_cloud_ptr, _leaf_size);
-    else
-      downsampled_cloud_ptr = removed_points_cloud_ptr;
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_,
+                               rubis::vision_instance_);
+}
 
-    clipCloud(downsampled_cloud_ptr, clipped_cloud_ptr, _clip_min_height, _clip_max_height);
+int main(int argc, char **argv) {
+    // Initialize ROS
+    ros::init(argc, argv, "euclidean_cluster");
+
+    ros::NodeHandle h;
+    ros::NodeHandle private_nh("~");
+
+    // Scheduling & Profiling Setup
+    std::string node_name = ros::this_node::getName();
+    std::string task_response_time_filename;
+    private_nh.param<std::string>(node_name + "/task_response_time_filename",
+                                  task_response_time_filename,
+                                  "~/Documents/profiling/response_time/"
+                                  "lidar_euclidean_cluster_detect.csv");
+
+    int rate;
+    private_nh.param<int>(node_name + "/rate", rate, 10);
+
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time, deadline, period;
+
+    private_nh.param(node_name + "/task_scheduling_configs/policy", policy,
+                     std::string("NONE"));
+    private_nh.param(node_name + "/task_scheduling_configs/priority", priority,
+                     99);
+    private_nh.param(node_name + "/task_scheduling_configs/exec_time",
+                     exec_time, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/deadline", deadline,
+                     0);
+    private_nh.param(node_name + "/task_scheduling_configs/period", period, 0);
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);
+    rubis::init_task_scheduling(policy, attr);
+
+    rubis::init_task_profiling(task_response_time_filename);
+
+    tf::StampedTransform transform;
+    tf::TransformListener listener;
+    tf::TransformListener vectormap_tf_listener;
+
+    _vectormap_transform_listener = &vectormap_tf_listener;
+    _transform = &transform;
+    _transform_listener = &listener;
+
+    generateColors(_colors, 255);
+
+    std::string label;
+    std::string output_lane_str;
+    std::string output_cluster_str;
+    std::string output_objects_str;
+    std::string rubis_output_objects_str;
+    std::string point_cluster_str;
+    std::string cluster_centeroids_str;
+    std::string points_ground_str;
+
+    private_nh.param<std::string>("label", label, "center");
+    output_lane_str = std::string("/points_lanes_") + label;
+    output_cluster_str =
+        std::string("/detection/lidar_detector/cloud_clusters_") + label;
+    output_objects_str =
+        std::string("/detection/lidar_detector/objects_") + label;
+    rubis_output_objects_str =
+        std::string("/detection/lidar_detector/rubis_objects_") + label;
+    point_cluster_str = std::string("/points_cluster_") + label;
+    cluster_centeroids_str = std::string("/points_ground_") + label;
+    points_ground_str = std::string("/cluster_centroids_") + label;
+
+    _pub_cluster_cloud =
+        h.advertise<sensor_msgs::PointCloud2>(point_cluster_str.c_str(), 1);
+    _pub_ground_cloud = h.advertise<sensor_msgs::PointCloud2>(
+        cluster_centeroids_str.c_str(), 1);
+    _centroid_pub =
+        h.advertise<autoware_msgs::Centroids>(points_ground_str.c_str(), 1);
+    _pub_points_lanes_cloud =
+        h.advertise<sensor_msgs::PointCloud2>(output_lane_str.c_str(), 1);
+    _pub_clusters_message = h.advertise<autoware_msgs::CloudClusterArray>(
+        output_cluster_str.c_str(), 1);
+    _pub_detected_objects = h.advertise<autoware_msgs::DetectedObjectArray>(
+        output_objects_str.c_str(), 1);
+    _pub_rubis_detected_objects = h.advertise<rubis_msgs::DetectedObjectArray>(
+        rubis_output_objects_str.c_str(), 1);
+
+    std::string points_topic, gridmap_topic;
 
-    if (_keep_lanes)
-      keepLanePoints(clipped_cloud_ptr, inlanes_cloud_ptr, _keep_lane_left_distance, _keep_lane_right_distance);
-    else
-      inlanes_cloud_ptr = clipped_cloud_ptr;
+    _using_sensor_cloud = false;
 
-    if (_remove_ground)
-    {
-      removeFloor(inlanes_cloud_ptr, nofloor_cloud_ptr, onlyfloor_cloud_ptr);
-      publishCloud(&_pub_ground_cloud, onlyfloor_cloud_ptr);
+    if (private_nh.getParam("points_node", points_topic)) {
+        ROS_INFO("euclidean_cluster > Setting points node to %s",
+                 points_topic.c_str());
+    } else {
+        ROS_INFO("euclidean_cluster > No points node received, defaulting to "
+                 "points_raw, you can use "
+                 "_points_node:=YOUR_TOPIC");
+        points_topic = "/points_raw";
     }
-    else
-    {
-      nofloor_cloud_ptr = inlanes_cloud_ptr;
-    }
-
-    publishCloud(&_pub_points_lanes_cloud, nofloor_cloud_ptr);
-
-    if (_use_diffnormals)
-      differenceNormalsSegmentation(nofloor_cloud_ptr, diffnormals_cloud_ptr);
-    else
-      diffnormals_cloud_ptr = nofloor_cloud_ptr;
-
-    segmentByDistance(diffnormals_cloud_ptr, colored_clustered_cloud_ptr, centroids,
-                      cloud_clusters);
-
-    publishColorCloud(&_pub_cluster_cloud, colored_clustered_cloud_ptr);
-
-    centroids.header = _velodyne_header;
-
-    publishCentroids(&_centroid_pub, centroids, _output_frame, _velodyne_header);
 
-    cloud_clusters.header = _velodyne_header;
-
-    publishCloudClusters(&_pub_clusters_message, cloud_clusters, _output_frame, _velodyne_header);
+    _use_diffnormals = false;
+    if (private_nh.getParam("use_diffnormals", _use_diffnormals)) {
+        if (_use_diffnormals)
+            ROS_INFO("Euclidean Clustering: Applying difference of normals on "
+                     "clustering pipeline");
+        else
+            ROS_INFO("Euclidean Clustering: Difference of Normals will not be "
+                     "used.");
+    }
 
-    _using_sensor_cloud = false;
-  }
-}
-int main(int argc, char **argv)
-{
-  // Initialize ROS
-  ros::init(argc, argv, "euclidean_cluster");
-
-  ros::NodeHandle h;
-  ros::NodeHandle private_nh("~");
-
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
-
-  int gpu_scheduling_flag;
-  int gpu_profiling_flag;
-  std::string gpu_execution_time_filename;
-  std::string gpu_response_time_filename;
-  std::string gpu_deadline_filename;
-
-  private_nh.param<int>("/lidar_euclidean_cluster_detect/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/lidar_euclidean_cluster_detect/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/lidar_euclidean_cluster_detect/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv");
-  private_nh.param<int>("/lidar_euclidean_cluster_detect/rate", rate, 10);
-  private_nh.param("/lidar_euclidean_cluster_detect/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/lidar_euclidean_cluster_detect/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/lidar_euclidean_cluster_detect/task_relative_deadline", task_relative_deadline, (double)10);
-  private_nh.param("/lidar_euclidean_cluster_detect/gpu_scheduling_flag", gpu_scheduling_flag, 0);
-  private_nh.param("/lidar_euclidean_cluster_detect/gpu_profiling_flag", gpu_profiling_flag, 0);
-  private_nh.param<std::string>("/lidar_euclidean_cluster_detect/gpu_execution_time_filename", gpu_execution_time_filename, "~/Documents/gpu_profiling/test_clustering_execution_time.csv");
-  private_nh.param<std::string>("/lidar_euclidean_cluster_detect/gpu_response_time_filename", gpu_response_time_filename, "~/Documents/gpu_profiling/test_clustering_response_time.csv");
-  private_nh.param<std::string>("/lidar_euclidean_cluster_detect/gpu_deadline_filename", gpu_deadline_filename, "~/Documents/gpu_deadline/cluster_gpu_deadline.csv");
-
-  tf::StampedTransform transform;
-  tf::TransformListener listener;
-  tf::TransformListener vectormap_tf_listener;
-
-  _vectormap_transform_listener = &vectormap_tf_listener;
-  _transform = &transform;
-  _transform_listener = &listener;
-
-  generateColors(_colors, 255);
-
-
-  /* For GPU scheduling */
-  #ifdef GPU_CLUSTERING
-  int key_id = 1;    
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-  if(gpu_profiling_flag) rubis::sched::init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename);
-  
-  if(gpu_scheduling_flag){
-    rubis::sched::init_gpu_scheduling("/tmp/cluster", gpu_deadline_filename, key_id);
-  }    
-  else if(gpu_scheduling_flag){
-    ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!");
-    exit(1);
-  }
-  #endif
-
-
-  std::string label;
-  std::string output_lane_str;
-  std::string output_cluster_str;
-  std::string output_objects_str;
-  std::string point_cluster_str;
-  std::string cluster_centeroids_str;
-  std::string points_ground_str;
-
-  private_nh.param<std::string>("label", label, "center");
-  output_lane_str = std::string("/points_lanes_")+label;
-  output_cluster_str = std::string("/detection/lidar_detector/cloud_clusters_")+label;
-  output_objects_str = std::string("/detection/lidar_detector/objects_")+label;
-  point_cluster_str = std::string("/points_cluster_")+label;
-  cluster_centeroids_str = std::string("/points_ground_")+label;
-  points_ground_str = std::string("/cluster_centroids_")+label;
-
-  _pub_cluster_cloud = h.advertise<sensor_msgs::PointCloud2>(point_cluster_str.c_str(), 1);
-  _pub_ground_cloud = h.advertise<sensor_msgs::PointCloud2>(cluster_centeroids_str.c_str(), 1);
-  _centroid_pub = h.advertise<autoware_msgs::Centroids>(points_ground_str.c_str(), 1);
-  _pub_points_lanes_cloud = h.advertise<sensor_msgs::PointCloud2>(output_lane_str.c_str(), 1);
-  _pub_clusters_message = h.advertise<autoware_msgs::CloudClusterArray>(output_cluster_str.c_str(), 1);
-  _pub_detected_objects = h.advertise<autoware_msgs::DetectedObjectArray>(output_objects_str.c_str(), 1);
-
-  std::string points_topic, gridmap_topic;
-
-  _using_sensor_cloud = false;
-
-  if (private_nh.getParam("points_node", points_topic))
-  {
-    ROS_INFO("euclidean_cluster > Setting points node to %s", points_topic.c_str());
-  }
-  else
-  {
-    ROS_INFO("euclidean_cluster > No points node received, defaulting to points_raw, you can use "
-               "_points_node:=YOUR_TOPIC");
-    points_topic = "/points_raw";
-  }
-
-  _use_diffnormals = false;
-  if (private_nh.getParam("use_diffnormals", _use_diffnormals))
-  {
-    if (_use_diffnormals)
-      ROS_INFO("Euclidean Clustering: Applying difference of normals on clustering pipeline");
-    else
-      ROS_INFO("Euclidean Clustering: Difference of Normals will not be used.");
-  }
-
-  /* Initialize tuning parameter */
-  private_nh.param("downsample_cloud", _downsample_cloud, false);
-  ROS_INFO("[%s] downsample_cloud: %d", __APP_NAME__, _downsample_cloud);
-  private_nh.param("remove_ground", _remove_ground, true);
-  ROS_INFO("[%s] remove_ground: %d", __APP_NAME__, _remove_ground);
-  private_nh.param("leaf_size", _leaf_size, 0.1);
-  ROS_INFO("[%s] leaf_size: %f", __APP_NAME__, _leaf_size);
-  private_nh.param("cluster_size_min", _cluster_size_min, 20);
-  ROS_INFO("[%s] cluster_size_min %d", __APP_NAME__, _cluster_size_min);
-  private_nh.param("cluster_size_max", _cluster_size_max, 100000);
-  ROS_INFO("[%s] cluster_size_max: %d", __APP_NAME__, _cluster_size_max);
-  private_nh.param("pose_estimation", _pose_estimation, false);
-  ROS_INFO("[%s] pose_estimation: %d", __APP_NAME__, _pose_estimation);
-  private_nh.param("clip_min_height", _clip_min_height, -1.3);
-  ROS_INFO("[%s] clip_min_height: %f", __APP_NAME__, _clip_min_height);
-  private_nh.param("clip_max_height", _clip_max_height, 0.5);
-  ROS_INFO("[%s] clip_max_height: %f", __APP_NAME__, _clip_max_height);
-  private_nh.param("keep_lanes", _keep_lanes, false);
-  ROS_INFO("[%s] keep_lanes: %d", __APP_NAME__, _keep_lanes);
-  private_nh.param("keep_lane_left_distance", _keep_lane_left_distance, 5.0);
-  ROS_INFO("[%s] keep_lane_left_distance: %f", __APP_NAME__, _keep_lane_left_distance);
-  private_nh.param("keep_lane_right_distance", _keep_lane_right_distance, 5.0);
-  ROS_INFO("[%s] keep_lane_right_distance: %f", __APP_NAME__, _keep_lane_right_distance);
-  private_nh.param("cluster_merge_threshold", _cluster_merge_threshold, 1.5);
-  ROS_INFO("[%s] cluster_merge_threshold: %f", __APP_NAME__, _cluster_merge_threshold);
-  private_nh.param<std::string>("output_frame", _output_frame, "velodyne");
-  ROS_INFO("[%s] output_frame: %s", __APP_NAME__, _output_frame.c_str());
-
-  private_nh.param("remove_points_upto", _remove_points_upto, 0.0);
-  ROS_INFO("[%s] remove_points_upto: %f", __APP_NAME__, _remove_points_upto);
-
-  private_nh.param("clustering_distance", _clustering_distance, 0.75);
-  ROS_INFO("[%s] clustering_distance: %f", __APP_NAME__, _clustering_distance);
-
-  private_nh.param("use_gpu", _use_gpu, false);
-  ROS_INFO("[%s] use_gpu: %d", __APP_NAME__, _use_gpu);
-
-  private_nh.param("use_multiple_thres", _use_multiple_thres, false);
-  ROS_INFO("[%s] use_multiple_thres: %d", __APP_NAME__, _use_multiple_thres);
-
-  std::string str_distances;
-  std::string str_ranges;
-  
-  private_nh.param("clustering_distances", str_distances, std::string("[0.5,1.1,1.6,2.1,2.6]"));
-  ROS_INFO("[%s] clustering_distances: %s", __APP_NAME__, str_distances.c_str());
-  private_nh.param("clustering_ranges", str_ranges, std::string("[15,30,45,60]"));
+    /* Initialize tuning parameter */
+    private_nh.param("downsample_cloud", _downsample_cloud, false);
+    ROS_INFO("[%s] downsample_cloud: %d", __APP_NAME__, _downsample_cloud);
+    private_nh.param("remove_ground", _remove_ground, true);
+    ROS_INFO("[%s] remove_ground: %d", __APP_NAME__, _remove_ground);
+    private_nh.param("leaf_size", _leaf_size, 0.1);
+    ROS_INFO("[%s] leaf_size: %f", __APP_NAME__, _leaf_size);
+    private_nh.param("cluster_size_min", _cluster_size_min, 20);
+    ROS_INFO("[%s] cluster_size_min %d", __APP_NAME__, _cluster_size_min);
+    private_nh.param("cluster_size_max", _cluster_size_max, 100000);
+    ROS_INFO("[%s] cluster_size_max: %d", __APP_NAME__, _cluster_size_max);
+    private_nh.param("pose_estimation", _pose_estimation, false);
+    ROS_INFO("[%s] pose_estimation: %d", __APP_NAME__, _pose_estimation);
+    private_nh.param("clip_min_height", _clip_min_height, -1.3);
+    ROS_INFO("[%s] clip_min_height: %f", __APP_NAME__, _clip_min_height);
+    private_nh.param("clip_max_height", _clip_max_height, 0.5);
+    ROS_INFO("[%s] clip_max_height: %f", __APP_NAME__, _clip_max_height);
+    private_nh.param("keep_lanes", _keep_lanes, false);
+    ROS_INFO("[%s] keep_lanes: %d", __APP_NAME__, _keep_lanes);
+    private_nh.param("keep_lane_left_distance", _keep_lane_left_distance, 5.0);
+    ROS_INFO("[%s] keep_lane_left_distance: %f", __APP_NAME__,
+             _keep_lane_left_distance);
+    private_nh.param("keep_lane_right_distance", _keep_lane_right_distance,
+                     5.0);
+    ROS_INFO("[%s] keep_lane_right_distance: %f", __APP_NAME__,
+             _keep_lane_right_distance);
+    private_nh.param("cluster_merge_threshold", _cluster_merge_threshold, 1.5);
+    ROS_INFO("[%s] cluster_merge_threshold: %f", __APP_NAME__,
+             _cluster_merge_threshold);
+    private_nh.param<std::string>("output_frame", _output_frame, "velodyne");
+    ROS_INFO("[%s] output_frame: %s", __APP_NAME__, _output_frame.c_str());
+
+    private_nh.param("remove_points_upto", _remove_points_upto, 0.0);
+    ROS_INFO("[%s] remove_points_upto: %f", __APP_NAME__, _remove_points_upto);
+
+    private_nh.param("clustering_distance", _clustering_distance, 0.75);
+    ROS_INFO("[%s] clustering_distance: %f", __APP_NAME__,
+             _clustering_distance);
+
+    private_nh.param("use_gpu", _use_gpu, false);
+    ROS_INFO("[%s] use_gpu: %d", __APP_NAME__, _use_gpu);
+
+    private_nh.param("use_multiple_thres", _use_multiple_thres, false);
+    ROS_INFO("[%s] use_multiple_thres: %d", __APP_NAME__, _use_multiple_thres);
+
+    std::string str_distances;
+    std::string str_ranges;
+
+    private_nh.param("clustering_distances", str_distances,
+                     std::string("[0.5,1.1,1.6,2.1,2.6]"));
+    ROS_INFO("[%s] clustering_distances: %s", __APP_NAME__,
+             str_distances.c_str());
+    private_nh.param("clustering_ranges", str_ranges,
+                     std::string("[15,30,45,60]"));
     ROS_INFO("[%s] clustering_ranges: %s", __APP_NAME__, str_ranges.c_str());
 
- 
-  if (_use_multiple_thres)
-  {    
-    YAML::Node distances = YAML::Load(str_distances);
-    YAML::Node ranges = YAML::Load(str_ranges);
-    size_t distances_size = distances.size();
-    size_t ranges_size = ranges.size();
-    if (distances_size == 0 || ranges_size == 0)
-    {
-      ROS_ERROR("Invalid size of clustering_ranges or/and clustering_distance. \
+    if (_use_multiple_thres) {
+        YAML::Node distances = YAML::Load(str_distances);
+        YAML::Node ranges = YAML::Load(str_ranges);
+        size_t distances_size = distances.size();
+        size_t ranges_size = ranges.size();
+        if (distances_size == 0 || ranges_size == 0) {
+            ROS_ERROR(
+                "Invalid size of clustering_ranges or/and clustering_distance. \
     The size of clustering distance and clustering_ranges should not be 0");
-      ros::shutdown();
-    }
-    if ((distances_size - ranges_size) != 1)
-    {
-      ROS_ERROR("Invalid size of clustering_ranges or/and clustering_distance. \
+            ros::shutdown();
+        }
+        if ((distances_size - ranges_size) != 1) {
+            ROS_ERROR(
+                "Invalid size of clustering_ranges or/and clustering_distance. \
     Expecting that (distances_size - ranges_size) == 1 ");
-      ros::shutdown();
-    }
-    for (size_t i_distance = 0; i_distance < distances_size; i_distance++)
-    {
-      _clustering_distances.push_back(distances[i_distance].as<double>());
-    }
-    for (size_t i_range = 0; i_range < ranges_size; i_range++)
-    {
-      _clustering_ranges.push_back(ranges[i_range].as<double>());
+            ros::shutdown();
+        }
+        for (size_t i_distance = 0; i_distance < distances_size; i_distance++) {
+            _clustering_distances.push_back(distances[i_distance].as<double>());
+        }
+        for (size_t i_range = 0; i_range < ranges_size; i_range++) {
+            _clustering_ranges.push_back(ranges[i_range].as<double>());
+        }
     }
-  }
 
-  _velodyne_transform_available = false;
+    _velodyne_transform_available = false;
 
-  // Create a ROS subscriber for the input point cloud
-  ros::Subscriber sub = h.subscribe(points_topic, 1, velodyne_callback);
+    // Create a ROS subscriber for the input point cloud
+    ros::Subscriber sub = h.subscribe(points_topic, 10, velodyne_callback);
 
-  if(!task_scheduling_flag && !task_profiling_flag){
     ros::spin();
-  }
-  else{
-    ros::Rate r(rate);
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){      
-      ros::spinOnce();
-      r.sleep();      
-      if(rubis::sched::is_task_ready_) break;
-    }
-
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();        
-      if(rubis::sched::task_state_ == TASK_STATE_READY){
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::start_job();
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
-
-      ros::spinOnce();
-
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){
-        if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::finish_job();        
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
-  }
 
-  return 0;
+    return 0;
 }
diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml
index 00901300..e8b25686 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml
+++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml
@@ -6,7 +6,7 @@
   <maintainer email="abrahammonrroy@yahoo.com">amc</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_msgs</depend>
@@ -22,4 +22,5 @@
   <depend>tf</depend>
   <depend>vector_map_server</depend>
   <depend>rubis_lib</depend>
+  <depend>rubis_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt
index 691c2316..15e30a95 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt
@@ -5,7 +5,7 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   amathutils_lib
@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf
   vector_map
   rubis_lib
+  rubis_msgs
 )
 
 
@@ -59,8 +60,4 @@ install(
 install(DIRECTORY launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
   PATTERN ".svn" EXCLUDE
-)
-
-if (CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-endif()
+)
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h
index 29b02a5a..e386b8df 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h
+++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h
@@ -38,6 +38,8 @@
 #include "autoware_msgs/DetectedObject.h"
 #include "autoware_msgs/DetectedObjectArray.h"
 
+#include "rubis_msgs/DetectedObjectArray.h"
+
 #include "ukf.h"
 
 extern int is_topic_ready;
@@ -90,7 +92,9 @@ class ImmUkfPda
   const double CENTROID_DISTANCE = 0.2;//distance to consider centroids the same
 
   std::string input_topic_;
+  std::string input_rubis_topic_;
   std::string output_topic_;
+  std::string output_rubis_topic_;
 
   std::string tracking_frame_;
 
@@ -102,12 +106,16 @@ class ImmUkfPda
   ros::NodeHandle node_handle_;
   ros::NodeHandle private_nh_;
   ros::Subscriber sub_detected_array_;
+  ros::Subscriber sub_rubis_detected_array_;
   ros::Publisher pub_object_array_;
+  ros::Publisher pub_rubis_object_array_;
 
   std_msgs::Header input_header_;
 
   void callback(const autoware_msgs::DetectedObjectArray& input);
 
+  void rubis_callback(const rubis_msgs::DetectedObjectArray& input);
+
   void transformPoseToGlobal(const autoware_msgs::DetectedObjectArray& input,
                              autoware_msgs::DetectedObjectArray& transformed_input);
   void transformPoseToLocal(autoware_msgs::DetectedObjectArray& detected_objects_output);
diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch
index 0ab3cb8d..4ff7ecef 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch
+++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch
@@ -4,6 +4,8 @@
   <arg name="namespace" default="/detection/object_tracker"/>
   <arg name="tracker_input_topic" default="/detection/lidar_detector/objects" />
   <arg name="tracker_output_topic" default="/detection/object_tracker/objects" />
+  <arg name="tracker_input_rubis_topic" default="/detection/lidar_detector/objects" />
+  <arg name="tracker_output_rubis_topic" default="/detection/object_tracker/objects" />
 
   <arg name="tracking_frame" default="/world" />
   <arg name="gating_threshold" default="9.22" />
@@ -26,6 +28,8 @@
   <node pkg="imm_ukf_pda_track" type="imm_ukf_pda" name="imm_ukf_pda_$(arg label)" output="screen">
     <param name="tracker_input_topic" value="$(arg tracker_input_topic)" />
     <param name="tracker_output_topic" value="$(arg tracker_output_topic)" />
+    <param name="tracker_input_rubis_topic" value="$(arg tracker_input_rubis_topic)" />
+    <param name="tracker_output_rubis_topic" value="$(arg tracker_output_rubis_topic)" />
     <param name="gating_threshold" value="$(arg gating_threshold)" />
     <param name="gate_probability" value="$(arg gate_probability)" />
     <param name="detection_probability" value="$(arg detection_probability)" />
@@ -40,9 +44,6 @@
     <param name="use_sukf" value="$(arg use_sukf)" />
     <param name="use_vectormap" value="$(arg use_map_info)" />
     <param name="merge_distance_threshold" value="$(arg merge_distance_threshold)" />
-
-    <!-- <remap from="detection/fusion_tools/objects_$(arg label)" to="$(arg tracker_input_topic)_$(arg label)" />
-    <remap from="detection/objects_$(arg label)" to="$(arg tracker_output_topic)_$(arg label)" /> -->
   </node>
 
   <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="ukf_track_visualization_$(arg label)"
diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp
index 131e0f1e..fbb518e6 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp
+++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp
@@ -54,13 +54,17 @@ ImmUkfPda::ImmUkfPda()
 
   // topic name
   private_nh_.param<std::string>("tracker_input_topic", input_topic_, "/detection/lidar_detector/objects");
+  private_nh_.param<std::string>("tracker_input_rubis_topic", input_rubis_topic_, "/detection/lidar_detector/rubis_objects_center");
   private_nh_.param<std::string>("tracker_output_topic", output_topic_, "/detection/object_tracker/objects");
+  private_nh_.param<std::string>("tracker_output_rubis_topic", output_rubis_topic_, "/detection/object_tracker/rubis_objects_center");
 }
 
 void ImmUkfPda::run()
 {
   pub_object_array_ = node_handle_.advertise<autoware_msgs::DetectedObjectArray>(output_topic_.c_str(), 1);
   sub_detected_array_ = node_handle_.subscribe(input_topic_.c_str(), 1, &ImmUkfPda::callback, this);
+  pub_rubis_object_array_ = node_handle_.advertise<rubis_msgs::DetectedObjectArray>(output_rubis_topic_.c_str(), 1);
+  sub_rubis_detected_array_ = node_handle_.subscribe(input_rubis_topic_.c_str(), 1, &ImmUkfPda::rubis_callback, this);
 
   if (use_vectormap_)
   {
@@ -70,6 +74,41 @@ void ImmUkfPda::run()
   }
 }
 
+void ImmUkfPda::rubis_callback(const rubis_msgs::DetectedObjectArray& input)
+{
+  rubis::instance_ = input.instance;
+  input_header_ = input.object_array.header;
+
+  if(use_vectormap_)
+  {
+    checkVectormapSubscription();
+  }
+
+  bool success = updateNecessaryTransform();
+  if (!success)
+  {
+    ROS_INFO("Could not find coordiante transformation");
+    return;
+  }
+
+  autoware_msgs::DetectedObjectArray transformed_input;
+  autoware_msgs::DetectedObjectArray detected_objects_output;
+  rubis_msgs::DetectedObjectArray rubis_detected_objects_output;
+  transformPoseToGlobal(input.object_array, transformed_input);
+  tracker(transformed_input, detected_objects_output);
+  transformPoseToLocal(detected_objects_output);
+
+  rubis_detected_objects_output.instance = rubis::instance_;
+  rubis_detected_objects_output.object_array = detected_objects_output;
+  pub_rubis_object_array_.publish(rubis_detected_objects_output);
+
+  if (is_benchmark_)
+  {
+    dumpResultText(detected_objects_output);
+  }
+
+}
+
 void ImmUkfPda::callback(const autoware_msgs::DetectedObjectArray& input)
 {
   input_header_ = input.header;
@@ -98,9 +137,6 @@ void ImmUkfPda::callback(const autoware_msgs::DetectedObjectArray& input)
   {
     dumpResultText(detected_objects_output);
   }
-
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
 }
 
 void ImmUkfPda::checkVectormapSubscription()
diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp
index 6598327b..9cf8ad00 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp
+++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp
@@ -24,59 +24,21 @@ int main(int argc, char** argv)
   ros::NodeHandle private_nh("~");
 
   // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
   int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
-
-  private_nh.param<int>("/imm_ukf_pda_track/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/imm_ukf_pda_track/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/imm_ukf_pda_track/task_response_time_filename", task_response_time_filename, "~/profiling/response_time/imm_ukf_pda_track.csv");
   private_nh.param<int>("/imm_ukf_pda_track/rate", rate, 10);
-  private_nh.param("/imm_ukf_pda_track/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/imm_ukf_pda_track/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/imm_ukf_pda_track/task_relative_deadline", task_relative_deadline, (double)10);
 
   ImmUkfPda app;
   app.run();
 
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{
-    ros::Rate r(rate);
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){
-      if(rubis::sched::is_task_ready_) break;
-      ros::spinOnce();
-      r.sleep();      
-    }
-
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-      if(rubis::sched::task_state_ == TASK_STATE_READY){
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
+  ros::Rate r(rate);
+  while(ros::ok()){
+    rubis::start_task_profiling();
 
-      ros::spinOnce();
+    ros::spinOnce();
 
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
+    r.sleep();
   }
 
   return 0;
diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml
index 0dd5b97c..71945104 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml
+++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml
@@ -6,7 +6,7 @@
   <maintainer email="cirpue49@yahoo.co.jp">Kosuke Murakami</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_msgs</depend>
@@ -16,6 +16,6 @@
   <depend>roslint</depend>
   <depend>tf</depend>
   <depend>vector_map</depend>
-  <depend>lanelet2_extension</depend>
   <depend>rubis_lib</depend>
+  <depend>rubis_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/lidar_localizer/CMakeLists.txt
index f85aa53e..d934516a 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/CMakeLists.txt
@@ -1,183 +1,291 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(lidar_localizer)
-
-find_package(PCL REQUIRED)
-
-if(NOT (PCL_VERSION VERSION_LESS "1.7.2"))
-  SET(PCL_OPENMP_PACKAGES pcl_omp_registration)
-endif()
-
-find_package(OpenMP)
-if(OPENMP_FOUND)
-  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-endif()
-
-find_package(CUDA)
-find_package(Eigen3 QUIET)
-find_package(autoware_build_flags REQUIRED)
-
-if(USE_CUDA)
-  AW_CHECK_CUDA()
-  add_definitions(-DCUDA_FOUND)
-  list(APPEND PCL_OPENMP_PACKAGES ndt_gpu)
-endif()
-
-if(NOT EIGEN3_FOUND)
-  # Fallback to cmake_modules
-  find_package(cmake_modules REQUIRED)
-  find_package(Eigen REQUIRED)
-  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
-  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
-  # Possibly map additional variables to the EIGEN3_ prefix.
-else()
-  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
-endif()
-
-find_package(catkin REQUIRED COMPONENTS
-  ${PCL_OPENMP_PACKAGES}
-  autoware_config_msgs
-  autoware_health_checker
-  autoware_msgs
-  jsk_rviz_plugins
-  nav_msgs
-  ndt_cpu
-  ndt_tku
-  pcl_conversions
-  pcl_ros
-  roscpp
-  sensor_msgs
-  std_msgs
-  tf
-  velodyne_pointcloud
-  rubis_lib
-  rubis_msgs
-)
-
-catkin_package(
-  CATKIN_DEPENDS
-    ${PCL_OPENMP_PACKAGES}
-    autoware_config_msgs
-    autoware_health_checker
-    autoware_msgs
-    jsk_rviz_plugins
-    ndt_cpu
-    ndt_tku
-    std_msgs
-    rubis_msgs
-    velodyne_pointcloud
-  DEPENDS PCL
-)
-
-include_directories(
-  include
-  lib
-  ${catkin_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIRS}
-)
-
-link_directories(lib)
-
-SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
-
-add_executable(ndt_matching   nodes/ndt_matching/ndt_matching.cpp
-                              lib/LKF.cpp)
-target_link_libraries(ndt_matching ${catkin_LIBRARIES})
-add_dependencies(ndt_matching ${catkin_EXPORTED_TARGETS})
-
-add_executable(ndt_mapping nodes/ndt_mapping/ndt_mapping.cpp)
-target_link_libraries(ndt_mapping ${catkin_LIBRARIES})
-add_dependencies(ndt_mapping ${catkin_EXPORTED_TARGETS})
-
-add_executable(gnss_mapping nodes/gnss_mapping/gnss_mapping.cpp)
-target_link_libraries(gnss_mapping ${catkin_LIBRARIES})
-add_dependencies(gnss_mapping ${catkin_EXPORTED_TARGETS})
-
-add_executable(modular_ndt_matching   nodes/modular_ndt_matching/modular_ndt_matching.cpp)
-target_link_libraries(modular_ndt_matching ${catkin_LIBRARIES})
-add_dependencies(modular_ndt_matching ${catkin_EXPORTED_TARGETS})
-
-add_executable(multi_ndt_combiner nodes/modular_ndt_matching/multi_ndt_combiner.cpp)
-target_link_libraries(multi_ndt_combiner ${catkin_LIBRARIES})
-add_dependencies(multi_ndt_combiner ${catkin_EXPORTED_TARGETS})
-
-if(USE_CUDA)
-  target_include_directories(ndt_matching PRIVATE ${CUDA_INCLUDE_DIRS})
-  target_include_directories(modular_ndt_matching PRIVATE ${CUDA_INCLUDE_DIRS})
-  target_include_directories(ndt_mapping PRIVATE ${CUDA_INCLUDE_DIRS})
-endif()
-
-
-if(NOT (PCL_VERSION VERSION_LESS "1.7.2"))
-  set_target_properties(ndt_matching PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
-  set_target_properties(modular_ndt_matching PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
-  set_target_properties(ndt_mapping PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
-endif()
-
-add_executable(approximate_ndt_mapping nodes/approximate_ndt_mapping/approximate_ndt_mapping.cpp)
-target_link_libraries(approximate_ndt_mapping ${catkin_LIBRARIES})
-add_dependencies(approximate_ndt_mapping ${catkin_EXPORTED_TARGETS})
-
-add_executable(queue_counter nodes/queue_counter/queue_counter.cpp)
-target_link_libraries(queue_counter ${catkin_LIBRARIES})
-add_dependencies(queue_counter ${catkin_EXPORTED_TARGETS})
-
-add_executable(ndt_matching_tku nodes/ndt_matching_tku/ndt_matching_tku.cpp)
-target_link_libraries(ndt_matching_tku ${catkin_LIBRARIES})
-add_dependencies(ndt_matching_tku ${catkin_EXPORTED_TARGETS})
-
-add_executable(ndt_mapping_tku nodes/ndt_mapping_tku/ndt_mapping_tku.cpp)
-target_link_libraries(ndt_mapping_tku ${catkin_LIBRARIES})
-add_dependencies(ndt_mapping_tku ${catkin_EXPORTED_TARGETS})
-
-add_executable(mapping nodes/ndt_mapping_tku/mapping.cpp)
-target_link_libraries(mapping ${catkin_LIBRARIES})
-add_dependencies(mapping ${catkin_EXPORTED_TARGETS})
-
-add_library(ndt_matching_monitor_lib SHARED
-  nodes/ndt_matching_monitor/ndt_matching_monitor.h
-  nodes/ndt_matching_monitor/ndt_matching_monitor.cpp
-)
-target_include_directories(ndt_matching_monitor_lib PRIVATE
-  ${catkin_INCLUDE_DIRS}
-  ${PCL_INCLUDE_DIRS}
-)
-add_dependencies(ndt_matching_monitor_lib
-  ${catkin_EXPORTED_TARGETS}
-)
-target_link_libraries(ndt_matching_monitor_lib ${catkin_LIBRARIES})
-
-add_executable(ndt_matching_monitor nodes/ndt_matching_monitor/ndt_matching_monitor_node.cpp)
-target_link_libraries(ndt_matching_monitor ndt_matching_monitor_lib ${catkin_LIBRARIES})
-
-add_executable(icp_matching nodes/icp_matching/icp_matching.cpp)
-target_link_libraries(icp_matching ${catkin_LIBRARIES})
-add_dependencies(icp_matching ${catkin_EXPORTED_TARGETS})
-
-install(DIRECTORY cfg/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
-)
-
-install(
-  TARGETS
-    approximate_ndt_mapping
-    icp_matching
-    mapping
-    gnss_mapping
-    ndt_mapping
-    ndt_mapping_tku
-    ndt_matching
-    modular_ndt_matching
-    multi_ndt_combiner
-    ndt_matching_monitor
-    ndt_matching_monitor_lib
-    queue_counter
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(DIRECTORY launch/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-  PATTERN ".svn" EXCLUDE
-)
\ No newline at end of file
+cmake_minimum_required(VERSION 2.8.3)
+project(lidar_localizer)
+
+find_package(PCL REQUIRED)
+
+if(NOT (PCL_VERSION VERSION_LESS "1.7.2"))
+  SET(PCL_OPENMP_PACKAGES pcl_omp_registration)
+endif()
+
+find_package(OpenMP)
+if(OPENMP_FOUND)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+find_package(autoware_build_flags COMPONENTS)
+if(autoware_build_flags_FOUND)
+
+  find_package(CUDA)
+  find_package(Eigen3 QUIET)
+  find_package(autoware_build_flags REQUIRED)
+
+  if(USE_CUDA)
+    AW_CHECK_CUDA()
+    add_definitions(-DCUDA_FOUND)
+    list(APPEND PCL_OPENMP_PACKAGES ndt_gpu)
+  endif()
+
+  if(NOT EIGEN3_FOUND)
+    # Fallback to cmake_modules
+    find_package(cmake_modules REQUIRED)
+    find_package(Eigen REQUIRED)
+    set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+    set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
+    # Possibly map additional variables to the EIGEN3_ prefix.
+  else()
+    set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+  endif()
+
+  find_package(catkin REQUIRED COMPONENTS
+    ${PCL_OPENMP_PACKAGES}
+    autoware_config_msgs
+    autoware_health_checker
+    autoware_msgs
+    jsk_rviz_plugins
+    nav_msgs
+    ndt_cpu
+    ndt_tku
+    pcl_conversions
+    pcl_ros
+    roscpp
+    sensor_msgs
+    std_msgs
+    tf
+    velodyne_pointcloud
+    rubis_lib
+    rubis_msgs
+  )
+
+  catkin_package(
+    CATKIN_DEPENDS
+      ${PCL_OPENMP_PACKAGES}
+      autoware_config_msgs
+      autoware_health_checker
+      autoware_msgs
+      jsk_rviz_plugins
+      ndt_cpu
+      ndt_tku
+      std_msgs
+      rubis_msgs
+      velodyne_pointcloud
+    DEPENDS PCL
+  )
+
+  include_directories(
+    include
+    lib
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+  )
+
+  link_directories(lib)
+
+  SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+  add_executable(ndt_matching   nodes/ndt_matching/ndt_matching.cpp
+                                lib/LKF.cpp)
+  target_link_libraries(ndt_matching ${catkin_LIBRARIES})
+  add_dependencies(ndt_matching ${catkin_EXPORTED_TARGETS})
+
+  add_executable(ndt_mapping nodes/ndt_mapping/ndt_mapping.cpp)
+  target_link_libraries(ndt_mapping ${catkin_LIBRARIES})
+  add_dependencies(ndt_mapping ${catkin_EXPORTED_TARGETS})
+
+  add_executable(gnss_mapping nodes/gnss_mapping/gnss_mapping.cpp)
+  target_link_libraries(gnss_mapping ${catkin_LIBRARIES})
+  add_dependencies(gnss_mapping ${catkin_EXPORTED_TARGETS})
+
+  add_executable(modular_ndt_matching   nodes/modular_ndt_matching/modular_ndt_matching.cpp)
+  target_link_libraries(modular_ndt_matching ${catkin_LIBRARIES})
+  add_dependencies(modular_ndt_matching ${catkin_EXPORTED_TARGETS})
+
+  add_executable(multi_ndt_combiner nodes/modular_ndt_matching/multi_ndt_combiner.cpp)
+  target_link_libraries(multi_ndt_combiner ${catkin_LIBRARIES})
+  add_dependencies(multi_ndt_combiner ${catkin_EXPORTED_TARGETS})
+
+  if(USE_CUDA)
+    target_include_directories(ndt_matching PRIVATE ${CUDA_INCLUDE_DIRS})
+    target_include_directories(modular_ndt_matching PRIVATE ${CUDA_INCLUDE_DIRS})
+    target_include_directories(ndt_mapping PRIVATE ${CUDA_INCLUDE_DIRS})
+  endif()
+
+
+  if(NOT (PCL_VERSION VERSION_LESS "1.7.2"))
+    set_target_properties(ndt_matching PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
+    set_target_properties(modular_ndt_matching PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
+    set_target_properties(ndt_mapping PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
+  endif()
+
+  add_executable(approximate_ndt_mapping nodes/approximate_ndt_mapping/approximate_ndt_mapping.cpp)
+  target_link_libraries(approximate_ndt_mapping ${catkin_LIBRARIES})
+  add_dependencies(approximate_ndt_mapping ${catkin_EXPORTED_TARGETS})
+
+  add_executable(queue_counter nodes/queue_counter/queue_counter.cpp)
+  target_link_libraries(queue_counter ${catkin_LIBRARIES})
+  add_dependencies(queue_counter ${catkin_EXPORTED_TARGETS})
+
+  add_executable(ndt_matching_tku nodes/ndt_matching_tku/ndt_matching_tku.cpp)
+  target_link_libraries(ndt_matching_tku ${catkin_LIBRARIES})
+  add_dependencies(ndt_matching_tku ${catkin_EXPORTED_TARGETS})
+
+  add_executable(ndt_mapping_tku nodes/ndt_mapping_tku/ndt_mapping_tku.cpp)
+  target_link_libraries(ndt_mapping_tku ${catkin_LIBRARIES})
+  add_dependencies(ndt_mapping_tku ${catkin_EXPORTED_TARGETS})
+
+  add_executable(mapping nodes/ndt_mapping_tku/mapping.cpp)
+  target_link_libraries(mapping ${catkin_LIBRARIES})
+  add_dependencies(mapping ${catkin_EXPORTED_TARGETS})
+
+  add_library(ndt_matching_monitor_lib SHARED
+    nodes/ndt_matching_monitor/ndt_matching_monitor.h
+    nodes/ndt_matching_monitor/ndt_matching_monitor.cpp
+  )
+  target_include_directories(ndt_matching_monitor_lib PRIVATE
+    ${catkin_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+  )
+  add_dependencies(ndt_matching_monitor_lib
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_link_libraries(ndt_matching_monitor_lib ${catkin_LIBRARIES})
+
+  add_executable(ndt_matching_monitor nodes/ndt_matching_monitor/ndt_matching_monitor_node.cpp)
+  target_link_libraries(ndt_matching_monitor ndt_matching_monitor_lib ${catkin_LIBRARIES})
+
+  add_executable(icp_matching nodes/icp_matching/icp_matching.cpp)
+  target_link_libraries(icp_matching ${catkin_LIBRARIES})
+  add_dependencies(icp_matching ${catkin_EXPORTED_TARGETS})
+
+  install(DIRECTORY cfg/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+  )
+
+  install(
+    TARGETS
+      approximate_ndt_mapping
+      icp_matching
+      mapping
+      gnss_mapping
+      ndt_mapping
+      ndt_mapping_tku
+      ndt_matching
+      modular_ndt_matching
+      multi_ndt_combiner
+      ndt_matching_monitor
+      ndt_matching_monitor_lib
+      queue_counter
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+  install(DIRECTORY launch/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+    PATTERN ".svn" EXCLUDE
+  )
+else()
+
+  find_package(Eigen3 QUIET)
+
+  if(NOT EIGEN3_FOUND)
+    # Fallback to cmake_modules
+    find_package(cmake_modules REQUIRED)
+    find_package(Eigen REQUIRED)
+    set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+    set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
+    # Possibly map additional variables to the EIGEN3_ prefix.
+  else()
+    set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+  endif()
+
+  find_package(catkin REQUIRED COMPONENTS
+    ${PCL_OPENMP_PACKAGES}
+    autoware_config_msgs
+    autoware_msgs
+    nav_msgs
+    ndt_cpu
+    pcl_conversions
+    pcl_ros
+    roscpp
+    sensor_msgs
+    std_msgs
+    tf
+    velodyne_pointcloud
+    rubis_lib
+    rubis_msgs
+  )
+
+  catkin_package(
+    CATKIN_DEPENDS
+      ${PCL_OPENMP_PACKAGES}
+      autoware_config_msgs
+      autoware_msgs
+      ndt_cpu
+      std_msgs
+      rubis_msgs
+      velodyne_pointcloud
+    DEPENDS PCL
+  )
+
+  include_directories(
+    include
+    lib
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+  )
+
+  link_directories(lib)
+
+  SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+  add_executable(ndt_matching   nodes/ndt_matching/ndt_matching.cpp
+                                lib/LKF.cpp)
+  target_link_libraries(ndt_matching ${catkin_LIBRARIES})
+  add_dependencies(ndt_matching ${catkin_EXPORTED_TARGETS})
+
+  add_executable(ndt_mapping nodes/ndt_mapping/ndt_mapping.cpp)
+  target_link_libraries(ndt_mapping ${catkin_LIBRARIES})
+  add_dependencies(ndt_mapping ${catkin_EXPORTED_TARGETS})
+
+  if(NOT (PCL_VERSION VERSION_LESS "1.7.2"))
+    set_target_properties(ndt_matching PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
+    set_target_properties(ndt_mapping PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
+  endif()
+
+  add_executable(approximate_ndt_mapping nodes/approximate_ndt_mapping/approximate_ndt_mapping.cpp)
+  target_link_libraries(approximate_ndt_mapping ${catkin_LIBRARIES})
+  add_dependencies(approximate_ndt_mapping ${catkin_EXPORTED_TARGETS})
+
+  add_executable(queue_counter nodes/queue_counter/queue_counter.cpp)
+  target_link_libraries(queue_counter ${catkin_LIBRARIES})
+  add_dependencies(queue_counter ${catkin_EXPORTED_TARGETS})
+
+  add_executable(icp_matching nodes/icp_matching/icp_matching.cpp)
+  target_link_libraries(icp_matching ${catkin_LIBRARIES})
+  add_dependencies(icp_matching ${catkin_EXPORTED_TARGETS})
+
+  install(DIRECTORY cfg/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+  )
+
+
+  install(
+    TARGETS
+      approximate_ndt_mapping
+      icp_matching
+      ndt_mapping
+      ndt_matching
+      queue_counter
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+  install(DIRECTORY launch/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+    PATTERN ".svn" EXCLUDE
+  )
+
+
+endif()
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/CMakeLists.txt.backup b/autoware.ai/src/autoware/core_perception/lidar_localizer/CMakeLists.txt.backup
new file mode 100644
index 00000000..abe22616
--- /dev/null
+++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/CMakeLists.txt.backup
@@ -0,0 +1,115 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lidar_localizer)
+
+find_package(PCL REQUIRED)
+
+if(NOT (PCL_VERSION VERSION_LESS "1.7.2"))
+  SET(PCL_OPENMP_PACKAGES pcl_omp_registration)
+endif()
+
+find_package(OpenMP)
+if(OPENMP_FOUND)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+find_package(Eigen3 QUIET)
+
+if(NOT EIGEN3_FOUND)
+  # Fallback to cmake_modules
+  find_package(cmake_modules REQUIRED)
+  find_package(Eigen REQUIRED)
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
+  # Possibly map additional variables to the EIGEN3_ prefix.
+else()
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif()
+
+find_package(catkin REQUIRED COMPONENTS
+  ${PCL_OPENMP_PACKAGES}
+  autoware_config_msgs
+  autoware_msgs
+  nav_msgs
+  ndt_cpu
+  pcl_conversions
+  pcl_ros
+  roscpp
+  sensor_msgs
+  std_msgs
+  tf
+  velodyne_pointcloud
+  rubis_lib
+  rubis_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    ${PCL_OPENMP_PACKAGES}
+    autoware_config_msgs
+    autoware_msgs
+    ndt_cpu
+    std_msgs
+    rubis_msgs
+    velodyne_pointcloud
+  DEPENDS PCL
+)
+
+include_directories(
+  include
+  lib
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+link_directories(lib)
+
+SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+add_executable(ndt_matching   nodes/ndt_matching/ndt_matching.cpp
+                              lib/LKF.cpp)
+target_link_libraries(ndt_matching ${catkin_LIBRARIES})
+add_dependencies(ndt_matching ${catkin_EXPORTED_TARGETS})
+
+add_executable(ndt_mapping nodes/ndt_mapping/ndt_mapping.cpp)
+target_link_libraries(ndt_mapping ${catkin_LIBRARIES})
+add_dependencies(ndt_mapping ${catkin_EXPORTED_TARGETS})
+
+if(NOT (PCL_VERSION VERSION_LESS "1.7.2"))
+  set_target_properties(ndt_matching PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
+  set_target_properties(ndt_mapping PROPERTIES COMPILE_DEFINITIONS "USE_PCL_OPENMP")
+endif()
+
+add_executable(approximate_ndt_mapping nodes/approximate_ndt_mapping/approximate_ndt_mapping.cpp)
+target_link_libraries(approximate_ndt_mapping ${catkin_LIBRARIES})
+add_dependencies(approximate_ndt_mapping ${catkin_EXPORTED_TARGETS})
+
+add_executable(queue_counter nodes/queue_counter/queue_counter.cpp)
+target_link_libraries(queue_counter ${catkin_LIBRARIES})
+add_dependencies(queue_counter ${catkin_EXPORTED_TARGETS})
+
+add_executable(icp_matching nodes/icp_matching/icp_matching.cpp)
+target_link_libraries(icp_matching ${catkin_LIBRARIES})
+add_dependencies(icp_matching ${catkin_EXPORTED_TARGETS})
+
+install(DIRECTORY cfg/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+)
+
+
+install(
+  TARGETS
+    approximate_ndt_mapping
+    icp_matching
+    ndt_mapping
+    ndt_matching
+    queue_counter
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch
index 20f7d298..d1ce9c94 100755
--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch
+++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch
@@ -2,7 +2,7 @@
 <launch>
 
   <arg name="method_type" default="0" /> <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
-  <arg name="use_gnss" default="1" />
+  <arg name="use_gnss" default="0" />
   <arg name="use_odom" default="false" />
   <arg name="use_imu" default="false" />
   <arg name="use_kalman_filter" default="true" />
@@ -20,7 +20,6 @@
   <arg name="recovery_score_diff_threshold" default="1.0" />  
   <arg name="failure_pose_diff_threshold" default="4.0" />
   <arg name="recovery_pose_diff_threshold" default="1.0" />  
-  <arg name="instance_mode" default="0" />
   <arg name="kalman_filter_cfg" default="$(find lidar_localizer)/cfg/kalman_filter.yaml"/>
 
   <rosparam command="load" file="$(arg kalman_filter_cfg)" />
@@ -45,7 +44,6 @@
     <param name="recovery_score_diff_threshold" value="$(arg recovery_score_diff_threshold)" />    
     <param name="failure_pose_diff_threshold" value="$(arg failure_pose_diff_threshold)" />    
     <param name="recovery_pose_diff_threshold" value="$(arg recovery_pose_diff_threshold)" />  
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 
 </launch>
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch
index 1b882f79..1a3f7c00 100755
--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch
+++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch
@@ -2,7 +2,7 @@
 <launch>
 
   <arg name="method_type" default="0" /> <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
-  <arg name="use_gnss" default="1" />
+  <arg name="use_gnss" default="0" />
   <arg name="use_odom" default="false" />
   <arg name="use_imu" default="false" />
   <!-- <arg name="use_kalman_filter" default="false" /> -->
@@ -19,8 +19,8 @@
   <arg name="failure_score_diff_threshold" default="10.0" />   
   <arg name="recovery_score_diff_threshold" default="1.0" />  
   <arg name="failure_pose_diff_threshold" default="4.0" />
-  <arg name="recovery_pose_diff_threshold" default="1.0" />  
-  <!-- <arg name="instance_mode" default="0" /> -->
+  <arg name="recovery_pose_diff_threshold" default="1.0" />
+  <arg name="use_svl_gnss" default="false" />  
   <!-- <arg name="kalman_filter_cfg" default="$(find lidar_localizer)/cfg/kalman_filter.yaml"/> -->
 
   <!-- <rosparam command="load" file="$(arg kalman_filter_cfg)" /> -->
@@ -45,7 +45,7 @@
     <param name="recovery_score_diff_threshold" value="$(arg recovery_score_diff_threshold)" />    
     <param name="failure_pose_diff_threshold" value="$(arg failure_pose_diff_threshold)" />    
     <param name="recovery_pose_diff_threshold" value="$(arg recovery_pose_diff_threshold)" />  
-    <!-- <param name="instance_mode" value="$(arg instance_mode)" /> -->
+    <param name="use_svl_gnss" value="$(arg use_svl_gnss)" />
   </node>
 
 </launch>
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp
index c97357f1..e2fcd02c 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp
+++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp
@@ -846,13 +846,11 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input)
 
   health_checker_ptr_->CHECK_RATE("topic_rate_ndt_pose_slow", 8, 5, 1, "topic ndt_pose publish rate slow.");
   ndt_pose_pub.publish(ndt_pose_msg);
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+  
 
-  if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){
-    rubis_ndt_pose_msg.instance = rubis::instance_;
-    rubis_ndt_pose_msg.msg = ndt_pose_msg;
-    rubis_ndt_pose_pub.publish(rubis_ndt_pose_msg);
-  }
+  rubis_ndt_pose_msg.instance = rubis::instance_;
+  rubis_ndt_pose_msg.msg = ndt_pose_msg;
+  rubis_ndt_pose_pub.publish(rubis_ndt_pose_msg);
 
   localizer_pose_pub.publish(localizer_pose_msg);
 
@@ -945,14 +943,10 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input)
   previous_velocity_z = current_velocity_z;
   previous_accel = current_accel;
   
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY){
-    rubis::sched::init_task();
-    if(rubis::sched::gpu_profiling_flag_) rubis::sched::start_gpu_profiling();
-  }  
 }
 
 static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input){
-  rubis::instance_ = RUBIS_NO_INSTANCE;
+  rubis::instance_ = 0;
   ndt_matching(input);
 }
 
@@ -1064,46 +1058,21 @@ int main(int argc, char** argv)
 #endif
 
   // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
   std::string task_response_time_filename;
   int rate;
   double task_minimum_inter_release_time;
   double task_execution_time;
   double task_relative_deadline;
 
-  int gpu_scheduling_flag;
-  int gpu_profiling_flag;
-  std::string gpu_execution_time_filename;
-  std::string gpu_response_time_filename;
-  std::string gpu_deadline_filename;
-
   std::string node_name = ros::this_node::getName();
-  private_nh.param<int>(node_name+"/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>(node_name+"/task_profiling_flag", task_profiling_flag, 0);
   private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ndt_matching.csv");
   private_nh.param<int>(node_name+"/rate", rate, 10);
   private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
   private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10);
   private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10);
-  private_nh.param(node_name+"/gpu_scheduling_flag", gpu_scheduling_flag, 0);
-  private_nh.param(node_name+"/gpu_profiling_flag", gpu_profiling_flag, 0);
-  private_nh.param<std::string>(node_name+"/gpu_execution_time_filename", gpu_execution_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv");
-  private_nh.param<std::string>(node_name+"/gpu_response_time_filename", gpu_response_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv");
-  private_nh.param<std::string>(node_name+"/gpu_deadline_filename", gpu_deadline_filename, "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv");
-  private_nh.param<int>(node_name+"/instance_mode", rubis::instance_mode_, 0);
   
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-  if(gpu_profiling_flag) rubis::sched::init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename);
+  rubis::init_task_profiling(task_response_time_filename);
   
-  if( (_method_type == MethodType::PCL_ANH_GPU) && (gpu_scheduling_flag == 1) ){
-    rubis::sched::init_gpu_scheduling("/tmp/ndt_matching", gpu_deadline_filename, 0);
-  }    
-  else if(_method_type != MethodType::PCL_ANH_GPU && gpu_scheduling_flag == 1){
-    ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!");
-    exit(1);
-  }
-
   Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
   Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
   Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
@@ -1113,11 +1082,10 @@ int main(int argc, char** argv)
   // Publishers
   ndt_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(_output_pose_topic, 10);
 
-  // if(rubis::instance_mode_) rubis_ndt_pose_pub = nh.advertise<rubis_msgs::PoseStamped>("/rubis_" + _output_pose_topic,10);
 
   //debug
   std::string _output_pose_topic_rubis = "/rubis_" + _output_pose_topic;
-  if(rubis::instance_mode_) rubis_ndt_pose_pub = nh.advertise<rubis_msgs::PoseStamped>(_output_pose_topic_rubis,10);
+  rubis_ndt_pose_pub = nh.advertise<rubis_msgs::PoseStamped>(_output_pose_topic_rubis,10);
 
   // localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose", 10);
   estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>(_twist_topic, 10);
@@ -1131,13 +1099,11 @@ int main(int argc, char** argv)
   ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback); 
 
   ros::Subscriber points_sub;
-  // if(rubis::instance_mode_) points_sub = nh.subscribe("rubis_" + _input_topic, _queue_size, rubis_points_callback);
   // else points_sub = nh.subscribe(_input_topic, _queue_size, points_callback);
 
   //debug
   std::string _input_topic_rubis = "/rubis_" + _input_topic;
-  if(rubis::instance_mode_) points_sub = nh.subscribe(_input_topic_rubis, _queue_size, rubis_points_callback);
-  else points_sub = nh.subscribe(_input_topic, _queue_size, points_callback);
+  points_sub = nh.subscribe(_input_topic_rubis, _queue_size, rubis_points_callback);
 
   ros::Subscriber ins_stat_sub = nh.subscribe("/ins_stat", 1, ins_stat_callback);
   
@@ -1149,45 +1115,28 @@ int main(int argc, char** argv)
   std::cout<<"----------------modular_ndt_matching_debug start------------------- "<< std::endl;
   std::cout<<"output pose topic:         " << _output_pose_topic << std::endl;
   std::cout<<"output pose topic rubis:         " << _output_pose_topic_rubis << std::endl;
-  std::cout<<"instance_mode param:          " << std::endl;
   std::cout<<"----------------modular_ndt_matching_debug end--------------------- "<< std::endl;
-  // SPIN  
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{ 
+  
     ros::Rate r(rate);
 
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){
-      if(map_loaded == 1) break;
-      ros::spinOnce();
-      r.sleep();      
-    }
-    
-    map_sub.shutdown();
-
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();        
-      if(rubis::sched::task_state_ == TASK_STATE_READY){        
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::start_job();
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
+  // Initialize task ( Wait until first necessary topic is published )
+  while(ros::ok()){
+    if(map_loaded == 1) break;
+    ros::spinOnce();
+    r.sleep();      
+  }
+  
+  map_sub.shutdown();
 
-      ros::spinOnce();
+  // Executing task
+  while(ros::ok()){
+    rubis::start_task_profiling();        
 
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
-      
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){      
-        if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::finish_job();        
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();        
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
+    ros::spinOnce();
+
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+    
+    r.sleep();
   }
 
   return 0;
diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp
index 92a4f873..575f8bf9 100644
--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp
+++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp
@@ -20,11 +20,11 @@
  Yuki KITSUKAWA
  */
 
-#include <pthread.h>
 #include <chrono>
 #include <fstream>
 #include <iostream>
 #include <memory>
+#include <pthread.h>
 #include <sstream>
 #include <string>
 
@@ -39,14 +39,13 @@
 #include <std_msgs/Bool.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/String.h>
-#include <std_msgs/Bool.h>
 #include <velodyne_pointcloud/point_types.h>
 #include <velodyne_pointcloud/rawdata.h>
 
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/TwistStamped.h>
 
 #include <tf/tf.h>
 #include <tf/transform_broadcaster.h>
@@ -74,13 +73,15 @@
 
 #include <autoware_msgs/NDTStat.h>
 
-//headers in Autoware Health Checker
-#include <autoware_health_checker/health_checker/health_checker.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/synchronizer.h>
 
 #include <rubis_lib/sched.hpp>
+#include <rubis_msgs/InsStat.h>
 #include <rubis_msgs/PointCloud2.h>
 #include <rubis_msgs/PoseStamped.h>
-#include <rubis_msgs/InsStat.h>
+#include <rubis_msgs/PoseTwistStamped.h>
 
 #define SPIN_PROFILING
 
@@ -95,35 +96,34 @@
 
 #define DEBUG
 
-static std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
-
-struct pose
-{
-  double x;
-  double y;
-  double z;
-  double roll;
-  double pitch;
-  double yaw;
+typedef message_filters::sync_policies::ExactTime<rubis_msgs::PointCloud2, rubis_msgs::PoseTwistStamped> SyncPolicy;
+typedef message_filters::Synchronizer<SyncPolicy> Sync;
+boost::shared_ptr<Sync> sync_;
+
+struct pose {
+    double x;
+    double y;
+    double z;
+    double roll;
+    double pitch;
+    double yaw;
 };
 
-enum class MethodType
-{
-  PCL_GENERIC = 0,
-  PCL_ANH = 1,
-  PCL_ANH_GPU = 2,
-  PCL_OPENMP = 3,
+enum class MethodType {
+    PCL_GENERIC = 0,
+    PCL_ANH = 1,
+    PCL_ANH_GPU = 2,
+    PCL_OPENMP = 3,
 };
 static MethodType _method_type = MethodType::PCL_GENERIC;
 
-static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose, previous_gnss_pose,
-    ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose, current_kalman_pose, previous_kalman_pose;
+static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose, previous_gnss_pose, ndt_pose,
+    current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose;
 
-static double offset_x, offset_y, offset_z, offset_yaw;  // current_pos - previous_pose
+static double offset_x, offset_y, offset_z, offset_yaw; // current_pos - previous_pose
 static double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw;
 static double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw;
-static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch,
-    offset_imu_odom_yaw;
+static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch, offset_imu_odom_yaw;
 
 // For GPS backup method
 static pose current_gnss_pose;
@@ -140,18 +140,17 @@ static int init_pos_set = 0;
 static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
 static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
 #ifdef CUDA_FOUND
-static std::shared_ptr<gpu::GNormalDistributionsTransform> anh_gpu_ndt_ptr =
-    std::make_shared<gpu::GNormalDistributionsTransform>();
+static std::shared_ptr<gpu::GNormalDistributionsTransform> anh_gpu_ndt_ptr = std::make_shared<gpu::GNormalDistributionsTransform>();
 #endif
 #ifdef USE_PCL_OPENMP
 static pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> omp_ndt;
 #endif
 
 // Default values
-static int max_iter = 30;        // Maximum iterations
-static float ndt_res = 1.0;      // Resolution
-static double step_size = 0.1;   // Step size
-static double trans_eps = 0.01;  // Transformation epsilon
+static int max_iter = 30;       // Maximum iterations
+static float ndt_res = 1.0;     // Resolution
+static double step_size = 0.1;  // Step size
+static double trans_eps = 0.01; // Transformation epsilon
 
 static ros::Publisher predict_pose_pub;
 static geometry_msgs::PoseStamped predict_pose_msg;
@@ -165,20 +164,18 @@ static geometry_msgs::PoseStamped predict_pose_odom_msg;
 static ros::Publisher predict_pose_imu_odom_pub;
 static geometry_msgs::PoseStamped predict_pose_imu_odom_msg;
 
-static ros::Publisher ndt_pose_pub;
 static geometry_msgs::PoseStamped ndt_pose_msg;
 
-static ros::Publisher rubis_ndt_pose_pub;
-static rubis_msgs::PoseStamped rubis_ndt_pose_msg;
-
 // current_pose is published by vel_pose_mux
 /*
 static ros::Publisher current_pose_pub;
 static geometry_msgs::PoseStamped current_pose_msg;
  */
 
+static ros::Publisher rubis_pose_twist_pub;
 static ros::Publisher localizer_pose_pub;
 static geometry_msgs::PoseStamped localizer_pose_msg;
+static rubis_msgs::PoseTwistStamped rubis_pose_twist_msg;
 
 static ros::Publisher estimate_twist_pub;
 static geometry_msgs::TwistStamped estimate_twist_msg;
@@ -197,18 +194,19 @@ static double _gnss_reinit_fitness = 500.0;
 static double diff = 0.0;
 static double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw;
 
-static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0;  // [m/s]
+static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0; // [m/s]
 static double current_velocity_x = 0.0, previous_velocity_x = 0.0;
 static double current_velocity_y = 0.0, previous_velocity_y = 0.0;
 static double current_velocity_z = 0.0, previous_velocity_z = 0.0;
 // static double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0;
 static double current_velocity_smooth = 0.0;
+static double current_gnss_velocity = 0.0, current_gnss_angular_velocity = 0.0;
 
 static double current_velocity_imu_x = 0.0;
 static double current_velocity_imu_y = 0.0;
 static double current_velocity_imu_z = 0.0;
 
-static double current_accel = 0.0, previous_accel = 0.0;  // [m/s^2]
+static double current_accel = 0.0, previous_accel = 0.0; // [m/s^2]
 static double current_accel_x = 0.0;
 static double current_accel_y = 0.0;
 static double current_accel_z = 0.0;
@@ -230,16 +228,9 @@ static double _previous_ins_stat_acc_x = 0.0, _previous_ins_stat_acc_y = 0.0;
 static double _previous_ins_stat_yaw = 0.0;
 static double _previous_ins_stat_linear_velocity = 0.0, _previous_ins_stat_linear_acceleration = 0.0, _previous_ins_stat_angular_velocity = 0.0;
 
-static ros::Publisher is_kalman_filter_on_pub, kalman_filtered_pose_pub;
-static bool _is_kalman_filter_on = false;
-static LKF linear_kalman_filter;
 static double _previous_success_score;
 static bool _is_matching_failed = false;
 
-
-static ros::Publisher estimated_vel_mps_pub, estimated_vel_kmph_pub, estimated_vel_pub;
-static std_msgs::Float32 estimated_vel_mps, estimated_vel_kmph, previous_estimated_vel_kmph;
-
 static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;
 
 static ros::Publisher time_ndt_matching_pub;
@@ -261,16 +252,13 @@ static double _tf_yaw = 0.0;
 static Eigen::Matrix4f tf_btol;
 
 static std::string _localizer = "velodyne";
-static std::string _offset = "linear";  // linear, zero, quadratic
-
-static ros::Publisher ndt_reliability_pub;
-static std_msgs::Float32 ndt_reliability;
+static std::string _offset = "linear"; // linear, zero, quadratic
 
 static bool _get_height = false;
 static bool _use_local_transform = false;
+static bool _use_svl_gnss = false;
 static bool _use_imu = false;
 static bool _use_odom = false;
-static bool _use_kalman_filter = false;
 static bool _imu_upside_down = false;
 static bool _output_log_data = false;
 
@@ -297,9 +285,9 @@ static float _recovery_score_diff_threshold = 1.0;
 static float _failure_pose_diff_threshold = 4.0;
 static float _recovery_pose_diff_threshold = 1.0;
 
+static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr &input);
 
-void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q)
-{
+void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q) {
     double cy = cos(yaw * 0.5);
     double sy = sin(yaw * 0.5);
     double cp = cos(pitch * 0.5);
@@ -313,8 +301,7 @@ void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaterni
     q.z = cr * cp * sy - sr * sp * cy;
 }
 
-static pose convertPoseIntoRelativeCoordinate(const pose &target_pose, const pose &reference_pose)
-{
+static pose convertPoseIntoRelativeCoordinate(const pose &target_pose, const pose &reference_pose) {
     tf::Quaternion target_q;
     target_q.setRPY(target_pose.roll, target_pose.pitch, target_pose.yaw);
     tf::Vector3 target_v(target_pose.x, target_pose.y, target_pose.z);
@@ -337,282 +324,261 @@ static pose convertPoseIntoRelativeCoordinate(const pose &target_pose, const pos
     return trans_target_pose;
 }
 
-static void param_callback(const autoware_config_msgs::ConfigNDT::ConstPtr& input)
-{
-  if (_use_gnss != input->init_pos_gnss)
-  {
-    init_pos_set = 0;
-  }
-  else if (_use_gnss == 0 &&
-           (initial_pose.x != input->x || initial_pose.y != input->y || initial_pose.z != input->z ||
-            initial_pose.roll != input->roll || initial_pose.pitch != input->pitch || initial_pose.yaw != input->yaw))
-  {
-    init_pos_set = 0;
-  }
+static void param_callback(const autoware_config_msgs::ConfigNDT::ConstPtr &input) {
+    if (_use_gnss != input->init_pos_gnss) {
+        init_pos_set = 0;
+    } else if (_use_gnss == 0 && (initial_pose.x != input->x || initial_pose.y != input->y || initial_pose.z != input->z ||
+                                  initial_pose.roll != input->roll || initial_pose.pitch != input->pitch || initial_pose.yaw != input->yaw)) {
+        init_pos_set = 0;
+    }
 
-  _use_gnss = input->init_pos_gnss;
+    _use_gnss = input->init_pos_gnss;
 
-  // Setting parameters
-  if (input->resolution != ndt_res)
-  {
-    ndt_res = input->resolution;
+    // Setting parameters
+    if (input->resolution != ndt_res) {
+        ndt_res = input->resolution;
 
-    if (_method_type == MethodType::PCL_GENERIC)
-      ndt.setResolution(ndt_res);
-    else if (_method_type == MethodType::PCL_ANH)
-      anh_ndt.setResolution(ndt_res);
+        if (_method_type == MethodType::PCL_GENERIC)
+            ndt.setResolution(ndt_res);
+        else if (_method_type == MethodType::PCL_ANH)
+            anh_ndt.setResolution(ndt_res);
 #ifdef CUDA_FOUND
-    else if (_method_type == MethodType::PCL_ANH_GPU)      
-      anh_gpu_ndt_ptr->setResolution(ndt_res);
+        else if (_method_type == MethodType::PCL_ANH_GPU)
+            anh_gpu_ndt_ptr->setResolution(ndt_res);
 #endif
 #ifdef USE_PCL_OPENMP
-    else if (_method_type == MethodType::PCL_OPENMP)
-      omp_ndt.setResolution(ndt_res);
+        else if (_method_type == MethodType::PCL_OPENMP)
+            omp_ndt.setResolution(ndt_res);
 #endif
-  }
+    }
 
-  if (input->step_size != step_size)
-  {
-    step_size = input->step_size;
+    if (input->step_size != step_size) {
+        step_size = input->step_size;
 
-    if (_method_type == MethodType::PCL_GENERIC)
-      ndt.setStepSize(step_size);
-    else if (_method_type == MethodType::PCL_ANH)
-      anh_ndt.setStepSize(step_size);
+        if (_method_type == MethodType::PCL_GENERIC)
+            ndt.setStepSize(step_size);
+        else if (_method_type == MethodType::PCL_ANH)
+            anh_ndt.setStepSize(step_size);
 #ifdef CUDA_FOUND
-    else if (_method_type == MethodType::PCL_ANH_GPU)
-      anh_gpu_ndt_ptr->setStepSize(step_size);
+        else if (_method_type == MethodType::PCL_ANH_GPU)
+            anh_gpu_ndt_ptr->setStepSize(step_size);
 #endif
 #ifdef USE_PCL_OPENMP
-    else if (_method_type == MethodType::PCL_OPENMP)
-      omp_ndt.setStepSize(ndt_res);
+        else if (_method_type == MethodType::PCL_OPENMP)
+            omp_ndt.setStepSize(ndt_res);
 #endif
-  }
+    }
 
-  if (input->trans_epsilon != trans_eps)
-  {
-    trans_eps = input->trans_epsilon;
+    if (input->trans_epsilon != trans_eps) {
+        trans_eps = input->trans_epsilon;
 
-    if (_method_type == MethodType::PCL_GENERIC)
-      ndt.setTransformationEpsilon(trans_eps);
-    else if (_method_type == MethodType::PCL_ANH)
-      anh_ndt.setTransformationEpsilon(trans_eps);
+        if (_method_type == MethodType::PCL_GENERIC)
+            ndt.setTransformationEpsilon(trans_eps);
+        else if (_method_type == MethodType::PCL_ANH)
+            anh_ndt.setTransformationEpsilon(trans_eps);
 #ifdef CUDA_FOUND
-    else if (_method_type == MethodType::PCL_ANH_GPU)
-      anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+        else if (_method_type == MethodType::PCL_ANH_GPU)
+            anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
 #endif
 #ifdef USE_PCL_OPENMP
-    else if (_method_type == MethodType::PCL_OPENMP)
-      omp_ndt.setTransformationEpsilon(ndt_res);
+        else if (_method_type == MethodType::PCL_OPENMP)
+            omp_ndt.setTransformationEpsilon(ndt_res);
 #endif
-  }
+    }
 
-  if (input->max_iterations != max_iter)
-  {
-    max_iter = input->max_iterations;
+    if (input->max_iterations != max_iter) {
+        max_iter = input->max_iterations;
 
-    if (_method_type == MethodType::PCL_GENERIC)
-      ndt.setMaximumIterations(max_iter);
-    else if (_method_type == MethodType::PCL_ANH)
-      anh_ndt.setMaximumIterations(max_iter);
+        if (_method_type == MethodType::PCL_GENERIC)
+            ndt.setMaximumIterations(max_iter);
+        else if (_method_type == MethodType::PCL_ANH)
+            anh_ndt.setMaximumIterations(max_iter);
 #ifdef CUDA_FOUND
-    else if (_method_type == MethodType::PCL_ANH_GPU)
-      anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
+        else if (_method_type == MethodType::PCL_ANH_GPU)
+            anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
 #endif
 #ifdef USE_PCL_OPENMP
-    else if (_method_type == MethodType::PCL_OPENMP)
-      omp_ndt.setMaximumIterations(ndt_res);
+        else if (_method_type == MethodType::PCL_OPENMP)
+            omp_ndt.setMaximumIterations(ndt_res);
 #endif
-  }
-
-  if (_use_gnss == 0 && init_pos_set == 0)
-  {
-    initial_pose.x = input->x;
-    initial_pose.y = input->y;
-    initial_pose.z = input->z;
-    initial_pose.roll = input->roll;
-    initial_pose.pitch = input->pitch;
-    initial_pose.yaw = input->yaw;
-
-    if (_use_local_transform == true)
-    {
-      tf::Vector3 v(input->x, input->y, input->z);
-      tf::Quaternion q;
-      q.setRPY(input->roll, input->pitch, input->yaw);
-      tf::Transform transform(q, v);
-      initial_pose.x = (local_transform.inverse() * transform).getOrigin().getX();
-      initial_pose.y = (local_transform.inverse() * transform).getOrigin().getY();
-      initial_pose.z = (local_transform.inverse() * transform).getOrigin().getZ();
-
-      tf::Matrix3x3 m(q);
-      m.getRPY(initial_pose.roll, initial_pose.pitch, initial_pose.yaw);
-
-      std::cout << "initial_pose.x: " << initial_pose.x << std::endl;
-      std::cout << "initial_pose.y: " << initial_pose.y << std::endl;
-      std::cout << "initial_pose.z: " << initial_pose.z << std::endl;
-      std::cout << "initial_pose.roll: " << initial_pose.roll << std::endl;
-      std::cout << "initial_pose.pitch: " << initial_pose.pitch << std::endl;
-      std::cout << "initial_pose.yaw: " << initial_pose.yaw << std::endl;
     }
 
-    // Setting position and posture for the first time.
-    localizer_pose.x = initial_pose.x;
-    localizer_pose.y = initial_pose.y;
-    localizer_pose.z = initial_pose.z;
-    localizer_pose.roll = initial_pose.roll;
-    localizer_pose.pitch = initial_pose.pitch;
-    localizer_pose.yaw = initial_pose.yaw;
-
-    previous_pose.x = initial_pose.x;
-    previous_pose.y = initial_pose.y;
-    previous_pose.z = initial_pose.z;
-    previous_pose.roll = initial_pose.roll;
-    previous_pose.pitch = initial_pose.pitch;
-    previous_pose.yaw = initial_pose.yaw;
-
-    current_pose.x = initial_pose.x;
-    current_pose.y = initial_pose.y;
-    current_pose.z = initial_pose.z;
-    current_pose.roll = initial_pose.roll;
-    current_pose.pitch = initial_pose.pitch;
-    current_pose.yaw = initial_pose.yaw;
-
-    current_velocity = 0;
-    current_velocity_x = 0;
-    current_velocity_y = 0;
-    current_velocity_z = 0;
-    angular_velocity = 0;
-
-    current_pose_imu.x = 0;
-    current_pose_imu.y = 0;
-    current_pose_imu.z = 0;
-    current_pose_imu.roll = 0;
-    current_pose_imu.pitch = 0;
-    current_pose_imu.yaw = 0;
-
-    current_velocity_imu_x = current_velocity_x;
-    current_velocity_imu_y = current_velocity_y;
-    current_velocity_imu_z = current_velocity_z;
-    init_pos_set = 1;
-  }
-}
-
-static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
-{
-  // if (map_loaded == 0)
-  if (points_map_num != input->width)
-  {
-    std::cout << "Update points_map." << std::endl;
-
-    points_map_num = input->width;
-
-    // Convert the data type(from sensor_msgs to pcl).
-    pcl::fromROSMsg(*input, map);
-
-    if (_use_local_transform == true)
-    {
-      tf::TransformListener local_transform_listener;
-      try
-      {
-        ros::Time now = ros::Time(0);
-        local_transform_listener.waitForTransform("/map", "/world", now, ros::Duration(10.0));
-        local_transform_listener.lookupTransform("/map", "world", now, local_transform);
-      }
-      catch (tf::TransformException& ex)
-      {
-        ROS_ERROR("%s", ex.what());
-      }
-
-      pcl_ros::transformPointCloud(map, map, local_transform.inverse());
+    if (_use_gnss == 0 && init_pos_set == 0) {
+        initial_pose.x = input->x;
+        initial_pose.y = input->y;
+        initial_pose.z = input->z;
+        initial_pose.roll = input->roll;
+        initial_pose.pitch = input->pitch;
+        initial_pose.yaw = input->yaw;
+
+        if (_use_local_transform == true) {
+            tf::Vector3 v(input->x, input->y, input->z);
+            tf::Quaternion q;
+            q.setRPY(input->roll, input->pitch, input->yaw);
+            tf::Transform transform(q, v);
+            initial_pose.x = (local_transform.inverse() * transform).getOrigin().getX();
+            initial_pose.y = (local_transform.inverse() * transform).getOrigin().getY();
+            initial_pose.z = (local_transform.inverse() * transform).getOrigin().getZ();
+
+            tf::Matrix3x3 m(q);
+            m.getRPY(initial_pose.roll, initial_pose.pitch, initial_pose.yaw);
+
+            std::cout << "initial_pose.x: " << initial_pose.x << std::endl;
+            std::cout << "initial_pose.y: " << initial_pose.y << std::endl;
+            std::cout << "initial_pose.z: " << initial_pose.z << std::endl;
+            std::cout << "initial_pose.roll: " << initial_pose.roll << std::endl;
+            std::cout << "initial_pose.pitch: " << initial_pose.pitch << std::endl;
+            std::cout << "initial_pose.yaw: " << initial_pose.yaw << std::endl;
+        }
+
+        // Setting position and posture for the first time.
+        localizer_pose.x = initial_pose.x;
+        localizer_pose.y = initial_pose.y;
+        localizer_pose.z = initial_pose.z;
+        localizer_pose.roll = initial_pose.roll;
+        localizer_pose.pitch = initial_pose.pitch;
+        localizer_pose.yaw = initial_pose.yaw;
+
+        previous_pose.x = initial_pose.x;
+        previous_pose.y = initial_pose.y;
+        previous_pose.z = initial_pose.z;
+        previous_pose.roll = initial_pose.roll;
+        previous_pose.pitch = initial_pose.pitch;
+        previous_pose.yaw = initial_pose.yaw;
+
+        current_pose.x = initial_pose.x;
+        current_pose.y = initial_pose.y;
+        current_pose.z = initial_pose.z;
+        current_pose.roll = initial_pose.roll;
+        current_pose.pitch = initial_pose.pitch;
+        current_pose.yaw = initial_pose.yaw;
+
+        current_velocity = 0;
+        current_velocity_x = 0;
+        current_velocity_y = 0;
+        current_velocity_z = 0;
+        angular_velocity = 0;
+
+        current_pose_imu.x = 0;
+        current_pose_imu.y = 0;
+        current_pose_imu.z = 0;
+        current_pose_imu.roll = 0;
+        current_pose_imu.pitch = 0;
+        current_pose_imu.yaw = 0;
+
+        current_velocity_imu_x = current_velocity_x;
+        current_velocity_imu_y = current_velocity_y;
+        current_velocity_imu_z = current_velocity_z;
+        init_pos_set = 1;
     }
+}
 
-    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));
-
-    // Setting point cloud to be aligned to.
-    if (_method_type == MethodType::PCL_GENERIC)
-    { 
-      pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;
-      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-      new_ndt.setResolution(ndt_res);
-      
-      new_ndt.setInputTarget(map_ptr);
-      new_ndt.setMaximumIterations(max_iter);
-      new_ndt.setStepSize(step_size);
-      new_ndt.setTransformationEpsilon(trans_eps);
-
-      new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
-
-      pthread_mutex_lock(&mutex);
-      ndt = new_ndt;
-      pthread_mutex_unlock(&mutex);
-    }
-    else if (_method_type == MethodType::PCL_ANH)
-    {
-      cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_anh_ndt;
-      new_anh_ndt.setResolution(ndt_res);
-      new_anh_ndt.setInputTarget(map_ptr);
-      new_anh_ndt.setMaximumIterations(max_iter);
-      new_anh_ndt.setStepSize(step_size);
-      new_anh_ndt.setTransformationEpsilon(trans_eps);
-
-      pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
-      pcl::PointXYZ dummy_point;
-      dummy_scan_ptr->push_back(dummy_point);
-      new_anh_ndt.setInputSource(dummy_scan_ptr);
-
-      new_anh_ndt.align(Eigen::Matrix4f::Identity());
-
-      pthread_mutex_lock(&mutex);
-      anh_ndt = new_anh_ndt;
-      pthread_mutex_unlock(&mutex);
-    }
+static void map_callback(const sensor_msgs::PointCloud2::ConstPtr &input) {
+    // if (map_loaded == 0)
+    if (points_map_num != input->width) {
+        std::cout << "Update points_map." << std::endl;
+
+        points_map_num = input->width;
+
+        // Convert the data type(from sensor_msgs to pcl).
+        pcl::fromROSMsg(*input, map);
+
+        if (_use_local_transform == true) {
+            tf::TransformListener local_transform_listener;
+            try {
+                ros::Time now = ros::Time(0);
+                local_transform_listener.waitForTransform("/map", "/world", now, ros::Duration(10.0));
+                local_transform_listener.lookupTransform("/map", "world", now, local_transform);
+            } catch (tf::TransformException &ex) {
+                ROS_ERROR("%s", ex.what());
+            }
+
+            pcl_ros::transformPointCloud(map, map, local_transform.inverse());
+        }
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));
+
+        // Setting point cloud to be aligned to.
+        if (_method_type == MethodType::PCL_GENERIC) {
+            pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;
+            pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+            new_ndt.setResolution(ndt_res);
+
+            new_ndt.setInputTarget(map_ptr);
+            new_ndt.setMaximumIterations(max_iter);
+            new_ndt.setStepSize(step_size);
+            new_ndt.setTransformationEpsilon(trans_eps);
+
+            new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+            pthread_mutex_lock(&mutex);
+            ndt = new_ndt;
+            pthread_mutex_unlock(&mutex);
+        } else if (_method_type == MethodType::PCL_ANH) {
+            cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_anh_ndt;
+            new_anh_ndt.setResolution(ndt_res);
+            new_anh_ndt.setInputTarget(map_ptr);
+            new_anh_ndt.setMaximumIterations(max_iter);
+            new_anh_ndt.setStepSize(step_size);
+            new_anh_ndt.setTransformationEpsilon(trans_eps);
+
+            pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+            pcl::PointXYZ dummy_point;
+            dummy_scan_ptr->push_back(dummy_point);
+            new_anh_ndt.setInputSource(dummy_scan_ptr);
+
+            new_anh_ndt.align(Eigen::Matrix4f::Identity());
+
+            pthread_mutex_lock(&mutex);
+            anh_ndt = new_anh_ndt;
+            pthread_mutex_unlock(&mutex);
+        }
 #ifdef CUDA_FOUND
-    else if (_method_type == MethodType::PCL_ANH_GPU)
-    {
-      std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
-          std::make_shared<gpu::GNormalDistributionsTransform>();
+        else if (_method_type == MethodType::PCL_ANH_GPU) {
+            std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr = std::make_shared<gpu::GNormalDistributionsTransform>();
 
-      new_anh_gpu_ndt_ptr->setResolution(ndt_res);
-      new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
-      new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
-      new_anh_gpu_ndt_ptr->setStepSize(step_size);
-      new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+            new_anh_gpu_ndt_ptr->setResolution(ndt_res);
+            new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
+            new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
+            new_anh_gpu_ndt_ptr->setStepSize(step_size);
+            new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
 
-      pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+            pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
 
-      pcl::PointXYZ dummy_point;
-      dummy_scan_ptr->push_back(dummy_point);
-      new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);
+            pcl::PointXYZ dummy_point;
+            dummy_scan_ptr->push_back(dummy_point);
+            new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);
 
-      new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());
+            new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());
 
-      pthread_mutex_lock(&mutex);
-      anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
-      pthread_mutex_unlock(&mutex);
-    }
+            pthread_mutex_lock(&mutex);
+            anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
+            pthread_mutex_unlock(&mutex);
+        }
 #endif
 #ifdef USE_PCL_OPENMP
-    else if (_method_type == MethodType::PCL_OPENMP)
-    {
-      pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_omp_ndt;
-      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-      new_omp_ndt.setResolution(ndt_res);
-      new_omp_ndt.setInputTarget(map_ptr);
-      new_omp_ndt.setMaximumIterations(max_iter);
-      new_omp_ndt.setStepSize(step_size);
-      new_omp_ndt.setTransformationEpsilon(trans_eps);
-
-      new_omp_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
-
-      pthread_mutex_lock(&mutex);
-      omp_ndt = new_omp_ndt;
-      pthread_mutex_unlock(&mutex);
-    }
+        else if (_method_type == MethodType::PCL_OPENMP) {
+            pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_omp_ndt;
+            pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+            new_omp_ndt.setResolution(ndt_res);
+            new_omp_ndt.setInputTarget(map_ptr);
+            new_omp_ndt.setMaximumIterations(max_iter);
+            new_omp_ndt.setStepSize(step_size);
+            new_omp_ndt.setTransformationEpsilon(trans_eps);
+
+            new_omp_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+            pthread_mutex_lock(&mutex);
+            omp_ndt = new_omp_ndt;
+            pthread_mutex_unlock(&mutex);
+        }
 #endif
-    map_loaded = 1;
-  }
+        map_loaded = 1;
+    }
 }
 
+// gnss_callback (legacy)
+/*
 static void gnss_callback(const geometry_msgs::PoseStamped::ConstPtr& input)
 {
   tf::Quaternion gnss_q(input->pose.orientation.x, input->pose.orientation.y, input->pose.orientation.z,
@@ -648,1394 +614,1056 @@ static void gnss_callback(const geometry_msgs::PoseStamped::ConstPtr& input)
   }
 
   previous_gnss_pose = current_gnss_pose;
-
 }
+*/
 
-static void initialpose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& input)
-{
-  tf::TransformListener listener;
-  tf::StampedTransform transform;
-  try
-  {
-    ros::Time now = ros::Time(0);
-    listener.waitForTransform("/map", input->header.frame_id, now, ros::Duration(10.0));
-    listener.lookupTransform("/map", input->header.frame_id, now, transform);
-  }
-  catch (tf::TransformException& ex)
-  {
-    ROS_ERROR("%s", ex.what());
-  }
-
-  tf::Quaternion q(input->pose.pose.orientation.x, input->pose.pose.orientation.y, input->pose.pose.orientation.z,
-                   input->pose.pose.orientation.w);
-  tf::Matrix3x3 m(q);
+static void gnss_pose_callback(const geometry_msgs::PoseStamped::ConstPtr &input) {
+    tf::Quaternion gnss_q(input->pose.orientation.x, input->pose.orientation.y, input->pose.orientation.z, input->pose.orientation.w);
+    tf::Matrix3x3 gnss_m(gnss_q);
 
-  if (_use_local_transform == true)
-  {
-    current_pose.x = input->pose.pose.position.x;
-    current_pose.y = input->pose.pose.position.y;
-    current_pose.z = input->pose.pose.position.z;
-  }
-  else
-  {
-    current_pose.x = input->pose.pose.position.x + transform.getOrigin().x();
-    current_pose.y = input->pose.pose.position.y + transform.getOrigin().y();
-    current_pose.z = input->pose.pose.position.z + transform.getOrigin().z();
-  }
-  m.getRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
-
-  if (_get_height == true && map_loaded == 1)
-  {
-    double min_distance = DBL_MAX;
-    double nearest_z = current_pose.z;
-    for (const auto& p : map)
-    {
-      double distance = hypot(current_pose.x - p.x, current_pose.y - p.y);
-      if (distance < min_distance)
-      {
-        min_distance = distance;
-        nearest_z = p.z;
-      }
-    }
-    current_pose.z = nearest_z;
-  }
+    current_gnss_pose.x = input->pose.position.x;
+    current_gnss_pose.y = input->pose.position.y;
+    current_gnss_pose.z = input->pose.position.z;
+    gnss_m.getRPY(current_gnss_pose.roll, current_gnss_pose.pitch, current_gnss_pose.yaw);
 
-  current_pose_imu = current_pose_odom = current_pose_imu_odom = current_pose;
-  previous_pose.x = current_pose.x;
-  previous_pose.y = current_pose.y;
-  previous_pose.z = current_pose.z;
-  previous_pose.roll = current_pose.roll;
-  previous_pose.pitch = current_pose.pitch;
-  previous_pose.yaw = current_pose.yaw;
-
-  current_velocity = 0.0;
-  current_velocity_x = 0.0;
-  current_velocity_y = 0.0;
-  current_velocity_z = 0.0;
-  angular_velocity = 0.0;
-
-  current_accel = 0.0;
-  current_accel_x = 0.0;
-  current_accel_y = 0.0;
-  current_accel_z = 0.0;
-
-  offset_x = 0.0;
-  offset_y = 0.0;
-  offset_z = 0.0;
-  offset_yaw = 0.0;
-
-  offset_imu_x = 0.0;
-  offset_imu_y = 0.0;
-  offset_imu_z = 0.0;
-  offset_imu_roll = 0.0;
-  offset_imu_pitch = 0.0;
-  offset_imu_yaw = 0.0;
-
-  offset_odom_x = 0.0;
-  offset_odom_y = 0.0;
-  offset_odom_z = 0.0;
-  offset_odom_roll = 0.0;
-  offset_odom_pitch = 0.0;
-  offset_odom_yaw = 0.0;
-
-  offset_imu_odom_x = 0.0;
-  offset_imu_odom_y = 0.0;
-  offset_imu_odom_z = 0.0;
-  offset_imu_odom_roll = 0.0;
-  offset_imu_odom_pitch = 0.0;
-  offset_imu_odom_yaw = 0.0;
-
-  previous_score = 0.0;
-
-  init_pos_set = 1;
+    previous_gnss_pose = current_gnss_pose;
 }
 
-static void imu_odom_calc(ros::Time current_time)
-{
-  static ros::Time previous_time = current_time;
-  double diff_time = (current_time - previous_time).toSec();
-
-  double diff_imu_roll = imu.angular_velocity.x * diff_time;
-  double diff_imu_pitch = imu.angular_velocity.y * diff_time;
-  double diff_imu_yaw = imu.angular_velocity.z * diff_time;
-
-  current_pose_imu_odom.roll += diff_imu_roll;
-  current_pose_imu_odom.pitch += diff_imu_pitch;
-  current_pose_imu_odom.yaw += diff_imu_yaw;
-
-  double diff_distance = odom.twist.twist.linear.x * diff_time;
-  offset_imu_odom_x += diff_distance * cos(-current_pose_imu_odom.pitch) * cos(current_pose_imu_odom.yaw);
-  offset_imu_odom_y += diff_distance * cos(-current_pose_imu_odom.pitch) * sin(current_pose_imu_odom.yaw);
-  offset_imu_odom_z += diff_distance * sin(-current_pose_imu_odom.pitch);
-
-  offset_imu_odom_roll += diff_imu_roll;
-  offset_imu_odom_pitch += diff_imu_pitch;
-  offset_imu_odom_yaw += diff_imu_yaw;
-
-  predict_pose_imu_odom.x = previous_pose.x + offset_imu_odom_x;
-  predict_pose_imu_odom.y = previous_pose.y + offset_imu_odom_y;
-  predict_pose_imu_odom.z = previous_pose.z + offset_imu_odom_z;
-  predict_pose_imu_odom.roll = previous_pose.roll + offset_imu_odom_roll;
-  predict_pose_imu_odom.pitch = previous_pose.pitch + offset_imu_odom_pitch;
-  predict_pose_imu_odom.yaw = previous_pose.yaw + offset_imu_odom_yaw;
-
-  previous_time = current_time;
+static void gnss_vel_callback(const geometry_msgs::TwistStamped::ConstPtr &input) {
+    current_gnss_velocity = input->twist.linear.x;
+    current_gnss_angular_velocity = input->twist.angular.z;
 }
 
-static void odom_calc(ros::Time current_time)
-{
-  static ros::Time previous_time = current_time;
-  double diff_time = (current_time - previous_time).toSec();
-
-  double diff_odom_roll = odom.twist.twist.angular.x * diff_time;
-  double diff_odom_pitch = odom.twist.twist.angular.y * diff_time;
-  double diff_odom_yaw = odom.twist.twist.angular.z * diff_time;
-
-  current_pose_odom.roll += diff_odom_roll;
-  current_pose_odom.pitch += diff_odom_pitch;
-  current_pose_odom.yaw += diff_odom_yaw;
-
-  double diff_distance = odom.twist.twist.linear.x * diff_time;
-  offset_odom_x += diff_distance * cos(-current_pose_odom.pitch) * cos(current_pose_odom.yaw);
-  offset_odom_y += diff_distance * cos(-current_pose_odom.pitch) * sin(current_pose_odom.yaw);
-  offset_odom_z += diff_distance * sin(-current_pose_odom.pitch);
-
-  offset_odom_roll += diff_odom_roll;
-  offset_odom_pitch += diff_odom_pitch;
-  offset_odom_yaw += diff_odom_yaw;
-
-  predict_pose_odom.x = previous_pose.x + offset_odom_x;
-  predict_pose_odom.y = previous_pose.y + offset_odom_y;
-  predict_pose_odom.z = previous_pose.z + offset_odom_z;
-  predict_pose_odom.roll = previous_pose.roll + offset_odom_roll;
-  predict_pose_odom.pitch = previous_pose.pitch + offset_odom_pitch;
-  predict_pose_odom.yaw = previous_pose.yaw + offset_odom_yaw;
-
-  previous_time = current_time;
-}
+static void svl_callback(const rubis_msgs::PointCloud2::ConstPtr &points_msg, const rubis_msgs::PoseTwistStamped::ConstPtr &pose_twist_msg) {
+    rubis::start_task_profiling();
 
-static void imu_calc(ros::Time current_time)
-{
-  static ros::Time previous_time = current_time;
-  double diff_time = (current_time - previous_time).toSec();
-
-  double diff_imu_roll = imu.angular_velocity.x * diff_time;
-  double diff_imu_pitch = imu.angular_velocity.y * diff_time;
-  double diff_imu_yaw = imu.angular_velocity.z * diff_time;
-
-  current_pose_imu.roll += diff_imu_roll;
-  current_pose_imu.pitch += diff_imu_pitch;
-  current_pose_imu.yaw += diff_imu_yaw;
-
-  double accX1 = imu.linear_acceleration.x;
-  double accY1 = std::cos(current_pose_imu.roll) * imu.linear_acceleration.y -
-                 std::sin(current_pose_imu.roll) * imu.linear_acceleration.z;
-  double accZ1 = std::sin(current_pose_imu.roll) * imu.linear_acceleration.y +
-                 std::cos(current_pose_imu.roll) * imu.linear_acceleration.z;
-
-  double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
-  double accY2 = accY1;
-  double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
-
-  double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
-  double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
-  double accZ = accZ2;
-
-  offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
-  offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
-  offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
-
-  current_velocity_imu_x += accX * diff_time;
-  current_velocity_imu_y += accY * diff_time;
-  current_velocity_imu_z += accZ * diff_time;
-
-  offset_imu_roll += diff_imu_roll;
-  offset_imu_pitch += diff_imu_pitch;
-  offset_imu_yaw += diff_imu_yaw;
-
-  predict_pose_imu.x = previous_pose.x + offset_imu_x;
-  predict_pose_imu.y = previous_pose.y + offset_imu_y;
-  predict_pose_imu.z = previous_pose.z + offset_imu_z;
-  predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
-  predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
-  predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
-
-  previous_time = current_time;
-}
+    rubis::instance_ = points_msg->instance;
+    rubis::lidar_instance_ = 0;
 
-static double wrapToPm(double a_num, const double a_max)
-{
-  if (a_num >= a_max)
-  {
-    a_num -= 2.0 * a_max;
-  }
-  return a_num;
-}
+    tf::Quaternion gnss_q(pose_twist_msg->pose.pose.orientation.x, pose_twist_msg->pose.pose.orientation.y, pose_twist_msg->pose.pose.orientation.z,
+                          pose_twist_msg->pose.pose.orientation.w);
+    tf::Matrix3x3 gnss_m(gnss_q);
 
-static double wrapToPmPi(const double a_angle_rad)
-{
-  return wrapToPm(a_angle_rad, M_PI);
-}
+    current_gnss_pose.x = pose_twist_msg->pose.pose.position.x;
+    current_gnss_pose.y = pose_twist_msg->pose.pose.position.y;
+    current_gnss_pose.z = pose_twist_msg->pose.pose.position.z;
+    gnss_m.getRPY(current_gnss_pose.roll, current_gnss_pose.pitch, current_gnss_pose.yaw);
 
-static double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
-{
-  double diff_rad = lhs_rad - rhs_rad;
-  if (diff_rad >= M_PI)
-    diff_rad = diff_rad - 2 * M_PI;
-  else if (diff_rad < -M_PI)
-    diff_rad = diff_rad + 2 * M_PI;
-  return diff_rad;
-}
+    previous_gnss_pose = current_gnss_pose;
 
-static void odom_callback(const nav_msgs::Odometry::ConstPtr& input)
-{
-  // std::cout << __func__ << std::endl;
+    current_gnss_velocity = pose_twist_msg->twist.twist.linear.x;
+    current_gnss_angular_velocity = pose_twist_msg->twist.twist.angular.z;
+
+    sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared<const sensor_msgs::PointCloud2>(points_msg->msg);
+    ndt_matching(input);
 
-  odom = *input;
-  odom_calc(input->header.stamp);
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
-static void imuUpsideDown(const sensor_msgs::Imu::Ptr input)
-{
-  double input_roll, input_pitch, input_yaw;
+static void initialpose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &input) {
+    tf::TransformListener listener;
+    tf::StampedTransform transform;
+    try {
+        ros::Time now = ros::Time(0);
+        listener.waitForTransform("/map", input->header.frame_id, now, ros::Duration(10.0));
+        listener.lookupTransform("/map", input->header.frame_id, now, transform);
+    } catch (tf::TransformException &ex) {
+        ROS_ERROR("%s", ex.what());
+    }
 
-  tf::Quaternion input_orientation;
-  tf::quaternionMsgToTF(input->orientation, input_orientation);
-  tf::Matrix3x3(input_orientation).getRPY(input_roll, input_pitch, input_yaw);
+    tf::Quaternion q(input->pose.pose.orientation.x, input->pose.pose.orientation.y, input->pose.pose.orientation.z, input->pose.pose.orientation.w);
+    tf::Matrix3x3 m(q);
+
+    if (_use_local_transform == true) {
+        current_pose.x = input->pose.pose.position.x;
+        current_pose.y = input->pose.pose.position.y;
+        current_pose.z = input->pose.pose.position.z;
+    } else {
+        current_pose.x = input->pose.pose.position.x + transform.getOrigin().x();
+        current_pose.y = input->pose.pose.position.y + transform.getOrigin().y();
+        current_pose.z = input->pose.pose.position.z + transform.getOrigin().z();
+    }
+    m.getRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
+
+    if (_get_height == true && map_loaded == 1) {
+        double min_distance = DBL_MAX;
+        double nearest_z = current_pose.z;
+        for (const auto &p : map) {
+            double distance = hypot(current_pose.x - p.x, current_pose.y - p.y);
+            if (distance < min_distance) {
+                min_distance = distance;
+                nearest_z = p.z;
+            }
+        }
+        current_pose.z = nearest_z;
+    }
 
-  input->angular_velocity.x *= -1;
-  input->angular_velocity.y *= -1;
-  input->angular_velocity.z *= -1;
+    current_pose_imu = current_pose_odom = current_pose_imu_odom = current_pose;
+    previous_pose.x = current_pose.x;
+    previous_pose.y = current_pose.y;
+    previous_pose.z = current_pose.z;
+    previous_pose.roll = current_pose.roll;
+    previous_pose.pitch = current_pose.pitch;
+    previous_pose.yaw = current_pose.yaw;
+
+    current_velocity = 0.0;
+    current_velocity_x = 0.0;
+    current_velocity_y = 0.0;
+    current_velocity_z = 0.0;
+    angular_velocity = 0.0;
+
+    current_accel = 0.0;
+    current_accel_x = 0.0;
+    current_accel_y = 0.0;
+    current_accel_z = 0.0;
 
-  input->linear_acceleration.x *= -1;
-  input->linear_acceleration.y *= -1;
-  input->linear_acceleration.z *= -1;
+    offset_x = 0.0;
+    offset_y = 0.0;
+    offset_z = 0.0;
+    offset_yaw = 0.0;
 
-  input_roll *= -1;
-  input_pitch *= -1;
-  input_yaw *= -1;
+    offset_imu_x = 0.0;
+    offset_imu_y = 0.0;
+    offset_imu_z = 0.0;
+    offset_imu_roll = 0.0;
+    offset_imu_pitch = 0.0;
+    offset_imu_yaw = 0.0;
+
+    offset_odom_x = 0.0;
+    offset_odom_y = 0.0;
+    offset_odom_z = 0.0;
+    offset_odom_roll = 0.0;
+    offset_odom_pitch = 0.0;
+    offset_odom_yaw = 0.0;
+
+    offset_imu_odom_x = 0.0;
+    offset_imu_odom_y = 0.0;
+    offset_imu_odom_z = 0.0;
+    offset_imu_odom_roll = 0.0;
+    offset_imu_odom_pitch = 0.0;
+    offset_imu_odom_yaw = 0.0;
 
-  input->orientation = tf::createQuaternionMsgFromRollPitchYaw(input_roll, input_pitch, input_yaw);
+    previous_score = 0.0;
+
+    init_pos_set = 1;
 }
 
-static void imu_callback(const sensor_msgs::Imu::Ptr& input)
-{
-  // std::cout << __func__ << std::endl;
-
-  if (_imu_upside_down)
-    imuUpsideDown(input);
-
-  const ros::Time current_time = input->header.stamp;
-  static ros::Time previous_time = current_time;
-  const double diff_time = (current_time - previous_time).toSec();
-
-  double imu_roll, imu_pitch, imu_yaw;
-  tf::Quaternion imu_orientation;
-  tf::quaternionMsgToTF(input->orientation, imu_orientation);
-  tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
-
-  imu_roll = wrapToPmPi(imu_roll);
-  imu_pitch = wrapToPmPi(imu_pitch);
-  imu_yaw = wrapToPmPi(imu_yaw);
-
-  static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
-  const double diff_imu_roll = calcDiffForRadian(imu_roll, previous_imu_roll);
-  const double diff_imu_pitch = calcDiffForRadian(imu_pitch, previous_imu_pitch);
-  const double diff_imu_yaw = calcDiffForRadian(imu_yaw, previous_imu_yaw);
-
-  imu.header = input->header;
-  imu.linear_acceleration.x = input->linear_acceleration.x;
-  // imu.linear_acceleration.y = input->linear_acceleration.y;
-  // imu.linear_acceleration.z = input->linear_acceleration.z;
-  imu.linear_acceleration.y = 0;
-  imu.linear_acceleration.z = 0;
-
-  if (diff_time != 0)
-  {
-    imu.angular_velocity.x = diff_imu_roll / diff_time;
-    imu.angular_velocity.y = diff_imu_pitch / diff_time;
-    imu.angular_velocity.z = diff_imu_yaw / diff_time;
-  }
-  else
-  {
-    imu.angular_velocity.x = 0;
-    imu.angular_velocity.y = 0;
-    imu.angular_velocity.z = 0;
-  }
+static void imu_odom_calc(ros::Time current_time) {
+    static ros::Time previous_time = current_time;
+    double diff_time = (current_time - previous_time).toSec();
+
+    double diff_imu_roll = imu.angular_velocity.x * diff_time;
+    double diff_imu_pitch = imu.angular_velocity.y * diff_time;
+    double diff_imu_yaw = imu.angular_velocity.z * diff_time;
 
-  imu_calc(input->header.stamp);
+    current_pose_imu_odom.roll += diff_imu_roll;
+    current_pose_imu_odom.pitch += diff_imu_pitch;
+    current_pose_imu_odom.yaw += diff_imu_yaw;
 
-  previous_time = current_time;
-  previous_imu_roll = imu_roll;
-  previous_imu_pitch = imu_pitch;
-  previous_imu_yaw = imu_yaw;
+    double diff_distance = odom.twist.twist.linear.x * diff_time;
+    offset_imu_odom_x += diff_distance * cos(-current_pose_imu_odom.pitch) * cos(current_pose_imu_odom.yaw);
+    offset_imu_odom_y += diff_distance * cos(-current_pose_imu_odom.pitch) * sin(current_pose_imu_odom.yaw);
+    offset_imu_odom_z += diff_distance * sin(-current_pose_imu_odom.pitch);
+
+    offset_imu_odom_roll += diff_imu_roll;
+    offset_imu_odom_pitch += diff_imu_pitch;
+    offset_imu_odom_yaw += diff_imu_yaw;
+
+    predict_pose_imu_odom.x = previous_pose.x + offset_imu_odom_x;
+    predict_pose_imu_odom.y = previous_pose.y + offset_imu_odom_y;
+    predict_pose_imu_odom.z = previous_pose.z + offset_imu_odom_z;
+    predict_pose_imu_odom.roll = previous_pose.roll + offset_imu_odom_roll;
+    predict_pose_imu_odom.pitch = previous_pose.pitch + offset_imu_odom_pitch;
+    predict_pose_imu_odom.yaw = previous_pose.yaw + offset_imu_odom_yaw;
+
+    previous_time = current_time;
 }
 
-static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input)
-{ 
-  static int match_cnt = 10;
+static void odom_calc(ros::Time current_time) {
+    static ros::Time previous_time = current_time;
+    double diff_time = (current_time - previous_time).toSec();
 
-  if (map_loaded != 1 || init_pos_set != 1) return;
+    double diff_odom_roll = odom.twist.twist.angular.x * diff_time;
+    double diff_odom_pitch = odom.twist.twist.angular.y * diff_time;
+    double diff_odom_yaw = odom.twist.twist.angular.z * diff_time;
 
-  // Check inital matching is success or not
-  if(_is_init_match_finished == false){
+    current_pose_odom.roll += diff_odom_roll;
+    current_pose_odom.pitch += diff_odom_pitch;
+    current_pose_odom.yaw += diff_odom_yaw;
 
-    if(previous_score < _init_match_threshold) match_cnt--;
-    else match_cnt = 10;
+    double diff_distance = odom.twist.twist.linear.x * diff_time;
+    offset_odom_x += diff_distance * cos(-current_pose_odom.pitch) * cos(current_pose_odom.yaw);
+    offset_odom_y += diff_distance * cos(-current_pose_odom.pitch) * sin(current_pose_odom.yaw);
+    offset_odom_z += diff_distance * sin(-current_pose_odom.pitch);
 
-    if(previous_score < _init_match_threshold && previous_score != 0.0 && match_cnt <0){
-      _is_init_match_finished = true;
-      #ifdef DEBUG
-        std::cout<<"Success initial matching!"<<std::endl;
-      #endif
-    }
-  }
+    offset_odom_roll += diff_odom_roll;
+    offset_odom_pitch += diff_odom_pitch;
+    offset_odom_yaw += diff_odom_yaw;
 
-  health_checker_ptr_->CHECK_RATE("topic_rate_filtered_points_slow", 8, 5, 1, "topic filtered_points subscribe rate slow.");
+    predict_pose_odom.x = previous_pose.x + offset_odom_x;
+    predict_pose_odom.y = previous_pose.y + offset_odom_y;
+    predict_pose_odom.z = previous_pose.z + offset_odom_z;
+    predict_pose_odom.roll = previous_pose.roll + offset_odom_roll;
+    predict_pose_odom.pitch = previous_pose.pitch + offset_odom_pitch;
+    predict_pose_odom.yaw = previous_pose.yaw + offset_odom_yaw;
 
-  matching_start = std::chrono::system_clock::now();
+    previous_time = current_time;
+}
 
-  static tf::TransformBroadcaster br, kalman_br;
-  tf::Transform transform, kalman_transform;
-  tf::Quaternion predict_q, ndt_q, current_q, localizer_q, kalman_q;
+static void imu_calc(ros::Time current_time) {
+    static ros::Time previous_time = current_time;
+    double diff_time = (current_time - previous_time).toSec();
 
-  pcl::PointXYZ p;
-  pcl::PointCloud<pcl::PointXYZ> filtered_scan;
+    double diff_imu_roll = imu.angular_velocity.x * diff_time;
+    double diff_imu_pitch = imu.angular_velocity.y * diff_time;
+    double diff_imu_yaw = imu.angular_velocity.z * diff_time;
 
-  ros::Time current_scan_time = input->header.stamp;
-  static ros::Time previous_scan_time = current_scan_time;
+    current_pose_imu.roll += diff_imu_roll;
+    current_pose_imu.pitch += diff_imu_pitch;
+    current_pose_imu.yaw += diff_imu_yaw;
 
-  pcl::fromROSMsg(*input, filtered_scan);
-  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan));
-  int scan_points_num = filtered_scan_ptr->size();
+    double accX1 = imu.linear_acceleration.x;
+    double accY1 = std::cos(current_pose_imu.roll) * imu.linear_acceleration.y - std::sin(current_pose_imu.roll) * imu.linear_acceleration.z;
+    double accZ1 = std::sin(current_pose_imu.roll) * imu.linear_acceleration.y + std::cos(current_pose_imu.roll) * imu.linear_acceleration.z;
 
-  Eigen::Matrix4f t(Eigen::Matrix4f::Identity());   // base_link
-  Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());  // localizer
+    double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+    double accY2 = accY1;
+    double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
 
-  std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
-      getFitnessScore_end;
-  static double align_time, getFitnessScore_time = 0.0;
+    double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+    double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+    double accZ = accZ2;
 
-  pthread_mutex_lock(&mutex);
+    offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+    offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+    offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
 
-  if (_method_type == MethodType::PCL_GENERIC)
-    ndt.setInputSource(filtered_scan_ptr);
-  else if (_method_type == MethodType::PCL_ANH)
-    anh_ndt.setInputSource(filtered_scan_ptr);
-#ifdef CUDA_FOUND
-  else if (_method_type == MethodType::PCL_ANH_GPU){
-    anh_gpu_ndt_ptr->setInputSource(filtered_scan_ptr);
-  }
-#endif
-#ifdef USE_PCL_OPENMP
-  else if (_method_type == MethodType::PCL_OPENMP)
-    omp_ndt.setInputSource(filtered_scan_ptr);
-#endif
+    current_velocity_imu_x += accX * diff_time;
+    current_velocity_imu_y += accY * diff_time;
+    current_velocity_imu_z += accZ * diff_time;
 
-  // Guess the initial gross estimation of the transformation
-  double diff_time = (current_scan_time - previous_scan_time).toSec();
+    offset_imu_roll += diff_imu_roll;
+    offset_imu_pitch += diff_imu_pitch;
+    offset_imu_yaw += diff_imu_yaw;
 
-  if (_offset == "linear")
-  {
-    offset_x = current_velocity_x * diff_time;
-    offset_y = current_velocity_y * diff_time;
-    offset_z = current_velocity_z * diff_time;
-    offset_yaw = angular_velocity * diff_time;
-  }
-  else if (_offset == "quadratic")
-  {
-    offset_x = (current_velocity_x + current_accel_x * diff_time) * diff_time;
-    offset_y = (current_velocity_y + current_accel_y * diff_time) * diff_time;
-    offset_z = current_velocity_z * diff_time;
-    offset_yaw = angular_velocity * diff_time;
-  }
-  else if (_offset == "zero")
-  {
-    offset_x = 0.0;
-    offset_y = 0.0;
-    offset_z = 0.0;
-    offset_yaw = 0.0;
-  }
+    predict_pose_imu.x = previous_pose.x + offset_imu_x;
+    predict_pose_imu.y = previous_pose.y + offset_imu_y;
+    predict_pose_imu.z = previous_pose.z + offset_imu_z;
+    predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+    predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+    predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
 
-  predict_pose.x = previous_pose.x + offset_x;
-  predict_pose.y = previous_pose.y + offset_y;
-  predict_pose.z = previous_pose.z + offset_z;
-  predict_pose.roll = previous_pose.roll;
-  predict_pose.pitch = previous_pose.pitch;
-  predict_pose.yaw = previous_pose.yaw + offset_yaw;
-
-  if (_use_imu == true && _use_odom == true)
-    imu_odom_calc(current_scan_time);
-  if (_use_imu == true && _use_odom == false)
-    imu_calc(current_scan_time);
-  if (_use_imu == false && _use_odom == true)
-    odom_calc(current_scan_time);
-
-  pose predict_pose_for_ndt;
-  if (_use_imu == true && _use_odom == true)
-    predict_pose_for_ndt = predict_pose_imu_odom;
-  else if (_use_imu == true && _use_odom == false)
-    predict_pose_for_ndt = predict_pose_imu;
-  else if (_use_imu == false && _use_odom == true)
-    predict_pose_for_ndt = predict_pose_odom;
-  else
-    predict_pose_for_ndt = predict_pose;
+    previous_time = current_time;
+}
 
-  Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
-  Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
-  Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
-  Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
-  Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
+static double wrapToPm(double a_num, const double a_max) {
+    if (a_num >= a_max) {
+        a_num -= 2.0 * a_max;
+    }
+    return a_num;
+}
 
-  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+static double wrapToPmPi(const double a_angle_rad) { return wrapToPm(a_angle_rad, M_PI); }
 
-  if (_method_type == MethodType::PCL_GENERIC)
-  {
-    align_start = std::chrono::system_clock::now();
-    ndt.align(*output_cloud, init_guess);
-    align_end = std::chrono::system_clock::now();
+static double calcDiffForRadian(const double lhs_rad, const double rhs_rad) {
+    double diff_rad = lhs_rad - rhs_rad;
+    if (diff_rad >= M_PI)
+        diff_rad = diff_rad - 2 * M_PI;
+    else if (diff_rad < -M_PI)
+        diff_rad = diff_rad + 2 * M_PI;
+    return diff_rad;
+}
 
-    has_converged = ndt.hasConverged();
+static void imuUpsideDown(const sensor_msgs::Imu::Ptr input) {
+    double input_roll, input_pitch, input_yaw;
 
-    t = ndt.getFinalTransformation();
-    iteration = ndt.getFinalNumIteration();
+    tf::Quaternion input_orientation;
+    tf::quaternionMsgToTF(input->orientation, input_orientation);
+    tf::Matrix3x3(input_orientation).getRPY(input_roll, input_pitch, input_yaw);
 
-    getFitnessScore_start = std::chrono::system_clock::now();
-    fitness_score = ndt.getFitnessScore();
-    getFitnessScore_end = std::chrono::system_clock::now();
+    input->angular_velocity.x *= -1;
+    input->angular_velocity.y *= -1;
+    input->angular_velocity.z *= -1;
 
-    trans_probability = ndt.getTransformationProbability();
-  }
-  else if (_method_type == MethodType::PCL_ANH)
-  {
-    align_start = std::chrono::system_clock::now();
-    anh_ndt.align(init_guess);
-    align_end = std::chrono::system_clock::now();
+    input->linear_acceleration.x *= -1;
+    input->linear_acceleration.y *= -1;
+    input->linear_acceleration.z *= -1;
 
-    has_converged = anh_ndt.hasConverged();
+    input_roll *= -1;
+    input_pitch *= -1;
+    input_yaw *= -1;
 
-    t = anh_ndt.getFinalTransformation();
-    iteration = anh_ndt.getFinalNumIteration();
+    input->orientation = tf::createQuaternionMsgFromRollPitchYaw(input_roll, input_pitch, input_yaw);
+}
 
-    getFitnessScore_start = std::chrono::system_clock::now();
-    fitness_score = anh_ndt.getFitnessScore();
-    getFitnessScore_end = std::chrono::system_clock::now();
+static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr &input) {
+    if (_use_svl_gnss) {
+        static tf::TransformBroadcaster br;
+        tf::Quaternion gnss_q;
+        tf::Transform transform;
+        ros::Time current_scan_time = input->header.stamp;
+
+        current_pose = current_gnss_pose;
+        ndt_pose = current_gnss_pose;
+
+        current_velocity = current_gnss_velocity;
+        angular_velocity = current_gnss_angular_velocity;
+
+        gnss_q.setRPY(current_gnss_pose.roll, current_gnss_pose.pitch, current_gnss_pose.yaw);
+        ndt_pose_msg.header.frame_id = "/map";
+        ndt_pose_msg.header.stamp = current_scan_time;
+        ndt_pose_msg.pose.position.x = current_gnss_pose.x;
+        ndt_pose_msg.pose.position.y = current_gnss_pose.y;
+        ndt_pose_msg.pose.position.z = current_gnss_pose.z;
+        ndt_pose_msg.pose.orientation.x = gnss_q.x();
+        ndt_pose_msg.pose.orientation.y = gnss_q.y();
+        ndt_pose_msg.pose.orientation.z = gnss_q.z();
+        ndt_pose_msg.pose.orientation.w = gnss_q.w();
+
+        estimate_twist_msg.header.stamp = current_scan_time;
+        estimate_twist_msg.header.frame_id = "/base_link";
+        estimate_twist_msg.twist.linear.x = current_velocity;
+        estimate_twist_msg.twist.linear.y = 0.0;
+        estimate_twist_msg.twist.linear.z = 0.0;
+        estimate_twist_msg.twist.angular.x = 0.0;
+        estimate_twist_msg.twist.angular.y = 0.0;
+        estimate_twist_msg.twist.angular.z = angular_velocity;
+
+        transform.setOrigin(tf::Vector3(current_gnss_pose.x, current_gnss_pose.y, current_gnss_pose.z));
+        transform.setRotation(gnss_q);
+        br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
+
+        rubis_pose_twist_msg.header.stamp = input->header.stamp;
+        rubis_pose_twist_msg.instance = rubis::instance_;
+        rubis_pose_twist_msg.pose = ndt_pose_msg;
+        rubis_pose_twist_msg.twist = estimate_twist_msg;
+        rubis_pose_twist_pub.publish(rubis_pose_twist_msg);
+
+        return;
+    }
 
-    trans_probability = anh_ndt.getTransformationProbability();
-  }
-#ifdef CUDA_FOUND
-  else if (_method_type == MethodType::PCL_ANH_GPU)
-  {
-    align_start = std::chrono::system_clock::now();
-    anh_gpu_ndt_ptr->align(init_guess);
-    align_end = std::chrono::system_clock::now();
+    /* BLOCK1: Check initial matching */
+    static int match_cnt = 10;
 
-    has_converged = anh_gpu_ndt_ptr->hasConverged();
+    if (map_loaded != 1 || init_pos_set != 1)
+        return;
 
-    t = anh_gpu_ndt_ptr->getFinalTransformation();
-    iteration = anh_gpu_ndt_ptr->getFinalNumIteration();
+    // Check inital matching is success or not
+    if (_is_init_match_finished == false) {
 
-    getFitnessScore_start = std::chrono::system_clock::now();
-    fitness_score = anh_gpu_ndt_ptr->getFitnessScore();
-    getFitnessScore_end = std::chrono::system_clock::now();
+        if (previous_score < _init_match_threshold)
+            match_cnt--;
+        else
+            match_cnt = 10;
 
-    trans_probability = anh_gpu_ndt_ptr->getTransformationProbability();      
-  }
+        if (previous_score < _init_match_threshold && previous_score != 0.0 && match_cnt < 0) {
+            _is_init_match_finished = true;
+#ifdef DEBUG
+            std::cout << "Success initial matching!" << std::endl;
 #endif
-#ifdef USE_PCL_OPENMP
-  else if (_method_type == MethodType::PCL_OPENMP)
-  {
-    align_start = std::chrono::system_clock::now();
-    omp_ndt.align(*output_cloud, init_guess);
-    align_end = std::chrono::system_clock::now();
+        }
+    }
 
-    has_converged = omp_ndt.hasConverged();
+    matching_start = std::chrono::system_clock::now();
 
-    t = omp_ndt.getFinalTransformation();
-    iteration = omp_ndt.getFinalNumIteration();
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion predict_q, ndt_q, current_q, localizer_q, gnss_q;
 
-    getFitnessScore_start = std::chrono::system_clock::now();
-    fitness_score = omp_ndt.getFitnessScore();
-    getFitnessScore_end = std::chrono::system_clock::now();
+    pcl::PointXYZ p;
+    pcl::PointCloud<pcl::PointXYZ> filtered_scan;
 
-    trans_probability = omp_ndt.getTransformationProbability();
-  }
+    ros::Time current_scan_time = input->header.stamp;
+    static ros::Time previous_scan_time = current_scan_time;
+
+    pcl::fromROSMsg(*input, filtered_scan);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan));
+    int scan_points_num = filtered_scan_ptr->size();
+
+    Eigen::Matrix4f t(Eigen::Matrix4f::Identity());  // base_link
+    Eigen::Matrix4f t2(Eigen::Matrix4f::Identity()); // localizer
+
+    std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start, getFitnessScore_end;
+    static double align_time, getFitnessScore_time = 0.0;
+
+    pthread_mutex_lock(&mutex);
+
+    if (_method_type == MethodType::PCL_GENERIC)
+        ndt.setInputSource(filtered_scan_ptr);
+    else if (_method_type == MethodType::PCL_ANH)
+        anh_ndt.setInputSource(filtered_scan_ptr);
+#ifdef CUDA_FOUND
+    else if (_method_type == MethodType::PCL_ANH_GPU) {
+        anh_gpu_ndt_ptr->setInputSource(filtered_scan_ptr);
+    }
+#endif
+#ifdef USE_PCL_OPENMP
+    else if (_method_type == MethodType::PCL_OPENMP)
+        omp_ndt.setInputSource(filtered_scan_ptr);
 #endif
-  align_time = std::chrono::duration_cast<std::chrono::microseconds>(align_end - align_start).count() / 1000.0;
 
-  t2 = t * tf_btol.inverse();
+    // Guess the initial gross estimation of the transformation
+    double diff_time = (current_scan_time - previous_scan_time).toSec();
+
+    if (_offset == "linear") {
+        offset_x = current_velocity_x * diff_time;
+        offset_y = current_velocity_y * diff_time;
+        offset_z = current_velocity_z * diff_time;
+        offset_yaw = angular_velocity * diff_time;
+    } else if (_offset == "quadratic") {
+        offset_x = (current_velocity_x + current_accel_x * diff_time) * diff_time;
+        offset_y = (current_velocity_y + current_accel_y * diff_time) * diff_time;
+        offset_z = current_velocity_z * diff_time;
+        offset_yaw = angular_velocity * diff_time;
+    } else if (_offset == "zero") {
+        offset_x = 0.0;
+        offset_y = 0.0;
+        offset_z = 0.0;
+        offset_yaw = 0.0;
+    }
 
-  getFitnessScore_time =
-      std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
-      1000.0;
+    predict_pose.x = previous_pose.x + offset_x;
+    predict_pose.y = previous_pose.y + offset_y;
+    predict_pose.z = previous_pose.z + offset_z;
+    predict_pose.roll = previous_pose.roll;
+    predict_pose.pitch = previous_pose.pitch;
+    predict_pose.yaw = previous_pose.yaw + offset_yaw;
+
+    if (_use_imu == true && _use_odom == true)
+        imu_odom_calc(current_scan_time);
+    if (_use_imu == true && _use_odom == false)
+        imu_calc(current_scan_time);
+    if (_use_imu == false && _use_odom == true)
+        odom_calc(current_scan_time);
+
+    pose predict_pose_for_ndt;
+    if (_use_imu == true && _use_odom == true)
+        predict_pose_for_ndt = predict_pose_imu_odom;
+    else if (_use_imu == true && _use_odom == false)
+        predict_pose_for_ndt = predict_pose_imu;
+    else if (_use_imu == false && _use_odom == true)
+        predict_pose_for_ndt = predict_pose_odom;
+    else
+        predict_pose_for_ndt = predict_pose;
 
-  pthread_mutex_unlock(&mutex);
+    Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
+    Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
+    Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
+    Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
 
-  tf::Matrix3x3 mat_l;  // localizer
-  mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
-                  static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
-                  static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
+    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
-  // Update localizer_pose
-  localizer_pose.x = t(0, 3);
-  localizer_pose.y = t(1, 3);
-  localizer_pose.z = t(2, 3);
-  mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
+    if (_method_type == MethodType::PCL_GENERIC) {
+        align_start = std::chrono::system_clock::now();
+        ndt.align(*output_cloud, init_guess);
+        align_end = std::chrono::system_clock::now();
 
-  tf::Matrix3x3 mat_b;  // base_link
-  mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
-                  static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
-                  static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
+        has_converged = ndt.hasConverged();
 
-  // Update ndt_pose
-  ndt_pose.x = t2(0, 3);
-  ndt_pose.y = t2(1, 3);
-  ndt_pose.z = t2(2, 3);
-  mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
+        t = ndt.getFinalTransformation();
+        iteration = ndt.getFinalNumIteration();
 
-  // Calculate the difference between ndt_pose and predict_pose
-  predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
-                            (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
-                            (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
+        getFitnessScore_start = std::chrono::system_clock::now();
+        fitness_score = ndt.getFitnessScore();
+        getFitnessScore_end = std::chrono::system_clock::now();
 
-  if (predict_pose_error <= PREDICT_POSE_THRESHOLD)
-  {
-    use_predict_pose = 0;
-  }
-  else
-  {
-    use_predict_pose = 1;
-  }
-  use_predict_pose = 0;
-
-  if (use_predict_pose == 0)
-  {
-    current_pose.x = ndt_pose.x;
-    current_pose.y = ndt_pose.y;
-    current_pose.z = ndt_pose.z;
-    current_pose.roll = ndt_pose.roll;
-    current_pose.pitch = ndt_pose.pitch;
-    current_pose.yaw = ndt_pose.yaw;
-  }
-  else
-  {
-    current_pose.x = predict_pose_for_ndt.x;
-    current_pose.y = predict_pose_for_ndt.y;
-    current_pose.z = predict_pose_for_ndt.z;
-    current_pose.roll = predict_pose_for_ndt.roll;
-    current_pose.pitch = predict_pose_for_ndt.pitch;
-    current_pose.yaw = predict_pose_for_ndt.yaw;
-  }
+        trans_probability = ndt.getTransformationProbability();
+    } else if (_method_type == MethodType::PCL_ANH) {
+        align_start = std::chrono::system_clock::now();
+        anh_ndt.align(init_guess);
+        align_end = std::chrono::system_clock::now();
 
-  // Compute the velocity and acceleration
-  diff_x = current_pose.x - previous_pose.x;
-  diff_y = current_pose.y - previous_pose.y;
-  diff_z = current_pose.z - previous_pose.z;
-  diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
-  diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
-
-  const pose trans_current_pose = convertPoseIntoRelativeCoordinate(current_pose, previous_pose);
-
-  current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
-  current_velocity =  (trans_current_pose.x >= 0) ? current_velocity : -current_velocity;
-  current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
-  current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
-  current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
-  angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
-
-  current_pose_imu.x = current_pose.x;
-  current_pose_imu.y = current_pose.y;
-  current_pose_imu.z = current_pose.z;
-  current_pose_imu.roll = current_pose.roll;
-  current_pose_imu.pitch = current_pose.pitch;
-  current_pose_imu.yaw = current_pose.yaw;
-
-  current_velocity_imu_x = current_velocity_x;
-  current_velocity_imu_y = current_velocity_y;
-  current_velocity_imu_z = current_velocity_z;
-
-  current_pose_odom.x = current_pose.x;
-  current_pose_odom.y = current_pose.y;
-  current_pose_odom.z = current_pose.z;
-  current_pose_odom.roll = current_pose.roll;
-  current_pose_odom.pitch = current_pose.pitch;
-  current_pose_odom.yaw = current_pose.yaw;
-
-  current_pose_imu_odom.x = current_pose.x;
-  current_pose_imu_odom.y = current_pose.y;
-  current_pose_imu_odom.z = current_pose.z;
-  current_pose_imu_odom.roll = current_pose.roll;
-  current_pose_imu_odom.pitch = current_pose.pitch;
-  current_pose_imu_odom.yaw = current_pose.yaw;
-
-  current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
-  if (std::fabs(current_velocity_smooth) < 0.2)
-  {
-    current_velocity_smooth = 0.0;
-  }
+        has_converged = anh_ndt.hasConverged();
 
-  current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
-  current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
-  current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
-  current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
+        t = anh_ndt.getFinalTransformation();
+        iteration = anh_ndt.getFinalNumIteration();
 
-  estimated_vel_mps.data = current_velocity;
-  estimated_vel_kmph.data = current_velocity * 3.6;
+        getFitnessScore_start = std::chrono::system_clock::now();
+        fitness_score = anh_ndt.getFitnessScore();
+        getFitnessScore_end = std::chrono::system_clock::now();
 
-  estimated_vel_mps_pub.publish(estimated_vel_mps);
-  estimated_vel_kmph_pub.publish(estimated_vel_kmph);
+        trans_probability = anh_ndt.getTransformationProbability();
+    }
+#ifdef CUDA_FOUND
+    else if (_method_type == MethodType::PCL_ANH_GPU) {
+        align_start = std::chrono::system_clock::now();
+        anh_gpu_ndt_ptr->align(init_guess);
+        align_end = std::chrono::system_clock::now();
 
-  // Enable Kalman Filter
-  if(!_is_kalman_filter_on && _use_kalman_filter && _is_init_match_finished && _is_ins_stat_received){
-    _is_kalman_filter_on = true;
-    linear_kalman_filter.set_init_pose(current_pose.x, current_pose.y);
-  }
+        has_converged = anh_gpu_ndt_ptr->hasConverged();
+
+        t = anh_gpu_ndt_ptr->getFinalTransformation();
+        iteration = anh_gpu_ndt_ptr->getFinalNumIteration();
+
+        getFitnessScore_start = std::chrono::system_clock::now();
+        fitness_score = anh_gpu_ndt_ptr->getFitnessScore();
+        getFitnessScore_end = std::chrono::system_clock::now();
 
-  // Run Kalman Filter
-  if(_is_kalman_filter_on){
-    Eigen::Vector2f u_k, z_k;
-
-    u_k << _previous_ins_stat_vel_x + 0.5f * _previous_ins_stat_acc_x * diff_time, _previous_ins_stat_vel_y + 0.5f * _previous_ins_stat_acc_y * diff_time;
-    z_k << current_pose.x, current_pose.y;
-
-    Eigen::Vector2f kalman_filtered_pose = linear_kalman_filter.run(diff_time, u_k, z_k);
-
-    current_kalman_pose.x = kalman_filtered_pose(0);
-    current_kalman_pose.y = kalman_filtered_pose(1);
-    current_kalman_pose.z = current_pose.z;
-    current_kalman_pose.roll = 0.0;
-    current_kalman_pose.pitch = 0.0;
-    current_kalman_pose.yaw = _current_ins_stat_yaw*M_PI/180.0;
-
-    // Check ndt matching failure    
-    double ndt_kalman_pose_diff = sqrt(pow(current_pose.x - current_kalman_pose.x,2) + pow(current_pose.y - current_kalman_pose.y, 2));
-
-    // std::cout<<"## pose diff: "<<ndt_kalman_pose_diff<< " | score diff: "<< abs(previous_score - fitness_score)<<std::endl;
-    // std::cout<<"fail score threshold: "<< _failure_score_diff_threshold <<" | restore score threshold: "<<_recovery_score_diff_threshold<<std::endl;\
-    // std::cout<<"fail pose threshold: "<< _failure_pose_diff_threshold <<" | restore pose threshold: "<<_recovery_pose_diff_threshold<<std::endl;
-    static int success_cnt = 1;
-    if(!_is_matching_failed && (ndt_kalman_pose_diff > _failure_pose_diff_threshold || abs(previous_score - fitness_score) > _failure_score_diff_threshold)){ 
-      #ifdef DEBUG
-        std::cout<<"NDT matching is FAILED! || FAILED?"<<_is_matching_failed <<" | score diff: " << abs(previous_score - fitness_score) << "|| pose_diff: " << ndt_kalman_pose_diff <<std::endl;
-      #endif
-      
-      _is_matching_failed = true;
-      
-    }    
-    else if( _is_matching_failed && (ndt_kalman_pose_diff < _recovery_pose_diff_threshold && abs(_previous_success_score - fitness_score) < _recovery_score_diff_threshold)){ // Recover success
-      if(success_cnt-- < 0){
-        #ifdef DEBUG
-        std::cout<<"NDT matching is ON! || FAILED?"<<_is_matching_failed << " | current score: "<<fitness_score<<" || previous_success_score: "<<_previous_success_score<<" || pose_diff: "<<ndt_kalman_pose_diff<<std::endl;
-        #endif
-        
-        _is_matching_failed = false;
-        success_cnt = 1;
-        
-      }
+        trans_probability = anh_gpu_ndt_ptr->getTransformationProbability();
     }
+#endif
+#ifdef USE_PCL_OPENMP
+    else if (_method_type == MethodType::PCL_OPENMP) {
+        align_start = std::chrono::system_clock::now();
+        omp_ndt.align(*output_cloud, init_guess);
+        align_end = std::chrono::system_clock::now();
 
-    if(_is_matching_failed){
-      linear_kalman_filter.restore();
-      kalman_filtered_pose = linear_kalman_filter.run_without_update(diff_time, u_k);
+        has_converged = omp_ndt.hasConverged();
 
-      current_kalman_pose.x = kalman_filtered_pose(0);
-      current_kalman_pose.y = kalman_filtered_pose(1);
-      current_kalman_pose.z = 0.0;
+        t = omp_ndt.getFinalTransformation();
+        iteration = omp_ndt.getFinalNumIteration();
 
-      std::cout<<"[Restore] pose_diff]: "<< ndt_kalman_pose_diff << " | score diff: "<< abs(previous_score - fitness_score) <<std::endl;
+        getFitnessScore_start = std::chrono::system_clock::now();
+        fitness_score = omp_ndt.getFitnessScore();
+        getFitnessScore_end = std::chrono::system_clock::now();
 
-      // current_kalman_pose.roll = 0.0;
-      // current_kalman_pose.pitch = 0.0;
-      // current_kalman_pose.yaw = _current_ins_stat_yaw*M_PI/180.0;
+        trans_probability = omp_ndt.getTransformationProbability();
     }
-  }
+#endif
+    align_time = std::chrono::duration_cast<std::chrono::microseconds>(align_end - align_start).count() / 1000.0;
 
-  // Set values for publishing pose
-  predict_q.setRPY(predict_pose.roll, predict_pose.pitch, predict_pose.yaw);
-  if (_use_local_transform == true)
-  {
-    tf::Vector3 v(predict_pose.x, predict_pose.y, predict_pose.z);
-    tf::Transform transform(predict_q, v);
-    predict_pose_msg.header.frame_id = "/map";
-    predict_pose_msg.header.stamp = current_scan_time;
-    predict_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
-    predict_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
-    predict_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
-    predict_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
-    predict_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
-    predict_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
-    predict_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
-  }
-  else
-  {
-    predict_pose_msg.header.frame_id = "/map";
-    predict_pose_msg.header.stamp = current_scan_time;
-    predict_pose_msg.pose.position.x = predict_pose.x;
-    predict_pose_msg.pose.position.y = predict_pose.y;
-    predict_pose_msg.pose.position.z = predict_pose.z;
-    predict_pose_msg.pose.orientation.x = predict_q.x();
-    predict_pose_msg.pose.orientation.y = predict_q.y();
-    predict_pose_msg.pose.orientation.z = predict_q.z();
-    predict_pose_msg.pose.orientation.w = predict_q.w();
-  }
+    t2 = t * tf_btol.inverse();
 
-  tf::Quaternion predict_q_imu;
-  predict_q_imu.setRPY(predict_pose_imu.roll, predict_pose_imu.pitch, predict_pose_imu.yaw);
-  predict_pose_imu_msg.header.frame_id = "map";
-  predict_pose_imu_msg.header.stamp = input->header.stamp;
-  predict_pose_imu_msg.pose.position.x = predict_pose_imu.x;
-  predict_pose_imu_msg.pose.position.y = predict_pose_imu.y;
-  predict_pose_imu_msg.pose.position.z = predict_pose_imu.z;
-  predict_pose_imu_msg.pose.orientation.x = predict_q_imu.x();
-  predict_pose_imu_msg.pose.orientation.y = predict_q_imu.y();
-  predict_pose_imu_msg.pose.orientation.z = predict_q_imu.z();
-  predict_pose_imu_msg.pose.orientation.w = predict_q_imu.w();
-  predict_pose_imu_pub.publish(predict_pose_imu_msg);
-
-  tf::Quaternion predict_q_odom;
-  predict_q_odom.setRPY(predict_pose_odom.roll, predict_pose_odom.pitch, predict_pose_odom.yaw);
-  predict_pose_odom_msg.header.frame_id = "map";
-  predict_pose_odom_msg.header.stamp = input->header.stamp;
-  predict_pose_odom_msg.pose.position.x = predict_pose_odom.x;
-  predict_pose_odom_msg.pose.position.y = predict_pose_odom.y;
-  predict_pose_odom_msg.pose.position.z = predict_pose_odom.z;
-  predict_pose_odom_msg.pose.orientation.x = predict_q_odom.x();
-  predict_pose_odom_msg.pose.orientation.y = predict_q_odom.y();
-  predict_pose_odom_msg.pose.orientation.z = predict_q_odom.z();
-  predict_pose_odom_msg.pose.orientation.w = predict_q_odom.w();
-  predict_pose_odom_pub.publish(predict_pose_odom_msg);
-
-  tf::Quaternion predict_q_imu_odom;
-  predict_q_imu_odom.setRPY(predict_pose_imu_odom.roll, predict_pose_imu_odom.pitch, predict_pose_imu_odom.yaw);
-  predict_pose_imu_odom_msg.header.frame_id = "map";
-  predict_pose_imu_odom_msg.header.stamp = input->header.stamp;
-  predict_pose_imu_odom_msg.pose.position.x = predict_pose_imu_odom.x;
-  predict_pose_imu_odom_msg.pose.position.y = predict_pose_imu_odom.y;
-  predict_pose_imu_odom_msg.pose.position.z = predict_pose_imu_odom.z;
-  predict_pose_imu_odom_msg.pose.orientation.x = predict_q_imu_odom.x();
-  predict_pose_imu_odom_msg.pose.orientation.y = predict_q_imu_odom.y();
-  predict_pose_imu_odom_msg.pose.orientation.z = predict_q_imu_odom.z();
-  predict_pose_imu_odom_msg.pose.orientation.w = predict_q_imu_odom.w();
-  predict_pose_imu_odom_pub.publish(predict_pose_imu_odom_msg);
-
-  ndt_q.setRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw);
-  
-  if(_is_kalman_filter_on){
-    kalman_q.setRPY(current_kalman_pose.roll, current_kalman_pose.pitch, current_kalman_pose.yaw);
-  }
-  
-  if(_is_matching_failed && _is_kalman_filter_on){
-    ndt_pose_msg.header.frame_id = "/map";
-    ndt_pose_msg.header.stamp = current_scan_time;
-    ndt_pose_msg.pose.position.x = current_kalman_pose.x;
-    ndt_pose_msg.pose.position.y = current_kalman_pose.y;
-    ndt_pose_msg.pose.position.z = current_kalman_pose.z;
-    ndt_pose_msg.pose.orientation.x = kalman_q.x();
-    ndt_pose_msg.pose.orientation.y = kalman_q.y();
-    ndt_pose_msg.pose.orientation.z = kalman_q.z();
-    ndt_pose_msg.pose.orientation.w = kalman_q.w();
-  }
-  else if (_use_local_transform == true){
-    tf::Vector3 v(ndt_pose.x, ndt_pose.y, ndt_pose.z);
-    tf::Transform transform(ndt_q, v);
-    ndt_pose_msg.header.frame_id = "/map";
-    ndt_pose_msg.header.stamp = current_scan_time;
-    ndt_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
-    ndt_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
-    ndt_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
-    ndt_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
-    ndt_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
-    ndt_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
-    ndt_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
-  }
-  else{
-    ndt_pose_msg.header.frame_id = "/map";
-    ndt_pose_msg.header.stamp = current_scan_time;
-    ndt_pose_msg.pose.position.x = ndt_pose.x;
-    ndt_pose_msg.pose.position.y = ndt_pose.y;
-    ndt_pose_msg.pose.position.z = ndt_pose.z;
-    ndt_pose_msg.pose.orientation.x = ndt_q.x();
-    ndt_pose_msg.pose.orientation.y = ndt_q.y();
-    ndt_pose_msg.pose.orientation.z = ndt_q.z();
-    ndt_pose_msg.pose.orientation.w = ndt_q.w();    
-  }
+    getFitnessScore_time = std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() / 1000.0;
 
+    pthread_mutex_unlock(&mutex);
 
-  current_q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
-  // current_pose is published by vel_pose_mux
-  /*
-  current_pose_msg.header.frame_id = "/map";
-  current_pose_msg.header.stamp = current_scan_time;
-  current_pose_msg.pose.position.x = current_pose.x;
-  current_pose_msg.pose.position.y = current_pose.y;
-  current_pose_msg.pose.position.z = current_pose.z;
-  current_pose_msg.pose.orientation.x = current_q.x();
-  current_pose_msg.pose.orientation.y = current_q.y();
-  current_pose_msg.pose.orientation.z = current_q.z();
-  current_pose_msg.pose.orientation.w = current_q.w();
-  */
-
-  localizer_q.setRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw);
-  
-  if (_use_local_transform == true)
-  {
-    tf::Vector3 v(localizer_pose.x, localizer_pose.y, localizer_pose.z);
-    tf::Transform transform(localizer_q, v);
-    localizer_pose_msg.header.frame_id = "/map";
-    localizer_pose_msg.header.stamp = current_scan_time;
-    localizer_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
-    localizer_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
-    localizer_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
-    localizer_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
-    localizer_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
-    localizer_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
-    localizer_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
-  }
-  else
-  {
-    localizer_pose_msg.header.frame_id = "/map";
-    localizer_pose_msg.header.stamp = current_scan_time;
-    localizer_pose_msg.pose.position.x = localizer_pose.x;
-    localizer_pose_msg.pose.position.y = localizer_pose.y;
-    localizer_pose_msg.pose.position.z = localizer_pose.z;
-    localizer_pose_msg.pose.orientation.x = localizer_q.x();
-    localizer_pose_msg.pose.orientation.y = localizer_q.y();
-    localizer_pose_msg.pose.orientation.z = localizer_q.z();
-    localizer_pose_msg.pose.orientation.w = localizer_q.w();
-  }
+    tf::Matrix3x3 mat_l; // localizer
+    mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)), static_cast<double>(t(1, 0)),
+                   static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)), static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)),
+                   static_cast<double>(t(2, 2)));
 
-  predict_pose_pub.publish(predict_pose_msg);
-  health_checker_ptr_->CHECK_RATE("topic_rate_ndt_pose_slow", 8, 5, 1, "topic ndt_pose publish rate slow.");
-  ndt_pose_pub.publish(ndt_pose_msg);
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+    // Update localizer_pose
+    localizer_pose.x = t(0, 3);
+    localizer_pose.y = t(1, 3);
+    localizer_pose.z = t(2, 3);
+    mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
 
-  if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){
-    rubis_ndt_pose_msg.instance = rubis::instance_;
-    rubis_ndt_pose_msg.msg = ndt_pose_msg;
-    rubis_ndt_pose_pub.publish(rubis_ndt_pose_msg);
-  }
+    tf::Matrix3x3 mat_b; // base_link
+    mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)), static_cast<double>(t2(1, 0)),
+                   static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)), static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)),
+                   static_cast<double>(t2(2, 2)));
 
-  // current_pose is published by vel_pose_mux
-  //    current_pose_pub.publish(current_pose_msg);
-  localizer_pose_pub.publish(localizer_pose_msg);
-
-  matching_end = std::chrono::system_clock::now();
-  exe_time = std::chrono::duration_cast<std::chrono::microseconds>(matching_end - matching_start).count() / 1000.0;
-  time_ndt_matching.data = exe_time;
-  health_checker_ptr_->CHECK_MAX_VALUE("time_ndt_matching", time_ndt_matching.data, 50, 70, 100, "value time_ndt_matching is too high.");
-  time_ndt_matching_pub.publish(time_ndt_matching);
-
-  // Set values for /estimate_twist
-  estimate_twist_msg.header.stamp = current_scan_time;
-  estimate_twist_msg.header.frame_id = "/base_link";
-  estimate_twist_msg.twist.linear.x = current_velocity;
-  estimate_twist_msg.twist.linear.y = 0.0;
-  estimate_twist_msg.twist.linear.z = 0.0;
-  estimate_twist_msg.twist.angular.x = 0.0;
-  estimate_twist_msg.twist.angular.y = 0.0;
-  estimate_twist_msg.twist.angular.z = angular_velocity;
-
-  estimate_twist_pub.publish(estimate_twist_msg);
-
-  geometry_msgs::Vector3Stamped estimate_vel_msg;
-  estimate_vel_msg.header.stamp = current_scan_time;
-  estimate_vel_msg.vector.x = current_velocity;
-  health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_linear", current_velocity, 5, 10, 15, "value linear estimated twist is too high.");
-  health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_angular", angular_velocity, 5, 10, 15, "value linear angular twist is too high.");
-  estimated_vel_pub.publish(estimate_vel_msg);
-
-  previous_score = fitness_score;
-  if(!_is_matching_failed) _previous_success_score = previous_score;
-
-  // Set values for /ndt_stat
-  ndt_stat_msg.header.stamp = current_scan_time;
-  ndt_stat_msg.exe_time = time_ndt_matching.data;
-  ndt_stat_msg.iteration = iteration;
-  ndt_stat_msg.score = fitness_score;
-  ndt_stat_msg.velocity = current_velocity;
-  ndt_stat_msg.acceleration = current_accel;
-  ndt_stat_msg.use_predict_pose = 0;
-
-  ndt_stat_pub.publish(ndt_stat_msg);
-
-
-
-
-  /* Compute NDT_Reliability */
-  ndt_reliability.data = Wa * (exe_time / 100.0) * 100.0 + Wb * (iteration / 10.0) * 100.0 +
-                          Wc * ((2.0 - trans_probability) / 2.0) * 100.0;
-  ndt_reliability_pub.publish(ndt_reliability);
-
-  // Write log
-  if(_output_log_data)
-  {
-    if (!ofs)
-    {
-      std::cerr << "Could not open " << filename << "." << std::endl;
+    // Update ndt_pose
+    ndt_pose.x = t2(0, 3);
+    ndt_pose.y = t2(1, 3);
+    ndt_pose.z = t2(2, 3);
+    mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
+
+    // Calculate the difference between ndt_pose and predict_pose
+    predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
+                              (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
+                              (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
+
+    if (predict_pose_error <= PREDICT_POSE_THRESHOLD) {
+        use_predict_pose = 0;
+    } else {
+        use_predict_pose = 1;
     }
-    else
-    {
-      ofs << input->header.seq << "," << scan_points_num << "," << step_size << "," << trans_eps << "," << std::fixed
-          << std::setprecision(5) << current_pose.x << "," << std::fixed << std::setprecision(5) << current_pose.y << ","
-          << std::fixed << std::setprecision(5) << current_pose.z << "," << current_pose.roll << "," << current_pose.pitch
-          << "," << current_pose.yaw << "," << predict_pose.x << "," << predict_pose.y << "," << predict_pose.z << ","
-          << predict_pose.roll << "," << predict_pose.pitch << "," << predict_pose.yaw << ","
-          << current_pose.x - predict_pose.x << "," << current_pose.y - predict_pose.y << ","
-          << current_pose.z - predict_pose.z << "," << current_pose.roll - predict_pose.roll << ","
-          << current_pose.pitch - predict_pose.pitch << "," << current_pose.yaw - predict_pose.yaw << ","
-          << predict_pose_error << "," << iteration << "," << fitness_score << "," << trans_probability << ","
-          << ndt_reliability.data << "," << current_velocity << "," << current_velocity_smooth << "," << current_accel
-          << "," << angular_velocity << "," << time_ndt_matching.data << "," << align_time << "," << getFitnessScore_time
-          << std::endl;
+    use_predict_pose = 0;
+
+    if (use_predict_pose == 0) {
+        current_pose.x = ndt_pose.x;
+        current_pose.y = ndt_pose.y;
+        current_pose.z = ndt_pose.z;
+        current_pose.roll = ndt_pose.roll;
+        current_pose.pitch = ndt_pose.pitch;
+        current_pose.yaw = ndt_pose.yaw;
+    } else {
+        current_pose.x = predict_pose_for_ndt.x;
+        current_pose.y = predict_pose_for_ndt.y;
+        current_pose.z = predict_pose_for_ndt.z;
+        current_pose.roll = predict_pose_for_ndt.roll;
+        current_pose.pitch = predict_pose_for_ndt.pitch;
+        current_pose.yaw = predict_pose_for_ndt.yaw;
     }
-  }
-  
-  // std::cout << "-----------------------------------------------------------------" << std::endl;
-  // std::cout << "Sequence: " << input->header.seq << std::endl;
-  // std::cout << "Timestamp: " << input->header.stamp << std::endl;
-  // std::cout << "Frame ID: " << input->header.frame_id << std::endl;
-  // //    std::cout << "Number of Scan Points: " << scan_ptr->size() << " points." << std::endl;
-  // std::cout << "Number of Filtered Scan Points: " << scan_points_num << " points." << std::endl;
-  // std::cout << "NDT has converged: " << has_converged << std::endl;
-  // std::cout << "Fitness Score: " << fitness_score << std::endl;
-  // std::cout << "Transformation Probability: " << trans_probability << std::endl;
-  // std::cout << "Execution Time: " << exe_time << " ms." << std::endl;
-  // std::cout << "Number of Iterations: " << iteration << std::endl;
-  // std::cout << "NDT Reliability: " << ndt_reliability.data << std::endl;
-  // std::cout << "(x,y,z,roll,pitch,yaw): " << std::endl;
-  // std::cout << "(" << current_pose.x << ", " << current_pose.y << ", " << current_pose.z << ", " << current_pose.roll
-  //           << ", " << current_pose.pitch << ", " << current_pose.yaw << ")" << std::endl;
-  // std::cout << "Transformation Matrix: " << std::endl;
-  // std::cout << t << std::endl;
-  // std::cout << "Align time: " << align_time << std::endl;
-  // std::cout << "Get fitness score time: " << getFitnessScore_time << std::endl;
-  // std::cout << "-----------------------------------------------------------------" << std::endl;
-
-  offset_imu_x = 0.0;
-  offset_imu_y = 0.0;
-  offset_imu_z = 0.0;
-  offset_imu_roll = 0.0;
-  offset_imu_pitch = 0.0;
-  offset_imu_yaw = 0.0;
-
-  offset_odom_x = 0.0;
-  offset_odom_y = 0.0;
-  offset_odom_z = 0.0;
-  offset_odom_roll = 0.0;
-  offset_odom_pitch = 0.0;
-  offset_odom_yaw = 0.0;
-
-  offset_imu_odom_x = 0.0;
-  offset_imu_odom_y = 0.0;
-  offset_imu_odom_z = 0.0;
-  offset_imu_odom_roll = 0.0;
-  offset_imu_odom_pitch = 0.0;
-  offset_imu_odom_yaw = 0.0;
-
-  // Send TF "/base_link" to "/map"
-  if(!_is_matching_failed){
-    transform.setOrigin(tf::Vector3(current_pose.x, current_pose.y, current_pose.z));
-    transform.setRotation(current_q);
-    //    br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
-    if (_use_local_transform == true)
-    {
-      br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, "/map", "/base_link"));
+
+    // Compute the velocity and acceleration
+    diff_x = current_pose.x - previous_pose.x;
+    diff_y = current_pose.y - previous_pose.y;
+    diff_z = current_pose.z - previous_pose.z;
+    diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
+    diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
+
+    const pose trans_current_pose = convertPoseIntoRelativeCoordinate(current_pose, previous_pose);
+
+    current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
+    current_velocity = (trans_current_pose.x >= 0) ? current_velocity : -current_velocity;
+    current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
+    current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
+    current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
+    angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
+
+    current_pose_imu.x = current_pose.x;
+    current_pose_imu.y = current_pose.y;
+    current_pose_imu.z = current_pose.z;
+    current_pose_imu.roll = current_pose.roll;
+    current_pose_imu.pitch = current_pose.pitch;
+    current_pose_imu.yaw = current_pose.yaw;
+
+    current_velocity_imu_x = current_velocity_x;
+    current_velocity_imu_y = current_velocity_y;
+    current_velocity_imu_z = current_velocity_z;
+
+    current_pose_odom.x = current_pose.x;
+    current_pose_odom.y = current_pose.y;
+    current_pose_odom.z = current_pose.z;
+    current_pose_odom.roll = current_pose.roll;
+    current_pose_odom.pitch = current_pose.pitch;
+    current_pose_odom.yaw = current_pose.yaw;
+
+    current_pose_imu_odom.x = current_pose.x;
+    current_pose_imu_odom.y = current_pose.y;
+    current_pose_imu_odom.z = current_pose.z;
+    current_pose_imu_odom.roll = current_pose.roll;
+    current_pose_imu_odom.pitch = current_pose.pitch;
+    current_pose_imu_odom.yaw = current_pose.yaw;
+
+    current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
+    if (std::fabs(current_velocity_smooth) < 0.2) {
+        current_velocity_smooth = 0.0;
     }
-    else
-    {
-      br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
+
+    current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
+    current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
+    current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
+    current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
+
+    // Set values for publishing pose
+    predict_q.setRPY(predict_pose.roll, predict_pose.pitch, predict_pose.yaw);
+    if (_use_local_transform == true) {
+        tf::Vector3 v(predict_pose.x, predict_pose.y, predict_pose.z);
+        tf::Transform transform(predict_q, v);
+        predict_pose_msg.header.frame_id = "/map";
+        predict_pose_msg.header.stamp = current_scan_time;
+        predict_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
+        predict_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
+        predict_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
+        predict_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
+        predict_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
+        predict_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
+        predict_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
+    } else {
+        predict_pose_msg.header.frame_id = "/map";
+        predict_pose_msg.header.stamp = current_scan_time;
+        predict_pose_msg.pose.position.x = predict_pose.x;
+        predict_pose_msg.pose.position.y = predict_pose.y;
+        predict_pose_msg.pose.position.z = predict_pose.z;
+        predict_pose_msg.pose.orientation.x = predict_q.x();
+        predict_pose_msg.pose.orientation.y = predict_q.y();
+        predict_pose_msg.pose.orientation.z = predict_q.z();
+        predict_pose_msg.pose.orientation.w = predict_q.w();
     }
-  }
-  else{ // When matching is failed
-    transform.setOrigin(tf::Vector3(current_kalman_pose.x, current_kalman_pose.y, current_kalman_pose.z));
-    current_q.setRPY(current_kalman_pose.roll, current_kalman_pose.pitch, current_kalman_pose.yaw);      
-    transform.setRotation(current_q);
-    br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
-  }
-  
-  // Send TF "/kalman" to "/map"
-  if(_is_kalman_filter_on){
-    kalman_transform.setOrigin(tf::Vector3(current_kalman_pose.x, current_kalman_pose.y, current_kalman_pose.z));    
-    kalman_transform.setRotation(kalman_q);
-    kalman_br.sendTransform(tf::StampedTransform(kalman_transform, current_scan_time, "/map", "/kalman"));
-  }
 
-  // Update previous_*** when kalman filter is not enabled
-  previous_pose.x = current_pose.x;
-  previous_pose.y = current_pose.y;
-  previous_pose.z = current_pose.z;
-  previous_pose.roll = current_pose.roll;
-  previous_pose.pitch = current_pose.pitch;
-  previous_pose.yaw = current_pose.yaw;
-
-  previous_scan_time = current_scan_time;
-
-  previous_previous_velocity = previous_velocity;
-  previous_velocity = current_velocity;
-  previous_velocity_x = current_velocity_x;
-  previous_velocity_y = current_velocity_y;
-  previous_velocity_z = current_velocity_z;
-  previous_accel = current_accel;
-
-  previous_estimated_vel_kmph.data = estimated_vel_kmph.data;    
-
-  if(_is_kalman_filter_on){
-    geometry_msgs::Quaternion q;
-    ToQuaternion(current_kalman_pose.yaw, current_kalman_pose.pitch, current_kalman_pose.roll, q);
-
-    geometry_msgs::PoseStamped kalman_filtered_pose_msg;
-    kalman_filtered_pose_msg.header = ndt_pose_msg.header;
-    kalman_filtered_pose_msg.pose.position.x = current_kalman_pose.x;
-    kalman_filtered_pose_msg.pose.position.y = current_kalman_pose.y;
-    kalman_filtered_pose_msg.pose.position.z = current_kalman_pose.z;
-    kalman_filtered_pose_msg.pose.orientation = q;
-    kalman_filtered_pose_pub.publish(kalman_filtered_pose_msg);
-
-    // Update previous by kalman filter output
-    previous_kalman_pose.x = current_kalman_pose.x;
-    previous_kalman_pose.y = current_kalman_pose.y;
-    previous_kalman_pose.z = current_kalman_pose.z;
-    previous_kalman_pose.roll = current_kalman_pose.roll;
-    previous_kalman_pose.pitch = current_kalman_pose.pitch;
-    previous_kalman_pose.yaw = current_kalman_pose.yaw;
-
-    if(_is_matching_failed) previous_pose = previous_kalman_pose;
-
-    _previous_ins_stat_vel_x = _current_ins_stat_vel_x;
-    _previous_ins_stat_vel_y = _current_ins_stat_vel_y;
-    _previous_ins_stat_acc_x = _current_ins_stat_acc_x;
-    _previous_ins_stat_acc_y = _current_ins_stat_acc_y;
-    _previous_ins_stat_yaw = _current_ins_stat_yaw;
-    _previous_ins_stat_linear_velocity = _current_ins_stat_linear_velocity;
-    _previous_ins_stat_linear_acceleration = _current_ins_stat_linear_acceleration;
-    _previous_ins_stat_angular_velocity = _current_ins_stat_angular_velocity;
+    // tf::Quaternion predict_q_imu;
+    // predict_q_imu.setRPY(predict_pose_imu.roll, predict_pose_imu.pitch, predict_pose_imu.yaw);
+    // predict_pose_imu_msg.header.frame_id = "/map";
+    // predict_pose_imu_msg.header.stamp = input->header.stamp;
+    // predict_pose_imu_msg.pose.position.x = predict_pose_imu.x;
+    // predict_pose_imu_msg.pose.position.y = predict_pose_imu.y;
+    // predict_pose_imu_msg.pose.position.z = predict_pose_imu.z;
+    // predict_pose_imu_msg.pose.orientation.x = predict_q_imu.x();
+    // predict_pose_imu_msg.pose.orientation.y = predict_q_imu.y();
+    // predict_pose_imu_msg.pose.orientation.z = predict_q_imu.z();
+    // predict_pose_imu_msg.pose.orientation.w = predict_q_imu.w();
+    // predict_pose_imu_pub.publish(predict_pose_imu_msg);
+
+    // tf::Quaternion predict_q_odom;
+    // predict_q_odom.setRPY(predict_pose_odom.roll, predict_pose_odom.pitch, predict_pose_odom.yaw);
+    // predict_pose_odom_msg.header.frame_id = "/map";
+    // predict_pose_odom_msg.header.stamp = input->header.stamp;
+    // predict_pose_odom_msg.pose.position.x = predict_pose_odom.x;
+    // predict_pose_odom_msg.pose.position.y = predict_pose_odom.y;
+    // predict_pose_odom_msg.pose.position.z = predict_pose_odom.z;
+    // predict_pose_odom_msg.pose.orientation.x = predict_q_odom.x();
+    // predict_pose_odom_msg.pose.orientation.y = predict_q_odom.y();
+    // predict_pose_odom_msg.pose.orientation.z = predict_q_odom.z();
+    // predict_pose_odom_msg.pose.orientation.w = predict_q_odom.w();
+    // predict_pose_odom_pub.publish(predict_pose_odom_msg);
+
+    // tf::Quaternion predict_q_imu_odom;
+    // predict_q_imu_odom.setRPY(predict_pose_imu_odom.roll, predict_pose_imu_odom.pitch, predict_pose_imu_odom.yaw);
+    // predict_pose_imu_odom_msg.header.frame_id = "/map";
+    // predict_pose_imu_odom_msg.header.stamp = input->header.stamp;
+    // predict_pose_imu_odom_msg.pose.position.x = predict_pose_imu_odom.x;
+    // predict_pose_imu_odom_msg.pose.position.y = predict_pose_imu_odom.y;
+    // predict_pose_imu_odom_msg.pose.position.z = predict_pose_imu_odom.z;
+    // predict_pose_imu_odom_msg.pose.orientation.x = predict_q_imu_odom.x();
+    // predict_pose_imu_odom_msg.pose.orientation.y = predict_q_imu_odom.y();
+    // predict_pose_imu_odom_msg.pose.orientation.z = predict_q_imu_odom.z();
+    // predict_pose_imu_odom_msg.pose.orientation.w = predict_q_imu_odom.w();
+    // predict_pose_imu_odom_pub.publish(predict_pose_imu_odom_msg);
+
+    ndt_q.setRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw);
+
+    if (_use_local_transform == true) {
+        tf::Vector3 v(ndt_pose.x, ndt_pose.y, ndt_pose.z);
+        tf::Transform transform(ndt_q, v);
+        ndt_pose_msg.header.frame_id = "/map";
+        ndt_pose_msg.header.stamp = current_scan_time;
+        ndt_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
+        ndt_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
+        ndt_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
+        ndt_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
+        ndt_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
+        ndt_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
+        ndt_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
+    } else {
+        ndt_pose_msg.header.frame_id = "/map";
+        ndt_pose_msg.header.stamp = current_scan_time;
+        ndt_pose_msg.pose.position.x = ndt_pose.x;
+        ndt_pose_msg.pose.position.y = ndt_pose.y;
+        ndt_pose_msg.pose.position.z = ndt_pose.z;
+        ndt_pose_msg.pose.orientation.x = ndt_q.x();
+        ndt_pose_msg.pose.orientation.y = ndt_q.y();
+        ndt_pose_msg.pose.orientation.z = ndt_q.z();
+        ndt_pose_msg.pose.orientation.w = ndt_q.w();
+    }
 
-  }
+    current_q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
+    localizer_q.setRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw);
+
+    if (_use_local_transform == true) {
+        tf::Vector3 v(localizer_pose.x, localizer_pose.y, localizer_pose.z);
+        tf::Transform transform(localizer_q, v);
+        localizer_pose_msg.header.frame_id = "/map";
+        localizer_pose_msg.header.stamp = current_scan_time;
+        localizer_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
+        localizer_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
+        localizer_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
+        localizer_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
+        localizer_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
+        localizer_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
+        localizer_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
+    } else {
+        localizer_pose_msg.header.frame_id = "/map";
+        localizer_pose_msg.header.stamp = current_scan_time;
+        localizer_pose_msg.pose.position.x = localizer_pose.x;
+        localizer_pose_msg.pose.position.y = localizer_pose.y;
+        localizer_pose_msg.pose.position.z = localizer_pose.z;
+        localizer_pose_msg.pose.orientation.x = localizer_q.x();
+        localizer_pose_msg.pose.orientation.y = localizer_q.y();
+        localizer_pose_msg.pose.orientation.z = localizer_q.z();
+        localizer_pose_msg.pose.orientation.w = localizer_q.w();
+    }
 
-  std_msgs::Bool is_kalman_filter_on_msgs;
-  is_kalman_filter_on_msgs.data = _is_kalman_filter_on;
-  is_kalman_filter_on_pub.publish(is_kalman_filter_on_msgs);
-  
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY){
-    rubis::sched::init_task();
-    if(rubis::sched::gpu_profiling_flag_) rubis::sched::start_gpu_profiling();
-  }  
-}
+    localizer_pose_pub.publish(localizer_pose_msg);
+
+    matching_end = std::chrono::system_clock::now();
+    exe_time = std::chrono::duration_cast<std::chrono::microseconds>(matching_end - matching_start).count() / 1000.0;
+    time_ndt_matching.data = exe_time;
+    time_ndt_matching_pub.publish(time_ndt_matching);
+
+    // Set values for /estimate_twist
+    estimate_twist_msg.header.stamp = current_scan_time;
+    estimate_twist_msg.header.frame_id = "/base_link";
+    estimate_twist_msg.twist.linear.x = current_velocity;
+    estimate_twist_msg.twist.linear.y = 0.0;
+    estimate_twist_msg.twist.linear.z = 0.0;
+    estimate_twist_msg.twist.angular.x = 0.0;
+    estimate_twist_msg.twist.angular.y = 0.0;
+    estimate_twist_msg.twist.angular.z = angular_velocity;
+
+    estimate_twist_pub.publish(estimate_twist_msg);
+
+    geometry_msgs::Vector3Stamped estimate_vel_msg;
+    estimate_vel_msg.header.stamp = current_scan_time;
+    estimate_vel_msg.vector.x = current_velocity;
+
+    previous_score = fitness_score;
+    if (!_is_matching_failed)
+        _previous_success_score = previous_score;
+
+    // Set values for /ndt_stat
+    ndt_stat_msg.header.stamp = current_scan_time;
+    ndt_stat_msg.exe_time = time_ndt_matching.data;
+    ndt_stat_msg.iteration = iteration;
+    ndt_stat_msg.score = fitness_score;
+    ndt_stat_msg.velocity = current_velocity;
+    ndt_stat_msg.acceleration = current_accel;
+    ndt_stat_msg.use_predict_pose = 0;
+
+    ndt_stat_pub.publish(ndt_stat_msg);
+
+    rubis_pose_twist_msg.header.stamp = input->header.stamp;
+    rubis_pose_twist_msg.instance = rubis::instance_;
+    rubis_pose_twist_msg.pose = ndt_pose_msg;
+    rubis_pose_twist_msg.twist = estimate_twist_msg;
+    rubis_pose_twist_pub.publish(rubis_pose_twist_msg);
+
+    // std::cout << "-----------------------------------------------------------------" << std::endl;
+    // std::cout << "Sequence: " << input->header.seq << std::endl;
+    // std::cout << "Timestamp: " << input->header.stamp << std::endl;
+    // std::cout << "Frame ID: " << input->header.frame_id << std::endl;
+    // //    std::cout << "Number of Scan Points: " << scan_ptr->size() << " points." << std::endl;
+    // std::cout << "Number of Filtered Scan Points: " << scan_points_num << " points." << std::endl;
+    // std::cout << "NDT has converged: " << has_converged << std::endl;
+    // std::cout << "Fitness Score: " << fitness_score << std::endl;
+    // std::cout << "Transformation Probability: " << trans_probability << std::endl;
+    // std::cout << "Execution Time: " << exe_time << " ms." << std::endl;
+    // std::cout << "Number of Iterations: " << iteration << std::endl;
+    // std::cout << "(x,y,z,roll,pitch,yaw): " << std::endl;
+    // std::cout << "(" << current_pose.x << ", " << current_pose.y << ", " << current_pose.z << ", " << current_pose.roll
+    //           << ", " << current_pose.pitch << ", " << current_pose.yaw << ")" << std::endl;
+    // std::cout << "Transformation Matrix: " << std::endl;
+    // std::cout << t << std::endl;
+    // std::cout << "Align time: " << align_time << std::endl;
+    // std::cout << "Get fitness score time: " << getFitnessScore_time << std::endl;
+    // std::cout << "-----------------------------------------------------------------" << std::endl;
+
+    offset_imu_x = 0.0;
+    offset_imu_y = 0.0;
+    offset_imu_z = 0.0;
+    offset_imu_roll = 0.0;
+    offset_imu_pitch = 0.0;
+    offset_imu_yaw = 0.0;
+
+    offset_odom_x = 0.0;
+    offset_odom_y = 0.0;
+    offset_odom_z = 0.0;
+    offset_odom_roll = 0.0;
+    offset_odom_pitch = 0.0;
+    offset_odom_yaw = 0.0;
+
+    offset_imu_odom_x = 0.0;
+    offset_imu_odom_y = 0.0;
+    offset_imu_odom_z = 0.0;
+    offset_imu_odom_roll = 0.0;
+    offset_imu_odom_pitch = 0.0;
+    offset_imu_odom_yaw = 0.0;
+
+    // Send TF "/base_link" to "/map"
+    if (!_is_matching_failed) {
+        transform.setOrigin(tf::Vector3(current_pose.x, current_pose.y, current_pose.z));
+        transform.setRotation(current_q);
+        //    br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
+        if (_use_local_transform == true) {
+            br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, "/map", "/base_link"));
+        } else {
+            br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
+        }
+    }
 
-static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input){
-  rubis::instance_ = RUBIS_NO_INSTANCE;
-  ndt_matching(input);
+    previous_pose.x = current_pose.x;
+    previous_pose.y = current_pose.y;
+    previous_pose.z = current_pose.z;
+    previous_pose.roll = current_pose.roll;
+    previous_pose.pitch = current_pose.pitch;
+    previous_pose.yaw = current_pose.yaw;
+
+    previous_scan_time = current_scan_time;
+
+    previous_previous_velocity = previous_velocity;
+    previous_velocity = current_velocity;
+    previous_velocity_x = current_velocity_x;
+    previous_velocity_y = current_velocity_y;
+    previous_velocity_z = current_velocity_z;
+    previous_accel = current_accel;
 }
 
-static void rubis_points_callback(const rubis_msgs::PointCloud2::ConstPtr& _input){
-  sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared<const sensor_msgs::PointCloud2>(_input->msg);
-  rubis::instance_ = _input->instance;
-  ndt_matching(input);
+static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input) {
+    rubis::instance_ = 0;
+    ndt_matching(input);
 }
 
-static void ins_stat_callback(const rubis_msgs::InsStat::ConstPtr& input){
-  _is_ins_stat_received = true;
-  _current_ins_stat_vel_x = input->vel_x;
-  _current_ins_stat_vel_y = input->vel_y;
-  _current_ins_stat_acc_x = input->acc_x;
-  _current_ins_stat_acc_y = input->acc_y;
-  _current_ins_stat_yaw = input->yaw;
-  _current_ins_stat_linear_velocity = input->linear_velocity;
-  _current_ins_stat_linear_acceleration = input->linear_acceleration;
-  _current_ins_stat_angular_velocity = input->angular_velocity;
-  return;
-}
+void *thread_func(void *args) {
+    ros::NodeHandle nh_map;
+    ros::CallbackQueue map_callback_queue;
+    nh_map.setCallbackQueue(&map_callback_queue);
 
-void* thread_func(void* args)
-{
-  ros::NodeHandle nh_map;
-  ros::CallbackQueue map_callback_queue;
-  nh_map.setCallbackQueue(&map_callback_queue);
-
-  ros::Subscriber map_sub = nh_map.subscribe("points_map", 10, map_callback); 
-  ros::Rate ros_rate(10);
-  while (nh_map.ok())
-  {
-    map_callback_queue.callAvailable(ros::WallDuration());
-    ros_rate.sleep();
-  }
+    ros::Subscriber map_sub = nh_map.subscribe("points_map", 10, map_callback);
+    ros::Rate ros_rate(10);
+    while (nh_map.ok()) {
+        map_callback_queue.callAvailable(ros::WallDuration());
+        ros_rate.sleep();
+    }
 
-  return nullptr;
+    return nullptr;
 }
 
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "ndt_matching");
-  pthread_mutex_init(&mutex, NULL);
-
-  ros::NodeHandle nh;
-  ros::NodeHandle private_nh("~");
-  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh);
-  health_checker_ptr_->ENABLE();
-  health_checker_ptr_->NODE_ACTIVATE();
-
-  // Set log file name.
-  private_nh.getParam("output_log_data", _output_log_data);
-  if(_output_log_data)
-  {
-    char buffer[80];
-    std::time_t now = std::time(NULL);
-    std::tm* pnow = std::localtime(&now);
-    std::strftime(buffer, 80, "%Y%m%d_%H%M%S", pnow);
-    std::string directory_name = "/tmp/Autoware/log/ndt_matching";
-    filename = directory_name + "/" + std::string(buffer) + ".csv";
-    boost::filesystem::create_directories(boost::filesystem::path(directory_name));
-    ofs.open(filename.c_str(), std::ios::app);
-  }
-
-  // Geting parameters
-  int method_type_tmp = 0;
-  private_nh.getParam("method_type", method_type_tmp);
-  _method_type = static_cast<MethodType>(method_type_tmp);
-  private_nh.getParam("use_gnss", _use_gnss);
-  private_nh.getParam("queue_size", _queue_size);
-  private_nh.getParam("offset", _offset);
-  private_nh.getParam("get_height", _get_height);
-  private_nh.getParam("use_local_transform", _use_local_transform);
-  private_nh.getParam("use_kalman_filter", _use_kalman_filter);
-  private_nh.getParam("use_odom", _use_odom);
-  private_nh.getParam("use_odom", _use_odom);
-  private_nh.getParam("imu_upside_down", _imu_upside_down);
-  private_nh.getParam("imu_topic", _imu_topic);
-  private_nh.param<double>("gnss_reinit_fitness", _gnss_reinit_fitness, 500.0);
-  private_nh.param<float>("init_match_threshold", _init_match_threshold, 8.0);
-  
-  
-
-  nh.param<std::string>("/ndt_matching/localizer", _localizer, "velodyne");  
-
-  nh.param<float>("/ndt_matching/failure_score_diff_threshold", _failure_score_diff_threshold, 10.0);  
-  nh.param<float>("/ndt_matching/recovery_score_diff_threshold", _recovery_score_diff_threshold, 1.0);  
-  nh.param<float>("/ndt_matching/failure_pose_diff_threshold", _failure_pose_diff_threshold, 4.0);
-  nh.param<float>("/ndt_matching/recovery_pose_diff_threshold", _recovery_pose_diff_threshold, 1.0);
-
-  if(_use_kalman_filter){
-    std::vector<float> H_k_vec, Q_k_vec, R_k_vec, P_k_vec;
-
-    if(!nh.getParam("/ndt_matching/H_k", H_k_vec)){
-      ROS_ERROR("Failed to get parameter H_k");
-      exit(1);
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "ndt_matching");
+    pthread_mutex_init(&mutex, NULL);
+
+    ros::NodeHandle nh;
+    ros::NodeHandle private_nh("~");
+
+    // Set log file name.
+    private_nh.getParam("output_log_data", _output_log_data);
+    if (_output_log_data) {
+        char buffer[80];
+        std::time_t now = std::time(NULL);
+        std::tm *pnow = std::localtime(&now);
+        std::strftime(buffer, 80, "%Y%m%d_%H%M%S", pnow);
+        std::string directory_name = "/tmp/Autoware/log/ndt_matching";
+        filename = directory_name + "/" + std::string(buffer) + ".csv";
+        boost::filesystem::create_directories(boost::filesystem::path(directory_name));
+        ofs.open(filename.c_str(), std::ios::app);
     }
-    if(!nh.getParam("/ndt_matching/Q_k", Q_k_vec)){
-      ROS_ERROR("Failed to get parameter Q_k");
-      exit(1);
-    }
-    if(!nh.getParam("/ndt_matching/R_k", R_k_vec)){
-      ROS_ERROR("Failed to get parameter R_k");
-      exit(1);
-    }
-    if(!nh.getParam("/ndt_matching/P_k", P_k_vec)){
-      ROS_ERROR("Failed to get parameter P_k");
-      exit(1);
-    }
-    
-    Eigen::Matrix2f H_k = Eigen::Matrix2f(H_k_vec.data());
-    Eigen::Matrix2f Q_k = Eigen::Matrix2f(Q_k_vec.data());
-    Eigen::Matrix2f R_k = Eigen::Matrix2f(R_k_vec.data());
-    Eigen::Matrix2f P_k = Eigen::Matrix2f(P_k_vec.data());
-    linear_kalman_filter = LKF(H_k, Q_k, R_k, P_k);
-  }
 
-  try
-  {
-    tf::TransformListener base_localizer_listener;
-    tf::StampedTransform  m_base_to_localizer;
-    base_localizer_listener.waitForTransform("/base_link", _localizer, ros::Time(0), ros::Duration(1.0));
-    base_localizer_listener.lookupTransform("/base_link", _localizer, ros::Time(0), m_base_to_localizer);
-
-    _tf_x = m_base_to_localizer.getOrigin().x();
-    _tf_y = m_base_to_localizer.getOrigin().y();
-    _tf_z = m_base_to_localizer.getOrigin().z();
-
-    tf::Quaternion b_l_q(
-      m_base_to_localizer.getRotation().x(),
-      m_base_to_localizer.getRotation().y(),
-      m_base_to_localizer.getRotation().z(),
-      m_base_to_localizer.getRotation().w()
-    );
-
-    tf::Matrix3x3 b_l_m(b_l_q);
-    b_l_m.getRPY(_tf_roll, _tf_pitch, _tf_yaw);
-  }
-  catch (tf::TransformException& ex)
-  {
-    ROS_ERROR("%s", ex.what());
-  }
+    // Geting parameters
+    int method_type_tmp = 0;
+    private_nh.getParam("method_type", method_type_tmp);
+    _method_type = static_cast<MethodType>(method_type_tmp);
+    private_nh.getParam("use_gnss", _use_gnss);
+    private_nh.getParam("queue_size", _queue_size);
+    private_nh.getParam("offset", _offset);
+    private_nh.getParam("get_height", _get_height);
+    private_nh.getParam("use_local_transform", _use_local_transform);
+    private_nh.getParam("use_svl_gnss", _use_svl_gnss);
+    private_nh.getParam("use_odom", _use_odom);
+    private_nh.getParam("use_odom", _use_odom);
+    private_nh.getParam("imu_upside_down", _imu_upside_down);
+    private_nh.getParam("imu_topic", _imu_topic);
+    private_nh.param<double>("gnss_reinit_fitness", _gnss_reinit_fitness, 500.0);
+    private_nh.param<float>("init_match_threshold", _init_match_threshold, 8.0);
+
+    nh.param<std::string>("/ndt_matching/localizer", _localizer, "velodyne");
+
+    nh.param<float>("/ndt_matching/failure_score_diff_threshold", _failure_score_diff_threshold, 10.0);
+    nh.param<float>("/ndt_matching/recovery_score_diff_threshold", _recovery_score_diff_threshold, 1.0);
+    nh.param<float>("/ndt_matching/failure_pose_diff_threshold", _failure_pose_diff_threshold, 4.0);
+    nh.param<float>("/ndt_matching/recovery_pose_diff_threshold", _recovery_pose_diff_threshold, 1.0);
+
+    try {
+        tf::TransformListener base_localizer_listener;
+        tf::StampedTransform m_base_to_localizer;
+        base_localizer_listener.waitForTransform("/base_link", _localizer, ros::Time(0), ros::Duration(1.0));
+        base_localizer_listener.lookupTransform("/base_link", _localizer, ros::Time(0), m_base_to_localizer);
+
+        _tf_x = m_base_to_localizer.getOrigin().x();
+        _tf_y = m_base_to_localizer.getOrigin().y();
+        _tf_z = m_base_to_localizer.getOrigin().z();
+
+        tf::Quaternion b_l_q(m_base_to_localizer.getRotation().x(), m_base_to_localizer.getRotation().y(), m_base_to_localizer.getRotation().z(),
+                             m_base_to_localizer.getRotation().w());
+
+        tf::Matrix3x3 b_l_m(b_l_q);
+        b_l_m.getRPY(_tf_roll, _tf_pitch, _tf_yaw);
+    } catch (tf::TransformException &ex) {
+        ROS_ERROR("%s", ex.what());
+    }
 
-  std::cout << "-----------------------------------------------------------------" << std::endl;
-  std::cout << "Log file: " << filename << std::endl;
-  std::cout << "method_type: " << static_cast<int>(_method_type) << std::endl;
-  std::cout << "use_gnss: " << _use_gnss << std::endl;
-  std::cout << "queue_size: " << _queue_size << std::endl;
-  std::cout << "offset: " << _offset << std::endl;
-  std::cout << "get_height: " << _get_height << std::endl;
-  std::cout << "use_local_transform: " << _use_local_transform << std::endl;
-  std::cout << "use_odom: " << _use_odom << std::endl;
-  std::cout << "use_imu: " << _use_imu << std::endl;
-  std::cout << "imu_upside_down: " << _imu_upside_down << std::endl;
-  std::cout << "imu_topic: " << _imu_topic << std::endl;
-  std::cout << "localizer: " << _localizer << std::endl;
-  std::cout << "gnss_reinit_fitness: " << _gnss_reinit_fitness << std::endl;
-  std::cout << "(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (" << _tf_x << ", " << _tf_y << ", " << _tf_z << ", "
-            << _tf_roll << ", " << _tf_pitch << ", " << _tf_yaw << ")" << std::endl;
-  std::cout << "-----------------------------------------------------------------" << std::endl;
+    std::cout << "-----------------------------------------------------------------" << std::endl;
+    std::cout << "Log file: " << filename << std::endl;
+    std::cout << "method_type: " << static_cast<int>(_method_type) << std::endl;
+    std::cout << "use_gnss: " << _use_gnss << std::endl;
+    std::cout << "queue_size: " << _queue_size << std::endl;
+    std::cout << "offset: " << _offset << std::endl;
+    std::cout << "get_height: " << _get_height << std::endl;
+    std::cout << "use_local_transform: " << _use_local_transform << std::endl;
+    std::cout << "use_odom: " << _use_odom << std::endl;
+    std::cout << "use_imu: " << _use_imu << std::endl;
+    std::cout << "imu_upside_down: " << _imu_upside_down << std::endl;
+    std::cout << "imu_topic: " << _imu_topic << std::endl;
+    std::cout << "localizer: " << _localizer << std::endl;
+    std::cout << "gnss_reinit_fitness: " << _gnss_reinit_fitness << std::endl;
+    std::cout << "(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (" << _tf_x << ", " << _tf_y << ", " << _tf_z << ", " << _tf_roll << ", " << _tf_pitch
+              << ", " << _tf_yaw << ")" << std::endl;
+    std::cout << "-----------------------------------------------------------------" << std::endl;
 
 #ifndef CUDA_FOUND
-  if (_method_type == MethodType::PCL_ANH_GPU)
-  {
-    std::cerr << "**************************************************************" << std::endl;
-    std::cerr << "[ERROR]PCL_ANH_GPU is not built. Please use other method type." << std::endl;
-    std::cerr << "**************************************************************" << std::endl;
-    exit(1);
-  }
+    if (_method_type == MethodType::PCL_ANH_GPU) {
+        std::cerr << "**************************************************************" << std::endl;
+        std::cerr << "[ERROR]PCL_ANH_GPU is not built. Please use other method type." << std::endl;
+        std::cerr << "**************************************************************" << std::endl;
+        exit(1);
+    }
 #endif
 #ifndef USE_PCL_OPENMP
-  if (_method_type == MethodType::PCL_OPENMP)
-  {
-    std::cerr << "**************************************************************" << std::endl;
-    std::cerr << "[ERROR]PCL_OPENMP is not built. Please use other method type." << std::endl;
-    std::cerr << "**************************************************************" << std::endl;
-    exit(1);
-  }
+    if (_method_type == MethodType::PCL_OPENMP) {
+        std::cerr << "**************************************************************" << std::endl;
+        std::cerr << "[ERROR]PCL_OPENMP is not built. Please use other method type." << std::endl;
+        std::cerr << "**************************************************************" << std::endl;
+        exit(1);
+    }
 #endif
+    // Scheduling & Profiling Setup
+    std::string node_name = ros::this_node::getName();
+    std::string task_response_time_filename;
+    private_nh.param<std::string>(node_name + "/task_response_time_filename", task_response_time_filename,
+                                  "~/Documents/profiling/response_time/ndt_matching.csv");
+
+    int rate;
+    private_nh.param<int>(node_name + "/rate", rate, 10);
+
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time, deadline, period;
+
+    private_nh.param(node_name + "/task_scheduling_configs/policy", policy, std::string("NONE"));
+    private_nh.param(node_name + "/task_scheduling_configs/priority", priority, 99);
+    private_nh.param(node_name + "/task_scheduling_configs/exec_time", exec_time, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/deadline", deadline, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/period", period, 0);
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);
+    rubis::init_task_scheduling(policy, attr);
+
+    rubis::init_task_profiling(task_response_time_filename);
+
+    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                // tl: translation
+    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+    // Updated in initialpose_callback or gnss_callback
+    initial_pose.x = 0.0;
+    initial_pose.y = 0.0;
+    initial_pose.z = 0.0;
+    initial_pose.roll = 0.0;
+    initial_pose.pitch = 0.0;
+    initial_pose.yaw = 0.0;
+
+    // Publishers
+    predict_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose", 10);
+    predict_pose_imu_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu", 10);
+    predict_pose_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_odom", 10);
+    predict_pose_imu_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu_odom", 10);
+
+    rubis_pose_twist_pub = nh.advertise<rubis_msgs::PoseTwistStamped>("/rubis_current_pose_twist", 10);
+
+    localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose", 10);
+    estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("/localizer_velocity", 10);
+    // estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 10);
+    time_ndt_matching_pub = nh.advertise<std_msgs::Float32>("/time_ndt_matching", 10);
+    ndt_stat_pub = nh.advertise<autoware_msgs::NDTStat>("/ndt_stat", 10);
+
+    // Subscribers
+    ros::Subscriber param_sub = nh.subscribe("config/ndt", 10, param_callback);
+    // ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback);
+    // ros::Subscriber gnss_pose_sub = nh.subscribe("gnss_pose", 10, gnss_pose_callback);
+    // ros::Subscriber gnss_vel_sub = nh.subscribe("gnss_vel", 10, gnss_vel_callback);
+
+    ros::Subscriber map_sub = nh.subscribe("points_map", 1, map_callback);
+    ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback);
+
+    // ros::Subscriber points_sub;
+
+    message_filters::Subscriber<rubis_msgs::PointCloud2> filtered_points_sub;
+    message_filters::Subscriber<rubis_msgs::PoseTwistStamped> svl_pose_twist_sub;
+    filtered_points_sub.subscribe(nh, "/rubis_filtered_points", 1);
+    svl_pose_twist_sub.subscribe(nh, "/svl_pose_twist", 1);
+
+    typedef message_filters::sync_policies::ExactTime<rubis_msgs::PointCloud2, rubis_msgs::PoseTwistStamped> SyncPolicy;
+    sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), filtered_points_sub, svl_pose_twist_sub));
+    sync_->registerCallback(boost::bind(&svl_callback, _1, _2));
+
+    pthread_t thread;
+    pthread_create(&thread, NULL, thread_func, NULL);
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline;
-
-  int gpu_scheduling_flag;
-  int gpu_profiling_flag;
-  std::string gpu_execution_time_filename;
-  std::string gpu_response_time_filename;
-  std::string gpu_deadline_filename;
-
-  std::string node_name = ros::this_node::getName();
-  private_nh.param<int>(node_name+"/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>(node_name+"/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ndt_matching.csv");
-  private_nh.param<int>(node_name+"/rate", rate, 10);
-  private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10);
-  private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10);
-  private_nh.param(node_name+"/gpu_scheduling_flag", gpu_scheduling_flag, 0);
-  private_nh.param(node_name+"/gpu_profiling_flag", gpu_profiling_flag, 0);
-  private_nh.param<std::string>(node_name+"/gpu_execution_time_filename", gpu_execution_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv");
-  private_nh.param<std::string>(node_name+"/gpu_response_time_filename", gpu_response_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv");
-  private_nh.param<std::string>(node_name+"/gpu_deadline_filename", gpu_deadline_filename, "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv");
-  private_nh.param<int>(node_name+"/instance_mode", rubis::instance_mode_, 0);
-  
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-  if(gpu_profiling_flag) rubis::sched::init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename);
-  
-  if( (_method_type == MethodType::PCL_ANH_GPU) && (gpu_scheduling_flag == 1) ){
-    rubis::sched::init_gpu_scheduling("/tmp/ndt_matching", gpu_deadline_filename, 0);
-  }    
-  else if(_method_type != MethodType::PCL_ANH_GPU && gpu_scheduling_flag == 1){
-    ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!");
-    exit(1);
-  }
-
-  Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
-  Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
-  Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
-  Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
-  tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
-
-  // Updated in initialpose_callback or gnss_callback
-  initial_pose.x = 0.0;
-  initial_pose.y = 0.0;
-  initial_pose.z = 0.0;
-  initial_pose.roll = 0.0;
-  initial_pose.pitch = 0.0;
-  initial_pose.yaw = 0.0;
-
-  // Publishers
-  predict_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose", 10); 
-  predict_pose_imu_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu", 10);
-  predict_pose_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_odom", 10);
-  predict_pose_imu_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu_odom", 10);
-  is_kalman_filter_on_pub = nh.advertise<std_msgs::Bool>("/is_kalman_filter_on", 10);
-  kalman_filtered_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/kalman_filtered_pose", 10);
-
-  ndt_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 10);
-  if(rubis::instance_mode_) rubis_ndt_pose_pub = nh.advertise<rubis_msgs::PoseStamped>("/rubis_ndt_pose",10);
-  
-  // current_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 10);
-  localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose", 10);
-  estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 10);
-  estimated_vel_mps_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_mps", 10);
-  estimated_vel_kmph_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_kmph", 10);
-  estimated_vel_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/estimated_vel", 10);
-  time_ndt_matching_pub = nh.advertise<std_msgs::Float32>("/time_ndt_matching", 10);
-  ndt_stat_pub = nh.advertise<autoware_msgs::NDTStat>("/ndt_stat", 10);
-  ndt_reliability_pub = nh.advertise<std_msgs::Float32>("/ndt_reliability", 10);
-
-  // Subscribers
-  ros::Subscriber param_sub = nh.subscribe("config/ndt", 10, param_callback);
-  ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback); 
-  ros::Subscriber map_sub = nh.subscribe("points_map", 1, map_callback);
-  ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback); 
-
-  ros::Subscriber points_sub;
-  if(rubis::instance_mode_) points_sub = nh.subscribe("rubis_filtered_points", _queue_size, rubis_points_callback); // _queue_size = 1000
-  else points_sub = nh.subscribe("filtered_points", _queue_size, points_callback); // _queue_size = 1000
-  
-  // ros::Subscriber odom_sub = nh.subscribe("/vehicle/odom", _queue_size * 10, odom_callback);
-  // ros::Subscriber imu_sub = nh.subscribe(_imu_topic.c_str(), _queue_size * 10, imu_callback);
-
-  ros::Subscriber ins_stat_sub = nh.subscribe("/ins_stat", 1, ins_stat_callback);
-  
-  pthread_t thread;
-  pthread_create(&thread, NULL, thread_func, NULL);
-
-  // SPIN  
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{ 
     ros::Rate r(rate);
-
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){
-      if(map_loaded == 1) break;
-      ros::spinOnce();
-      r.sleep();      
+    while (ros::ok()) {
+        if (map_loaded == 1)
+            break;
+        ros::spinOnce();
+        r.sleep();
     }
-    
+
     map_sub.shutdown();
 
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();        
-      if(rubis::sched::task_state_ == TASK_STATE_READY){        
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::start_job();
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
-
-      ros::spinOnce();
-
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
-      
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){      
-        if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::finish_job();        
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();        
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
-  }
+    ros::spin();
 
-  return 0;
+    return 0;
 }
diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp.origin b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp.origin
new file mode 100644
index 00000000..5a8c1ed3
--- /dev/null
+++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp.origin
@@ -0,0 +1,1977 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ Localization program using Normal Distributions Transform
+
+ Yuki KITSUKAWA
+ */
+
+#include <pthread.h>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include <LKF.hpp>
+
+#include <boost/filesystem.hpp>
+
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Bool.h>
+#include <velodyne_pointcloud/point_types.h>
+#include <velodyne_pointcloud/rawdata.h>
+
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Quaternion.h>
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <ndt_cpu/NormalDistributionsTransform.h>
+#include <pcl/registration/ndt.h>
+#ifdef CUDA_FOUND
+#include <ndt_gpu/NormalDistributionsTransform.h>
+#endif
+#ifdef USE_PCL_OPENMP
+#include <pcl_omp_registration/ndt.h>
+#endif
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
+
+#include <autoware_config_msgs/ConfigNDT.h>
+
+#include <autoware_msgs/NDTStat.h>
+
+#include <rubis_lib/sched.hpp>
+#include <rubis_msgs/PointCloud2.h>
+#include <rubis_msgs/PoseStamped.h>
+#include <rubis_msgs/InsStat.h>
+#include <rubis_msgs/PoseTwistStamped.h>
+
+#define SPIN_PROFILING
+
+#define PREDICT_POSE_THRESHOLD 0.5
+
+#define USING_GPS_THRESHOLD 50
+
+#define Wa 0.4
+#define Wb 0.3
+#define Wc 0.3
+#define M_PI 3.14159265358979323846
+
+#define DEBUG
+
+struct pose
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+};
+
+enum class MethodType
+{
+  PCL_GENERIC = 0,
+  PCL_ANH = 1,
+  PCL_ANH_GPU = 2,
+  PCL_OPENMP = 3,
+};
+static MethodType _method_type = MethodType::PCL_GENERIC;
+
+static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose, previous_gnss_pose,
+    ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose, current_kalman_pose, previous_kalman_pose;
+
+static double offset_x, offset_y, offset_z, offset_yaw;  // current_pos - previous_pose
+static double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw;
+static double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw;
+static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch,
+    offset_imu_odom_yaw;
+
+// For GPS backup method
+static pose current_gnss_pose;
+static double previous_score = 0.0;
+
+// Can't load if typed "pcl::PointCloud<pcl::PointXYZRGB> map, add;"
+static pcl::PointCloud<pcl::PointXYZ> map, add;
+
+// If the map is loaded, map_loaded will be 1.
+static int map_loaded = 0;
+static int _use_gnss = 1;
+static int init_pos_set = 0;
+
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
+#ifdef CUDA_FOUND
+static std::shared_ptr<gpu::GNormalDistributionsTransform> anh_gpu_ndt_ptr =
+    std::make_shared<gpu::GNormalDistributionsTransform>();
+#endif
+#ifdef USE_PCL_OPENMP
+static pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> omp_ndt;
+#endif
+
+// Default values
+static int max_iter = 30;        // Maximum iterations
+static float ndt_res = 1.0;      // Resolution
+static double step_size = 0.1;   // Step size
+static double trans_eps = 0.01;  // Transformation epsilon
+
+static ros::Publisher predict_pose_pub;
+static geometry_msgs::PoseStamped predict_pose_msg;
+
+static ros::Publisher predict_pose_imu_pub;
+static geometry_msgs::PoseStamped predict_pose_imu_msg;
+
+static ros::Publisher predict_pose_odom_pub;
+static geometry_msgs::PoseStamped predict_pose_odom_msg;
+
+static ros::Publisher predict_pose_imu_odom_pub;
+static geometry_msgs::PoseStamped predict_pose_imu_odom_msg;
+
+static ros::Publisher ndt_pose_pub;
+static geometry_msgs::PoseStamped ndt_pose_msg;
+
+// current_pose is published by vel_pose_mux
+/*
+static ros::Publisher current_pose_pub;
+static geometry_msgs::PoseStamped current_pose_msg;
+ */
+
+static ros::Publisher rubis_pose_twist_pub;
+static ros::Publisher localizer_pose_pub;
+static geometry_msgs::PoseStamped localizer_pose_msg;
+static rubis_msgs::PoseTwistStamped rubis_pose_twist_msg;
+
+static ros::Publisher estimate_twist_pub;
+static geometry_msgs::TwistStamped estimate_twist_msg;
+
+static ros::Duration scan_duration;
+
+static double exe_time = 0.0;
+static bool has_converged;
+static int iteration = 0;
+static double fitness_score = 0.0;
+static double trans_probability = 0.0;
+
+// reference for comparing fitness_score, default value set to 500.0
+static double _gnss_reinit_fitness = 500.0;
+
+static double diff = 0.0;
+static double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw;
+
+static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0;  // [m/s]
+static double current_velocity_x = 0.0, previous_velocity_x = 0.0;
+static double current_velocity_y = 0.0, previous_velocity_y = 0.0;
+static double current_velocity_z = 0.0, previous_velocity_z = 0.0;
+// static double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0;
+static double current_velocity_smooth = 0.0;
+
+static double current_velocity_imu_x = 0.0;
+static double current_velocity_imu_y = 0.0;
+static double current_velocity_imu_z = 0.0;
+
+static double current_accel = 0.0, previous_accel = 0.0;  // [m/s^2]
+static double current_accel_x = 0.0;
+static double current_accel_y = 0.0;
+static double current_accel_z = 0.0;
+// static double current_accel_yaw = 0.0;
+
+static double angular_velocity = 0.0;
+
+static int use_predict_pose = 0;
+
+// INS Stat information
+static bool _is_ins_stat_received = false;
+static double _current_ins_stat_vel_x = 0.0, _current_ins_stat_vel_y = 0.0;
+static double _current_ins_stat_acc_x = 0.0, _current_ins_stat_acc_y = 0.0;
+static double _current_ins_stat_yaw = 0.0;
+static double _current_ins_stat_linear_velocity = 0.0, _current_ins_stat_linear_acceleration = 0.0, _current_ins_stat_angular_velocity = 0.0;
+
+static double _previous_ins_stat_vel_x = 0.0, _previous_ins_stat_vel_y = 0.0;
+static double _previous_ins_stat_acc_x = 0.0, _previous_ins_stat_acc_y = 0.0;
+static double _previous_ins_stat_yaw = 0.0;
+static double _previous_ins_stat_linear_velocity = 0.0, _previous_ins_stat_linear_acceleration = 0.0, _previous_ins_stat_angular_velocity = 0.0;
+
+static ros::Publisher is_kalman_filter_on_pub, kalman_filtered_pose_pub;
+static bool _is_kalman_filter_on = false;
+static LKF linear_kalman_filter;
+static double _previous_success_score;
+static bool _is_matching_failed = false;
+
+
+static ros::Publisher estimated_vel_mps_pub, estimated_vel_kmph_pub, estimated_vel_pub;
+static std_msgs::Float32 estimated_vel_mps, estimated_vel_kmph, previous_estimated_vel_kmph;
+
+static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;
+
+static ros::Publisher time_ndt_matching_pub;
+static std_msgs::Float32 time_ndt_matching;
+
+static int _queue_size = 1000;
+
+static ros::Publisher ndt_stat_pub;
+static autoware_msgs::NDTStat ndt_stat_msg;
+
+static double predict_pose_error = 0.0;
+
+static double _tf_x = 1.2;
+static double _tf_y = 0.0;
+static double _tf_z = 2.0;
+static double _tf_roll = 0.0;
+static double _tf_pitch = 0.0;
+static double _tf_yaw = 0.0;
+static Eigen::Matrix4f tf_btol;
+
+static std::string _localizer = "velodyne";
+static std::string _offset = "linear";  // linear, zero, quadratic
+
+static ros::Publisher ndt_reliability_pub;
+static std_msgs::Float32 ndt_reliability;
+
+static bool _get_height = false;
+static bool _use_local_transform = false;
+static bool _use_imu = false;
+static bool _use_odom = false;
+static bool _use_kalman_filter = false;
+static bool _imu_upside_down = false;
+static bool _output_log_data = false;
+
+static std::string _imu_topic = "/imu_raw";
+
+static std::ofstream ofs;
+static std::string filename;
+
+static sensor_msgs::Imu imu;
+static nav_msgs::Odometry odom;
+
+// static tf::TransformListener local_transform_listener;
+static tf::StampedTransform local_transform;
+
+static unsigned int points_map_num = 0;
+
+pthread_mutex_t mutex;
+
+static bool _is_init_match_finished = false;
+static float _init_match_threshold = 8.0;
+// Failure detection and recovery parpameters
+static float _failure_score_diff_threshold = 10.0;
+static float _recovery_score_diff_threshold = 1.0;
+static float _failure_pose_diff_threshold = 4.0;
+static float _recovery_pose_diff_threshold = 1.0;
+
+void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q)
+{
+    double cy = cos(yaw * 0.5);
+    double sy = sin(yaw * 0.5);
+    double cp = cos(pitch * 0.5);
+    double sp = sin(pitch * 0.5);
+    double cr = cos(roll * 0.5);
+    double sr = sin(roll * 0.5);
+
+    q.w = cr * cp * cy + sr * sp * sy;
+    q.x = sr * cp * cy - cr * sp * sy;
+    q.y = cr * sp * cy + sr * cp * sy;
+    q.z = cr * cp * sy - sr * sp * cy;
+}
+
+static pose convertPoseIntoRelativeCoordinate(const pose &target_pose, const pose &reference_pose)
+{
+    tf::Quaternion target_q;
+    target_q.setRPY(target_pose.roll, target_pose.pitch, target_pose.yaw);
+    tf::Vector3 target_v(target_pose.x, target_pose.y, target_pose.z);
+    tf::Transform target_tf(target_q, target_v);
+
+    tf::Quaternion reference_q;
+    reference_q.setRPY(reference_pose.roll, reference_pose.pitch, reference_pose.yaw);
+    tf::Vector3 reference_v(reference_pose.x, reference_pose.y, reference_pose.z);
+    tf::Transform reference_tf(reference_q, reference_v);
+
+    tf::Transform trans_target_tf = reference_tf.inverse() * target_tf;
+
+    pose trans_target_pose;
+    trans_target_pose.x = trans_target_tf.getOrigin().getX();
+    trans_target_pose.y = trans_target_tf.getOrigin().getY();
+    trans_target_pose.z = trans_target_tf.getOrigin().getZ();
+    tf::Matrix3x3 tmp_m(trans_target_tf.getRotation());
+    tmp_m.getRPY(trans_target_pose.roll, trans_target_pose.pitch, trans_target_pose.yaw);
+
+    return trans_target_pose;
+}
+
+static void param_callback(const autoware_config_msgs::ConfigNDT::ConstPtr& input)
+{
+  if (_use_gnss != input->init_pos_gnss)
+  {
+    init_pos_set = 0;
+  }
+  else if (_use_gnss == 0 &&
+           (initial_pose.x != input->x || initial_pose.y != input->y || initial_pose.z != input->z ||
+            initial_pose.roll != input->roll || initial_pose.pitch != input->pitch || initial_pose.yaw != input->yaw))
+  {
+    init_pos_set = 0;
+  }
+
+  _use_gnss = input->init_pos_gnss;
+
+  // Setting parameters
+  if (input->resolution != ndt_res)
+  {
+    ndt_res = input->resolution;
+
+    if (_method_type == MethodType::PCL_GENERIC)
+      ndt.setResolution(ndt_res);
+    else if (_method_type == MethodType::PCL_ANH)
+      anh_ndt.setResolution(ndt_res);
+#ifdef CUDA_FOUND
+    else if (_method_type == MethodType::PCL_ANH_GPU)      
+      anh_gpu_ndt_ptr->setResolution(ndt_res);
+#endif
+#ifdef USE_PCL_OPENMP
+    else if (_method_type == MethodType::PCL_OPENMP)
+      omp_ndt.setResolution(ndt_res);
+#endif
+  }
+
+  if (input->step_size != step_size)
+  {
+    step_size = input->step_size;
+
+    if (_method_type == MethodType::PCL_GENERIC)
+      ndt.setStepSize(step_size);
+    else if (_method_type == MethodType::PCL_ANH)
+      anh_ndt.setStepSize(step_size);
+#ifdef CUDA_FOUND
+    else if (_method_type == MethodType::PCL_ANH_GPU)
+      anh_gpu_ndt_ptr->setStepSize(step_size);
+#endif
+#ifdef USE_PCL_OPENMP
+    else if (_method_type == MethodType::PCL_OPENMP)
+      omp_ndt.setStepSize(ndt_res);
+#endif
+  }
+
+  if (input->trans_epsilon != trans_eps)
+  {
+    trans_eps = input->trans_epsilon;
+
+    if (_method_type == MethodType::PCL_GENERIC)
+      ndt.setTransformationEpsilon(trans_eps);
+    else if (_method_type == MethodType::PCL_ANH)
+      anh_ndt.setTransformationEpsilon(trans_eps);
+#ifdef CUDA_FOUND
+    else if (_method_type == MethodType::PCL_ANH_GPU)
+      anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+#endif
+#ifdef USE_PCL_OPENMP
+    else if (_method_type == MethodType::PCL_OPENMP)
+      omp_ndt.setTransformationEpsilon(ndt_res);
+#endif
+  }
+
+  if (input->max_iterations != max_iter)
+  {
+    max_iter = input->max_iterations;
+
+    if (_method_type == MethodType::PCL_GENERIC)
+      ndt.setMaximumIterations(max_iter);
+    else if (_method_type == MethodType::PCL_ANH)
+      anh_ndt.setMaximumIterations(max_iter);
+#ifdef CUDA_FOUND
+    else if (_method_type == MethodType::PCL_ANH_GPU)
+      anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
+#endif
+#ifdef USE_PCL_OPENMP
+    else if (_method_type == MethodType::PCL_OPENMP)
+      omp_ndt.setMaximumIterations(ndt_res);
+#endif
+  }
+
+  if (_use_gnss == 0 && init_pos_set == 0)
+  {
+    initial_pose.x = input->x;
+    initial_pose.y = input->y;
+    initial_pose.z = input->z;
+    initial_pose.roll = input->roll;
+    initial_pose.pitch = input->pitch;
+    initial_pose.yaw = input->yaw;
+
+    if (_use_local_transform == true)
+    {
+      tf::Vector3 v(input->x, input->y, input->z);
+      tf::Quaternion q;
+      q.setRPY(input->roll, input->pitch, input->yaw);
+      tf::Transform transform(q, v);
+      initial_pose.x = (local_transform.inverse() * transform).getOrigin().getX();
+      initial_pose.y = (local_transform.inverse() * transform).getOrigin().getY();
+      initial_pose.z = (local_transform.inverse() * transform).getOrigin().getZ();
+
+      tf::Matrix3x3 m(q);
+      m.getRPY(initial_pose.roll, initial_pose.pitch, initial_pose.yaw);
+
+      std::cout << "initial_pose.x: " << initial_pose.x << std::endl;
+      std::cout << "initial_pose.y: " << initial_pose.y << std::endl;
+      std::cout << "initial_pose.z: " << initial_pose.z << std::endl;
+      std::cout << "initial_pose.roll: " << initial_pose.roll << std::endl;
+      std::cout << "initial_pose.pitch: " << initial_pose.pitch << std::endl;
+      std::cout << "initial_pose.yaw: " << initial_pose.yaw << std::endl;
+    }
+
+    // Setting position and posture for the first time.
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+    current_pose_imu.x = 0;
+    current_pose_imu.y = 0;
+    current_pose_imu.z = 0;
+    current_pose_imu.roll = 0;
+    current_pose_imu.pitch = 0;
+    current_pose_imu.yaw = 0;
+
+    current_velocity_imu_x = current_velocity_x;
+    current_velocity_imu_y = current_velocity_y;
+    current_velocity_imu_z = current_velocity_z;
+    init_pos_set = 1;
+  }
+}
+
+static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
+{
+  // if (map_loaded == 0)
+  if (points_map_num != input->width)
+  {
+    std::cout << "Update points_map." << std::endl;
+
+    points_map_num = input->width;
+
+    // Convert the data type(from sensor_msgs to pcl).
+    pcl::fromROSMsg(*input, map);
+
+    if (_use_local_transform == true)
+    {
+      tf::TransformListener local_transform_listener;
+      try
+      {
+        ros::Time now = ros::Time(0);
+        local_transform_listener.waitForTransform("/map", "/world", now, ros::Duration(10.0));
+        local_transform_listener.lookupTransform("/map", "world", now, local_transform);
+      }
+      catch (tf::TransformException& ex)
+      {
+        ROS_ERROR("%s", ex.what());
+      }
+
+      pcl_ros::transformPointCloud(map, map, local_transform.inverse());
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));
+
+    // Setting point cloud to be aligned to.
+    if (_method_type == MethodType::PCL_GENERIC)
+    { 
+      pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+      new_ndt.setResolution(ndt_res);
+      
+      new_ndt.setInputTarget(map_ptr);
+      new_ndt.setMaximumIterations(max_iter);
+      new_ndt.setStepSize(step_size);
+      new_ndt.setTransformationEpsilon(trans_eps);
+
+      new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+      pthread_mutex_lock(&mutex);
+      ndt = new_ndt;
+      pthread_mutex_unlock(&mutex);
+    }
+    else if (_method_type == MethodType::PCL_ANH)
+    {
+      cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_anh_ndt;
+      new_anh_ndt.setResolution(ndt_res);
+      new_anh_ndt.setInputTarget(map_ptr);
+      new_anh_ndt.setMaximumIterations(max_iter);
+      new_anh_ndt.setStepSize(step_size);
+      new_anh_ndt.setTransformationEpsilon(trans_eps);
+
+      pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+      pcl::PointXYZ dummy_point;
+      dummy_scan_ptr->push_back(dummy_point);
+      new_anh_ndt.setInputSource(dummy_scan_ptr);
+
+      new_anh_ndt.align(Eigen::Matrix4f::Identity());
+
+      pthread_mutex_lock(&mutex);
+      anh_ndt = new_anh_ndt;
+      pthread_mutex_unlock(&mutex);
+    }
+#ifdef CUDA_FOUND
+    else if (_method_type == MethodType::PCL_ANH_GPU)
+    {
+      std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
+          std::make_shared<gpu::GNormalDistributionsTransform>();
+
+      new_anh_gpu_ndt_ptr->setResolution(ndt_res);
+      new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
+      new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
+      new_anh_gpu_ndt_ptr->setStepSize(step_size);
+      new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+
+      pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+
+      pcl::PointXYZ dummy_point;
+      dummy_scan_ptr->push_back(dummy_point);
+      new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);
+
+      new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());
+
+      pthread_mutex_lock(&mutex);
+      anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
+      pthread_mutex_unlock(&mutex);
+    }
+#endif
+#ifdef USE_PCL_OPENMP
+    else if (_method_type == MethodType::PCL_OPENMP)
+    {
+      pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_omp_ndt;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+      new_omp_ndt.setResolution(ndt_res);
+      new_omp_ndt.setInputTarget(map_ptr);
+      new_omp_ndt.setMaximumIterations(max_iter);
+      new_omp_ndt.setStepSize(step_size);
+      new_omp_ndt.setTransformationEpsilon(trans_eps);
+
+      new_omp_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+      pthread_mutex_lock(&mutex);
+      omp_ndt = new_omp_ndt;
+      pthread_mutex_unlock(&mutex);
+    }
+#endif
+    map_loaded = 1;
+  }
+}
+
+static void gnss_callback(const geometry_msgs::PoseStamped::ConstPtr& input)
+{
+  tf::Quaternion gnss_q(input->pose.orientation.x, input->pose.orientation.y, input->pose.orientation.z,
+                        input->pose.orientation.w);
+  tf::Matrix3x3 gnss_m(gnss_q);
+
+  current_gnss_pose.x = input->pose.position.x;
+  current_gnss_pose.y = input->pose.position.y;
+  current_gnss_pose.z = input->pose.position.z;
+  gnss_m.getRPY(current_gnss_pose.roll, current_gnss_pose.pitch, current_gnss_pose.yaw);
+
+  static int matching_fail_cnt = 0;
+
+  if(previous_score <= USING_GPS_THRESHOLD)
+    matching_fail_cnt = 0;
+  else
+    matching_fail_cnt++;
+
+  if( previous_score == 0.0){
+    previous_score = 0.0;
+    current_pose = current_gnss_pose;
+    previous_pose = previous_gnss_pose;
+  }
+  else if( previous_score > USING_GPS_THRESHOLD && _is_init_match_finished == true){
+    previous_score = 0.0;
+    current_pose = current_gnss_pose;
+    previous_pose = previous_gnss_pose;
+  }
+  else if(matching_fail_cnt > 30){
+    previous_score = 0.0;
+    current_pose = current_gnss_pose;
+    previous_pose = previous_gnss_pose;
+  }
+
+  previous_gnss_pose = current_gnss_pose;
+
+}
+
+static void initialpose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& input)
+{
+  tf::TransformListener listener;
+  tf::StampedTransform transform;
+  try
+  {
+    ros::Time now = ros::Time(0);
+    listener.waitForTransform("/map", input->header.frame_id, now, ros::Duration(10.0));
+    listener.lookupTransform("/map", input->header.frame_id, now, transform);
+  }
+  catch (tf::TransformException& ex)
+  {
+    ROS_ERROR("%s", ex.what());
+  }
+
+  tf::Quaternion q(input->pose.pose.orientation.x, input->pose.pose.orientation.y, input->pose.pose.orientation.z,
+                   input->pose.pose.orientation.w);
+  tf::Matrix3x3 m(q);
+
+  if (_use_local_transform == true)
+  {
+    current_pose.x = input->pose.pose.position.x;
+    current_pose.y = input->pose.pose.position.y;
+    current_pose.z = input->pose.pose.position.z;
+  }
+  else
+  {
+    current_pose.x = input->pose.pose.position.x + transform.getOrigin().x();
+    current_pose.y = input->pose.pose.position.y + transform.getOrigin().y();
+    current_pose.z = input->pose.pose.position.z + transform.getOrigin().z();
+  }
+  m.getRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
+
+  if (_get_height == true && map_loaded == 1)
+  {
+    double min_distance = DBL_MAX;
+    double nearest_z = current_pose.z;
+    for (const auto& p : map)
+    {
+      double distance = hypot(current_pose.x - p.x, current_pose.y - p.y);
+      if (distance < min_distance)
+      {
+        min_distance = distance;
+        nearest_z = p.z;
+      }
+    }
+    current_pose.z = nearest_z;
+  }
+
+  current_pose_imu = current_pose_odom = current_pose_imu_odom = current_pose;
+  previous_pose.x = current_pose.x;
+  previous_pose.y = current_pose.y;
+  previous_pose.z = current_pose.z;
+  previous_pose.roll = current_pose.roll;
+  previous_pose.pitch = current_pose.pitch;
+  previous_pose.yaw = current_pose.yaw;
+
+  current_velocity = 0.0;
+  current_velocity_x = 0.0;
+  current_velocity_y = 0.0;
+  current_velocity_z = 0.0;
+  angular_velocity = 0.0;
+
+  current_accel = 0.0;
+  current_accel_x = 0.0;
+  current_accel_y = 0.0;
+  current_accel_z = 0.0;
+
+  offset_x = 0.0;
+  offset_y = 0.0;
+  offset_z = 0.0;
+  offset_yaw = 0.0;
+
+  offset_imu_x = 0.0;
+  offset_imu_y = 0.0;
+  offset_imu_z = 0.0;
+  offset_imu_roll = 0.0;
+  offset_imu_pitch = 0.0;
+  offset_imu_yaw = 0.0;
+
+  offset_odom_x = 0.0;
+  offset_odom_y = 0.0;
+  offset_odom_z = 0.0;
+  offset_odom_roll = 0.0;
+  offset_odom_pitch = 0.0;
+  offset_odom_yaw = 0.0;
+
+  offset_imu_odom_x = 0.0;
+  offset_imu_odom_y = 0.0;
+  offset_imu_odom_z = 0.0;
+  offset_imu_odom_roll = 0.0;
+  offset_imu_odom_pitch = 0.0;
+  offset_imu_odom_yaw = 0.0;
+
+  previous_score = 0.0;
+
+  init_pos_set = 1;
+}
+
+static void imu_odom_calc(ros::Time current_time)
+{
+  static ros::Time previous_time = current_time;
+  double diff_time = (current_time - previous_time).toSec();
+
+  double diff_imu_roll = imu.angular_velocity.x * diff_time;
+  double diff_imu_pitch = imu.angular_velocity.y * diff_time;
+  double diff_imu_yaw = imu.angular_velocity.z * diff_time;
+
+  current_pose_imu_odom.roll += diff_imu_roll;
+  current_pose_imu_odom.pitch += diff_imu_pitch;
+  current_pose_imu_odom.yaw += diff_imu_yaw;
+
+  double diff_distance = odom.twist.twist.linear.x * diff_time;
+  offset_imu_odom_x += diff_distance * cos(-current_pose_imu_odom.pitch) * cos(current_pose_imu_odom.yaw);
+  offset_imu_odom_y += diff_distance * cos(-current_pose_imu_odom.pitch) * sin(current_pose_imu_odom.yaw);
+  offset_imu_odom_z += diff_distance * sin(-current_pose_imu_odom.pitch);
+
+  offset_imu_odom_roll += diff_imu_roll;
+  offset_imu_odom_pitch += diff_imu_pitch;
+  offset_imu_odom_yaw += diff_imu_yaw;
+
+  predict_pose_imu_odom.x = previous_pose.x + offset_imu_odom_x;
+  predict_pose_imu_odom.y = previous_pose.y + offset_imu_odom_y;
+  predict_pose_imu_odom.z = previous_pose.z + offset_imu_odom_z;
+  predict_pose_imu_odom.roll = previous_pose.roll + offset_imu_odom_roll;
+  predict_pose_imu_odom.pitch = previous_pose.pitch + offset_imu_odom_pitch;
+  predict_pose_imu_odom.yaw = previous_pose.yaw + offset_imu_odom_yaw;
+
+  previous_time = current_time;
+}
+
+static void odom_calc(ros::Time current_time)
+{
+  static ros::Time previous_time = current_time;
+  double diff_time = (current_time - previous_time).toSec();
+
+  double diff_odom_roll = odom.twist.twist.angular.x * diff_time;
+  double diff_odom_pitch = odom.twist.twist.angular.y * diff_time;
+  double diff_odom_yaw = odom.twist.twist.angular.z * diff_time;
+
+  current_pose_odom.roll += diff_odom_roll;
+  current_pose_odom.pitch += diff_odom_pitch;
+  current_pose_odom.yaw += diff_odom_yaw;
+
+  double diff_distance = odom.twist.twist.linear.x * diff_time;
+  offset_odom_x += diff_distance * cos(-current_pose_odom.pitch) * cos(current_pose_odom.yaw);
+  offset_odom_y += diff_distance * cos(-current_pose_odom.pitch) * sin(current_pose_odom.yaw);
+  offset_odom_z += diff_distance * sin(-current_pose_odom.pitch);
+
+  offset_odom_roll += diff_odom_roll;
+  offset_odom_pitch += diff_odom_pitch;
+  offset_odom_yaw += diff_odom_yaw;
+
+  predict_pose_odom.x = previous_pose.x + offset_odom_x;
+  predict_pose_odom.y = previous_pose.y + offset_odom_y;
+  predict_pose_odom.z = previous_pose.z + offset_odom_z;
+  predict_pose_odom.roll = previous_pose.roll + offset_odom_roll;
+  predict_pose_odom.pitch = previous_pose.pitch + offset_odom_pitch;
+  predict_pose_odom.yaw = previous_pose.yaw + offset_odom_yaw;
+
+  previous_time = current_time;
+}
+
+static void imu_calc(ros::Time current_time)
+{
+  static ros::Time previous_time = current_time;
+  double diff_time = (current_time - previous_time).toSec();
+
+  double diff_imu_roll = imu.angular_velocity.x * diff_time;
+  double diff_imu_pitch = imu.angular_velocity.y * diff_time;
+  double diff_imu_yaw = imu.angular_velocity.z * diff_time;
+
+  current_pose_imu.roll += diff_imu_roll;
+  current_pose_imu.pitch += diff_imu_pitch;
+  current_pose_imu.yaw += diff_imu_yaw;
+
+  double accX1 = imu.linear_acceleration.x;
+  double accY1 = std::cos(current_pose_imu.roll) * imu.linear_acceleration.y -
+                 std::sin(current_pose_imu.roll) * imu.linear_acceleration.z;
+  double accZ1 = std::sin(current_pose_imu.roll) * imu.linear_acceleration.y +
+                 std::cos(current_pose_imu.roll) * imu.linear_acceleration.z;
+
+  double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+  double accY2 = accY1;
+  double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
+
+  double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+  double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+  double accZ = accZ2;
+
+  offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+  offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+  offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
+
+  current_velocity_imu_x += accX * diff_time;
+  current_velocity_imu_y += accY * diff_time;
+  current_velocity_imu_z += accZ * diff_time;
+
+  offset_imu_roll += diff_imu_roll;
+  offset_imu_pitch += diff_imu_pitch;
+  offset_imu_yaw += diff_imu_yaw;
+
+  predict_pose_imu.x = previous_pose.x + offset_imu_x;
+  predict_pose_imu.y = previous_pose.y + offset_imu_y;
+  predict_pose_imu.z = previous_pose.z + offset_imu_z;
+  predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+  predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+  predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+  previous_time = current_time;
+}
+
+static double wrapToPm(double a_num, const double a_max)
+{
+  if (a_num >= a_max)
+  {
+    a_num -= 2.0 * a_max;
+  }
+  return a_num;
+}
+
+static double wrapToPmPi(const double a_angle_rad)
+{
+  return wrapToPm(a_angle_rad, M_PI);
+}
+
+static double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
+{
+  double diff_rad = lhs_rad - rhs_rad;
+  if (diff_rad >= M_PI)
+    diff_rad = diff_rad - 2 * M_PI;
+  else if (diff_rad < -M_PI)
+    diff_rad = diff_rad + 2 * M_PI;
+  return diff_rad;
+}
+
+static void odom_callback(const nav_msgs::Odometry::ConstPtr& input)
+{
+  // std::cout << __func__ << std::endl;
+
+  odom = *input;
+  odom_calc(input->header.stamp);
+}
+
+static void imuUpsideDown(const sensor_msgs::Imu::Ptr input)
+{
+  double input_roll, input_pitch, input_yaw;
+
+  tf::Quaternion input_orientation;
+  tf::quaternionMsgToTF(input->orientation, input_orientation);
+  tf::Matrix3x3(input_orientation).getRPY(input_roll, input_pitch, input_yaw);
+
+  input->angular_velocity.x *= -1;
+  input->angular_velocity.y *= -1;
+  input->angular_velocity.z *= -1;
+
+  input->linear_acceleration.x *= -1;
+  input->linear_acceleration.y *= -1;
+  input->linear_acceleration.z *= -1;
+
+  input_roll *= -1;
+  input_pitch *= -1;
+  input_yaw *= -1;
+
+  input->orientation = tf::createQuaternionMsgFromRollPitchYaw(input_roll, input_pitch, input_yaw);
+}
+
+static void imu_callback(const sensor_msgs::Imu::Ptr& input)
+{
+  // std::cout << __func__ << std::endl;
+
+  if (_imu_upside_down)
+    imuUpsideDown(input);
+
+  const ros::Time current_time = input->header.stamp;
+  static ros::Time previous_time = current_time;
+  const double diff_time = (current_time - previous_time).toSec();
+
+  double imu_roll, imu_pitch, imu_yaw;
+  tf::Quaternion imu_orientation;
+  tf::quaternionMsgToTF(input->orientation, imu_orientation);
+  tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
+
+  imu_roll = wrapToPmPi(imu_roll);
+  imu_pitch = wrapToPmPi(imu_pitch);
+  imu_yaw = wrapToPmPi(imu_yaw);
+
+  static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
+  const double diff_imu_roll = calcDiffForRadian(imu_roll, previous_imu_roll);
+  const double diff_imu_pitch = calcDiffForRadian(imu_pitch, previous_imu_pitch);
+  const double diff_imu_yaw = calcDiffForRadian(imu_yaw, previous_imu_yaw);
+
+  imu.header = input->header;
+  imu.linear_acceleration.x = input->linear_acceleration.x;
+  // imu.linear_acceleration.y = input->linear_acceleration.y;
+  // imu.linear_acceleration.z = input->linear_acceleration.z;
+  imu.linear_acceleration.y = 0;
+  imu.linear_acceleration.z = 0;
+
+  if (diff_time != 0)
+  {
+    imu.angular_velocity.x = diff_imu_roll / diff_time;
+    imu.angular_velocity.y = diff_imu_pitch / diff_time;
+    imu.angular_velocity.z = diff_imu_yaw / diff_time;
+  }
+  else
+  {
+    imu.angular_velocity.x = 0;
+    imu.angular_velocity.y = 0;
+    imu.angular_velocity.z = 0;
+  }
+
+  imu_calc(input->header.stamp);
+
+  previous_time = current_time;
+  previous_imu_roll = imu_roll;
+  previous_imu_pitch = imu_pitch;
+  previous_imu_yaw = imu_yaw;
+}
+
+static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input)
+{ 
+  // std::cout<<"@@@@ "<<input->data.size()<<std::endl;
+  static int match_cnt = 10;
+
+  if (map_loaded != 1 || init_pos_set != 1) return;
+
+  // Check inital matching is success or not
+  if(_is_init_match_finished == false){
+
+    if(previous_score < _init_match_threshold) match_cnt--;
+    else match_cnt = 10;
+
+    if(previous_score < _init_match_threshold && previous_score != 0.0 && match_cnt <0){
+      _is_init_match_finished = true;
+      #ifdef DEBUG
+        std::cout<<"Success initial matching!"<<std::endl;
+      #endif
+    }
+  }
+
+  matching_start = std::chrono::system_clock::now();
+
+  static tf::TransformBroadcaster br, kalman_br;
+  tf::Transform transform, kalman_transform;
+  tf::Quaternion predict_q, ndt_q, current_q, localizer_q, kalman_q;
+
+  pcl::PointXYZ p;
+  pcl::PointCloud<pcl::PointXYZ> filtered_scan;
+
+  ros::Time current_scan_time = input->header.stamp;
+  static ros::Time previous_scan_time = current_scan_time;
+
+  pcl::fromROSMsg(*input, filtered_scan);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan));
+  int scan_points_num = filtered_scan_ptr->size();
+
+  Eigen::Matrix4f t(Eigen::Matrix4f::Identity());   // base_link
+  Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());  // localizer
+
+  std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
+      getFitnessScore_end;
+  static double align_time, getFitnessScore_time = 0.0;
+
+  pthread_mutex_lock(&mutex);
+
+  if (_method_type == MethodType::PCL_GENERIC)
+    ndt.setInputSource(filtered_scan_ptr);
+  else if (_method_type == MethodType::PCL_ANH)
+    anh_ndt.setInputSource(filtered_scan_ptr);
+#ifdef CUDA_FOUND
+  else if (_method_type == MethodType::PCL_ANH_GPU){
+    anh_gpu_ndt_ptr->setInputSource(filtered_scan_ptr);
+  }
+#endif
+#ifdef USE_PCL_OPENMP
+  else if (_method_type == MethodType::PCL_OPENMP)
+    omp_ndt.setInputSource(filtered_scan_ptr);
+#endif
+
+  // Guess the initial gross estimation of the transformation
+  double diff_time = (current_scan_time - previous_scan_time).toSec();
+
+  if (_offset == "linear")
+  {
+    offset_x = current_velocity_x * diff_time;
+    offset_y = current_velocity_y * diff_time;
+    offset_z = current_velocity_z * diff_time;
+    offset_yaw = angular_velocity * diff_time;
+  }
+  else if (_offset == "quadratic")
+  {
+    offset_x = (current_velocity_x + current_accel_x * diff_time) * diff_time;
+    offset_y = (current_velocity_y + current_accel_y * diff_time) * diff_time;
+    offset_z = current_velocity_z * diff_time;
+    offset_yaw = angular_velocity * diff_time;
+  }
+  else if (_offset == "zero")
+  {
+    offset_x = 0.0;
+    offset_y = 0.0;
+    offset_z = 0.0;
+    offset_yaw = 0.0;
+  }
+
+  predict_pose.x = previous_pose.x + offset_x;
+  predict_pose.y = previous_pose.y + offset_y;
+  predict_pose.z = previous_pose.z + offset_z;
+  predict_pose.roll = previous_pose.roll;
+  predict_pose.pitch = previous_pose.pitch;
+  predict_pose.yaw = previous_pose.yaw + offset_yaw;
+
+  if (_use_imu == true && _use_odom == true)
+    imu_odom_calc(current_scan_time);
+  if (_use_imu == true && _use_odom == false)
+    imu_calc(current_scan_time);
+  if (_use_imu == false && _use_odom == true)
+    odom_calc(current_scan_time);
+
+  pose predict_pose_for_ndt;
+  if (_use_imu == true && _use_odom == true)
+    predict_pose_for_ndt = predict_pose_imu_odom;
+  else if (_use_imu == true && _use_odom == false)
+    predict_pose_for_ndt = predict_pose_imu;
+  else if (_use_imu == false && _use_odom == true)
+    predict_pose_for_ndt = predict_pose_odom;
+  else
+    predict_pose_for_ndt = predict_pose;
+
+  Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
+  Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
+  Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
+  Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
+  Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+  if (_method_type == MethodType::PCL_GENERIC)
+  {
+    align_start = std::chrono::system_clock::now();
+    ndt.align(*output_cloud, init_guess);
+    align_end = std::chrono::system_clock::now();
+
+    has_converged = ndt.hasConverged();
+
+    t = ndt.getFinalTransformation();
+    iteration = ndt.getFinalNumIteration();
+
+    getFitnessScore_start = std::chrono::system_clock::now();
+    fitness_score = ndt.getFitnessScore();
+    getFitnessScore_end = std::chrono::system_clock::now();
+
+    trans_probability = ndt.getTransformationProbability();
+  }
+  else if (_method_type == MethodType::PCL_ANH)
+  {
+    align_start = std::chrono::system_clock::now();
+    anh_ndt.align(init_guess);
+    align_end = std::chrono::system_clock::now();
+
+    has_converged = anh_ndt.hasConverged();
+
+    t = anh_ndt.getFinalTransformation();
+    iteration = anh_ndt.getFinalNumIteration();
+
+    getFitnessScore_start = std::chrono::system_clock::now();
+    fitness_score = anh_ndt.getFitnessScore();
+    getFitnessScore_end = std::chrono::system_clock::now();
+
+    trans_probability = anh_ndt.getTransformationProbability();
+  }
+#ifdef CUDA_FOUND
+  else if (_method_type == MethodType::PCL_ANH_GPU)
+  {
+    align_start = std::chrono::system_clock::now();
+    anh_gpu_ndt_ptr->align(init_guess);
+    align_end = std::chrono::system_clock::now();
+
+    has_converged = anh_gpu_ndt_ptr->hasConverged();
+
+    t = anh_gpu_ndt_ptr->getFinalTransformation();
+    iteration = anh_gpu_ndt_ptr->getFinalNumIteration();
+
+    getFitnessScore_start = std::chrono::system_clock::now();
+    fitness_score = anh_gpu_ndt_ptr->getFitnessScore();
+    getFitnessScore_end = std::chrono::system_clock::now();
+
+    trans_probability = anh_gpu_ndt_ptr->getTransformationProbability();      
+  }
+#endif
+#ifdef USE_PCL_OPENMP
+  else if (_method_type == MethodType::PCL_OPENMP)
+  {
+    align_start = std::chrono::system_clock::now();
+    omp_ndt.align(*output_cloud, init_guess);
+    align_end = std::chrono::system_clock::now();
+
+    has_converged = omp_ndt.hasConverged();
+
+    t = omp_ndt.getFinalTransformation();
+    iteration = omp_ndt.getFinalNumIteration();
+
+    getFitnessScore_start = std::chrono::system_clock::now();
+    fitness_score = omp_ndt.getFitnessScore();
+    getFitnessScore_end = std::chrono::system_clock::now();
+
+    trans_probability = omp_ndt.getTransformationProbability();
+  }
+#endif
+  align_time = std::chrono::duration_cast<std::chrono::microseconds>(align_end - align_start).count() / 1000.0;
+
+  t2 = t * tf_btol.inverse();
+
+  getFitnessScore_time =
+      std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
+      1000.0;
+
+  pthread_mutex_unlock(&mutex);
+
+  tf::Matrix3x3 mat_l;  // localizer
+  mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
+                  static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
+                  static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
+
+  // Update localizer_pose
+  localizer_pose.x = t(0, 3);
+  localizer_pose.y = t(1, 3);
+  localizer_pose.z = t(2, 3);
+  mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
+
+  tf::Matrix3x3 mat_b;  // base_link
+  mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
+                  static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
+                  static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
+
+  // Update ndt_pose
+  ndt_pose.x = t2(0, 3);
+  ndt_pose.y = t2(1, 3);
+  ndt_pose.z = t2(2, 3);
+  mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
+
+  // Calculate the difference between ndt_pose and predict_pose
+  predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
+                            (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
+                            (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
+
+  if (predict_pose_error <= PREDICT_POSE_THRESHOLD)
+  {
+    use_predict_pose = 0;
+  }
+  else
+  {
+    use_predict_pose = 1;
+  }
+  use_predict_pose = 0;
+
+  if (use_predict_pose == 0)
+  {
+    current_pose.x = ndt_pose.x;
+    current_pose.y = ndt_pose.y;
+    current_pose.z = ndt_pose.z;
+    current_pose.roll = ndt_pose.roll;
+    current_pose.pitch = ndt_pose.pitch;
+    current_pose.yaw = ndt_pose.yaw;
+  }
+  else
+  {
+    current_pose.x = predict_pose_for_ndt.x;
+    current_pose.y = predict_pose_for_ndt.y;
+    current_pose.z = predict_pose_for_ndt.z;
+    current_pose.roll = predict_pose_for_ndt.roll;
+    current_pose.pitch = predict_pose_for_ndt.pitch;
+    current_pose.yaw = predict_pose_for_ndt.yaw;
+  }
+
+  // Compute the velocity and acceleration
+  diff_x = current_pose.x - previous_pose.x;
+  diff_y = current_pose.y - previous_pose.y;
+  diff_z = current_pose.z - previous_pose.z;
+  diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
+  diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
+
+  const pose trans_current_pose = convertPoseIntoRelativeCoordinate(current_pose, previous_pose);
+
+  current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
+  current_velocity =  (trans_current_pose.x >= 0) ? current_velocity : -current_velocity;
+  current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
+  current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
+  current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
+  angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
+
+  current_pose_imu.x = current_pose.x;
+  current_pose_imu.y = current_pose.y;
+  current_pose_imu.z = current_pose.z;
+  current_pose_imu.roll = current_pose.roll;
+  current_pose_imu.pitch = current_pose.pitch;
+  current_pose_imu.yaw = current_pose.yaw;
+
+  current_velocity_imu_x = current_velocity_x;
+  current_velocity_imu_y = current_velocity_y;
+  current_velocity_imu_z = current_velocity_z;
+
+  current_pose_odom.x = current_pose.x;
+  current_pose_odom.y = current_pose.y;
+  current_pose_odom.z = current_pose.z;
+  current_pose_odom.roll = current_pose.roll;
+  current_pose_odom.pitch = current_pose.pitch;
+  current_pose_odom.yaw = current_pose.yaw;
+
+  current_pose_imu_odom.x = current_pose.x;
+  current_pose_imu_odom.y = current_pose.y;
+  current_pose_imu_odom.z = current_pose.z;
+  current_pose_imu_odom.roll = current_pose.roll;
+  current_pose_imu_odom.pitch = current_pose.pitch;
+  current_pose_imu_odom.yaw = current_pose.yaw;
+
+  current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
+  if (std::fabs(current_velocity_smooth) < 0.2)
+  {
+    current_velocity_smooth = 0.0;
+  }
+
+  current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
+  current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
+  current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
+  current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
+
+  estimated_vel_mps.data = current_velocity;
+  estimated_vel_kmph.data = current_velocity * 3.6;
+
+  estimated_vel_mps_pub.publish(estimated_vel_mps);
+  estimated_vel_kmph_pub.publish(estimated_vel_kmph);
+
+  // Enable Kalman Filter
+  if(!_is_kalman_filter_on && _use_kalman_filter && _is_init_match_finished && _is_ins_stat_received){
+    _is_kalman_filter_on = true;
+    linear_kalman_filter.set_init_pose(current_pose.x, current_pose.y);
+  }
+
+  // Run Kalman Filter
+  if(_is_kalman_filter_on){
+    Eigen::Vector2f u_k, z_k;
+
+    u_k << _previous_ins_stat_vel_x + 0.5f * _previous_ins_stat_acc_x * diff_time, _previous_ins_stat_vel_y + 0.5f * _previous_ins_stat_acc_y * diff_time;
+    z_k << current_pose.x, current_pose.y;
+
+    Eigen::Vector2f kalman_filtered_pose = linear_kalman_filter.run(diff_time, u_k, z_k);
+
+    current_kalman_pose.x = kalman_filtered_pose(0);
+    current_kalman_pose.y = kalman_filtered_pose(1);
+    current_kalman_pose.z = current_pose.z;
+    current_kalman_pose.roll = 0.0;
+    current_kalman_pose.pitch = 0.0;
+    current_kalman_pose.yaw = _current_ins_stat_yaw*M_PI/180.0;
+
+    // Check ndt matching failure    
+    double ndt_kalman_pose_diff = sqrt(pow(current_pose.x - current_kalman_pose.x,2) + pow(current_pose.y - current_kalman_pose.y, 2));
+
+    // std::cout<<"## pose diff: "<<ndt_kalman_pose_diff<< " | score diff: "<< abs(previous_score - fitness_score)<<std::endl;
+    // std::cout<<"fail score threshold: "<< _failure_score_diff_threshold <<" | restore score threshold: "<<_recovery_score_diff_threshold<<std::endl;\
+    // std::cout<<"fail pose threshold: "<< _failure_pose_diff_threshold <<" | restore pose threshold: "<<_recovery_pose_diff_threshold<<std::endl;
+    static int success_cnt = 1;
+    if(!_is_matching_failed && (ndt_kalman_pose_diff > _failure_pose_diff_threshold || abs(previous_score - fitness_score) > _failure_score_diff_threshold)){ 
+      #ifdef DEBUG
+        std::cout<<"NDT matching is FAILED! || FAILED?"<<_is_matching_failed <<" | score diff: " << abs(previous_score - fitness_score) << "|| pose_diff: " << ndt_kalman_pose_diff <<std::endl;
+      #endif
+      
+      _is_matching_failed = true;
+      
+    }    
+    else if( _is_matching_failed && (ndt_kalman_pose_diff < _recovery_pose_diff_threshold && abs(_previous_success_score - fitness_score) < _recovery_score_diff_threshold)){ // Recover success
+      if(success_cnt-- < 0){
+        #ifdef DEBUG
+        std::cout<<"NDT matching is ON! || FAILED?"<<_is_matching_failed << " | current score: "<<fitness_score<<" || previous_success_score: "<<_previous_success_score<<" || pose_diff: "<<ndt_kalman_pose_diff<<std::endl;
+        #endif
+        
+        _is_matching_failed = false;
+        success_cnt = 1;
+        
+      }
+    }
+
+    if(_is_matching_failed){
+      linear_kalman_filter.restore();
+      kalman_filtered_pose = linear_kalman_filter.run_without_update(diff_time, u_k);
+
+      current_kalman_pose.x = kalman_filtered_pose(0);
+      current_kalman_pose.y = kalman_filtered_pose(1);
+      current_kalman_pose.z = 0.0;
+
+      std::cout<<"[Restore] pose_diff]: "<< ndt_kalman_pose_diff << " | score diff: "<< abs(previous_score - fitness_score) <<std::endl;
+
+      // current_kalman_pose.roll = 0.0;
+      // current_kalman_pose.pitch = 0.0;
+      // current_kalman_pose.yaw = _current_ins_stat_yaw*M_PI/180.0;
+    }
+  }
+
+  // Set values for publishing pose
+  predict_q.setRPY(predict_pose.roll, predict_pose.pitch, predict_pose.yaw);
+  if (_use_local_transform == true)
+  {
+    tf::Vector3 v(predict_pose.x, predict_pose.y, predict_pose.z);
+    tf::Transform transform(predict_q, v);
+    predict_pose_msg.header.frame_id = "/map";
+    predict_pose_msg.header.stamp = current_scan_time;
+    predict_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
+    predict_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
+    predict_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
+    predict_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
+    predict_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
+    predict_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
+    predict_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
+  }
+  else
+  {
+    predict_pose_msg.header.frame_id = "/map";
+    predict_pose_msg.header.stamp = current_scan_time;
+    predict_pose_msg.pose.position.x = predict_pose.x;
+    predict_pose_msg.pose.position.y = predict_pose.y;
+    predict_pose_msg.pose.position.z = predict_pose.z;
+    predict_pose_msg.pose.orientation.x = predict_q.x();
+    predict_pose_msg.pose.orientation.y = predict_q.y();
+    predict_pose_msg.pose.orientation.z = predict_q.z();
+    predict_pose_msg.pose.orientation.w = predict_q.w();
+  }
+
+  tf::Quaternion predict_q_imu;
+  predict_q_imu.setRPY(predict_pose_imu.roll, predict_pose_imu.pitch, predict_pose_imu.yaw);
+  predict_pose_imu_msg.header.frame_id = "map";
+  predict_pose_imu_msg.header.stamp = input->header.stamp;
+  predict_pose_imu_msg.pose.position.x = predict_pose_imu.x;
+  predict_pose_imu_msg.pose.position.y = predict_pose_imu.y;
+  predict_pose_imu_msg.pose.position.z = predict_pose_imu.z;
+  predict_pose_imu_msg.pose.orientation.x = predict_q_imu.x();
+  predict_pose_imu_msg.pose.orientation.y = predict_q_imu.y();
+  predict_pose_imu_msg.pose.orientation.z = predict_q_imu.z();
+  predict_pose_imu_msg.pose.orientation.w = predict_q_imu.w();
+  predict_pose_imu_pub.publish(predict_pose_imu_msg);
+
+  tf::Quaternion predict_q_odom;
+  predict_q_odom.setRPY(predict_pose_odom.roll, predict_pose_odom.pitch, predict_pose_odom.yaw);
+  predict_pose_odom_msg.header.frame_id = "map";
+  predict_pose_odom_msg.header.stamp = input->header.stamp;
+  predict_pose_odom_msg.pose.position.x = predict_pose_odom.x;
+  predict_pose_odom_msg.pose.position.y = predict_pose_odom.y;
+  predict_pose_odom_msg.pose.position.z = predict_pose_odom.z;
+  predict_pose_odom_msg.pose.orientation.x = predict_q_odom.x();
+  predict_pose_odom_msg.pose.orientation.y = predict_q_odom.y();
+  predict_pose_odom_msg.pose.orientation.z = predict_q_odom.z();
+  predict_pose_odom_msg.pose.orientation.w = predict_q_odom.w();
+  predict_pose_odom_pub.publish(predict_pose_odom_msg);
+
+  tf::Quaternion predict_q_imu_odom;
+  predict_q_imu_odom.setRPY(predict_pose_imu_odom.roll, predict_pose_imu_odom.pitch, predict_pose_imu_odom.yaw);
+  predict_pose_imu_odom_msg.header.frame_id = "map";
+  predict_pose_imu_odom_msg.header.stamp = input->header.stamp;
+  predict_pose_imu_odom_msg.pose.position.x = predict_pose_imu_odom.x;
+  predict_pose_imu_odom_msg.pose.position.y = predict_pose_imu_odom.y;
+  predict_pose_imu_odom_msg.pose.position.z = predict_pose_imu_odom.z;
+  predict_pose_imu_odom_msg.pose.orientation.x = predict_q_imu_odom.x();
+  predict_pose_imu_odom_msg.pose.orientation.y = predict_q_imu_odom.y();
+  predict_pose_imu_odom_msg.pose.orientation.z = predict_q_imu_odom.z();
+  predict_pose_imu_odom_msg.pose.orientation.w = predict_q_imu_odom.w();
+  predict_pose_imu_odom_pub.publish(predict_pose_imu_odom_msg);
+
+  ndt_q.setRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw);
+  
+  if(_is_kalman_filter_on){
+    kalman_q.setRPY(current_kalman_pose.roll, current_kalman_pose.pitch, current_kalman_pose.yaw);
+  }
+  
+  if(_is_matching_failed && _is_kalman_filter_on){
+    ndt_pose_msg.header.frame_id = "/map";
+    ndt_pose_msg.header.stamp = current_scan_time;
+    ndt_pose_msg.pose.position.x = current_kalman_pose.x;
+    ndt_pose_msg.pose.position.y = current_kalman_pose.y;
+    ndt_pose_msg.pose.position.z = current_kalman_pose.z;
+    ndt_pose_msg.pose.orientation.x = kalman_q.x();
+    ndt_pose_msg.pose.orientation.y = kalman_q.y();
+    ndt_pose_msg.pose.orientation.z = kalman_q.z();
+    ndt_pose_msg.pose.orientation.w = kalman_q.w();
+  }
+  else if (_use_local_transform == true){
+    tf::Vector3 v(ndt_pose.x, ndt_pose.y, ndt_pose.z);
+    tf::Transform transform(ndt_q, v);
+    ndt_pose_msg.header.frame_id = "/map";
+    ndt_pose_msg.header.stamp = current_scan_time;
+    ndt_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
+    ndt_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
+    ndt_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
+    ndt_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
+    ndt_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
+    ndt_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
+    ndt_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
+  }
+  else{
+    ndt_pose_msg.header.frame_id = "/map";
+    ndt_pose_msg.header.stamp = current_scan_time;
+    ndt_pose_msg.pose.position.x = ndt_pose.x;
+    ndt_pose_msg.pose.position.y = ndt_pose.y;
+    ndt_pose_msg.pose.position.z = ndt_pose.z;
+    ndt_pose_msg.pose.orientation.x = ndt_q.x();
+    ndt_pose_msg.pose.orientation.y = ndt_q.y();
+    ndt_pose_msg.pose.orientation.z = ndt_q.z();
+    ndt_pose_msg.pose.orientation.w = ndt_q.w();    
+  }
+
+
+  current_q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
+  // current_pose is published by vel_pose_mux
+  /*
+  current_pose_msg.header.frame_id = "/map";
+  current_pose_msg.header.stamp = current_scan_time;
+  current_pose_msg.pose.position.x = current_pose.x;
+  current_pose_msg.pose.position.y = current_pose.y;
+  current_pose_msg.pose.position.z = current_pose.z;
+  current_pose_msg.pose.orientation.x = current_q.x();
+  current_pose_msg.pose.orientation.y = current_q.y();
+  current_pose_msg.pose.orientation.z = current_q.z();
+  current_pose_msg.pose.orientation.w = current_q.w();
+  */
+
+  localizer_q.setRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw);
+  
+  if (_use_local_transform == true)
+  {
+    tf::Vector3 v(localizer_pose.x, localizer_pose.y, localizer_pose.z);
+    tf::Transform transform(localizer_q, v);
+    localizer_pose_msg.header.frame_id = "/map";
+    localizer_pose_msg.header.stamp = current_scan_time;
+    localizer_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
+    localizer_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
+    localizer_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
+    localizer_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
+    localizer_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
+    localizer_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
+    localizer_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
+  }
+  else
+  {
+    localizer_pose_msg.header.frame_id = "/map";
+    localizer_pose_msg.header.stamp = current_scan_time;
+    localizer_pose_msg.pose.position.x = localizer_pose.x;
+    localizer_pose_msg.pose.position.y = localizer_pose.y;
+    localizer_pose_msg.pose.position.z = localizer_pose.z;
+    localizer_pose_msg.pose.orientation.x = localizer_q.x();
+    localizer_pose_msg.pose.orientation.y = localizer_q.y();
+    localizer_pose_msg.pose.orientation.z = localizer_q.z();
+    localizer_pose_msg.pose.orientation.w = localizer_q.w();
+  }
+
+  predict_pose_pub.publish(predict_pose_msg);
+  ndt_pose_pub.publish(ndt_pose_msg);
+  
+
+  // current_pose is published by vel_pose_mux
+  //    current_pose_pub.publish(current_pose_msg);
+  localizer_pose_pub.publish(localizer_pose_msg);  
+  
+  matching_end = std::chrono::system_clock::now();
+  exe_time = std::chrono::duration_cast<std::chrono::microseconds>(matching_end - matching_start).count() / 1000.0;
+  time_ndt_matching.data = exe_time;
+  time_ndt_matching_pub.publish(time_ndt_matching);
+
+  // Set values for /estimate_twist
+  estimate_twist_msg.header.stamp = current_scan_time;
+  estimate_twist_msg.header.frame_id = "/base_link";
+  estimate_twist_msg.twist.linear.x = current_velocity;
+  estimate_twist_msg.twist.linear.y = 0.0;
+  estimate_twist_msg.twist.linear.z = 0.0;
+  estimate_twist_msg.twist.angular.x = 0.0;
+  estimate_twist_msg.twist.angular.y = 0.0;
+  estimate_twist_msg.twist.angular.z = angular_velocity;
+
+  estimate_twist_pub.publish(estimate_twist_msg);
+
+  geometry_msgs::Vector3Stamped estimate_vel_msg;
+  estimate_vel_msg.header.stamp = current_scan_time;
+  estimate_vel_msg.vector.x = current_velocity;
+  estimated_vel_pub.publish(estimate_vel_msg);
+
+  previous_score = fitness_score;
+  if(!_is_matching_failed) _previous_success_score = previous_score;
+
+  // Set values for /ndt_stat
+  ndt_stat_msg.header.stamp = current_scan_time;
+  ndt_stat_msg.exe_time = time_ndt_matching.data;
+  ndt_stat_msg.iteration = iteration;
+  ndt_stat_msg.score = fitness_score;
+  ndt_stat_msg.velocity = current_velocity;
+  ndt_stat_msg.acceleration = current_accel;
+  ndt_stat_msg.use_predict_pose = 0;
+
+  ndt_stat_pub.publish(ndt_stat_msg);
+
+  rubis_pose_twist_msg.instance = rubis::instance_;
+  rubis_pose_twist_msg.pose = ndt_pose_msg;
+  rubis_pose_twist_msg.twist = estimate_twist_msg;
+  rubis_pose_twist_pub.publish(rubis_pose_twist_msg);
+
+  /* Compute NDT_Reliability */
+  ndt_reliability.data = Wa * (exe_time / 100.0) * 100.0 + Wb * (iteration / 10.0) * 100.0 +
+                          Wc * ((2.0 - trans_probability) / 2.0) * 100.0;
+  ndt_reliability_pub.publish(ndt_reliability);
+
+  // Write log
+  if(_output_log_data)
+  {
+    if (!ofs)
+    {
+      std::cerr << "Could not open " << filename << "." << std::endl;
+    }
+    else
+    {
+      ofs << input->header.seq << "," << scan_points_num << "," << step_size << "," << trans_eps << "," << std::fixed
+          << std::setprecision(5) << current_pose.x << "," << std::fixed << std::setprecision(5) << current_pose.y << ","
+          << std::fixed << std::setprecision(5) << current_pose.z << "," << current_pose.roll << "," << current_pose.pitch
+          << "," << current_pose.yaw << "," << predict_pose.x << "," << predict_pose.y << "," << predict_pose.z << ","
+          << predict_pose.roll << "," << predict_pose.pitch << "," << predict_pose.yaw << ","
+          << current_pose.x - predict_pose.x << "," << current_pose.y - predict_pose.y << ","
+          << current_pose.z - predict_pose.z << "," << current_pose.roll - predict_pose.roll << ","
+          << current_pose.pitch - predict_pose.pitch << "," << current_pose.yaw - predict_pose.yaw << ","
+          << predict_pose_error << "," << iteration << "," << fitness_score << "," << trans_probability << ","
+          << ndt_reliability.data << "," << current_velocity << "," << current_velocity_smooth << "," << current_accel
+          << "," << angular_velocity << "," << time_ndt_matching.data << "," << align_time << "," << getFitnessScore_time
+          << std::endl;
+    }
+  }
+  
+  // std::cout << "-----------------------------------------------------------------" << std::endl;
+  // std::cout << "Sequence: " << input->header.seq << std::endl;
+  // std::cout << "Timestamp: " << input->header.stamp << std::endl;
+  // std::cout << "Frame ID: " << input->header.frame_id << std::endl;
+  // //    std::cout << "Number of Scan Points: " << scan_ptr->size() << " points." << std::endl;
+  // std::cout << "Number of Filtered Scan Points: " << scan_points_num << " points." << std::endl;
+  // std::cout << "NDT has converged: " << has_converged << std::endl;
+  // std::cout << "Fitness Score: " << fitness_score << std::endl;
+  // std::cout << "Transformation Probability: " << trans_probability << std::endl;
+  // std::cout << "Execution Time: " << exe_time << " ms." << std::endl;
+  // std::cout << "Number of Iterations: " << iteration << std::endl;
+  // std::cout << "NDT Reliability: " << ndt_reliability.data << std::endl;
+  // std::cout << "(x,y,z,roll,pitch,yaw): " << std::endl;
+  // std::cout << "(" << current_pose.x << ", " << current_pose.y << ", " << current_pose.z << ", " << current_pose.roll
+  //           << ", " << current_pose.pitch << ", " << current_pose.yaw << ")" << std::endl;
+  // std::cout << "Transformation Matrix: " << std::endl;
+  // std::cout << t << std::endl;
+  // std::cout << "Align time: " << align_time << std::endl;
+  // std::cout << "Get fitness score time: " << getFitnessScore_time << std::endl;
+  // std::cout << "-----------------------------------------------------------------" << std::endl;
+
+  offset_imu_x = 0.0;
+  offset_imu_y = 0.0;
+  offset_imu_z = 0.0;
+  offset_imu_roll = 0.0;
+  offset_imu_pitch = 0.0;
+  offset_imu_yaw = 0.0;
+
+  offset_odom_x = 0.0;
+  offset_odom_y = 0.0;
+  offset_odom_z = 0.0;
+  offset_odom_roll = 0.0;
+  offset_odom_pitch = 0.0;
+  offset_odom_yaw = 0.0;
+
+  offset_imu_odom_x = 0.0;
+  offset_imu_odom_y = 0.0;
+  offset_imu_odom_z = 0.0;
+  offset_imu_odom_roll = 0.0;
+  offset_imu_odom_pitch = 0.0;
+  offset_imu_odom_yaw = 0.0;
+
+  // Send TF "/base_link" to "/map"
+  if(!_is_matching_failed){
+    transform.setOrigin(tf::Vector3(current_pose.x, current_pose.y, current_pose.z));
+    transform.setRotation(current_q);
+    //    br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
+    if (_use_local_transform == true)
+    {
+      br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, "/map", "/base_link"));
+    }
+    else
+    {
+      br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
+    }
+  }
+  else{ // When matching is failed
+    transform.setOrigin(tf::Vector3(current_kalman_pose.x, current_kalman_pose.y, current_kalman_pose.z));
+    current_q.setRPY(current_kalman_pose.roll, current_kalman_pose.pitch, current_kalman_pose.yaw);      
+    transform.setRotation(current_q);
+    br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
+  }
+  
+  // Send TF "/kalman" to "/map"
+  if(_is_kalman_filter_on){
+    kalman_transform.setOrigin(tf::Vector3(current_kalman_pose.x, current_kalman_pose.y, current_kalman_pose.z));    
+    kalman_transform.setRotation(kalman_q);
+    kalman_br.sendTransform(tf::StampedTransform(kalman_transform, current_scan_time, "/map", "/kalman"));
+  }
+
+  // Update previous_*** when kalman filter is not enabled
+  previous_pose.x = current_pose.x;
+  previous_pose.y = current_pose.y;
+  previous_pose.z = current_pose.z;
+  previous_pose.roll = current_pose.roll;
+  previous_pose.pitch = current_pose.pitch;
+  previous_pose.yaw = current_pose.yaw;
+
+  previous_scan_time = current_scan_time;
+
+  previous_previous_velocity = previous_velocity;
+  previous_velocity = current_velocity;
+  previous_velocity_x = current_velocity_x;
+  previous_velocity_y = current_velocity_y;
+  previous_velocity_z = current_velocity_z;
+  previous_accel = current_accel;
+
+  previous_estimated_vel_kmph.data = estimated_vel_kmph.data;    
+
+  if(_is_kalman_filter_on){
+    geometry_msgs::Quaternion q;
+    ToQuaternion(current_kalman_pose.yaw, current_kalman_pose.pitch, current_kalman_pose.roll, q);
+
+    geometry_msgs::PoseStamped kalman_filtered_pose_msg;
+    kalman_filtered_pose_msg.header = ndt_pose_msg.header;
+    kalman_filtered_pose_msg.pose.position.x = current_kalman_pose.x;
+    kalman_filtered_pose_msg.pose.position.y = current_kalman_pose.y;
+    kalman_filtered_pose_msg.pose.position.z = current_kalman_pose.z;
+    kalman_filtered_pose_msg.pose.orientation = q;
+    kalman_filtered_pose_pub.publish(kalman_filtered_pose_msg);
+
+    // Update previous by kalman filter output
+    previous_kalman_pose.x = current_kalman_pose.x;
+    previous_kalman_pose.y = current_kalman_pose.y;
+    previous_kalman_pose.z = current_kalman_pose.z;
+    previous_kalman_pose.roll = current_kalman_pose.roll;
+    previous_kalman_pose.pitch = current_kalman_pose.pitch;
+    previous_kalman_pose.yaw = current_kalman_pose.yaw;
+
+    if(_is_matching_failed) previous_pose = previous_kalman_pose;
+
+    _previous_ins_stat_vel_x = _current_ins_stat_vel_x;
+    _previous_ins_stat_vel_y = _current_ins_stat_vel_y;
+    _previous_ins_stat_acc_x = _current_ins_stat_acc_x;
+    _previous_ins_stat_acc_y = _current_ins_stat_acc_y;
+    _previous_ins_stat_yaw = _current_ins_stat_yaw;
+    _previous_ins_stat_linear_velocity = _current_ins_stat_linear_velocity;
+    _previous_ins_stat_linear_acceleration = _current_ins_stat_linear_acceleration;
+    _previous_ins_stat_angular_velocity = _current_ins_stat_angular_velocity;
+
+  }
+
+  std_msgs::Bool is_kalman_filter_on_msgs;
+  is_kalman_filter_on_msgs.data = _is_kalman_filter_on;
+  is_kalman_filter_on_pub.publish(is_kalman_filter_on_msgs);
+  
+}
+
+static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input){
+  rubis::instance_ = 0;
+  ndt_matching(input);
+}
+
+static void rubis_points_callback(const rubis_msgs::PointCloud2::ConstPtr& _input){
+  rubis::start_task_profiling();
+
+  sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared<const sensor_msgs::PointCloud2>(_input->msg);
+  rubis::instance_ = _input->instance;
+  ndt_matching(input);
+  // std::cout<<"Finish ndt_matching"<<std::endl;
+
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+}
+
+static void ins_stat_callback(const rubis_msgs::InsStat::ConstPtr& input){
+  _is_ins_stat_received = true;
+  _current_ins_stat_vel_x = input->vel_x;
+  _current_ins_stat_vel_y = input->vel_y;
+  _current_ins_stat_acc_x = input->acc_x;
+  _current_ins_stat_acc_y = input->acc_y;
+  _current_ins_stat_yaw = input->yaw;
+  _current_ins_stat_linear_velocity = input->linear_velocity;
+  _current_ins_stat_linear_acceleration = input->linear_acceleration;
+  _current_ins_stat_angular_velocity = input->angular_velocity;
+  return;
+}
+
+void* thread_func(void* args)
+{
+  ros::NodeHandle nh_map;
+  ros::CallbackQueue map_callback_queue;
+  nh_map.setCallbackQueue(&map_callback_queue);
+
+  ros::Subscriber map_sub = nh_map.subscribe("points_map", 10, map_callback); 
+  ros::Rate ros_rate(10);
+  while (nh_map.ok())
+  {
+    map_callback_queue.callAvailable(ros::WallDuration());
+    ros_rate.sleep();
+  }
+
+  return nullptr;
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "ndt_matching");
+  pthread_mutex_init(&mutex, NULL);
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  // Set log file name.
+  private_nh.getParam("output_log_data", _output_log_data);
+  if(_output_log_data)
+  {
+    char buffer[80];
+    std::time_t now = std::time(NULL);
+    std::tm* pnow = std::localtime(&now);
+    std::strftime(buffer, 80, "%Y%m%d_%H%M%S", pnow);
+    std::string directory_name = "/tmp/Autoware/log/ndt_matching";
+    filename = directory_name + "/" + std::string(buffer) + ".csv";
+    boost::filesystem::create_directories(boost::filesystem::path(directory_name));
+    ofs.open(filename.c_str(), std::ios::app);
+  }
+
+  // Geting parameters
+  int method_type_tmp = 0;
+  private_nh.getParam("method_type", method_type_tmp);
+  _method_type = static_cast<MethodType>(method_type_tmp);
+  private_nh.getParam("use_gnss", _use_gnss);
+  private_nh.getParam("queue_size", _queue_size);
+  private_nh.getParam("offset", _offset);
+  private_nh.getParam("get_height", _get_height);
+  private_nh.getParam("use_local_transform", _use_local_transform);
+  private_nh.getParam("use_kalman_filter", _use_kalman_filter);
+  private_nh.getParam("use_odom", _use_odom);
+  private_nh.getParam("use_odom", _use_odom);
+  private_nh.getParam("imu_upside_down", _imu_upside_down);
+  private_nh.getParam("imu_topic", _imu_topic);
+  private_nh.param<double>("gnss_reinit_fitness", _gnss_reinit_fitness, 500.0);
+  private_nh.param<float>("init_match_threshold", _init_match_threshold, 8.0);
+  
+  
+
+  nh.param<std::string>("/ndt_matching/localizer", _localizer, "velodyne");  
+
+  nh.param<float>("/ndt_matching/failure_score_diff_threshold", _failure_score_diff_threshold, 10.0);  
+  nh.param<float>("/ndt_matching/recovery_score_diff_threshold", _recovery_score_diff_threshold, 1.0);  
+  nh.param<float>("/ndt_matching/failure_pose_diff_threshold", _failure_pose_diff_threshold, 4.0);
+  nh.param<float>("/ndt_matching/recovery_pose_diff_threshold", _recovery_pose_diff_threshold, 1.0);
+
+  if(_use_kalman_filter){
+    std::vector<float> H_k_vec, Q_k_vec, R_k_vec, P_k_vec;
+
+    if(!nh.getParam("/ndt_matching/H_k", H_k_vec)){
+      ROS_ERROR("Failed to get parameter H_k");
+      exit(1);
+    }
+    if(!nh.getParam("/ndt_matching/Q_k", Q_k_vec)){
+      ROS_ERROR("Failed to get parameter Q_k");
+      exit(1);
+    }
+    if(!nh.getParam("/ndt_matching/R_k", R_k_vec)){
+      ROS_ERROR("Failed to get parameter R_k");
+      exit(1);
+    }
+    if(!nh.getParam("/ndt_matching/P_k", P_k_vec)){
+      ROS_ERROR("Failed to get parameter P_k");
+      exit(1);
+    }
+    
+    Eigen::Matrix2f H_k = Eigen::Matrix2f(H_k_vec.data());
+    Eigen::Matrix2f Q_k = Eigen::Matrix2f(Q_k_vec.data());
+    Eigen::Matrix2f R_k = Eigen::Matrix2f(R_k_vec.data());
+    Eigen::Matrix2f P_k = Eigen::Matrix2f(P_k_vec.data());
+    linear_kalman_filter = LKF(H_k, Q_k, R_k, P_k);
+  }
+
+  try
+  {
+    tf::TransformListener base_localizer_listener;
+    tf::StampedTransform  m_base_to_localizer;
+    base_localizer_listener.waitForTransform("/base_link", _localizer, ros::Time(0), ros::Duration(1.0));
+    base_localizer_listener.lookupTransform("/base_link", _localizer, ros::Time(0), m_base_to_localizer);
+
+    _tf_x = m_base_to_localizer.getOrigin().x();
+    _tf_y = m_base_to_localizer.getOrigin().y();
+    _tf_z = m_base_to_localizer.getOrigin().z();
+
+    tf::Quaternion b_l_q(
+      m_base_to_localizer.getRotation().x(),
+      m_base_to_localizer.getRotation().y(),
+      m_base_to_localizer.getRotation().z(),
+      m_base_to_localizer.getRotation().w()
+    );
+
+    tf::Matrix3x3 b_l_m(b_l_q);
+    b_l_m.getRPY(_tf_roll, _tf_pitch, _tf_yaw);
+  }
+  catch (tf::TransformException& ex)
+  {
+    ROS_ERROR("%s", ex.what());
+  }
+
+  std::cout << "-----------------------------------------------------------------" << std::endl;
+  std::cout << "Log file: " << filename << std::endl;
+  std::cout << "method_type: " << static_cast<int>(_method_type) << std::endl;
+  std::cout << "use_gnss: " << _use_gnss << std::endl;
+  std::cout << "queue_size: " << _queue_size << std::endl;
+  std::cout << "offset: " << _offset << std::endl;
+  std::cout << "get_height: " << _get_height << std::endl;
+  std::cout << "use_local_transform: " << _use_local_transform << std::endl;
+  std::cout << "use_odom: " << _use_odom << std::endl;
+  std::cout << "use_imu: " << _use_imu << std::endl;
+  std::cout << "imu_upside_down: " << _imu_upside_down << std::endl;
+  std::cout << "imu_topic: " << _imu_topic << std::endl;
+  std::cout << "localizer: " << _localizer << std::endl;
+  std::cout << "gnss_reinit_fitness: " << _gnss_reinit_fitness << std::endl;
+  std::cout << "(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (" << _tf_x << ", " << _tf_y << ", " << _tf_z << ", "
+            << _tf_roll << ", " << _tf_pitch << ", " << _tf_yaw << ")" << std::endl;
+  std::cout << "-----------------------------------------------------------------" << std::endl;
+
+#ifndef CUDA_FOUND
+  if (_method_type == MethodType::PCL_ANH_GPU)
+  {
+    std::cerr << "**************************************************************" << std::endl;
+    std::cerr << "[ERROR]PCL_ANH_GPU is not built. Please use other method type." << std::endl;
+    std::cerr << "**************************************************************" << std::endl;
+    exit(1);
+  }
+#endif
+#ifndef USE_PCL_OPENMP
+  if (_method_type == MethodType::PCL_OPENMP)
+  {
+    std::cerr << "**************************************************************" << std::endl;
+    std::cerr << "[ERROR]PCL_OPENMP is not built. Please use other method type." << std::endl;
+    std::cerr << "**************************************************************" << std::endl;
+    exit(1);
+  }
+#endif
+  // Scheduling & Profiling Setup
+  std::string node_name = ros::this_node::getName();
+  std::string task_response_time_filename;
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ndt_matching.csv");
+
+  int rate;
+  private_nh.param<int>(node_name+"/rate", rate, 10);
+
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
+
+  rubis::init_task_profiling(task_response_time_filename);
+
+  
+  Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
+  Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
+  Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+  Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+  tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+  // Updated in initialpose_callback or gnss_callback
+  initial_pose.x = 0.0;
+  initial_pose.y = 0.0;
+  initial_pose.z = 0.0;
+  initial_pose.roll = 0.0;
+  initial_pose.pitch = 0.0;
+  initial_pose.yaw = 0.0;
+
+  // Publishers
+  predict_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose", 10); 
+  predict_pose_imu_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu", 10);
+  predict_pose_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_odom", 10);
+  predict_pose_imu_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu_odom", 10);
+  is_kalman_filter_on_pub = nh.advertise<std_msgs::Bool>("/is_kalman_filter_on", 10);
+  kalman_filtered_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/kalman_filtered_pose", 10);
+
+  ndt_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 10);
+  rubis_pose_twist_pub = nh.advertise<rubis_msgs::PoseTwistStamped>("/rubis_current_pose_twist",10);
+  
+  localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 10);
+  // localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose", 10);
+  estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("/current_velocity", 10);
+  // estimate_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 10);
+  estimated_vel_mps_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_mps", 10);
+  estimated_vel_kmph_pub = nh.advertise<std_msgs::Float32>("/estimated_vel_kmph", 10);
+  estimated_vel_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/estimated_vel", 10);
+  time_ndt_matching_pub = nh.advertise<std_msgs::Float32>("/time_ndt_matching", 10);
+  ndt_stat_pub = nh.advertise<autoware_msgs::NDTStat>("/ndt_stat", 10);
+  ndt_reliability_pub = nh.advertise<std_msgs::Float32>("/ndt_reliability", 10);
+
+  // Subscribers
+  ros::Subscriber param_sub = nh.subscribe("config/ndt", 10, param_callback);
+  // ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback);
+  ros::Subscriber map_sub = nh.subscribe("points_map", 1, map_callback);
+  ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback); 
+
+  ros::Subscriber points_sub;
+  points_sub = nh.subscribe("rubis_filtered_points", 1, rubis_points_callback); // _queue_size = 1000
+  
+  // ros::Subscriber odom_sub = nh.subscribe("/vehicle/odom", _queue_size * 10, odom_callback);
+  // ros::Subscriber imu_sub = nh.subscribe(_imu_topic.c_str(), _queue_size * 10, imu_callback);
+
+  // ros::Subscriber ins_stat_sub = nh.subscribe("/ins_stat", 1, ins_stat_callback);
+  
+  pthread_t thread;
+  pthread_create(&thread, NULL, thread_func, NULL);
+
+  ros::Rate r(rate);
+  while(ros::ok()){
+    if(map_loaded == 1) break;
+    ros::spinOnce();
+    r.sleep();      
+  }
+  
+  map_sub.shutdown();
+
+  ros::spin();
+
+  return 0;
+}
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/CHANGELOG.rst b/autoware.ai/src/autoware/core_perception/ndt_gpu/CHANGELOG.rst
deleted file mode 100644
index 5c32e665..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/CHANGELOG.rst
+++ /dev/null
@@ -1,185 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package ndt_gpu
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.11.0 (2019-03-21)
--------------------
-* Fix license notice in corresponding package.xml
-* Contributors: amc-nu
-
-1.10.0 (2019-01-17)
--------------------
-
-1.9.1 (2018-11-06)
-------------------
-
-1.9.0 (2018-10-31)
-------------------
-
-1.8.0 (2018-08-31)
-------------------
-* [Feature] Cross compile for NVIDIA DriveWorks (`#1447 <https://github.com/CPFL/Autoware/pull/1447>`_)
-* Fix ndt not working correctly on drivePx2 and JetsonTX2
-* Fix ndt_matching/ndt_mapping on drivePX2 and JetsonTX2
-* Contributors: Esteve Fernandez, anhnv3991
-
-1.7.0 (2018-05-18)
-------------------
-* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
-* Removed unnecessary pakcage from ndt
-* Fix the bug ndt_mapping/matching use GPU resources even when selecting pcl_generic.
-* [fix] Fixes for all packages and dependencies (`#1240 <https://github.com/CPFL/Autoware/pull/1240>`_)
-  * Initial Cleanup
-  * fixed also for indigo
-  * kf cjeck
-  * Fix road wizard
-  * Added travis ci
-  * Trigger CI
-  * Fixes to cv_tracker and lidar_tracker cmake
-  * Fix kitti player dependencies
-  * Removed unnecessary dependencies
-  * messages fixing for can
-  * Update build script travis
-  * Travis Path
-  * Travis Paths fix
-  * Travis test
-  * Eigen checks
-  * removed unnecessary dependencies
-  * Eigen Detection
-  * Job number reduced
-  * Eigen3 more fixes
-  * More Eigen3
-  * Even more Eigen
-  * find package cmake modules included
-  * More fixes to cmake modules
-  * Removed non ros dependency
-  * Enable industrial_ci for indidog and kinetic
-  * Wrong install command
-  * fix rviz_plugin install
-  * FastVirtualScan fix
-  * Fix Qt5 Fastvirtualscan
-  * Fixed qt5 system dependencies for rosdep
-  * NDT TKU Fix catkin not pacakged
-  * More in detail dependencies fixes for more packages
-  * GLEW library for ORB
-  * Ignore OrbLocalizer
-  * Ignore Version checker
-  * Fix for driveworks interface
-  * driveworks not catkinpackagedd
-  * Missing catkin for driveworks
-  * libdpm opencv not catkin packaged
-  * catkin lib gnss  not included in obj_db
-  * Points2Polygon fix
-  * More missing dependencies
-  * image viewer not packaged
-  * Fixed SSH2 detection, added viewers for all distros
-  * Fix gnss localizer incorrect dependency config
-  * Fixes to multiple packages dependencies
-  * gnss plib and package
-  * More fixes to gnss
-  * gnss dependencies for gnss_loclaizer
-  * Missing gnss dependency for gnss on localizer
-  * More fixes for dependencies
-  Replaced gnss for autoware_gnss_library
-  * gnss more fixes
-  * fixes to more dependencies
-  * header dependency
-  * Debug message
-  * more debug messages changed back to gnss
-  * debud messages
-  * gnss test
-  * gnss install command
-  * Several fixes for OpenPlanner and its lbiraries
-  * Fixes to ROSInterface
-  * More fixes to robotsdk and rosinterface
-  * robotsdk calibration fix
-  * Fixes to rosinterface robotsdk libraries and its nodes
-  * Fixes to Qt5 missing dependencies in robotsdk
-  * glviewer missing dependencies
-  * Missing qt specific config cmake for robotsdk
-  * disable cv_tracker
-  * Fix to open planner un needed dependendecies
-  * Fixes for libraries indecision maker
-  * Fixes to libraries decision_maker installation
-  * Gazebo on Kinetic
-  * Added Missing library
-  * * Removed Gazebo and synchonization packages
-  * Renames vmap in lane_planner
-  * Added installation commands for missing pakcages
-  * Fixes to lane_planner
-  * Added NDT TKU Glut extra dependencies
-  * ndt localizer/lib fast pcl fixes
-  re enable cv_tracker
-  * Fix kf_lib
-  * Keep industrial_ci
-  * Fixes for dpm library
-  * Fusion lib fixed
-  * dpm and fusion header should match exported project name
-  * Fixes to dpm_ocv  ndt_localizer and pcl_omp
-  * no fast_pcl anymore
-  * fixes to libdpm and its package
-  * CI test
-  * test with native travis ci
-  * missing update for apt
-  * Fixes to pcl_omp installation and headers
-  * Final fixes for tests, modified README
-  * * Fixes to README
-  * Enable industrial_ci
-  * re enable native travis tests
-* Contributors: Abraham Monrroy, Kosuke Murakami, amc-nu, anhnv3991
-
-1.6.3 (2018-03-06)
-------------------
-
-1.6.2 (2018-02-27)
-------------------
-* Update CHANGELOG
-* Contributors: Yusuke FUJII
-
-1.6.1 (2018-01-20)
-------------------
-* update CHANGELOG
-* Contributors: Yusuke FUJII
-
-1.6.0 (2017-12-11)
-------------------
-* Prepare release for 1.6.0
-* adapted the version to the current version
-* GPU NormalDistributionsTransform optimization
-* fix memory error
-* remove inline functions
-* add ndt_gpu in fast_pcl library
-* Contributors: Yamato ANDO, anhnv-3991, yukikitsukawa
-
-1.5.1 (2017-09-25)
-------------------
-
-1.5.0 (2017-09-21)
-------------------
-
-1.4.0 (2017-08-04)
-------------------
-
-1.3.1 (2017-07-16)
-------------------
-
-1.3.0 (2017-07-14)
-------------------
-
-1.2.0 (2017-06-07)
-------------------
-
-1.1.2 (2017-02-27 23:10)
-------------------------
-
-1.1.1 (2017-02-27 22:25)
-------------------------
-
-1.1.0 (2017-02-24)
-------------------
-
-1.0.1 (2017-01-14)
-------------------
-
-1.0.0 (2016-12-22)
-------------------
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt
deleted file mode 100644
index 5f60e19d..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt
+++ /dev/null
@@ -1,109 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(ndt_gpu)
-
-find_package(autoware_build_flags REQUIRED)
-find_package(PCL REQUIRED)
-find_package(CUDA)
-find_package(catkin REQUIRED COMPONENTS rubis_lib)
-
-find_package(Eigen3 QUIET)
-
-if(NOT EIGEN3_FOUND)
-  # Fallback to cmake_modules
-  find_package(cmake_modules REQUIRED)
-  find_package(Eigen REQUIRED)
-  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
-  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
-  # Possibly map additional variables to the EIGEN3_ prefix.
-else()
-  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
-endif()
-
-AW_CHECK_CUDA()
-if(USE_CUDA)  
-  
-  set_directory_properties(PROPERTIES COMPILE_DEFINITIONS "")
-  
-  if(CMAKE_CROSSCOMPILING)
-    if(NOT CUDA_ARCH)
-      message(FATAL_ERROR "Please define the CUDA_ARCH CMake variable")
-    endif()
-  else()
-    if(NOT DEFINED CUDA_CAPABILITY_VERSION_CHECKER)
-      set(CUDA_CAPABILITY_VERSION_CHECKER
-        "${CATKIN_DEVEL_PREFIX}/lib/capability_version_checker"
-      )
-    endif()
-  
-    execute_process(COMMAND ${CUDA_CAPABILITY_VERSION_CHECKER}
-      OUTPUT_VARIABLE CUDA_CAPABILITY_VERSION
-      OUTPUT_STRIP_TRAILING_WHITESPACE
-    )
-  
-    if("${CUDA_CAPABILITY_VERSION}" MATCHES "^[1-9][0-9]+$")
-      set(CUDA_ARCH "sm_${CUDA_CAPABILITY_VERSION}")
-    else()
-      set(CUDA_ARCH "sm_52")
-    endif()
-  endif()
-  
-  set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-arch=${CUDA_ARCH};-std=c++11;--ptxas-options=-v)
-  
-  set(SUBSYS_DESC "Point cloud ndt gpu library")
-  
-  catkin_package(
-    DEPENDS PCL                                #Non-catkin CMake projects
-    INCLUDE_DIRS include                        #The exported include paths
-    LIBRARIES ndt_gpu                           #The exported libraries from the project
-  )
-  
-  include_directories(
-    include
-    ${PCL_INCLUDE_DIRS}
-    ${catkin_INCLUDE_DIRS}
-    ${CUDA_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIRS}
-  )
-  
-  set(srcs
-    src/MatrixDevice.cu
-    src/MatrixHost.cu
-    src/NormalDistributionsTransform.cu
-    src/Registration.cu
-    src/VoxelGrid.cu
-    src/SymmetricEigenSolver.cu
-  )
-  
-  set(incs
-    include/ndt_gpu/common.h
-    include/ndt_gpu/debug.h
-    include/ndt_gpu/Matrix.h
-    include/ndt_gpu/MatrixDevice.h
-    include/ndt_gpu/MatrixHost.h
-    include/ndt_gpu/NormalDistributionsTransform.h
-    include/ndt_gpu/Registration.h
-    include/ndt_gpu/SymmetricEigenSolver.h
-    include/ndt_gpu/VoxelGrid.h
-  )
-  
-  cuda_add_library(ndt_gpu ${srcs} ${incs})
-  
-  target_link_libraries(ndt_gpu
-    ${CUDA_LIBRARIES}
-    ${CUDA_CUBLAS_LIBRARIES}
-    ${CUDA_curand_LIBRARY}
-    ${PCL_LIBRARIES}
-    ${catkin_LIBRARIES}
-  )
-  
-  install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-    FILES_MATCHING PATTERN "*.h"
-  )
-  
-  install(TARGETS ndt_gpu
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-endif()
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt.origin b/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt.origin
deleted file mode 100644
index cc923973..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt.origin
+++ /dev/null
@@ -1,110 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(ndt_gpu)
-
-find_package(autoware_build_flags REQUIRED)
-find_package(catkin REQUIRED)
-find_package(PCL REQUIRED)
-find_package(CUDA)
-
-find_package(Eigen3 QUIET)
-
-if(NOT EIGEN3_FOUND)
-  # Fallback to cmake_modules
-  find_package(cmake_modules REQUIRED)
-  find_package(Eigen REQUIRED)
-  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
-  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
-  # Possibly map additional variables to the EIGEN3_ prefix.
-else()
-  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
-endif()
-
-AW_CHECK_CUDA()
-
-if(USE_CUDA)
-  set_directory_properties(PROPERTIES COMPILE_DEFINITIONS "")
-
-  if(CMAKE_CROSSCOMPILING)
-    if(NOT CUDA_ARCH)
-      message(FATAL_ERROR "Please define the CUDA_ARCH CMake variable")
-    endif()
-  else()
-    if(NOT DEFINED CUDA_CAPABILITY_VERSION_CHECKER)
-      set(CUDA_CAPABILITY_VERSION_CHECKER
-        "${CATKIN_DEVEL_PREFIX}/lib/capability_version_checker"
-      )
-    endif()
-
-    execute_process(COMMAND ${CUDA_CAPABILITY_VERSION_CHECKER}
-      OUTPUT_VARIABLE CUDA_CAPABILITY_VERSION
-      OUTPUT_STRIP_TRAILING_WHITESPACE
-    )
-
-    if("${CUDA_CAPABILITY_VERSION}" MATCHES "^[1-9][0-9]+$")
-      set(CUDA_ARCH "sm_${CUDA_CAPABILITY_VERSION}")
-    else()
-      set(CUDA_ARCH "sm_52")
-    endif()
-  endif()
-
-  set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-arch=${CUDA_ARCH};-std=c++11;--ptxas-options=-v)
-
-  set(SUBSYS_DESC "Point cloud ndt gpu library")
-
-  catkin_package(
-    DEPENDS PCL                                #Non-catkin CMake projects
-    INCLUDE_DIRS include                        #The exported include paths
-    LIBRARIES ndt_gpu                           #The exported libraries from the project
-  )
-
-  include_directories(
-    include
-    ${PCL_INCLUDE_DIRS}
-    ${catkin_INCLUDE_DIRS}
-    ${CUDA_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIRS}
-  )
-
-  set(srcs
-    src/MatrixDevice.cu
-    src/MatrixHost.cu
-    src/NormalDistributionsTransform.cu
-    src/Registration.cu
-    src/VoxelGrid.cu
-    src/SymmetricEigenSolver.cu
-  )
-
-  set(incs
-    include/ndt_gpu/common.h
-    include/ndt_gpu/debug.h
-    include/ndt_gpu/Matrix.h
-    include/ndt_gpu/MatrixDevice.h
-    include/ndt_gpu/MatrixHost.h
-    include/ndt_gpu/NormalDistributionsTransform.h
-    include/ndt_gpu/Registration.h
-    include/ndt_gpu/SymmetricEigenSolver.h
-    include/ndt_gpu/VoxelGrid.h
-  )
-
-  cuda_add_library(ndt_gpu ${srcs} ${incs})
-
-  target_link_libraries(ndt_gpu
-    ${CUDA_LIBRARIES}
-    ${CUDA_CUBLAS_LIBRARIES}
-    ${CUDA_curand_LIBRARY}
-    ${PCL_LIBRARIES}
-  )
-
-  install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-    FILES_MATCHING PATTERN "*.h"
-  )
-
-  install(TARGETS ndt_gpu
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-else()
-  message("ndt_gpu will not be built, CUDA was not found.")
-endif()
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Matrix.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Matrix.h
deleted file mode 100644
index 16e5033d..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Matrix.h
+++ /dev/null
@@ -1,255 +0,0 @@
-#ifndef GMATRIX_H_
-#define GMATRIX_H_
-
-#include <cuda.h>
-#include <cuda_runtime.h>
-#include "common.h"
-#include <float.h>
-
-namespace gpu {
-
-class Matrix {
-public:
-  CUDAH Matrix();
-
-  CUDAH Matrix(int rows, int cols, int offset, double *buffer);
-
-  CUDAH int rows() const;
-
-  CUDAH int cols() const;
-
-  CUDAH int offset() const;
-
-  CUDAH double *buffer() const;
-
-  CUDAH void setRows(int rows);
-  CUDAH void setCols(int cols);
-  CUDAH void setOffset(int offset);
-  CUDAH void setBuffer(double *buffer);
-  CUDAH void setCellVal(int row, int col, double val);
-
-  CUDAH void copy(Matrix &output);
-
-  //Need to fix. Only reducing rows is OK now.
-  CUDAH void resize(int rows, int cols);
-
-  CUDAH double *cellAddr(int row, int col);
-
-  CUDAH double *cellAddr(int index);
-
-  //Assignment operator
-  CUDAH void operator=(const Matrix input);
-
-  CUDAH double& operator()(int row, int col);
-
-  CUDAH void set(int row, int col, double val);
-
-  CUDAH double& operator()(int index);
-
-  CUDAH double at(int row, int col) const;
-
-  CUDAH bool operator*=(double val);
-
-  CUDAH bool operator/=(double val);
-
-  CUDAH bool transpose(Matrix &output);
-
-  //Only applicable for 3x3 matrix or below
-  CUDAH bool inverse(Matrix &output);
-
-  CUDAH Matrix col(int index);
-
-  CUDAH Matrix row(int index);
-
-protected:
-  double *buffer_;
-  int rows_, cols_, offset_;
-};
-
-
-CUDAH Matrix::Matrix() {
-  buffer_ = NULL;
-  rows_ = cols_ = offset_ = 0;
-}
-
-CUDAH Matrix::Matrix(int rows, int cols, int offset, double *buffer) {
-  rows_ = rows;
-  cols_ = cols;
-  offset_ = offset;
-  buffer_ = buffer;
-}
-
-CUDAH int Matrix::rows() const {
-  return rows_;
-}
-
-CUDAH int Matrix::cols() const {
-  return cols_;
-}
-
-CUDAH int Matrix::offset() const {
-  return offset_;
-}
-
-CUDAH double *Matrix::buffer() const {
-  return buffer_;
-}
-
-CUDAH void Matrix::setRows(int rows) { rows_ = rows; }
-CUDAH void Matrix::setCols(int cols) { cols_ = cols; }
-CUDAH void Matrix::setOffset(int offset) { offset_ = offset; }
-CUDAH void Matrix::setBuffer(double *buffer) { buffer_ = buffer; }
-CUDAH void Matrix::setCellVal(int row, int col, double val) {
-  buffer_[(row * cols_ + col) * offset_] = val;
-}
-
-CUDAH void Matrix::copy(Matrix &output) {
-  for (int i = 0; i < rows_; i++) {
-    for (int j = 0; j < cols_; j++) {
-      output(i, j) = buffer_[(i * cols_ + j) * offset_];
-    }
-  }
-}
-
-//Need to fix. Only reducing rows is OK now.
-CUDAH void Matrix::resize(int rows, int cols) {
-  rows_ = rows;
-  cols_ = cols;
-}
-
-CUDAH double *Matrix::cellAddr(int row, int col) {
-  if (row >= rows_ || col >= cols_ || row < 0 || col < 0)
-    return NULL;
-
-  return buffer_ + (row * cols_ + col) * offset_;
-}
-
-CUDAH double *Matrix::cellAddr(int index) {
-  if (rows_ == 1 && index >= 0 && index < cols_) {
-      return buffer_ + index * offset_;
-  }
-  else if (cols_ == 1 && index >= 0 && index < rows_) {
-      return buffer_ + index * offset_;
-  }
-
-  return NULL;
-}
-
-//Assignment operator
-CUDAH void Matrix::operator=(const Matrix input) {
-  rows_ = input.rows_;
-  cols_ = input.cols_;
-  offset_ = input.offset_;
-  buffer_ = input.buffer_;
-}
-
-CUDAH double& Matrix::operator()(int row, int col) {
-  return buffer_[(row * cols_ + col) * offset_];
-}
-
-CUDAH void Matrix::set(int row, int col, double val) {
-  buffer_[(row * cols_ + col) * offset_] = val;
-}
-
-CUDAH double& Matrix::operator()(int index) {
-  return buffer_[index * offset_];
-}
-
-CUDAH double Matrix::at(int row, int col) const {
-  return buffer_[(row * cols_ + col) * offset_];
-}
-
-CUDAH bool Matrix::operator*=(double val) {
-  for (int i = 0; i < rows_; i++) {
-    for (int j = 0; j < cols_; j++) {
-      buffer_[(i * cols_ + j) * offset_] *= val;
-    }
-  }
-
-  return true;
-}
-
-CUDAH bool Matrix::operator/=(double val) {
-  if (val == 0)
-    return false;
-
-  for (int i = 0; i < rows_ * cols_; i++) {
-      buffer_[i * offset_] /= val;
-  }
-
-  return true;
-}
-
-CUDAH bool Matrix::transpose(Matrix &output) {
-  if (rows_ != output.cols_ || cols_ != output.rows_)
-    return false;
-
-  for (int i = 0; i < rows_; i++) {
-    for (int j = 0; j < cols_; j++) {
-      output(j, i) = buffer_[(i * cols_ + j) * offset_];
-    }
-  }
-
-  return true;
-}
-
-//Only applicable for 3x3 matrix or below
-CUDAH bool Matrix::inverse(Matrix &output) {
-  if (rows_ != cols_ || rows_ == 0 || cols_ == 0)
-    return false;
-
-  if (rows_ == 1) {
-    if (buffer_[0] != 0)
-      output(0, 0) = 1 / buffer_[0];
-    else
-      return false;
-  }
-
-  if (rows_ == 2) {
-    double det = at(0, 0) * at(1, 1) - at(0, 1) * at(1, 0);
-
-    if (det != 0) {
-      output(0, 0) = at(1, 1) / det;
-      output(0, 1) = - at(0, 1) / det;
-
-      output(1, 0) = - at(1, 0) / det;
-      output(1, 1) = at(0, 0) / det;
-    } else
-      return false;
-  }
-
-  if (rows_ == 3) {
-    double det = at(0, 0) * at(1, 1) * at(2, 2) + at(0, 1) * at(1, 2) * at(2, 0) + at(1, 0) * at (2, 1) * at(0, 2)
-            - at(0, 2) * at(1, 1) * at(2, 0) - at(0, 1) * at(1, 0) * at(2, 2) - at(0, 0) * at(1, 2) * at(2, 1);
-    double idet = 1.0 / det;
-
-    if (det != 0) {
-      output(0, 0) = (at(1, 1) * at(2, 2) - at(1, 2) * at(2, 1)) * idet;
-      output(0, 1) = - (at(0, 1) * at(2, 2) - at(0, 2) * at(2, 1)) * idet;
-      output(0, 2) = (at(0, 1) * at(1, 2) - at(0, 2) * at(1, 1)) * idet;
-
-      output(1, 0) = - (at(1, 0) * at(2, 2) - at(1, 2) * at(2, 0)) * idet;
-      output(1, 1) = (at(0, 0) * at(2, 2) - at(0, 2) * at(2, 0)) * idet;
-      output(1, 2) = - (at(0, 0) * at(1, 2) - at(0, 2) * at(1, 0)) * idet;
-
-      output(2, 0) = (at(1, 0) * at(2, 1) - at(1, 1) * at(2, 0)) * idet;
-      output(2, 1) = - (at(0, 0) * at(2, 1) - at(0, 1) * at(2, 0)) * idet;
-      output(2, 2) = (at(0, 0) * at(1, 1) - at(0, 1) * at(1, 0)) * idet;
-    } else
-      return false;
-  }
-
-  return true;
-}
-
-CUDAH Matrix Matrix::col(int index) {
-  return Matrix(rows_, 1, offset_ * cols_, buffer_ + index * offset_);
-}
-
-CUDAH Matrix Matrix::row(int index) {
-  return Matrix(1, cols_, offset_, buffer_ + index * cols_ * offset_);
-}
-
-}
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixDevice.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixDevice.h
deleted file mode 100644
index f48cbf60..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixDevice.h
+++ /dev/null
@@ -1,83 +0,0 @@
-#ifndef MATRIX_DEVICE_H_
-#define MATRIX_DEVICE_H_
-
-#include "Matrix.h"
-
-namespace gpu {
-class MatrixDevice : public Matrix {
-public:
-  int memAllocId;
-  int memFreeId;
-
-  CUDAH MatrixDevice();
-
-  MatrixDevice(int rows, int cols);
-
-  CUDAH MatrixDevice(int rows, int cols, int offset, double *buffer);
-
-  CUDAH bool isEmpty();
-
-  CUDAH MatrixDevice col(int index);
-
-  CUDAH MatrixDevice row(int index);
-
-  CUDAH void setBuffer(double *buffer);
-
-  void memAlloc();
-
-  void memAlloc_free();
-  void memAlloc_malloc();
-  void memAlloc_memset();
-
-  void memFree();
-
-private:
-  bool fr_;
-};
-
-CUDAH MatrixDevice::MatrixDevice()
-{
-  rows_ = cols_ = offset_ = 0;
-  buffer_ = NULL;
-  fr_ = true;
-}
-
-CUDAH MatrixDevice::MatrixDevice(int rows, int cols, int offset, double *buffer)
-{
-  rows_ = rows;
-  cols_ = cols;
-  offset_ = offset;
-  buffer_ = buffer;
-  fr_ = false;
-}
-
-CUDAH bool MatrixDevice::isEmpty()
-{
-  return (rows_ == 0 || cols_ == 0 || buffer_ == NULL);
-}
-
-CUDAH MatrixDevice MatrixDevice::col(int index)
-{
-  return MatrixDevice(rows_, 1, offset_ * cols_, buffer_ + index * offset_);
-}
-
-CUDAH MatrixDevice MatrixDevice::row(int index)
-{
-  return MatrixDevice(1, cols_, offset_, buffer_ + index * cols_ * offset_);
-}
-
-CUDAH void MatrixDevice::setBuffer(double *buffer)
-{
-  buffer_ = buffer;
-}
-
-
-
-class SquareMatrixDevice : public MatrixDevice {
-public:
-  SquareMatrixDevice(int size);
-};
-
-}
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixHost.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixHost.h
deleted file mode 100644
index fb359a31..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixHost.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#ifndef MATRIX_HOST_H_
-#define MATRIX_HOST_H_
-
-#include "Matrix.h"
-#include "MatrixDevice.h"
-
-namespace gpu {
-class MatrixHost : public Matrix {
-public:
-  int moveToGpuId;
-  int moveToHostId;
-
-  MatrixHost();
-  MatrixHost(int rows, int cols);
-  MatrixHost(int rows, int cols, int offset, double *buffer);
-  MatrixHost(const MatrixHost& other);
-
-  bool moveToGpu(MatrixDevice output);
-  bool moveToHost(MatrixDevice input);
-
-  MatrixHost &operator=(const MatrixHost &other);
-
-  void debug();
-
-  ~MatrixHost();
-private:
-  bool fr_;
-};
-
-class SquareMatrixHost: public MatrixHost {
-public:
-  SquareMatrixHost(int size);
-};
-
-}
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/NormalDistributionsTransform.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/NormalDistributionsTransform.h
deleted file mode 100644
index 6b7ecf97..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/NormalDistributionsTransform.h
+++ /dev/null
@@ -1,112 +0,0 @@
-#ifndef GPU_NDT_H_
-#define GPU_NDT_H_
-
-#include <cuda.h>
-#include <cuda_runtime.h>
-#include "Registration.h"
-#include "common.h"
-#include "VoxelGrid.h"
-#include "Eigen/Geometry"
-#include "rubis_lib/sched.hpp"
-
-using namespace rubis;
-
-namespace gpu {
-
-class GNormalDistributionsTransform: public GRegistration {
-public:
-  GNormalDistributionsTransform();
-
-  GNormalDistributionsTransform(const GNormalDistributionsTransform &other);
-
-  void setStepSize(double step_size);
-
-  void setResolution(float resolution);
-
-  void setOutlierRatio(double olr);
-
-  double getStepSize() const;
-
-  float getResolution() const;
-
-  double getOutlierRatio() const;
-
-  double getTransformationProbability() const;
-
-  int getRealIterations();
-
-  /* Set the input map points */
-  void setInputTarget(pcl::PointCloud<pcl::PointXYZI>::Ptr input);
-  void setInputTarget(pcl::PointCloud<pcl::PointXYZ>::Ptr input);
-
-  /* Compute and get fitness score */
-  double getFitnessScore(double max_range = DBL_MAX);
-
-  ~GNormalDistributionsTransform();
-
-
-protected:
-  void computeTransformation(const Eigen::Matrix<float, 4, 4> &guess);
-  double computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
-                float *trans_x, float *trans_y, float *trans_z,
-                int points_num, Eigen::Matrix<double, 6, 1> pose, bool compute_hessian = true);
-
-private:
-  //Copied from ndt.h
-    double auxilaryFunction_PsiMT (double a, double f_a, double f_0, double g_0, double mu = 1.e-4);
-
-    //Copied from ndt.h
-    double auxilaryFunction_dPsiMT (double g_a, double g_0, double mu = 1.e-4);
-
-    double updateIntervalMT (double &a_l, double &f_l, double &g_l,
-                double &a_u, double &f_u, double &g_u,
-                double a_t, double f_t, double g_t);
-
-    double trialValueSelectionMT (double a_l, double f_l, double g_l,
-                  double a_u, double f_u, double g_u,
-                  double a_t, double f_t, double g_t);
-
-  void transformPointCloud(float *in_x, float *in_y, float *in_z,
-                float *out_x, float *out_y, float *out_z,
-                int points_number, Eigen::Matrix<float, 4, 4> transform);
-
-  void computeAngleDerivatives(MatrixHost pose, bool compute_hessian = true);
-
-  double computeStepLengthMT(const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir,
-                double step_init, double step_max, double step_min, double &score,
-                Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
-                float *out_x, float *out_y, float *out_z, int points_num);
-
-  void computeHessian(Eigen::Matrix<double, 6, 6> &hessian, float *trans_x, float *trans_y, float *trans_z, int points_num, Eigen::Matrix<double, 6, 1> &p);
-
-
-  double gauss_d1_, gauss_d2_;
-  double outlier_ratio_;
-  //MatrixHost j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_;
-  MatrixHost j_ang_;
-
-  //MatrixHost h_ang_a2_, h_ang_a3_, h_ang_b2_, h_ang_b3_, h_ang_c2_, h_ang_c3_, h_ang_d1_, h_ang_d2_, h_ang_d3_,
-  //      h_ang_e1_, h_ang_e2_, h_ang_e3_, h_ang_f1_, h_ang_f2_, h_ang_f3_;
-  MatrixHost h_ang_;
-
-
-  //MatrixDevice dj_ang_a_, dj_ang_b_, dj_ang_c_, dj_ang_d_, dj_ang_e_, dj_ang_f_, dj_ang_g_, dj_ang_h_;
-  MatrixDevice dj_ang_;
-
-
-  //MatrixDevice dh_ang_a2_, dh_ang_a3_, dh_ang_b2_, dh_ang_b3_, dh_ang_c2_, dh_ang_c3_, dh_ang_d1_, dh_ang_d2_, dh_ang_d3_,
-  //      dh_ang_e1_, dh_ang_e2_, dh_ang_e3_, dh_ang_f1_, dh_ang_f2_, dh_ang_f3_;
-  MatrixDevice dh_ang_;
-
-  double step_size_;
-  float resolution_;
-  double trans_probability_;
-
-  int real_iterations_;
-
-
-  GVoxelGrid voxel_grid_;
-};
-}
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Registration.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Registration.h
deleted file mode 100644
index 4eea6185..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Registration.h
+++ /dev/null
@@ -1,98 +0,0 @@
-#ifndef GNDT_H_
-#define GNDT_H_
-
-#include <cuda.h>
-#include <cuda_runtime.h>
-#include "Matrix.h"
-#include "MatrixHost.h"
-#include "MatrixDevice.h"
-#include "common.h"
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-#include <stdio.h>
-#include <rubis_lib/sched.hpp>
-
-#define HTOD 0
-#define DTOH 1
-#define LAUNCH 2
-#define GPU_PROFILING 1
-
-using namespace rubis;
-
-void start_profiling_execution_time();
-void start_profiling_response_time();
-void start_profiling_cpu_time();
-void stop_cpu_profiling();
-void stop_profiling(int id, int type);
-void write_profiling_data(const char* id, float e_time, float r_time, int type);
-void write_cpu_profiling_data(const char *id, long long int c_time);
-void write_dummy_line();
-void initialize_file(const char* execution_time_filename, const char* response_time_filename, const char* remain_time_filename);
-void close_file();
-
-namespace gpu {
-class GRegistration {
-public:
-  GRegistration();
-  GRegistration(const GRegistration &other);
-
-  void align(const Eigen::Matrix<float, 4, 4> &guess);
-
-  void setTransformationEpsilon(double trans_eps);
-
-  double getTransformationEpsilon() const;
-
-  void setMaximumIterations(int max_itr);
-
-  int getMaximumIterations() const;
-
-  Eigen::Matrix<float, 4, 4> getFinalTransformation() const;
-
-  /* Set input Scanned point cloud.
-   * Copy input points from the main memory to the GPU memory */
-  void setInputSource(pcl::PointCloud<pcl::PointXYZI>::Ptr input);
-  void setInputSource(pcl::PointCloud<pcl::PointXYZ>::Ptr input);
-
-  /* Set input reference map point cloud.
-   * Copy input points from the main memory to the GPU memory */
-  void setInputTarget(pcl::PointCloud<pcl::PointXYZI>::Ptr input);
-  void setInputTarget(pcl::PointCloud<pcl::PointXYZ>::Ptr input);
-
-  int getFinalNumIteration() const;
-
-  bool hasConverged() const;
-
-  virtual ~GRegistration();
-
-protected:
-
-  virtual void computeTransformation(const Eigen::Matrix<float, 4, 4> &guess);
-
-  double transformation_epsilon_;
-  int max_iterations_;
-
-  //Original scanned point clouds
-  float *x_, *y_, *z_;
-  int points_number_;
-
-  //Transformed point clouds
-  float *trans_x_, *trans_y_, *trans_z_;
-
-  bool converged_;
-  int nr_iterations_;
-
-  Eigen::Matrix<float, 4, 4> final_transformation_, transformation_, previous_transformation_;
-
-  bool target_cloud_updated_;
-
-  // Reference map point
-  float *target_x_, *target_y_, *target_z_;
-  int target_points_number_;
-
-  bool is_copied_;
-};
-}
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/SymmetricEigenSolver.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/SymmetricEigenSolver.h
deleted file mode 100644
index 2cf5faa3..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/SymmetricEigenSolver.h
+++ /dev/null
@@ -1,416 +0,0 @@
-#ifndef GSYMMETRIC_EIGEN_
-#define GSYMMETRIC_EIGEN_
-
-#include <cuda.h>
-#include <cuda_runtime.h>
-#include "MatrixDevice.h"
-#include <math.h>
-
-namespace gpu {
-
-class SymmetricEigensolver3x3 {
-public:
-  CUDAH SymmetricEigensolver3x3();
-
-  SymmetricEigensolver3x3(int offset);
-
-  CUDAH SymmetricEigensolver3x3(const SymmetricEigensolver3x3& other);
-
-  void setInputMatrices(double *input_matrices);
-
-  void setEigenvectors(double *eigenvectors);
-
-  void setEigenvalues(double *eigenvalues);
-
-  double *getBuffer() const;
-
-  /* Normalize input matrices by dividing each matrix
-   * to the element that has the largest absolute value
-   * in each matrix */
-  CUDAH void normalizeInput(int tid);
-
-  /* Compute eigenvalues */
-  CUDAH void computeEigenvalues(int tid);
-
-  /* First step to compute the eigenvector 0
-   * Because computing the eigenvector 0 using
-   * only one kernel is too expensive (which causes
-   * the "too many resources requested for launch" error,
-   * I have to divide them into two distinct kernels. */
-  CUDAH void computeEigenvector00(int tid);
-
-  /* Second step to compute the eigenvector 0 */
-  CUDAH void computeEigenvector01(int tid);
-
-  /* First step to compute the eigenvector 1 */
-  CUDAH void computeEigenvector10(int tid);
-
-  /* Second step to compute the eigenvector 1 */
-  CUDAH void computeEigenvector11(int tid);
-
-  /* Compute the final eigenvector by crossing
-   * eigenvector 0 and eigenvector 1 */
-  CUDAH void computeEigenvector2(int tid);
-
-  /* Final step to compute eigenvalues */
-  CUDAH void updateEigenvalues(int tid);
-
-  /* Free memory */
-  void memFree();
-
-private:
-  CUDAH void computeOrthogonalComplement(MatrixDevice w, MatrixDevice u, MatrixDevice v);
-
-  //Operators
-  CUDAH void multiply(MatrixDevice u, double mult, MatrixDevice output);
-
-  CUDAH void subtract(MatrixDevice u, MatrixDevice v, MatrixDevice output);
-
-  CUDAH void divide(MatrixDevice u, double div, MatrixDevice output);
-
-  CUDAH double dot(MatrixDevice u, MatrixDevice v);
-
-  CUDAH void cross(MatrixDevice in0, MatrixDevice in1, MatrixDevice out);
-
-  int offset_;
-
-  // Buffers for intermediate calculation
-  double *buffer_;
-  int *i02_;
-  double *maxAbsElement_;
-  double *norm_;
-
-  double *eigenvectors_;
-  double *eigenvalues_;
-  double *input_matrices_;
-
-  bool is_copied_;
-};
-
-
-CUDAH SymmetricEigensolver3x3::SymmetricEigensolver3x3()
-{
-    buffer_ = NULL;
-    eigenvectors_ = NULL;
-    eigenvalues_ = NULL;
-    input_matrices_ = NULL;
-    maxAbsElement_ = NULL;
-    norm_ = NULL;
-    i02_ = NULL;
-    offset_ = 0;
-    is_copied_ = false;
-}
-
-CUDAH SymmetricEigensolver3x3::SymmetricEigensolver3x3(const SymmetricEigensolver3x3& other)
-{
-  buffer_ = other.buffer_;
-  offset_ = other.offset_;
-  eigenvectors_ = other.eigenvectors_;
-  eigenvalues_ = other.eigenvalues_;
-  input_matrices_ = other.input_matrices_;
-
-  maxAbsElement_ = other.maxAbsElement_;
-  norm_ = other.norm_;
-  i02_ = other.i02_;
-  is_copied_ = true;
-}
-
-CUDAH void SymmetricEigensolver3x3::normalizeInput(int tid)
-{
-  MatrixDevice input(3, 3, offset_, input_matrices_ + tid);
-
-  double a00 = input(0, 0);
-  double a01 = input(0, 1);
-  double a02 = input(0, 2);
-  double a11 = input(1, 1);
-  double a12 = input(1, 2);
-  double a22 = input(2, 2);
-
-  double max0 = (fabs(a00) > fabs(a01)) ? fabs(a00) : fabs(a01);
-  double max1 = (fabs(a02) > fabs(a11)) ? fabs(a02) : fabs(a11);
-  double max2 = (fabs(a12) > fabs(a22)) ? fabs(a12) : fabs(a22);
-
-  double maxAbsElement = (max0 > max1) ? max0 : max1;
-
-  maxAbsElement = (maxAbsElement > max2) ? maxAbsElement : max2;
-
-  if (maxAbsElement == 0.0) {
-    MatrixDevice evec(3, 3, offset_, eigenvectors_ + tid);
-
-    evec(0, 0) = 1.0;
-    evec(1, 1) = 1.0;
-    evec(2, 2) = 1.0;
-
-    norm_[tid] = 0.0;
-    return;
-  }
-
-  double invMaxAbsElement = 1.0 / maxAbsElement;
-
-  a00 *= invMaxAbsElement;
-  a01 *= invMaxAbsElement;
-  a02 *= invMaxAbsElement;
-  a11 *= invMaxAbsElement;
-  a12 *= invMaxAbsElement;
-  a22 *= invMaxAbsElement;
-
-  input(0, 0) = a00;
-  input(0, 1) = a01;
-  input(0, 2) = a02;
-  input(1, 1) = a11;
-  input(1, 2) = a12;
-  input(2, 2) = a22;
-  input(1, 0) = a01;
-  input(2, 0) = a02;
-  input(2, 1) = a12;
-
-  norm_[tid] = a01 * a01 + a02 * a02 + a12 * a12;
-
-  maxAbsElement_[tid] = maxAbsElement;
-}
-
-CUDAH void SymmetricEigensolver3x3::computeEigenvalues(int tid)
-{
-  MatrixDevice input(3, 3, offset_, input_matrices_ + tid);
-  MatrixDevice eval(3, 1, offset_, eigenvalues_ + tid);
-
-  double a00 = input(0, 0);
-  double a01 = input(0, 1);
-  double a02 = input(0, 2);
-  double a11 = input(1, 1);
-  double a12 = input(1, 2);
-  double a22 = input(2, 2);
-
-  double norm = norm_[tid];
-
-  if (norm > 0.0) {
-    double traceDiv3 = (a00 + a11 + a22) / 3.0;
-    double b00 = a00 - traceDiv3;
-    double b11 = a11 - traceDiv3;
-    double b22 = a22 - traceDiv3;
-    double denom = sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0);
-    double c00 = b11 * b22 - a12 * a12;
-    double c01 = a01 * b22 - a12 * a02;
-    double c02 = a01 * a12 - b11 * a02;
-    double det = (b00 * c00 - a01 * c01 + a02 * c02) / (denom * denom * denom);
-    double halfDet = det * 0.5;
-
-    halfDet = (halfDet > -1.0) ? halfDet : -1.0;
-    halfDet = (halfDet < 1.0) ? halfDet : 1.0;
-
-    double angle = acos(halfDet) / 3.0;
-    double beta2 = cos(angle) * 2.0;
-    double beta0 = cos(angle + M_PI * 2.0 / 3.0) * 2.0;
-    double beta1 = -(beta0 + beta2);
-
-    eval(0) = traceDiv3 + denom * beta0;
-    eval(1) = traceDiv3 + denom * beta1;
-    eval(2) = traceDiv3 + denom * beta2;
-
-    i02_[tid] = (halfDet >= 0) ? 2 : 0;
-    i02_[tid + offset_] = (halfDet >= 0) ? 0 : 2;
-
-  } else {
-    eval(0) = a00;
-    eval(1) = a11;
-    eval(2) = a22;
-  }
-}
-
-CUDAH void SymmetricEigensolver3x3::updateEigenvalues(int tid)
-{
-  double maxAbsElement = maxAbsElement_[tid];
-  MatrixDevice eval(3, 1, offset_, eigenvalues_ + tid);
-
-  eval(0) *= maxAbsElement;
-  eval(1) *= maxAbsElement;
-  eval(2) *= maxAbsElement;
-}
-
-CUDAH void SymmetricEigensolver3x3::computeEigenvector00(int tid)
-{
-  if (norm_[tid] > 0.0) {
-    MatrixDevice input(3, 3, offset_, input_matrices_ + tid);
-    MatrixDevice row_mat(3, 3, offset_, buffer_ + tid);
-    double eval0 = eigenvalues_[tid + i02_[tid] * offset_];
-
-    input.copy(row_mat);
-
-    row_mat(0, 0) -= eval0;
-    row_mat(1, 1) -= eval0;
-    row_mat(2, 2) -= eval0;
-
-    //row0 is r0xr1, row1 is r0xr2, row2 is r1xr2
-    MatrixDevice rxr(3, 3, offset_, buffer_ + 3 * 3 * offset_ + tid);
-
-    cross(row_mat.row(0), row_mat.row(1), rxr.row(0));
-    cross(row_mat.row(0), row_mat.row(2), rxr.row(1));
-    cross(row_mat.row(1), row_mat.row(2), rxr.row(2));
-
-  } else {
-    eigenvectors_[tid] = 1.0;
-  }
-}
-
-CUDAH void SymmetricEigensolver3x3::computeEigenvector01(int tid)
-{
-  if (norm_[tid] > 0.0) {
-    MatrixDevice evec0(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid] * offset_);
-
-    //row0 is r0xr1, row1 is r0xr2, row2 is r1xr2
-    MatrixDevice rxr(3, 3, offset_, buffer_ + 3 * 3 * offset_ + tid);
-
-
-    double d0 = rxr(0, 0) * rxr(0, 0) + rxr(0, 1) * rxr(0, 1) * rxr(0, 2) * rxr(0, 2);
-    double d1 = rxr(1, 0) * rxr(1, 0) + rxr(1, 1) * rxr(1, 1) * rxr(1, 2) * rxr(1, 2);
-    double d2 = rxr(2, 0) * rxr(2, 0) + rxr(2, 1) * rxr(2, 1) * rxr(2, 2) * rxr(2, 2);
-
-    double dmax = (d0 > d1) ? d0 : d1;
-    int imax = (d0 > d1) ? 0 : 1;
-
-    dmax = (d2 > dmax) ? d2 : dmax;
-    imax = (d2 > dmax) ? 2 : imax;
-
-    divide(rxr.row(imax), sqrt(dmax), evec0);
-  }
-}
-
-CUDAH void SymmetricEigensolver3x3::computeEigenvector10(int tid)
-{
-  if (norm_[tid] > 0.0) {
-    MatrixDevice input(3, 3, offset_, input_matrices_ + tid);
-    MatrixDevice evec0(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid] * offset_);
-    double eval1 = eigenvalues_[tid + offset_];
-
-    MatrixDevice u(3, 1, offset_, buffer_ + tid);
-    MatrixDevice v(3, 1, offset_, buffer_ + 3 * offset_ + tid);
-
-    computeOrthogonalComplement(evec0, u, v);
-
-    MatrixDevice au(3, 1, offset_, buffer_ + 6 * offset_ + tid);
-    MatrixDevice av(3, 1, offset_, buffer_ + 9 * offset_ + tid);
-
-    double t0, t1, t2;
-
-    t0 = u(0);
-    t1 = u(1);
-    t2 = u(2);
-
-    au(0) = (input(0, 0) - eval1) * t0 + input(0, 1) * t1 + input(0, 2) * t2;
-    au(1) = input(0, 1) * t0 + (input(1, 1) - eval1) * t1 + input(1, 2) * t2;
-    au(2) = input(0, 2) * t0 + input(1, 2) * t1 + (input(2, 2) - eval1) * t2;
-
-    t0 = v(0);
-    t1 = v(1);
-    t2 = v(2);
-
-    av(0) = (input(0, 0) - eval1) * t0 + input(0, 1) * t1 + input(0, 2) * t2;
-    av(1) = input(0, 1) * t0 + (input(1, 1) - eval1) * t1 + input(1, 2) * t2;
-    av(2) = input(0, 2) * t0 + input(1, 2) * t1 + (input(2, 2) - eval1) * t2;
-  } else {
-    eigenvectors_[tid + offset_ * 4] = 1.0;
-  }
-}
-
-CUDAH void SymmetricEigensolver3x3::computeEigenvector11(int tid)
-{
-  if (norm_[tid] > 0.0) {
-    MatrixDevice evec1(3, 1, offset_ * 3, eigenvectors_ + tid + offset_);
-
-    MatrixDevice u(3, 1, offset_, buffer_ + tid);
-    MatrixDevice v(3, 1, offset_, buffer_ + 3 * offset_ + tid);
-
-    MatrixDevice au(3, 1, offset_, buffer_ + 6 * offset_ + tid);
-    MatrixDevice av(3, 1, offset_, buffer_ + 9 * offset_ + tid);
-
-    double m00 = u(0) * au(0) + u(1) * au(1) + u(2) * au(2);
-    double m01 = u(0) * av(0) + u(1) * av(1) + u(2) * av(2);
-    double m11 = v(0) * av(0) + v(1) * av(1) + v(2) * av(2);
-
-    double abs_m00 = fabs(m00);
-    double abs_m01 = fabs(m01);
-    double abs_m11 = fabs(m11);
-
-    if (abs_m00 > 0 || abs_m01 > 0 || abs_m11 > 0) {
-      double u_mult = (abs_m00 >= abs_m11) ? m01 : m11;
-      double v_mult = (abs_m00 >= abs_m11) ? m00 : m01;
-
-      bool res = fabs(u_mult) >= fabs(v_mult);
-      double *large = (res) ? &u_mult : &v_mult;
-      double *small = (res) ? &v_mult : &u_mult;
-
-      *small /= (*large);
-      *large = 1.0 / sqrt(1.0 + (*small) * (*small));
-      *small *= (*large);
-
-      multiply(u, u_mult, u);
-      multiply(v, v_mult, v);
-      subtract(u, v, evec1);
-
-    } else {
-      u.copy(evec1);
-    }
-  }
-}
-
-CUDAH void SymmetricEigensolver3x3::multiply(MatrixDevice u, double mult, MatrixDevice output)
-{
-  output(0) = u(0) * mult;
-  output(1) = u(1) * mult;
-  output(2) = u(2) * mult;
-}
-
-CUDAH void SymmetricEigensolver3x3::subtract(MatrixDevice u, MatrixDevice v, MatrixDevice output)
-{
-  output(0) = u(0) - v(0);
-  output(1) = u(1) - v(1);
-  output(2) = u(2) - v(2);
-}
-
-CUDAH void SymmetricEigensolver3x3::divide(MatrixDevice u, double div, MatrixDevice output)
-{
-  output(0) = u(0) / div;
-  output(1) = u(1) / div;
-  output(2) = u(2) / div;
-}
-
-CUDAH double SymmetricEigensolver3x3::dot(MatrixDevice u, MatrixDevice v)
-{
-  return (u(0) * v(0) + u(1) * v(1) + u(2) * v(2));
-}
-
-CUDAH void SymmetricEigensolver3x3::cross(MatrixDevice u, MatrixDevice v, MatrixDevice out)
-{
-  out(0) = u(1) * v(2) - u(2) * v(1);
-  out(1) = u(2) * v(0) - u(0) * v(2);
-  out(2) = u(0) * v(1) - u(1) * v(0);
-}
-
-CUDAH void SymmetricEigensolver3x3::computeOrthogonalComplement(MatrixDevice w, MatrixDevice u, MatrixDevice v)
-{
-  bool c = (fabs(w(0)) > fabs(w(1)));
-
-  double inv_length = (c) ? (1.0 / sqrt(w(0) * w(0) + w(2) * w(2))) : (1.0 / sqrt(w(1) * w(1) + w(2) * w(2)));
-
-  u(0) = (c) ? -w(2) * inv_length : 0.0;
-  u(1) = (c) ? 0.0 : w(2) * inv_length;
-  u(2) = (c) ? w(0) * inv_length : -w(1) * inv_length;
-
-  cross(w, u, v);
-}
-
-CUDAH void SymmetricEigensolver3x3::computeEigenvector2(int tid)
-{
-  if (norm_[tid] > 0.0) {
-    MatrixDevice evec0(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid] * offset_);
-    MatrixDevice evec1(3, 1, offset_ * 3, eigenvectors_ + tid + offset_);
-    MatrixDevice evec2(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid + offset_] * offset_);
-
-    cross(evec0, evec1, evec2);
-  } else {
-    eigenvectors_[tid + offset_ * 8] = 1.0;
-  }
-}
-}
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/VoxelGrid.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/VoxelGrid.h
deleted file mode 100644
index a273b8d4..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/VoxelGrid.h
+++ /dev/null
@@ -1,163 +0,0 @@
-#ifndef GPU_OCTREE_H_
-#define GPU_OCTREE_H_
-
-#include <cuda.h>
-#include <cuda_runtime.h>
-#include <device_launch_parameters.h>
-#include "common.h"
-#include "MatrixDevice.h"
-#include "MatrixHost.h"
-#include <float.h>
-#include <vector>
-
-namespace gpu {
-class GVoxelGrid {
-public:
-  GVoxelGrid();
-
-  GVoxelGrid(const GVoxelGrid &other);
-
-  /* Set input points */
-  void setInput(float *x, float *y, float *z, int points_num);
-
-  void setMinVoxelSize(int size);
-
-  /* For each input point, search for voxels whose distance between their centroids and
-   * the input point are less than radius.
-   * Results of the search are stored into valid_points, starting_voxel_id, and voxel_id.
-   * Valid points: the function return one or more voxels for these points. Other points
-   * are considered as invalid.
-   * Valid voxels: voxels returned from the search.
-   * The number of valid points is stored in valid_points_num.
-   * The total number of valid voxels. If the query of both point A and point B return
-   * voxel X, then the number of valid voxels is 2, not 1. */
-  void radiusSearch(float *qx, float *qy, float *qz, int points_num, float radius, int max_nn,
-                      int **valid_points, int **starting_voxel_id, int **voxel_id,
-                      int *valid_voxel_num, int *valid_points_num);
-
-  int getVoxelNum() const;
-
-  float getMaxX() const;
-  float getMaxY() const;
-  float getMaxZ() const;
-
-  float getMinX() const;
-  float getMinY() const;
-  float getMinZ() const;
-
-  float getVoxelX() const;
-  float getVoxelY() const;
-  float getVoxelZ() const;
-
-  int getMaxBX() const;
-  int getMaxBY() const;
-  int getMaxBZ() const;
-
-  int getMinBX() const;
-  int getMinBY() const;
-  int getMinBZ() const;
-
-  int getVgridX() const;
-  int getVgridY() const;
-  int getVgridZ() const;
-
-  void setLeafSize(float voxel_x, float voxel_y, float voxel_z);
-
-  /* Get the centroid list. */
-  double *getCentroidList() const;
-
-  /* Get the covariance list. */
-  double *getCovarianceList() const;
-
-  /* Get the pointer to the inverse covariances list. */
-  double *getInverseCovarianceList() const;
-
-  int *getPointsPerVoxelList() const;
-
-  /* Searching for the nearest point of each input query point.
-   * Coordinates of query points are input by trans_x, trans_y, and trans_z.
-   * The ith element of valid_distance array is 1 if the distance between
-   * the ith input point and its nearest neighbor is less than or equal
-   * to max_range. Otherwise, it is 0.
-   * The ith element of min_distance array stores the distance between
-   * the corresponding input point and its nearest neighbor. It is 0 if
-   * the distance is larger than max_range. */
-  void nearestNeighborSearch(float *trans_x, float *trans_y, float *trans_z, int point_num, int *valid_distance, double *min_distance, float max_range);
-
-  ~GVoxelGrid();
-private:
-
-  /* Construct the voxel grid and the build the octree. */
-  void initialize();
-
-  /* Compute centroids and covariances of voxels. */
-  void computeCentroidAndCovariance();
-
-  /* Find boundaries of input point cloud and compute
-   * the number of necessary voxels as well as boundaries
-   * measured in number of leaf size */
-  void findBoundaries();
-
-  /* Put points into voxels */
-  void scatterPointsToVoxelGrid();
-
-  /* Build octrees for nearest neighbor search.
-   * Only used for searching one nearest neighbor point.
-   * Cannot used for searching multiple nearest neighbors. */
-  void buildOctree();
-
-
-  /* A wrapper for exclusive scan using thrust.
-   * Output sum of all elements is stored at sum. */
-  template <typename T = int>
-  void ExclusiveScan(T *input, int ele_num, T *sum);
-
-  /* A wrapper for exclusive scan using thrust.
-   * Output sum is not required. */
-  template <typename T = int>
-  void ExclusiveScan(T *input, int ele_num);
-
-  /* Size of the octree in each level,
-   * measured in number of tree nodes. */
-  typedef struct _OctreeGridSize {
-    int size_x;
-    int size_y;
-    int size_z;
-  } OctreeGridSize;
-
-  //Coordinate of input points
-  float *x_, *y_, *z_;
-  int points_num_;
-  double *centroid_;         // List of 3x1 double vector
-  double *covariance_;      // List of 3x3 double matrix
-  double *inverse_covariance_;  // List of 3x3 double matrix
-  int *points_per_voxel_;
-
-  int voxel_num_;            // Number of voxels
-  float max_x_, max_y_, max_z_;    // Upper bounds of the grid (maximum coordinate)
-  float min_x_, min_y_, min_z_;    // Lower bounds of the grid (minimum coordinate)
-  float voxel_x_, voxel_y_, voxel_z_;  // Leaf size, a.k.a, size of each voxel
-
-  int max_b_x_, max_b_y_, max_b_z_;  // Upper bounds of the grid, measured in number of voxels
-  int min_b_x_, min_b_y_, min_b_z_;  // Lower bounds of the grid, measured in number of voxels
-  int vgrid_x_, vgrid_y_, vgrid_z_;  // Size of the voxel grid, measured in number of voxels
-  int min_points_per_voxel_;
-
-  int *starting_point_ids_;
-  int *point_ids_;
-
-  /* Centroids of octree nodes. Each element stores a list
-   * of 3x1 matrices containing centroids of octree nodes. */
-  std::vector<double*> octree_centroids_;
-
-  /* The number of points per octree node per level. */
-  std::vector<int*> octree_points_per_node_;
-
-  /* The number of octree nodes per level in three dimensions. */
-  std::vector<OctreeGridSize> octree_grid_size_;
-
-  bool is_copied_;
-};
-}
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/common.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/common.h
deleted file mode 100644
index 2d14d2c4..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/common.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef GPU_COMMON_H_
-#define GPU_COMMON_H_
-
-#include <cuda.h>
-#include <cuda_runtime.h>
-
-#define CUDAH __forceinline__ __host__ __device__
-#define BLOCK_SIZE_X 1024
-#define BLOCK_SIZE_X2 512
-#define BLOCK_SIZE_X3 256
-
-#define BLOCK_X 16
-#define BLOCK_Y 16
-#define BLOCK_Z 4
-
-#define SHARED_MEM_SIZE 3072
-#endif
-
-//  This is the temploary patch for CUDA9 build problem 
-#if ( __CUDACC_VER_MAJOR__ >=9 )
-#undef  __CUDACC_VER__
-#define __CUDACC_VER__ 90000 
-#endif
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/debug.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/debug.h
deleted file mode 100644
index bdc41dd2..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/debug.h
+++ /dev/null
@@ -1,24 +0,0 @@
-#ifndef GDEBUG_H_
-#define GDEBUG_H_
-
-#include <cuda.h>
-#include <cuda_runtime.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <sys/time.h>
-#include <time.h>
-
-inline void gassert(cudaError_t err_code, const char *file, int line)
-{
-  if (err_code != cudaSuccess) {
-    fprintf(stderr, "Error: %s %s %d\n", cudaGetErrorString(err_code), file, line);
-    cudaDeviceReset();
-    exit(EXIT_FAILURE);
-  }
-}
-
-#define checkCudaErrors(err_code) gassert(err_code, __FILE__, __LINE__)
-
-#define timeDiff(start, end) ((end.tv_sec - start.tv_sec) * 1000000 + end.tv_usec - start.tv_usec)
-
-#endif
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/package.xml b/autoware.ai/src/autoware/core_perception/ndt_gpu/package.xml
deleted file mode 100644
index 3f5cad32..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/package.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>ndt_gpu</name>
-  <version>1.12.0</version>
-  <description>The ndt_gpu package</description>
-  <maintainer email="yuki@ertl.jp">Yuki Kitsukawa</maintainer>
-  <maintainer email="anh@ertl.jp">Anh Viet Nguyen</maintainer>
-  <license>Apache 2</license>
-
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>libpcl-all-dev</build_depend>
-
-  <exec_depend>libpcl-all</exec_depend>
-  <depend>rubis_lib</depend>
-</package>
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixDevice.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixDevice.cu
deleted file mode 100644
index 6e84a466..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixDevice.cu
+++ /dev/null
@@ -1,82 +0,0 @@
-#include "ndt_gpu/MatrixDevice.h"
-#include "ndt_gpu/debug.h"
-#include "rubis_lib/sched.hpp"
-
-namespace gpu {
-MatrixDevice::MatrixDevice(int rows, int cols) {
-	rows_ = rows;
-	cols_ = cols;
-	offset_ = 1;
-	fr_ = true;
-	buffer_ = NULL;
-	memAllocId = 0;
-	memFreeId = 0;
-}
-
-void MatrixDevice::memAlloc()
-{
-	if (buffer_ != NULL && fr_) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(buffer_));
-		rubis::sched::yield_gpu("1_free");
-		buffer_ = NULL;
-	}
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&buffer_, sizeof(double) * rows_ * cols_ * offset_));
-	rubis::sched::yield_gpu("2_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(buffer_, 0, sizeof(double) * rows_ * cols_ * offset_));
-	rubis::sched::yield_gpu("3_cudaMemset");
-
-	checkCudaErrors(cudaDeviceSynchronize());
-	fr_ = true;
-}
-
-void MatrixDevice::memAlloc_free()
-{
-	if (buffer_ != NULL && fr_) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(buffer_));
-		rubis::sched::yield_gpu("4_free");
-		buffer_ = NULL;
-	}
-}
-
-void MatrixDevice::memAlloc_malloc()
-{
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&buffer_, sizeof(double) * rows_ * cols_ * offset_));
-	rubis::sched::yield_gpu("5_cudaMalloc");
-}
-
-void MatrixDevice::memAlloc_memset()
-{
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(buffer_, 0, sizeof(double) * rows_ * cols_ * offset_));
-	rubis::sched::yield_gpu("6_cudaMemset");
-
-	fr_ = true;
-}
-
-void MatrixDevice::memFree()
-{
-	if (fr_) {
-		if (buffer_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(buffer_));
-			rubis::sched::yield_gpu("7_free");
-			buffer_ = NULL;
-		}
-	}
-}
-
-
-SquareMatrixDevice::SquareMatrixDevice(int size) :
-	MatrixDevice(size, size)
-{
-
-}
-
-}
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixHost.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixHost.cu
deleted file mode 100644
index 65eaefac..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixHost.cu
+++ /dev/null
@@ -1,181 +0,0 @@
-#include "ndt_gpu/MatrixHost.h"
-#include "ndt_gpu/debug.h"
-#include <iostream>
-#include <stdio.h>
-#include <stdlib.h>
-#include "rubis_lib/sched.hpp"
-
-namespace gpu {
-
-MatrixHost::MatrixHost()
-{
-	fr_ = false;
-}
-
-MatrixHost::MatrixHost(int rows, int cols) {
-	rows_ = rows;
-	cols_ = cols;
-	offset_ = 1;
-
-	buffer_ = (double*)malloc(sizeof(double) * rows_ * cols_ * offset_);
-	memset(buffer_, 0, sizeof(double) * rows_ * cols_ * offset_);
-	fr_ = true;
-}
-
-MatrixHost::MatrixHost(int rows, int cols, int offset, double *buffer)
-{
-	rows_ = rows;
-	cols_ = cols;
-	offset_ = offset;
-	buffer_ = buffer;
-	fr_ = false;
-}
-
-MatrixHost::MatrixHost(const MatrixHost& other) {
-	rows_ = other.rows_;
-	cols_ = other.cols_;
-	offset_ = other.offset_;
-	fr_ = other.fr_;
-
-	if (fr_) {
-		buffer_ = (double*)malloc(sizeof(double) * rows_ * cols_ * offset_);
-		memcpy(buffer_, other.buffer_, sizeof(double) * rows_ * cols_ * offset_);
-	} else {
-		buffer_ = other.buffer_;
-	}
-}
-
-extern "C" __global__ void copyMatrixDevToDev(MatrixDevice input, MatrixDevice output) {
-	int row = threadIdx.x;
-	int col = threadIdx.y;
-	int rows_num = input.rows();
-	int cols_num = input.cols();
-
-	if (row < rows_num && col < cols_num)
-		output(row, col) = input(row, col);
-}
-
-bool MatrixHost::moveToGpu(MatrixDevice output) {
-	if (rows_ != output.rows() || cols_ != output.cols())
-		return false;
-
-	if (offset_ == output.offset()) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(output.buffer(), buffer_, sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyHostToDevice));
-		rubis::sched::yield_gpu("8_htod");
-		return true;
-	}
-	else {
-		double *tmp;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&tmp, sizeof(double) * rows_ * cols_ * offset_));
-		rubis::sched::yield_gpu("9_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(tmp, buffer_, sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyHostToDevice));
-		rubis::sched::yield_gpu("10_htod");
-
-		MatrixDevice tmp_output(rows_, cols_, offset_, tmp);
-
-		dim3 block_x(rows_, cols_, 1);
-		dim3 grid_x(1, 1, 1);
-
-		rubis::sched::request_gpu();
-		copyMatrixDevToDev<<<grid_x, block_x>>>(tmp_output, output);
-		rubis::sched::yield_gpu("11_copyMatrixDevToDev");
-
-		checkCudaErrors(cudaDeviceSynchronize());
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(tmp));
-		rubis::sched::yield_gpu("12_free");
-
-		return true;
-	}
-}
-
-bool MatrixHost::moveToHost(MatrixDevice input) {
-	if (rows_ != input.rows() || cols_ != input.cols())
-		return false;
-
-	if (offset_ == input.offset()) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(buffer_, input.buffer(), sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyDeviceToHost));
-		rubis::sched::yield_gpu("13_dtoh");
-		return true;
-	}
-	else {
-		double *tmp;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&tmp, sizeof(double) * rows_ * cols_ * offset_));
-		rubis::sched::yield_gpu("14_cudaMalloc");
-
-		MatrixDevice tmp_output(rows_, cols_, offset_, tmp);
-
-		dim3 block_x(rows_, cols_, 1);
-		dim3 grid_x(1, 1, 1);
-
-		rubis::sched::request_gpu();
-		copyMatrixDevToDev << <grid_x, block_x >> >(input, tmp_output);
-		rubis::sched::yield_gpu("15_copyMatrixDevToDev");
-
-		checkCudaErrors(cudaDeviceSynchronize());
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(buffer_, tmp, sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyDeviceToHost));
-		rubis::sched::yield_gpu("16_dtoh");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(tmp));
-		rubis::sched::yield_gpu("17_free");
-
-		return true;
-	}
-}
-
-MatrixHost &MatrixHost::operator=(const MatrixHost &other)
-{
-	rows_ = other.rows_;
-	cols_ = other.cols_;
-	offset_ = other.offset_;
-	fr_ = other.fr_;
-
-	if (fr_) {
-		buffer_ = (double*)malloc(sizeof(double) * rows_ * cols_ * offset_);
-		memcpy(buffer_, other.buffer_, sizeof(double) * rows_ * cols_ * offset_);
-	} else {
-		buffer_ = other.buffer_;
-	}
-
-	return *this;
-}
-
-void MatrixHost::debug()
-{
-	for (int i = 0; i < rows_; i++) {
-		for (int j = 0; j < cols_; j++) {
-			std::cout << buffer_[(i * cols_ + j) * offset_] << " ";
-		}
-
-		std::cout << std::endl;
-	}
-
-	std::cout << std::endl;
-}
-
-MatrixHost::~MatrixHost()
-{
-	if (fr_)
-		free(buffer_);
-}
-
-
-SquareMatrixHost::SquareMatrixHost(int size) :
-	 MatrixHost(size, size)
-{
-
-}
-
-}
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/NormalDistributionsTransform.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/NormalDistributionsTransform.cu
deleted file mode 100644
index 005268d4..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/NormalDistributionsTransform.cu
+++ /dev/null
@@ -1,1918 +0,0 @@
-#include "ndt_gpu/NormalDistributionsTransform.h"
-#include "ndt_gpu/debug.h"
-#include <cmath>
-#include <iostream>
-#include <pcl/common/transforms.h>
-#include "rubis_lib/sched.hpp"
-
-using namespace rubis::sched;
-
-namespace gpu {
-
-GNormalDistributionsTransform::GNormalDistributionsTransform()
-{
-	//GRegistration::GRegistration();
-
-	gauss_d1_ = gauss_d2_ = 0;
-	outlier_ratio_ = 0.55;
-	step_size_ = 0.1;
-	resolution_ = 1.0f;
-	trans_probability_ = 0;
-
-	double gauss_c1, gauss_c2, gauss_d3;
-
-	// Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
-	gauss_c1 = 10.0 * (1 - outlier_ratio_);
-	gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
-	gauss_d3 = -log (gauss_c2);
-	gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3;
-	gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_);
-
-	transformation_epsilon_ = 0.1;
-	max_iterations_ = 35;
-
-	j_ang_ = MatrixHost(24, 1);
-
-	h_ang_ = MatrixHost(45, 1);
-
-	dj_ang_ = MatrixDevice(24, 1);
-
-	dh_ang_ = MatrixDevice(45, 1);
-
-	real_iterations_ = 0;
-
-}
-
-GNormalDistributionsTransform::GNormalDistributionsTransform(const GNormalDistributionsTransform &other)
-{
-	gauss_d1_ = other.gauss_d1_;
-	gauss_d2_ = other.gauss_d2_;
-
-	outlier_ratio_ = other.outlier_ratio_;
-
-	j_ang_ = other.j_ang_;
-	h_ang_ = other.h_ang_;
-	dj_ang_ = other.dj_ang_;
-	dh_ang_ = other.dh_ang_;
-
-	step_size_ = other.step_size_;
-	resolution_ = other.resolution_;
-	trans_probability_ = other.trans_probability_;
-	real_iterations_ = other.real_iterations_;
-
-	voxel_grid_ = other.voxel_grid_;
-
-}
-
-GNormalDistributionsTransform::~GNormalDistributionsTransform()
-{
-	dj_ang_.memFree();
-	dh_ang_.memFree();
-
-}
-
-void GNormalDistributionsTransform::setStepSize(double step_size)
-{
-	step_size_ = step_size;
-}
-
-void GNormalDistributionsTransform::setResolution(float resolution)
-{
-	resolution_ = resolution;
-}
-
-void GNormalDistributionsTransform::setOutlierRatio(double olr)
-{
-	outlier_ratio_ = olr;
-}
-
-double GNormalDistributionsTransform::getStepSize() const
-{
-	return step_size_;
-}
-
-float GNormalDistributionsTransform::getResolution() const
-{
-	return resolution_;
-}
-
-double GNormalDistributionsTransform::getOutlierRatio() const
-{
-	return outlier_ratio_;
-}
-
-double GNormalDistributionsTransform::getTransformationProbability() const
-{
-	return trans_probability_;
-}
-
-int GNormalDistributionsTransform::getRealIterations()
-{
-	 return real_iterations_;
-}
-
-double GNormalDistributionsTransform::auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu)
-{
-  return (f_a - f_0 - mu * g_0 * a);
-}
-
-double GNormalDistributionsTransform::auxilaryFunction_dPsiMT(double g_a, double g_0, double mu)
-{
-  return (g_a - mu * g_0);
-}
-
-void GNormalDistributionsTransform::setInputTarget(pcl::PointCloud<pcl::PointXYZI>::Ptr input)
-{
-	// Copy input map data from the host memory to the GPU memory
-	GRegistration::setInputTarget(input);
-
-	// Build the voxel grid
-	if (target_points_number_ != 0) {
-		voxel_grid_.setLeafSize(resolution_, resolution_, resolution_);
-		voxel_grid_.setInput(target_x_, target_y_, target_z_, target_points_number_);
-	}
-	printf("complete set Input Target\n");
-}
-
-void GNormalDistributionsTransform::setInputTarget(pcl::PointCloud<pcl::PointXYZ>::Ptr input)
-{
-	// Copy input map data from the host memory to the GPU memory
-	GRegistration::setInputTarget(input);
-
-	// Build the voxel grid
-	if (target_points_number_ != 0) {
-		voxel_grid_.setLeafSize(resolution_, resolution_, resolution_);
-		voxel_grid_.setInput(target_x_, target_y_, target_z_, target_points_number_);
-	}
-	printf("complete set Input Target\n");
-}
-
-void GNormalDistributionsTransform::computeTransformation(const Eigen::Matrix<float, 4, 4> &guess)
-{
-	if (dj_ang_.isEmpty()) {
-		dj_ang_.memAlloc();
-	}
-
-	if (dh_ang_.isEmpty()) {
-		dh_ang_.memAlloc();
-	}
-	
-	nr_iterations_ = 0;
-	converged_ = false;
-
-	double gauss_c1, gauss_c2, gauss_d3;
-
-	gauss_c1 = 10 * ( 1 - outlier_ratio_);
-	gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
-	gauss_d3 = - log(gauss_c2);
-	gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3;
-	gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3) / gauss_d1_);
-
-	if (guess != Eigen::Matrix4f::Identity()) {
-		final_transformation_ = guess;
-
-		transformPointCloud(x_, y_, z_, trans_x_, trans_y_, trans_z_, points_number_, guess);
-	}
-
-	Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
-	eig_transformation.matrix() = final_transformation_;
-
-	Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
-	Eigen::Vector3f init_translation = eig_transformation.translation();
-	Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);
-
-	p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), init_rotation(1), init_rotation(2);
-
-	Eigen::Matrix<double, 6, 6> hessian;
-
-	double score = 0;
-	double delta_p_norm;
-
-	score = computeDerivatives(score_gradient, hessian, trans_x_, trans_y_, trans_z_, points_number_, p);
-
-	int loop_time = 0;
-
-	while (!converged_) {
-		previous_transformation_ = transformation_;
-
-		Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> sv(hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
-
-		delta_p = sv.solve(-score_gradient);
-
-		delta_p_norm = delta_p.norm();
-
-		if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) {
-
-			trans_probability_ = score / static_cast<double>(points_number_);
-			converged_ = delta_p_norm == delta_p_norm;
-			return;
-		}
-
-		delta_p.normalize();
-		delta_p_norm = computeStepLengthMT(p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, trans_x_, trans_y_, trans_z_, points_number_);
-
-		delta_p *= delta_p_norm;
-
-		Eigen::Translation<float, 3> translation(static_cast<float>(delta_p(0)), static_cast<float>(delta_p(1)), static_cast<float>(delta_p(2)));
-		Eigen::AngleAxis<float> tmp1(static_cast<float>(delta_p(3)), Eigen::Vector3f::UnitX());
-		Eigen::AngleAxis<float> tmp2(static_cast<float>(delta_p(4)), Eigen::Vector3f::UnitY());
-		Eigen::AngleAxis<float> tmp3(static_cast<float>(delta_p(5)), Eigen::Vector3f::UnitZ());
-		Eigen::AngleAxis<float> tmp4(tmp1 * tmp2 * tmp3);
-
-		transformation_ = (translation * tmp4).matrix();
-
-		p = p + delta_p;
-
-		//Not update visualizer
-		if (nr_iterations_ > max_iterations_ || (nr_iterations_ && (std::fabs(delta_p_norm) < transformation_epsilon_)))
-			converged_ = true;
-
-		nr_iterations_++;
-
-		loop_time++;
-	}
-
-	trans_probability_ = score / static_cast<double>(points_number_);
-}
-
-/* First step of computing point gradients */
-__global__ void computePointGradients0(float *x, float *y, float *z, int points_num,
-													int *valid_points, int valid_points_num,
-													double *dj_ang,
-													double *pg00, double *pg11, double *pg22,
-													double *pg13, double *pg23, double *pg04, double *pg14)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	__shared__ double j_ang[12];
-
-
-	if (threadIdx.x < 12) {
-		j_ang[threadIdx.x] = dj_ang[threadIdx.x];
-	}
-
-	__syncthreads();
-
-	for (int i = id; i < valid_points_num; i += stride) {
-		int pid = valid_points[i];
-
-		//Orignal coordinates
-		double o_x = static_cast<double>(x[pid]);
-		double o_y = static_cast<double>(y[pid]);
-		double o_z = static_cast<double>(z[pid]);
-
-		//Set the 3x3 block start from (0, 0) to identity matrix
-		pg00[i] = 1;
-		pg11[i] = 1;
-		pg22[i] = 1;
-
-		//Compute point derivatives
-		pg13[i] = o_x * j_ang[0] + o_y * j_ang[1] + o_z * j_ang[2];
-		pg23[i] = o_x * j_ang[3] + o_y * j_ang[4] + o_z * j_ang[5];
-		pg04[i] = o_x * j_ang[6] + o_y * j_ang[7] + o_z * j_ang[8];
-		pg14[i] = o_x * j_ang[9] + o_y * j_ang[10] + o_z * j_ang[11];
-	}
-}
-
-/* Second step of computing point gradients */
-__global__ void computePointGradients1(float *x, float *y, float *z, int points_num,
-													int *valid_points, int valid_points_num,
-													double *dj_ang,
-													double *pg24, double *pg05, double *pg15, double *pg25)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	__shared__ double j_ang[12];
-
-
-	if (threadIdx.x < 12) {
-		j_ang[threadIdx.x] = dj_ang[threadIdx.x + 12];
-	}
-
-	__syncthreads();
-
-	for (int i = id; i < valid_points_num; i += stride) {
-		int pid = valid_points[i];
-
-		//Orignal coordinates
-		double o_x = static_cast<double>(x[pid]);
-		double o_y = static_cast<double>(y[pid]);
-		double o_z = static_cast<double>(z[pid]);
-
-		//Compute point derivatives
-
-		pg24[i] = o_x * j_ang[0] + o_y * j_ang[1] + o_z * j_ang[2];
-		pg05[i] = o_x * j_ang[3] + o_y * j_ang[4] + o_z * j_ang[5];
-		pg15[i] = o_x * j_ang[6] + o_y * j_ang[7] + o_z * j_ang[8];
-		pg25[i] = o_x * j_ang[9] + o_y * j_ang[10] + o_z * j_ang[11];
-	}
-}
-
-
-/* First step of computing point hessians */
-__global__ void computePointHessian0(float *x, float *y, float *z, int points_num,
-												int *valid_points, int valid_points_num,
-												double *dh_ang,
-												double *ph93, double *ph103, double *ph113,
-												double *ph123, double *ph94, double *ph133,
-												double *ph104, double *ph143, double *ph114,
-												double *ph153, double *ph95, double *ph163,
-												double *ph105, double *ph173, double *ph115)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	__shared__ double h_ang[18];
-
-	if (threadIdx.x < 18) {
-		h_ang[threadIdx.x] = dh_ang[threadIdx.x];
-	}
-
-	__syncthreads();
-
-	for (int i = id; i < valid_points_num; i += stride) {
-		int pid = valid_points[i];
-
-		//Orignal coordinates
-		double o_x = static_cast<double>(x[pid]);
-		double o_y = static_cast<double>(y[pid]);
-		double o_z = static_cast<double>(z[pid]);
-
-
-		ph93[i] = 0;
-		ph103[i] = o_x * h_ang[0] + o_y * h_ang[1] + o_z * h_ang[2];
-		ph113[i] = o_x * h_ang[3] + o_y * h_ang[4] + o_z * h_ang[5];
-
-		ph123[i] = ph94[i] = 0;
-		ph133[i] = ph104[i] = o_x * h_ang[6] + o_y * h_ang[7] + o_z * h_ang[8];
-		ph143[i] = ph114[i] = o_x * h_ang[9] + o_y * h_ang[10] + o_z * h_ang[11];
-
-		ph153[i] = ph95[i] = 0;
-		ph163[i] = ph105[i] = o_x * h_ang[12] + o_y * h_ang[13] + o_z * h_ang[14];
-		ph173[i] = ph115[i] = o_x * h_ang[15] + o_y * h_ang[16] + o_z * h_ang[17];
-
-	}
-}
-
-__global__ void computePointHessian1(float *x, float *y, float *z, int points_num,
-												int *valid_points, int valid_points_num,
-												double *dh_ang,
-												double *ph124, double *ph134, double *ph144,
-												double *ph154, double *ph125, double *ph164,
-												double *ph135, double *ph174, double *ph145)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	__shared__ double h_ang[18];
-
-	if (threadIdx.x < 18) {
-		h_ang[threadIdx.x] = dh_ang[18 + threadIdx.x];
-	}
-
-	__syncthreads();
-
-	for (int i = id; i < valid_points_num; i += stride) {
-		int pid = valid_points[i];
-
-		//Orignal coordinates
-		double o_x = static_cast<double>(x[pid]);
-		double o_y = static_cast<double>(y[pid]);
-		double o_z = static_cast<double>(z[pid]);
-
-		ph124[i] = o_x * h_ang[0] + o_y * h_ang[1] + o_z * h_ang[2];
-		ph134[i] = o_x * h_ang[3] + o_y * h_ang[4] + o_z * h_ang[5];
-		ph144[i] = o_x * h_ang[6] + o_y * h_ang[7] + o_z * h_ang[8];
-
-		ph154[i] = ph125[i] = o_x * h_ang[9] + o_y * h_ang[10] + o_z * h_ang[11];
-		ph164[i] = ph135[i] = o_x * h_ang[12] + o_y * h_ang[13] + o_z * h_ang[14];
-		ph174[i] = ph145[i] = o_x * h_ang[15] + o_y * h_ang[16] + o_z * h_ang[17];
-	}
-}
-
-__global__ void computePointHessian2(float *x, float *y, float *z, int points_num,
-												int *valid_points, int valid_points_num,
-												double *dh_ang,
-												double *ph155, double *ph165, double *ph175)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	__shared__ double h_ang[9];
-
-	if (threadIdx.x < 9) {
-		h_ang[threadIdx.x] = dh_ang[36 + threadIdx.x];
-	}
-
-	__syncthreads();
-
-	for (int i = id; i < valid_points_num; i += stride) {
-		int pid = valid_points[i];
-
-		//Orignal coordinates
-		double o_x = static_cast<double>(x[pid]);
-		double o_y = static_cast<double>(y[pid]);
-		double o_z = static_cast<double>(z[pid]);
-
-		ph155[i] = o_x * h_ang[0] + o_y * h_ang[1] + o_z * h_ang[2];
-		ph165[i] = o_x * h_ang[3] + o_y * h_ang[4] + o_z * h_ang[5];
-		ph175[i] = o_x * h_ang[6] + o_y * h_ang[7] + o_z * h_ang[8];
-
-	}
-}
-
-/* compute score_inc list for input points.
- * The final score_inc is calculated by a reduction sum
- * on this score_inc list. */
-__global__ void computeScoreList(int *starting_voxel_id, int *voxel_id, int valid_points_num,
-												double *e_x_cov_x, double gauss_d1, double *score)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = id; i < valid_points_num; i += stride) {
-
-		double score_inc = 0;
-
-		for (int vid = starting_voxel_id[i]; vid < starting_voxel_id[i + 1]; vid++) {
-			double tmp_ex = e_x_cov_x[vid];
-
-			score_inc += (tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex) ? 0 : -gauss_d1 * tmp_ex;
-		}
-
-		score[i] = score_inc;
-	}
-}
-
-/* First step to compute score gradient list for input points */
-__global__ void computeScoreGradientList(float *trans_x, float *trans_y, float *trans_z,
-														int *valid_points,
-														int *starting_voxel_id, int *voxel_id, int valid_points_num,
-														double *centroid_x, double *centroid_y, double *centroid_z,
-														int voxel_num, double *e_x_cov_x,
-														double *cov_dxd_pi, double gauss_d1, int valid_voxel_num,
-														double *score_gradients)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	int col = blockIdx.y;
-
-	if (col < 6) {
-		double *sg = score_gradients + col * valid_points_num;
-		double *cov_dxd_pi_mat0 = cov_dxd_pi + col * valid_voxel_num;
-		double *cov_dxd_pi_mat1 = cov_dxd_pi_mat0 + 6 * valid_voxel_num;
-		double *cov_dxd_pi_mat2 = cov_dxd_pi_mat1 + 6 * valid_voxel_num;
-
-		for (int i = id; i < valid_points_num; i += stride) {
-			int pid = valid_points[i];
-			double d_x = static_cast<double>(trans_x[pid]);
-			double d_y = static_cast<double>(trans_y[pid]);
-			double d_z = static_cast<double>(trans_z[pid]);
-
-			double tmp_sg = 0.0;
-
-			for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) {
-				int vid = voxel_id[j];
-				double tmp_ex = e_x_cov_x[j];
-
-				if (!(tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex)) {
-					tmp_ex *= gauss_d1;
-
-					tmp_sg += ((d_x - centroid_x[vid]) * cov_dxd_pi_mat0[j] + (d_y - centroid_y[vid]) * cov_dxd_pi_mat1[j] + (d_z - centroid_z[vid]) * cov_dxd_pi_mat2[j]) * tmp_ex;
-				}
-			}
-
-			sg[i] = tmp_sg;
-		}
-	}
-}
-
-/* Intermediate step to compute e_x_cov_x */
-__global__ void computeExCovX(float *trans_x, float *trans_y, float *trans_z, int *valid_points,
-											int *starting_voxel_id, int *voxel_id, int valid_points_num,
-											double *centr_x, double *centr_y, double *centr_z,
-											double gauss_d1, double gauss_d2,
-											double *e_x_cov_x,
-											double *icov00, double *icov01, double *icov02,
-											double *icov10, double *icov11, double *icov12,
-											double *icov20, double *icov21, double *icov22)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = id; i < valid_points_num; i += stride) {
-		int pid = valid_points[i];
-		double d_x = static_cast<double>(trans_x[pid]);
-		double d_y = static_cast<double>(trans_y[pid]);
-		double d_z = static_cast<double>(trans_z[pid]);
-		double t_x, t_y, t_z;
-
-
-		for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) {
-			int vid = voxel_id[j];
-
-			t_x = d_x - centr_x[vid];
-			t_y = d_y - centr_y[vid];
-			t_z = d_z - centr_z[vid];
-
-			e_x_cov_x[j] =  exp(-gauss_d2 * ((t_x * icov00[vid] + t_y * icov01[vid] + t_z * icov02[vid]) * t_x
-										+ ((t_x * icov10[vid] + t_y * icov11[vid] + t_z * icov12[vid]) * t_y)
-										+ ((t_x * icov20[vid] + t_y * icov21[vid] + t_z * icov22[vid]) * t_z)) / 2.0);
-		}
-	}
-}
-
-/* update e_x_cov_x - Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] */
-__global__ void updateExCovX(double *e_x_cov_x, double gauss_d2, int valid_voxel_num)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = id; i < valid_voxel_num; i += stride) {
-		e_x_cov_x[i] *= gauss_d2;
-	}
-}
-
-/* compute cov_dxd_pi as reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]*/
-__global__ void computeCovDxdPi(int *valid_points, int *starting_voxel_id, int *voxel_id, int valid_points_num,
-											double *inverse_covariance, int voxel_num,
-											double gauss_d1, double gauss_d2, double *point_gradients,
-											double *cov_dxd_pi, int valid_voxel_num)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	int row = blockIdx.y;
-	int col = blockIdx.z;
-
-	if (row < 3 && col < 6) {
-		double *icov0 = inverse_covariance + row * 3 * voxel_num;
-		double *icov1 = icov0 + voxel_num;
-		double *icov2 = icov1 + voxel_num;
-		double *cov_dxd_pi_tmp = cov_dxd_pi + (row * 6 + col) * valid_voxel_num;
-		double *pg_tmp0 = point_gradients + col * valid_points_num;
-		double *pg_tmp1 = pg_tmp0 + 6 * valid_points_num;
-		double *pg_tmp2 = pg_tmp1 + 6 * valid_points_num;
-
-		for (int i = id; i < valid_points_num; i += stride) {
-			double pg0 = pg_tmp0[i];
-			double pg1 = pg_tmp1[i];
-			double pg2 = pg_tmp2[i];
-
-			for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) {
-				int vid = voxel_id[j];
-
-				cov_dxd_pi_tmp[j] = icov0[vid] * pg0 + icov1[vid] * pg1 + icov2[vid] * pg2;
-			}
-	}
-	}
-}
-
-
-/* First step to compute hessian list for input points */
-__global__ void computeHessianListS0(float *trans_x, float *trans_y, float *trans_z,
-													int *valid_points,
-													int *starting_voxel_id, int *voxel_id, int valid_points_num,
-													double *centroid_x, double *centroid_y, double *centroid_z,
-													double *icov00, double *icov01, double *icov02,
-													double *icov10, double *icov11, double *icov12,
-													double *icov20, double *icov21, double *icov22,
-													double *point_gradients,
-													double *tmp_hessian,
-													int valid_voxel_num)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	int col = blockIdx.y;
-
-	if (col < 6) {
-		double *tmp_pg0 = point_gradients + col * valid_points_num;
-		double *tmp_pg1 = tmp_pg0 + 6 * valid_points_num;
-		double *tmp_pg2 = tmp_pg1 + 6 * valid_points_num;
-		double *tmp_h = tmp_hessian + col * valid_voxel_num;
-
-		for (int i = id; i < valid_points_num; i += stride) {
-			int pid = valid_points[i];
-			double d_x = static_cast<double>(trans_x[pid]);
-			double d_y = static_cast<double>(trans_y[pid]);
-			double d_z = static_cast<double>(trans_z[pid]);
-
-			double pg0 = tmp_pg0[i];
-			double pg1 = tmp_pg1[i];
-			double pg2 = tmp_pg2[i];
-
-			for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) {
-				int vid = voxel_id[j];
-
-				tmp_h[j] = (d_x - centroid_x[vid]) * (icov00[vid] * pg0 + icov01[vid] * pg1 + icov02[vid] * pg2)
-							+ (d_y - centroid_y[vid]) * (icov10[vid] * pg0 + icov11[vid] * pg1 + icov12[vid] * pg2)
-							+ (d_z - centroid_z[vid]) * (icov20[vid] * pg0 + icov21[vid] * pg1 + icov22[vid] * pg2);
-			}
-	}
-	}
-}
-
-/* Fourth step to compute hessian list */
-__global__ void computeHessianListS1(float *trans_x, float *trans_y, float *trans_z,
-												int *valid_points,
-												int *starting_voxel_id, int *voxel_id, int valid_points_num,
-												double *centroid_x, double *centroid_y, double *centroid_z,
-												double gauss_d1, double gauss_d2, double *hessians,
-												double *e_x_cov_x, double *tmp_hessian, double *cov_dxd_pi,
-												double *point_gradients,
-												int valid_voxel_num)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	int row = blockIdx.y;
-	int col = blockIdx.z;
-
-	if (row < 6 && col < 6) {
-		double *cov_dxd_pi_mat0 = cov_dxd_pi + row * valid_voxel_num;
-		double *cov_dxd_pi_mat1 = cov_dxd_pi_mat0 + 6 * valid_voxel_num;
-		double *cov_dxd_pi_mat2 = cov_dxd_pi_mat1 + 6 * valid_voxel_num;
-		double *tmp_h = tmp_hessian + col * valid_voxel_num;
-		double *h = hessians + (row * 6 + col) * valid_points_num;
-		double *tmp_pg0 = point_gradients + col * valid_points_num;
-		double *tmp_pg1 = tmp_pg0 + 6 * valid_points_num;
-		double *tmp_pg2 = tmp_pg1 + 6 * valid_points_num;
-
-		for (int i = id; i < valid_points_num; i += stride) {
-			int pid = valid_points[i];
-			double d_x = static_cast<double>(trans_x[pid]);
-			double d_y = static_cast<double>(trans_y[pid]);
-			double d_z = static_cast<double>(trans_z[pid]);
-
-			double pg0 = tmp_pg0[i];
-			double pg1 = tmp_pg1[i];
-			double pg2 = tmp_pg2[i];
-
-			double final_hessian = 0.0;
-
-			for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) {
-				//Transformed coordinates
-				int vid = voxel_id[j];
-
-				double tmp_ex = e_x_cov_x[j];
-
-				if (!(tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex)) {
-					double cov_dxd0 = cov_dxd_pi_mat0[j];
-					double cov_dxd1 = cov_dxd_pi_mat1[j];
-					double cov_dxd2 = cov_dxd_pi_mat2[j];
-
-					tmp_ex *= gauss_d1;
-
-					final_hessian += -gauss_d2 * ((d_x - centroid_x[vid]) * cov_dxd0 + (d_y - centroid_y[vid]) * cov_dxd1 + (d_z - centroid_z[vid]) * cov_dxd2) * tmp_h[j] * tmp_ex;
-					final_hessian += (pg0 * cov_dxd0 + pg1 * cov_dxd1 + pg2 * cov_dxd2) * tmp_ex;
-				}
-			}
-
-			h[i] = final_hessian;
-		}
-	}
-}
-
-__global__ void computeHessianListS2(float *trans_x, float *trans_y, float *trans_z,
-												int *valid_points,
-												int *starting_voxel_id, int *voxel_id, int valid_points_num,
-												double *centroid_x, double *centroid_y, double *centroid_z,
-												double gauss_d1, double *e_x_cov_x,
-												double *icov00, double *icov01, double *icov02,
-												double *icov10, double *icov11, double *icov12,
-												double *icov20, double *icov21, double *icov22,
-												double *point_hessians, double *hessians,
-												int valid_voxel_num)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	int row = blockIdx.y;
-	int col = blockIdx.z;
-
-	if (row < 6 && col < 6) {
-		double *h = hessians + (row * 6 + col) * valid_points_num;
-		double *tmp_ph0 = point_hessians + ((3 * row) * 6 + col) * valid_points_num;
-		double *tmp_ph1 = tmp_ph0 + 6 * valid_points_num;
-		double *tmp_ph2 = tmp_ph1 + 6 * valid_points_num;
-
-		for (int i = id; i < valid_points_num; i += stride) {
-			int pid = valid_points[i];
-			double d_x = static_cast<double>(trans_x[pid]);
-			double d_y = static_cast<double>(trans_y[pid]);
-			double d_z = static_cast<double>(trans_z[pid]);
-			double ph0 = tmp_ph0[i];
-			double ph1 = tmp_ph1[i];
-			double ph2 = tmp_ph2[i];
-
-			double final_hessian = h[i];
-
-			for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) {
-				//Transformed coordinates
-				int vid = voxel_id[j];
-				double tmp_ex = e_x_cov_x[j];
-
-				if (!(tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex)) {
-					tmp_ex *= gauss_d1;
-
-					final_hessian += (d_x - centroid_x[vid]) * (icov00[vid] * ph0 + icov01[vid] * ph1 + icov02[vid] * ph2) * tmp_ex;
-					final_hessian += (d_y - centroid_y[vid]) * (icov10[vid] * ph0 + icov11[vid] * ph1 + icov12[vid] * ph2) * tmp_ex;
-					final_hessian += (d_z - centroid_z[vid]) * (icov20[vid] * ph0 + icov21[vid] * ph1 + icov22[vid] * ph2) * tmp_ex;
-
-				}
-			}
-
-			h[i] = final_hessian;
-		}
-	}
-}
-
-
-/* Compute sum of a list of matrices */
-__global__ void matrixSum(double *matrix_list, int full_size, int half_size, int rows, int cols, int offset)
-{
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	int row = blockIdx.y;
-	int col = blockIdx.z;
-
-	for (int i = index; i < half_size && row < rows && col < cols; i += stride) {
-		MatrixDevice left(rows, cols, offset, matrix_list + i);
-		double *right_ptr = (i + half_size < full_size) ? matrix_list + i + half_size : NULL;
-		MatrixDevice right(rows, cols, offset, right_ptr);
-
-		if (right_ptr != NULL) {
-			left(row, col) += right(row, col);
-		}
-	}
-}
-
-/* Compute sum of score_inc list */
-__global__ void sumScore(double *score, int full_size, int half_size)
-{
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = index; i < half_size; i += stride) {
-		score[i] += (i + half_size < full_size) ? score[i + half_size] : 0;
-	}
-}
-
-
-double GNormalDistributionsTransform::computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
-														float *trans_x, float *trans_y, float *trans_z,
-														int points_num, Eigen::Matrix<double, 6, 1> pose, bool compute_hessian)
-{
-	MatrixHost p(6, 1);
-
-	for (int i = 0; i < 6; i++) {
-		p(i) = pose(i, 0);
-	}
-
-	score_gradient.setZero ();
-	hessian.setZero ();
-
-	//Compute Angle Derivatives
-	computeAngleDerivatives(p);
-
-	//Radius Search
-	int *valid_points, *voxel_id, *starting_voxel_id;
-	int valid_voxel_num, valid_points_num;
-
-	valid_points = voxel_id = starting_voxel_id = NULL;
-
-	voxel_grid_.radiusSearch(trans_x, trans_y, trans_z, points_num, resolution_, INT_MAX, &valid_points, &starting_voxel_id, &voxel_id, &valid_voxel_num, &valid_points_num);
-
-	double *covariance = voxel_grid_.getCovarianceList();
-	double *inverse_covariance = voxel_grid_.getInverseCovarianceList();
-	double *centroid = voxel_grid_.getCentroidList();
-	int *points_per_voxel = voxel_grid_.getPointsPerVoxelList();
-	int voxel_num = voxel_grid_.getVoxelNum();
-
-	if (valid_points_num == 0)
-		return 0;
-
-	//Update score gradient and hessian matrix
-
-	double *gradients, *hessians, *point_gradients, *point_hessians, *score;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&gradients, sizeof(double) * valid_points_num * 6));
-	rubis::sched::yield_gpu("18_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&hessians, sizeof(double) * valid_points_num * 6 * 6));
-	rubis::sched::yield_gpu("19_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&point_gradients, sizeof(double) * valid_points_num * 3 * 6));
-	rubis::sched::yield_gpu("20_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&point_hessians, sizeof(double) * valid_points_num * 18 * 6));
-	rubis::sched::yield_gpu("21_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&score, sizeof(double) * valid_points_num));
-	rubis::sched::yield_gpu("22_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(gradients, 0, sizeof(double) * valid_points_num * 6));
-	rubis::sched::yield_gpu("23_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(hessians, 0, sizeof(double) * valid_points_num * 6 * 6));
-	rubis::sched::yield_gpu("24_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(point_gradients, 0, sizeof(double) * valid_points_num * 3 * 6));
-	rubis::sched::yield_gpu("25_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(point_hessians, 0, sizeof(double) * valid_points_num * 18 * 6));
-	rubis::sched::yield_gpu("26_cudaMemset");
-
-	int block_x = (valid_points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_points_num;
-
-	int grid_x = (valid_points_num - 1) / block_x + 1;
-
-	dim3 grid;
-
-
-	rubis::sched::request_gpu();
-	computePointGradients0<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dj_ang_.buffer(),
-												point_gradients,
-												point_gradients + valid_points_num * 7,
-												point_gradients + valid_points_num * 14,
-												point_gradients + valid_points_num * 9,
-												point_gradients + valid_points_num * 15,
-												point_gradients + valid_points_num * 4,
-												point_gradients + valid_points_num * 10);
-	rubis::sched::yield_gpu("27_computePointGradients0");
-	checkCudaErrors(cudaGetLastError());
-	
-	rubis::sched::request_gpu();
-	computePointGradients1<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dj_ang_.buffer(),
-												point_gradients + valid_points_num * 16,
-												point_gradients + valid_points_num * 5,
-												point_gradients + valid_points_num * 11,
-												point_gradients + valid_points_num * 17);
-	rubis::sched::yield_gpu("28_computePointGradients1");
-	checkCudaErrors(cudaGetLastError());
-
-	if (compute_hessian) {
-		rubis::sched::request_gpu();
-		computePointHessian0<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dh_ang_.buffer(),
-												point_hessians + valid_points_num * 57, 
-												point_hessians + valid_points_num * 63, point_hessians + valid_points_num * 69,
-												point_hessians + valid_points_num * 75, point_hessians + valid_points_num * 58, point_hessians + valid_points_num * 81,
-												point_hessians + valid_points_num * 64, point_hessians + valid_points_num * 87, point_hessians + valid_points_num * 70,
-												point_hessians + valid_points_num * 93, point_hessians + valid_points_num * 59, point_hessians + valid_points_num * 99,
-												point_hessians + valid_points_num * 65, point_hessians + valid_points_num * 105, point_hessians + valid_points_num * 71);
-		rubis::sched::yield_gpu("29_computePointHessian0");
-		checkCudaErrors(cudaGetLastError());
-
-
-		rubis::sched::request_gpu();
-		computePointHessian1<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dh_ang_.buffer(),
-												point_hessians + valid_points_num * 76, point_hessians + valid_points_num * 82, point_hessians + valid_points_num * 88,
-												point_hessians + valid_points_num * 94, point_hessians + valid_points_num * 77, point_hessians + valid_points_num * 100,
-												point_hessians + valid_points_num * 83, point_hessians + valid_points_num * 106, point_hessians + valid_points_num * 89);
-		rubis::sched::yield_gpu("30_computePointHessian1");
-		checkCudaErrors(cudaGetLastError());
-
-		rubis::sched::request_gpu();
-		computePointHessian2<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dh_ang_.buffer(),
-												point_hessians + valid_points_num * 95, point_hessians + valid_points_num * 101, point_hessians + valid_points_num * 107);
-		rubis::sched::yield_gpu("31_computePointHessian2");
-		checkCudaErrors(cudaGetLastError());
-
-	}
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-
-	double *tmp_hessian;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&tmp_hessian, sizeof(double) * valid_voxel_num * 6));
-	rubis::sched::yield_gpu("32_cudaMalloc");
-	
-	double *e_x_cov_x;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&e_x_cov_x, sizeof(double) * valid_voxel_num));
-	rubis::sched::yield_gpu("33_cudaMalloc");
-
-	double *cov_dxd_pi;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&cov_dxd_pi, sizeof(double) * valid_voxel_num * 3 * 6));
-	rubis::sched::yield_gpu("34_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	computeExCovX<<<grid_x, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-										starting_voxel_id, voxel_id, valid_points_num,
-										centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-										gauss_d1_, gauss_d2_,
-										e_x_cov_x,
-										inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num,
-										inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num,
-										inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num);
-	rubis::sched::yield_gpu("35_computeExCovX");
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeScoreList<<<grid_x, block_x>>>(starting_voxel_id, voxel_id, valid_points_num, e_x_cov_x, gauss_d1_, score);
-	rubis::sched::yield_gpu("36_computeScoreList");
-
-	checkCudaErrors(cudaGetLastError());
-
-	int block_x2 = (valid_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_voxel_num;
-	int grid_x2 = (valid_voxel_num - 1) / block_x2 + 1;
-
-	rubis::sched::request_gpu();
-	updateExCovX<<<grid_x2, block_x2>>>(e_x_cov_x, gauss_d2_, valid_voxel_num);
-	rubis::sched::yield_gpu("37_computeScoreList");
-
-	checkCudaErrors(cudaGetLastError());
-
-	grid.x = grid_x;
-	grid.y = 3;
-	grid.z = 6;
-
-	rubis::sched::request_gpu();
-	computeCovDxdPi<<<grid, block_x>>>(valid_points, starting_voxel_id, voxel_id, valid_points_num,
-											inverse_covariance, voxel_num,
-											gauss_d1_, gauss_d2_, point_gradients,
-											cov_dxd_pi, valid_voxel_num);
-	rubis::sched::yield_gpu("38_computeCovDxdPi");
-	checkCudaErrors(cudaGetLastError());
-
-	grid.x = grid_x;
-	grid.y = 6;
-	grid.z = 1;
-
-	rubis::sched::request_gpu();
-	computeScoreGradientList<<<grid, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-													starting_voxel_id, voxel_id, valid_points_num,
-													centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-													voxel_num, e_x_cov_x,
-													cov_dxd_pi, gauss_d1_, valid_voxel_num, gradients);
-	rubis::sched::yield_gpu("39_computeCovDxdPi");
-	checkCudaErrors(cudaGetLastError());
-
-	if (compute_hessian) {
-
-		grid.y = 6;
-		grid.z = 1;
-
-		rubis::sched::request_gpu();
-		computeHessianListS0<<<grid, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-												starting_voxel_id, voxel_id, valid_points_num,
-												centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-												inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num,
-												inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num,
-												inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num,
-												point_gradients,
-												tmp_hessian, valid_voxel_num);
-		rubis::sched::yield_gpu("40_computeHessianListS0");
-
-		checkCudaErrors(cudaGetLastError());
-		grid.z = 6;
-
-		rubis::sched::request_gpu();
-		computeHessianListS1<<<grid, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-													starting_voxel_id, voxel_id, valid_points_num,
-													centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-													gauss_d1_, gauss_d2_, hessians,
-													e_x_cov_x, tmp_hessian, cov_dxd_pi,
-													point_gradients,
-													valid_voxel_num);
-		rubis::sched::yield_gpu("41_computeHessianListS1");
-		checkCudaErrors(cudaGetLastError());
-		
-		rubis::sched::request_gpu();
-		computeHessianListS2<<<grid, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-													starting_voxel_id, voxel_id, valid_points_num,
-													centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-													gauss_d1_, e_x_cov_x,
-													inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num,
-													inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num,
-													inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num,
-													point_hessians, hessians, valid_voxel_num);
-		rubis::sched::yield_gpu("42_computeHessianListS2");
-		checkCudaErrors(cudaGetLastError());
-
-	}
-
-	int full_size = valid_points_num;
-	int half_size = (full_size - 1) / 2 + 1;
-
-	while (full_size > 1) {
-		block_x = (half_size > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_size;
-		grid_x = (half_size - 1) / block_x + 1;
-
-		grid.x = grid_x;
-		grid.y = 1;
-		grid.z = 6;
-
-		rubis::sched::request_gpu_in_loop(GPU_SEG_LOOP_START);
-		matrixSum<<<grid, block_x>>>(gradients, full_size, half_size, 1, 6, valid_points_num);
-		rubis::sched::yield_gpu_in_loop(GPU_SEG_LOOP_START,"43_matrixSum");
-
-		checkCudaErrors(cudaGetLastError());
-
-		grid.y = 6;
-
-		rubis::sched::request_gpu_in_loop(GPU_SEG_LOOP_MID);
-		matrixSum<<<grid, block_x>>>(hessians, full_size, half_size, 6, 6, valid_points_num);
-		rubis::sched::yield_gpu_in_loop(GPU_SEG_LOOP_MID, "44_matrixSum");
-
-		checkCudaErrors(cudaGetLastError());
-
-		rubis::sched::request_gpu_in_loop(GPU_SEG_LOOP_END);
-		sumScore<<<grid_x, block_x>>>(score, full_size, half_size);
-		rubis::sched::yield_gpu_in_loop(GPU_SEG_LOOP_END, "45_sumScore");
-
-		checkCudaErrors(cudaGetLastError());
-
-		full_size = half_size;
-		half_size = (full_size - 1) / 2 + 1;
-	}
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	MatrixDevice dgrad(1, 6, valid_points_num, gradients), dhess(6, 6, valid_points_num, hessians);
-	MatrixHost hgrad(1, 6), hhess(6, 6);
-
-	hgrad.moveToHost(dgrad);
-
-	hhess.moveToHost(dhess);
-
-	for (int i = 0; i < 6; i++) {
-		score_gradient(i) = hgrad(i);
-	}
-
-	for (int i = 0; i < 6; i++) {
-		for (int j = 0; j < 6; j++) {
-			hessian(i, j) = hhess(i, j);
-		}
-	}
-
-	double score_inc;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&score_inc, score, sizeof(double), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("46_dtoh");
-	
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(gradients));
-	rubis::sched::yield_gpu("47_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(hessians));
-	rubis::sched::yield_gpu("48_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(point_hessians));
-	rubis::sched::yield_gpu("49_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(point_gradients));
-	rubis::sched::yield_gpu("50_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(score));
-	rubis::sched::yield_gpu("51_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(tmp_hessian));
-	rubis::sched::yield_gpu("52_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(e_x_cov_x));
-	rubis::sched::yield_gpu("53_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(cov_dxd_pi));
-	rubis::sched::yield_gpu("54_free");
-
-	if (valid_points != NULL){
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_points));
-		rubis::sched::yield_gpu("55_free");
-	}
-
-	if (voxel_id != NULL){
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(voxel_id));
-		rubis::sched::yield_gpu("56_free");
-	}
-
-	if (starting_voxel_id != NULL){
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(starting_voxel_id));
-		rubis::sched::yield_gpu("57_free");
-	}
-
-
-
-	return score_inc;
-}
-
-void GNormalDistributionsTransform::computeAngleDerivatives(MatrixHost pose, bool compute_hessian)
-{
-	double cx, cy, cz, sx, sy, sz;
-
-	if (fabs(pose(3)) < 10e-5) {
-		cx = 1.0;
-		sx = 0.0;
-	} else {
-		cx = cos(pose(3));
-		sx = sin(pose(3));
-	}
-
-	if (fabs(pose(4)) < 10e-5) {
-		cy = 1.0;
-		sy = 0.0;
-	} else {
-		cy = cos(pose(4));
-		sy = sin(pose(4));
-	}
-
-	if (fabs(pose(5)) < 10e-5) {
-		cz = 1.0;
-		sz = 0.0;
-	} else {
-		cz = cos(pose(5));
-		sz = sin(pose(5));
-	}
-
-
-	j_ang_(0) = -sx * sz + cx * sy * cz;
-	j_ang_(1) = -sx * cz - cx * sy * sz;
-	j_ang_(2) = -cx * cy;
-
-	j_ang_(3) = cx * sz + sx * sy * cz;
-	j_ang_(4) = cx * cz - sx * sy * sz;
-	j_ang_(5) = -sx * cy;
-
-	j_ang_(6) = -sy * cz;
-	j_ang_(7) = sy * sz;
-	j_ang_(8) = cy;
-
-	j_ang_(9) = sx * cy * cz;
-	j_ang_(10) = -sx * cy * sz;
-	j_ang_(11) = sx * sy;
-
-	j_ang_(12) = -cx * cy * cz;
-	j_ang_(13) = cx * cy * sz;
-	j_ang_(14) = -cx * sy;
-
-	j_ang_(15) = -cy * sz;
-	j_ang_(16) = -cy * cz;
-	j_ang_(17) = 0;
-
-	j_ang_(18) = cx * cz - sx * sy * sz;
-	j_ang_(19) = -cx * sz - sx * sy * cz;
-	j_ang_(20) = 0;
-
-	j_ang_(21) = sx * cz + cx * sy * sz;
-	j_ang_(22) = cx * sy * cz - sx * sz;
-	j_ang_(23) = 0;
-
-	j_ang_.moveToGpu(dj_ang_);
-
-	if (compute_hessian) {
-
-		h_ang_(0) = -cx * sz - sx * sy * cz;
-		h_ang_(1) = -cx * cz + sx * sy * sz;
-		h_ang_(2) = sx * cy;
-
-		h_ang_(3) = -sx * sz + cx * sy * cz;
-		h_ang_(4) = -cx * sy * sz - sx * cz;
-		h_ang_(5) = -cx * cy;
-
-		h_ang_(6) = cx * cy * cz;
-		h_ang_(7) = -cx * cy * sz;
-		h_ang_(8) = cx * sy;
-
-		h_ang_(9) = sx * cy * cz;
-		h_ang_(10) = -sx * cy * sz;
-		h_ang_(11) = sx * sy;
-
-		h_ang_(12) = -sx * cz - cx * sy * sz;
-		h_ang_(13) = sx * sz - cx * sy * cz;
-		h_ang_(14) = 0;
-
-		h_ang_(15) = cx * cz - sx * sy * sz;
-		h_ang_(16) = -sx * sy * cz - cx * sz;
-		h_ang_(17) = 0;
-
-		h_ang_(18) = -cy * cz;
-		h_ang_(19) = cy * sz;
-		h_ang_(20) = sy;
-
-		h_ang_(21) = -sx * sy * cz;
-		h_ang_(22) = sx * sy * sz;
-		h_ang_(23) = sx * cy;
-
-		h_ang_(24) = cx * sy * cz;
-		h_ang_(25) = -cx * sy * sz;
-		h_ang_(26) = -cx * cy;
-
-		h_ang_(27) = sy * sz;
-		h_ang_(28) = sy * cz;
-		h_ang_(29) = 0;
-
-		h_ang_(30) = -sx * cy * sz;
-		h_ang_(31) = -sx * cy * cz;
-		h_ang_(32) = 0;
-
-		h_ang_(33) = cx * cy * sz;
-		h_ang_(34) = cx * cy * cz;
-		h_ang_(35) = 0;
-
-		h_ang_(36) = -cy * cz;
-		h_ang_(37) = cy * sz;
-		h_ang_(38) = 0;
-
-		h_ang_(39) = -cx * sz - sx * sy * cz;
-		h_ang_(40) = -cx * cz + sx * sy * sz;
-		h_ang_(41) = 0;
-
-		h_ang_(42) = -sx * sz + cx * sy * cz;
-		h_ang_(43) = -cx * sy * sz - sx * cz;
-		h_ang_(44) = 0;
-
-		h_ang_.moveToGpu(dh_ang_);
-	}
-
-}
-
-
-
-
-__global__ void gpuTransform(float *in_x, float *in_y, float *in_z,
-										float *trans_x, float *trans_y, float *trans_z,
-										int point_num, MatrixDevice transform)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-	float x, y, z;
-
-	for (int i = idx; i < point_num; i += stride) {
-		x = in_x[i];
-		y = in_y[i];
-		z = in_z[i];
-		trans_x[i] = transform(0, 0) * x + transform(0, 1) * y + transform(0, 2) * z + transform(0, 3);
-		trans_y[i] = transform(1, 0) * x + transform(1, 1) * y + transform(1, 2) * z + transform(1, 3);
-		trans_z[i] = transform(2, 0) * x + transform(2, 1) * y + transform(2, 2) * z + transform(2, 3);
-	}
-}
-
-void GNormalDistributionsTransform::transformPointCloud(float *in_x, float *in_y, float *in_z,
-														float *trans_x, float *trans_y, float *trans_z,
-														int points_number, Eigen::Matrix<float, 4, 4> transform)
-{
-	Eigen::Transform<float, 3, Eigen::Affine> t(transform);
-
-	MatrixHost htrans(3, 4);
-	MatrixDevice dtrans(3, 4);
-
-
-	dtrans.memAlloc();
-
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 4; j++) {
-			htrans(i, j) = t(i, j);
-		}
-	}
-	htrans.moveToGpu(dtrans);
-
-	if (points_number > 0) {
-		int block_x = (points_number <= BLOCK_SIZE_X) ? points_number : BLOCK_SIZE_X;
-		int grid_x = (points_number - 1) / block_x + 1;
-
-		rubis::sched::request_gpu();
-		gpuTransform<<<grid_x, block_x >>>(in_x, in_y, in_z, trans_x, trans_y, trans_z, points_number, dtrans);
-		rubis::sched::yield_gpu("58_gpuTransform");
-		
-		checkCudaErrors(cudaGetLastError());
-		checkCudaErrors(cudaDeviceSynchronize());
-
-	}
-
-	dtrans.memFree();
-}
-
-double GNormalDistributionsTransform::computeStepLengthMT(const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir,
-															double step_init, double step_max, double step_min, double &score,
-															Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
-															float *trans_x, float *trans_y, float *trans_z, int points_num)
-{
-	double phi_0 = -score;
-	double d_phi_0 = -(score_gradient.dot(step_dir));
-
-	Eigen::Matrix<double, 6, 1> x_t;
-
-	if (d_phi_0 >= 0) {
-		if (d_phi_0 == 0)
-			return 0;
-		else {
-			d_phi_0 *= -1;
-			step_dir *= -1;
-		}
-	}
-
-	int max_step_iterations = 10;
-	int step_iterations = 0;
-
-
-	double mu = 1.e-4;
-	double nu = 0.9;
-	double a_l = 0, a_u = 0;
-
-	double f_l = auxilaryFunction_PsiMT(a_l, phi_0, phi_0, d_phi_0, mu);
-	double g_l = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
-
-	double f_u = auxilaryFunction_PsiMT(a_u, phi_0, phi_0, d_phi_0, mu);
-	double g_u = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
-
-	bool interval_converged = (step_max - step_min) > 0, open_interval = true;
-
-	double a_t = step_init;
-	a_t = std::min(a_t, step_max);
-	a_t = std::max(a_t, step_min);
-
-	x_t = x + step_dir * a_t;
-
-	Eigen::Translation<float, 3> translation(static_cast<float>(x_t(0)), static_cast<float>(x_t(1)), static_cast<float>(x_t(2)));
-	Eigen::AngleAxis<float> tmp1(static_cast<float>(x_t(3)), Eigen::Vector3f::UnitX());
-	Eigen::AngleAxis<float> tmp2(static_cast<float>(x_t(4)), Eigen::Vector3f::UnitY());
-	Eigen::AngleAxis<float> tmp3(static_cast<float>(x_t(5)), Eigen::Vector3f::UnitZ());
-	Eigen::AngleAxis<float> tmp4(tmp1 * tmp2 * tmp3);
-
-	final_transformation_ = (translation * tmp4).matrix();
-
-	transformPointCloud(x_, y_, z_, trans_x, trans_y, trans_z, points_num, final_transformation_);
-
-	score = computeDerivatives(score_gradient, hessian, trans_x, trans_y, trans_z, points_num, x_t);
-
-	double phi_t = -score;
-	double d_phi_t = -(score_gradient.dot(step_dir));
-	double psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
-	double d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
-
-	while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 && d_phi_t <= -nu * d_phi_0)) {
-		if (open_interval) {
-			a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
-		} else {
-			a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
-		}
-
-		a_t = (a_t < step_max) ? a_t : step_max;
-		a_t = (a_t > step_min) ? a_t : step_min;
-
-		x_t = x + step_dir * a_t;
-
-		translation = Eigen::Translation<float, 3>(static_cast<float>(x_t(0)), static_cast<float>(x_t(1)), static_cast<float>(x_t(2)));
-		tmp1 = Eigen::AngleAxis<float>(static_cast<float>(x_t(3)), Eigen::Vector3f::UnitX());
-		tmp2 = Eigen::AngleAxis<float>(static_cast<float>(x_t(4)), Eigen::Vector3f::UnitY());
-		tmp3 = Eigen::AngleAxis<float>(static_cast<float>(x_t(5)), Eigen::Vector3f::UnitZ());
-		tmp4 = tmp1 * tmp2 * tmp3;
-
-		final_transformation_ = (translation * tmp4).matrix();
-
-		transformPointCloud(x_, y_, z_, trans_x, trans_y, trans_z, points_num, final_transformation_);
-
-		score = computeDerivatives(score_gradient, hessian, trans_x, trans_y, trans_z, points_num, x_t, false);
-
-		phi_t -= score;
-		d_phi_t -= (score_gradient.dot(step_dir));
-		psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
-		d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
-
-		if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) {
-			open_interval = false;
-
-			f_l += phi_0 - mu * d_phi_0 * a_l;
-			g_l += mu * d_phi_0;
-
-			f_u += phi_0 - mu * d_phi_0 * a_u;
-			g_u += mu * d_phi_0;
-		}
-
-		if (open_interval) {
-			interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
-		} else {
-			interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
-		}
-		step_iterations++;
-	}
-
-	if (step_iterations) {
-		computeHessian(hessian, trans_x, trans_y, trans_z, points_num, x_t);
-	}
-
-	real_iterations_ += step_iterations;
-
-	return a_t;
-}
-
-
-//Copied from ndt.hpp
-double GNormalDistributionsTransform::trialValueSelectionMT (double a_l, double f_l, double g_l,
-															double a_u, double f_u, double g_u,
-															double a_t, double f_t, double g_t)
-{
-	// Case 1 in Trial Value Selection [More, Thuente 1994]
-	if (f_t > f_l) {
-		// Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
-		// Equation 2.4.52 [Sun, Yuan 2006]
-		double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
-		double w = std::sqrt (z * z - g_t * g_l);
-		// Equation 2.4.56 [Sun, Yuan 2006]
-		double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
-
-		// Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l
-		// Equation 2.4.2 [Sun, Yuan 2006]
-		double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
-
-		if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l))
-		  return (a_c);
-		else
-		  return (0.5 * (a_q + a_c));
-	}
-	// Case 2 in Trial Value Selection [More, Thuente 1994]
-	else if (g_t * g_l < 0) {
-		// Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
-		// Equation 2.4.52 [Sun, Yuan 2006]
-		double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
-		double w = std::sqrt (z * z - g_t * g_l);
-		// Equation 2.4.56 [Sun, Yuan 2006]
-		double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
-
-		// Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t
-		// Equation 2.4.5 [Sun, Yuan 2006]
-		double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
-
-		if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t))
-		  return (a_c);
-		else
-		  return (a_s);
-	}
-	// Case 3 in Trial Value Selection [More, Thuente 1994]
-	else if (std::fabs (g_t) <= std::fabs (g_l)) {
-		// Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
-		// Equation 2.4.52 [Sun, Yuan 2006]
-		double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
-		double w = std::sqrt (z * z - g_t * g_l);
-		double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
-
-		// Calculate the minimizer of the quadratic that interpolates g_l and g_t
-		// Equation 2.4.5 [Sun, Yuan 2006]
-		double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
-
-		double a_t_next;
-
-		if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t))
-		  a_t_next = a_c;
-		else
-		  a_t_next = a_s;
-
-		if (a_t > a_l)
-		  return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next));
-		else
-		  return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next));
-	}
-	// Case 4 in Trial Value Selection [More, Thuente 1994]
-	else {
-		// Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t
-		// Equation 2.4.52 [Sun, Yuan 2006]
-		double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
-		double w = std::sqrt (z * z - g_t * g_u);
-		// Equation 2.4.56 [Sun, Yuan 2006]
-		return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
-	}
-}
-
-//Copied from ndt.hpp
-double GNormalDistributionsTransform::updateIntervalMT (double &a_l, double &f_l, double &g_l,
-														double &a_u, double &f_u, double &g_u,
-														double a_t, double f_t, double g_t)
-{
-  // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994]
-	if (f_t > f_l) {
-		a_u = a_t;
-		f_u = f_t;
-		g_u = g_t;
-		return (false);
-	}
-	// Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994]
-	else if (g_t * (a_l - a_t) > 0) {
-		a_l = a_t;
-		f_l = f_t;
-		g_l = g_t;
-		return (false);
-	}
-	// Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994]
-	else if (g_t * (a_l - a_t) < 0) {
-		a_u = a_l;
-		f_u = f_l;
-		g_u = g_l;
-
-		a_l = a_t;
-		f_l = f_t;
-		g_l = g_t;
-		return (false);
-	}
-	// Interval Converged
-	else
-		return (true);
-}
-
-void GNormalDistributionsTransform::computeHessian(Eigen::Matrix<double, 6, 6> &hessian, float *trans_x, float *trans_y, float *trans_z, int points_num, Eigen::Matrix<double, 6, 1> &p)
-{
-	int *valid_points, *voxel_id, *starting_voxel_id;
-	int valid_voxel_num, valid_points_num;
-	//Radius Search
-	voxel_grid_.radiusSearch(trans_x, trans_y, trans_z, points_num, resolution_, INT_MAX, &valid_points, &starting_voxel_id, &voxel_id, &valid_voxel_num, &valid_points_num);
-
-	double *centroid = voxel_grid_.getCentroidList();
-	double *covariance = voxel_grid_.getCovarianceList();
-	double *inverse_covariance = voxel_grid_.getInverseCovarianceList();
-	int *points_per_voxel = voxel_grid_.getPointsPerVoxelList();
-	int voxel_num = voxel_grid_.getVoxelNum();
-
-	if (valid_points_num <= 0)
-		return;
-
-	//Update score gradient and hessian matrix
-	double *hessians, *point_gradients, *point_hessians;
-	
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&hessians, sizeof(double) * valid_points_num * 6 * 6));
-	rubis::sched::yield_gpu("59_cudaMalloc");
-	
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&point_gradients, sizeof(double) * valid_points_num * 3 * 6));
-	rubis::sched::yield_gpu("60_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&point_hessians, sizeof(double) * valid_points_num * 18 * 6));
-	rubis::sched::yield_gpu("61_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(hessians, 0, sizeof(double) * valid_points_num * 6 * 6));
-	rubis::sched::yield_gpu("62_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(point_gradients, 0, sizeof(double) * valid_points_num * 3 * 6));		
-	rubis::sched::yield_gpu("63_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(point_hessians, 0, sizeof(double) * valid_points_num * 18 * 6));
-	rubis::sched::yield_gpu("64_cudaMemset");
-
-	int block_x = (valid_points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_points_num;
-	int grid_x = (valid_points_num - 1) / block_x + 1;
-	dim3 grid;
-
-	rubis::sched::request_gpu();
-	computePointGradients0<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dj_ang_.buffer(),
-												point_gradients,
-												point_gradients + valid_points_num * 7,
-												point_gradients + valid_points_num * 14,
-												point_gradients + valid_points_num * 9,
-												point_gradients + valid_points_num * 15,
-												point_gradients + valid_points_num * 4,
-												point_gradients + valid_points_num * 10);
-	rubis::sched::yield_gpu("65_computePointGradients0");
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computePointGradients1<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dj_ang_.buffer(),
-												point_gradients + valid_points_num * 16,
-												point_gradients + valid_points_num * 5,
-												point_gradients + valid_points_num * 11,
-												point_gradients + valid_points_num * 17);
-	rubis::sched::yield_gpu("66_computePointGradients1");
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computePointHessian0<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dh_ang_.buffer(),
-												point_hessians + valid_points_num * 57, point_hessians + valid_points_num * 63, point_hessians + valid_points_num * 69,
-												point_hessians + valid_points_num * 75, point_hessians + valid_points_num * 58, point_hessians + valid_points_num * 81,
-												point_hessians + valid_points_num * 64, point_hessians + valid_points_num * 87, point_hessians + valid_points_num * 70,
-												point_hessians + valid_points_num * 93, point_hessians + valid_points_num * 59, point_hessians + valid_points_num * 99,
-												point_hessians + valid_points_num * 65, point_hessians + valid_points_num * 105, point_hessians + valid_points_num * 71);
-	rubis::sched::yield_gpu("67_computePointHessian0");
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computePointHessian1<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dh_ang_.buffer(),
-												point_hessians + valid_points_num * 76, point_hessians + valid_points_num * 82, point_hessians + valid_points_num * 88,
-												point_hessians + valid_points_num * 94, point_hessians + valid_points_num * 77, point_hessians + valid_points_num * 100,
-												point_hessians + valid_points_num * 83, point_hessians + valid_points_num * 106, point_hessians + valid_points_num * 89);
-	rubis::sched::yield_gpu("68_computePointHessian1");
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computePointHessian2<<<grid_x, block_x>>>(x_, y_, z_, points_number_,
-												valid_points, valid_points_num,
-												dh_ang_.buffer(),
-												point_hessians + valid_points_num * 95, point_hessians + valid_points_num * 101, point_hessians + valid_points_num * 107);
-	rubis::sched::yield_gpu("69_computePointHessian2");
-	checkCudaErrors(cudaGetLastError());
-
-	double *tmp_hessian;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&tmp_hessian, sizeof(double) * valid_voxel_num * 6));
-	rubis::sched::yield_gpu("70_cudaMalloc");
-
-	double *e_x_cov_x;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&e_x_cov_x, sizeof(double) * valid_voxel_num));
-	rubis::sched::yield_gpu("71_cudaMalloc");
-
-	double *cov_dxd_pi;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&cov_dxd_pi, sizeof(double) * valid_voxel_num * 3 * 6));
-	rubis::sched::yield_gpu("72_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	computeExCovX<<<grid_x, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-										starting_voxel_id, voxel_id, valid_points_num,
-										centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-										gauss_d1_, gauss_d2_,
-										e_x_cov_x,
-										inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num,
-										inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num,
-										inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num);
-	rubis::sched::yield_gpu("73_computeExCovX");
-	checkCudaErrors(cudaGetLastError());
-
-	grid.x = grid_x;
-	grid.y = 3;
-	grid.z = 6;
-
-	rubis::sched::request_gpu();
-	computeCovDxdPi<<<grid, block_x>>>(valid_points, starting_voxel_id, voxel_id, valid_points_num,
-											inverse_covariance, voxel_num,
-											gauss_d1_, gauss_d2_, point_gradients,
-											cov_dxd_pi, valid_voxel_num);
-	rubis::sched::yield_gpu("74_computeCovDxdPi");
-	checkCudaErrors(cudaGetLastError());
-
-	int block_x2 = (valid_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_voxel_num;
-	int grid_x2 = (valid_voxel_num - 1) / block_x2 + 1;
-
-	rubis::sched::request_gpu();
-	updateExCovX<<<grid_x2, block_x2>>>(e_x_cov_x, gauss_d2_, valid_voxel_num);
-	rubis::sched::yield_gpu("75_updateExCovX");
-	checkCudaErrors(cudaGetLastError());
-
-	grid.y = 6;
-	grid.z = 1;
-
-	rubis::sched::request_gpu();
-	computeHessianListS0<<<grid, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-												starting_voxel_id, voxel_id, valid_points_num,
-												centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-												inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num,
-												inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num,
-												inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num,
-												point_gradients,
-												tmp_hessian, valid_voxel_num);
-	
-	rubis::sched::yield_gpu("76_computeHessianListS0");
-	checkCudaErrors(cudaGetLastError());
-
-	grid.z = 6;
-
-	rubis::sched::request_gpu();
-	computeHessianListS1<<<grid, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-												starting_voxel_id, voxel_id, valid_points_num,
-												centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-												gauss_d1_, gauss_d2_, hessians,
-												e_x_cov_x, tmp_hessian, cov_dxd_pi,
-												point_gradients,
-												valid_voxel_num);
-	rubis::sched::yield_gpu("77_computeHessianListS1");
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeHessianListS2<<<grid, block_x>>>(trans_x, trans_y, trans_z, valid_points,
-												starting_voxel_id, voxel_id, valid_points_num,
-												centroid, centroid + voxel_num, centroid + 2 * voxel_num,
-												gauss_d1_, e_x_cov_x,
-												inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num,
-												inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num,
-												inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num,
-												point_hessians, hessians, valid_voxel_num);
-	rubis::sched::yield_gpu("78_computeHessianListS2");
-
-	checkCudaErrors(cudaGetLastError());
-
-
-	int full_size = valid_points_num;
-	int half_size = (full_size - 1) / 2 + 1;
-
-	while (full_size > 1) {
-		block_x = (half_size > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_size;
-		grid_x = (half_size - 1) / block_x + 1;
-
-		grid.x = grid_x;
-		grid.y = 6;
-		grid.z = 6;
-		matrixSum<<<grid_x, block_x>>>(hessians, full_size, half_size, 6, 6, valid_points_num);
-
-		full_size = half_size;
-		half_size = (full_size - 1) / 2 + 1;
-	}
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	MatrixDevice dhessian(6, 6, valid_points_num, hessians);
-	MatrixHost hhessian(6, 6);
-
-	hhessian.moveToHost(dhessian);
-
-	for (int i = 0; i < 6; i++) {
-		for (int j = 0; j < 6; j++) {
-			hessian(i, j) = hhessian(i, j);
-		}
-	}
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(hessians));
-	rubis::sched::yield_gpu("79_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(point_hessians));
-	rubis::sched::yield_gpu("80_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(point_gradients));
-	rubis::sched::yield_gpu("81_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(tmp_hessian));
-	rubis::sched::yield_gpu("82_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(e_x_cov_x));
-	rubis::sched::yield_gpu("83_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(cov_dxd_pi));
-	rubis::sched::yield_gpu("84_free");
-
-	if (valid_points != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_points));
-		rubis::sched::yield_gpu("85_free");
-	}
-
-	if (voxel_id != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(voxel_id));
-		rubis::sched::yield_gpu("86_free");
-	}
-
-	if (starting_voxel_id != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(starting_voxel_id));
-		rubis::sched::yield_gpu("87_free");
-	}
-
-	dhessian.memFree();
-}
-
-template <typename T>
-__global__ void gpuSum(T *input, int size, int half_size)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = idx; i < half_size; i += stride) {
-		if (i + half_size < size) {
-			input[i] += (half_size < size) ? input[i + half_size] : 0;
-		}
-	}
-}
-
-double GNormalDistributionsTransform::getFitnessScore(double max_range)
-{
-	double fitness_score = 0.0;
-
-	float *trans_x, *trans_y, *trans_z;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&trans_x, sizeof(float) * points_number_));
-	rubis::sched::yield_gpu("88_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&trans_y, sizeof(float) * points_number_));
-	rubis::sched::yield_gpu("89_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&trans_z, sizeof(float) * points_number_));
-	rubis::sched::yield_gpu("90_cudaMalloc");
-
-	transformPointCloud(x_, y_, z_, trans_x, trans_y, trans_z, points_number_, final_transformation_);
-	
-	int *valid_distance;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&valid_distance, sizeof(int) * points_number_));
-	rubis::sched::yield_gpu("91_cudaMalloc");
-
-	double *min_distance;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&min_distance, sizeof(double) * points_number_));
-	rubis::sched::yield_gpu("92_cudaMalloc");
-
-	voxel_grid_.nearestNeighborSearch(trans_x, trans_y, trans_z, points_number_, valid_distance, min_distance, max_range);
-
-	int size = points_number_;
-	int half_size;
-
-	while (size > 1) {
-		half_size = (size - 1) / 2 + 1;
-
-		int block_x = (half_size > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_size;
-		int grid_x = (half_size - 1) / block_x + 1;
-
-		rubis::sched::request_gpu();
-		gpuSum<double><<<grid_x, block_x>>>(min_distance, size, half_size);
-		rubis::sched::yield_gpu("93_gpuSum");
-
-		checkCudaErrors(cudaGetLastError());
-
-		rubis::sched::request_gpu();
-		gpuSum<int><<<grid_x, block_x>>>(valid_distance, size, half_size);
-		rubis::sched::yield_gpu("94_gpuSum");
-
-		checkCudaErrors(cudaGetLastError());
-
-		size = half_size;
-	}
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	int nr;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&nr, valid_distance, sizeof(int), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("95_dtoh");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&fitness_score, min_distance, sizeof(double), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("96_dtoh");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(trans_x));
-	rubis::sched::yield_gpu("97_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(trans_y));
-	rubis::sched::yield_gpu("98_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(trans_z));
-	rubis::sched::yield_gpu("99_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(valid_distance));
-	rubis::sched::yield_gpu("100_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(min_distance));
-	rubis::sched::yield_gpu("101_free");
-
-	if (nr > 0)
-		return (fitness_score / nr);
-
-	return DBL_MAX;
-}
-}
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/Registration.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/Registration.cu
deleted file mode 100644
index d7962776..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/Registration.cu
+++ /dev/null
@@ -1,624 +0,0 @@
-#include "ndt_gpu/Registration.h"
-#include "ndt_gpu/debug.h"
-#include <iostream>
-#include <sys/stat.h>
-#include "rubis_lib/sched.hpp"
-
-//using namespace rubis::sched;
-
-namespace gpu {
-GRegistration::GRegistration()
-{
-	max_iterations_ = 0;
-	x_ = y_ = z_ = NULL;
-	points_number_ = 0;
-
-	trans_x_ = trans_y_ = trans_z_ = NULL;
-
-	converged_ = false;
-	nr_iterations_ = 0;
-
-	transformation_epsilon_ = 0;
-	target_cloud_updated_ = true;
-	target_points_number_ = 0;
-
-	target_x_ = target_y_ = target_z_ = NULL;
-	is_copied_ = false;	
-}
-
-GRegistration::GRegistration(const GRegistration &other)
-{
-	transformation_epsilon_ = other.transformation_epsilon_;
-	max_iterations_ = other.max_iterations_;
-
-	//Original scanned point clouds
-	x_ = other.x_;
-	y_ = other.y_;
-	z_ = other.z_;
-
-	points_number_ = other.points_number_;
-
-	trans_x_ = other.trans_x_;
-	trans_y_ = other.trans_y_;
-	trans_z_ = other.trans_z_;
-
-	converged_ = other.converged_;
-
-	nr_iterations_ = other.nr_iterations_;
-	final_transformation_ = other.final_transformation_;
-	transformation_ = other.transformation_;
-	previous_transformation_ = other.previous_transformation_;
-
-	target_cloud_updated_ = other.target_cloud_updated_;
-
-	target_x_ = other.target_x_;
-	target_y_ = other.target_y_;
-	target_z_ = other.target_z_;
-
-	target_points_number_ = other.target_points_number_;
-	is_copied_ = true;
-}
-
-GRegistration::~GRegistration()
-{
-	if (!is_copied_) {
-		if (x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(x_));
-			rubis::sched::yield_gpu("102_free");
-			x_ = NULL;
-		}
-
-		if (y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(y_));
-			rubis::sched::yield_gpu("103_free");
-
-			y_ = NULL;
-		}
-
-		if (z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(z_));
-			rubis::sched::yield_gpu("104_free");
-	
-			z_ = NULL;
-		}
-
-		if (trans_x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_x_));
-			rubis::sched::yield_gpu("105_free");
-
-			trans_x_ = NULL;
-		}
-
-		if (trans_y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_y_));
-			rubis::sched::yield_gpu("106_free");
-
-			trans_y_ = NULL;
-		}
-
-		if (trans_z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_z_));
-			rubis::sched::yield_gpu("107_free");
-			
-			trans_z_ = NULL;
-		}
-
-		if (target_x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_x_));
-			rubis::sched::yield_gpu("108_free");
-			
-			target_x_ = NULL;
-		}
-
-		if (target_y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_y_));
-			rubis::sched::yield_gpu("109_free");
-
-			target_y_ = NULL;
-		}
-
-		if (target_z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_z_));
-			rubis::sched::yield_gpu("110_free");
-
-			target_z_ = NULL;
-		}
-	}
-}
-
-void GRegistration::setTransformationEpsilon(double trans_eps)
-{
-	transformation_epsilon_ = trans_eps;
-}
-
-double GRegistration::getTransformationEpsilon() const
-{
-	return transformation_epsilon_;
-}
-
-void GRegistration::setMaximumIterations(int max_itr)
-{
-	max_iterations_ = max_itr;
-}
-
-int GRegistration::getMaximumIterations() const
-{
-	return max_iterations_;
-}
-
-Eigen::Matrix<float, 4, 4> GRegistration::getFinalTransformation() const
-{
-	return final_transformation_;
-}
-
-int GRegistration::getFinalNumIteration() const
-{
-	return nr_iterations_;
-}
-
-bool GRegistration::hasConverged() const
-{
-	return converged_;
-}
-
-
-template <typename T>
-__global__ void convertInput(T *input, float *out_x, float *out_y, float *out_z, int point_num)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = idx; i < point_num; i += stride) {
-		T tmp = input[i];
-		out_x[i] = tmp.x;
-		out_y[i] = tmp.y;
-		out_z[i] = tmp.z;
-	}
-}
-
-void GRegistration::setInputSource(pcl::PointCloud<pcl::PointXYZI>::Ptr input)
-{
-	//Convert point cloud to float x, y, z
-	if (input->size() > 0) {
-		points_number_ = input->size();
-
-		pcl::PointXYZI *tmp;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZI) * points_number_));
-		rubis::sched::yield_gpu("111_cudaMalloc");
-		
-		pcl::PointXYZI *host_tmp = input->points.data();
-
-		// Pin the host buffer for accelerating the memory copy
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZI) * points_number_, cudaHostRegisterDefault));
-		rubis::sched::yield_gpu("112_htod");
-#endif
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZI) * points_number_, cudaMemcpyHostToDevice));
-		rubis::sched::yield_gpu("113_htod");
-		
-
-		if (x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(x_));
-			rubis::sched::yield_gpu("114_free");
-			x_ = NULL;
-		}
-
-		if (y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(y_));
-			rubis::sched::yield_gpu("115_free");
-			y_ = NULL;
-		}
-
-		if (z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(z_));
-			rubis::sched::yield_gpu("116_free");
-			z_ = NULL;
-		}
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&x_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("117_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&y_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("118_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&z_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("119_cudaMalloc");
-
-		int block_x = (points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_number_;
-		int grid_x = (points_number_ - 1) / block_x + 1;
-
-		rubis::sched::request_gpu();
-		convertInput<pcl::PointXYZI><<<grid_x, block_x>>>(tmp, x_, y_, z_, points_number_);
-		rubis::sched::yield_gpu("120_convertInput");
-
-		checkCudaErrors(cudaGetLastError());
-		checkCudaErrors(cudaDeviceSynchronize());
-
-
-		if (trans_x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_x_));
-			rubis::sched::yield_gpu("121_free");
-			trans_x_ = NULL;
-		}
-
-		if (trans_y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_y_));
-			rubis::sched::yield_gpu("122_free");
-			trans_y_ = NULL;
-		}
-
-		if (trans_z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_z_));
-			rubis::sched::yield_gpu("123_free");
-			trans_z_ = NULL;
-		}
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&trans_x_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("124_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&trans_y_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("125_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&trans_z_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("126_cudaMalloc");
-
-		// Initially, also copy scanned points to transformed buffers
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(trans_x_, x_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice));
-		rubis::sched::yield_gpu("127_dtod");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(trans_y_, y_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice));
-		rubis::sched::yield_gpu("128_dtod");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(trans_z_, z_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice));
-		rubis::sched::yield_gpu("129_dtod");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(tmp));
-		rubis::sched::yield_gpu("130_free");
-
-		// Unpin host buffer
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostUnregister(host_tmp));
-		rubis::sched::yield_gpu("131_free");
-#endif
-	}
-}
-
-void GRegistration::setInputSource(pcl::PointCloud<pcl::PointXYZ>::Ptr input)
-{
-	//Convert point cloud to float x, y, z
-	if (input->size() > 0) {
-		points_number_ = input->size();
-
-		pcl::PointXYZ *tmp;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZ) * points_number_));
-		rubis::sched::yield_gpu("132_cudaMalloc");
-
-		pcl::PointXYZ *host_tmp = input->points.data();
-
-		// Pin the host buffer for accelerating the memory copy
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZ) * points_number_, cudaHostRegisterDefault));
-		rubis::sched::yield_gpu("133_htod");
-#endif			
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZ) * points_number_, cudaMemcpyHostToDevice));
-		rubis::sched::yield_gpu("134_htod");
-
-		if (x_ != NULL) {			
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(x_));
-			rubis::sched::yield_gpu("135_free");
-			
-			x_ = NULL;
-		}
-
-		if (y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(y_));
-			rubis::sched::yield_gpu("136_free");
-			
-			y_ = NULL;
-		}
-
-		if (z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(z_));
-			rubis::sched::yield_gpu("137_free");
-
-			z_ = NULL;
-		}
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&x_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("138_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&y_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("139_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&z_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("140_cudaMalloc");
-
-		int block_x = (points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_number_;
-		int grid_x = (points_number_ - 1) / block_x + 1;
-
-		rubis::sched::request_gpu();
-		convertInput<pcl::PointXYZ><<<grid_x, block_x>>>(tmp, x_, y_, z_, points_number_);
-		rubis::sched::yield_gpu("141_convertInput");
-
-		checkCudaErrors(cudaGetLastError());
-		checkCudaErrors(cudaDeviceSynchronize());		
-
-		if (trans_x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_x_));
-			rubis::sched::yield_gpu("142_free");
-
-			trans_x_ = NULL;
-		}
-
-		if (trans_y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_y_));
-			rubis::sched::yield_gpu("143_free");
-
-			trans_y_ = NULL;
-		}
-
-		if (trans_z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(trans_z_));
-			rubis::sched::yield_gpu("144_free");
-
-			trans_z_ = NULL;
-		}
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&trans_x_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("145_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&trans_y_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("146_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&trans_z_, sizeof(float) * points_number_));
-		rubis::sched::yield_gpu("147_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(trans_x_, x_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice));		
-		rubis::sched::yield_gpu("148_dtod");
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(trans_y_, y_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice));
-		rubis::sched::yield_gpu("149_dtod");
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(trans_z_, z_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice));
-		rubis::sched::yield_gpu("150_dtod");
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(tmp));
-		rubis::sched::yield_gpu("151_free");
-
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostUnregister(host_tmp));
-		rubis::sched::yield_gpu("152_free");
-#endif
-	}
-}
-
-
-
-//Set input MAP data
-void GRegistration::setInputTarget(pcl::PointCloud<pcl::PointXYZI>::Ptr input)
-{
-	if (input->size() > 0) {
-		target_points_number_ = input->size();
-
-		pcl::PointXYZI *tmp;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZI) * target_points_number_));
-		rubis::sched::yield_gpu("153_cudaMalloc");
-
-		pcl::PointXYZI *host_tmp = input->points.data();
-
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZI) * target_points_number_, cudaHostRegisterDefault));
-		rubis::sched::yield_gpu("154_htod");
-#endif
-
-		rubis::sched::request_gpu();	
-		checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZI) * target_points_number_, cudaMemcpyHostToDevice));
-		rubis::sched::yield_gpu("155_htod");
-
-		if (target_x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_x_));
-			rubis::sched::yield_gpu("156_free");
-
-			target_x_ = NULL;
-		}
-
-		if (target_y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_y_));
-			rubis::sched::yield_gpu("157_free");
-
-			target_y_ = NULL;
-		}
-
-		if (target_z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_z_));
-			rubis::sched::yield_gpu("158_free");
-
-			target_z_ = NULL;
-		}
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&target_x_, sizeof(float) * target_points_number_));
-		rubis::sched::yield_gpu("159_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&target_y_, sizeof(float) * target_points_number_));
-		rubis::sched::yield_gpu("160_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&target_z_, sizeof(float) * target_points_number_));
-		rubis::sched::yield_gpu("161_cudaMalloc");
-
-		int block_x = (target_points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : target_points_number_;
-		int grid_x = (target_points_number_ - 1) / block_x + 1;
-
-		rubis::sched::request_gpu();
-		convertInput<pcl::PointXYZI><<<grid_x, block_x>>>(tmp, target_x_, target_y_, target_z_, target_points_number_);
-		rubis::sched::yield_gpu("162_convertInput");
-
-		checkCudaErrors(cudaGetLastError());
-		checkCudaErrors(cudaDeviceSynchronize());
-
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostUnregister(host_tmp));
-		rubis::sched::yield_gpu("163_free");
-#endif
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(tmp));
-		rubis::sched::yield_gpu("164_free");
-	}
-}
-
-void GRegistration::setInputTarget(pcl::PointCloud<pcl::PointXYZ>::Ptr input)
-{
-	if (input->size() > 0) {
-		target_points_number_ = input->size();
-
-		pcl::PointXYZ *tmp;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZ) * target_points_number_));
-		rubis::sched::yield_gpu("165_cudaMalloc");
-
-		pcl::PointXYZ *host_tmp = input->points.data();
-
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZ) * target_points_number_, cudaHostRegisterDefault));
-		rubis::sched::yield_gpu("166_htod");
-#endif
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZ) * target_points_number_, cudaMemcpyHostToDevice));
-		rubis::sched::yield_gpu("167_htod");
-
-		if (target_x_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_x_));
-			rubis::sched::yield_gpu("168_free");
-
-			target_x_ = NULL;
-		}
-
-		if (target_y_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_y_));
-			rubis::sched::yield_gpu("169_free");
-
-			target_y_ = NULL;
-		}
-
-		if (target_z_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(target_z_));
-			rubis::sched::yield_gpu("170_free");
-
-			target_z_ = NULL;
-		}
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&target_x_, sizeof(float) * target_points_number_));
-		rubis::sched::yield_gpu("171_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&target_y_, sizeof(float) * target_points_number_));
-		rubis::sched::yield_gpu("172_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&target_z_, sizeof(float) * target_points_number_));
-		rubis::sched::yield_gpu("173_cudaMalloc");
-
-		int block_x = (target_points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : target_points_number_;
-		int grid_x = (target_points_number_ - 1) / block_x + 1;
-
-		rubis::sched::request_gpu();
-		convertInput<pcl::PointXYZ><<<grid_x, block_x>>>(tmp, target_x_, target_y_, target_z_, target_points_number_);	
-		rubis::sched::yield_gpu("174_convertInput");
-
-		checkCudaErrors(cudaGetLastError());
-		checkCudaErrors(cudaDeviceSynchronize());
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(tmp));
-		rubis::sched::yield_gpu("175_free");
-#ifndef __aarch64__
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaHostUnregister(host_tmp));
-		rubis::sched::yield_gpu("176_free");
-#endif
-	}
-}
-
-void GRegistration::align(const Eigen::Matrix<float, 4, 4> &guess)
-{
-	converged_ = false;
-
-	final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix<float, 4, 4>::Identity();
-
-	computeTransformation(guess);
-}
-
-void GRegistration::computeTransformation(const Eigen::Matrix<float, 4, 4> &guess) {
-	printf("Unsupported by Registration\n");
-}
-}
-
-
-
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/SymmetricEigenSolver.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/SymmetricEigenSolver.cu
deleted file mode 100644
index cad83da7..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/SymmetricEigenSolver.cu
+++ /dev/null
@@ -1,90 +0,0 @@
-#include "ndt_gpu/SymmetricEigenSolver.h"
-#include "ndt_gpu/debug.h"
-#include "rubis_lib/sched.hpp"
-
-namespace gpu {
-
-SymmetricEigensolver3x3::SymmetricEigensolver3x3(int offset)
-{
-	offset_ = offset;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&buffer_, sizeof(double) * 18 * offset_));
-	rubis::sched::yield_gpu("177_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&maxAbsElement_, sizeof(double) * offset_));
-	rubis::sched::yield_gpu("178_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&norm_, sizeof(double) * offset_));
-	rubis::sched::yield_gpu("179_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&i02_, sizeof(int) * 2 * offset_));
-	rubis::sched::yield_gpu("180_cudaMalloc");
-
-	eigenvectors_ = NULL;
-	eigenvalues_ = NULL;
-	input_matrices_ = NULL;
-
-	is_copied_ = false;
-}
-
-void SymmetricEigensolver3x3::setInputMatrices(double *input_matrices)
-{
-	input_matrices_ = input_matrices;
-}
-
-void SymmetricEigensolver3x3::setEigenvectors(double *eigenvectors)
-{
-	eigenvectors_ = eigenvectors;
-}
-
-void SymmetricEigensolver3x3::setEigenvalues(double *eigenvalues)
-{
-	eigenvalues_ = eigenvalues;
-}
-
-double* SymmetricEigensolver3x3::getBuffer() const
-{
-	return buffer_;
-}
-
-void SymmetricEigensolver3x3::memFree()
-{
-	if (!is_copied_) {
-		if (buffer_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(buffer_));
-			rubis::sched::yield_gpu("181_free");
-			
-			buffer_ = NULL;
-		}
-
-		if (maxAbsElement_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(maxAbsElement_));
-			rubis::sched::yield_gpu("182_free");
-
-			maxAbsElement_ = NULL;
-		}
-
-		if (norm_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(norm_));
-			rubis::sched::yield_gpu("183_free");
-
-			norm_ = NULL;
-		}
-
-		if (i02_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(i02_));
-			rubis::sched::yield_gpu("184_free");
-
-			i02_ = NULL;
-		}
-	}
-}
-}
diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/VoxelGrid.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/VoxelGrid.cu
deleted file mode 100644
index 5d7def9f..00000000
--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/VoxelGrid.cu
+++ /dev/null
@@ -1,2125 +0,0 @@
-#include "ndt_gpu/VoxelGrid.h"
-#include "ndt_gpu/debug.h"
-#include "ndt_gpu/common.h"
-#include <math.h>
-#include <limits>
-#include <thrust/device_ptr.h>
-#include <thrust/device_vector.h>
-#include <thrust/copy.h>
-#include <thrust/scan.h>
-#include <thrust/fill.h>
-#include <inttypes.h>
-
-#include <vector>
-#include <cmath>
-
-#include <stdio.h>
-#include <sys/time.h>
-#include "rubis_lib/sched.hpp"
-
-#include "ndt_gpu/SymmetricEigenSolver.h"
-
-namespace gpu {
-
-GVoxelGrid::GVoxelGrid():
-	x_(NULL),
-	y_(NULL),
-	z_(NULL),
-	points_num_(0),
-	centroid_(NULL),
-	covariance_(NULL),
-	inverse_covariance_(NULL),
-	points_per_voxel_(NULL),
-	voxel_num_(0),
-	max_x_(FLT_MAX),
-	max_y_(FLT_MAX),
-	max_z_(FLT_MAX),
-	min_x_(FLT_MIN),
-	min_y_(FLT_MIN),
-	min_z_(FLT_MIN),
-	voxel_x_(0),
-	voxel_y_(0),
-	voxel_z_(0),
-	max_b_x_(0),
-	max_b_y_(0),
-	max_b_z_(0),
-	min_b_x_(0),
-	min_b_y_(0),
-	min_b_z_(0),
-	vgrid_x_(0),
-	vgrid_y_(0),
-	vgrid_z_(0),
-	min_points_per_voxel_(6),
-	starting_point_ids_(NULL),
-	point_ids_(NULL),
-	is_copied_(false)
-{
-
-};
-
-GVoxelGrid::GVoxelGrid(const GVoxelGrid &other)
-{
-	x_ = other.x_;
-	y_ = other.y_;
-	z_ = other.z_;
-
-	points_num_ = other.points_num_;
-
-	centroid_ = other.centroid_;
-	covariance_ = other.covariance_;
-	inverse_covariance_ = other.inverse_covariance_;
-	points_per_voxel_ = other.points_per_voxel_;
-
-	voxel_num_ = other.voxel_num_;
-	max_x_ = other.max_x_;
-	max_y_ = other.max_y_;
-	max_z_ = other.max_z_;
-
-	min_x_ = other.min_x_;
-	min_y_ = other.min_y_;
-	min_z_ = other.min_z_;
-
-	voxel_x_ = other.voxel_x_;
-	voxel_y_ = other.voxel_y_;
-	voxel_z_ = other.voxel_z_;
-
-	max_b_x_ = other.max_b_x_;
-	max_b_y_ = other.max_b_y_;
-	max_b_z_ = other.max_b_z_;
-
-	min_b_x_ = other.min_b_x_;
-	min_b_y_ = other.min_b_y_;
-	min_b_z_ = other.min_b_z_;
-
-	vgrid_x_ = other.vgrid_x_;
-	vgrid_y_ = other.vgrid_y_;
-	vgrid_z_ = other.vgrid_z_;
-
-	min_points_per_voxel_ = other.min_points_per_voxel_;
-
-	starting_point_ids_ = other.starting_point_ids_;
-	point_ids_ = other.point_ids_;
-
-
-	is_copied_ = true;
-}
-
-
-GVoxelGrid::~GVoxelGrid() {
-	if (!is_copied_) {
-
-		for (unsigned int i = 1; i < octree_centroids_.size(); i++) {
-			if (octree_centroids_[i] != NULL) {
-				rubis::sched::request_gpu();
-				checkCudaErrors(cudaFree(octree_centroids_[i]));
-				rubis::sched::yield_gpu("185_free");
-				
-				octree_centroids_[i] = NULL;
-			}
-			if (octree_points_per_node_[i] != NULL) {
-				rubis::sched::request_gpu();
-				checkCudaErrors(cudaFree(octree_points_per_node_[i]));
-				rubis::sched::yield_gpu("186_free");
-
-				octree_points_per_node_[i] = NULL;
-			}
-		}
-
-		octree_centroids_.clear();
-		octree_points_per_node_.clear();
-		octree_grid_size_.clear();
-
-		if (starting_point_ids_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(starting_point_ids_));
-			rubis::sched::yield_gpu("187_free");
-
-			starting_point_ids_ = NULL;
-		}
-
-		if (point_ids_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(point_ids_));
-			rubis::sched::yield_gpu("188_free");
-
-			point_ids_ = NULL;
-		}
-
-		if (centroid_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(centroid_));
-			rubis::sched::yield_gpu("189_free");
-			
-			centroid_ = NULL;
-		}
-
-		if (covariance_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(covariance_));
-			rubis::sched::yield_gpu("190_free");
-
-			covariance_ = NULL;
-		}
-
-		if (inverse_covariance_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(inverse_covariance_));
-			rubis::sched::yield_gpu("191_free");
-
-			inverse_covariance_ = NULL;
-		}
-
-		if (points_per_voxel_ != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(points_per_voxel_));
-			rubis::sched::yield_gpu("192_free");
-
-			points_per_voxel_ = NULL;
-		}
-	}
-}
-
-
-void GVoxelGrid::initialize()
-{
-	if (centroid_ != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(centroid_));
-		rubis::sched::yield_gpu("193_free");
-
-		centroid_ = NULL;
-	}
-
-	if (covariance_ != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(covariance_));
-		rubis::sched::yield_gpu("194_free");
-
-		covariance_ = NULL;
-	}
-
-	if (inverse_covariance_ != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(inverse_covariance_));
-		rubis::sched::yield_gpu("195_free");
-
-		inverse_covariance_ = NULL;
-	}
-
-	if (points_per_voxel_ != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(points_per_voxel_));
-		rubis::sched::yield_gpu("196_free");
-
-		points_per_voxel_ = NULL;
-	}
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&centroid_, sizeof(double) * 3 * voxel_num_));
-	rubis::sched::yield_gpu("197_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&covariance_, sizeof(double) * 9 * voxel_num_));
-	rubis::sched::yield_gpu("198_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&inverse_covariance_, sizeof(double) * 9 * voxel_num_));
-	rubis::sched::yield_gpu("199_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&points_per_voxel_, sizeof(int) * voxel_num_));
-	rubis::sched::yield_gpu("200_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(inverse_covariance_, 0, sizeof(double) * 9 * voxel_num_));
-	rubis::sched::yield_gpu("201_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(points_per_voxel_, 0, sizeof(int) * voxel_num_));
-	rubis::sched::yield_gpu("202_cudaMemset");
-
-	checkCudaErrors(cudaDeviceSynchronize());
-}
-
-int GVoxelGrid::getVoxelNum() const
-{
-	return voxel_num_;
-}
-
-
-float GVoxelGrid::getMaxX() const
-{
-	return max_x_;
-}
-float GVoxelGrid::getMaxY() const
-{
-	return max_y_;
-}
-float GVoxelGrid::getMaxZ() const
-{
-	return max_z_;
-}
-
-
-float GVoxelGrid::getMinX() const
-{
-	return min_x_;
-}
-float GVoxelGrid::getMinY() const
-{
-	return min_y_;
-}
-float GVoxelGrid::getMinZ() const
-{
-	return min_z_;
-}
-
-
-float GVoxelGrid::getVoxelX() const
-{
-	return voxel_x_;
-}
-float GVoxelGrid::getVoxelY() const
-{
-	return voxel_y_;
-}
-float GVoxelGrid::getVoxelZ() const
-{
-	return voxel_z_;
-}
-
-
-int GVoxelGrid::getMaxBX() const
-{
-	return max_b_x_;
-}
-int GVoxelGrid::getMaxBY() const
-{
-	return max_b_y_;
-}
-int GVoxelGrid::getMaxBZ() const
-{
-	return max_b_z_;
-}
-
-
-int GVoxelGrid::getMinBX() const
-{
-	return min_b_x_;
-}
-int GVoxelGrid::getMinBY() const
-{
-	return min_b_y_;
-}
-int GVoxelGrid::getMinBZ() const
-{
-	return min_b_z_;
-}
-
-
-int GVoxelGrid::getVgridX() const
-{
-	return vgrid_x_;
-}
-int GVoxelGrid::getVgridY() const
-{
-	return vgrid_y_;
-}
-int GVoxelGrid::getVgridZ() const
-{
-	return vgrid_z_;
-}
-
-
-void GVoxelGrid::setLeafSize(float voxel_x, float voxel_y, float voxel_z)
-{
-	voxel_x_ = voxel_x;
-	voxel_y_ = voxel_y;
-	voxel_z_ = voxel_z;
-}
-
-double* GVoxelGrid::getCentroidList() const
-{
-	return centroid_;
-}
-
-double* GVoxelGrid::getCovarianceList() const
-{
-	return covariance_;
-}
-
-double* GVoxelGrid::getInverseCovarianceList() const
-{
-	return inverse_covariance_;
-}
-
-int* GVoxelGrid::getPointsPerVoxelList() const
-{
-	return points_per_voxel_;
-}
-
-
-extern "C" __device__ int voxelId(float x, float y, float z,
-									float voxel_x, float voxel_y, float voxel_z,
-									int min_b_x, int min_b_y, int min_b_z,
-									int vgrid_x, int vgrid_y, int vgrid_z)
-{
-	int id_x = static_cast<int>(floorf(x / voxel_x) - static_cast<float>(min_b_x));
-	int id_y = static_cast<int>(floorf(y / voxel_y) - static_cast<float>(min_b_y));
-	int id_z = static_cast<int>(floorf(z / voxel_z) - static_cast<float>(min_b_z));
-
-	return (id_x + id_y * vgrid_x + id_z * vgrid_x * vgrid_y);
-}
-
-/* First step to compute centroids and covariances of voxels. */
-extern "C" __global__ void initCentroidAndCovariance(float *x, float *y, float *z, int *starting_point_ids, int *point_ids,
-														double *centroids, double *covariances, int voxel_num)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = idx; i < voxel_num; i += stride) {
-		MatrixDevice centr(3, 1, voxel_num, centroids + i);
-		MatrixDevice cov(3, 3, voxel_num, covariances + i);
-
-		double centr0, centr1, centr2;
-		double cov00, cov01, cov02, cov11, cov12, cov22;
-
-		centr0 = centr1 = centr2 = 0.0;
-		cov00 = cov11 = cov22 = 1.0;
-		cov01 = cov02 = cov12 = 0.0;
-
-		for (int j = starting_point_ids[i]; j < starting_point_ids[i + 1]; j++) {
-			int pid = point_ids[j];
-			double t_x = static_cast<double>(x[pid]);
-			double t_y = static_cast<double>(y[pid]);
-			double t_z = static_cast<double>(z[pid]);
-
-			centr0 += t_x;
-			centr1 += t_y;
-			centr2 += t_z;
-
-			cov00 += t_x * t_x;
-			cov01 += t_x * t_y;
-			cov02 += t_x * t_z;
-			cov11 += t_y * t_y;
-			cov12 += t_y * t_z;
-			cov22 += t_z * t_z;
-		}
-
-		centr(0) = centr0;
-		centr(1) = centr1;
-		centr(2) = centr2;
-
-		cov(0, 0) = cov00;
-		cov(0, 1) = cov01;
-		cov(0, 2) = cov02;
-		cov(1, 1) = cov11;
-		cov(1, 2) = cov12;
-		cov(2, 2) = cov22;
-	}
-}
-
-/* Update centroids of voxels. */
-extern "C" __global__ void updateVoxelCentroid(double *centroid, int *points_per_voxel, int voxel_num)
-{
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int vid = index; vid < voxel_num; vid += stride) {
-		MatrixDevice centr(3, 1, voxel_num, centroid + vid);
-		double points_num = static_cast<double>(points_per_voxel[vid]);
-
-		if (points_num > 0) {
-			centr /= points_num;
-		}
-	}
-}
-
-/* Update covariance of voxels. */
-extern "C" __global__ void updateVoxelCovariance(double *centroid, double *pt_sum, double *covariance, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int vid = index; vid < voxel_num; vid += stride) {
-		MatrixDevice centr(3, 1, voxel_num, centroid + vid);
-		MatrixDevice cov(3, 3, voxel_num, covariance + vid);
-		MatrixDevice pts(3, 1, voxel_num, pt_sum + vid);
-		double points_num = static_cast<double>(points_per_voxel[vid]);
-
-		double c0 = centr(0);
-		double c1 = centr(1);
-		double c2 = centr(2);
-		double p0 = pts(0);
-		double p1 = pts(1);
-		double p2 = pts(2);
-
-		points_per_voxel[vid] = (points_num < min_points_per_voxel) ? 0 : points_num;
-
-		if (points_num >= min_points_per_voxel) {
-
-			double mult = (points_num - 1.0) / points_num;
-
-			cov(0, 0) = ((cov(0, 0) - 2.0 * p0 * c0) / points_num + c0 * c0) * mult;
-			cov(0, 1) = ((cov(0, 1) - 2.0 * p0 * c1) / points_num + c0 * c1) * mult;
-			cov(0, 2) = ((cov(0, 2) - 2.0 * p0 * c2) / points_num + c0 * c2) * mult;
-			cov(1, 0) = cov(0, 1);
-			cov(1, 1) = ((cov(1, 1) - 2.0 * p1 * c1) / points_num + c1 * c1) * mult;
-			cov(1, 2) = ((cov(1, 2) - 2.0 * p1 * c2) / points_num + c1 * c2) * mult;
-			cov(2, 0) = cov(0, 2);
-			cov(2, 1) = cov(1, 2);
-			cov(2, 2) = ((cov(2, 2) - 2.0 * p2 * c2) / points_num + c2 * c2) * mult;
-		}
-	}
-}
-
-extern "C" __global__ void computeInverseEigenvectors(double *inverse_covariance, int *points_per_voxel, int voxel_num, double *eigenvectors, int min_points_per_voxel)
-{
-
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int vid = index; vid < voxel_num; vid += stride) {
-		if (points_per_voxel[vid] >= min_points_per_voxel) {
-			MatrixDevice icov(3, 3, voxel_num, inverse_covariance + vid);
-			MatrixDevice eigen_vectors(3, 3, voxel_num, eigenvectors + vid);
-
-			eigen_vectors.inverse(icov);
-		}
-
-		__syncthreads();
-	}
-}
-
-//eigen_vecs = eigen_vecs * eigen_val
-
-extern "C" __global__ void updateCovarianceS0(int *points_per_voxel, int voxel_num, double *eigenvalues, double *eigenvectors, int min_points_per_voxel)
-{
-
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int vid = index; vid < voxel_num; vid += stride) {
-		if (points_per_voxel[vid] >= min_points_per_voxel) {
-			MatrixDevice eigen_vectors(3, 3, voxel_num, eigenvectors + vid);
-
-			double eig_val0 = eigenvalues[vid];
-			double eig_val1 = eigenvalues[vid + voxel_num];
-			double eig_val2 = eigenvalues[vid + 2 * voxel_num];
-
-			eigen_vectors(0, 0) *= eig_val0;
-			eigen_vectors(1, 0) *= eig_val0;
-			eigen_vectors(2, 0) *= eig_val0;
-
-			eigen_vectors(0, 1) *= eig_val1;
-			eigen_vectors(1, 1) *= eig_val1;
-			eigen_vectors(2, 1) *= eig_val1;
-
-			eigen_vectors(0, 2) *= eig_val2;
-			eigen_vectors(1, 2) *= eig_val2;
-			eigen_vectors(2, 2) *= eig_val2;
-		}
-
-		__syncthreads();
-	}
-}
-
-//cov = new eigen_vecs * eigen_vecs transpose
-
-extern "C" __global__ void updateCovarianceS1(double *covariance, double *inverse_covariance, int *points_per_voxel, int voxel_num, double *eigenvectors, int min_points_per_voxel, int col)
-{
-
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int vid = index; vid < voxel_num; vid += stride) {
-		if (points_per_voxel[vid] >= min_points_per_voxel) {
-			MatrixDevice cov(3, 3, voxel_num, covariance + vid);
-			MatrixDevice icov(3, 3, voxel_num, inverse_covariance + vid);
-			MatrixDevice eigen_vectors(3, 3, voxel_num, eigenvectors + vid);
-
-			double tmp0 = icov(0, col);
-			double tmp1 = icov(1, col);
-			double tmp2 = icov(2, col);
-
-			cov(0, col) = eigen_vectors(0, 0) * tmp0 + eigen_vectors(0, 1) * tmp1 + eigen_vectors(0, 2) * tmp2;
-			cov(1, col) = eigen_vectors(1, 0) * tmp0 + eigen_vectors(1, 1) * tmp1 + eigen_vectors(1, 2) * tmp2;
-			cov(2, col) = eigen_vectors(2, 0) * tmp0 + eigen_vectors(2, 1) * tmp1 + eigen_vectors(2, 2) * tmp2;
-		}
-
-		__syncthreads();
-	}
-}
-
-extern "C" __global__ void computeInverseCovariance(double *covariance, double *inverse_covariance, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int vid = index; vid < voxel_num; vid += stride) {
-		if (points_per_voxel[vid] >= min_points_per_voxel) {
-			MatrixDevice cov(3, 3, voxel_num, covariance + vid);
-			MatrixDevice icov(3, 3, voxel_num, inverse_covariance + vid);
-
-			cov.inverse(icov);
-		}
-		__syncthreads();
-	}
-}
-
-
-template<typename T>
-__global__ void init(T *input, int size, int local_size)
-{
-	for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x) {
-		for (int j = 0; j < local_size; j++)
-			input[i + j * size] = 1;
-	}
-}
-
-extern "C" __global__ void initBoolean(bool *input, int size)
-{
-	for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x) {
-		input[i] = (i % 2 == 0) ? true : false;
-	}
-}
-
-/* Normalize input matrices to avoid overflow. */
-extern "C" __global__ void normalize(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel) {}
-			sv.normalizeInput(id);
-		__syncthreads();
-	}
-}
-
-/* Compute eigenvalues. Eigenvalues are arranged in increasing order.
- * (eigen(0) <= eigen(1) <= eigen(2). */
-extern "C" __global__ void computeEigenvalues(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel)
-			sv.computeEigenvalues(id);
-		__syncthreads();
-	}
-}
-
-/* First step to compute eigenvector 0 of covariance matrices. */
-extern "C" __global__ void computeEvec00(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel)
-			sv.computeEigenvector00(id);
-		__syncthreads();
-	}
-}
-
-/* Second step to compute eigenvector 0 of covariance matrices. */
-extern "C" __global__ void computeEvec01(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel)
-			sv.computeEigenvector01(id);
-		__syncthreads();
-	}
-}
-
-/* First step to compute eigenvector 1 of covariance matrices. */
-extern "C" __global__ void computeEvec10(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel)
-			sv.computeEigenvector10(id);
-		__syncthreads();
-	}
-}
-
-/* Second step to compute eigenvector 1 of covariance matrices. */
-extern "C" __global__ void computeEvec11(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel)
-			sv.computeEigenvector11(id);
-		__syncthreads();
-	}
-}
-
-/* Compute eigenvector 2 of covariance matrices. */
-extern "C" __global__ void computeEvec2(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel)
-			sv.computeEigenvector2(id);
-		__syncthreads();
-	}
-}
-
-/* Final step to compute eigenvalues. */
-extern "C" __global__ void updateEval(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel)
-			sv.updateEigenvalues(id);
-		__syncthreads();
-	}
-}
-
-/* Update eigenvalues in the case covariance matrix is nearly singular. */
-extern "C" __global__ void updateEval2(double *eigenvalues, int *points_per_voxel, int voxel_num, int min_points_per_voxel)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) {
-		if (points_per_voxel[id] >= min_points_per_voxel) {
-			MatrixDevice eigen_val(3, 1, voxel_num, eigenvalues + id);
-			double ev0 = eigen_val(0);
-			double ev1 = eigen_val(1);
-			double ev2 = eigen_val(2);
-
-
-			if (ev0 < 0 || ev1 < 0 || ev2 <= 0) {
-				points_per_voxel[id] = 0;
-				continue;
-			}
-
-			double min_cov_eigvalue = ev2 * 0.01;
-
-			if (ev0 < min_cov_eigvalue) {
-				ev0 = min_cov_eigvalue;
-
-				if (ev1 < min_cov_eigvalue) {
-					ev1 = min_cov_eigvalue;
-				}
-			}
-
-			eigen_val(0) = ev0;
-			eigen_val(1) = ev1;
-			eigen_val(2) = ev2;
-
-			__syncthreads();
-		}
-	}
-}
-
-void GVoxelGrid::computeCentroidAndCovariance()
-{
-	int block_x = (voxel_num_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : voxel_num_;
-	int grid_x = (voxel_num_ - 1) / block_x + 1;
-
-	rubis::sched::request_gpu();
-	initCentroidAndCovariance<<<grid_x, block_x>>>(x_, y_, z_, starting_point_ids_, point_ids_, centroid_, covariance_, voxel_num_);
-	rubis::sched::yield_gpu("203_initCentroidAndCovariance");
-
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	double *pt_sum;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&pt_sum, sizeof(double) * voxel_num_ * 3));
-	rubis::sched::yield_gpu("204_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(pt_sum, centroid_, sizeof(double) * voxel_num_ * 3, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("205_cudaMemcpy");
-
-	rubis::sched::request_gpu();
-	updateVoxelCentroid<<<grid_x, block_x>>>(centroid_, points_per_voxel_, voxel_num_);
-	rubis::sched::yield_gpu("206_updateVoxelCentroid");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	updateVoxelCovariance<<<grid_x, block_x>>>(centroid_, pt_sum, covariance_, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("207_updateVoxelCovariance");
-
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(pt_sum));
-	rubis::sched::yield_gpu("208_free");
-
-	double *eigenvalues_dev, *eigenvectors_dev;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&eigenvalues_dev, sizeof(double) * 3 * voxel_num_));
-	rubis::sched::yield_gpu("209_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&eigenvectors_dev, sizeof(double) * 9 * voxel_num_));
-	rubis::sched::yield_gpu("210_cudaMalloc");
-
-	// Solving eigenvalues and eigenvectors problem by the GPU.
-	SymmetricEigensolver3x3 sv(voxel_num_);
-
-	sv.setInputMatrices(covariance_);
-	sv.setEigenvalues(eigenvalues_dev);
-	sv.setEigenvectors(eigenvectors_dev);
-
-	rubis::sched::request_gpu();
-	normalize<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("211_normalize");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeEigenvalues<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("212_computeEigenvalues");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeEvec00<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("213_computeEvec00");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeEvec01<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("214_computeEvec01");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeEvec10<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("215_computeEvec10");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeEvec11<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("216_computeEvec11");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeEvec2<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("217_computeEvec2");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	updateEval<<<grid_x, block_x>>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("218_updateEval");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	updateEval2<<<grid_x, block_x>>>(eigenvalues_dev, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("219_updateEval2");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	computeInverseEigenvectors<<<grid_x, block_x>>>(inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_);
-	rubis::sched::yield_gpu("220_computeInverseEigenvectors");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	updateCovarianceS0<<<grid_x, block_x>>>(points_per_voxel_, voxel_num_, eigenvalues_dev, eigenvectors_dev, min_points_per_voxel_);
-	rubis::sched::yield_gpu("221_updateCovarianceS0");
-
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	updateCovarianceS1<<<grid_x, block_x>>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_, 0);
-	rubis::sched::yield_gpu("222_updateCovarianceS1");
-	
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	updateCovarianceS1<<<grid_x, block_x>>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_, 1);
-	rubis::sched::yield_gpu("223_updateCovarianceS1");
-	
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	updateCovarianceS1<<<grid_x, block_x>>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_, 2);
-	rubis::sched::yield_gpu("224_updateCovarianceS1");
-	
-	checkCudaErrors(cudaGetLastError());
-	
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	computeInverseCovariance<<<grid_x, block_x>>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, min_points_per_voxel_);
-	rubis::sched::yield_gpu("225_computeInverseCovariance");
-
-	checkCudaErrors(cudaGetLastError());
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	sv.memFree();
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(eigenvalues_dev));
-	rubis::sched::yield_gpu("226_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(eigenvectors_dev));
-	rubis::sched::yield_gpu("227_free");
-}
-
-//Input are supposed to be in device memory
-void GVoxelGrid::setInput(float *x, float *y, float *z, int points_num)
-{
-	if (points_num <= 0)
-		return;
-	x_ = x;
-	y_ = y;
-	z_ = z;
-	points_num_ = points_num;
-
-	findBoundaries();
-
-	voxel_num_ = vgrid_x_ * vgrid_y_ * vgrid_z_;
-
-	initialize();
-
-	scatterPointsToVoxelGrid();
-
-	computeCentroidAndCovariance();
-
-	buildOctree();
-}
-
-/* Find the largest coordinate values */
-extern "C"  __global__ void findMax(float *x, float *y, float *z, int full_size, int half_size)
-{
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = index; i < half_size; i += stride) {
-		x[i] = (i + half_size < full_size) ? ((x[i] >= x[i + half_size]) ? x[i] : x[i + half_size]) : x[i];
-		y[i] = (i + half_size < full_size) ? ((y[i] >= y[i + half_size]) ? y[i] : y[i + half_size]) : y[i];
-		z[i] = (i + half_size < full_size) ? ((z[i] >= z[i + half_size]) ? z[i] : z[i + half_size]) : z[i];
-	}
-}
-
-/* Find the smallest coordinate values */
-extern "C"  __global__ void findMin(float *x, float *y, float *z, int full_size, int half_size)
-{
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = index; i < half_size; i += stride) {
-		x[i] = (i + half_size < full_size) ? ((x[i] <= x[i + half_size]) ? x[i] : x[i + half_size]) : x[i];
-		y[i] = (i + half_size < full_size) ? ((y[i] <= y[i + half_size]) ? y[i] : y[i + half_size]) : y[i];
-		z[i] = (i + half_size < full_size) ? ((z[i] <= z[i + half_size]) ? z[i] : z[i + half_size]) : z[i];
-	}
-}
-
-
-void GVoxelGrid::findBoundaries()
-{
-	float *max_x, *max_y, *max_z, *min_x, *min_y, *min_z;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&max_x, sizeof(float) * points_num_));
-	rubis::sched::yield_gpu("228_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&max_y, sizeof(float) * points_num_));
-	rubis::sched::yield_gpu("229_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&max_z, sizeof(float) * points_num_));
-	rubis::sched::yield_gpu("230_cudaMalloc");
-	
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&min_x, sizeof(float) * points_num_));
-	rubis::sched::yield_gpu("231_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&min_y, sizeof(float) * points_num_));
-	rubis::sched::yield_gpu("232_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&min_z, sizeof(float) * points_num_));
-	rubis::sched::yield_gpu("233_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(max_x, x_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("234_dtod");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(max_y, y_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("235_dtod");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(max_z, z_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("236_dtod");
-	
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(min_x, x_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("237_dtod");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(min_y, y_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("238_dtod");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(min_z, z_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("239_dtod");
-
-	int points_num = points_num_;
-
-	while (points_num > 1) {
-		int half_points_num = (points_num - 1) / 2 + 1;
-		int block_x = (half_points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_points_num;
-		int grid_x = (half_points_num - 1) / block_x + 1;
-
-		rubis::sched::request_gpu();
-		findMax<<<grid_x, block_x>>>(max_x, max_y, max_z, points_num, half_points_num);
-		rubis::sched::yield_gpu("240_findMax");
-
-		checkCudaErrors(cudaGetLastError());
-
-		rubis::sched::request_gpu();
-		findMin<<<grid_x, block_x>>>(min_x, min_y, min_z, points_num, half_points_num);
-		rubis::sched::yield_gpu("241_findMin");
-
-		checkCudaErrors(cudaGetLastError());
-
-		points_num = half_points_num;
-	}
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&max_x_, max_x, sizeof(float), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("242_dtoh");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&max_y_, max_y, sizeof(float), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("243_dtoh");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&max_z_, max_z, sizeof(float), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("244_dtoh");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&min_x_, min_x, sizeof(float), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("245_dtoh");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&min_y_, min_y, sizeof(float), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("246_dtoh");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(&min_z_, min_z, sizeof(float), cudaMemcpyDeviceToHost));
-	rubis::sched::yield_gpu("247_dtoh");
-
-	max_b_x_ = static_cast<int> (floor(max_x_ / voxel_x_));
-	max_b_y_ = static_cast<int> (floor(max_y_ / voxel_y_));
-	max_b_z_ = static_cast<int> (floor(max_z_ / voxel_z_));
-
-	min_b_x_ = static_cast<int> (floor(min_x_ / voxel_x_));
-	min_b_y_ = static_cast<int> (floor(min_y_ / voxel_y_));
-	min_b_z_ = static_cast<int> (floor(min_z_ / voxel_z_));
-
-	vgrid_x_ = max_b_x_ - min_b_x_ + 1;
-	vgrid_y_ = max_b_y_ - min_b_y_ + 1;
-	vgrid_z_ = max_b_z_ - min_b_z_ + 1;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(max_x));
-	rubis::sched::yield_gpu("248_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(max_y));
-	rubis::sched::yield_gpu("249_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(max_z));
-	rubis::sched::yield_gpu("250_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(min_x));
-	rubis::sched::yield_gpu("251_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(min_y));
-	rubis::sched::yield_gpu("252_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(min_z));
-	rubis::sched::yield_gpu("253_free");
-}
-
-/* Find indexes idx, idy and idz of candidate voxels */
-extern "C"  __global__ void findBoundariesOfCandidateVoxels(float *x, float *y, float *z,
-																float radius, int points_num,
-																float voxel_x, float voxel_y, float voxel_z,
-																int max_b_x, int max_b_y, int max_b_z,
-																int min_b_x, int min_b_y, int min_b_z,
-																int *max_vid_x, int *max_vid_y, int *max_vid_z,
-																int *min_vid_x, int *min_vid_y, int *min_vid_z,
-																int *candidate_voxel_per_point)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = id; i < points_num; i += stride) {
-		float t_x = x[i];
-		float t_y = y[i];
-		float t_z = z[i];
-
-		int max_id_x = static_cast<int>(floorf((t_x + radius) / voxel_x));
-		int max_id_y = static_cast<int>(floorf((t_y + radius) / voxel_y));
-		int max_id_z = static_cast<int>(floorf((t_z + radius) / voxel_z));
-
-		int min_id_x = static_cast<int>(floorf((t_x - radius) / voxel_x));
-		int min_id_y = static_cast<int>(floorf((t_y - radius) / voxel_y));
-		int min_id_z = static_cast<int>(floorf((t_z - radius) / voxel_z));
-
-		/* Find intersection of the cube containing
-		 * the NN sphere of the point and the voxel grid
-		 */
-		max_id_x = (max_id_x > max_b_x) ? max_b_x - min_b_x : max_id_x - min_b_x;
-		max_id_y = (max_id_y > max_b_y) ? max_b_y - min_b_y : max_id_y - min_b_y;
-		max_id_z = (max_id_z > max_b_z) ? max_b_z - min_b_z : max_id_z - min_b_z;
-
-		min_id_x = (min_id_x < min_b_x) ? 0 : min_id_x - min_b_x;
-		min_id_y = (min_id_y < min_b_y) ? 0 : min_id_y - min_b_y;
-		min_id_z = (min_id_z < min_b_z) ? 0 : min_id_z - min_b_z;
-
-		int vx = max_id_x - min_id_x + 1;
-		int vy = max_id_y - min_id_y + 1;
-		int vz = max_id_z - min_id_z + 1;
-
-		candidate_voxel_per_point[i] = (vx > 0 && vy > 0 && vz > 0) ? vx * vy * vz : 0;
-
-		max_vid_x[i] = max_id_x;
-		max_vid_y[i] = max_id_y;
-		max_vid_z[i] = max_id_z;
-
-		min_vid_x[i] = min_id_x;
-		min_vid_y[i] = min_id_y;
-		min_vid_z[i] = min_id_z;
-	}
-}
-
-/* Write id of valid points to the output buffer */
-extern "C"  __global__ void collectValidPoints(int *valid_points_mark, int *valid_points_id, int *valid_points_location, int points_num)
-{
-	for (int index = threadIdx.x + blockIdx.x * blockDim.x; index < points_num; index += blockDim.x * gridDim.x) {
-		if (valid_points_mark[index] != 0) {
-			valid_points_id[valid_points_location[index]] = index;
-		}
-	}
-}
-
-/* Compute the global index of candidate voxels.
- * global index = idx + idy * grid size x + idz * grid_size x * grid size y */
-extern "C"  __global__ void updateCandidateVoxelIds(int points_num,
-													int vgrid_x, int vgrid_y, int vgrid_z,
-													int *max_vid_x, int *max_vid_y, int *max_vid_z,
-													int *min_vid_x, int *min_vid_y, int *min_vid_z,
-													int *starting_voxel_id,
-													int *candidate_voxel_id)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = id; i < points_num; i += stride) {
-		int max_id_x = max_vid_x[i];
-		int max_id_y = max_vid_y[i];
-		int max_id_z = max_vid_z[i];
-
-		int min_id_x = min_vid_x[i];
-		int min_id_y = min_vid_y[i];
-		int min_id_z = min_vid_z[i];
-
-		int write_location = starting_voxel_id[i];
-
-		for (int j = min_id_x; j <= max_id_x; j++) {
-			for (int k = min_id_y; k <= max_id_y; k++) {
-				for (int l = min_id_z; l <= max_id_z; l++) {
-					candidate_voxel_id[write_location] = j + k * vgrid_x + l * vgrid_x * vgrid_y;
-					write_location++;
-				}
-			}
-		}
-	}
-}
-
-
-/* Find out which voxels are really inside the radius.
- * This is done by comparing the distance between the centroid
- * of the voxel and the query point with the radius.
- *
- * The valid_voxel_mark store the result of the inspection, which is 0
- * if the centroid is outside the radius and 1 otherwise.
- *
- * The valid_points_mark store the status of the inspection per point.
- * It is 0 if there is no voxels in the candidate list is truly a neighbor
- * of the point, and 1 otherwise.
- *
- * The valid_voxel_count store the number of true neighbor voxels.
- */
-extern "C" __global__ void inspectCandidateVoxels(float *x, float *y, float *z,
-													float radius, int max_nn, int points_num,
-													double *centroid, int *points_per_voxel, int offset,
-													int *starting_voxel_id, int *candidate_voxel_id,
-													int *valid_voxel_mark, int *valid_voxel_count, int *valid_points_mark)
-{
-	int id = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = id; i < points_num; i += stride) {
-		double t_x = static_cast<double>(x[i]);
-		double t_y = static_cast<double>(y[i]);
-		double t_z = static_cast<double>(z[i]);
-
-		int nn = 0;
-		for (int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1] && nn <= max_nn; j++) {
-			int point_num = points_per_voxel[candidate_voxel_id[j]];
-			MatrixDevice centr(3, 1, offset, centroid + candidate_voxel_id[j]);
-
-			double centroid_x = (point_num > 0) ? (t_x - centr(0)) : radius + 1;
-			double centroid_y = (point_num > 0) ? (t_y - centr(1)) : 0;
-			double centroid_z = (point_num > 0) ? (t_z - centr(2)) : 0;
-
-			bool res = (norm3d(centroid_x, centroid_y, centroid_z) <= radius);
-
-			valid_voxel_mark[j] = (res) ? 1 : 0;
-			nn += (res) ? 1 : 0;
-		}
-
-		valid_voxel_count[i] = nn;
-		valid_points_mark[i] = (nn > 0) ? 1 : 0;
-
-		__syncthreads();
-	}
-}
-
-/* Write the id of valid voxels to the output buffer */
-extern "C"  __global__ void collectValidVoxels(int *valid_voxels_mark, int *candidate_voxel_id, int *output, int *writing_location, int candidate_voxel_num)
-{
-	for (int index = threadIdx.x + blockIdx.x * blockDim.x; index < candidate_voxel_num; index += blockDim.x * gridDim.x) {
-		if (valid_voxels_mark[index] == 1) {
-			output[writing_location[index]] = candidate_voxel_id[index];
-		}
-	}
-}
-
-/* Write the number of valid voxel per point to the output buffer */
-extern "C" __global__ void collectValidVoxelCount(int *input_valid_voxel_count, int *output_valid_voxel_count, int *writing_location, int points_num)
-{
-	for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < points_num; id += blockDim.x * gridDim.x) {
-		if (input_valid_voxel_count[id] != 0)
-			output_valid_voxel_count[writing_location[id]] = input_valid_voxel_count[id];
-	}
-}
-
-template <typename T>
-void GVoxelGrid::ExclusiveScan(T *input, int ele_num, T *sum)
-{
-	thrust::device_ptr<T> dev_ptr(input);
-
-	rubis::sched::request_gpu();
-	thrust::exclusive_scan(dev_ptr, dev_ptr + ele_num, dev_ptr);
-	rubis::sched::yield_gpu("254_exclusive_scan");
-
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	*sum = *(dev_ptr + ele_num - 1);
-}
-
-template <typename T>
-void GVoxelGrid::ExclusiveScan(T *input, int ele_num)
-{
-	thrust::device_ptr<T> dev_ptr(input);
-
-	rubis::sched::request_gpu();
-	thrust::exclusive_scan(dev_ptr, dev_ptr + ele_num, dev_ptr);
-	rubis::sched::yield_gpu("255_exclusive_scan");
-
-	checkCudaErrors(cudaDeviceSynchronize());
-}
-
-void GVoxelGrid::radiusSearch(float *qx, float *qy, float *qz, int points_num, float radius, int max_nn,
-										int **valid_points, int **starting_voxel_id, int **valid_voxel_id,
-										int *valid_voxel_num, int *valid_points_num)
-{
-	//Testing input query points
-	int block_x = (points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_num;
-	int grid_x = (points_num - 1) / block_x + 1;
-
-	//Boundaries of candidate voxels per points
-	int *max_vid_x, *max_vid_y, *max_vid_z;
-	int *min_vid_x, *min_vid_y, *min_vid_z;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&max_vid_x, sizeof(int) * points_num));
-	rubis::sched::yield_gpu("256_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&max_vid_y, sizeof(int) * points_num));
-	rubis::sched::yield_gpu("257_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&max_vid_z, sizeof(int) * points_num));
-	rubis::sched::yield_gpu("258_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&min_vid_x, sizeof(int) * points_num));
-	rubis::sched::yield_gpu("259_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&min_vid_y, sizeof(int) * points_num));
-	rubis::sched::yield_gpu("260_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&min_vid_z, sizeof(int) * points_num));
-	rubis::sched::yield_gpu("261_cudaMalloc");
-
-	//Determine the number of candidate voxel per points
-	int *candidate_voxel_num_per_point;
-	int total_candidate_voxel_num;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&candidate_voxel_num_per_point, sizeof(int) * (points_num + 1)));
-	rubis::sched::yield_gpu("262_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	findBoundariesOfCandidateVoxels<<<grid_x, block_x>>>(qx, qy, qz, radius, points_num,
-															voxel_x_, voxel_y_, voxel_z_,
-															max_b_x_, max_b_y_, max_b_z_,
-															min_b_x_, min_b_y_, min_b_z_,
-															max_vid_x, max_vid_y, max_vid_z,
-															min_vid_x, min_vid_y, min_vid_z,
-															candidate_voxel_num_per_point);
-	rubis::sched::yield_gpu("263_findBoundariesOfCandidateVoxels");
-
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-	//Total candidate voxel num is determined by an exclusive scan on candidate_voxel_num_per_point
-	ExclusiveScan(candidate_voxel_num_per_point, points_num + 1, &total_candidate_voxel_num);
-
-	if (total_candidate_voxel_num <= 0) {
-		std::cout << "No candidate voxel was found. Exiting..." << std::endl;
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_x));
-		rubis::sched::yield_gpu("264_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_y));
-		rubis::sched::yield_gpu("265_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_z));
-		rubis::sched::yield_gpu("266_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_x));
-		rubis::sched::yield_gpu("267_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_y));
-		rubis::sched::yield_gpu("268_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_z));
-		rubis::sched::yield_gpu("269_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(candidate_voxel_num_per_point));
-		rubis::sched::yield_gpu("270_free");
-
-		valid_points = NULL;
-		starting_voxel_id = NULL;
-		valid_voxel_id = NULL;
-
-		*valid_voxel_num = 0;
-		*valid_points_num = 0;
-
-		return;
-	}
-
-
-	//Determine the voxel id of candidate voxels
-	int *candidate_voxel_id;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&candidate_voxel_id, sizeof(int) * total_candidate_voxel_num));
-	rubis::sched::yield_gpu("271_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	updateCandidateVoxelIds<<<grid_x, block_x>>>(points_num, vgrid_x_, vgrid_y_, vgrid_z_,
-													max_vid_x, max_vid_y, max_vid_z,
-													min_vid_x, min_vid_y, min_vid_z,
-													candidate_voxel_num_per_point, candidate_voxel_id);
-	rubis::sched::yield_gpu("272_updateCandidateVoxelIds");
-	
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	//Go through the candidate voxel id list and find out which voxels are really inside the radius
-	int *valid_voxel_mark;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&valid_voxel_mark, sizeof(int) * total_candidate_voxel_num));
-	rubis::sched::yield_gpu("273_cudaMalloc");
-
-	int *valid_voxel_count;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&valid_voxel_count, sizeof(int) * (points_num + 1)));
-	rubis::sched::yield_gpu("274_cudaMalloc");
-
-	int *valid_points_mark;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&valid_points_mark, sizeof(int) * points_num));
-	rubis::sched::yield_gpu("275_cudaMalloc");
-
-	block_x = (total_candidate_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : total_candidate_voxel_num;
-	grid_x = (total_candidate_voxel_num - 1) / block_x + 1;
-
-	///CHECK VALID VOXEL COUNT AGAIN
-	
-	rubis::sched::request_gpu();
-	inspectCandidateVoxels<<<grid_x, block_x>>>(qx, qy, qz, radius, max_nn, points_num,
-													centroid_, points_per_voxel_, voxel_num_,
-													candidate_voxel_num_per_point, candidate_voxel_id,
-													valid_voxel_mark, valid_voxel_count, valid_points_mark);
-	rubis::sched::yield_gpu("276_inspectCandidateVoxels");
-
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	//Collect valid points
-	int *valid_points_location;
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&valid_points_location, sizeof(int) * (points_num + 1)));
-	rubis::sched::yield_gpu("277_cudaMalloc");
-	
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(valid_points_location, 0, sizeof(int) * (points_num + 1)));
-	rubis::sched::yield_gpu("278_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(valid_points_location, valid_points_mark, sizeof(int) * points_num, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("279_dtod");
-
-	//Writing location to the output buffer is determined by an exclusive scan
-	ExclusiveScan(valid_points_location, points_num + 1, valid_points_num);
-
-	if (*valid_points_num <= 0) {
-		//std::cout << "No valid point was found. Exiting..." << std::endl;
-		std::cout << "No valid point was found. Exiting...: " << *valid_points_num << std::endl;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_x));
-		rubis::sched::yield_gpu("280_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_y));
-		rubis::sched::yield_gpu("281_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_z));
-		rubis::sched::yield_gpu("282_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_x));
-		rubis::sched::yield_gpu("283_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_y));
-		rubis::sched::yield_gpu("284_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_z));
-		rubis::sched::yield_gpu("285_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(candidate_voxel_num_per_point));
-		rubis::sched::yield_gpu("286_free");
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(candidate_voxel_id));
-		rubis::sched::yield_gpu("287_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_voxel_mark));
-		rubis::sched::yield_gpu("288_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_voxel_count));
-		rubis::sched::yield_gpu("289_free");
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_points_mark));
-		rubis::sched::yield_gpu("290_free");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_points_location));
-		rubis::sched::yield_gpu("291_free");
-
-		valid_points = NULL;
-		starting_voxel_id = NULL;
-		valid_voxel_id = NULL;
-
-		*valid_voxel_num = 0;
-		*valid_points_num = 0;
-
-		return;
-	}
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(valid_points, sizeof(int) * (*valid_points_num)));
-	rubis::sched::yield_gpu("292_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	collectValidPoints<<<grid_x, block_x>>>(valid_points_mark, *valid_points, valid_points_location, points_num);
-	rubis::sched::yield_gpu("293_collectValidPoints");
-
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(starting_voxel_id, sizeof(int) * (*valid_points_num + 1)));
-	rubis::sched::yield_gpu("294_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	collectValidVoxelCount<<<grid_x, block_x>>>(valid_voxel_count, *starting_voxel_id, valid_points_location, points_num);
-	rubis::sched::yield_gpu("295_collectValidVoxelCount");
-	
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	//Determine the starting location of voxels per points in the valid points list
-	ExclusiveScan(*starting_voxel_id, *valid_points_num + 1, valid_voxel_num);
-
-	//Collect valid voxels
-	int *valid_voxel_location;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&valid_voxel_location, sizeof(int) * (total_candidate_voxel_num + 1)));
-	rubis::sched::yield_gpu("296_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(valid_voxel_location, valid_voxel_mark, sizeof(int) * total_candidate_voxel_num, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("297_dtod");
-
-	ExclusiveScan(valid_voxel_location, total_candidate_voxel_num + 1, valid_voxel_num);
-
-	if (*valid_voxel_num <= 0) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_x));
-		rubis::sched::yield_gpu("298_free");
-
-		max_vid_x = NULL;
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_y));
-		rubis::sched::yield_gpu("299_free");
-
-		max_vid_y = NULL;
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(max_vid_z));
-		rubis::sched::yield_gpu("300_free");
-
-		max_vid_z = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_x));
-		rubis::sched::yield_gpu("301_free");
-
-		min_vid_x = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_y));
-		rubis::sched::yield_gpu("302_free");
-
-		min_vid_y = NULL;
-		
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(min_vid_z));
-		rubis::sched::yield_gpu("303_free");
-
-		min_vid_z = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(candidate_voxel_num_per_point));
-		rubis::sched::yield_gpu("304_free");
-
-		candidate_voxel_num_per_point = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(candidate_voxel_id));
-		rubis::sched::yield_gpu("305_free");
-
-		candidate_voxel_id = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_voxel_mark));
-		rubis::sched::yield_gpu("306_free");
-
-		valid_voxel_mark = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_voxel_count));
-		rubis::sched::yield_gpu("307_free");
-
-		valid_voxel_count = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_points_mark));
-		rubis::sched::yield_gpu("308_free");
-
-		valid_points_mark = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_points_location));
-		rubis::sched::yield_gpu("309_free");
-
-		valid_points_location = NULL;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(valid_voxel_location));
-		rubis::sched::yield_gpu("310_free");
-
-		valid_voxel_location = NULL;
-
-		valid_points = NULL;
-		starting_voxel_id = NULL;
-		valid_voxel_id = NULL;
-
-		*valid_voxel_num = 0;
-		*valid_points_num = 0;
-	}
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(valid_voxel_id, sizeof(int) * (*valid_voxel_num)));
-	rubis::sched::yield_gpu("311_cudaMalloc");
-
-	block_x = (total_candidate_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : total_candidate_voxel_num;
-	grid_x = (total_candidate_voxel_num - 1) / block_x + 1;
-
-	rubis::sched::request_gpu();
-	collectValidVoxels<<<grid_x, block_x>>>(valid_voxel_mark, candidate_voxel_id, *valid_voxel_id, valid_voxel_location, total_candidate_voxel_num);
-	rubis::sched::yield_gpu("312_collectValidVoxels");
-
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(max_vid_x));
-	rubis::sched::yield_gpu("313_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(max_vid_y));
-	rubis::sched::yield_gpu("314_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(max_vid_z));
-	rubis::sched::yield_gpu("315_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(min_vid_x));
-	rubis::sched::yield_gpu("316_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(min_vid_y));
-	rubis::sched::yield_gpu("317_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(min_vid_z));
-	rubis::sched::yield_gpu("318_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(candidate_voxel_num_per_point));
-	rubis::sched::yield_gpu("319_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(candidate_voxel_id));
-	rubis::sched::yield_gpu("320_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(valid_voxel_mark));
-	rubis::sched::yield_gpu("321_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(valid_points_mark));
-	rubis::sched::yield_gpu("322_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(valid_voxel_count));
-	rubis::sched::yield_gpu("323_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(valid_points_location));
-	rubis::sched::yield_gpu("324_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(valid_voxel_location));
-	rubis::sched::yield_gpu("325_free");
-}
-
-/* Build parent nodes from child nodes of the octree */
-extern "C" __global__ void buildParent(double *child_centroids, int *points_per_child,
-										int child_grid_x, int child_grid_y, int child_grid_z, int child_num,
-										double *parent_centroids, int *points_per_parent,
-										int parent_grid_x, int parent_grid_y, int parent_grid_z)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int idy = threadIdx.y + blockIdx.y * blockDim.y;
-	int idz = threadIdx.z + blockIdx.z * blockDim.z;
-
-	if (idx < parent_grid_x && idy < parent_grid_y && idz < parent_grid_z) {
-		int parent_idx = idx + idy * parent_grid_x + idz * parent_grid_x * parent_grid_y;
-		MatrixDevice parent_centr(3, 1, parent_grid_x * parent_grid_y * parent_grid_z, parent_centroids + parent_idx);
-		double pc0, pc1, pc2;
-		int points_num = 0;
-		double dpoints_num;
-
-		pc0 = 0.0;
-		pc1 = 0.0;
-		pc2 = 0.0;
-
-		for (int i = idx * 2; i < idx * 2 + 2 && i < child_grid_x; i++) {
-			for (int j = idy * 2; j < idy * 2 + 2 && j < child_grid_y; j++) {
-				for (int k = idz * 2; k < idz * 2 + 2 && k < child_grid_z; k++) {
-					int child_idx = i + j * child_grid_x + k * child_grid_x * child_grid_y;
-					MatrixDevice child_centr(3, 1, child_num, child_centroids + child_idx);
-					int child_points = points_per_child[child_idx];
-					double dchild_points = static_cast<double>(child_points);
-
-
-					pc0 += (child_points > 0) ? dchild_points * child_centr(0) : 0.0;
-					pc1 += (child_points > 0) ? dchild_points * child_centr(1) : 0.0;
-					pc2 += (child_points > 0) ? dchild_points * child_centr(2) : 0.0;
-					points_num += (child_points > 0) ? child_points : 0;
-
-					__syncthreads();
-				}
-			}
-		}
-
-		dpoints_num = static_cast<double>(points_num);
-
-		parent_centr(0) = (points_num <= 0) ? DBL_MAX : pc0 / dpoints_num;
-		parent_centr(1) = (points_num <= 0) ? DBL_MAX : pc1 / dpoints_num;
-		parent_centr(2) = (points_num <= 0) ? DBL_MAX : pc2 / dpoints_num;
-		points_per_parent[parent_idx] = points_num;
-	}
-}
-
-/* Compute the number of points per voxel using atomicAdd */
-extern "C"  __global__ void insertPointsToGrid(float *x, float *y, float *z, int points_num,
-												int *points_per_voxel, int voxel_num,
-												int vgrid_x, int vgrid_y, int vgrid_z,
-												float voxel_x, float voxel_y, float voxel_z,
-												int min_b_x, int min_b_y, int min_b_z)
-{
-	int index = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = index; i < points_num; i += stride) {
-		float t_x = x[i];
-		float t_y = y[i];
-		float t_z = z[i];
-		int voxel_id = voxelId(t_x, t_y, t_z, voxel_x, voxel_y, voxel_z, min_b_x, min_b_y, min_b_z, vgrid_x, vgrid_y, vgrid_z);
-
-		// Update number of points in the voxel
-		int ptr_increment = (voxel_id < voxel_num) * voxel_id; // if (voxel_id < voxel_num), then use voxel_id
-		int incremental_value = (voxel_id < voxel_num);
-		//atomicAdd(points_per_voxel + voxel_id, 1);
-		atomicAdd(points_per_voxel + ptr_increment, incremental_value);
-	}
-}
-
-/* Rearrange points to locations corresponding to voxels */
-extern "C" __global__ void scatterPointsToVoxels(float *x, float *y, float *z, int points_num, int voxel_num,
-													float voxel_x, float voxel_y, float voxel_z,
-													int min_b_x, int min_b_y, int min_b_z,
-													int vgrid_x, int vgrid_y, int vgrid_z,
-													int *writing_locations, int *point_ids)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = idx; i < points_num; i += stride) {
-		int voxel_id = voxelId(x[i], y[i], z[i], voxel_x, voxel_y, voxel_z,
-								min_b_x, min_b_y, min_b_z, vgrid_x, vgrid_y, vgrid_z);
-
-		int ptr_increment = (voxel_id < voxel_num) * voxel_id;
-		int incremental_value = (voxel_id < voxel_num);
-		//int loc = atomicAdd(writing_locations + voxel_id, 1);
-		int loc =  atomicAdd(writing_locations + ptr_increment, incremental_value);
-
-		point_ids[loc] = i;
-	}
-}
-
-void GVoxelGrid::scatterPointsToVoxelGrid()
-{
-	if (starting_point_ids_ != NULL) {
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(starting_point_ids_));
-		rubis::sched::yield_gpu("326_free");
-
-		starting_point_ids_ = NULL;
-	}
-
-	if (point_ids_ != NULL) {
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaFree(point_ids_));
-		rubis::sched::yield_gpu("327_free");
-
-		point_ids_ = NULL;
-	}
-
-	int block_x = (points_num_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_num_;
-	int grid_x = (points_num_ - 1) / block_x + 1;
-
-	rubis::sched::request_gpu();
-	insertPointsToGrid<<<grid_x, block_x>>>(x_, y_, z_, points_num_, points_per_voxel_, voxel_num_,
-												vgrid_x_, vgrid_y_, vgrid_z_,
-												voxel_x_, voxel_y_, voxel_z_,
-												min_b_x_, min_b_y_, min_b_z_);
-	rubis::sched::yield_gpu("328_insertPointsToGrid");
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&starting_point_ids_, sizeof(int) * (voxel_num_ + 1)));
-	rubis::sched::yield_gpu("329_cudaMalloc");
-
-	int *writing_location;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&writing_location, sizeof(int) * voxel_num_));
-	rubis::sched::yield_gpu("330_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(starting_point_ids_, points_per_voxel_, sizeof(int) * voxel_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("331_dtod");
-
-	ExclusiveScan(starting_point_ids_, voxel_num_ + 1);
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemcpy(writing_location, starting_point_ids_, sizeof(int) * voxel_num_, cudaMemcpyDeviceToDevice));
-	rubis::sched::yield_gpu("332_dtod");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&point_ids_, sizeof(int) * points_num_));
-	rubis::sched::yield_gpu("333_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	scatterPointsToVoxels<<<grid_x, block_x>>>(x_, y_, z_, points_num_, voxel_num_,
-												voxel_x_, voxel_y_, voxel_z_,
-												min_b_x_, min_b_y_, min_b_z_,
-												vgrid_x_, vgrid_y_, vgrid_z_,
-												writing_location, point_ids_);
-	rubis::sched::yield_gpu("334_scatterPointsToVoxels");
-	
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(writing_location));
-	rubis::sched::yield_gpu("335_free");
-}
-
-void GVoxelGrid::buildOctree()
-{
-	for (unsigned int i = 1; i < octree_centroids_.size(); i++) {
-		if (octree_centroids_[i] != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(octree_centroids_[i]));
-			rubis::sched::yield_gpu("336_free");
-
-			octree_centroids_[i] = NULL;
-		}
-		if (octree_points_per_node_[i] != NULL) {
-			rubis::sched::request_gpu();
-			checkCudaErrors(cudaFree(octree_points_per_node_[i]));
-			rubis::sched::yield_gpu("337_free");
-
-			octree_points_per_node_[i] = NULL;
-		}
-	}
-
-	octree_centroids_.clear();
-	octree_points_per_node_.clear();
-	octree_grid_size_.clear();
-
-	//Push leafs to the octree list
-	octree_centroids_.push_back(centroid_);
-	octree_points_per_node_.push_back(points_per_voxel_);
-	OctreeGridSize grid_size;
-
-	grid_size.size_x = vgrid_x_;
-	grid_size.size_y = vgrid_y_;
-	grid_size.size_z = vgrid_z_;
-
-	octree_grid_size_.push_back(grid_size);
-
-	int node_number = voxel_num_;
-	int child_grid_x, child_grid_y, child_grid_z;
-	int parent_grid_x, parent_grid_y, parent_grid_z;
-
-	int i = 0;
-
-	while (node_number > 8) {
-		child_grid_x = octree_grid_size_[i].size_x;
-		child_grid_y = octree_grid_size_[i].size_y;
-		child_grid_z = octree_grid_size_[i].size_z;
-
-		parent_grid_x = (child_grid_x - 1) / 2 + 1;
-		parent_grid_y = (child_grid_y - 1) / 2 + 1;
-		parent_grid_z = (child_grid_z - 1) / 2 + 1;
-
-		node_number = parent_grid_x * parent_grid_y * parent_grid_z;
-
-		double *parent_centroids;
-		int *points_per_parent;
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&parent_centroids, sizeof(double) * 3 * node_number));
-		rubis::sched::yield_gpu("338_cudaMalloc");
-
-		rubis::sched::request_gpu();
-		checkCudaErrors(cudaMalloc(&points_per_parent, sizeof(int) * node_number));
-		rubis::sched::yield_gpu("339_cudaMalloc");
-
-		double *child_centroids = octree_centroids_[i];
-		int *points_per_child = octree_points_per_node_[i];
-
-		int block_x = (parent_grid_x > BLOCK_X) ? BLOCK_X : parent_grid_x;
-		int block_y = (parent_grid_y > BLOCK_Y) ? BLOCK_Y : parent_grid_y;
-		int block_z = (parent_grid_z > BLOCK_Z) ? BLOCK_Z : parent_grid_z;
-
-		int grid_x = (parent_grid_x - 1) / block_x + 1;
-		int grid_y = (parent_grid_y - 1) / block_y + 1;
-		int grid_z = (parent_grid_z - 1) / block_z + 1;
-
-		dim3 block(block_x, block_y, block_z);
-		dim3 grid(grid_x, grid_y, grid_z);
-
-		rubis::sched::request_gpu();
-		buildParent<<<grid, block>>>(child_centroids, points_per_child,
-										child_grid_x, child_grid_y, child_grid_z, child_grid_x * child_grid_y * child_grid_z,
-										parent_centroids, points_per_parent,
-										parent_grid_x, parent_grid_y, parent_grid_z);
-		rubis::sched::yield_gpu("340_buildParent");
-		checkCudaErrors(cudaGetLastError());
-		octree_centroids_.push_back(parent_centroids);
-		octree_points_per_node_.push_back(points_per_parent);
-
-		grid_size.size_x = parent_grid_x;
-		grid_size.size_y = parent_grid_y;
-		grid_size.size_z = parent_grid_z;
-
-		octree_grid_size_.push_back(grid_size);
-
-		i++;
-	}
-	
-	checkCudaErrors(cudaDeviceSynchronize());
-}
-
-/* Search for the nearest octree node */
-extern "C" __global__ void nearestOctreeNodeSearch(float *x, float *y, float *z,
-													int *vid_x, int *vid_y, int *vid_z,
-													int points_num,
-													double *centroids, int *points_per_node,
-													int vgrid_x, int vgrid_y, int vgrid_z, int node_num)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = idx; i < points_num; i += stride) {
-		int vx = vid_x[i];
-		int vy = vid_y[i];
-		int vz = vid_z[i];
-		double min_dist = DBL_MAX;
-		double t_x = static_cast<double>(x[i]);
-		double t_y = static_cast<double>(y[i]);
-		double t_z = static_cast<double>(z[i]);
-		double cur_dist;
-
-		int out_x, out_y, out_z;
-
-		out_x = vx;
-		out_y = vy;
-		out_z = vz;
-
-		double tmp_x, tmp_y, tmp_z;
-
-		for (int j = vx * 2; j < vx * 2 + 2 && j < vgrid_x; j++) {
-			for (int k = vy * 2; k < vy * 2 + 2 && k < vgrid_y; k++) {
-				for (int l = vz * 2; l < vz * 2 + 2 && l < vgrid_z; l++) {
-					int node_id = j + k * vgrid_x + l * vgrid_x * vgrid_y;
-					MatrixDevice node_centr(3, 1, node_num, centroids + node_id);
-					int points = points_per_node[node_id];
-
-					tmp_x = (points > 0) ? node_centr(0) - t_x : DBL_MAX;
-					tmp_y = (points > 0) ? node_centr(1) - t_y : 0.0;
-					tmp_z = (points > 0) ? node_centr(2) - t_z : 0.0;
-
-					cur_dist = norm3d(tmp_x, tmp_y, tmp_z);
-					bool res = (cur_dist < min_dist);
-
-					out_x = (res) ? j : out_x;
-					out_y = (res) ? k : out_y;
-					out_z = (res) ? l : out_z;
-
-					min_dist = (res) ? cur_dist : min_dist;
-				}
-			}
-		}
-
-		vid_x[i] = out_x;
-		vid_y[i] = out_y;
-		vid_z[i] = out_z;
-	}
-}
-
-/* Search for the nearest point from nearest voxel */
-extern "C" __global__ void nearestPointSearch(float *qx, float *qy, float *qz, int qpoints_num,
-												float *rx, float *ry, float *rz, int rpoints_num,
-												int *vid_x, int *vid_y, int *vid_z,
-												int vgrid_x, int vgrid_y, int vgrid_z, int voxel_num,
-												int *starting_point_id, int *point_id, double *min_distance)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = idx; i < qpoints_num; i += stride) {
-		int voxel_id = vid_x[i] + vid_y[i] * vgrid_x + vid_z[i] * vgrid_x * vgrid_y;
-		float cor_qx = qx[i];
-		float cor_qy = qy[i];
-		float cor_qz = qz[i];
-		float min_dist = FLT_MAX;
-
-		for (int j = starting_point_id[voxel_id]; j < starting_point_id[voxel_id + 1]; j++) {
-			int pid = point_id[j];
-			float cor_rx = rx[pid];
-			float cor_ry = ry[pid];
-			float cor_rz = rz[pid];
-
-			cor_rx -= cor_qx;
-			cor_ry -= cor_qy;
-			cor_rz -= cor_qz;
-
-			min_dist = fminf(norm3df(cor_rx, cor_ry, cor_rz), min_dist);
-		}
-
-		min_distance[i] = static_cast<double>(min_dist);
-	}
-}
-
-/* Verify if min distances are really smaller than or equal to max_range */
-extern "C" __global__ void verifyDistances(int *valid_distance, double *min_distance, double max_range, int points_num)
-{
-	int idx = threadIdx.x + blockIdx.x * blockDim.x;
-	int stride = blockDim.x * gridDim.x;
-
-	for (int i = idx; i < points_num; i += stride) {
-		bool check = (min_distance[i] <= max_range);
-
-		valid_distance[i] = (check) ? 1 : 0;
-
-		if (!check) {
-			min_distance[i] = 0;
-		}
-	}
-}
-
-void GVoxelGrid::nearestNeighborSearch(float *trans_x, float *trans_y, float *trans_z, int point_num, int *valid_distance, double *min_distance, float max_range)
-{
-
-	int *vid_x, *vid_y, *vid_z;
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&vid_x, sizeof(int) * point_num));
-	rubis::sched::yield_gpu("338_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&vid_y, sizeof(int) * point_num));
-	rubis::sched::yield_gpu("339_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMalloc(&vid_z, sizeof(int) * point_num));
-	rubis::sched::yield_gpu("340_cudaMalloc");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(vid_x, 0, sizeof(int) * point_num));
-	rubis::sched::yield_gpu("341_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(vid_y, 0, sizeof(int) * point_num));
-	rubis::sched::yield_gpu("342_cudaMemset");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaMemset(vid_z, 0, sizeof(int) * point_num));
-	rubis::sched::yield_gpu("343_cudaMemset");
-	
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	int block_x = (point_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : point_num;
-	int grid_x = (point_num - 1) / block_x + 1;
-
-	// Go through top of the octree to the bottom
-	for (int i = octree_centroids_.size() - 1; i >= 0; i--) {
-		double *centroids = octree_centroids_[i];
-		int *points_per_node = octree_points_per_node_[i];
-		int vgrid_x = octree_grid_size_[i].size_x;
-		int vgrid_y = octree_grid_size_[i].size_y;
-		int vgrid_z = octree_grid_size_[i].size_z;
-		int node_num = vgrid_x * vgrid_y * vgrid_z;
-
-		rubis::sched::request_gpu();
-		nearestOctreeNodeSearch<<<grid_x, block_x>>>(trans_x, trans_y, trans_z,
-														vid_x, vid_y, vid_z,
-														point_num,
-														centroids, points_per_node,
-														vgrid_x, vgrid_y, vgrid_z, node_num);
-		rubis::sched::yield_gpu("344_nearestOctreeNodeSearch");
-
-		checkCudaErrors(cudaGetLastError());
-	}
-
-	rubis::sched::request_gpu();
-	nearestPointSearch<<<grid_x, block_x>>>(trans_x, trans_y, trans_z, point_num,
-												x_, y_, z_, points_num_,
-												vid_x, vid_y, vid_z,
-												vgrid_x_, vgrid_y_, vgrid_z_, voxel_num_,
-												starting_point_ids_, point_ids_,
-												min_distance);
-	rubis::sched::yield_gpu("345_nearestPointSearch");
-	
-	checkCudaErrors(cudaGetLastError());
-
-	rubis::sched::request_gpu();
-	verifyDistances<<<grid_x, block_x>>>(valid_distance, min_distance, max_range, point_num);
-	rubis::sched::yield_gpu("346_verifyDistances");
-
-	checkCudaErrors(cudaGetLastError());
-	checkCudaErrors(cudaDeviceSynchronize());
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(vid_x));
-	rubis::sched::yield_gpu("347_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(vid_y));
-	rubis::sched::yield_gpu("348_free");
-
-	rubis::sched::request_gpu();
-	checkCudaErrors(cudaFree(vid_z));
-	rubis::sched::yield_gpu("349_free");
-}
-
-}
diff --git a/autoware.ai/src/autoware/core_perception/pcl_omp_registration/package.xml b/autoware.ai/src/autoware/core_perception/pcl_omp_registration/package.xml
index 3f577a7d..8b4a7098 100644
--- a/autoware.ai/src/autoware/core_perception/pcl_omp_registration/package.xml
+++ b/autoware.ai/src/autoware/core_perception/pcl_omp_registration/package.xml
@@ -7,4 +7,6 @@
   <license>Apache 2</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>pcl</depend>
+  <depend>eigen</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/points_downsampler/CMakeLists.txt
index d9276a41..4d5a4f6a 100644
--- a/autoware.ai/src/autoware/core_perception/points_downsampler/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/points_downsampler/CMakeLists.txt
@@ -5,7 +5,7 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   autoware_config_msgs
diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch b/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch
index 46a29468..73d458d3 100644
--- a/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch
+++ b/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch
@@ -4,13 +4,11 @@
   <arg name="points_topic" default="points_raw" />
   <arg name="output_log" default="false" />
   <arg name="measurement_range" default="200" />
-  <arg name="instance_mode" default="0"/>
 
   <node pkg="points_downsampler" name="$(arg node_name)" type="$(arg node_name)">
     <param name="points_topic" value="$(arg points_topic)" />
     <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
     <param name="output_log" value="$(arg output_log)" />
     <param name="measurement_range" value="$(arg measurement_range)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 </launch>
diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter_params.launch b/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter_params.launch
index 285c5dd3..53433519 100644
--- a/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter_params.launch
+++ b/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter_params.launch
@@ -1,11 +1,12 @@
 <launch>
-  <arg name="output_log" default="false" />
+  <arg name="sync" default="false" />
   <arg name="leaf_size" default="0.1" />
+  <arg name="node_name" default="voxel_grid_filter" />
+  <arg name="points_topic" default="points_raw" />
+  <arg name="output_log" default="false" />
   <arg name="measurement_range" default="200" />
-  <arg name="input_topic_name" default="points_raw" />
-  <arg name="output_topic_name" default="filtered_points"/>
-  
-  <node pkg="points_downsampler" name="voxel_grid_filter" type="voxel_grid_filter">
+
+  <node pkg="points_downsampler" name="$(arg node_name)" type="$(arg node_name)" output="screen">
     <param name="input_topic_name" value="points_raw" />
     <param name="output_topic_name" value="filtered_points" />
     <param name="leaf_size" value="$(arg leaf_size)" />
diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp
index 2aa58dfa..ea75526b 100644
--- a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp
+++ b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp
@@ -62,7 +62,7 @@ static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::C
 }
 
 inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstPtr& input)
-{
+{  
   pcl::PointCloud<pcl::PointXYZI> scan;
   pcl::fromROSMsg(*input, scan);
 
@@ -97,12 +97,11 @@ inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstP
   filtered_msg.header = input->header;
   filtered_points_pub.publish(filtered_msg);
 
-  if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){
-    rubis_msgs::PointCloud2 rubis_filtered_msg;
-    rubis_filtered_msg.msg = filtered_msg;
-    rubis_filtered_msg.instance = rubis::instance_;
-    rubis_filtered_points_pub.publish(rubis_filtered_msg);
-  }
+  rubis_msgs::PointCloud2 rubis_filtered_msg;
+  rubis_filtered_msg.header = filtered_msg.header;
+  rubis_filtered_msg.msg = filtered_msg;
+  rubis_filtered_msg.instance = rubis::instance_;
+  rubis_filtered_points_pub.publish(rubis_filtered_msg);
 
   points_downsampler_info_msg.header = input->header;
   points_downsampler_info_msg.filter_name = "voxel_grid_filter";
@@ -137,22 +136,24 @@ inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstP
       << points_downsampler_info_msg.exe_time << ","
       << std::endl;
   }
-
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+  
 }
 
 static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
 {
-  rubis::instance_ = RUBIS_NO_INSTANCE;
+  rubis::instance_ = 0;
   publish_filtered_cloud(input);
 }
 
 static void rubis_scan_callback(const rubis_msgs::PointCloud2::ConstPtr& _input)
 {
+  rubis::start_task_profiling();
+
   sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared<const sensor_msgs::PointCloud2>(_input->msg);
   rubis::instance_ = _input->instance;
   publish_filtered_cloud(input);
+
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
 int main(int argc, char** argv)
@@ -162,15 +163,6 @@ int main(int argc, char** argv)
   ros::NodeHandle nh;
   ros::NodeHandle private_nh("~");
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
-
   private_nh.param<std::string>("input_topic_name", input_topic_name_, std::string("points_raw"));
   private_nh.param<std::string>("output_topic_name", output_topic_name_, std::string("filtered_points"));
   private_nh.getParam("output_log", _output_log);
@@ -185,23 +177,32 @@ int main(int argc, char** argv)
   private_nh.param<double>("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE);
   private_nh.param<double>("leaf_size", voxel_leaf_size, 0.1);
 
+  // Scheduling & Profiling Setup
   std::string node_name = ros::this_node::getName();
-  private_nh.param<int>(node_name+"/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>(node_name+"/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voxel_grid_filter.csv");
+  std::string task_response_time_filename;
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voexl_grid_filter.csv");
+
+  int rate;
   private_nh.param<int>(node_name+"/rate", rate, 10);
-  private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10);
-  private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10);
-  private_nh.param<int>(node_name+"/instance_mode", rubis::instance_mode_, 0);
 
-  /* For Task scheduling */
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
+
+  rubis::init_task_profiling(task_response_time_filename);
+
 
   // Publishers
   filtered_points_pub = nh.advertise<sensor_msgs::PointCloud2>(output_topic_name_, 10);
-  if(rubis::instance_mode_)
-    rubis_filtered_points_pub = nh.advertise<rubis_msgs::PointCloud2>("/rubis_" + output_topic_name_, 10);
+  rubis_filtered_points_pub = nh.advertise<rubis_msgs::PointCloud2>("/rubis_" + output_topic_name_, 10);
   points_downsampler_info_pub = nh.advertise<points_downsampler::PointsDownsamplerInfo>("/points_downsampler_info", 1000);
 
   // Subscribers
@@ -209,47 +210,9 @@ int main(int argc, char** argv)
   // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback);
   ros::Subscriber scan_sub;
   
-  if(rubis::instance_mode_) scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback);
-  else scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback);
-
-  /*  RT Scheduling setup  */
-  // ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 1, config_callback); // origin 10
-  // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 1, scan_callback); // origin 10
-
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{
-    ros::Rate r(rate);
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){
-      if(rubis::sched::is_task_ready_ == TASK_READY) break;
-      ros::spinOnce();
-      r.sleep();      
-    }
-
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();
-      if(rubis::sched::task_state_ == TASK_STATE_READY){        
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
-
-      ros::spinOnce();
-
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){        
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();        
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
-  }
+  scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback); // Def: 10
 
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+  ros::spin();
 
   return 0;
 }
diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp
new file mode 100644
index 00000000..5b5c5435
--- /dev/null
+++ b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp
@@ -0,0 +1,211 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/voxel_grid.h>
+
+#include "autoware_config_msgs/ConfigVoxelGridFilter.h"
+
+#include <points_downsampler/PointsDownsamplerInfo.h>
+
+#include <chrono>
+
+#include "points_downsampler.h"
+
+#include <rubis_lib/sched.hpp>
+#include <rubis_msgs/PointCloud2.h>
+
+#define SPIN_PROFILING
+
+#define MAX_MEASUREMENT_RANGE 200.0
+
+ros::Publisher filtered_points_pub;
+ros::Publisher rubis_filtered_points_pub;
+
+// Leaf size of VoxelGrid filter.
+static double voxel_leaf_size = 2.0;
+
+static ros::Publisher points_downsampler_info_pub;
+static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg;
+
+static std::chrono::time_point<std::chrono::system_clock> filter_start, filter_end;
+
+static bool _output_log = false;
+static std::ofstream ofs;
+static std::string filename;
+
+static std::string input_topic_name_;
+static std::string output_topic_name_;
+static double measurement_range = MAX_MEASUREMENT_RANGE;
+
+static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::ConstPtr& input)
+{
+  voxel_leaf_size = input->voxel_leaf_size;
+  measurement_range = input->measurement_range;
+}
+
+inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstPtr& input)
+{  
+  pcl::PointCloud<pcl::PointXYZI> scan;
+  pcl::fromROSMsg(*input, scan);
+
+  if(measurement_range != MAX_MEASUREMENT_RANGE){
+    scan = removePointsByRange(scan, 0, measurement_range);
+  }
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan));
+  pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
+
+  sensor_msgs::PointCloud2 filtered_msg;
+
+  filter_start = std::chrono::system_clock::now();
+
+  // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
+  if (voxel_leaf_size >= 0.1)
+  {
+    // Downsampling the velodyne scan using VoxelGrid filter
+    pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
+    voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
+    voxel_grid_filter.setInputCloud(scan_ptr);
+    voxel_grid_filter.filter(*filtered_scan_ptr);
+    pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
+  }
+  else
+  {
+    pcl::toROSMsg(*scan_ptr, filtered_msg);
+  }
+
+  filter_end = std::chrono::system_clock::now();
+
+  filtered_msg.header = input->header;
+  filtered_points_pub.publish(filtered_msg);
+
+  rubis_msgs::PointCloud2 rubis_filtered_msg;
+  rubis_filtered_msg.msg = filtered_msg;
+  rubis_filtered_msg.instance = rubis::instance_;
+  rubis_filtered_points_pub.publish(rubis_filtered_msg);
+
+  points_downsampler_info_msg.header = input->header;
+  points_downsampler_info_msg.filter_name = "voxel_grid_filter";
+  points_downsampler_info_msg.measurement_range = measurement_range;
+  points_downsampler_info_msg.original_points_size = scan.size();
+  if (voxel_leaf_size >= 0.1)
+  {
+    points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size();
+  }
+  else
+  {
+    points_downsampler_info_msg.filtered_points_size = scan_ptr->size();
+  }
+  points_downsampler_info_msg.original_ring_size = 0;
+  points_downsampler_info_msg.filtered_ring_size = 0;
+  points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0;
+  points_downsampler_info_pub.publish(points_downsampler_info_msg);
+
+  if(_output_log == true){
+    if(!ofs){
+      std::cerr << "Could not open " << filename << "." << std::endl;
+      exit(1);
+    }
+    ofs << points_downsampler_info_msg.header.seq << ","
+      << points_downsampler_info_msg.header.stamp << ","
+      << points_downsampler_info_msg.header.frame_id << ","
+      << points_downsampler_info_msg.filter_name << ","
+      << points_downsampler_info_msg.original_points_size << ","
+      << points_downsampler_info_msg.filtered_points_size << ","
+      << points_downsampler_info_msg.original_ring_size << ","
+      << points_downsampler_info_msg.filtered_ring_size << ","
+      << points_downsampler_info_msg.exe_time << ","
+      << std::endl;
+  }
+  
+}
+
+static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
+{
+  rubis::instance_ = 0;
+  publish_filtered_cloud(input);
+}
+
+static void rubis_scan_callback(const rubis_msgs::PointCloud2::ConstPtr& _input)
+{
+  rubis::start_task_profiling();
+
+  sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared<const sensor_msgs::PointCloud2>(_input->msg);
+  rubis::instance_ = _input->instance;
+  publish_filtered_cloud(input);
+
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "rubis_voxel_grid_filter");
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  // Scheduling Setup
+  std::string task_response_time_filename;
+  int rate;
+  double task_minimum_inter_release_time;
+  double task_execution_time;
+  double task_relative_deadline; 
+
+  private_nh.param<std::string>("input_topic_name", input_topic_name_, std::string("points_raw"));
+  private_nh.param<std::string>("output_topic_name", output_topic_name_, std::string("filtered_points"));
+  private_nh.getParam("output_log", _output_log);
+  if(_output_log == true){
+    char buffer[80];
+    std::time_t now = std::time(NULL);
+    std::tm *pnow = std::localtime(&now);
+    std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);
+    filename = "voxel_grid_filter_" + std::string(buffer) + ".csv";
+    ofs.open(filename.c_str(), std::ios::app);
+  }
+  private_nh.param<double>("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE);
+  private_nh.param<double>("leaf_size", voxel_leaf_size, 0.1);
+
+  std::string node_name = ros::this_node::getName();
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voxel_grid_filter.csv");
+  private_nh.param<int>(node_name+"/rate", rate, 10);
+  private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
+  private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10);
+  private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10);
+
+  /* For Task scheduling */
+  rubis::init_task_profiling(task_response_time_filename);
+
+  // Publishers
+  filtered_points_pub = nh.advertise<sensor_msgs::PointCloud2>(output_topic_name_, 10);
+  rubis_filtered_points_pub = nh.advertise<rubis_msgs::PointCloud2>("/rubis_" + output_topic_name_, 10);
+  points_downsampler_info_pub = nh.advertise<points_downsampler::PointsDownsamplerInfo>("/points_downsampler_info", 1000);
+
+  // Subscribers
+  ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback);
+  // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback);
+  ros::Subscriber scan_sub;
+  
+  scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback);
+
+  ros::spin();
+
+  return 0;
+}
diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/package.xml b/autoware.ai/src/autoware/core_perception/points_downsampler/package.xml
index 6257f151..2e8dcdd4 100644
--- a/autoware.ai/src/autoware/core_perception/points_downsampler/package.xml
+++ b/autoware.ai/src/autoware/core_perception/points_downsampler/package.xml
@@ -6,7 +6,7 @@
   <maintainer email="yuki@ertl.jp">Yuki KITSUKAWA</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>message_generation</build_depend>
diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/points_preprocessor/CMakeLists.txt
index 9f307cad..f678b609 100644
--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/CMakeLists.txt
@@ -5,12 +5,11 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(catkin REQUIRED COMPONENTS
   autoware_msgs
   autoware_config_msgs
-  autoware_health_checker
   cv_bridge
   pcl_conversions
   pcl_ros
@@ -36,7 +35,6 @@ roslint_cpp(
   include/points_preprocessor/ray_ground_filter/ray_ground_filter.h
 )
 
-find_package(Qt5Core REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(PCL 1.7 REQUIRED)
 find_package(OpenMP)
@@ -58,36 +56,6 @@ SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
 
 link_directories(${PCL_LIBRARY_DIRS})
 
-# Space Filter
-add_executable(space_filter
-  nodes/space_filter/space_filter.cpp
-)
-target_link_libraries(space_filter
-  ${catkin_LIBRARIES}
-  ${PCL_LIBRARIES}
-)
-
-add_dependencies(space_filter ${catkin_EXPORTED_TARGETS})
-
-# Ring Ground Filter
-add_definitions(${PCL_DEFINITIONS})
-
-add_executable(ring_ground_filter
-  nodes/ring_ground_filter/ring_ground_filter.cpp
-)
-
-target_include_directories(ring_ground_filter PRIVATE
-  ${PCL_INCLUDE_DIRS}
-)
-
-target_link_libraries(ring_ground_filter
-  ${catkin_LIBRARIES}
-  ${PCL_LIBRARIES}
-  ${Qt5Core_LIBRARIES}
-)
-
-add_dependencies(ring_ground_filter ${catkin_EXPORTED_TARGETS})
-
 # Ray Ground Filter
 add_library(ray_ground_filter_lib SHARED
   nodes/ray_ground_filter/ray_ground_filter.cpp
@@ -109,7 +77,6 @@ target_link_libraries(ray_ground_filter_lib
   ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
   ${OpenCV_LIBRARIES}
-  ${Qt5Core_LIBRARIES}
 )
 
 add_dependencies(ray_ground_filter_lib ${catkin_EXPORTED_TARGETS})
@@ -120,39 +87,6 @@ add_executable(ray_ground_filter
 target_link_libraries(ray_ground_filter ray_ground_filter_lib)
 add_dependencies(ray_ground_filter ${catkin_EXPORTED_TARGETS})
 
-# Points Concat filter
-add_executable(points_concat_filter
-  nodes/points_concat_filter/points_concat_filter.cpp
-)
-
-target_include_directories(points_concat_filter PRIVATE
-  ${PCL_INCLUDE_DIRS}
-)
-
-target_link_libraries(points_concat_filter
-  ${catkin_LIBRARIES}
-  ${PCL_LIBRARIES}
-  ${YAML_CPP_LIBRARIES}
-)
-
-add_dependencies(points_concat_filter ${catkin_EXPORTED_TARGETS})
-
-#Cloud Transformer
-add_executable(cloud_transformer
-  nodes/cloud_transformer/cloud_transformer_node.cpp
-)
-
-target_include_directories(cloud_transformer PRIVATE
-  ${PCL_INCLUDE_DIRS}
-)
-
-target_link_libraries(cloud_transformer
-  ${catkin_LIBRARIES}
-  ${PCL_LIBRARIES}
-  ${Qt5Core_LIBRARIES}
-)
-add_dependencies(cloud_transformer ${catkin_EXPORTED_TARGETS})
-
 #Compare Map Filter
 add_executable(compare_map_filter
   nodes/compare_map_filter/compare_map_filter.cpp
@@ -165,34 +99,14 @@ target_include_directories(compare_map_filter PRIVATE
 target_link_libraries(compare_map_filter
   ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
-  ${Qt5Core_LIBRARIES}
 )
 add_dependencies(compare_map_filter ${catkin_EXPORTED_TARGETS})
 
-### Unit Tests ###
-#if (CATKIN_ENABLE_TESTING)
-#    find_package(rostest REQUIRED)
-#    find_package(roslaunch REQUIRED)
-
-#    add_rostest_gtest(test_points_preprocessor
-#      test/test_points_preprocessor.test
-#      test/src/test_points_preprocessor.cpp)
-#   target_include_directories(test_points_preprocessor PRIVATE
-#          nodes/ray_ground_filter/include
-#            test/include)
-#    target_link_libraries(test_points_preprocessor
-#            ray_ground_filter_lib
-#            ${catkin_LIBRARIES})
-#endif ()
 
 install(
   TARGETS
-    cloud_transformer
-    points_concat_filter
     ray_ground_filter_lib
     ray_ground_filter
-    ring_ground_filter
-    space_filter
     compare_map_filter
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -207,7 +121,3 @@ install(DIRECTORY launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
   PATTERN ".svn" EXCLUDE
 )
-
-if (CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-endif()
diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h
index 58e1bafa..c709ebb3 100644
--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h
+++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h
@@ -33,14 +33,13 @@
 #include <pcl/filters/extract_indices.h>
 #include <velodyne_pointcloud/point_types.h>
 #include "autoware_config_msgs/ConfigRayGroundFilter.h"
+#include "rubis_msgs/PointCloud2.h"
 
 #include <tf2/transform_datatypes.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_eigen/tf2_eigen.h>
 
 // headers in Autoware Health Checker
-#include <autoware_health_checker/health_checker/health_checker.h>
-
 #include <opencv2/core/version.hpp>
 
 #include "gencolors.cpp"
@@ -51,12 +50,11 @@
 class RayGroundFilter
 {
 private:
-  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
   ros::NodeHandle node_handle_;
   ros::Subscriber points_node_sub_;
   ros::Subscriber config_node_sub_;
   ros::Publisher groundless_points_pub_;
-  ros::Publisher ground_points_pub_;
+  ros::Publisher debug_groundless_points_pub_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
@@ -80,8 +78,6 @@ class RayGroundFilter
   std::vector<cv::Scalar> colors_;
   const size_t color_num_ = 60;  // different number of color to generate
 
-  int instance_mode_ = 0;
-
   struct PointXYZIRTColor
   {
     pcl::PointXYZI point;
@@ -113,6 +109,10 @@ class RayGroundFilter
   bool TransformPointCloud(const std::string& in_target_frame, const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
                            const sensor_msgs::PointCloud2::Ptr& out_cloud_ptr);
 
+  void publish_rubis_cloud(const ros::Publisher& in_publisher,
+                     const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
+                     const std_msgs::Header& in_header);
+
   void publish_cloud(const ros::Publisher& in_publisher,
                      const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
                      const std_msgs::Header& in_header);
diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch
index d4fcb70e..5418c56f 100755
--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch
+++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch
@@ -14,7 +14,6 @@
   <arg name="reclass_distance_threshold" default="0.2" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
   <arg name="no_ground_point_topic" default="/points_no_ground" />
   <arg name="ground_point_topic" default="/points_ground" />
-  <arg name="instance_mode" default="0" />
 
   <!-- rosrun points_preprocessor ray_ground_filter -->
   <node pkg="points_preprocessor" type="ray_ground_filter" name="ray_ground_filter_$(arg label)" output="log">
@@ -31,6 +30,5 @@
     <param name="reclass_distance_threshold" value="$(arg reclass_distance_threshold)" />
     <param name="no_ground_point_topic" value="$(arg no_ground_point_topic)_$(arg label)" />
     <param name="ground_point_topic" value="$(arg ground_point_topic)_$(arg label)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 </launch>
diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch
index 45d448f1..eb60b4a3 100755
--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch
+++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch
@@ -14,10 +14,9 @@
   <arg name="reclass_distance_threshold" default="0.2" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
   <arg name="no_ground_point_topic" default="/points_no_ground" />
   <arg name="ground_point_topic" default="/points_ground" />
-  <!-- <arg name="instance_mode" default="0" /> -->
 
   <!-- rosrun points_preprocessor ray_ground_filter -->
-  <node pkg="points_preprocessor" type="ray_ground_filter" name="ray_ground_filter_$(arg label)" output="log">
+  <node pkg="points_preprocessor" type="ray_ground_filter" name="ray_ground_filter_$(arg label)" output="screen">
     <param name="input_point_topic" value="$(arg input_point_topic)" />
     <param name="base_frame" value="$(arg base_frame)" />
     <param name="max_clipping_height" value="$(arg max_clipping_height)" />
@@ -31,6 +30,5 @@
     <param name="reclass_distance_threshold" value="$(arg reclass_distance_threshold)" />
     <param name="no_ground_point_topic" value="$(arg no_ground_point_topic)_$(arg label)" />
     <param name="ground_point_topic" value="$(arg ground_point_topic)_$(arg label)" />
-    <!-- <param name="instance_mode" value="$(arg instance_mode)" /> -->
   </node>
 </launch>
diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp
index 92454425..d205d30f 100644
--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp
+++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp
@@ -221,8 +221,7 @@ void CompareMapFilter::sensorPointsCallback(const sensor_msgs::PointCloud2::Cons
     return;
   }
   unmatch_points_pub_.publish(sensorTF_unmatch_cloud_msg);
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+  
 }
 
 void CompareMapFilter::searchMatchingCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp
index bc9dd3de..4a185a08 100644
--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp
+++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp
@@ -65,14 +65,15 @@ bool RayGroundFilter::TransformPointCloud(const std::string& in_target_frame,
   }
 
   geometry_msgs::TransformStamped transform_stamped;
+  
   try
   {
     transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
-                                                   in_cloud_ptr->header.stamp, ros::Duration(1.0));
+                                                   ros::Time(0), ros::Duration(0.1));
   }
   catch (tf2::TransformException& ex)
   {
-    ROS_WARN("%s", ex.what());
+    ROS_WARN("[ray_ground_filter] %s, %lld.%lld", ex.what(), in_cloud_ptr->header.stamp.sec, in_cloud_ptr->header.stamp.nsec);
     return false;
   }
   // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
@@ -82,6 +83,30 @@ bool RayGroundFilter::TransformPointCloud(const std::string& in_target_frame,
   return true;
 }
 
+void RayGroundFilter::publish_rubis_cloud(const ros::Publisher& in_publisher,
+                                    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
+                                    const std_msgs::Header& in_header)
+{
+  sensor_msgs::PointCloud2::Ptr cloud_msg_ptr(new sensor_msgs::PointCloud2);
+  sensor_msgs::PointCloud2::Ptr trans_cloud_msg_ptr(new sensor_msgs::PointCloud2);
+  pcl::toROSMsg(*in_cloud_to_publish_ptr, *cloud_msg_ptr);
+  cloud_msg_ptr->header.frame_id = base_frame_;
+  cloud_msg_ptr->header = in_header;
+  const bool succeeded = TransformPointCloud(in_header.frame_id, cloud_msg_ptr, trans_cloud_msg_ptr);
+  if (!succeeded)
+  {
+    ROS_ERROR_STREAM_THROTTLE(10, "Failed transform from " << cloud_msg_ptr->header.frame_id << " to "
+                                                           << in_header.frame_id);
+    return;
+  }
+
+  rubis_msgs::PointCloud2 msg;
+  msg.header = in_header;
+  msg.instance = rubis::instance_;
+  msg.msg = *trans_cloud_msg_ptr;
+  in_publisher.publish(msg);
+}
+
 void RayGroundFilter::publish_cloud(const ros::Publisher& in_publisher,
                                     const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
                                     const std_msgs::Header& in_header)
@@ -98,7 +123,11 @@ void RayGroundFilter::publish_cloud(const ros::Publisher& in_publisher,
                                                            << in_header.frame_id);
     return;
   }
-  in_publisher.publish(*trans_cloud_msg_ptr);
+
+  sensor_msgs::PointCloud2 msg;
+  msg.header = in_header;
+  msg = *trans_cloud_msg_ptr;
+  in_publisher.publish(msg);
 }
 
 /*!
@@ -329,9 +358,6 @@ void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Pt
 
 
 inline void RayGroundFilter::PublishFilteredClouds(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud){
-  health_checker_ptr_->NODE_ACTIVATE();
-  health_checker_ptr_->CHECK_RATE("topic_rate_points_raw_slow", 8, 5, 1, "topic points_raw subscribe rate slow.");
-
   sensor_msgs::PointCloud2::Ptr trans_sensor_cloud(new sensor_msgs::PointCloud2);
   const bool succeeded = TransformPointCloud(base_frame_, in_sensor_cloud, trans_sensor_cloud);
   
@@ -375,11 +401,9 @@ inline void RayGroundFilter::PublishFilteredClouds(const sensor_msgs::PointCloud
 
   ExtractPointsIndices(filtered_cloud_ptr, *ground_indices, ground_cloud_ptr, no_ground_cloud_ptr);
 
-  publish_cloud(ground_points_pub_, ground_cloud_ptr, in_sensor_cloud->header);
-  publish_cloud(groundless_points_pub_, no_ground_cloud_ptr, in_sensor_cloud->header);
+  publish_cloud(debug_groundless_points_pub_, no_ground_cloud_ptr, in_sensor_cloud->header);
+  publish_rubis_cloud(groundless_points_pub_, no_ground_cloud_ptr, in_sensor_cloud->header);
 
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
 }
 
 void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud)
@@ -390,8 +414,12 @@ void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_s
 
 void RayGroundFilter::RubisCloudCallback(const rubis_msgs::PointCloud2ConstPtr in_rubis_cloud)
 {
+  rubis::start_task_profiling();
   sensor_msgs::PointCloud2ConstPtr in_sensor_cloud = boost::make_shared<const sensor_msgs::PointCloud2>(in_rubis_cloud->msg);
+  rubis::instance_ = in_rubis_cloud->instance;
+  rubis::lidar_instance_ = in_rubis_cloud->instance;
   PublishFilteredClouds(in_sensor_cloud);
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
 RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_)
@@ -399,21 +427,10 @@ RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_)
 {
   ros::NodeHandle nh;
   ros::NodeHandle pnh("~");
-  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh, pnh);
-  health_checker_ptr_->ENABLE();
 }
 
 void RayGroundFilter::Run()
 {
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline;
-
   // Model   |   Horizontal   |   Vertical   | FOV(Vertical)    degrees / rads
   // ----------------------------------------------------------
   // HDL-64  |0.08-0.35(0.32) |     0.4      |  -24.9 <=x<=2.0   (26.9  / 0.47)
@@ -466,55 +483,35 @@ void RayGroundFilter::Run()
   config_node_sub_ =
       node_handle_.subscribe("/config/ray_ground_filter", 1, &RayGroundFilter::update_config_params, this);
 
-  groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(no_ground_topic, 2);
-  ground_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(ground_topic, 2);
+  groundless_points_pub_ = node_handle_.advertise<rubis_msgs::PointCloud2>(no_ground_topic, 2);
+  debug_groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>("/debug_points_no_ground", 2);
   
+  // Scheduling & Profiling Setup
   std::string node_name = ros::this_node::getName();
-  node_handle_.param<int>(node_name+"/task_scheduling_flag", task_scheduling_flag, 0);
-  node_handle_.param<int>(node_name+"/task_profiling_flag", task_profiling_flag, 0);
+  std::string task_response_time_filename;
   node_handle_.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ray_ground_filter.csv");
-  node_handle_.param<int>(node_name+"/rate", rate, 10);
-  node_handle_.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  node_handle_.param(node_name+"/task_execution_time", task_execution_time, (double)10);
-  node_handle_.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10);
-  node_handle_.param<int>(node_name+"/instance_mode", instance_mode_, 0);
 
-  if(instance_mode_) points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 1, &RayGroundFilter::RubisCloudCallback, this);
-  else points_node_sub_ = node_handle_.subscribe(input_point_topic_, 1, &RayGroundFilter::CloudCallback, this);
-
-  ROS_INFO("Ready");
+  int rate;
+  node_handle_.param<int>(node_name+"/rate", rate, 10);
 
-  /* For Task scheduling */
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  node_handle_.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  node_handle_.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  node_handle_.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  node_handle_.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  node_handle_.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
 
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{
-    ros::Rate r(rate);    
-    while(ros::ok()){
-      if(rubis::sched::is_task_ready_) break;
-      ros::spinOnce();
-      r.sleep();      
-    }
+  rubis::init_task_profiling(task_response_time_filename);
 
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();
-      if(rubis::sched::task_state_ == TASK_STATE_READY){        
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
+  std::cout<<"Subscribe topic: "<<"/rubis_"+input_point_topic_.substr(1)<<std::endl;
+  points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 10, &RayGroundFilter::RubisCloudCallback, this);
 
-      ros::spinOnce();
+  ROS_INFO("Ready");
 
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
-  }
+  ros::spin();
 }
diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/package.xml b/autoware.ai/src/autoware/core_perception/points_preprocessor/package.xml
index 712e0c9d..32b542b8 100644
--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/package.xml
+++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/package.xml
@@ -8,19 +8,16 @@
   <author email="akihito.ohsato@tier4.jp">aohsato</author>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>roslint</depend>
   <depend>autoware_msgs</depend>
   <depend>autoware_config_msgs</depend>
-  <depend>autoware_health_checker</depend>
   <depend>cv_bridge</depend>
-  <depend>gtest</depend>
   <depend>message_filters</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
-  <depend>qtbase5-dev</depend>
   <depend>roscpp</depend>
   <depend>rostest</depend>
   <depend>sensor_msgs</depend>
@@ -33,4 +30,5 @@
   <depend>yaml-cpp</depend>
   <depend>rubis_lib</depend>
   <depend>rubis_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch b/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch
index 0ff66af8..41579902 100644
--- a/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch
+++ b/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch
@@ -12,7 +12,7 @@
   <arg name="use_vector_map" default="false"/>
   <arg name="namespace" default="/detection/fusion_tools"/>
 
-  <node name="range_vision_fusion_$(arg label)" pkg="range_vision_fusion" type="range_vision_fusion" output="screen">
+  <node name="range_vision_fusion_$(arg label)" pkg="range_vision_fusion" type="range_vision_fusion">
     <param name="label" value="$(arg label)"/>
     <param name="detected_objects_range" value="$(arg detected_objects_range)"/>
     <param name="detected_objects_vision" value="$(arg detected_objects_vision)"/>
@@ -26,21 +26,21 @@
 
   <!-- <group if="$(arg use_vector_map)">
     <node name="object_roi_filter_range_fusion_$(arg label)" pkg="roi_object_filter" type="roi_object_filter"
-          output="screen" ns="$(arg namespace)">
+          ns="$(arg namespace)">
       <param name="objects_src_topic" value="/objects_$(arg label)"/>
       <param name="sync_topics" value="false"/>
       <param name="exception_list" value="[person, bicycle]"/>
       <param name="label" value="$(arg label)" />
     </node>
     <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="range_fusion_visualization_$(arg label)"
-          output="screen" ns="$(arg namespace)">
+          ns="$(arg namespace)">
       <param name="objects_src_topic" value="/objects_filtered_$(arg label)"/>
       <param name="label" value="$(arg label)" />
     </node>
   </group>
   <group unless="$(arg use_vector_map)"> -->
   <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="range_fusion_visualization_$(arg label)"
-        output="screen" ns="$(arg namespace)">
+        ns="$(arg namespace)">
     <param name="objects_src_topic" value="/objects_$(arg label)"/>
     <param name="label" value="$(arg label)" />
     <param name="hull_color" value="[255.,0.,0.,0.8]" />
diff --git a/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp b/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp
index 1ced3fe7..65943460 100644
--- a/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp
+++ b/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp
@@ -717,15 +717,13 @@ ROSRangeVisionFusionApp::Run()
   ros::NodeHandle private_node_handle("~");
 
   // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
   std::string task_response_time_filename;
   int rate;
   double task_minimum_inter_release_time;
   double task_execution_time;
   double task_relative_deadline;
 
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
+  rubis::init_task_profiling(task_response_time_filename);
   
   tf::TransformListener transform_listener;
 
diff --git a/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch b/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch
index 1e50181d..fca05249 100644
--- a/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch
+++ b/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch
@@ -5,7 +5,7 @@
   <arg name="enable_adaptive_estimate" default="True"/>
   <arg name="enable_steering_offset_estimate" default="False"/>
 
-  <node pkg="twist_generator" type="vehicle_status_converter" name="vehicle_status_converter" output="screen">
+  <node pkg="twist_generator" type="vehicle_status_converter" name="vehicle_status_converter">
     <param name="wheelbase" value="$(arg wheelbase)"/>
     <param name="steering_gear_ratio" value="$(arg steering_gear_ratio)"/>
     <param name="enable_adaptive_estimate" value="$(arg enable_adaptive_estimate)"/>
diff --git a/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch b/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch
index b07a0716..c9df1538 100644
--- a/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch
+++ b/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch
@@ -3,7 +3,7 @@
   <arg name="objects_topic_src" default="/detection/image_detector/objects"/>
   <arg name="camera_height" default="1.2"/>
 
-  <node pkg="vision_beyond_track" name="vision_beyond_track_01" type="vision_beyond_track" output="screen">
+  <node pkg="vision_beyond_track" name="vision_beyond_track_01" type="vision_beyond_track">
     <param name="camera_info_src" type="str" value="$(arg camera_info_src)"/>
     <param name="objects_topic_src" type="str" value="$(arg objects_topic_src)"/>
     <param name="camera_height" type="double" value="$(arg camera_height)"/>
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/CMakeLists.txt
index e9bc483d..4adfd03b 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   std_msgs
   rubis_lib
+  rubis_msgs
 )
 
 find_package(CUDA)
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu
index c7f2ef0b..83a0a55f 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu
@@ -156,10 +156,8 @@ __global__ void binary_gradient_array_kernel(float *x, float *dy, int n, int s,
 
 extern "C" void binary_gradient_array_gpu(float *x, float *dx, int n, int size, BINARY_ACTIVATION a, float *y) 
 {
-    request_gpu();
-    binary_gradient_array_kernel<<<cuda_gridsize(n/2), BLOCK>>>(x, dx, n/2, size, a, y);
-    yield_gpu_with_remark("binary_gradient_array_kernel");
-    check_error(cudaPeekAtLastError());
+        binary_gradient_array_kernel<<<cuda_gridsize(n/2), BLOCK>>>(x, dx, n/2, size, a, y);
+        check_error(cudaPeekAtLastError());
 }
 __global__ void binary_activate_array_kernel(float *x, int n, int s, BINARY_ACTIVATION a, float *y)
 {
@@ -173,9 +171,7 @@ __global__ void binary_activate_array_kernel(float *x, int n, int s, BINARY_ACTI
 
 extern "C" void binary_activate_array_gpu(float *x, int n, int size, BINARY_ACTIVATION a, float *y) 
 {
-    request_gpu();
-    binary_activate_array_kernel<<<cuda_gridsize(n/2), BLOCK>>>(x, n/2, size, a, y);
-    yield_gpu_with_remark("binary_activate_array_kernel");
+        binary_activate_array_kernel<<<cuda_gridsize(n/2), BLOCK>>>(x, n/2, size, a, y);
     check_error(cudaPeekAtLastError());
 }
 
@@ -193,16 +189,12 @@ __global__ void gradient_array_kernel(float *x, int n, ACTIVATION a, float *delt
 
 extern "C" void activate_array_gpu(float *x, int n, ACTIVATION a) 
 {
-    request_gpu();
-    activate_array_kernel<<<cuda_gridsize(n), BLOCK>>>(x, n, a);
-    yield_gpu_with_remark("activate_array_kernel");
+        activate_array_kernel<<<cuda_gridsize(n), BLOCK>>>(x, n, a);
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void gradient_array_gpu(float *x, int n, ACTIVATION a, float *delta) 
 {
-    request_gpu();
-    gradient_array_kernel<<<cuda_gridsize(n), BLOCK>>>(x, n, a, delta);
-    yield_gpu_with_remark("gradient_array_kernel");
+        gradient_array_kernel<<<cuda_gridsize(n), BLOCK>>>(x, n, a, delta);
     check_error(cudaPeekAtLastError());
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu
index 5a0473bc..65a521a2 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu
@@ -47,10 +47,7 @@ extern "C" void forward_avgpool_layer_gpu(avgpool_layer layer, network net)
 {
     size_t n = layer.c*layer.batch;
 
-    request_gpu();
-    forward_avgpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.w, layer.h, layer.c, net.input_gpu, layer.output_gpu);
-    yield_gpu_with_remark("forward_avgpool_layer_kernel");
-
+        forward_avgpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.w, layer.h, layer.c, net.input_gpu, layer.output_gpu);
     check_error(cudaPeekAtLastError());
 }
 
@@ -58,9 +55,7 @@ extern "C" void backward_avgpool_layer_gpu(avgpool_layer layer, network net)
 {
     size_t n = layer.c*layer.batch;
 
-    request_gpu();
-    backward_avgpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.w, layer.h, layer.c, net.delta_gpu, layer.delta_gpu);
-    yield_gpu_with_remark("forward_avgpool_layer_kernel");
+        backward_avgpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.w, layer.h, layer.c, net.delta_gpu, layer.delta_gpu);
 
     check_error(cudaPeekAtLastError());
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu
index 95c61fd3..0ad326d9 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu
@@ -23,9 +23,7 @@ void scale_bias_gpu(float *output, float *biases, int batch, int n, int size)
     dim3 dimGrid((size-1)/BLOCK + 1, n, batch);
     dim3 dimBlock(BLOCK, 1, 1);
 
-    request_gpu();
-    scale_bias_kernel<<<dimGrid, dimBlock>>>(output, biases, n, size);
-    yield_gpu_with_remark("scale_bias_kernel");
+        scale_bias_kernel<<<dimGrid, dimBlock>>>(output, biases, n, size);
 
     check_error(cudaPeekAtLastError());
 }
@@ -52,9 +50,7 @@ __global__ void backward_scale_kernel(float *x_norm, float *delta, int batch, in
 
 void backward_scale_gpu(float *x_norm, float *delta, int batch, int n, int size, float *scale_updates)
 {
-    request_gpu();
-    backward_scale_kernel<<<n, BLOCK>>>(x_norm, delta, batch, n, size, scale_updates);
-    yield_gpu_with_remark("backward_scale_kernel");
+        backward_scale_kernel<<<n, BLOCK>>>(x_norm, delta, batch, n, size, scale_updates);
     
     check_error(cudaPeekAtLastError());
 }
@@ -76,9 +72,7 @@ void add_bias_gpu(float *output, float *biases, int batch, int n, int size)
 {
     int num = n*size*batch;
 
-    request_gpu();
-    add_bias_kernel<<<cuda_gridsize(num), BLOCK>>>(output, biases, batch, n, size);
-    yield_gpu_with_remark("add_bias_kernel");
+        add_bias_kernel<<<cuda_gridsize(num), BLOCK>>>(output, biases, batch, n, size);
 
     check_error(cudaPeekAtLastError());
 }
@@ -119,13 +113,9 @@ __global__ void backward_bias_kernel(float *bias_updates, float *delta, int batc
 void backward_bias_gpu(float *bias_updates, float *delta, int batch, int n, int size)
 {
     if(size == 1){
-        request_gpu();
-        backward_bias_conn_kernel<<<cuda_gridsize(n), BLOCK>>>(bias_updates, delta, batch, n);
-        yield_gpu_with_remark("backward_bias_conn_kernel");
+                backward_bias_conn_kernel<<<cuda_gridsize(n), BLOCK>>>(bias_updates, delta, batch, n);
     }else{
-        request_gpu();
-        backward_bias_kernel<<<n, BLOCK>>>(bias_updates, delta, batch, n, size);
-        yield_gpu_with_remark("backward_bias_kernel");
+                backward_bias_kernel<<<n, BLOCK>>>(bias_updates, delta, batch, n, size);
     }
     check_error(cudaPeekAtLastError());
 }
@@ -186,9 +176,7 @@ __global__ void adam_kernel(int N, float *x, float *m, float *v, float B1, float
 
 extern "C" void adam_gpu(int n, float *x, float *m, float *v, float B1, float B2, float rate, float eps, int t)
 {
-    request_gpu();
-    adam_kernel<<<cuda_gridsize(n), BLOCK>>>(n, x, m, v, B1, B2, rate, eps, t);
-    yield_gpu_with_remark("adam_kernel");
+        adam_kernel<<<cuda_gridsize(n), BLOCK>>>(n, x, m, v, B1, B2, rate, eps, t);
     
     check_error(cudaPeekAtLastError());
 }
@@ -229,9 +217,7 @@ extern "C" void normalize_delta_gpu(float *x, float *mean, float *variance, floa
 {
     size_t N = batch*filters*spatial;
 
-    request_gpu();
-    normalize_delta_kernel<<<cuda_gridsize(N), BLOCK>>>(N, x, mean, variance, mean_delta, variance_delta, batch, filters, spatial, delta);
-    yield_gpu_with_remark("normalize_delta_kernel");
+        normalize_delta_kernel<<<cuda_gridsize(N), BLOCK>>>(N, x, mean, variance, mean_delta, variance_delta, batch, filters, spatial, delta);
 
     check_error(cudaPeekAtLastError());
 }
@@ -339,27 +325,21 @@ __global__ void mean_delta_kernel(float *delta, float *variance, int batch, int
 
 extern "C" void mean_delta_gpu(float *delta, float *variance, int batch, int filters, int spatial, float *mean_delta)
 {
-    request_gpu();
-    mean_delta_kernel<<<cuda_gridsize(filters), BLOCK>>>(delta, variance, batch, filters, spatial, mean_delta);
-    yield_gpu_with_remark("mean_delta_kernel");
+        mean_delta_kernel<<<cuda_gridsize(filters), BLOCK>>>(delta, variance, batch, filters, spatial, mean_delta);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void fast_mean_delta_gpu(float *delta, float *variance, int batch, int filters, int spatial, float *mean_delta)
 {
-    request_gpu();
-    fast_mean_delta_kernel<<<filters, BLOCK>>>(delta, variance, batch, filters, spatial, mean_delta);
-    yield_gpu_with_remark("fast_mean_delta_kernel");
+        fast_mean_delta_kernel<<<filters, BLOCK>>>(delta, variance, batch, filters, spatial, mean_delta);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void fast_variance_delta_gpu(float *x, float *delta, float *mean, float *variance, int batch, int filters, int spatial, float *variance_delta)
 {
-    request_gpu();
-    fast_variance_delta_kernel<<<filters, BLOCK>>>(x, delta, mean, variance, batch, filters, spatial, variance_delta);
-    yield_gpu_with_remark("fast_variance_delta_kernel");
+        fast_variance_delta_kernel<<<filters, BLOCK>>>(x, delta, mean, variance, batch, filters, spatial, variance_delta);
 
     check_error(cudaPeekAtLastError());
 }
@@ -495,9 +475,7 @@ extern "C" void normalize_gpu(float *x, float *mean, float *variance, int batch,
 {
     size_t N = batch*filters*spatial;
 
-    request_gpu();
-    normalize_kernel<<<cuda_gridsize(N), BLOCK>>>(N, x, mean, variance, batch, filters, spatial);
-    yield_gpu_with_remark("normalize_kernel");
+        normalize_kernel<<<cuda_gridsize(N), BLOCK>>>(N, x, mean, variance, batch, filters, spatial);
 
     check_error(cudaPeekAtLastError());
 }
@@ -528,9 +506,7 @@ extern "C" void l2normalize_gpu(float *x, float *dx, int batch, int filters, int
 {
     size_t N = batch*spatial;
 
-    request_gpu();
-    l2norm_kernel<<<cuda_gridsize(N), BLOCK>>>(N, x, dx, batch, filters, spatial);
-    yield_gpu_with_remark("l2norm_kernel");
+        l2norm_kernel<<<cuda_gridsize(N), BLOCK>>>(N, x, dx, batch, filters, spatial);
 
     check_error(cudaPeekAtLastError());
 }
@@ -596,18 +572,14 @@ __global__ void  fast_variance_kernel(float *x, float *mean, int batch, int filt
 
 extern "C" void fast_mean_gpu(float *x, int batch, int filters, int spatial, float *mean)
 {
-    request_gpu();
-    fast_mean_kernel<<<filters, BLOCK>>>(x, batch, filters, spatial, mean);
-    yield_gpu_with_remark("fast_mean_kernel");
+        fast_mean_kernel<<<filters, BLOCK>>>(x, batch, filters, spatial, mean);
     
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void fast_variance_gpu(float *x, float *mean, int batch, int filters, int spatial, float *variance)
 {
-    request_gpu();
-    fast_variance_kernel<<<filters, BLOCK>>>(x, mean, batch, filters, spatial, variance);
-    yield_gpu_with_remark("fast_mean_kernel");
+        fast_variance_kernel<<<filters, BLOCK>>>(x, mean, batch, filters, spatial, variance);
 
     check_error(cudaPeekAtLastError());
 }
@@ -615,18 +587,14 @@ extern "C" void fast_variance_gpu(float *x, float *mean, int batch, int filters,
 
 extern "C" void mean_gpu(float *x, int batch, int filters, int spatial, float *mean)
 {
-    request_gpu();
-    mean_kernel<<<cuda_gridsize(filters), BLOCK>>>(x, batch, filters, spatial, mean);
-    yield_gpu_with_remark("mean_kernel");
+        mean_kernel<<<cuda_gridsize(filters), BLOCK>>>(x, batch, filters, spatial, mean);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void variance_gpu(float *x, float *mean, int batch, int filters, int spatial, float *variance)
 {
-    request_gpu();
-    variance_kernel<<<cuda_gridsize(filters), BLOCK>>>(x, mean, batch, filters, spatial, variance);
-    yield_gpu_with_remark("variance_kernel");
+        variance_kernel<<<cuda_gridsize(filters), BLOCK>>>(x, mean, batch, filters, spatial, variance);
 
     check_error(cudaPeekAtLastError());
 }
@@ -638,18 +606,14 @@ extern "C" void axpy_gpu(int N, float ALPHA, float * X, int INCX, float * Y, int
 
 extern "C" void pow_gpu(int N, float ALPHA, float * X, int INCX, float * Y, int INCY)
 {
-    request_gpu();
-    pow_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX, Y, INCY);
-    yield_gpu_with_remark("pow_kernel");
+        pow_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX, Y, INCY);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void axpy_gpu_offset(int N, float ALPHA, float * X, int OFFX, int INCX, float * Y, int OFFY, int INCY)
 {
-    request_gpu();
-    axpy_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, OFFX, INCX, Y, OFFY, INCY);
-    yield_gpu_with_remark("pow_kernel");
+        axpy_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, OFFX, INCX, Y, OFFY, INCY);
 
     check_error(cudaPeekAtLastError());
 }
@@ -661,18 +625,14 @@ extern "C" void copy_gpu(int N, float * X, int INCX, float * Y, int INCY)
 
 extern "C" void mul_gpu(int N, float * X, int INCX, float * Y, int INCY)
 {
-    request_gpu();
-    mul_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, INCX, Y, INCY);
-    yield_gpu_with_remark("mul_kernel");
+        mul_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, INCX, Y, INCY);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void copy_gpu_offset(int N, float * X, int OFFX, int INCX, float * Y, int OFFY, int INCY)
 {
-    request_gpu();
-    copy_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, OFFX, INCX, Y, OFFY, INCY);
-    yield_gpu_with_remark("copy_kernel");
+        copy_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, OFFX, INCX, Y, OFFY, INCY);
 
     check_error(cudaPeekAtLastError());
 }
@@ -698,9 +658,7 @@ extern "C" void flatten_gpu(float *x, int spatial, int layers, int batch, int fo
 {
     int size = spatial*batch*layers;
 
-    request_gpu();
-    flatten_kernel<<<cuda_gridsize(size), BLOCK>>>(size, x, spatial, layers, batch, forward, out);
-    yield_gpu_with_remark("flatten_kernel");
+        flatten_kernel<<<cuda_gridsize(size), BLOCK>>>(size, x, spatial, layers, batch, forward, out);
 
     check_error(cudaPeekAtLastError());
 }
@@ -709,9 +667,7 @@ extern "C" void reorg_gpu(float *x, int w, int h, int c, int batch, int stride,
 {
     int size = w*h*c*batch;
 
-    request_gpu();
-    reorg_kernel<<<cuda_gridsize(size), BLOCK>>>(size, x, w, h, c, batch, stride, forward, out);
-    yield_gpu_with_remark("reorg_kernel");
+        reorg_kernel<<<cuda_gridsize(size), BLOCK>>>(size, x, w, h, c, batch, stride, forward, out);
 
     check_error(cudaPeekAtLastError());
 }
@@ -724,9 +680,7 @@ __global__ void mask_kernel(int n,  float *x, float mask_num, float *mask, float
 
 extern "C" void mask_gpu(int N, float * X, float mask_num, float * mask, float val)
 {
-    request_gpu();
-    mask_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, mask_num, mask, val);
-    yield_gpu_with_remark("mask_kernel");
+        mask_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, mask_num, mask, val);
 
     check_error(cudaPeekAtLastError());
 }
@@ -739,27 +693,21 @@ __global__ void scale_mask_kernel(int n,  float *x, float mask_num, float *mask,
 
 extern "C" void scale_mask_gpu(int N, float * X, float mask_num, float * mask, float scale)
 {
-    request_gpu();
-    scale_mask_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, mask_num, mask, scale);
-    yield_gpu_with_remark("scale_mask_kernel");
+        scale_mask_kernel<<<cuda_gridsize(N), BLOCK>>>(N, X, mask_num, mask, scale);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void const_gpu(int N, float ALPHA, float * X, int INCX)
 {
-    request_gpu();
-    const_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
-    yield_gpu_with_remark("const_kernel");
+        const_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void constrain_gpu(int N, float ALPHA, float * X, int INCX)
 {
-    request_gpu();
-    constrain_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
-    yield_gpu_with_remark("constrain_kernel");
+        constrain_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
 
     check_error(cudaPeekAtLastError());
 }
@@ -767,36 +715,28 @@ extern "C" void constrain_gpu(int N, float ALPHA, float * X, int INCX)
 
 extern "C" void add_gpu(int N, float ALPHA, float * X, int INCX)
 {
-    request_gpu();
-    add_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
-    yield_gpu_with_remark("add_kernel");
+        add_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void scal_gpu(int N, float ALPHA, float * X, int INCX)
 {
-    request_gpu();
-    scal_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
-    yield_gpu_with_remark("scal_kernel");
+        scal_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void supp_gpu(int N, float ALPHA, float * X, int INCX)
 {
-    request_gpu();
-    supp_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
-    yield_gpu_with_remark("supp_kernel");
+        supp_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void fill_gpu(int N, float ALPHA, float * X, int INCX)
 {
-    request_gpu();
-    fill_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
-    yield_gpu_with_remark("fill_kernel");
+        fill_kernel<<<cuda_gridsize(N), BLOCK>>>(N, ALPHA, X, INCX);
 
     check_error(cudaPeekAtLastError());
 }
@@ -834,9 +774,7 @@ extern "C" void shortcut_gpu(int batch, int w1, int h1, int c1, float *add, int
 
     int size = batch * minw * minh * minc;
     
-    request_gpu();
-    shortcut_kernel<<<cuda_gridsize(size), BLOCK>>>(size, minw, minh, minc, stride, sample, batch, w1, h1, c1, add, w2, h2, c2, s1, s2, out);
-    yield_gpu_with_remark("shortcut_kernel");
+        shortcut_kernel<<<cuda_gridsize(size), BLOCK>>>(size, minw, minh, minc, stride, sample, batch, w1, h1, c1, add, w2, h2, c2, s1, s2, out);
 
     check_error(cudaPeekAtLastError());
 }
@@ -860,9 +798,7 @@ __global__ void smooth_l1_kernel(int n, float *pred, float *truth, float *delta,
 
 extern "C" void smooth_l1_gpu(int n, float *pred, float *truth, float *delta, float *error)
 {
-    request_gpu();
-    smooth_l1_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
-    yield_gpu_with_remark("smooth_l1_kernel");
+        smooth_l1_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
 
     check_error(cudaPeekAtLastError());
 }
@@ -880,9 +816,7 @@ __global__ void softmax_x_ent_kernel(int n, float *pred, float *truth, float *de
 
 extern "C" void softmax_x_ent_gpu(int n, float *pred, float *truth, float *delta, float *error)
 {
-    request_gpu();
-    softmax_x_ent_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
-    yield_gpu_with_remark("softmax_x_ent_kernel");
+        softmax_x_ent_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
     
     check_error(cudaPeekAtLastError());
 }
@@ -900,9 +834,7 @@ __global__ void logistic_x_ent_kernel(int n, float *pred, float *truth, float *d
 
 extern "C" void logistic_x_ent_gpu(int n, float *pred, float *truth, float *delta, float *error)
 {
-    request_gpu();
-    logistic_x_ent_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
-    yield_gpu_with_remark("logistic_x_ent_kernel");
+        logistic_x_ent_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
 
     check_error(cudaPeekAtLastError());
 }
@@ -919,9 +851,7 @@ __global__ void l2_kernel(int n, float *pred, float *truth, float *delta, float
 
 extern "C" void l2_gpu(int n, float *pred, float *truth, float *delta, float *error)
 {
-    request_gpu();
-    l2_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
-    yield_gpu_with_remark("l2_kernel");
+        l2_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
 
     check_error(cudaPeekAtLastError());
 }
@@ -938,9 +868,7 @@ __global__ void l1_kernel(int n, float *pred, float *truth, float *delta, float
 
 extern "C" void l1_gpu(int n, float *pred, float *truth, float *delta, float *error)
 {
-    request_gpu();
-    l1_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
-    yield_gpu_with_remark("l1_kernel");
+        l1_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
 
     check_error(cudaPeekAtLastError());
 }
@@ -956,9 +884,7 @@ __global__ void wgan_kernel(int n, float *pred, float *truth, float *delta, floa
 
 extern "C" void wgan_gpu(int n, float *pred, float *truth, float *delta, float *error)
 {
-    request_gpu();
-    wgan_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
-    yield_gpu_with_remark("wgan_kernel");
+        wgan_kernel<<<cuda_gridsize(n), BLOCK>>>(n, pred, truth, delta, error);
 
     check_error(cudaPeekAtLastError());
 }
@@ -990,9 +916,7 @@ __global__ void deinter_kernel(int NX, float *X, int NY, float *Y, int B, float
 
 extern "C" void deinter_gpu(int NX, float *X, int NY, float *Y, int B, float *OUT)
 {
-    request_gpu();
-    deinter_kernel<<<cuda_gridsize((NX+NY)*B), BLOCK>>>(NX, X, NY, Y, B, OUT);
-    yield_gpu_with_remark("deinter_kernel");
+        deinter_kernel<<<cuda_gridsize((NX+NY)*B), BLOCK>>>(NX, X, NY, Y, B, OUT);
 
     check_error(cudaPeekAtLastError());
 }
@@ -1013,18 +937,14 @@ __global__ void inter_kernel(int NX, float *X, int NY, float *Y, int B, float *O
 
 extern "C" void inter_gpu(int NX, float *X, int NY, float *Y, int B, float *OUT)
 {
-    request_gpu();
-    inter_kernel<<<cuda_gridsize((NX+NY)*B), BLOCK>>>(NX, X, NY, Y, B, OUT);
-    yield_gpu_with_remark("inter_kernel");
+        inter_kernel<<<cuda_gridsize((NX+NY)*B), BLOCK>>>(NX, X, NY, Y, B, OUT);
 
     check_error(cudaPeekAtLastError());
 }
 
 extern "C" void weighted_sum_gpu(float *a, float *b, float *s, int num, float *c)
 {
-    request_gpu();
-    weighted_sum_kernel<<<cuda_gridsize(num), BLOCK>>>(num, a, b, s, c);
-    yield_gpu_with_remark("weighted_sum_kernel");
+        weighted_sum_kernel<<<cuda_gridsize(num), BLOCK>>>(num, a, b, s, c);
 
     check_error(cudaPeekAtLastError());
 }
@@ -1041,9 +961,7 @@ __global__ void weighted_delta_kernel(int n, float *a, float *b, float *s, float
 
 extern "C" void weighted_delta_gpu(float *a, float *b, float *s, float *da, float *db, float *ds, int num, float *dc)
 {
-    request_gpu();
-    weighted_delta_kernel<<<cuda_gridsize(num), BLOCK>>>(num, a, b, s, da, db, ds, dc);
-    yield_gpu_with_remark("weighted_delta_kernel");
+        weighted_delta_kernel<<<cuda_gridsize(num), BLOCK>>>(num, a, b, s, da, db, ds, dc);
 
     check_error(cudaPeekAtLastError());
 }
@@ -1058,9 +976,7 @@ __global__ void mult_add_into_kernel(int n, float *a, float *b, float *c)
 
 extern "C" void mult_add_into_gpu(int num, float *a, float *b, float *c)
 {
-    request_gpu();
-    mult_add_into_kernel<<<cuda_gridsize(num), BLOCK>>>(num, a, b, c);
-    yield_gpu_with_remark("mult_add_into_kernel");
+        mult_add_into_kernel<<<cuda_gridsize(num), BLOCK>>>(num, a, b, c);
 
     check_error(cudaPeekAtLastError());
 }
@@ -1113,9 +1029,7 @@ extern "C" void softmax_tree(float *input, int spatial, int batch, int stride, f
      */
     int num = spatial*batch*hier.groups;
 
-    request_gpu();
-    softmax_tree_kernel<<<cuda_gridsize(num), BLOCK>>>(input, spatial, batch, stride, temp, output, hier.groups, tree_groups_size, tree_groups_offset);
-    yield_gpu_with_remark("softmax_tree_kernel");
+        softmax_tree_kernel<<<cuda_gridsize(num), BLOCK>>>(input, spatial, batch, stride, temp, output, hier.groups, tree_groups_size, tree_groups_offset);
 
     check_error(cudaPeekAtLastError());
     cuda_free((float *)tree_groups_size);
@@ -1133,9 +1047,7 @@ __global__ void softmax_kernel(float *input, int n, int batch, int batch_offset,
 
 extern "C" void softmax_gpu(float *input, int n, int batch, int batch_offset, int groups, int group_offset, int stride, float temp, float *output)
 {
-    request_gpu();
-    softmax_kernel<<<cuda_gridsize(batch*groups), BLOCK>>>(input, n, batch, batch_offset, groups, group_offset, stride, temp, output);
-    yield_gpu_with_remark("softmax_tree_kernel");
+        softmax_kernel<<<cuda_gridsize(batch*groups), BLOCK>>>(input, n, batch, batch_offset, groups, group_offset, stride, temp, output);
 
     check_error(cudaPeekAtLastError());
 }
@@ -1168,9 +1080,7 @@ extern "C" void upsample_gpu(float *in, int w, int h, int c, int batch, int stri
 {
     size_t size = w*h*c*batch*stride*stride;
 
-    request_gpu();
-    upsample_kernel<<<cuda_gridsize(size), BLOCK>>>(size, in, w, h, c, batch, stride, forward, scale, out);
-    yield_gpu_with_remark("upsample_kernel");
+        upsample_kernel<<<cuda_gridsize(size), BLOCK>>>(size, in, w, h, c, batch, stride, forward, scale, out);
 
     check_error(cudaPeekAtLastError());
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu
index ad00edde..64a35528 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu
@@ -50,12 +50,10 @@ void col2im_gpu(float *data_col,
     int width_col = (width + 2 * pad - ksize) / stride + 1;
     int num_kernels = channels * height * width;
     
-    request_gpu();
-    col2im_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK,
+        col2im_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK,
         BLOCK>>>(
                 num_kernels, data_col, height, width, ksize, pad,
                 stride, height_col,
                 width_col, data_im);
-    yield_gpu_with_remark("col2im_gpu_kernel");
 }
 
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu
index 7792dc81..95f07477 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu
@@ -22,9 +22,7 @@ __global__ void binarize_kernel(float *x, int n, float *binary)
 
 void binarize_gpu(float *x, int n, float *binary)
 {
-    request_gpu();
-    binarize_kernel<<<cuda_gridsize(n), BLOCK>>>(x, n, binary);
-    yield_gpu_with_remark("binarize_kernel");
+        binarize_kernel<<<cuda_gridsize(n), BLOCK>>>(x, n, binary);
 
     check_error(cudaPeekAtLastError());
 }
@@ -46,9 +44,7 @@ __global__ void binarize_input_kernel(float *input, int n, int size, float *bina
 
 void binarize_input_gpu(float *input, int n, int size, float *binary)
 {
-    request_gpu();
-    binarize_input_kernel<<<cuda_gridsize(size), BLOCK>>>(input, n, size, binary);
-    yield_gpu_with_remark("binarize_input_kernel");
+        binarize_input_kernel<<<cuda_gridsize(size), BLOCK>>>(input, n, size, binary);
 
     check_error(cudaPeekAtLastError());
 }
@@ -72,9 +68,7 @@ __global__ void binarize_weights_kernel(float *weights, int n, int size, float *
 
 void binarize_weights_gpu(float *weights, int n, int size, float *binary)
 {
-    request_gpu();
-    binarize_weights_kernel<<<cuda_gridsize(n), BLOCK>>>(weights, n, size, binary);
-    yield_gpu_with_remark("binarize_weights_kernel");
+        binarize_weights_kernel<<<cuda_gridsize(n), BLOCK>>>(weights, n, size, binary);
 
     check_error(cudaPeekAtLastError());
 }
@@ -177,9 +171,7 @@ extern "C" void smooth_layer(layer l, int size, float rate)
 
     size_t n = h*w*c*l.batch;
 
-    request_gpu();
-    smooth_kernel<<<cuda_gridsize(n), BLOCK>>>(l.output_gpu, n, l.w, l.h, l.c, size, rate, l.delta_gpu);
-    yield_gpu_with_remark("smooth_kernel");
+        smooth_kernel<<<cuda_gridsize(n), BLOCK>>>(l.output_gpu, n, l.w, l.h, l.c, size, rate, l.delta_gpu);
     
     check_error(cudaPeekAtLastError());
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu
index d75e257d..539cfdba 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu
@@ -195,17 +195,13 @@ extern "C" void forward_crop_layer_gpu(crop_layer layer, network net)
 
     int size = layer.batch * layer.w * layer.h;
 
-    request_gpu();
-    levels_image_kernel<<<cuda_gridsize(size), BLOCK>>>(net.input_gpu, layer.rand_gpu, layer.batch, layer.w, layer.h, net.train, layer.saturation, layer.exposure, translate, scale, layer.shift);
-    yield_gpu_with_remark("levels_image_kernel");
+        levels_image_kernel<<<cuda_gridsize(size), BLOCK>>>(net.input_gpu, layer.rand_gpu, layer.batch, layer.w, layer.h, net.train, layer.saturation, layer.exposure, translate, scale, layer.shift);
 
     check_error(cudaPeekAtLastError());
 
     size = layer.batch*layer.c*layer.out_w*layer.out_h;
 
-    request_gpu();
-    forward_crop_layer_kernel<<<cuda_gridsize(size), BLOCK>>>(net.input_gpu, layer.rand_gpu, size, layer.c, layer.h, layer.w, layer.out_h, layer.out_w, net.train, layer.flip, radians, layer.output_gpu);
-    yield_gpu_with_remark("forward_crop_layer_kernel");
+        forward_crop_layer_kernel<<<cuda_gridsize(size), BLOCK>>>(net.input_gpu, layer.rand_gpu, size, layer.c, layer.h, layer.w, layer.out_h, layer.out_w, net.train, layer.flip, radians, layer.output_gpu);
 
     check_error(cudaPeekAtLastError());
 
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c
index a9da26f1..5c1bfc0d 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c
@@ -92,15 +92,11 @@ float *cuda_make_array(float *x, size_t n)
     float *x_gpu;
     size_t size = sizeof(float)*n;
     
-    request_gpu();
-    cudaError_t status = cudaMalloc((void **)&x_gpu, size);
-    yield_gpu_with_remark("cudaMalloc");
+        cudaError_t status = cudaMalloc((void **)&x_gpu, size);
 
     check_error(status);
     if(x){
-        request_gpu();
-        status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice);
-        yield_gpu_with_remark("cuda_make_array");
+                status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice);
         check_error(status);
     } else {
         fill_gpu(n, 0, x_gpu, 1);
@@ -114,25 +110,17 @@ void cuda_random(float *x_gpu, size_t n)
     static curandGenerator_t gen[16];
     static int init[16] = {0};
 
-    request_gpu();
-    int i = cuda_get_device();
-    yield_gpu_with_remark("cuda_get_device");
+        int i = cuda_get_device();
 
     if(!init[i]){
-        request_gpu();
-        curandCreateGenerator(&gen[i], CURAND_RNG_PSEUDO_DEFAULT);
-        yield_gpu_with_remark("curandCreateGenerator");
+                curandCreateGenerator(&gen[i], CURAND_RNG_PSEUDO_DEFAULT);
 
-        request_gpu();
-        curandSetPseudoRandomGeneratorSeed(gen[i], time(0));
-        yield_gpu_with_remark("curandSetPseudoRandomGeneratorSeed");
+                curandSetPseudoRandomGeneratorSeed(gen[i], time(0));
 
         init[i] = 1;
     }
 
-    request_gpu();
-    curandGenerateUniform(gen[i], x_gpu, n);
-    yield_gpu_with_remark("curandGenerateUniform");
+        curandGenerateUniform(gen[i], x_gpu, n);
 
     check_error(cudaPeekAtLastError());
 }
@@ -155,15 +143,11 @@ int *cuda_make_int_array(int *x, size_t n)
     int *x_gpu;
     size_t size = sizeof(int)*n;
 
-    request_gpu();
-    cudaError_t status = cudaMalloc((void **)&x_gpu, size);
-    yield_gpu_with_remark("cudaMalloc");
+        cudaError_t status = cudaMalloc((void **)&x_gpu, size);
 
     check_error(status);
     if(x){
-        request_gpu();
-        status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice);
-        yield_gpu_with_remark("cuda_make_int_array");
+                status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice);
 
         check_error(status);
     }
@@ -173,9 +157,7 @@ int *cuda_make_int_array(int *x, size_t n)
 
 void cuda_free(float *x_gpu)
 {
-    request_gpu();
-    cudaError_t status = cudaFree(x_gpu);
-    yield_gpu_with_remark("free");
+        cudaError_t status = cudaFree(x_gpu);
 
     check_error(status);
 }
@@ -183,9 +165,7 @@ void cuda_free(float *x_gpu)
 void cuda_push_array(float *x_gpu, float *x, size_t n)
 {
     size_t size = sizeof(float)*n;
-    request_gpu();
-    cudaError_t status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice);
-    yield_gpu_with_remark("cuda_push_array");
+        cudaError_t status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice);
 
     check_error(status);
 }
@@ -194,9 +174,7 @@ void cuda_pull_array(float *x_gpu, float *x, size_t n)
 {
     size_t size = sizeof(float)*n;
 
-    request_gpu();
-    cudaError_t status = cudaMemcpy(x, x_gpu, size, cudaMemcpyDeviceToHost);
-    yield_gpu_with_remark("cuda_pull_array");
+        cudaError_t status = cudaMemcpy(x, x_gpu, size, cudaMemcpyDeviceToHost);
 
     check_error(status);
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu
index ad24b679..c6774fa7 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu
@@ -27,9 +27,7 @@ void forward_dropout_layer_gpu(dropout_layer layer, network net)
     cuda_push_array(layer.rand_gpu, layer.rand, size);
     */
 
-    request_gpu();
-    yoloswag420blazeit360noscope<<<cuda_gridsize(size), BLOCK>>>(net.input_gpu, size, layer.rand_gpu, layer.probability, layer.scale);
-    yield_gpu_with_remark("yoloswag420blazeit360noscope_forward");
+        yoloswag420blazeit360noscope<<<cuda_gridsize(size), BLOCK>>>(net.input_gpu, size, layer.rand_gpu, layer.probability, layer.scale);
 
     check_error(cudaPeekAtLastError());
 }
@@ -39,9 +37,7 @@ void backward_dropout_layer_gpu(dropout_layer layer, network net)
     if(!net.delta_gpu) return;
     int size = layer.inputs*layer.batch;
 
-    request_gpu();
-    yoloswag420blazeit360noscope<<<cuda_gridsize(size), BLOCK>>>(net.delta_gpu, size, layer.rand_gpu, layer.probability, layer.scale);
-    yield_gpu_with_remark("yoloswag420blazeit360noscope_backward");
+        yoloswag420blazeit360noscope<<<cuda_gridsize(size), BLOCK>>>(net.delta_gpu, size, layer.rand_gpu, layer.probability, layer.scale);
 
     check_error(cudaPeekAtLastError());
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu
index 0a73ec88..d2ccbdee 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu
@@ -54,11 +54,9 @@ void im2col_gpu(float *im,
     int width_col = (width + 2 * pad - ksize) / stride + 1;
     int num_kernels = channels * height_col * width_col;
 
-    request_gpu();
-    im2col_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK,
+        im2col_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK,
         BLOCK>>>(
                 num_kernels, im, height, width, ksize, pad,
                 stride, height_col,
                 width_col, data_col);
-    yield_gpu_with_remark("im2col_gpu_kernel");
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu
index 285ae322..12d920ee 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu
@@ -92,9 +92,7 @@ extern "C" void forward_maxpool_layer_gpu(maxpool_layer layer, network net)
 
     size_t n = h*w*c*layer.batch;
 
-    request_gpu();
-    forward_maxpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, net.input_gpu, layer.output_gpu, layer.indexes_gpu);
-    yield_gpu_with_remark("im2col_gpu_kernel");
+        forward_maxpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, net.input_gpu, layer.output_gpu, layer.indexes_gpu);
 
     check_error(cudaPeekAtLastError());
 }
@@ -103,9 +101,7 @@ extern "C" void backward_maxpool_layer_gpu(maxpool_layer layer, network net)
 {
     size_t n = layer.h*layer.w*layer.c*layer.batch;
 
-    request_gpu();
-    backward_maxpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, layer.delta_gpu, net.delta_gpu, layer.indexes_gpu);
-    yield_gpu_with_remark("im2col_gpu_kernel");
+        backward_maxpool_layer_kernel<<<cuda_gridsize(n), BLOCK>>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, layer.delta_gpu, net.delta_gpu, layer.indexes_gpu);
 
     check_error(cudaPeekAtLastError());
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch
index a79ffa80..1d3592d7 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch
@@ -7,7 +7,7 @@
   <arg name="camera_id" default="/"/>
   <arg name="image_src" default="/image_raw"/>  
 
-  <node pkg="vision_darknet_detect" name="vision_darknet_detect" type="vision_darknet_detect" output="screen">
+  <node pkg="vision_darknet_detect" name="vision_darknet_detect" type="vision_darknet_detect">
     <param name="score_threshold" type="double" value="$(arg score_threshold)"/>
     <param name="nms_threshold" type="double" value="$(arg nms_threshold)"/>
     <param name="gpu_device_id" type="int" value="$(arg gpu_device_id)"/>
@@ -15,12 +15,11 @@
     <param name="names_file" type="str" value="$(arg names_file)"/>
   </node>
 
-  <node pkg="detected_objects_visualizer" type="visualize_rects" name="yolo3_rects"
-        output="screen">
+  <node pkg="detected_objects_visualizer" type="visualize_rects" name="yolo3_rects">
     <param name="image_src" value="$(arg image_src)"/>
     <param name="image_out" value="/image_rects"/>
-    <param name="object_src" value="/detection/image_detector/objects"/> <!-- this is fixed by definition -->
+    <param name="object_src" value="/detection/image_detector/objects"/>
   </node>
 
-
 </launch>
+
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch
index 6fc73fc4..08d93182 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch
@@ -10,7 +10,7 @@
     <arg name="camera_id" default="/"/>
     <arg name="image_src" default="/image_raw"/>
 
-    <node pkg="vision_darknet_detect" name="vision_darknet_detect" type="vision_darknet_detect" output="screen">
+    <node pkg="vision_darknet_detect" name="vision_darknet_detect" type="vision_darknet_detect">
         <param name="network_definition_file" type="str" value="$(arg network_definition_file)"/>
         <param name="pretrained_model_file" type="str" value="$(arg pretrained_model_file)"/>
         <param name="score_threshold" type="double" value="$(arg score_threshold)"/>
@@ -21,7 +21,7 @@
     </node>
 
     <node pkg="detected_objects_visualizer" type="visualize_rects" name="yolo2_rects"
-          output="screen">
+         >
         <param name="image_src" value="$(arg image_src)"/>
         <param name="image_out" value="/image_rects"/>
         <param name="object_src" value="/detection/image_detector/objects"/> <!-- this is fixed by definition -->
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch
index e1376f17..944cc4da 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch
@@ -7,9 +7,9 @@
   <arg name="pretrained_model_file" default="$(find vision_darknet_detect)/darknet/data/yolov3.weights"/>
   <arg name="names_file" default="$(find vision_darknet_detect)/darknet/cfg/coco.names"/>
   <arg name="camera_id" default="/"/>
-  <arg name="image_src" default="/image_raw"/>  
+  <arg name="image_src" default="/rubis_image_raw"/>  
 
-  <node pkg="vision_darknet_detect" name="vision_darknet_detect" type="vision_darknet_detect" output="screen">
+  <node pkg="vision_darknet_detect" name="vision_darknet_detect" type="vision_darknet_detect">
     <param name="network_definition_file" type="str" value="$(arg network_definition_file)"/>
     <param name="pretrained_model_file" type="str" value="$(arg pretrained_model_file)"/>
     <param name="score_threshold" type="double" value="$(arg score_threshold)"/>
@@ -20,7 +20,7 @@
   </node>
 
   <node pkg="detected_objects_visualizer" type="visualize_rects" name="yolo3_rects"
-        output="screen">
+       >
     <param name="image_src" value="$(arg image_src)"/>
     <param name="image_out" value="/image_rects"/>
     <param name="object_src" value="/detection/image_detector/objects"/> <!-- this is fixed by definition -->
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/package.xml b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/package.xml
index b4f935c0..2af9f0f8 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/package.xml
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/package.xml
@@ -17,4 +17,5 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>rubis_lib</depend>
+  <depend>rubis_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp
index 133eaf33..17c3fb7d 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp
@@ -25,6 +25,11 @@
 #include <sys/syscall.h>
 #include "../darknet/src/cuda.h"
 
+#include "rubis_msgs/Image.h"
+#include "rubis_msgs/DetectedObjectArray.h"
+#include "autoware_msgs/DetectedObject.h"
+#include "autoware_msgs/DetectedObjectArray.h"
+
 #define SPIN_PROFILING
 
 #if (CV_MAJOR_VERSION <= 2)
@@ -51,7 +56,6 @@ namespace darknet
         darknet_network_ = parse_network_cfg(&in_model_file[0]);
         load_weights(darknet_network_, &in_trained_file[0]);
         set_batch_network(darknet_network_, 1);
-
         layer output_layer = darknet_network_->layers[darknet_network_->n - 1];
         darknet_boxes_.resize(output_layer.w * output_layer.h * output_layer.n);
     }
@@ -103,7 +107,7 @@ namespace darknet
     {
         float * in_data = in_darknet_image.data;
         
-        double time = what_time_is_it_now();
+        // double time = what_time_is_it_now();
         
         float *prediction = network_predict(darknet_network_, in_data);
 
@@ -155,6 +159,50 @@ namespace darknet
 
 ///////////////////
 
+void Yolo3DetectorNode::rubis_convert_rect_to_image_obj(std::vector< RectClassScore<float> >& in_objects, rubis_msgs::DetectedObjectArray& out_message)
+{
+    for (unsigned int i = 0; i < in_objects.size(); ++i)
+    {
+        {
+            autoware_msgs::DetectedObject obj;
+
+            obj.x = (in_objects[i].x /image_ratio_) - image_left_right_border_/image_ratio_;
+            obj.y = (in_objects[i].y /image_ratio_) - image_top_bottom_border_/image_ratio_;
+            obj.width = in_objects[i].w /image_ratio_;
+            obj.height = in_objects[i].h /image_ratio_;
+            if (in_objects[i].x < 0)
+                obj.x = 0;
+            if (in_objects[i].y < 0)
+                obj.y = 0;
+            if (in_objects[i].w < 0)
+                obj.width = 0;
+            if (in_objects[i].h < 0)
+                obj.height = 0;
+
+            obj.score = in_objects[i].score;
+            if (use_coco_names_)
+            {
+                obj.label = in_objects[i].GetClassString();
+            }
+            else
+            {
+                if (in_objects[i].class_type < custom_names_.size())
+                    obj.label = custom_names_[in_objects[i].class_type];
+                else
+                    obj.label = "unknown";
+            }
+            obj.valid = true;
+
+            obj.pose.orientation.x = 1;
+            obj.pose.orientation.y = 0;
+            obj.pose.orientation.z = 0;
+            obj.pose.orientation.w = 0;
+            
+            out_message.object_array.objects.push_back(obj);            
+        }
+    }
+}
+
 void Yolo3DetectorNode::convert_rect_to_image_obj(std::vector< RectClassScore<float> >& in_objects, autoware_msgs::DetectedObjectArray& out_message)
 {
     for (unsigned int i = 0; i < in_objects.size(); ++i)
@@ -263,11 +311,42 @@ image Yolo3DetectorNode::convert_ipl_to_image(const sensor_msgs::ImageConstPtr&
     return darknet_image;
 }
 
-void Yolo3DetectorNode::image_callback(const sensor_msgs::ImageConstPtr& in_image_message)
+// BUGS
+void Yolo3DetectorNode::rubis_image_callback(const rubis_msgs::ImageConstPtr& in_image_message)
 {
+    rubis::start_task_profiling();        
+    rubis::instance_ = in_image_message->instance;
+    rubis::lidar_instance_ = in_image_message->instance;
+
+    // sensor_msgs::Image image_test;
+    // rubis_msgs::Image rubis_msg = *in_image_message;
+    // const sensor_msgs::ImageConstPtr& msgPtr = &rubis_msg.msg;
+    sensor_msgs::Image::ConstPtr in_image_msg_origin = boost::make_shared<const sensor_msgs::Image>(in_image_message->msg);
+
     std::vector< RectClassScore<float> > detections;
 
+    darknet_image_ = convert_ipl_to_image(in_image_msg_origin);
+    detections = yolo_detector_.detect(darknet_image_);
+    //Prepare Output message
+    rubis_msgs::DetectedObjectArray output_message;
+    output_message.object_array.header = in_image_msg_origin->header;
+
     
+    rubis_convert_rect_to_image_obj(detections, output_message);
+
+    publisher_rubis_objects_.publish(output_message);
+
+    free(darknet_image_.data);
+
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+}
+
+void Yolo3DetectorNode::image_callback(const sensor_msgs::ImageConstPtr& in_image_message)
+{
+    rubis::start_task_profiling();  
+
+    std::vector< RectClassScore<float> > detections;
+
     darknet_image_ = convert_ipl_to_image(in_image_message);
 
     detections = yolo_detector_.detect(darknet_image_);
@@ -281,13 +360,8 @@ void Yolo3DetectorNode::image_callback(const sensor_msgs::ImageConstPtr& in_imag
     publisher_objects_.publish(output_message);
 
     free(darknet_image_.data);
-    
-    if(is_task_ready_ == TASK_NOT_READY){
-        init_task();
-        if(gpu_profiling_flag_) start_gpu_profiling();
-    }
-    
-    task_state_ = TASK_STATE_DONE;
+
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
 void Yolo3DetectorNode::config_cb(const autoware_config_msgs::ConfigSSD::ConstPtr& param)
@@ -324,55 +398,46 @@ void Yolo3DetectorNode::Run()
     //ROS STUFF
     ros::NodeHandle private_node_handle("~");//to receive args   
 
-    int key_id = 2;
+    // int key_id = 2;
 
     // Scheduling Setup
-    int task_scheduling_flag;
-    int task_profiling_flag;
-    std::string task_response_time_filename_str;
+    // std::string task_response_time_filename_str;
+    // int rate;
+    // double task_minimum_inter_release_time;
+    // double task_execution_time;
+    // double task_relative_deadline;
+
+    // private_node_handle.param<std::string>("/vision_darknet_detect/task_response_time_filename", task_response_time_filename_str, "~/Documents/profiling/response_time/vision_darknet_detect.csv");
+    // private_node_handle.param<int>("/vision_darknet_detect/rate", rate, 10);
+    // private_node_handle.param("/vision_darknet_detect/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
+    // private_node_handle.param("/vision_darknet_detect/task_execution_time", task_execution_time, (double)10);
+    // private_node_handle.param("/vision_darknet_detect/task_relative_deadline", task_relative_deadline, (double)10);
+
+
+    std::string node_name = ros::this_node::getName();
+    std::string task_response_time_filename;
+    private_node_handle.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv");
+
     int rate;
-    double task_minimum_inter_release_time;
-    double task_execution_time;
-    double task_relative_deadline;
-
-    int gpu_scheduling_flag;
-    int gpu_profiling_flag;
-    std::string gpu_execution_time_filename_str;
-    std::string gpu_response_time_filename_str;
-    std::string gpu_deadline_filename_str;
-
-    private_node_handle.param<int>("/vision_darknet_detect/task_scheduling_flag", task_scheduling_flag, 0);
-    private_node_handle.param<int>("/vision_darknet_detect/task_profiling_flag", task_profiling_flag, 0);
-    private_node_handle.param<std::string>("/vision_darknet_detect/task_response_time_filename", task_response_time_filename_str, "~/Documents/profiling/response_time/vision_darknet_detect.csv");
-    private_node_handle.param<int>("/vision_darknet_detect/rate", rate, 10);
-    private_node_handle.param("/vision_darknet_detect/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-    private_node_handle.param("/vision_darknet_detect/task_execution_time", task_execution_time, (double)10);
-    private_node_handle.param("/vision_darknet_detect/task_relative_deadline", task_relative_deadline, (double)10);
-    private_node_handle.param("/vision_darknet_detect/gpu_scheduling_flag", gpu_scheduling_flag, 0);
-    private_node_handle.param("/vision_darknet_detect/gpu_profiling_flag", gpu_profiling_flag, 0);
-    private_node_handle.param<std::string>("/vision_darknet_detect/gpu_execution_time_filename", gpu_execution_time_filename_str, "~/Documents/gpu_profiling/test_yolo_execution_time.csv");
-    private_node_handle.param<std::string>("/vision_darknet_detect/gpu_response_time_filename", gpu_response_time_filename_str, "~/Documents/gpu_profiling/test_yolo_response_time.csv");
-    private_node_handle.param<std::string>("/vision_darknet_detect/gpu_deadline_filename", gpu_deadline_filename_str, "~/Documents/gpu_deadline/yolo_gpu_deadline.csv");
+    private_node_handle.param<int>(node_name+"/rate", rate, 10);
 
-    
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time ,deadline, period;
+
+    private_node_handle.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+    private_node_handle.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+    private_node_handle.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+    private_node_handle.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+    private_node_handle.param(node_name+"/task_scheduling_configs/period", period, 0);
+
+    rubis::init_task_scheduling(policy, attr);
+    rubis::init_task_profiling(task_response_time_filename);
 
     
-    char* task_response_time_filename = strdup(task_response_time_filename_str.c_str());
-    char* gpu_execution_time_filename = strdup(gpu_execution_time_filename_str.c_str());
-    char* gpu_response_time_filename = strdup(gpu_response_time_filename_str.c_str());
-    char* gpu_deadline_filename = strdup(gpu_deadline_filename_str.c_str());
-
-    if(task_profiling_flag) init_task_profiling(task_response_time_filename);
-    if(gpu_profiling_flag) init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename);
-
-    if(gpu_scheduling_flag){
-        init_gpu_scheduling("/tmp/yolo", gpu_deadline_filename, key_id);
-    }else if(gpu_scheduling_flag){
-        ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!");
-        exit(1);
-    }
-    
-    
+    // char* task_response_time_filename = strdup(task_response_time_filename_str.c_str());
+
+
     //RECEIVE IMAGE TOPIC NAME
     std::string image_raw_topic_str;
     if (private_node_handle.getParam("image_raw_node", image_raw_topic_str))
@@ -430,6 +495,7 @@ void Yolo3DetectorNode::Run()
     std::string _network_definition_file = convert_to_absolute_path(network_definition_file);
     std::string _pretrained_model_file = convert_to_absolute_path(pretrained_model_file);
 
+    
     yolo_detector_.load(_network_definition_file, _pretrained_model_file, score_threshold_, nms_threshold_);
     ROS_INFO("Initialization complete.");
 
@@ -439,50 +505,37 @@ void Yolo3DetectorNode::Run()
         generateColors(colors_, 80);
     #endif
 
-    publisher_objects_ = node_handle_.advertise<autoware_msgs::DetectedObjectArray>("/detection/image_detector/objects", 1);
 
     ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str());
-    subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &Yolo3DetectorNode::image_callback, this);    
+
+    if(image_raw_topic_str.find(std::string("rubis")) != std::string::npos){ // For rubis topic
+      subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &Yolo3DetectorNode::rubis_image_callback, this);  
+      publisher_objects_ = node_handle_.advertise<rubis_msgs::DetectedObjectArray>("/detection/image_detector/rubis_objects", 1);
+    }
+    else{// For normal topic      
+      subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &Yolo3DetectorNode::image_callback, this);  
+      publisher_objects_ = node_handle_.advertise<autoware_msgs::DetectedObjectArray>("/detection/image_detector/objects", 1);
+    }
+
     std::string config_topic("/config");
     config_topic += "/Yolo3";
     subscriber_yolo_config_ = node_handle_.subscribe(config_topic, 1, &Yolo3DetectorNode::config_cb, this);
 
     ROS_INFO_STREAM( __APP_NAME__ << "" );
 
-    if(!task_scheduling_flag && !task_profiling_flag){
-        ros::spin();
-    }
-    else{
-        ros::Rate r(rate);
-        // Initialize task ( Wait until first necessary topic is published )
-        while(ros::ok()){
-            if(is_task_ready_) break;
-            ros::spinOnce();
-            r.sleep();      
-        }
+    ros::Rate r(rate);
 
-        // Executing task
-        while(ros::ok()){
-            if(task_profiling_flag) start_task_profiling();          
-            if(task_state_ == TASK_STATE_READY){
-                if(task_scheduling_flag) request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-                if(gpu_profiling_flag || gpu_scheduling_flag) start_job();
-                task_state_ = TASK_STATE_RUNNING;     
-            }
+    // Executing task
+    // while(ros::ok()){
+    //     start_task_profiling();          
 
-            ros::spinOnce();
+    //     ros::spinOnce();
 
-            if(task_profiling_flag) stop_task_profiling(0, task_state_);
+    //     stop_task_profiling(0, 0);
 
-            if(task_state_ == TASK_STATE_DONE){
-                if(gpu_profiling_flag || gpu_scheduling_flag) finish_job();
-                if(task_scheduling_flag) yield_task_scheduling();
-                task_state_ = TASK_STATE_READY;
-            }
-            
-            r.sleep();
-        }
-    }
-    ROS_INFO("END Yolo");
+    //     r.sleep();
+    // }
+    ros::spin();
 
+    ROS_INFO("END Yolo");
 }
diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.h b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.h
index 7a69dadb..db46157f 100644
--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.h
+++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.h
@@ -38,6 +38,10 @@
 #include <autoware_config_msgs/ConfigSSD.h>
 #include <autoware_msgs/DetectedObject.h>
 #include <autoware_msgs/DetectedObjectArray.h>
+#include <sensor_msgs/Image.h>
+#include <rubis_msgs/Image.h>
+#include <rubis_msgs/DetectedObjectArray.h>
+#include <rubis_lib/sched.hpp>
 
 #include <rect_class_score.h>
 
@@ -55,7 +59,6 @@ extern "C"
 #include "utils.h"
 #include "image.h"
 #include "../darknet/src/cuda.h"
-#include "rubis_lib/sched_c.h"
 #define __cplusplus
 }
 
@@ -88,8 +91,10 @@ namespace darknet {
 
 class Yolo3DetectorNode {
     ros::Subscriber                 subscriber_image_raw_;
+    ros::Subscriber                 subscriber_rubis_image_raw_;
     ros::Subscriber                 subscriber_yolo_config_;
     ros::Publisher                  publisher_objects_;
+    ros::Publisher                  publisher_rubis_objects_;
     ros::NodeHandle                 node_handle_;
 
     darknet::Yolo3Detector          yolo_detector_;
@@ -111,9 +116,12 @@ class Yolo3DetectorNode {
 
     void                            convert_rect_to_image_obj(std::vector< RectClassScore<float> >& in_objects,
                                       autoware_msgs::DetectedObjectArray& out_message);
+    void                            rubis_convert_rect_to_image_obj(std::vector< RectClassScore<float> >& in_objects,
+                                      rubis_msgs::DetectedObjectArray& out_message);
     void                            rgbgr_image(image& im);
     image                           convert_ipl_to_image(const sensor_msgs::ImageConstPtr& msg);
     void                            image_callback(const sensor_msgs::ImageConstPtr& in_image_message);
+    void                            rubis_image_callback(const rubis_msgs::ImageConstPtr& in_image_message);
     void                            config_cb(const autoware_config_msgs::ConfigSSD::ConstPtr& param);
     std::vector<std::string>        read_custom_names_file(const std::string& in_path);
     std::string                     convert_to_absolute_path(std::string relative_path);
diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/op_global_planner/CMakeLists.txt
index 3f9e3869..8e1cb452 100644
--- a/autoware.ai/src/autoware/core_planning/op_global_planner/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_planning/op_global_planner/CMakeLists.txt
@@ -5,7 +5,7 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(
   catkin REQUIRED COMPONENTS
@@ -15,7 +15,6 @@ find_package(
     libwaypoint_follower
     op_planner
     op_ros_helpers
-    op_simu
     op_utility
     pcl_conversions
     pcl_ros
@@ -24,6 +23,8 @@ find_package(
     tf
     vector_map_msgs
     rubis_lib
+    nav_msgs
+    rubis_msgs
 )
 
 catkin_package(
diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/include/op_global_planner_core.h b/autoware.ai/src/autoware/core_planning/op_global_planner/include/op_global_planner_core.h
index 6e6bb021..2c5e1c9b 100644
--- a/autoware.ai/src/autoware/core_planning/op_global_planner/include/op_global_planner_core.h
+++ b/autoware.ai/src/autoware/core_planning/op_global_planner/include/op_global_planner_core.h
@@ -51,6 +51,8 @@
 #include "op_planner/MappingHelpers.h"
 #include "op_planner/PlannerH.h"
 
+#include "rubis_msgs/PoseTwistStamped.h"
+
 namespace GlobalPlanningNS
 {
 
@@ -130,6 +132,7 @@ class GlobalPlanner
   ros::Subscriber sub_current_velocity;
   ros::Subscriber sub_can_info;
   ros::Subscriber sub_road_status_occupancy;
+  ros::Subscriber sub_pose_twist;
 
 public:
   GlobalPlanner();
@@ -149,6 +152,7 @@ class GlobalPlanner
   void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg);
   void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg);
   void callbackGetRoadStatusOccupancyGrid(const nav_msgs::OccupancyGridConstPtr& msg);
+  void callbackPoseTwist(const rubis_msgs::PoseTwistStampedConstPtr& msg);
 
   protected:
     PlannerHNS::RoadNetwork m_Map;
diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch b/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch
index de166464..46f665b2 100644
--- a/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch
+++ b/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch
@@ -9,6 +9,7 @@
   <arg name="mapSource"             default="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
   <arg name="mapFileName"           default="" /> <!-- incase of kml map source -->
   <arg name="enableDynamicMapUpdate"       default="false" />  
+  <arg name="use_svl_sensor"        default="falst" />
   <arg name="use_static_goal"       default="false" />
   <arg name="goal_pose_x"           default="0.0" />
   <arg name="goal_pose_y"           default="0.0" />
@@ -18,10 +19,7 @@
   <arg name="goal_ori_z"           default="0.0" />
   <arg name="goal_ori_w"           default="0.0" />
 
-  <arg name="LaneInfoFile"          default="cubetown_autorunner/default_lane_info.yaml" />
-
   <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner" output="screen">    
-    <rosparam command="load" file="$(find rubis_autorunner)/cfg/$(arg LaneInfoFile)"/>
     <param name="pathDensity"         value="$(arg pathDensity)" />
     <param name="enableSmoothing"       value="$(arg enableSmoothing)" />
     <param name="enableLaneChange"       value="$(arg enableLaneChange)" />
@@ -31,6 +29,7 @@
     <param name="mapSource"         value="$(arg mapSource)" />
     <param name="mapFileName"         value="$(arg mapFileName)" />    
     <param name="enableDynamicMapUpdate"   value="$(arg enableDynamicMapUpdate)" />    
+    <param name="use_svl_sensor"        value="$(arg use_svl_sensor)" />
     <param name="use_static_goal"       value="$(arg use_static_goal)"/>
     <param name="goal_pose_x"           value="$(arg goal_pose_x)" />
     <param name="goal_pose_y"           value="$(arg goal_pose_y)" />
diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp
index baa5bc9d..544b4190 100644
--- a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp
+++ b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp
@@ -22,713 +22,841 @@
 
 #define MINICAR_SCALE 0.3
 
-namespace GlobalPlanningNS
-{
-
-GlobalPlanner::GlobalPlanner()
-{
-  m_pCurrGoal = 0;
-  m_iCurrentGoalIndex = 0;
-  m_bKmlMap = false;
-  m_bFirstStart = false;
-  m_GlobalPathID = 1;
-  UtilityHNS::UtilityH::GetTickCount(m_ReplnningTimer);
-
-  nh.getParam("/op_global_planner/pathDensity" , m_params.pathDensity);
-  nh.getParam("/op_global_planner/enableSmoothing" , m_params.bEnableSmoothing);
-  nh.getParam("/op_global_planner/enableLaneChange" , m_params.bEnableLaneChange);
-  nh.getParam("/op_global_planner/enableRvizInput" , m_params.bEnableRvizInput);
-  nh.getParam("/op_global_planner/enableReplan" , m_params.bEnableReplanning);
-  nh.getParam("/op_global_planner/enableDynamicMapUpdate" , m_params.bEnableDynamicMapUpdate);
-  nh.getParam("/op_global_planner/mapFileName" , m_params.KmlMapPath);
-
-  bool use_static_goal = false;
-  double goal_pose_x, goal_pose_y, goal_pose_z, goal_ori_x, goal_ori_y, goal_ori_z, goal_ori_w;
-  nh.param<bool>("/op_global_planner/use_static_goal", use_static_goal, false);
-  nh.param<double>("/op_global_planner/goal_pose_x", goal_pose_x, -1);
-  nh.param<double>("/op_global_planner/goal_pose_y", goal_pose_y, -1);
-  nh.param<double>("/op_global_planner/goal_pose_z", goal_pose_z, -1);
-  nh.param<double>("/op_global_planner/goal_ori_x", goal_ori_x, -1);
-  nh.param<double>("/op_global_planner/goal_ori_y", goal_ori_y, -1);
-  nh.param<double>("/op_global_planner/goal_ori_z", goal_ori_z, -1);
-  nh.param<double>("/op_global_planner/goal_ori_w", goal_ori_w, -1);  
-
-  int iSource = 0;
-  nh.getParam("/op_global_planner/mapSource", iSource);
-  if(iSource == 0)
-    m_params.mapSource = PlannerHNS::MAP_AUTOWARE;
-  else if (iSource == 1)
-    m_params.mapSource = PlannerHNS::MAP_FOLDER;
-  else if(iSource == 2)
-    m_params.mapSource = PlannerHNS::MAP_KML_FILE;
-
-  tf::StampedTransform transform;
-  PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
-  m_OriginPos.position.x  = transform.getOrigin().x();
-  m_OriginPos.position.y  = transform.getOrigin().y();
-  m_OriginPos.position.z  = transform.getOrigin().z();
-
-  if(use_static_goal == true){
-    geometry_msgs::Quaternion orientation;
-    orientation.x = goal_ori_x;
-    orientation.y = goal_ori_y;
-    orientation.z = goal_ori_z;
-    orientation.w = goal_ori_w;
-    PlannerHNS::WayPoint wp = PlannerHNS::WayPoint(goal_pose_x, goal_pose_y, goal_pose_z, tf::getYaw(orientation));
-    m_GoalsPos.push_back(wp);
-  }
-
-  pub_Paths = nh.advertise<autoware_msgs::LaneArray>("lane_waypoints_array", 1, true);
-  pub_PathsRviz = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_rviz", 1, true);
-  pub_MapRviz  = nh.advertise<visualization_msgs::MarkerArray>("vector_map_center_lines_rviz", 1, true);
-  pub_GoalsListRviz = nh.advertise<visualization_msgs::MarkerArray>("op_destinations_rviz", 1, true);
-  pub_PathsRvizMinicar = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_rviz_minicar", 1, true);
-  pub_MapRvizMinicar = nh.advertise<visualization_msgs::MarkerArray>("vector_map_center_lines_rviz_minicar", 1, true);
-
-  if(m_params.bEnableRvizInput)
-  {
-    sub_start_pose = nh.subscribe("/initialpose", 1, &GlobalPlanner::callbackGetStartPose, this);
-    sub_goal_pose = nh.subscribe("move_base_simple/goal", 1, &GlobalPlanner::callbackGetGoalPose, this);
-  }
-  else
-  {
-    LoadSimulationData();
-  }
-
-  sub_current_pose = nh.subscribe("/current_pose", 10, &GlobalPlanner::callbackGetCurrentPose, this); // origin: 10
-
-  int bVelSource = 1;
-  nh.getParam("/op_global_planner/velocitySource", bVelSource);
-  if(bVelSource == 0)
-    sub_robot_odom = nh.subscribe("/odom", 10, &GlobalPlanner::callbackGetRobotOdom, this); // origin: 10
-  else if(bVelSource == 1)
-    sub_current_velocity = nh.subscribe("/current_velocity", 10, &GlobalPlanner::callbackGetVehicleStatus, this); // origin: 10
-  else if(bVelSource == 2)
-    sub_can_info = nh.subscribe("/can_info", 10, &GlobalPlanner::callbackGetCANInfo, this); // origin: 10
-
-  /*  RT Scheduling setup  */
-  // sub_current_pose = nh.subscribe("/current_pose", 1, &GlobalPlanner::callbackGetCurrentPose, this); // origin: 10
-  // if(bVelSource == 0)
-  //   sub_robot_odom = nh.subscribe("/odom", 1, &GlobalPlanner::callbackGetRobotOdom, this); // origin: 10
-  // else if(bVelSource == 1)
-  //   sub_current_velocity = nh.subscribe("/current_velocity", 1, &GlobalPlanner::callbackGetVehicleStatus, this); // origin: 10
-  // else if(bVelSource == 2)
-  //   sub_can_info = nh.subscribe("/can_info", 1, &GlobalPlanner::callbackGetCANInfo, this); // origin: 10
-
-  if(m_params.bEnableDynamicMapUpdate)
-    sub_road_status_occupancy = nh.subscribe<>("/occupancy_road_status", 1, &GlobalPlanner::callbackGetRoadStatusOccupancyGrid, this);
-
-  //Mapping Section
-  sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &GlobalPlanner::callbackGetVMLanes,  this);
-  sub_points = nh.subscribe("/vector_map_info/point", 1, &GlobalPlanner::callbackGetVMPoints,  this);
-  sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &GlobalPlanner::callbackGetVMdtLanes,  this);
-  sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &GlobalPlanner::callbackGetVMIntersections,  this);
-  sup_area = nh.subscribe("/vector_map_info/area", 1, &GlobalPlanner::callbackGetVMAreas,  this);
-  sub_lines = nh.subscribe("/vector_map_info/line", 1, &GlobalPlanner::callbackGetVMLines,  this);
-  sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &GlobalPlanner::callbackGetVMStopLines,  this);
-  sub_signals = nh.subscribe("/vector_map_info/signal", 1, &GlobalPlanner::callbackGetVMSignal,  this);
-  sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &GlobalPlanner::callbackGetVMVectors,  this);
-  sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &GlobalPlanner::callbackGetVMCurbs,  this);
-  sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &GlobalPlanner::callbackGetVMRoadEdges,  this);
-  sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &GlobalPlanner::callbackGetVMWayAreas,  this);
-  sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &GlobalPlanner::callbackGetVMCrossWalks,  this);
-  sub_nodes = nh.subscribe("/vector_map_info/node", 1, &GlobalPlanner::callbackGetVMNodes,  this);
+namespace GlobalPlanningNS {
+
+GlobalPlanner::GlobalPlanner() {
+    m_pCurrGoal = 0;
+    m_iCurrentGoalIndex = 0;
+    m_bKmlMap = false;
+    m_bFirstStart = false;
+    m_GlobalPathID = 1;
+    UtilityHNS::UtilityH::GetTickCount(m_ReplnningTimer);
+
+    nh.getParam("/op_global_planner/pathDensity", m_params.pathDensity);
+    nh.getParam("/op_global_planner/enableSmoothing",
+                m_params.bEnableSmoothing);
+    nh.getParam("/op_global_planner/enableLaneChange",
+                m_params.bEnableLaneChange);
+    nh.getParam("/op_global_planner/enableRvizInput",
+                m_params.bEnableRvizInput);
+    nh.getParam("/op_global_planner/enableReplan", m_params.bEnableReplanning);
+    nh.getParam("/op_global_planner/enableDynamicMapUpdate",
+                m_params.bEnableDynamicMapUpdate);
+    nh.getParam("/op_global_planner/mapFileName", m_params.KmlMapPath);
+
+    bool use_static_goal = false;
+    double goal_pose_x, goal_pose_y, goal_pose_z, goal_ori_x, goal_ori_y,
+        goal_ori_z, goal_ori_w;
+    nh.param<bool>("/op_global_planner/use_static_goal", use_static_goal,
+                   false);
+    nh.param<double>("/op_global_planner/goal_pose_x", goal_pose_x, -1);
+    nh.param<double>("/op_global_planner/goal_pose_y", goal_pose_y, -1);
+    nh.param<double>("/op_global_planner/goal_pose_z", goal_pose_z, -1);
+    nh.param<double>("/op_global_planner/goal_ori_x", goal_ori_x, -1);
+    nh.param<double>("/op_global_planner/goal_ori_y", goal_ori_y, -1);
+    nh.param<double>("/op_global_planner/goal_ori_z", goal_ori_z, -1);
+    nh.param<double>("/op_global_planner/goal_ori_w", goal_ori_w, -1);
+
+    int iSource = 0;
+    nh.getParam("/op_global_planner/mapSource", iSource);
+    if (iSource == 0)
+        m_params.mapSource = PlannerHNS::MAP_AUTOWARE;
+    else if (iSource == 1)
+        m_params.mapSource = PlannerHNS::MAP_FOLDER;
+    else if (iSource == 2)
+        m_params.mapSource = PlannerHNS::MAP_KML_FILE;
+
+    tf::StampedTransform transform;
+    PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
+    m_OriginPos.position.x = transform.getOrigin().x();
+    m_OriginPos.position.y = transform.getOrigin().y();
+    m_OriginPos.position.z = transform.getOrigin().z();
+
+    if (use_static_goal == true) {
+        geometry_msgs::Quaternion orientation;
+        orientation.x = goal_ori_x;
+        orientation.y = goal_ori_y;
+        orientation.z = goal_ori_z;
+        orientation.w = goal_ori_w;
+        PlannerHNS::WayPoint wp = PlannerHNS::WayPoint(
+            goal_pose_x, goal_pose_y, goal_pose_z, tf::getYaw(orientation));
+        m_GoalsPos.push_back(wp);
+    }
+
+    pub_Paths =
+        nh.advertise<autoware_msgs::LaneArray>("lane_waypoints_array", 1, true);
+    pub_PathsRviz = nh.advertise<visualization_msgs::MarkerArray>(
+        "global_waypoints_rviz", 1, true);
+    pub_MapRviz = nh.advertise<visualization_msgs::MarkerArray>(
+        "vector_map_center_lines_rviz", 1, true);
+    pub_GoalsListRviz = nh.advertise<visualization_msgs::MarkerArray>(
+        "op_destinations_rviz", 1, true);
+    pub_PathsRvizMinicar = nh.advertise<visualization_msgs::MarkerArray>(
+        "global_waypoints_rviz_minicar", 1, true);
+    pub_MapRvizMinicar = nh.advertise<visualization_msgs::MarkerArray>(
+        "vector_map_center_lines_rviz_minicar", 1, true);
+
+    if (m_params.bEnableRvizInput) {
+        sub_start_pose = nh.subscribe(
+            "/initialpose", 1, &GlobalPlanner::callbackGetStartPose, this);
+        sub_goal_pose = nh.subscribe("move_base_simple/goal", 1,
+                                     &GlobalPlanner::callbackGetGoalPose, this);
+    } else {
+        LoadSimulationData();
+    }
+
+    bool use_svl_sensor;
+    nh.param<bool>("/op_global_planner/use_svl_sensor", use_svl_sensor, false);
+    if (use_svl_sensor) {
+        sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 10,
+                                      &GlobalPlanner::callbackPoseTwist, this);
+    } else {
+        sub_current_pose = nh.subscribe("/current_pose", 10,
+                                        &GlobalPlanner::callbackGetCurrentPose,
+                                        this); // origin: 10
+
+        int bVelSource = 1;
+        nh.getParam("/op_global_planner/velocitySource", bVelSource);
+        if (bVelSource == 0)
+            sub_robot_odom =
+                nh.subscribe("/odom", 10, &GlobalPlanner::callbackGetRobotOdom,
+                             this); // origin: 10
+        else if (bVelSource == 1)
+            sub_current_velocity = nh.subscribe(
+                "/current_velocity", 10,
+                &GlobalPlanner::callbackGetVehicleStatus, this); // origin: 10
+        else if (bVelSource == 2)
+            sub_can_info = nh.subscribe("/can_info", 10,
+                                        &GlobalPlanner::callbackGetCANInfo,
+                                        this); // origin: 10
+    }
 
+    if (m_params.bEnableDynamicMapUpdate)
+        sub_road_status_occupancy = nh.subscribe<>(
+            "/occupancy_road_status", 1,
+            &GlobalPlanner::callbackGetRoadStatusOccupancyGrid, this);
+
+    // Mapping Section
+    sub_lanes = nh.subscribe("/vector_map_info/lane", 1,
+                             &GlobalPlanner::callbackGetVMLanes, this);
+    sub_points = nh.subscribe("/vector_map_info/point", 1,
+                              &GlobalPlanner::callbackGetVMPoints, this);
+    sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1,
+                                &GlobalPlanner::callbackGetVMdtLanes, this);
+    sub_intersect =
+        nh.subscribe("/vector_map_info/cross_road", 1,
+                     &GlobalPlanner::callbackGetVMIntersections, this);
+    sup_area = nh.subscribe("/vector_map_info/area", 1,
+                            &GlobalPlanner::callbackGetVMAreas, this);
+    sub_lines = nh.subscribe("/vector_map_info/line", 1,
+                             &GlobalPlanner::callbackGetVMLines, this);
+    sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1,
+                                 &GlobalPlanner::callbackGetVMStopLines, this);
+    sub_signals = nh.subscribe("/vector_map_info/signal", 1,
+                               &GlobalPlanner::callbackGetVMSignal, this);
+    sub_vectors = nh.subscribe("/vector_map_info/vector", 1,
+                               &GlobalPlanner::callbackGetVMVectors, this);
+    sub_curbs = nh.subscribe("/vector_map_info/curb", 1,
+                             &GlobalPlanner::callbackGetVMCurbs, this);
+    sub_edges = nh.subscribe("/vector_map_info/road_edge", 1,
+                             &GlobalPlanner::callbackGetVMRoadEdges, this);
+    sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1,
+                                 &GlobalPlanner::callbackGetVMWayAreas, this);
+    sub_cross_walk =
+        nh.subscribe("/vector_map_info/cross_walk", 1,
+                     &GlobalPlanner::callbackGetVMCrossWalks, this);
+    sub_nodes = nh.subscribe("/vector_map_info/node", 1,
+                             &GlobalPlanner::callbackGetVMNodes, this);
 }
 
-GlobalPlanner::~GlobalPlanner()
-{
-  if(m_params.bEnableRvizInput)
-    SaveSimulationData();
+GlobalPlanner::~GlobalPlanner() {
+    if (m_params.bEnableRvizInput)
+        SaveSimulationData();
 }
 
-void GlobalPlanner::callbackGetRoadStatusOccupancyGrid(const nav_msgs::OccupancyGridConstPtr& msg)
-{
-//  std::cout << "Occupancy Grid Origin (" << msg->info.origin.position.x << ", " << msg->info.origin.position.x << ") , " << msg->header.frame_id << ", Res: " << msg->info.resolution <<  std::endl;
-
-  m_GridMapIntType.clear();
-
-  //std::cout << "Found Map Data: Zero " <<  std::endl;
-  for(unsigned int i=0; i < msg->data.size(); i++)
-  {
-    if((int8_t)msg->data.at(i) == 0)
-      m_GridMapIntType.push_back(0);
-    else if((int8_t)msg->data.at(i) == 50)
-      m_GridMapIntType.push_back(75);
-    else if((int8_t)msg->data.at(i) == 100)
-      m_GridMapIntType.push_back(255);
-    else
-      m_GridMapIntType.push_back(128);
-
-      //std::cout << msg->data.at(i) << ",";
-  }
-  //std::cout << std::endl << "--------------------------------------------------------" << std::endl;
-
-  //std::cout << "Found Map Data: Zero : " << m_GridMapIntType.size() <<  std::endl;
-  PlannerHNS::WayPoint center(msg->info.origin.position.x, msg->info.origin.position.y, msg->info.origin.position.z, tf::getYaw(msg->info.origin.orientation));
-  PlannerHNS::OccupancyToGridMap grid(msg->info.width,msg->info.height, msg->info.resolution, center);
-  std::vector<PlannerHNS::WayPoint*> modified_nodes;
-  timespec t;
-  UtilityHNS::UtilityH::GetTickCount(t);
-  PlannerHNS::MappingHelpers::UpdateMapWithOccupancyGrid(grid, m_GridMapIntType, m_Map, modified_nodes);
-  m_ModifiedMapItemsTimes.push_back(std::make_pair(modified_nodes, t));
-
-  visualization_msgs::MarkerArray map_marker_array;
-  PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array);
-
-//  visualization_msgs::Marker mkr = PlannerHNS::ROSHelpers::CreateGenMarker(center.pos.x, center.pos.y, center.pos.z, 0, 0,0,1,0.5, 1000, "TestCenter", visualization_msgs::Marker::SPHERE);
-//
-//  map_marker_array.markers.push_back(mkr);
-
-  pub_MapRviz.publish(map_marker_array);
+void GlobalPlanner::callbackGetRoadStatusOccupancyGrid(
+    const nav_msgs::OccupancyGridConstPtr &msg) {
+    //  std::cout << "Occupancy Grid Origin (" << msg->info.origin.position.x <<
+    //  ", " << msg->info.origin.position.x << ") , " << msg->header.frame_id <<
+    //  ", Res: " << msg->info.resolution <<  std::endl;
+
+    m_GridMapIntType.clear();
+
+    // std::cout << "Found Map Data: Zero " <<  std::endl;
+    for (unsigned int i = 0; i < msg->data.size(); i++) {
+        if ((int8_t)msg->data.at(i) == 0)
+            m_GridMapIntType.push_back(0);
+        else if ((int8_t)msg->data.at(i) == 50)
+            m_GridMapIntType.push_back(75);
+        else if ((int8_t)msg->data.at(i) == 100)
+            m_GridMapIntType.push_back(255);
+        else
+            m_GridMapIntType.push_back(128);
+
+        // std::cout << msg->data.at(i) << ",";
+    }
+    // std::cout << std::endl <<
+    // "--------------------------------------------------------" << std::endl;
+
+    // std::cout << "Found Map Data: Zero : " << m_GridMapIntType.size() <<
+    // std::endl;
+    PlannerHNS::WayPoint center(
+        msg->info.origin.position.x, msg->info.origin.position.y,
+        msg->info.origin.position.z, tf::getYaw(msg->info.origin.orientation));
+    PlannerHNS::OccupancyToGridMap grid(msg->info.width, msg->info.height,
+                                        msg->info.resolution, center);
+    std::vector<PlannerHNS::WayPoint *> modified_nodes;
+    timespec t;
+    UtilityHNS::UtilityH::GetTickCount(t);
+    PlannerHNS::MappingHelpers::UpdateMapWithOccupancyGrid(
+        grid, m_GridMapIntType, m_Map, modified_nodes);
+    m_ModifiedMapItemsTimes.push_back(std::make_pair(modified_nodes, t));
+
+    visualization_msgs::MarkerArray map_marker_array;
+    PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(
+        m_Map, map_marker_array);
+
+    //  visualization_msgs::Marker mkr =
+    //  PlannerHNS::ROSHelpers::CreateGenMarker(center.pos.x, center.pos.y,
+    //  center.pos.z, 0, 0,0,1,0.5, 1000, "TestCenter",
+    //  visualization_msgs::Marker::SPHERE);
+    //
+    //  map_marker_array.markers.push_back(mkr);
+
+    pub_MapRviz.publish(map_marker_array);
 }
 
-void GlobalPlanner::ClearOldCostFromMap()
-{
-  for(int i=0; i < (int)m_ModifiedMapItemsTimes.size(); i++)
-  {
-    if(UtilityHNS::UtilityH::GetTimeDiffNow(m_ModifiedMapItemsTimes.at(i).second) > CLEAR_COSTS_TIME)
-    {
-      for(unsigned int j= 0 ; j < m_ModifiedMapItemsTimes.at(i).first.size(); j++)
-      {
-        for(unsigned int i_action=0; i_action < m_ModifiedMapItemsTimes.at(i).first.at(j)->actionCost.size(); i_action++)
-        {
-          if(m_ModifiedMapItemsTimes.at(i).first.at(j)->actionCost.at(i_action).first == PlannerHNS::FORWARD_ACTION)
-          {
-            m_ModifiedMapItemsTimes.at(i).first.at(j)->actionCost.at(i_action).second = 0;
-          }
-        }
-      }
+void GlobalPlanner::ClearOldCostFromMap() {
+    for (int i = 0; i < (int)m_ModifiedMapItemsTimes.size(); i++) {
+        if (UtilityHNS::UtilityH::GetTimeDiffNow(
+                m_ModifiedMapItemsTimes.at(i).second) > CLEAR_COSTS_TIME) {
+            for (unsigned int j = 0;
+                 j < m_ModifiedMapItemsTimes.at(i).first.size(); j++) {
+                for (unsigned int i_action = 0;
+                     i_action < m_ModifiedMapItemsTimes.at(i)
+                                    .first.at(j)
+                                    ->actionCost.size();
+                     i_action++) {
+                    if (m_ModifiedMapItemsTimes.at(i)
+                            .first.at(j)
+                            ->actionCost.at(i_action)
+                            .first == PlannerHNS::FORWARD_ACTION) {
+                        m_ModifiedMapItemsTimes.at(i)
+                            .first.at(j)
+                            ->actionCost.at(i_action)
+                            .second = 0;
+                    }
+                }
+            }
 
-      m_ModifiedMapItemsTimes.erase(m_ModifiedMapItemsTimes.begin()+i);
-      i--;
+            m_ModifiedMapItemsTimes.erase(m_ModifiedMapItemsTimes.begin() + i);
+            i--;
+        }
     }
-  }
 }
 
-void GlobalPlanner::callbackGetGoalPose(const geometry_msgs::PoseStampedConstPtr &msg)
-{
-  PlannerHNS::WayPoint wp = PlannerHNS::WayPoint(msg->pose.position.x+m_OriginPos.position.x, msg->pose.position.y+m_OriginPos.position.y, msg->pose.position.z+m_OriginPos.position.z, tf::getYaw(msg->pose.orientation));
-  m_GoalsPos.push_back(wp);
-  ROS_INFO("Received Goal Pose");
+void GlobalPlanner::callbackGetGoalPose(
+    const geometry_msgs::PoseStampedConstPtr &msg) {
+    PlannerHNS::WayPoint wp =
+        PlannerHNS::WayPoint(msg->pose.position.x + m_OriginPos.position.x,
+                             msg->pose.position.y + m_OriginPos.position.y,
+                             msg->pose.position.z + m_OriginPos.position.z,
+                             tf::getYaw(msg->pose.orientation));
+    m_GoalsPos.push_back(wp);
+    ROS_INFO("Received Goal Pose");
 }
 
-void GlobalPlanner::callbackGetStartPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
-{
-  m_CurrentPose = PlannerHNS::WayPoint(msg->pose.pose.position.x+m_OriginPos.position.x, msg->pose.pose.position.y+m_OriginPos.position.y, msg->pose.pose.position.z+m_OriginPos.position.z, tf::getYaw(msg->pose.pose.orientation));
-  ROS_INFO("Received Start pose");
+void GlobalPlanner::callbackGetStartPose(
+    const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) {
+    m_CurrentPose =
+        PlannerHNS::WayPoint(msg->pose.pose.position.x + m_OriginPos.position.x,
+                             msg->pose.pose.position.y + m_OriginPos.position.y,
+                             msg->pose.pose.position.z + m_OriginPos.position.z,
+                             tf::getYaw(msg->pose.pose.orientation));
+    ROS_INFO("Received Start pose");
 }
 
-void GlobalPlanner::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
-{
-  m_CurrentPose = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation));
+void GlobalPlanner::callbackGetCurrentPose(
+    const geometry_msgs::PoseStampedConstPtr &msg) {
+    m_CurrentPose = PlannerHNS::WayPoint(
+        msg->pose.position.x, msg->pose.position.y, msg->pose.position.z,
+        tf::getYaw(msg->pose.orientation));
 }
 
-void GlobalPlanner::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
-{
-  m_VehicleState.speed = msg->twist.twist.linear.x;
-  m_CurrentPose.v = m_VehicleState.speed;
-  if(fabs(msg->twist.twist.linear.x) > 0.25)
-    m_VehicleState.steer += atan(2.7 * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
+void GlobalPlanner::callbackGetRobotOdom(
+    const nav_msgs::OdometryConstPtr &msg) {
+    m_VehicleState.speed = msg->twist.twist.linear.x;
+    m_CurrentPose.v = m_VehicleState.speed;
+    if (fabs(msg->twist.twist.linear.x) > 0.25)
+        m_VehicleState.steer +=
+            atan(2.7 * msg->twist.twist.angular.z / msg->twist.twist.linear.x);
 }
 
-void GlobalPlanner::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  m_VehicleState.speed = msg->twist.linear.x;
-  m_CurrentPose.v = m_VehicleState.speed;
-  if(fabs(msg->twist.linear.x) > 0.25)
-    m_VehicleState.steer = atan(2.7 * msg->twist.angular.z/msg->twist.linear.x);
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleState.tStamp);
-
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
+void GlobalPlanner::callbackGetVehicleStatus(
+    const geometry_msgs::TwistStampedConstPtr &msg) {
+    m_VehicleState.speed = msg->twist.linear.x;
+    m_CurrentPose.v = m_VehicleState.speed;
+    if (fabs(msg->twist.linear.x) > 0.25)
+        m_VehicleState.steer =
+            atan(2.7 * msg->twist.angular.z / msg->twist.linear.x);
+    UtilityHNS::UtilityH::GetTickCount(m_VehicleState.tStamp);
 }
 
-void GlobalPlanner::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
-{
-  m_VehicleState.speed = msg->speed/3.6;
-  m_CurrentPose.v = m_VehicleState.speed;
-  m_VehicleState.steer = msg->angle * 0.45 / 660;
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleState.tStamp);
+void GlobalPlanner::callbackPoseTwist(
+    const rubis_msgs::PoseTwistStampedConstPtr &msg) {
+    m_CurrentPose = PlannerHNS::WayPoint(
+        msg->pose.pose.position.x, msg->pose.pose.position.y,
+        msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation));
+
+    m_VehicleState.speed = msg->twist.twist.linear.x;
+    m_CurrentPose.v = m_VehicleState.speed;
+    if (fabs(msg->twist.twist.linear.x) > 0.25)
+        m_VehicleState.steer =
+            atan(2.7 * msg->twist.twist.angular.z / msg->twist.twist.linear.x);
+    UtilityHNS::UtilityH::GetTickCount(m_VehicleState.tStamp);
 }
 
-bool GlobalPlanner::GenerateGlobalPlan(PlannerHNS::WayPoint& startPoint, PlannerHNS::WayPoint& goalPoint, std::vector<std::vector<PlannerHNS::WayPoint> >& generatedTotalPaths)
-{
-  std::vector<int> predefinedLanesIds;
-  double ret = 0;
-
-  ret = m_PlannerH.PlanUsingDP(startPoint, goalPoint, MAX_GLOBAL_PLAN_DISTANCE, m_params.bEnableLaneChange, predefinedLanesIds, m_Map, generatedTotalPaths);
+void GlobalPlanner::callbackGetCANInfo(
+    const autoware_can_msgs::CANInfoConstPtr &msg) {
+    m_VehicleState.speed = msg->speed / 3.6;
+    m_CurrentPose.v = m_VehicleState.speed;
+    m_VehicleState.steer = msg->angle * 0.45 / 660;
+    UtilityHNS::UtilityH::GetTickCount(m_VehicleState.tStamp);
+}
 
-  if(ret == 0)
-  {
-    std::cout << "Can't Generate Global Path for Start (" << startPoint.pos.ToString()
-                        << ") and Goal (" << goalPoint.pos.ToString() << ")" << std::endl;
-    return false;
-  }
-
-
-  if(generatedTotalPaths.size() > 0 && generatedTotalPaths.at(0).size()>0)
-  {
-    if(m_params.bEnableSmoothing)
-    {
-      for(unsigned int i=0; i < generatedTotalPaths.size(); i++)
-      {
-        PlannerHNS::PlanningHelpers::FixPathDensity(generatedTotalPaths.at(i), m_params.pathDensity);
-        PlannerHNS::PlanningHelpers::SmoothPath(generatedTotalPaths.at(i), 0.49, 0.35 , 0.01);
-      }
+bool GlobalPlanner::GenerateGlobalPlan(
+    PlannerHNS::WayPoint &startPoint, PlannerHNS::WayPoint &goalPoint,
+    std::vector<std::vector<PlannerHNS::WayPoint>> &generatedTotalPaths) {
+    std::vector<int> predefinedLanesIds;
+    double ret = 0;
+
+    ret =
+        m_PlannerH.PlanUsingDP(startPoint, goalPoint, MAX_GLOBAL_PLAN_DISTANCE,
+                               m_params.bEnableLaneChange, predefinedLanesIds,
+                               m_Map, generatedTotalPaths);
+
+    if (ret == 0) {
+        // std::cout << "Can't Generate Global Path for Start (" <<
+        // startPoint.pos.ToString()
+        //                     << ") and Goal (" << goalPoint.pos.ToString() <<
+        //                     ")" << std::endl;
+        return false;
     }
 
-    for(unsigned int i=0; i < generatedTotalPaths.size(); i++)
-    {
-      PlannerHNS::PlanningHelpers::CalcAngleAndCost(generatedTotalPaths.at(i));
-      if(m_GlobalPathID > 10000)
-        m_GlobalPathID = 1;
+    if (generatedTotalPaths.size() > 0 &&
+        generatedTotalPaths.at(0).size() > 0) {
+        if (m_params.bEnableSmoothing) {
+            for (unsigned int i = 0; i < generatedTotalPaths.size(); i++) {
+                PlannerHNS::PlanningHelpers::FixPathDensity(
+                    generatedTotalPaths.at(i), m_params.pathDensity);
+                PlannerHNS::PlanningHelpers::SmoothPath(
+                    generatedTotalPaths.at(i), 0.49, 0.35, 0.01);
+            }
+        }
 
-      for(unsigned int j=0; j < generatedTotalPaths.at(i).size(); j++)
-        generatedTotalPaths.at(i).at(j).gid = m_GlobalPathID;
+        for (unsigned int i = 0; i < generatedTotalPaths.size(); i++) {
+            PlannerHNS::PlanningHelpers::CalcAngleAndCost(
+                generatedTotalPaths.at(i));
+            if (m_GlobalPathID > 10000)
+                m_GlobalPathID = 1;
 
-      m_GlobalPathID++;
+            for (unsigned int j = 0; j < generatedTotalPaths.at(i).size(); j++)
+                generatedTotalPaths.at(i).at(j).gid = m_GlobalPathID;
 
-      std::cout << "New DP Path -> " << generatedTotalPaths.at(i).size() << std::endl;
-    }
-    return true;
-  }
-  else
-  {
-    std::cout << "Can't Generate Global Path for Start (" << startPoint.pos.ToString() << ") and Goal (" << goalPoint.pos.ToString() << ")" << std::endl;
-  }
-  return false;
-}
+            m_GlobalPathID++;
 
-void GlobalPlanner::VisualizeAndSend(const std::vector<std::vector<PlannerHNS::WayPoint> > generatedTotalPaths)
-{
-  autoware_msgs::LaneArray lane_array;
-  visualization_msgs::MarkerArray pathsToVisualize;
-  visualization_msgs::MarkerArray pathsToVisualizeMinicar;
-
-  for(unsigned int i=0; i < generatedTotalPaths.size(); i++)
-  {
-    autoware_msgs::Lane lane;
-    PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(generatedTotalPaths.at(i), lane);
-    lane_array.lanes.push_back(lane);
-  }
-
-  std_msgs::ColorRGBA total_color;
-  total_color.r = 0;
-  total_color.g = 0.7;
-  total_color.b = 1.0;
-  total_color.a = 0.9;
-  PlannerHNS::ROSHelpers::createGlobalLaneArrayMarker(total_color, lane_array, pathsToVisualize);
-  PlannerHNS::ROSHelpers::createGlobalLaneArrayOrientationMarker(lane_array, pathsToVisualize);
-  // PlannerHNS::ROSHelpers::createGlobalLaneArrayVelocityMarker(lane_array, pathsToVisualize);
-
-  PlannerHNS::ROSHelpers::createGlobalLaneArrayMarker(total_color, lane_array, pathsToVisualizeMinicar, MINICAR_SCALE);
-  PlannerHNS::ROSHelpers::createGlobalLaneArrayOrientationMarker(lane_array, pathsToVisualizeMinicar, MINICAR_SCALE);
-  // PlannerHNS::ROSHelpers::createGlobalLaneArrayVelocityMarker(lane_array, pathsToVisualizeMinicar, MINICAR_SCALE);
-  
-  pub_PathsRvizMinicar.publish(pathsToVisualizeMinicar);
-  pub_PathsRviz.publish(pathsToVisualize);
-  if((m_bFirstStart && m_params.bEnableHMI) || !m_params.bEnableHMI)
-    pub_Paths.publish(lane_array);
-
-  for(unsigned int i=0; i < generatedTotalPaths.size(); i++)
-  {
-    std::ostringstream str_out;
-    str_out << UtilityHNS::UtilityH::GetHomeDirectory();
-    str_out << UtilityHNS::DataRW::LoggingMainfolderName;
-    str_out << UtilityHNS::DataRW::GlobalPathLogFolderName;
-    str_out << "GlobalPath_";
-    str_out << i;
-    str_out << "_";
-    PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), generatedTotalPaths.at(i));
-  }
+            std::cout << "New DP Path -> " << generatedTotalPaths.at(i).size()
+                      << std::endl;
+        }
+        return true;
+    } else {
+        std::cout << "Can't Generate Global Path for Start ("
+                  << startPoint.pos.ToString() << ") and Goal ("
+                  << goalPoint.pos.ToString() << ")" << std::endl;
+    }
+    return false;
 }
 
-void GlobalPlanner::VisualizeDestinations(std::vector<PlannerHNS::WayPoint>& destinations, const int& iSelected)
-{
-  visualization_msgs::MarkerArray goals_array;
-
-  for(unsigned int i=0; i< destinations.size(); i++)
-  {
-    visualization_msgs::Marker marker;
-    marker.header.frame_id = "map";
-    marker.header.stamp = ros::Time();
-    marker.ns = "HMI_Destinations";
-    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-    marker.action = visualization_msgs::Marker::ADD;
-    marker.scale.x = 3.25;
-    marker.scale.y = 3.25;
-    marker.scale.z = 3.25;
-    marker.color.a = 0.9;
-    marker.id = i;
-    if(i == iSelected)
-    {
-      marker.color.r = 1;
-      marker.color.g = 0;
-      marker.color.b = 0;
+void GlobalPlanner::VisualizeAndSend(
+    const std::vector<std::vector<PlannerHNS::WayPoint>> generatedTotalPaths) {
+    autoware_msgs::LaneArray lane_array;
+    visualization_msgs::MarkerArray pathsToVisualize;
+    visualization_msgs::MarkerArray pathsToVisualizeMinicar;
+
+    for (unsigned int i = 0; i < generatedTotalPaths.size(); i++) {
+        autoware_msgs::Lane lane;
+        PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(
+            generatedTotalPaths.at(i), lane);
+        lane_array.lanes.push_back(lane);
     }
-    else
-    {
-      marker.color.r = 0.2;
-      marker.color.g = 0.8;
-      marker.color.b = 0.2;
+
+    std_msgs::ColorRGBA total_color;
+    total_color.r = 0;
+    total_color.g = 0.7;
+    total_color.b = 1.0;
+    total_color.a = 0.9;
+    PlannerHNS::ROSHelpers::createGlobalLaneArrayMarker(total_color, lane_array,
+                                                        pathsToVisualize);
+    PlannerHNS::ROSHelpers::createGlobalLaneArrayOrientationMarker(
+        lane_array, pathsToVisualize);
+    // PlannerHNS::ROSHelpers::createGlobalLaneArrayVelocityMarker(lane_array,
+    // pathsToVisualize);
+
+    PlannerHNS::ROSHelpers::createGlobalLaneArrayMarker(
+        total_color, lane_array, pathsToVisualizeMinicar, MINICAR_SCALE);
+    PlannerHNS::ROSHelpers::createGlobalLaneArrayOrientationMarker(
+        lane_array, pathsToVisualizeMinicar, MINICAR_SCALE);
+    // PlannerHNS::ROSHelpers::createGlobalLaneArrayVelocityMarker(lane_array,
+    // pathsToVisualizeMinicar, MINICAR_SCALE);
+
+    pub_PathsRvizMinicar.publish(pathsToVisualizeMinicar);
+    pub_PathsRviz.publish(pathsToVisualize);
+    if ((m_bFirstStart && m_params.bEnableHMI) || !m_params.bEnableHMI)
+        pub_Paths.publish(lane_array);
+
+    for (unsigned int i = 0; i < generatedTotalPaths.size(); i++) {
+        std::ostringstream str_out;
+        str_out << UtilityHNS::UtilityH::GetHomeDirectory();
+        str_out << UtilityHNS::DataRW::LoggingMainfolderName;
+        str_out << UtilityHNS::DataRW::GlobalPathLogFolderName;
+        str_out << "GlobalPath_";
+        str_out << i;
+        str_out << "_";
+        PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(),
+                                                     generatedTotalPaths.at(i));
     }
-    marker.pose.position.x = destinations.at(i).pos.x;
-    marker.pose.position.y = destinations.at(i).pos.y;
-    marker.pose.position.z = destinations.at(i).pos.z;
-    marker.pose.orientation = tf::createQuaternionMsgFromYaw(destinations.at(i).pos.a);
-
-    std::ostringstream str_out;
-    str_out << "G";
-    marker.text = str_out.str();
-
-    goals_array.markers.push_back(marker);
-  }
-  pub_GoalsListRviz.publish(goals_array);
 }
 
-void GlobalPlanner::SaveSimulationData()
-{
-  std::vector<std::string> simulationDataPoints;
-  std::ostringstream startStr;
-  startStr << m_CurrentPose.pos.x << "," << m_CurrentPose.pos.y << "," << m_CurrentPose.pos.z << "," << m_CurrentPose.pos.a << ","<< m_CurrentPose.cost << "," << 0 << ",";
-  simulationDataPoints.push_back(startStr.str());
-
-  for(unsigned int i=0; i < m_GoalsPos.size(); i++)
-  {
-    std::ostringstream goalStr;
-    goalStr << m_GoalsPos.at(i).pos.x << "," << m_GoalsPos.at(i).pos.y << "," << m_GoalsPos.at(i).pos.z << "," << m_GoalsPos.at(i).pos.a << "," << 0 << "," << 0 << ",destination_" << i+1 << ",";
-    simulationDataPoints.push_back(goalStr.str());
-  }
-
-  std::string header = "X,Y,Z,A,C,V,name,";
-
-  std::ostringstream fileName;
-  fileName << UtilityHNS::UtilityH::GetHomeDirectory()+UtilityHNS::DataRW::LoggingMainfolderName+UtilityHNS::DataRW::SimulationFolderName;
-  fileName << "EgoCar.csv";
-  std::ofstream f(fileName.str().c_str());
-
-  if(f.is_open())
-  {
-    if(header.size() > 0)
-      f << header << "\r\n";
-    for(unsigned int i = 0 ; i < simulationDataPoints.size(); i++)
-      f << simulationDataPoints.at(i) << "\r\n";
-  }
-
-  f.close();
-}
+void GlobalPlanner::VisualizeDestinations(
+    std::vector<PlannerHNS::WayPoint> &destinations, const int &iSelected) {
+    visualization_msgs::MarkerArray goals_array;
+
+    for (unsigned int i = 0; i < destinations.size(); i++) {
+        visualization_msgs::Marker marker;
+        marker.header.frame_id = "map";
+        marker.header.stamp = ros::Time();
+        marker.ns = "HMI_Destinations";
+        marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+        marker.action = visualization_msgs::Marker::ADD;
+        marker.scale.x = 3.25;
+        marker.scale.y = 3.25;
+        marker.scale.z = 3.25;
+        marker.color.a = 0.9;
+        marker.id = i;
+        if (i == iSelected) {
+            marker.color.r = 1;
+            marker.color.g = 0;
+            marker.color.b = 0;
+        } else {
+            marker.color.r = 0.2;
+            marker.color.g = 0.8;
+            marker.color.b = 0.2;
+        }
+        marker.pose.position.x = destinations.at(i).pos.x;
+        marker.pose.position.y = destinations.at(i).pos.y;
+        marker.pose.position.z = destinations.at(i).pos.z;
+        marker.pose.orientation =
+            tf::createQuaternionMsgFromYaw(destinations.at(i).pos.a);
 
-int GlobalPlanner::LoadSimulationData()
-{
-  std::ostringstream fileName;
-  fileName << "EgoCar.csv";
+        std::ostringstream str_out;
+        str_out << "G";
+        marker.text = str_out.str();
 
-  std::string simuDataFileName = UtilityHNS::UtilityH::GetHomeDirectory()+UtilityHNS::DataRW::LoggingMainfolderName+UtilityHNS::DataRW::SimulationFolderName + fileName.str();
-  UtilityHNS::SimulationFileReader sfr(simuDataFileName);
-  UtilityHNS::SimulationFileReader::SimulationData data;
+        goals_array.markers.push_back(marker);
+    }
+    pub_GoalsListRviz.publish(goals_array);
+}
 
-  int nData = sfr.ReadAllData(data);
-  if(nData == 0)
-    return 0;
+void GlobalPlanner::SaveSimulationData() {
+    std::vector<std::string> simulationDataPoints;
+    std::ostringstream startStr;
+    startStr << m_CurrentPose.pos.x << "," << m_CurrentPose.pos.y << ","
+             << m_CurrentPose.pos.z << "," << m_CurrentPose.pos.a << ","
+             << m_CurrentPose.cost << "," << 0 << ",";
+    simulationDataPoints.push_back(startStr.str());
+
+    for (unsigned int i = 0; i < m_GoalsPos.size(); i++) {
+        std::ostringstream goalStr;
+        goalStr << m_GoalsPos.at(i).pos.x << "," << m_GoalsPos.at(i).pos.y
+                << "," << m_GoalsPos.at(i).pos.z << ","
+                << m_GoalsPos.at(i).pos.a << "," << 0 << "," << 0
+                << ",destination_" << i + 1 << ",";
+        simulationDataPoints.push_back(goalStr.str());
+    }
 
-  m_CurrentPose = PlannerHNS::WayPoint(data.startPoint.x, data.startPoint.y, data.startPoint.z, data.startPoint.a);
-  m_GoalsPos.push_back(PlannerHNS::WayPoint(data.goalPoint.x, data.goalPoint.y, data.goalPoint.z, data.goalPoint.a));
+    std::string header = "X,Y,Z,A,C,V,name,";
 
-  for(unsigned int i=0; i < data.simuCars.size(); i++)
-  {
-    m_GoalsPos.push_back(PlannerHNS::WayPoint(data.simuCars.at(i).x, data.simuCars.at(i).y, data.simuCars.at(i).z, data.simuCars.at(i).a));
-  }
+    std::ostringstream fileName;
+    fileName << UtilityHNS::UtilityH::GetHomeDirectory() +
+                    UtilityHNS::DataRW::LoggingMainfolderName +
+                    UtilityHNS::DataRW::SimulationFolderName;
+    fileName << "EgoCar.csv";
+    std::ofstream f(fileName.str().c_str());
 
-  return nData;
+    if (f.is_open()) {
+        if (header.size() > 0)
+            f << header << "\r\n";
+        for (unsigned int i = 0; i < simulationDataPoints.size(); i++)
+            f << simulationDataPoints.at(i) << "\r\n";
+    }
+
+    f.close();
 }
 
-void GlobalPlanner::MainLoop()
-{
-  ros::NodeHandle private_nh("~");
-
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
-  int multilap_flag;
-  double multilap_replanning_distance;
-  int planning_fail_cnt;
-
-  private_nh.param<int>("/op_global_planner/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/op_global_planner/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/op_global_planner/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_global_planner.csv");
-  private_nh.param<int>("/op_global_planner/rate", rate, 10);
-  private_nh.param("/op_global_planner/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/op_global_planner/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/op_global_planner/task_relative_deadline", task_relative_deadline, (double)10);
-  private_nh.param("/op_global_planner/multilap_flag", multilap_flag, 0);
-  private_nh.param("/op_global_planner/multilap_replanning_distance", multilap_replanning_distance, (double)50);
-
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-  timespec animation_timer;
-  UtilityHNS::UtilityH::GetTickCount(animation_timer);
-
-  ros::Rate loop_rate(rate);
-  if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25);
-  
-
-
-  while (ros::ok())
-  {
-    if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){
-      if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-      rubis::sched::task_state_ = TASK_STATE_RUNNING;     
+int GlobalPlanner::LoadSimulationData() {
+    std::ostringstream fileName;
+    fileName << "EgoCar.csv";
+
+    std::string simuDataFileName = UtilityHNS::UtilityH::GetHomeDirectory() +
+                                   UtilityHNS::DataRW::LoggingMainfolderName +
+                                   UtilityHNS::DataRW::SimulationFolderName +
+                                   fileName.str();
+    UtilityHNS::SimulationFileReader sfr(simuDataFileName);
+    UtilityHNS::SimulationFileReader::SimulationData data;
+
+    int nData = sfr.ReadAllData(data);
+    if (nData == 0)
+        return 0;
+
+    m_CurrentPose = PlannerHNS::WayPoint(data.startPoint.x, data.startPoint.y,
+                                         data.startPoint.z, data.startPoint.a);
+    m_GoalsPos.push_back(
+        PlannerHNS::WayPoint(data.goalPoint.x, data.goalPoint.y,
+                             data.goalPoint.z, data.goalPoint.a));
+
+    for (unsigned int i = 0; i < data.simuCars.size(); i++) {
+        m_GoalsPos.push_back(
+            PlannerHNS::WayPoint(data.simuCars.at(i).x, data.simuCars.at(i).y,
+                                 data.simuCars.at(i).z, data.simuCars.at(i).a));
     }
 
-    ros::spinOnce();
-    bool bMakeNewPlan = false;
+    return nData;
+}
 
-    if(m_params.mapSource == PlannerHNS::MAP_KML_FILE && !m_bKmlMap)
-    {
-      m_bKmlMap = true;
-      PlannerHNS::MappingHelpers::LoadKML(m_params.KmlMapPath, m_Map);
-      visualization_msgs::MarkerArray map_marker_array;
-      PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array);
-      pub_MapRviz.publish(map_marker_array);
-    }
-    else if (m_params.mapSource == PlannerHNS::MAP_FOLDER && !m_bKmlMap)
-    {
-      m_bKmlMap = true;
-      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_params.KmlMapPath, m_Map, true);
-      visualization_msgs::MarkerArray map_marker_array;
-      PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array);
-
-      pub_MapRviz.publish(map_marker_array);
-    }
-    else if (m_params.mapSource == PlannerHNS::MAP_AUTOWARE && !m_bKmlMap)
-    {
-      std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;;
-
-      if(m_MapRaw.GetVersion()==2)
-      {
-        std::cout << "Map Version 2" << endl;
-        m_bKmlMap = true;
-        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
-            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
-            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
-            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
-            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,
-            m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_params.bEnableLaneChange, false);
-
-            XmlRpc::XmlRpcValue lane_info_xml;
-            // Add Lane Info from yaml file if exist
-            try{
-              nh.getParam("/op_global_planner/lane_info_list", lane_info_xml);
+void GlobalPlanner::MainLoop() {
+    ros::NodeHandle private_nh("~");
+
+    // Scheduling Setup
+    std::string task_response_time_filename;
+    int rate;
+    double task_minimum_inter_release_time;
+    double task_execution_time;
+    double task_relative_deadline;
+    int multilap_flag;
+    double multilap_replanning_distance;
+    int planning_fail_cnt;
+
+    private_nh.param<std::string>(
+        "/op_global_planner/task_response_time_filename",
+        task_response_time_filename,
+        "~/Documents/profiling/response_time/op_global_planner.csv");
+    private_nh.param<int>("/op_global_planner/rate", rate, 10);
+    private_nh.param("/op_global_planner/task_minimum_inter_release_time",
+                     task_minimum_inter_release_time, (double)10);
+    private_nh.param("/op_global_planner/task_execution_time",
+                     task_execution_time, (double)10);
+    private_nh.param("/op_global_planner/task_relative_deadline",
+                     task_relative_deadline, (double)10);
+    private_nh.param("/op_global_planner/multilap_flag", multilap_flag, 0);
+    private_nh.param("/op_global_planner/multilap_replanning_distance",
+                     multilap_replanning_distance, (double)50);
+
+    rubis::init_task_profiling(task_response_time_filename);
+
+    timespec animation_timer;
+    UtilityHNS::UtilityH::GetTickCount(animation_timer);
+
+    ros::Rate loop_rate(rate);
+
+    while (ros::ok()) {
+        rubis::start_task_profiling();
+
+        ros::spinOnce();
+        bool bMakeNewPlan = false;
+
+        if (m_params.mapSource == PlannerHNS::MAP_KML_FILE && !m_bKmlMap) {
+            m_bKmlMap = true;
+            PlannerHNS::MappingHelpers::LoadKML(m_params.KmlMapPath, m_Map);
+            visualization_msgs::MarkerArray map_marker_array;
+            PlannerHNS::ROSHelpers::
+                ConvertFromRoadNetworkToAutowareVisualizeMapFormat(
+                    m_Map, map_marker_array);
+            pub_MapRviz.publish(map_marker_array);
+        } else if (m_params.mapSource == PlannerHNS::MAP_FOLDER && !m_bKmlMap) {
+            m_bKmlMap = true;
+            PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(
+                m_params.KmlMapPath, m_Map, true);
+            visualization_msgs::MarkerArray map_marker_array;
+            PlannerHNS::ROSHelpers::
+                ConvertFromRoadNetworkToAutowareVisualizeMapFormat(
+                    m_Map, map_marker_array);
+
+            pub_MapRviz.publish(map_marker_array);
+        } else if (m_params.mapSource == PlannerHNS::MAP_AUTOWARE &&
+                   !m_bKmlMap) {
+            std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>
+                conn_data;
+            ;
+
+            if (m_MapRaw.GetVersion() == 2) {
+                std::cout << "Map Version 2" << endl;
+                m_bKmlMap = true;
+                PlannerHNS::MappingHelpers::
+                    ConstructRoadNetworkFromROSMessageV2(
+                        m_MapRaw.pLanes->m_data_list,
+                        m_MapRaw.pPoints->m_data_list,
+                        m_MapRaw.pCenterLines->m_data_list,
+                        m_MapRaw.pIntersections->m_data_list,
+                        m_MapRaw.pAreas->m_data_list,
+                        m_MapRaw.pLines->m_data_list,
+                        m_MapRaw.pStopLines->m_data_list,
+                        m_MapRaw.pSignals->m_data_list,
+                        m_MapRaw.pVectors->m_data_list,
+                        m_MapRaw.pCurbs->m_data_list,
+                        m_MapRaw.pRoadedges->m_data_list,
+                        m_MapRaw.pWayAreas->m_data_list,
+                        m_MapRaw.pCrossWalks->m_data_list,
+                        m_MapRaw.pNodes->m_data_list, conn_data,
+                        m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes,
+                        m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true,
+                        m_params.bEnableLaneChange, false);
+
+                // XmlRpc::XmlRpcValue lane_info_xml;
+                // // Add Lane Info from yaml file if exist
+                // try{
+                //   nh.getParam("/op_global_planner/lane_info_list",
+                //   lane_info_xml);
+                // }
+                // catch(XmlRpc::XmlRpcException& e){
+                //   ROS_WARN("No Lane Info yaml file");
+                //   lane_info_xml.clear();
+                // }
+                // PlannerHNS::MappingHelpers::ConstructLaneInfo_RUBIS(m_Map,
+                // lane_info_xml);
+            } else if (m_MapRaw.GetVersion() == 1) {
+                std::cout << "Map Version 1" << endl;
+                m_bKmlMap = true;
+                PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(
+                    m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
+                    m_MapRaw.pCenterLines->m_data_list,
+                    m_MapRaw.pIntersections->m_data_list,
+                    m_MapRaw.pAreas->m_data_list, m_MapRaw.pLines->m_data_list,
+                    m_MapRaw.pStopLines->m_data_list,
+                    m_MapRaw.pSignals->m_data_list,
+                    m_MapRaw.pVectors->m_data_list,
+                    m_MapRaw.pCurbs->m_data_list,
+                    m_MapRaw.pRoadedges->m_data_list,
+                    m_MapRaw.pWayAreas->m_data_list,
+                    m_MapRaw.pCrossWalks->m_data_list,
+                    m_MapRaw.pNodes->m_data_list, conn_data,
+                    PlannerHNS::GPSPoint(), m_Map, true,
+                    m_params.bEnableLaneChange, false);
             }
-            catch(XmlRpc::XmlRpcException& e){
-              ROS_WARN("No Lane Info yaml file");
-              lane_info_xml.clear();
-            }
-            PlannerHNS::MappingHelpers::ConstructLaneInfo_RUBIS(m_Map, lane_info_xml);
-      }
-      else if(m_MapRaw.GetVersion()==1)
-      {
-        std::cout << "Map Version 1" << endl;
-        m_bKmlMap = true;
-        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
-            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
-            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
-            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
-            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,  PlannerHNS::GPSPoint(), m_Map, true, m_params.bEnableLaneChange, false);
-      }
-
-      if(m_bKmlMap)
-      {
-        visualization_msgs::MarkerArray map_marker_array;
-        visualization_msgs::MarkerArray map_marker_array_minicar;
-        PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array);
-        PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array_minicar, MINICAR_SCALE);
-
-        pub_MapRvizMinicar.publish(map_marker_array_minicar);
-        pub_MapRviz.publish(map_marker_array);
-      }
-    }
 
-    ClearOldCostFromMap();
-
-    // HJW updated for multi-lab
-    // goal pose is appended at goal pose callback function
-    if(m_GoalsPos.size() > 0)
-    {
-      if(m_GeneratedTotalPaths.size() == 0) // initialize two paths
-      {
-        // Try 50 Times to planning
-        if(planning_fail_cnt == 0){
-          planning_fail_cnt = 50;
-        }
-        std::vector<std::vector<PlannerHNS::WayPoint>> tmp_path_list;
-        std::vector<std::vector<PlannerHNS::WayPoint>> tmp_path_list_2;
-        PlannerHNS::WayPoint goalPoint = m_GoalsPos.at(m_iCurrentGoalIndex);
-
-        // Generate path from initial pose to goal pose (rviz axis) and save to tmp_path_list
-        bool bNewPlan = GenerateGlobalPlan(m_CurrentPose, goalPoint, tmp_path_list);
-
-        if(bNewPlan)
-        {
-          m_GeneratedTotalPaths.push_back(tmp_path_list);
-
-          // Do multi-lab driving only current position and goal point is close
-          if(multilap_flag){
-            if(hypot(m_CurrentPose.pos.x - goalPoint.pos.x, m_CurrentPose.pos.y - goalPoint.pos.y) < 30){
-              int wp_size = tmp_path_list.at(0).size();
-              PlannerHNS::WayPoint path2_start_wp = tmp_path_list.at(0).at(wp_size / 2 + 10);
-              PlannerHNS::WayPoint path2_end_wp = tmp_path_list.at(0).at(wp_size / 2 - 10);
-              bool bNewPlan_2 = GenerateGlobalPlan(path2_start_wp, path2_end_wp, tmp_path_list_2);
-
-              if(bNewPlan_2){
-                m_GeneratedTotalPaths.push_back(tmp_path_list_2);
-              }
+            if (m_bKmlMap) {
+                visualization_msgs::MarkerArray map_marker_array;
+                visualization_msgs::MarkerArray map_marker_array_minicar;
+                PlannerHNS::ROSHelpers::
+                    ConvertFromRoadNetworkToAutowareVisualizeMapFormat(
+                        m_Map, map_marker_array);
+                PlannerHNS::ROSHelpers::
+                    ConvertFromRoadNetworkToAutowareVisualizeMapFormat(
+                        m_Map, map_marker_array_minicar, MINICAR_SCALE);
+
+                pub_MapRvizMinicar.publish(map_marker_array_minicar);
+                pub_MapRviz.publish(map_marker_array);
             }
-          }
-          selectedGlobalPathIdx = 0;
-          VisualizeAndSend(m_GeneratedTotalPaths.at(selectedGlobalPathIdx));
-        }
-        else{
-          planning_fail_cnt--;
-          if(planning_fail_cnt == 0){
-            m_GoalsPos.clear();
-          }
-        }
-      }
-      else if(m_GeneratedTotalPaths.size() > 1){
-        PlannerHNS::RelativeInfo info;
-        bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths.at(selectedGlobalPathIdx), m_CurrentPose, 0.75, info);
-        if(ret == true && info.iGlobalPath >= 0 &&  info.iGlobalPath < m_GeneratedTotalPaths.at(selectedGlobalPathIdx).size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(selectedGlobalPathIdx).at(info.iGlobalPath).size())
-        {
-          double remaining_distance = m_GeneratedTotalPaths.at(selectedGlobalPathIdx).at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(selectedGlobalPathIdx).at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(selectedGlobalPathIdx).at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);
-          if(remaining_distance <= multilap_replanning_distance)
-          {
-            selectedGlobalPathIdx = 1 - selectedGlobalPathIdx;
-            VisualizeAndSend(m_GeneratedTotalPaths.at(selectedGlobalPathIdx));
-          }
         }
-      }
-      VisualizeDestinations(m_GoalsPos, m_iCurrentGoalIndex);
-    }
 
-    rubis::sched::task_state_ = TASK_STATE_DONE;
+        ClearOldCostFromMap();
+
+        // HJW updated for multi-lab
+        // goal pose is appended at goal pose callback function
+        if (m_GoalsPos.size() > 0) {
+            if (m_GeneratedTotalPaths.size() == 0) // initialize two paths
+            {
+                // Try 50 Times to planning
+                if (planning_fail_cnt == 0) {
+                    planning_fail_cnt = 50;
+                }
+                std::vector<std::vector<PlannerHNS::WayPoint>> tmp_path_list;
+                std::vector<std::vector<PlannerHNS::WayPoint>> tmp_path_list_2;
+                PlannerHNS::WayPoint goalPoint =
+                    m_GoalsPos.at(m_iCurrentGoalIndex);
+
+                // Generate path from initial pose to goal pose (rviz axis) and
+                // save to tmp_path_list
+                bool bNewPlan =
+                    GenerateGlobalPlan(m_CurrentPose, goalPoint, tmp_path_list);
+
+                if (bNewPlan) {
+                    m_GeneratedTotalPaths.push_back(tmp_path_list);
+
+                    // Do multi-lab driving only current position and goal point
+                    // is close
+                    if (multilap_flag) {
+                        if (hypot(m_CurrentPose.pos.x - goalPoint.pos.x,
+                                  m_CurrentPose.pos.y - goalPoint.pos.y) < 30) {
+                            int wp_size = tmp_path_list.at(0).size();
+                            PlannerHNS::WayPoint path2_start_wp =
+                                tmp_path_list.at(0).at(wp_size / 2 + 10);
+                            PlannerHNS::WayPoint path2_end_wp =
+                                tmp_path_list.at(0).at(wp_size / 2 - 10);
+                            bool bNewPlan_2 = GenerateGlobalPlan(
+                                path2_start_wp, path2_end_wp, tmp_path_list_2);
+
+                            if (bNewPlan_2) {
+                                m_GeneratedTotalPaths.push_back(
+                                    tmp_path_list_2);
+                            }
+                        }
+                    }
+                    selectedGlobalPathIdx = 0;
+                    VisualizeAndSend(
+                        m_GeneratedTotalPaths.at(selectedGlobalPathIdx));
+                } else {
+                    planning_fail_cnt--;
+                    if (planning_fail_cnt == 0) {
+                        m_GoalsPos.clear();
+                    }
+                }
+            } else if (m_GeneratedTotalPaths.size() > 1) {
+                PlannerHNS::RelativeInfo info;
+                bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(
+                    m_GeneratedTotalPaths.at(selectedGlobalPathIdx),
+                    m_CurrentPose, 0.75, info);
+                if (ret == true && info.iGlobalPath >= 0 &&
+                    info.iGlobalPath <
+                        m_GeneratedTotalPaths.at(selectedGlobalPathIdx)
+                            .size() &&
+                    info.iFront > 0 &&
+                    info.iFront <
+                        m_GeneratedTotalPaths.at(selectedGlobalPathIdx)
+                            .at(info.iGlobalPath)
+                            .size()) {
+                    double remaining_distance =
+                        m_GeneratedTotalPaths.at(selectedGlobalPathIdx)
+                            .at(info.iGlobalPath)
+                            .at(m_GeneratedTotalPaths.at(selectedGlobalPathIdx)
+                                    .at(info.iGlobalPath)
+                                    .size() -
+                                1)
+                            .cost -
+                        (m_GeneratedTotalPaths.at(selectedGlobalPathIdx)
+                             .at(info.iGlobalPath)
+                             .at(info.iFront)
+                             .cost +
+                         info.to_front_distance);
+                    if (remaining_distance <= multilap_replanning_distance) {
+                        selectedGlobalPathIdx = 1 - selectedGlobalPathIdx;
+                        VisualizeAndSend(
+                            m_GeneratedTotalPaths.at(selectedGlobalPathIdx));
+                    }
+                }
+            }
+            VisualizeDestinations(m_GoalsPos, m_iCurrentGoalIndex);
+        }
 
-    if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
+        rubis::stop_task_profiling(0, 0, 0);
 
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){
-      if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-      rubis::sched::task_state_ = TASK_STATE_READY;
+        loop_rate.sleep();
     }
-    loop_rate.sleep();
-  }
 }
 
+// Mapping Section
 
-//Mapping Section
-
-void GlobalPlanner::callbackGetVMLanes(const vector_map_msgs::LaneArray& msg)
-{
-  std::cout << "Received Lanes" << msg.data.size() << endl;
-  if(m_MapRaw.pLanes == nullptr)
-    m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg);
+void GlobalPlanner::callbackGetVMLanes(const vector_map_msgs::LaneArray &msg) {
+    std::cout << "Received Lanes" << msg.data.size() << endl;
+    if (m_MapRaw.pLanes == nullptr)
+        m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMPoints(const vector_map_msgs::PointArray& msg)
-{
-  std::cout << "Received Points" << msg.data.size() << endl;
-  if(m_MapRaw.pPoints  == nullptr)
-    m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg);
+void GlobalPlanner::callbackGetVMPoints(
+    const vector_map_msgs::PointArray &msg) {
+    std::cout << "Received Points" << msg.data.size() << endl;
+    if (m_MapRaw.pPoints == nullptr)
+        m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray& msg)
-{
-  std::cout << "Received dtLanes" << msg.data.size() << endl;
-  if(m_MapRaw.pCenterLines == nullptr)
-    m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg);
+void GlobalPlanner::callbackGetVMdtLanes(
+    const vector_map_msgs::DTLaneArray &msg) {
+    std::cout << "Received dtLanes" << msg.data.size() << endl;
+    if (m_MapRaw.pCenterLines == nullptr)
+        m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray& msg)
-{
-  std::cout << "Received CrossRoads" << msg.data.size() << endl;
-  if(m_MapRaw.pIntersections == nullptr)
-    m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg);
+void GlobalPlanner::callbackGetVMIntersections(
+    const vector_map_msgs::CrossRoadArray &msg) {
+    std::cout << "Received CrossRoads" << msg.data.size() << endl;
+    if (m_MapRaw.pIntersections == nullptr)
+        m_MapRaw.pIntersections =
+            new UtilityHNS::AisanIntersectionFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMAreas(const vector_map_msgs::AreaArray& msg)
-{
-  std::cout << "Received Areas" << msg.data.size() << endl;
-  if(m_MapRaw.pAreas == nullptr)
-    m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg);
+void GlobalPlanner::callbackGetVMAreas(const vector_map_msgs::AreaArray &msg) {
+    std::cout << "Received Areas" << msg.data.size() << endl;
+    if (m_MapRaw.pAreas == nullptr)
+        m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMLines(const vector_map_msgs::LineArray& msg)
-{
-  std::cout << "Received Lines" << msg.data.size() << endl;
-  if(m_MapRaw.pLines == nullptr)
-    m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg);
+void GlobalPlanner::callbackGetVMLines(const vector_map_msgs::LineArray &msg) {
+    std::cout << "Received Lines" << msg.data.size() << endl;
+    if (m_MapRaw.pLines == nullptr)
+        m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg)
-{
-  std::cout << "Received StopLines" << msg.data.size() << endl;
-  if(m_MapRaw.pStopLines == nullptr)
-    m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg);
+void GlobalPlanner::callbackGetVMStopLines(
+    const vector_map_msgs::StopLineArray &msg) {
+    std::cout << "Received StopLines" << msg.data.size() << endl;
+    if (m_MapRaw.pStopLines == nullptr)
+        m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMSignal(const vector_map_msgs::SignalArray& msg)
-{
-  std::cout << "Received Signals" << msg.data.size() << endl;
-  if(m_MapRaw.pSignals  == nullptr)
-    m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg);
+void GlobalPlanner::callbackGetVMSignal(
+    const vector_map_msgs::SignalArray &msg) {
+    std::cout << "Received Signals" << msg.data.size() << endl;
+    if (m_MapRaw.pSignals == nullptr)
+        m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMVectors(const vector_map_msgs::VectorArray& msg)
-{
-  std::cout << "Received Vectors" << msg.data.size() << endl;
-  if(m_MapRaw.pVectors  == nullptr)
-    m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg);
+void GlobalPlanner::callbackGetVMVectors(
+    const vector_map_msgs::VectorArray &msg) {
+    std::cout << "Received Vectors" << msg.data.size() << endl;
+    if (m_MapRaw.pVectors == nullptr)
+        m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMCurbs(const vector_map_msgs::CurbArray& msg)
-{
-  std::cout << "Received Curbs" << msg.data.size() << endl;
-  if(m_MapRaw.pCurbs == nullptr)
-    m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg);
+void GlobalPlanner::callbackGetVMCurbs(const vector_map_msgs::CurbArray &msg) {
+    std::cout << "Received Curbs" << msg.data.size() << endl;
+    if (m_MapRaw.pCurbs == nullptr)
+        m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray& msg)
-{
-  std::cout << "Received Edges" << msg.data.size() << endl;
-  if(m_MapRaw.pRoadedges  == nullptr)
-    m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg);
+void GlobalPlanner::callbackGetVMRoadEdges(
+    const vector_map_msgs::RoadEdgeArray &msg) {
+    std::cout << "Received Edges" << msg.data.size() << endl;
+    if (m_MapRaw.pRoadedges == nullptr)
+        m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray& msg)
-{
-  std::cout << "Received Wayareas" << msg.data.size() << endl;
-  if(m_MapRaw.pWayAreas  == nullptr)
-    m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg);
+void GlobalPlanner::callbackGetVMWayAreas(
+    const vector_map_msgs::WayAreaArray &msg) {
+    std::cout << "Received Wayareas" << msg.data.size() << endl;
+    if (m_MapRaw.pWayAreas == nullptr)
+        m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray& msg)
-{
-  std::cout << "Received CrossWalks" << msg.data.size() << endl;
-  if(m_MapRaw.pCrossWalks == nullptr)
-    m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg);
+void GlobalPlanner::callbackGetVMCrossWalks(
+    const vector_map_msgs::CrossWalkArray &msg) {
+    std::cout << "Received CrossWalks" << msg.data.size() << endl;
+    if (m_MapRaw.pCrossWalks == nullptr)
+        m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg);
 }
 
-void GlobalPlanner::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg)
-{
-  std::cout << "Received Nodes" << msg.data.size() << endl;
-  if(m_MapRaw.pNodes == nullptr)
-    m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg);
+void GlobalPlanner::callbackGetVMNodes(const vector_map_msgs::NodeArray &msg) {
+    std::cout << "Received Nodes" << msg.data.size() << endl;
+    if (m_MapRaw.pNodes == nullptr)
+        m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg);
 }
 
-}
+} // namespace GlobalPlanningNS
diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/package.xml b/autoware.ai/src/autoware/core_planning/op_global_planner/package.xml
index 7fe0aed9..d49f4e19 100644
--- a/autoware.ai/src/autoware/core_planning/op_global_planner/package.xml
+++ b/autoware.ai/src/autoware/core_planning/op_global_planner/package.xml
@@ -7,7 +7,7 @@
   <author email="hatem.darweesh@g.sp.m.is.nagoya-u.ac.jp">Hatem Darweesh</author>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
   
   <depend>autoware_can_msgs</depend>
@@ -15,7 +15,6 @@
   <depend>message_generation</depend>
   <depend>op_planner</depend>
   <depend>op_ros_helpers</depend>
-  <depend>op_simu</depend>
   <depend>op_utility</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
@@ -25,4 +24,6 @@
   <depend>vector_map_msgs</depend>
   <depend>libwaypoint_follower</depend>
   <depend>rubis_lib</depend>
+  <depend>nav_msgs</depend>
+  <depend>rubis_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt
index cbec051b..843a8a8b 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt
@@ -5,10 +5,11 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(
   catkin REQUIRED COMPONENTS
+    autoware_msgs
     autoware_can_msgs
     geometry_msgs
     jsk_recognition_msgs
@@ -16,7 +17,6 @@ find_package(
     map_file
     op_planner
     op_ros_helpers
-    op_simu  
     op_utility
     pcl_conversions
     pcl_ros
@@ -25,6 +25,8 @@ find_package(
     tf
     vector_map_msgs
     rubis_lib
+    nav_msgs
+    rubis_msgs
 )
 
 find_package(PkgConfig REQUIRED)
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h
index 708301ff..372c6986 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h
@@ -61,12 +61,18 @@
 #include <visualization_msgs/Marker.h>
 #include <tf/transform_listener.h>
 
+#include "rubis_msgs/LaneArrayWithPoseTwist.h"
+#include "rubis_msgs/LaneWithPoseTwist.h"
+#include "rubis_msgs/PlanningInfo.h"
+
 
 namespace BehaviorGeneratorNS
 {
 
 class BehaviorGen
 {
+public:
+  const rubis_msgs::LaneArrayWithPoseTwist lane_array_with_pose_twist_msg_;
 protected: //Planning Related variables
   double PI = 3.14159265;
 
@@ -109,10 +115,6 @@ class BehaviorGen
   tf::TransformListener m_map_base_listener;
   tf::StampedTransform m_map_base_transform;
 
-  geometry_msgs::TwistStamped m_Twist_raw;
-  geometry_msgs::TwistStamped m_Twist_cmd;
-  autoware_msgs::ControlCommand m_Ctrl_cmd;
-
   //Added by PHY
   double m_distanceToPedestrianThreshold;
   double m_turnAngle;
@@ -120,12 +122,14 @@ class BehaviorGen
   double m_sprintSpeed;
   bool m_sprintSwitch;
   double m_obstacleWaitingTimeinIntersection;
+  double distance_to_pdestrian_;
 
   //ROS messages (topics)
   ros::NodeHandle nh;
 
   //define publishers
   ros::Publisher pub_LocalPath;
+  ros::Publisher pub_LocalPathWithPosePub;
   ros::Publisher pub_LocalBasePath;
   ros::Publisher pub_ClosestIndex;
   ros::Publisher pub_BehaviorState;
@@ -140,46 +144,27 @@ class BehaviorGen
   ros::Publisher pub_currentState;
 
   // define subscribers.
-  ros::Subscriber sub_current_pose;
-  ros::Subscriber sub_current_velocity;
-  ros::Subscriber sub_robot_odom;
-  ros::Subscriber sub_can_info;
   ros::Subscriber sub_GlobalPlannerPaths;
-  ros::Subscriber sub_LocalPlannerPaths;
+  // ros::Subscriber sub_LocalPlannerPaths;
   ros::Subscriber sub_TrafficLightStatus;
   ros::Subscriber sub_TrafficLightSignals;
-  ros::Subscriber sub_Trajectory_Cost;
+  // ros::Subscriber sub_Trajectory_Cost;
 
-  ros::Subscriber sub_twist_cmd;
-  ros::Subscriber sub_twist_raw;
-  ros::Subscriber sub_ctrl_cmd;
+  ros::Subscriber sub_PlanningInfo;
 
-  //Added by PHY & HJW
-  ros::Subscriber sub_DistanceToPedestrian;
-  ros::Subscriber sub_SprintSwitch;
-  ros::Subscriber sub_IntersectionCondition;
+  // Others
+  timespec planningTimer;
+  std_msgs::Bool emergency_stop_msg;
 
   // Callback function for subscriber.
-  void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
-  void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg);
-  void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg);
-  void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg);
   void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg);
-  void callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg);
-  void callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg);
-  void callbackDistanceToPedestrian(const std_msgs::Float64& msg);
-  void callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg);
-
-  void callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg);
+  void callbackPlanningInfo(const rubis_msgs::PlanningInfoConstPtr& msg);
 
-  void callbackGetTwistCMD(const geometry_msgs::TwistStampedConstPtr& msg);
-  void callbackGetTwistRaw(const geometry_msgs::TwistStampedConstPtr& msg);
-  void callbackGetCommandCMD(const autoware_msgs::ControlCommandConstPtr& msg);
-  void callbackSprintSwitch(const std_msgs::Bool& msg);
+  // void callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg);
 
   //Helper Functions
   void UpdatePlanningParams(ros::NodeHandle& _nh);
-  void SendLocalPlanningTopics();
+  void SendLocalPlanningTopics(const rubis_msgs::PlanningInfoConstPtr& msg);
   void VisualizeLocalPlanner();
   void LogLocalPlanningInfo(double dt);
   bool GetBaseMapTF();  
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h
index 42eab8ba..608d0524 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h
@@ -50,6 +50,16 @@
 #include "op_planner/BehaviorPrediction.h"
 #include "op_utility/DataRW.h"
 
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include "rubis_msgs/DetectedObjectArray.h"
+#include "rubis_msgs/PoseTwistStamped.h"
+
+typedef message_filters::sync_policies::ExactTime<rubis_msgs::DetectedObjectArray, rubis_msgs::PoseTwistStamped> SyncPolicy;
+typedef message_filters::Synchronizer<SyncPolicy> Sync;
+
 namespace MotionPredictorNS
 {
 
@@ -101,7 +111,7 @@ class MotionPrediction
   timespec m_SensingTimer;
 
   // Object Msg List
-  std::vector<autoware_msgs::DetectedObjectArray> object_msg_list_;  
+  std::vector<autoware_msgs::DetectedObjectArray> object_msg_list_;
 
   // TF
   std::vector<std::string> tf_str_list_;
@@ -110,6 +120,7 @@ class MotionPrediction
 
   ros::NodeHandle nh;
   ros::Publisher pub_predicted_objects_trajectories;
+  ros::Publisher pub_rubis_predicted_objects_trajectories;
   ros::Publisher pub_PredictedTrajectoriesRviz ;
   ros::Publisher pub_CurbsRviz ;
   ros::Publisher pub_ParticlesRviz;
@@ -121,14 +132,23 @@ class MotionPrediction
   ros::Subscriber sub_robot_odom      ;
   ros::Subscriber sub_can_info      ;
   ros::Subscriber sub_StepSignal;
+  
+  message_filters::Subscriber<rubis_msgs::DetectedObjectArray> svl_objects_sub_;
+  message_filters::Subscriber<rubis_msgs::PoseTwistStamped> pose_twist_sub_;    
+  boost::shared_ptr<Sync> sync_;
+
+  rubis_msgs::DetectedObjectArray objects_msgs_;
 
   // Callback function for subscriber.
   void callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg);
+  void callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg);
+  void _callbackGetRubisTrackedObjects(rubis_msgs::DetectedObjectArray& objects_msg);
   void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
   void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg);
   void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg);
   void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg);
   void callbackGetStepForwardSignals(const geometry_msgs::TwistStampedConstPtr& msg);
+  void callbackSvl(const rubis_msgs::DetectedObjectArray::ConstPtr& objects_msg, const rubis_msgs::PoseTwistStamped::ConstPtr& pose_twist_msg);
 
   //Helper functions
   void VisualizePrediction();
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h
index 828e6ea3..0283f3eb 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h
@@ -18,138 +18,165 @@
 #define OP_TRAJECTORY_EVALUATOR_CORE
 #define TF_DEBUG false
 
-#include <ros/ros.h>
-#include <geometry_msgs/TwistStamped.h>
-#include <geometry_msgs/Vector3Stamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseArray.h>
-#include <nav_msgs/Odometry.h>
-#include <autoware_msgs/LaneArray.h>
+#include "rubis_msgs/Bool.h"
+#include "rubis_msgs/LaneArrayWithPoseTwist.h"
 #include <autoware_can_msgs/CANInfo.h>
-#include <autoware_msgs/DetectedObjectArray.h>
 #include <autoware_msgs/DetectedObject.h>
+#include <autoware_msgs/DetectedObjectArray.h>
 #include <autoware_msgs/IntersectionCondition.h>
-#include <visualization_msgs/MarkerArray.h>
-#include <tf/transform_listener.h>
-#include <geometry_msgs/TransformStamped.h>
+#include <autoware_msgs/LaneArray.h>
+#include <geometry_msgs/PoseArray.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
 #include <std_msgs/Float64.h>
 #include <std_msgs/Int32.h>
-#include <std_msgs/Bool.h>
+#include <tf/transform_listener.h>
+#include <visualization_msgs/MarkerArray.h>
 
 #include "op_planner/PlannerCommonDef.h"
 #include "op_planner/TrajectoryDynamicCosts.h"
-
-
-
-namespace TrajectoryEvaluatorNS
-{
-
-class TrajectoryEval
-{
-protected:
-
-  PlannerHNS::TrajectoryDynamicCosts m_TrajectoryCostsCalculator;
-  bool m_bUseMoveingObjectsPrediction;
-
-  geometry_msgs::Pose m_OriginPos;
-
-  PlannerHNS::WayPoint m_CurrentPos;
-  bool bNewCurrentPos;
-
-  PlannerHNS::VehicleState m_VehicleStatus;
-  bool bVehicleStatus;
-
-  std::vector<PlannerHNS::WayPoint> m_temp_path;
-  std::vector<std::vector<PlannerHNS::WayPoint> > m_GlobalPaths;
-  std::vector<std::vector<PlannerHNS::WayPoint> > m_GlobalPathsToUse;
-  std::vector<std::vector<PlannerHNS::WayPoint> > m_GlobalPathSections;
-  std::vector<PlannerHNS::WayPoint> t_centerTrajectorySmoothed;
-  bool bWayGlobalPath;
-  bool bWayGlobalPathToUse;
-  std::vector<std::vector<PlannerHNS::WayPoint> > m_GeneratedRollOuts;
-  bool bRollOuts;
-
-  std::vector<PlannerHNS::DetectedObject> m_PredictedObjects;
-  bool bPredictedObjects;
-  // Added by PHY
-  double m_SprintDecisionTime;
-  int m_pedestrian_stop_img_height_threshold;
-
-  struct timespec m_PlanningTimer;
-  std::vector<std::string>    m_LogData;
-  PlannerHNS::PlanningParams  m_PlanningParams;
-  PlannerHNS::CAR_BASIC_INFO  m_CarInfo;
-  PlannerHNS::BehaviorState   m_CurrentBehavior;
-  visualization_msgs::MarkerArray m_CollisionsDummy;
-  visualization_msgs::MarkerArray m_CollisionsActual;
-  int m_ImageWidth;
-  int m_ImageHeight;
-  double m_PedestrianRightThreshold;
-  double m_PedestrianLeftThreshold;
-  double m_PedestrianImageDetectionRange;  
-  double m_VehicleImageDetectionRange;
-  double m_VehicleImageWidthThreshold;
-  int m_noVehicleCnt;
-
-  //ROS messages (topics)
-  ros::NodeHandle nh;
-
-  //define publishers
-  ros::Publisher pub_CollisionPointsRviz;
-  ros::Publisher pub_LocalWeightedTrajectoriesRviz;
-  ros::Publisher pub_LocalWeightedTrajectories;
-  ros::Publisher pub_TrajectoryCost;
-  ros::Publisher pub_SafetyBorderRviz;
-  ros::Publisher pub_DistanceToPedestrian;
-  ros::Publisher pub_IntersectionCondition;
-  ros::Publisher pub_SprintSwitch;
-  ros::Publisher pub_currentTraj;
-
-  // define subscribers.
-  ros::Subscriber sub_current_pose;
-  ros::Subscriber sub_current_velocity;
-  ros::Subscriber sub_robot_odom;
-  ros::Subscriber sub_can_info;
-  ros::Subscriber sub_GlobalPlannerPaths;
-  ros::Subscriber sub_LocalPlannerPaths;
-  ros::Subscriber sub_predicted_objects;
-  ros::Subscriber sub_current_behavior;
-
-  // HJW added
-  ros::Subscriber sub_current_state;
-
-  // TF
-  tf::TransformListener m_vtob_listener;
-  tf::TransformListener m_vtom_listener;
-  tf::StampedTransform  m_velodyne_to_base_link;
-  tf::StampedTransform  m_velodyne_to_map;
-
-
-  // Callback function for subscriber.
-  void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
-  void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg);
-  void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg);
-  void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg);
-  void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg);
-  void callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg);
-  void callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg);
-  void callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr & msg);
-  void callbackGetCurrentState(const std_msgs::Int32 & msg);
-
-  //Helper Functions
-  void UpdatePlanningParams(ros::NodeHandle& _nh);
-
-  void UpdateMyParams();
-  bool UpdateTf();
-
-public:
-  TrajectoryEval();
-  ~TrajectoryEval();
-  void MainLoop();
+#include "rubis_msgs/DetectedObjectArray.h"
+#include "rubis_msgs/Image.h"
+#include "rubis_msgs/PlanningInfo.h"
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/time_synchronizer.h>
+
+typedef message_filters::sync_policies::ExactTime<rubis_msgs::LaneArrayWithPoseTwist, rubis_msgs::DetectedObjectArray> SyncPolicy;
+typedef message_filters::sync_policies::ExactTime<rubis_msgs::LaneArrayWithPoseTwist, rubis_msgs::DetectedObjectArray, rubis_msgs::Bool>
+    LaneSyncPolicy;
+typedef message_filters::Synchronizer<SyncPolicy> Sync;
+typedef message_filters::Synchronizer<LaneSyncPolicy> LaneSync;
+
+namespace TrajectoryEvaluatorNS {
+
+class TrajectoryEval {
+  protected:
+    bool is_objects_updated_;
+
+    PlannerHNS::TrajectoryDynamicCosts m_TrajectoryCostsCalculator;
+    bool m_bUseMoveingObjectsPrediction;
+
+    geometry_msgs::Pose m_OriginPos;
+
+    PlannerHNS::WayPoint m_CurrentPos;
+    bool bNewCurrentPos;
+
+    PlannerHNS::VehicleState m_VehicleStatus;
+    bool bVehicleStatus;
+
+    std::vector<PlannerHNS::WayPoint> m_temp_path;
+    std::vector<std::vector<PlannerHNS::WayPoint>> m_GlobalPaths;
+    std::vector<std::vector<PlannerHNS::WayPoint>> m_GlobalPathsToUse;
+    std::vector<std::vector<PlannerHNS::WayPoint>> m_GlobalPathSections;
+    std::vector<PlannerHNS::WayPoint> t_centerTrajectorySmoothed;
+    bool bWayGlobalPath;
+    bool bWayGlobalPathToUse;
+    std::vector<std::vector<PlannerHNS::WayPoint>> m_GeneratedRollOuts;
+    bool bRollOuts;
+
+    std::vector<PlannerHNS::DetectedObject> m_PredictedObjects;
+    bool bPredictedObjects;
+    // Added by PHY
+    double m_SprintDecisionTime;
+    int m_pedestrian_stop_img_height_threshold;
+
+    struct timespec m_PlanningTimer;
+    std::vector<std::string> m_LogData;
+    PlannerHNS::PlanningParams m_PlanningParams;
+    PlannerHNS::CAR_BASIC_INFO m_CarInfo;
+    PlannerHNS::BehaviorState m_CurrentBehavior;
+    visualization_msgs::MarkerArray m_CollisionsDummy;
+    visualization_msgs::MarkerArray m_CollisionsActual;
+    int m_ImageWidth;
+    int m_ImageHeight;
+    double m_PedestrianRightThreshold;
+    double m_PedestrianLeftThreshold;
+    double m_PedestrianImageDetectionRange;
+    double m_VehicleImageDetectionRange;
+    double m_VehicleImageWidthThreshold;
+    int m_noVehicleCnt;
+    std::string m_LaneTopic;
+    std_msgs::Bool m_sprint_switch;
+    std_msgs::Float64 m_distance_to_pedestrian;
+
+    // ROS messages (topics)
+    ros::NodeHandle nh;
+
+    // define publishers
+    ros::Publisher pub_CollisionPointsRviz;
+    ros::Publisher pub_LocalWeightedTrajectoriesRviz;
+    ros::Publisher pub_LocalWeightedTrajectories;
+    // ros::Publisher pub_LocalWeightedTrajectoriesWithPoseTwist;
+    // ros::Publisher pub_TrajectoryCost;
+    ros::Publisher pub_SafetyBorderRviz;
+    // ros::Publisher pub_DistanceToPedestrian;
+    // ros::Publisher pub_IntersectionCondition;
+    // ros::Publisher pub_SprintSwitch;
+    ros::Publisher pub_currentTraj;
+    ros::Publisher pub_PlanningInfo;
+
+    // define subscribers.
+    ros::Subscriber sub_current_pose;
+    ros::Subscriber sub_current_velocity;
+    ros::Subscriber sub_robot_odom;
+    ros::Subscriber sub_can_info;
+    ros::Subscriber sub_GlobalPlannerPaths;
+    ros::Subscriber sub_LocalPlannerPaths;
+    ros::Subscriber sub_predicted_objects;
+    ros::Subscriber sub_rubis_predicted_objects;
+
+    message_filters::Subscriber<rubis_msgs::LaneArrayWithPoseTwist> trajectories_sub_;
+    message_filters::Subscriber<rubis_msgs::DetectedObjectArray> objects_sub_;
+    message_filters::Subscriber<rubis_msgs::Bool> lane_sub_;
+    boost::shared_ptr<Sync> sync_;
+    boost::shared_ptr<LaneSync> lane_sync_;
+
+    // HJW added
+    ros::Subscriber sub_current_state;
+
+    // TF
+    tf::TransformListener m_vtob_listener;
+    tf::TransformListener m_vtom_listener;
+    tf::StampedTransform m_velodyne_to_base_link;
+    tf::StampedTransform m_velodyne_to_map;
+
+    // Others
+    std::vector<PlannerHNS::Crossing> intersection_list_;
+    autoware_msgs::DetectedObjectArray object_msg_;
+
+    // Callback function for subscriber.
+    void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr &msg);
+    void _callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr &msg);
+    void callbackGetPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr &msg);
+    void _callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArray &objects);
+    void callbackGetRubisPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr &msg);
+    void callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr &msg);
+    void callbackGetCurrentState(const std_msgs::Int32 &msg);
+    void callback(const rubis_msgs::LaneArrayWithPoseTwist::ConstPtr &trajectories_msg, const rubis_msgs::DetectedObjectArray::ConstPtr &objects_msg);
+    void callbackWithLane(const rubis_msgs::LaneArrayWithPoseTwist::ConstPtr &trajectories_msg,
+                          const rubis_msgs::DetectedObjectArray::ConstPtr &objects_msg, const rubis_msgs::Bool::ConstPtr &lane_msg);
+
+    // Helper Functions
+    void UpdatePlanningParams(ros::NodeHandle &_nh);
+
+    void UpdateMyParams();
+    bool UpdateTf();
+
+  public:
+    TrajectoryEval();
+    ~TrajectoryEval();
+    void MainLoop();
 };
 
-}
+} // namespace TrajectoryEvaluatorNS
 
-#endif  // OP_TRAJECTORY_EVALUATOR_CORE
+#endif // OP_TRAJECTORY_EVALUATOR_CORE
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h
index f7dddce1..b4e70301 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h
@@ -30,12 +30,18 @@
 #include "op_planner/PlannerH.h"
 #include "op_planner/PlannerCommonDef.h"
 
+#include "rubis_msgs/PoseTwistStamped.h"
+#include "rubis_msgs/LaneArrayWithPoseTwist.h"
+
 namespace TrajectoryGeneratorNS
 {
 
 class TrajectoryGen
 {
 protected:
+  geometry_msgs::PoseStamped current_pose_;
+  geometry_msgs::TwistStamped current_twist_;
+
   PlannerHNS::PlannerH m_Planner;
   geometry_msgs::Pose m_OriginPos;
   PlannerHNS::WayPoint m_InitPos;
@@ -59,11 +65,12 @@ class TrajectoryGen
     PlannerHNS::CAR_BASIC_INFO m_CarInfo;
 
 
-    //ROS messages (topics)
+  //ROS messages (topics)
   ros::NodeHandle nh;
 
   //define publishers
   ros::Publisher pub_LocalTrajectories;
+  ros::Publisher pub_LocalTrajectoriesWithPoseTwist;
   ros::Publisher pub_LocalTrajectoriesRviz;
 
   // define subscribers.
@@ -73,16 +80,17 @@ class TrajectoryGen
   ros::Subscriber sub_robot_odom;
   ros::Subscriber sub_can_info;
   ros::Subscriber sub_GlobalPlannerPaths;
+  ros::Subscriber sub_pose_twist;
 
+  // Others
+  
 
   // Callback function for subscriber.
   void callbackGetInitPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &input);
-  void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
-  void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg);
-  void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg);
-  void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg);
+  void callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg);
   void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg);
 
+
   //Helper Functions
   void UpdatePlanningParams(ros::NodeHandle& _nh);
 
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch
index 2af9250d..69cb1a60 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch
@@ -12,7 +12,7 @@
   <!-- <arg name="intersectionFile" default="crossing_test/crossing_intersection.yaml" /> -->
   
       
-  <node pkg="op_local_planner" type="op_behavior_selector" name="op_behavior_selector" output="screen">
+  <node pkg="op_local_planner" type="op_behavior_selector" name="op_behavior_selector" ouptut="screen">
     <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg trafficLightFile)"/>
     <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg stopLineFile)"/>
     <!-- <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg intersectionFile)"/> -->
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch
index 6e095350..57e6b294 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch
@@ -1,114 +1,124 @@
 <launch>
 
-  <arg name="mapSource"         default="0" /> <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
-  <arg name="mapFileName"       default="" />
-    
-  <arg name="pathDensity"       default="0.5" />
-  <arg name="rollOutDensity"       default="0.5" />
-  <arg name="rollOutsNumber"       default="6"    />
-  
-  <arg name="maxVelocity"         default="6.0" />
-  <arg name="minVelocity"         default="0.1" />  
-  <arg name="maxLocalPlanDistance"     default="50" />  
-  <arg name="horizonDistance"       default="200"  />
-  
-  <arg name="minFollowingDistance"     default="35.0"  /> <!-- should be bigger than Distance to follow -->  
-  <arg name="minDistanceToAvoid"     default="20.0" /> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
-  <arg name="maxDistanceToAvoid"     default="5.0"  /> <!-- should be smaller than minDistanceToAvoid -->
-  <arg name="speedProfileFactor"    default="1.2"  />
-  
-  <arg name="smoothingDataWeight"      default="0.45"  />
-  <arg name="smoothingSmoothWeight"    default="0.4"  />  
-  
-  <arg name="horizontalSafetyDistance"  default="1.2"  />
-  <arg name="verticalSafetyDistance"  default="0.8"  />
-  
-  <arg name="enableSwerving"       default="true"  />
-  <arg name="enableFollowing"       default="true" />  
+  <arg name="mapSource" default="0" /> <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
+  <arg name="mapFileName" default="" />
+
+  <arg name="pathDensity" default="0.5" />
+  <arg name="rollOutDensity" default="0.5" />
+  <arg name="rollOutsNumber" default="6" />
+
+  <arg name="maxVelocity" default="6.0" />
+  <arg name="minVelocity" default="0.1" />
+  <arg name="maxLocalPlanDistance" default="50" />
+  <arg name="horizonDistance" default="200" />
+
+  <arg name="minFollowingDistance" default="35.0" /> <!-- should be bigger than Distance to follow -->
+  <arg name="minDistanceToAvoid" default="20.0" /> <!-- should be smaller than minFollowingDistance
+  and larger than maxDistanceToAvoid -->
+  <arg name="maxDistanceToAvoid" default="5.0" /> <!-- should be smaller than minDistanceToAvoid -->
+  <arg name="speedProfileFactor" default="1.2" />
+
+  <arg name="smoothingDataWeight" default="0.45" />
+  <arg name="smoothingSmoothWeight" default="0.4" />
+
+  <arg name="horizontalSafetyDistance" default="1.2" />
+  <arg name="verticalSafetyDistance" default="0.8" />
+
+  <arg name="enableSwerving" default="true" />
+  <arg name="enableFollowing" default="true" />
   <arg name="enableTrafficLightBehavior" default="false" />
-  <arg name="enableStopSignBehavior"   default="false" />  
-  <arg name="enableLaneChange"       default="false" />  
-  
-  <arg name="width"           default="1.85"  />
-  <arg name="length"           default="4.2"  />
-  <arg name="wheelBaseLength"       default="2.7"  />
-  <arg name="turningRadius"        default="5.2"  />
-  <arg name="maxSteerAngle"       default="0.45" />
-
-  <arg name="steeringDelay"       default="1.2" />
-  <arg name="minPursuiteDistance"   default="3.0"  />
-  <arg name="additionalBrakingDistance" default="5.0"  />
-  
-  <arg name="giveUpDistance"       default="-4.0"  />
-  
-  <arg name="maxAcceleration"       default="3.0" />
-  <arg name="maxDeceleration"       default="-3.0" />
-      
-  <arg name="velocitySource"      default="2" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->    
+  <arg name="enableStopSignBehavior" default="false" />
+  <arg name="enableLaneChange" default="false" />
 
-  <!-- HJW Added -->
-  <arg name="stopLineMargin"       default="10.0" />
-  <arg name="stopLineDetectionDistance"       default="20.0" />
-  <arg name="enableSlowDownOnCurve"       default="false" />
-  <arg name="curveVelocityRatio"       default="0.5" />  
-
-  <node pkg="op_local_planner" type="op_common_params" name="op_common_params" output="screen">
-  
-  <!-- Common Parameters -->
-  
-  <param name="mapSource"         value="$(arg mapSource)" /> <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
-  <param name="mapFileName"         value="$(arg mapFileName)" />
-    
-  <param name="pathDensity"           value="$(arg pathDensity)" />
-  <param name="rollOutDensity"       value="$(arg rollOutDensity)" />
-  <param name="rollOutsNumber"       value="$(arg rollOutsNumber)"    />
-  
-  <param name="maxVelocity"         value="$(arg maxVelocity)" />
-  <param name="minVelocity"         value="$(arg minVelocity)" />  
-  <param name="maxLocalPlanDistance"     value="$(arg maxLocalPlanDistance)" />  
-  <param name="horizonDistance"       value="$(arg horizonDistance)" />
-  
-  <param name="minFollowingDistance"     value="$(arg minFollowingDistance)"  /> <!-- should be bigger than Distance to follow -->  
-  <param name="minDistanceToAvoid"     value="$(arg minDistanceToAvoid)" /> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
-  <param name="maxDistanceToAvoid"     value="$(arg maxDistanceToAvoid)"  /> <!-- should be smaller than minDistanceToAvoid -->
-  <param name="speedProfileFactor"    value="$(arg speedProfileFactor)"  />
-  
-  <param name="smoothingDataWeight"    value="$(arg smoothingDataWeight)"  />
-  <param name="smoothingSmoothWeight"    value="$(arg smoothingSmoothWeight)"  />
-  
-  <param name="horizontalSafetyDistance"  value="$(arg horizontalSafetyDistance)"  />
-  <param name="verticalSafetyDistance"  value="$(arg verticalSafetyDistance)"  />
-  
-  <param name="enableSwerving"       value="$(arg enableSwerving)"  />
-  <param name="enableFollowing"       value="$(arg enableFollowing)" />  
-  <param name="enableTrafficLightBehavior" value="$(arg enableTrafficLightBehavior)" />
-  <param name="enableStopSignBehavior"   value="$(arg enableStopSignBehavior)" />  
-  <param name="enableLaneChange"       value="$(arg enableLaneChange)" />  
-  
-  <param name="width"           value="$(arg width)"  />
-  <param name="length"           value="$(arg length)"  />
-  <param name="wheelBaseLength"       value="$(arg wheelBaseLength)"  />
-  <param name="turningRadius"        value="$(arg turningRadius)"  />
-  <param name="maxSteerAngle"       value="$(arg maxSteerAngle)" />
-
-  <param name="steeringDelay"       value="$(arg steeringDelay)" />
-  <param name="minPursuiteDistance"     value="$(arg minPursuiteDistance)"  />  
-  <param name="additionalBrakingDistance" value="$(arg additionalBrakingDistance)"  />
-
-  <param name="enableSlowDownOnCurve" value="$(arg enableSlowDownOnCurve)"  />
-  <param name="curveVelocityRatio" value="$(arg curveVelocityRatio)"  />
-  
-  <param name="giveUpDistance" value="$(arg giveUpDistance)"  />
-  
-  <param name="maxAcceleration"       value="$(arg maxAcceleration)" />
-  <param name="maxDeceleration"       value="$(arg maxDeceleration)" />
-      
-  <param name="velocitySource"      value="$(arg velocitySource)" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->    
+  <arg name="width" default="1.85" />
+  <arg name="length" default="4.2" />
+  <arg name="wheelBaseLength" default="2.7" />
+  <arg name="turningRadius" default="5.2" />
+  <arg name="maxSteerAngle" default="0.45" />
+
+  <arg name="steeringDelay" default="1.2" />
+  <arg name="minPursuiteDistance" default="3.0" />
+  <arg name="additionalBrakingDistance" default="5.0" />
+
+  <arg name="giveUpDistance" default="-4.0" />
+
+  <arg name="maxAcceleration" default="3.0" />
+  <arg name="maxDeceleration" default="-3.0" />
+
+  <arg name="velocitySource" default="2" /> <!-- read velocities from (0- Odometry, 1- autoware
+  current_velocities, 2- car_info) "" -->
+
+  <arg name="blockIdx" deafult="-1" />
 
   <!-- HJW Added -->
-  <param name="stopLineMargin"       value="$(arg stopLineMargin)" />
-  <param name="stopLineDetectionDistance"       value="$(arg stopLineDetectionDistance)" />
-      
-  </node>        
-      
-</launch>
+  <arg name="stopLineMargin" default="10.0" />
+  <arg name="stopLineDetectionDistance" default="20.0" />
+  <arg name="enableSlowDownOnCurve" default="false" />
+  <arg name="curveVelocityRatio" default="0.5" />
+
+  <node pkg="op_local_planner" type="op_common_params" name="op_common_params">
+
+    <!-- Common Parameters -->
+
+    <param name="mapSource" value="$(arg mapSource)" /> <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
+    <param name="mapFileName" value="$(arg mapFileName)" />
+
+    <param name="pathDensity" value="$(arg pathDensity)" />
+    <param name="rollOutDensity" value="$(arg rollOutDensity)" />
+    <param name="rollOutsNumber" value="$(arg rollOutsNumber)" />
+
+    <param name="maxVelocity" value="$(arg maxVelocity)" />
+    <param name="minVelocity" value="$(arg minVelocity)" />
+    <param name="maxLocalPlanDistance" value="$(arg maxLocalPlanDistance)" />
+    <param name="horizonDistance" value="$(arg horizonDistance)" />
+
+    <param name="minFollowingDistance" value="$(arg minFollowingDistance)" /> <!-- should be bigger
+    than Distance to follow -->
+    <param name="minDistanceToAvoid" value="$(arg minDistanceToAvoid)" /> <!-- should be smaller than
+    minFollowingDistance and larger than maxDistanceToAvoid -->
+    <param name="maxDistanceToAvoid" value="$(arg maxDistanceToAvoid)" /> <!-- should be smaller than
+    minDistanceToAvoid -->
+    <param name="speedProfileFactor" value="$(arg speedProfileFactor)" />
+
+    <param name="smoothingDataWeight" value="$(arg smoothingDataWeight)" />
+    <param name="smoothingSmoothWeight" value="$(arg smoothingSmoothWeight)" />
+
+    <param name="horizontalSafetyDistance" value="$(arg horizontalSafetyDistance)" />
+    <param name="verticalSafetyDistance" value="$(arg verticalSafetyDistance)" />
+
+    <param name="enableSwerving" value="$(arg enableSwerving)" />
+    <param name="enableFollowing" value="$(arg enableFollowing)" />
+    <param name="enableTrafficLightBehavior" value="$(arg enableTrafficLightBehavior)" />
+    <param name="enableStopSignBehavior" value="$(arg enableStopSignBehavior)" />
+    <param name="enableLaneChange" value="$(arg enableLaneChange)" />
+
+    <param name="width" value="$(arg width)" />
+    <param name="length" value="$(arg length)" />
+    <param name="wheelBaseLength" value="$(arg wheelBaseLength)" />
+    <param name="turningRadius" value="$(arg turningRadius)" />
+    <param name="maxSteerAngle" value="$(arg maxSteerAngle)" />
+
+    <param name="steeringDelay" value="$(arg steeringDelay)" />
+    <param name="minPursuiteDistance" value="$(arg minPursuiteDistance)" />
+    <param name="additionalBrakingDistance" value="$(arg additionalBrakingDistance)" />
+
+    <param name="enableSlowDownOnCurve" value="$(arg enableSlowDownOnCurve)" />
+    <param name="curveVelocityRatio" value="$(arg curveVelocityRatio)" />
+
+    <param name="giveUpDistance" value="$(arg giveUpDistance)" />
+
+    <param name="maxAcceleration" value="$(arg maxAcceleration)" />
+    <param name="maxDeceleration" value="$(arg maxDeceleration)" />
+
+    <param name="velocitySource" value="$(arg velocitySource)" /> <!-- read velocities from (0-
+    Odometry, 1- autoware current_velocities, 2- car_info) "" -->
+
+    <!-- HJW Added -->
+    <param name="stopLineMargin" value="$(arg stopLineMargin)" />
+    <param name="stopLineDetectionDistance" value="$(arg stopLineDetectionDistance)" />
+
+    <param name="blockIdx" value="$(arg blockIdx)" />
+
+  </node>
+
+</launch>
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch
index 589dc93c..40b901f5 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch
@@ -52,7 +52,7 @@
   <arg name="stopLineMargin"       default="10.0" />
   <arg name="stopLineDetectionDistance"       default="20.0" />
 
-  <node pkg="op_local_planner" type="op_common_params" name="op_common_params" output="screen">
+  <node pkg="op_local_planner" type="op_common_params" name="op_common_params">
   
   <!-- Common Parameters -->
   
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch
index 123351f3..273895a0 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch
@@ -10,9 +10,10 @@
   <arg name="enableParticleFilterPrediction"   default="false" />
   <arg name="input_object_list" default="[/tracked_objects]" />
   <arg name="object_tf_list" default="[/velodyne]" />
+  <arg name="use_svl_sensing" default="false" />
   
   
-  <node pkg="op_local_planner" type="op_motion_predictor" name="op_motion_predictor" output="screen">    
+  <node pkg="op_local_planner" type="op_motion_predictor" name="op_motion_predictor" output="screen" >
     <param name="max_distance_to_lane"     value="$(arg max_distance_to_lane)"/>
     <param name="prediction_distance"     value="$(arg prediction_distance)"/>
     <param name="enableGenrateBranches"   value="$(arg enableGenrateBranches)"/>
@@ -23,10 +24,11 @@
     <param name="enableParticleFilterPrediction"   value="$(arg enableParticleFilterPrediction)" />
     <param name="input_object_list" value="$(arg input_object_list)" />
     <param name="object_tf_list" value="$(arg object_tf_list)" />
+    <param name="use_svl_sensing" value="$(arg use_svl_sensing)" />
   </node>
 
   <node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="predicted_objects_visualizer"
-        output="screen">
+       >
     <param name="objects_src_topic" value="/predicted_objects"/>
     <param name="label" value="total" />
   </node>
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch
index 37026b00..519783b3 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch
@@ -1,51 +1,76 @@
 <launch>
   <!-- Trajectory Evaluation Specific Parameters -->
-  
-  <arg name="enablePrediction"       default="false" />                
-  <arg name="horizontalSafetyDistance"   default="1.2" />
-  <arg name="verticalSafetyDistance"     default="0.8" />
 
-  <arg name="weightPriority"   default="0.5" />
-  <arg name="weightTransition"     default="4" />
-  <arg name="weightLong"   default="0.5" />
-  <arg name="weightLat"     default="5" />
-  <arg name="LateralSkipDistance"   default="5.0" />
+  <arg name="enableDebug" default="0" />
+  <arg name="laneTopic" default="None" />
+  <arg name="enablePrediction" default="false" />
+  <arg name="horizontalSafetyDistance" default="1.2" />
+  <arg name="verticalSafetyDistance" default="0.8" />
+
+  <arg name="weightPriority" default="0.5" />
+  <arg name="weightTransition" default="4" />
+  <arg name="weightLong" default="0.5" />
+  <arg name="weightLat" default="5" />
+  <arg name="LateralSkipDistance" default="5.0" />
+
+  <arg name="lateralBlockingThreshold" default="1.5" />
+  <arg name="frontLongitudinalBlockingThreshold" default="30" />
+  <arg name="rearLongitudinalBlockingThreshold" default="-5" />
 
   <arg name="ImageWidth" default="1920" />
   <arg name="ImageHeight" default="1080" />
   <arg name="PedestrianRightThreshold" default="7.0" />
   <arg name="PedestrianLeftThreshold" default="2.0" />
-  <arg name="PedestrianImageDetectionRange" default="0.7" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-  <arg name="PedestrianStopImgHeightThreshold" default="120" /> <!-- Threshold value of detected pedestrian object box's height. If pedestrian object's hegiht is lower than this value, ego will be stoped -->               
-  <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is located in this range, our car set speed as max velocity --> 
-  <arg name="VehicleImageWidthThreshold" default="0.025" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
+
+  <arg name="PedestrianImageDetectionRange" default="0.7" />
+  <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->
+
+  <arg name="PedestrianStopImgHeightThreshold" default="120" />
+  <!-- Threshold value of detected pedestrian object box's height. If pedestrian object's hegiht is
+  lower than this value, ego will be stoped -->
+
+  <arg name="VehicleImageDetectionRange" default="0.3" />
+  <!-- (Ratio) If vehicle's center point is located in this range, our car set speed as max velocity -->
+
+  <arg name="VehicleImageWidthThreshold" default="0.025" />
+  <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
+
   <arg name="SprintDecisionTime" default="5.0" /> <!-- Seconds -->
 
   <arg name="intersectionFile" default="crossing_test/crossing_intersection.yaml" />
-      
-  <node pkg="op_local_planner" type="op_trajectory_evaluator" name="op_trajectory_evaluator" output="screen">
-    <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg intersectionFile)"/>
-  
-    <param name="enablePrediction"       value="$(arg enablePrediction)" />            
-    <param name="horizontalSafetyDistance"   value="$(arg horizontalSafetyDistance)" />
-    <param name="verticalSafetyDistance"   value="$(arg verticalSafetyDistance)" />    
-
-    <param name="weightPriority"       value="$(arg weightPriority)" />            
-    <param name="weightTransition"       value="$(arg weightTransition)" />            
-    <param name="weightLong"       value="$(arg weightLong)" />            
-    <param name="weightLat"       value="$(arg weightLat)" />            
-    <param name="LateralSkipDistance"       value="$(arg LateralSkipDistance)" />                
+
+  <node pkg="op_local_planner" type="op_trajectory_evaluator" name="op_trajectory_evaluator"
+    output="screen">
+    <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg intersectionFile)" />
+
+    <param name="enableDebug" value="$(arg enableDebug)" />
+    <param name="laneTopic" value="$(arg laneTopic)" />
+
+    <param name="enablePrediction" value="$(arg enablePrediction)" />
+    <param name="horizontalSafetyDistance" value="$(arg horizontalSafetyDistance)" />
+    <param name="verticalSafetyDistance" value="$(arg verticalSafetyDistance)" />
+
+    <param name="weightPriority" value="$(arg weightPriority)" />
+    <param name="weightTransition" value="$(arg weightTransition)" />
+    <param name="weightLong" value="$(arg weightLong)" />
+    <param name="weightLat" value="$(arg weightLat)" />
+    <param name="LateralSkipDistance" value="$(arg LateralSkipDistance)" />
+
+    <param name="lateralBlockingThreshold" value="$(arg lateralBlockingThreshold)" />
+    <param name="frontLongitudinalBlockingThreshold"
+      value="$(arg frontLongitudinalBlockingThreshold)" />
+    <param name="rearLongitudinalBlockingThreshold" value="$(arg rearLongitudinalBlockingThreshold)" />
 
     <!-- For detecting person and vehicle -->
     <param name="ImageWidth" value="$(arg ImageWidth)" />
     <param name="ImageHeight" value="$(arg ImageHeight)" />
-    <param name="PedestrianRightThreshold"  value="$(arg PedestrianRightThreshold)" />                
-    <param name="PedestrianLeftThreshold"   value="$(arg PedestrianLeftThreshold)" />  
-    <param name="PedestrianImageDetectionRange" value="$(arg PedestrianImageDetectionRange)" />        
-    <param name="PedestrianStopImgHeightThreshold" value="$(arg PedestrianStopImgHeightThreshold)" />        
-    <param name="VehicleImageDetectionRange" value="$(arg VehicleImageDetectionRange)" />    
+    <param name="PedestrianRightThreshold" value="$(arg PedestrianRightThreshold)" />
+    <param name="PedestrianLeftThreshold" value="$(arg PedestrianLeftThreshold)" />
+    <param name="PedestrianImageDetectionRange" value="$(arg PedestrianImageDetectionRange)" />
+    <param name="PedestrianStopImgHeightThreshold" value="$(arg PedestrianStopImgHeightThreshold)" />
+    <param name="VehicleImageDetectionRange" value="$(arg VehicleImageDetectionRange)" />
     <param name="VehicleImageWidthThreshold" value="$(arg VehicleImageWidthThreshold)" />
     <param name="SprintDecisionTime" value="$(arg SprintDecisionTime)" />
-  </node>        
-      
-</launch>
+  </node>
+
+</launch>
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch
index 2e79bd1e..a0ff82c3 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch
@@ -1,35 +1,36 @@
 <launch>
   <!-- Trajectory Evaluation Specific Parameters -->
-  
-  <arg name="enablePrediction"       default="false" />                
-  <arg name="horizontalSafetyDistance"   default="1.2" />
-  <arg name="verticalSafetyDistance"     default="0.8" />
+  <arg name="enableDebug" default="0" />
+  <arg name="laneTopic" default="None" />
+  <arg name="enablePrediction" default="false" />
+  <arg name="horizontalSafetyDistance" default="1.2" />
+  <arg name="verticalSafetyDistance" default="0.8" />
+  <arg name="LateralSkipDistance" default="5.0" />
+  <arg name="lateralBlockingThreshold" default="1.5" />
+  <arg name="frontLongitudinalBlockingThreshold" default="30" />
+  <arg name="rearLongitudinalBlockingThreshold" default="-5" />
+  <arg name="SprintDecisionTime" default="9999.0" />
+  <arg name="intersectionFile" default="crossing_test/crossing_intersection.yaml" />
 
-  <arg name="weightPriority"   default="0.5" />
-  <arg name="weightTransition"     default="4" />
-  <arg name="weightLong"   default="0.5" />
-  <arg name="weightLat"     default="5" />
-  <arg name="LateralSkipDistance"   default="5.0" />
+  <node pkg="op_local_planner" type="op_trajectory_evaluator" name="op_trajectory_evaluator"
+    output="screen">
+    <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg intersectionFile)" />
 
-  <arg name="SprintDecisionTime" default="9999.0" /> 
+    <param name="enableDebug" value="$(arg enableDebug)" />
 
-  <arg name="intersectionFile" default="crossing_test/crossing_intersection.yaml" />
-      
-  <node pkg="op_local_planner" type="op_trajectory_evaluator" name="op_trajectory_evaluator" output="screen">
-    <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg intersectionFile)"/>
-  
-    <param name="enablePrediction"       value="$(arg enablePrediction)" />            
-    <param name="horizontalSafetyDistance"   value="$(arg horizontalSafetyDistance)" />
-    <param name="verticalSafetyDistance"   value="$(arg verticalSafetyDistance)" />    
+    <param name="laneTopic" value="$(arg laneTopic)" />
+    <param name="enablePrediction" value="$(arg enablePrediction)" />
+    <param name="horizontalSafetyDistance" value="$(arg horizontalSafetyDistance)" />
+    <param name="verticalSafetyDistance" value="$(arg verticalSafetyDistance)" />
+    <param name="LateralSkipDistance" value="$(arg LateralSkipDistance)" />
 
-    <param name="weightPriority"       value="$(arg weightPriority)" />            
-    <param name="weightTransition"       value="$(arg weightTransition)" />            
-    <param name="weightLong"       value="$(arg weightLong)" />            
-    <param name="weightLat"       value="$(arg weightLat)" />            
-    <param name="LateralSkipDistance"       value="$(arg LateralSkipDistance)" />                
+    <param name="lateralBlockingThreshold" value="$(arg lateralBlockingThreshold)" />
+    <param name="frontLongitudinalBlockingThreshold"
+      value="$(arg frontLongitudinalBlockingThreshold)" />
+    <param name="rearLongitudinalBlockingThreshold" value="$(arg rearLongitudinalBlockingThreshold)" />
 
     <!-- For detecting person and vehicle -->
     <param name="SprintDecisionTime" value="$(arg SprintDecisionTime)" />
-  </node>        
-      
-</launch>
+  </node>
+
+</launch>
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch.backup b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch.backup
deleted file mode 100644
index a8ffc17c..00000000
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch.backup
+++ /dev/null
@@ -1,49 +0,0 @@
-<launch>
-  <!-- Trajectory Evaluation Specific Parameters -->
-  
-  <arg name="enablePrediction"       default="false" />                
-  <arg name="horizontalSafetyDistance"   default="1.2" />
-  <arg name="verticalSafetyDistance"     default="0.8" />
-
-  <!-- <arg name="weightPriority"   default="0.5" />
-  <arg name="weightTransition"     default="4" />
-  <arg name="weightLong"   default="0.5" />
-  <arg name="weightLat"     default="5" /> -->
-  <arg name="LateralSkipDistance"   default="5.0" />
-
-  <!-- <arg name="ImageWidth" default="1920" />
-  <arg name="ImageHeight" default="1080" /> -->
-  <arg name="PedestrianRightThreshold" default="7.0" />
-  <arg name="PedestrianLeftThreshold" default="2.0" />
-  <arg name="PedestrianImageDetectionRange" default="0.7" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-  <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is located in this range, our car set speed as max velocity --> 
-  <arg name="VehicleImageWidthThreshold" default="0.025" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
-  <!-- <arg name="SprintDecisionTime" default="5.0" />  -->
-
-  <arg name="intersectionFile" default="crossing_test/crossing_intersection.yaml" />
-      
-  <node pkg="op_local_planner" type="op_trajectory_evaluator" name="op_trajectory_evaluator" output="screen">
-    <rosparam command="load" file="$(find rubis_pkg)/cfg/$(arg intersectionFile)"/>
-  
-    <param name="enablePrediction"       value="$(arg enablePrediction)" />            
-    <param name="horizontalSafetyDistance"   value="$(arg horizontalSafetyDistance)" />
-    <param name="verticalSafetyDistance"   value="$(arg verticalSafetyDistance)" />    
-
-    <!-- <param name="weightPriority"       value="$(arg weightPriority)" />            
-    <param name="weightTransition"       value="$(arg weightTransition)" />            
-    <param name="weightLong"       value="$(arg weightLong)" />            
-    <param name="weightLat"       value="$(arg weightLat)" />             -->
-    <param name="LateralSkipDistance"       value="$(arg LateralSkipDistance)" />                
-
-    <!-- For detecting person and vehicle -->
-    <!-- <param name="ImageWidth" value="$(arg ImageWidth)" /> -->
-    <!-- <param name="ImageHeight" value="$(arg ImageHeight)" /> -->
-    <param name="PedestrianRightThreshold"  value="$(arg PedestrianRightThreshold)" />                
-    <param name="PedestrianLeftThreshold"   value="$(arg PedestrianLeftThreshold)" />  
-    <param name="PedestrianImageDetectionRange" value="$(arg PedestrianImageDetectionRange)" />        
-    <param name="VehicleImageDetectionRange" value="$(arg VehicleImageDetectionRange)" />    
-    <param name="VehicleImageWidthThreshold" value="$(arg VehicleImageWidthThreshold)" />
-    <!-- <param name="SprintDecisionTime" value="$(arg SprintDecisionTime)" /> -->
-  </node>        
-      
-</launch>
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp
index 9d824cfd..263055ec 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp
@@ -15,403 +15,453 @@
  */
 
 #include "op_behavior_selector_core.h"
-#include "op_ros_helpers/op_ROSHelpers.h"
 #include "op_planner/MappingHelpers.h"
+#include "op_ros_helpers/op_ROSHelpers.h"
 #include <rubis_lib/sched.hpp>
 
-namespace BehaviorGeneratorNS
-{
-
-BehaviorGen::BehaviorGen()
-{
-  sleep(2);
-  bNewCurrentPos = false;
-  bVehicleStatus = false;
-  bWayGlobalPath = false;
-  bWayGlobalPathLogs = false;
-  bNewLightStatus = false;
-  bNewLightSignal = false;
-  bBestCost = false;
-  bMap = false;
-  bRollOuts = false;
-
-  ros::NodeHandle _nh;
-  UpdatePlanningParams(_nh);
-
-  // RUBIS driving parameter
-  nh.getParam("/op_behavior_selector/distanceToPedestrianThreshold", m_distanceToPedestrianThreshold);
-  nh.param("/op_behavior_selector/turnThreshold", m_turnThreshold, 20.0);
-
-
-  tf::StampedTransform transform;
-  PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
-  m_OriginPos.position.x  = transform.getOrigin().x();
-  m_OriginPos.position.y  = transform.getOrigin().y();
-  m_OriginPos.position.z  = transform.getOrigin().z();
-
-  pub_LocalPath = nh.advertise<autoware_msgs::Lane>("final_waypoints", 1,true);
-  pub_LocalBasePath = nh.advertise<autoware_msgs::Lane>("base_waypoints", 1,true);
-  pub_ClosestIndex = nh.advertise<std_msgs::Int32>("closest_waypoint", 1,true);
-  pub_BehaviorState = nh.advertise<geometry_msgs::TwistStamped>("current_behavior", 1);
-  pub_SimuBoxPose    = nh.advertise<geometry_msgs::PoseArray>("sim_box_pose_ego", 1);
-  pub_BehaviorStateRviz = nh.advertise<visualization_msgs::MarkerArray>("behavior_state", 1);
-  pub_SelectedPathRviz = nh.advertise<visualization_msgs::MarkerArray>("local_selected_trajectory_rviz", 1);
-  pub_EmergencyStop = nh.advertise<std_msgs::Bool>("emergency_stop", 1);
-  pub_turnAngle = nh.advertise<std_msgs::Float64>("turn_angle", 1);
-  pub_turnMarker = nh.advertise<visualization_msgs::MarkerArray>("turn_marker", 1);
-  pub_currentState = nh.advertise<std_msgs::Int32>("current_state", 1);
-
-  sub_current_pose = nh.subscribe("/current_pose", 10,  &BehaviorGen::callbackGetCurrentPose, this); 
-
-  int bVelSource = 1;
-  _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
-  if(bVelSource == 0)
-    sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this);
-  else if(bVelSource == 1)
-    sub_current_velocity = nh.subscribe("/current_velocity", 10, &BehaviorGen::callbackGetVehicleStatus, this);
-  else if(bVelSource == 2)
-    sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this);
-  
-  /*  RT Scheduling setup  */
-  // sub_current_pose = nh.subscribe("/current_pose", 1,  &BehaviorGen::callbackGetCurrentPose, this); //origin 10
-
-  // int bVelSource = 1;
-  // _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
-  // if(bVelSource == 0)
-  //   sub_robot_odom = nh.subscribe("/odom", 1, &BehaviorGen::callbackGetRobotOdom, this); //origin 10
-  // else if(bVelSource == 1)
-  //   sub_current_velocity = nh.subscribe("/current_velocity", 1, &BehaviorGen::callbackGetVehicleStatus, this); //origin 10
-  // else if(bVelSource == 2)
-  //   sub_can_info = nh.subscribe("/can_info", 1, &BehaviorGen::callbackGetCANInfo, this); //origin 10
-
-  sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this);
-  sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories", 1, &BehaviorGen::callbackGetLocalPlannerPath, this);
-  // sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this);
-  // sub_TrafficLightSignals  = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this);
-  sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this);
-
-  sub_TrafficLightSignals  = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this);
-
-  sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this);
-  sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this);
-  //sub_ctrl_cmd = nh.subscribe("/ctrl_cmd", 1, &BehaviorGen::callbackGetCommandCMD, this);
-  sub_DistanceToPedestrian = nh.subscribe("/distance_to_pedestrian", 1, &BehaviorGen::callbackDistanceToPedestrian, this);
-  sub_IntersectionCondition = nh.subscribe("/intersection_condition", 1, &BehaviorGen::callbackIntersectionCondition, this);
-  sub_SprintSwitch = nh.subscribe("/sprint_switch", 1, &BehaviorGen::callbackSprintSwitch, this);
-
-  //Mapping Section
-  sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &BehaviorGen::callbackGetVMLanes,  this);
-  sub_points = nh.subscribe("/vector_map_info/point", 1, &BehaviorGen::callbackGetVMPoints,  this);
-  sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &BehaviorGen::callbackGetVMdtLanes,  this);
-  sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &BehaviorGen::callbackGetVMIntersections,  this);
-  sup_area = nh.subscribe("/vector_map_info/area", 1, &BehaviorGen::callbackGetVMAreas,  this);
-  sub_lines = nh.subscribe("/vector_map_info/line", 1, &BehaviorGen::callbackGetVMLines,  this);
-  sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &BehaviorGen::callbackGetVMStopLines,  this);
-  sub_signals = nh.subscribe("/vector_map_info/signal", 1, &BehaviorGen::callbackGetVMSignal,  this);
-  sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &BehaviorGen::callbackGetVMVectors,  this);
-  sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &BehaviorGen::callbackGetVMCurbs,  this);
-  sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &BehaviorGen::callbackGetVMRoadEdges,  this);
-  sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &BehaviorGen::callbackGetVMWayAreas,  this);
-  sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &BehaviorGen::callbackGetVMCrossWalks,  this);
-  sub_nodes = nh.subscribe("/vector_map_info/node", 1, &BehaviorGen::callbackGetVMNodes,  this);
-}
+namespace BehaviorGeneratorNS {
 
-BehaviorGen::~BehaviorGen()
-{
-  UtilityHNS::DataRW::WriteLogData(UtilityHNS::UtilityH::GetHomeDirectory()+UtilityHNS::DataRW::LoggingMainfolderName+UtilityHNS::DataRW::StatesLogFolderName, "MainLog",
+BehaviorGen::BehaviorGen() {
+    sleep(2);
+    bNewCurrentPos = false;
+    bVehicleStatus = false;
+    bWayGlobalPath = false;
+    bWayGlobalPathLogs = false;
+    bNewLightStatus = false;
+    bNewLightSignal = false;
+    bBestCost = false;
+    bMap = false;
+    bRollOuts = false;
+    UtilityHNS::UtilityH::GetTickCount(planningTimer);
+    distance_to_pdestrian_ = 1000.0;
+
+    ros::NodeHandle _nh;
+    UpdatePlanningParams(_nh);
+
+    // RUBIS driving parameter
+    nh.getParam("/op_behavior_selector/distanceToPedestrianThreshold", m_distanceToPedestrianThreshold);
+    nh.param("/op_behavior_selector/turnThreshold", m_turnThreshold, 20.0);
+
+    tf::StampedTransform transform;
+    PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
+    m_OriginPos.position.x = transform.getOrigin().x();
+    m_OriginPos.position.y = transform.getOrigin().y();
+    m_OriginPos.position.z = transform.getOrigin().z();
+
+    // pub_LocalPath = nh.advertise<autoware_msgs::Lane>("final_waypoints", 1,true);
+    pub_LocalPathWithPosePub = nh.advertise<rubis_msgs::LaneWithPoseTwist>("final_waypoints_with_pose_twist", 1, true);
+    // pub_LocalBasePath = nh.advertise<autoware_msgs::Lane>("base_waypoints", 1,true);
+    pub_ClosestIndex = nh.advertise<std_msgs::Int32>("closest_waypoint", 1, true);
+    // pub_BehaviorState = nh.advertise<geometry_msgs::TwistStamped>("current_behavior", 1);
+    pub_SimuBoxPose = nh.advertise<geometry_msgs::PoseArray>("sim_box_pose_ego", 1);
+    pub_BehaviorStateRviz = nh.advertise<visualization_msgs::MarkerArray>("behavior_state", 1);
+    pub_SelectedPathRviz = nh.advertise<visualization_msgs::MarkerArray>("local_selected_trajectory_rviz", 1);
+    pub_EmergencyStop = nh.advertise<std_msgs::Bool>("emergency_stop", 1);
+    pub_turnAngle = nh.advertise<std_msgs::Float64>("turn_angle", 1);
+    pub_turnMarker = nh.advertise<visualization_msgs::MarkerArray>("turn_marker", 1);
+    // pub_currentState = nh.advertise<std_msgs::Int32>("current_state", 1);
+
+    // int bVelSource = 1;
+    // _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
+
+    sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this);
+    // sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories_with_pose_twist", 1, &BehaviorGen::callbackGetLocalPlannerPath, this);
+    // sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this);
+
+    // sub_DistanceToPedestrian = nh.subscribe("/distance_to_pedestrian", 1, &BehaviorGen::callbackDistanceToPedestrian, this);
+    // sub_IntersectionCondition = nh.subscribe("/intersection_condition", 1, &BehaviorGen::callbackIntersectionCondition, this);
+    // sub_SprintSwitch = nh.subscribe("/sprint_switch", 1, &BehaviorGen::callbackSprintSwitch, this);
+    sub_PlanningInfo = nh.subscribe("/planning_info", 10, &BehaviorGen::callbackPlanningInfo, this);
+
+    // Mapping Section
+    sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &BehaviorGen::callbackGetVMLanes, this);
+    sub_points = nh.subscribe("/vector_map_info/point", 1, &BehaviorGen::callbackGetVMPoints, this);
+    sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &BehaviorGen::callbackGetVMdtLanes, this);
+    sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &BehaviorGen::callbackGetVMIntersections, this);
+    sup_area = nh.subscribe("/vector_map_info/area", 1, &BehaviorGen::callbackGetVMAreas, this);
+    sub_lines = nh.subscribe("/vector_map_info/line", 1, &BehaviorGen::callbackGetVMLines, this);
+    sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &BehaviorGen::callbackGetVMStopLines, this);
+    sub_signals = nh.subscribe("/vector_map_info/signal", 1, &BehaviorGen::callbackGetVMSignal, this);
+    sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &BehaviorGen::callbackGetVMVectors, this);
+    sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &BehaviorGen::callbackGetVMCurbs, this);
+    sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &BehaviorGen::callbackGetVMRoadEdges, this);
+    sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &BehaviorGen::callbackGetVMWayAreas, this);
+    sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &BehaviorGen::callbackGetVMCrossWalks, this);
+    sub_nodes = nh.subscribe("/vector_map_info/node", 1, &BehaviorGen::callbackGetVMNodes, this);
+}
+
+BehaviorGen::~BehaviorGen() {
+    UtilityHNS::DataRW::WriteLogData(
+        UtilityHNS::UtilityH::GetHomeDirectory() + UtilityHNS::DataRW::LoggingMainfolderName + UtilityHNS::DataRW::StatesLogFolderName, "MainLog",
         "time,dt, Behavior_i, Behavior_str, RollOuts_n, Blocked_i, Central_i, Selected_i, StopSign_id, Light_id, Stop_Dist, Follow_Dist, Follow_Vel,"
-        "Target_Vel, PID_Vel, T_cmd_Vel, C_cmd_Vel, Vel, Steer, X, Y, Z, Theta,"
-        , m_LogData);
+        "Target_Vel, PID_Vel, T_cmd_Vel, C_cmd_Vel, Vel, Steer, X, Y, Z, Theta,",
+        m_LogData);
 }
 
-void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh)
-{
-  _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
-  if(m_PlanningParams.enableSwerving)
-    m_PlanningParams.enableFollowing = true;
-  else
-    _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
-
-  _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
-  _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
-
-  _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
-  _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
-  _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
-
-  _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
-
-  _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
-  if(m_PlanningParams.enableSwerving)
-    _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
-  else
-    m_PlanningParams.rollOutNumber = 0;
-
-  _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
-  _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
-  _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
-  _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
-  _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
-
-  _nh.getParam("/op_trajectory_evaluator/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
-  _nh.getParam("/op_trajectory_evaluator/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
-
-  _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
-
-  _nh.getParam("/op_common_params/width", m_CarInfo.width);
-  _nh.getParam("/op_common_params/length", m_CarInfo.length);
-  _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
-  _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
-  _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
-  _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
-  _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
-  m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
-  m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
-
-  _nh.getParam("/op_common_params/stopLineMargin", m_PlanningParams.stopLineMargin);
-  _nh.getParam("/op_common_params/stopLineDetectionDistance", m_PlanningParams.stopLineDetectionDistance);
-
-  PlannerHNS::ControllerParams controlParams;
-  controlParams.Steering_Gain = PlannerHNS::PID_CONST(0.07, 0.02, 0.01);
-  controlParams.Velocity_Gain = PlannerHNS::PID_CONST(0.1, 0.005, 0.1);
-  nh.getParam("/op_common_params/steeringDelay", controlParams.SteeringDelay);
-  nh.getParam("/op_common_params/minPursuiteDistance", controlParams.minPursuiteDistance );
-  nh.getParam("/op_common_params/additionalBrakingDistance", m_PlanningParams.additionalBrakingDistance );
-  nh.getParam("/op_common_params/giveUpDistance", m_PlanningParams.giveUpDistance );
-  nh.getParam("/op_common_params/enableSlowDownOnCurve", m_PlanningParams.enableSlowDownOnCurve );
-  nh.getParam("/op_common_params/curveVelocityRatio", m_PlanningParams.curveVelocityRatio );
-
-  int iSource = 0;
-  _nh.getParam("/op_common_params/mapSource" , iSource);
-  if(iSource == 0)
-    m_MapType = PlannerHNS::MAP_AUTOWARE;
-  else if (iSource == 1)
-    m_MapType = PlannerHNS::MAP_FOLDER;
-  else if(iSource == 2)
-    m_MapType = PlannerHNS::MAP_KML_FILE;
-
-  _nh.getParam("/op_common_params/mapFileName" , m_MapPath);
-
-  _nh.getParam("/op_behavior_selector/evidence_tust_number", m_PlanningParams.nReliableCount);
-
-  //std::cout << "nReliableCount: " << m_PlanningParams.nReliableCount << std::endl;
-  
-  _nh.param("/op_behavior_selector/sprintSpeed", m_sprintSpeed, 13.5);
-  _nh.param("/op_behavior_selector/obstacleWaitingTimeinIntersection", m_obstacleWaitingTimeinIntersection, 1.0);
-
-  m_BehaviorGenerator.Init(controlParams, m_PlanningParams, m_CarInfo, m_sprintSpeed);  
-
-  m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior = PlannerHNS::INITIAL_STATE;
-  
-  m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection;
-}
+void BehaviorGen::UpdatePlanningParams(ros::NodeHandle &_nh) {
+    _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
+    if (m_PlanningParams.enableSwerving)
+        m_PlanningParams.enableFollowing = true;
+    else
+        _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
 
-void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){
-  double distance = msg.data;
-  if(distance < m_distanceToPedestrianThreshold){
-    m_PlanningParams.pedestrianAppearence = true;
-  }
-  else
-  {
-    m_PlanningParams.pedestrianAppearence = false;
-  }
-  m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence);
-  // m_BehaviorGenerator.printPedestrianAppearence();
-}
+    _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
+    _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
 
-void BehaviorGen::callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg){
-  m_BehaviorGenerator.m_isInsideIntersection = msg.isIntersection;
-  m_BehaviorGenerator.m_closestIntersectionDistance = msg.intersectionDistance;
-  m_BehaviorGenerator.m_riskyLeft = msg.riskyLeftTurn;
-  m_BehaviorGenerator.m_riskyRight = msg.riskyRightTurn;
-}
+    _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
+    _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
+    _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
 
-void BehaviorGen::callbackSprintSwitch(const std_msgs::Bool& msg){
-  m_sprintSwitch = msg.data;
-}
+    _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
 
-void BehaviorGen::callbackGetTwistRaw(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  m_Twist_raw = *msg;
-}
+    _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
+    if (m_PlanningParams.enableSwerving)
+        _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
+    else
+        m_PlanningParams.rollOutNumber = 0;
 
-void BehaviorGen::callbackGetTwistCMD(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  m_Twist_cmd = *msg;
-}
+    _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
+    _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
+    _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
+    _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
+    _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
 
-void BehaviorGen::callbackGetCommandCMD(const autoware_msgs::ControlCommandConstPtr& msg)
-{
-  m_Ctrl_cmd = *msg;
-}
+    _nh.getParam("/op_trajectory_evaluator/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
+    _nh.getParam("/op_trajectory_evaluator/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
 
-void BehaviorGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
-{
-  m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation));
-  bNewCurrentPos = true;
-}
+    _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
 
-void BehaviorGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  m_VehicleStatus.speed = msg->twist.linear.x;
-  m_CurrentPos.v = m_VehicleStatus.speed;
-  if(fabs(msg->twist.linear.x) > 0.25)
-    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x);
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
-
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-}
+    _nh.getParam("/op_common_params/width", m_CarInfo.width);
+    _nh.getParam("/op_common_params/length", m_CarInfo.length);
+    _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
+    _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
+    _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
+    _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
+    _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
+    m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
+    m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
 
-void BehaviorGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
-{
-  m_VehicleStatus.speed = msg->speed/3.6;
-  m_CurrentPos.v = m_VehicleStatus.speed;
-  m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value;
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
-}
+    _nh.getParam("/op_common_params/stopLineMargin", m_PlanningParams.stopLineMargin);
+    _nh.getParam("/op_common_params/stopLineDetectionDistance", m_PlanningParams.stopLineDetectionDistance);
 
-void BehaviorGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
-{
-  m_VehicleStatus.speed = msg->twist.twist.linear.x;
-  m_CurrentPos.v = m_VehicleStatus.speed ;
-  if(msg->twist.twist.linear.x != 0)
-    m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
+    PlannerHNS::ControllerParams controlParams;
+    controlParams.Steering_Gain = PlannerHNS::PID_CONST(0.07, 0.02, 0.01);
+    controlParams.Velocity_Gain = PlannerHNS::PID_CONST(0.1, 0.005, 0.1);
+    nh.getParam("/op_common_params/steeringDelay", controlParams.SteeringDelay);
+    nh.getParam("/op_common_params/minPursuiteDistance", controlParams.minPursuiteDistance);
+    nh.getParam("/op_common_params/additionalBrakingDistance", m_PlanningParams.additionalBrakingDistance);
+    nh.getParam("/op_common_params/giveUpDistance", m_PlanningParams.giveUpDistance);
+    nh.getParam("/op_common_params/enableSlowDownOnCurve", m_PlanningParams.enableSlowDownOnCurve);
+    nh.getParam("/op_common_params/curveVelocityRatio", m_PlanningParams.curveVelocityRatio);
+
+    int iSource = 0;
+    _nh.getParam("/op_common_params/mapSource", iSource);
+    if (iSource == 0)
+        m_MapType = PlannerHNS::MAP_AUTOWARE;
+    else if (iSource == 1)
+        m_MapType = PlannerHNS::MAP_FOLDER;
+    else if (iSource == 2)
+        m_MapType = PlannerHNS::MAP_KML_FILE;
+
+    _nh.getParam("/op_common_params/mapFileName", m_MapPath);
+
+    _nh.getParam("/op_behavior_selector/evidence_tust_number", m_PlanningParams.nReliableCount);
+
+    // std::cout << "nReliableCount: " << m_PlanningParams.nReliableCount << std::endl;
+
+    _nh.param("/op_behavior_selector/sprintSpeed", m_sprintSpeed, 13.5);
+    _nh.param("/op_behavior_selector/obstacleWaitingTimeinIntersection", m_obstacleWaitingTimeinIntersection, 1.0);
+
+    m_BehaviorGenerator.Init(controlParams, m_PlanningParams, m_CarInfo, m_sprintSpeed);
+
+    m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior = PlannerHNS::INITIAL_STATE;
+
+    m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection;
 }
 
-void BehaviorGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
-{
-  if(msg->lanes.size() > 0 && bMap)
-  {
+void BehaviorGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr &msg) {
+    if (msg->lanes.size() > 0 && bMap) {
 
-    bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
+        bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
 
-    m_GlobalPaths.clear();
+        m_GlobalPaths.clear();
 
-    for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
-    {
-      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
-      PlannerHNS::Lane* pPrevValid = 0;
-      for(unsigned int j = 0 ; j < m_temp_path.size(); j++)
-      {
-        PlannerHNS::Lane* pLane = 0;
-        pLane = PlannerHNS::MappingHelpers::GetLaneById(m_temp_path.at(j).laneId, m_Map);
-        if(!pLane)
-        {
-          pLane = PlannerHNS::MappingHelpers::GetClosestLaneFromMapDirectionBased(m_temp_path.at(j), m_Map, 1);
-
-          if(!pLane && !pPrevValid)
-          {
-            ROS_ERROR("Map inconsistency between Global Path and Local Planer Map, Can't identify current lane.");
-            return;
-          }
-
-          if(!pLane)
-            m_temp_path.at(j).pLane = pPrevValid;
-          else
-          {
-            m_temp_path.at(j).pLane = pLane;
-            pPrevValid = pLane ;
-          }
-
-          m_temp_path.at(j).laneId = m_temp_path.at(j).pLane->id;
-        }
-        else
-          m_temp_path.at(j).pLane = pLane;
+        for (unsigned int i = 0; i < msg->lanes.size(); i++) {
+            PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
+            PlannerHNS::Lane *pPrevValid = 0;
+            for (unsigned int j = 0; j < m_temp_path.size(); j++) {
+                PlannerHNS::Lane *pLane = 0;
+                pLane = PlannerHNS::MappingHelpers::GetLaneById(m_temp_path.at(j).laneId, m_Map);
+                if (!pLane) {
+                    pLane = PlannerHNS::MappingHelpers::GetClosestLaneFromMapDirectionBased(m_temp_path.at(j), m_Map, 1);
 
+                    if (!pLane && !pPrevValid) {
+                        ROS_ERROR("Map inconsistency between Global Path and Local Planer Map, Can't identify current lane.");
+                        return;
+                    }
 
-        //std::cout << "StopLineInGlobalPath: " << m_temp_path.at(j).stopLineID << std::endl;
-      }
+                    if (!pLane)
+                        m_temp_path.at(j).pLane = pPrevValid;
+                    else {
+                        m_temp_path.at(j).pLane = pLane;
+                        pPrevValid = pLane;
+                    }
 
-      PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
-      m_GlobalPaths.push_back(m_temp_path);
+                    m_temp_path.at(j).laneId = m_temp_path.at(j).pLane->id;
+                } else
+                    m_temp_path.at(j).pLane = pLane;
 
-      if(bOldGlobalPath)
-      {
-        bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
-      }
-    }
+                // std::cout << "StopLineInGlobalPath: " << m_temp_path.at(j).stopLineID << std::endl;
+            }
 
-    if(!bOldGlobalPath)
-    {
-      bWayGlobalPath = true;
-      bWayGlobalPathLogs = true;
-      for(unsigned int i = 0; i < m_GlobalPaths.size(); i++)
-      {
-        PlannerHNS::PlanningHelpers::FixPathDensity(m_GlobalPaths.at(i), m_PlanningParams.pathDensity);
-        PlannerHNS::PlanningHelpers::SmoothPath(m_GlobalPaths.at(i), 0.35, 0.4, 0.05);
+            PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
+            m_GlobalPaths.push_back(m_temp_path);
 
-        PlannerHNS::PlanningHelpers::GenerateRecommendedSpeed(m_GlobalPaths.at(i), m_CarInfo.max_speed_forward, m_PlanningParams.speedProfileFactor);
-        m_GlobalPaths.at(i).at(m_GlobalPaths.at(i).size()-1).v = 0;
-      }
+            if (bOldGlobalPath) {
+                bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
+            }
+        }
 
-      std::cout << "Received New Global Path Selector! " << std::endl;
-    }
-    else
-    {
-      m_GlobalPaths.clear();
+        if (!bOldGlobalPath) {
+            bWayGlobalPath = true;
+            bWayGlobalPathLogs = true;
+            for (unsigned int i = 0; i < m_GlobalPaths.size(); i++) {
+                PlannerHNS::PlanningHelpers::FixPathDensity(m_GlobalPaths.at(i), m_PlanningParams.pathDensity);
+                PlannerHNS::PlanningHelpers::SmoothPath(m_GlobalPaths.at(i), 0.35, 0.4, 0.05);
+                PlannerHNS::PlanningHelpers::GenerateRecommendedSpeed(m_GlobalPaths.at(i), m_CarInfo.max_speed_forward,
+                                                                      m_PlanningParams.speedProfileFactor);
+                m_GlobalPaths.at(i).at(m_GlobalPaths.at(i).size() - 1).v = 0;
+            }
+
+            std::cout << "Received New Global Path Selector! " << std::endl;
+        } else {
+            m_GlobalPaths.clear();
+        }
     }
-  }
 }
 
-void BehaviorGen::callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg)
-{
-  if(m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior == PlannerHNS::INTERSECTION_STATE){
+void BehaviorGen::callbackPlanningInfo(const rubis_msgs::PlanningInfoConstPtr &msg) {
+    rubis::start_task_profiling();
+    rubis::instance_ = msg->instance;
+    rubis::lidar_instance_ = msg->lidar_instance;
+    rubis::vision_instance_ = msg->vision_instance;
+    static double prev_x = 0.0, prev_y = 0.0, prev_speed = 0.0;
+
+    // distance to pedestrian
+    distance_to_pdestrian_ = msg->distance_to_pedestrian.data;
+    if (distance_to_pdestrian_ < m_distanceToPedestrianThreshold) {
+        m_PlanningParams.pedestrianAppearence = true;
+    }
+    m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence);
+
+    // intersection condition
+    m_BehaviorGenerator.m_isInsideIntersection = msg->intersection_condition.isIntersection;
+    m_BehaviorGenerator.m_closestIntersectionDistance = msg->intersection_condition.intersectionDistance;
+    m_BehaviorGenerator.m_riskyLeft = msg->intersection_condition.riskyLeftTurn;
+    m_BehaviorGenerator.m_riskyRight = msg->intersection_condition.riskyRightTurn;
+
+    // sprint switch
+    m_sprintSwitch = msg->sprint_switch.data;
+
+    // current velocity
+    if (prev_speed != msg->twist.twist.linear.x) {
+        m_VehicleStatus.speed = msg->twist.twist.linear.x;
+        m_CurrentPos.v = m_VehicleStatus.speed;
+        if (fabs(msg->twist.twist.linear.x) > 0.25)
+            m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z / msg->twist.twist.linear.x);
+        UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+        bVehicleStatus = true;
+        prev_speed = msg->twist.twist.linear.x;
+    }
+
+    // current pose
+    if (prev_x != msg->pose.pose.position.x || prev_y != msg->pose.pose.position.y) {
+        m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z,
+                                            tf::getYaw(msg->pose.pose.orientation));
+        bNewCurrentPos = true;
+        prev_x = msg->pose.pose.position.x;
+        prev_y = msg->pose.pose.position.y;
+    }
+
+    // trajectory cost
+    if (m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior == PlannerHNS::INTERSECTION_STATE) {
+        bBestCost = true;
+        m_TrajectoryBestCost.closest_obj_distance = msg->trajectory_cost.closest_object_distance;
+        m_TrajectoryBestCost.closest_obj_velocity = msg->trajectory_cost.closest_object_velocity;
+        return;
+    }
     bBestCost = true;
-    m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance;
-    m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity;
-    return;
-  }
-  bBestCost = true;
-  m_TrajectoryBestCost.bBlocked = msg->is_blocked;
-  m_TrajectoryBestCost.index = msg->lane_index;
-  m_TrajectoryBestCost.cost = msg->cost;
-  m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance;
-  m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity;
-}
+    m_TrajectoryBestCost.bBlocked = msg->trajectory_cost.is_blocked;
+    m_TrajectoryBestCost.index = msg->trajectory_cost.lane_index;
+    m_TrajectoryBestCost.cost = msg->trajectory_cost.cost;
+    m_TrajectoryBestCost.closest_obj_distance = msg->trajectory_cost.closest_object_distance;
+    m_TrajectoryBestCost.closest_obj_velocity = msg->trajectory_cost.closest_object_velocity;
+
+    // lane array
+    if (msg->lane_array.lanes.size() > 0) {
+        m_RollOuts.clear();
+        int globalPathId_roll_outs = -1;
+
+        for (unsigned int i = 0; i < msg->lane_array.lanes.size(); i++) {
+            std::vector<PlannerHNS::WayPoint> path;
+            PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path);
+            m_RollOuts.push_back(path);
+
+            if (path.size() > 0)
+                globalPathId_roll_outs = path.at(0).gid;
+        }
 
-void BehaviorGen::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
-{
-  if(msg->lanes.size() > 0)
-  {
-    m_RollOuts.clear();
-    int globalPathId_roll_outs = -1;
+        if (bWayGlobalPath && m_GlobalPaths.size() > 0) {
+            if (m_GlobalPaths.at(0).size() > 0) {
+                int globalPathId = m_GlobalPaths.at(0).at(0).gid;
+                std::cout << "Before Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs
+                          << std::endl;
+
+                if (globalPathId_roll_outs == globalPathId) {
+                    bWayGlobalPath = false;
+                    m_GlobalPathsToUse = m_GlobalPaths;
+                    m_BehaviorGenerator.SetNewGlobalPath(m_GlobalPathsToUse);
+                    std::cout << "Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs
+                              << std::endl;
+                }
+            }
+        }
 
-    for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
-    {
-      std::vector<PlannerHNS::WayPoint> path;
-      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path);
-      m_RollOuts.push_back(path);
+        m_BehaviorGenerator.m_RollOuts = m_RollOuts;
+        bRollOuts = true;
+    }
 
-      if(path.size() > 0)
-        globalPathId_roll_outs = path.at(0).gid;
+    // Main Loop
+    // Check Pedestrian is Appeared
+    double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer);
+    UtilityHNS::UtilityH::GetTickCount(planningTimer);
+
+    if (m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) {
+        bMap = true;
+        PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map);
+    } else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) {
+        bMap = true;
+        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true);
+
+    } else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) {
+        std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;
+        ;
+
+        if (m_MapRaw.GetVersion() == 2) {
+            PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(
+                m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,
+                m_MapRaw.pAreas->m_data_list, m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list,
+                m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+                m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes,
+                m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false);
+
+            try {
+                // Add Traffic Signal Info from yaml file
+                XmlRpc::XmlRpcValue traffic_light_list;
+                nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list);
+
+                // Add Stop Line Info from yaml file
+                XmlRpc::XmlRpcValue stop_line_list;
+                nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list);
+
+                // Add Crossing Info from yaml file
+                // XmlRpc::XmlRpcValue intersection_list;
+                // nh.getParam("/op_behavior_selector/intersection_list", intersection_list);
+
+                PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list);
+            } catch (XmlRpc::XmlRpcException &e) {
+                ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str());
+                exit(1);
+            }
+
+            m_BehaviorGenerator.m_Map = m_Map;
+
+            if (m_Map.roadSegments.size() > 0) {
+                bMap = true;
+                std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl;
+            }
+        } else if (m_MapRaw.GetVersion() == 1) {
+            PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(
+                m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,
+                m_MapRaw.pAreas->m_data_list, m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list,
+                m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+                m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true,
+                m_PlanningParams.enableLaneChange, false);
+
+            if (m_Map.roadSegments.size() > 0) {
+                bMap = true;
+                std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl;
+            }
+        }
     }
 
-    if(bWayGlobalPath && m_GlobalPaths.size() > 0)
-    {
-      if(m_GlobalPaths.at(0).size() > 0)
-      {
-        int globalPathId = m_GlobalPaths.at(0).at(0).gid;
-        std::cout << "Before Synchronization At Behavior Selector: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
-
-        if(globalPathId_roll_outs == globalPathId)
-        {
-          bWayGlobalPath = false;
-          m_GlobalPathsToUse = m_GlobalPaths;
-          m_BehaviorGenerator.SetNewGlobalPath(m_GlobalPathsToUse);
-          std::cout << "Synchronization At Behavior Selector: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
+    if (bNewCurrentPos && m_GlobalPaths.size() > 0) {
+        if (bNewLightSignal) {
+            m_PrevTrafficLight = m_CurrTrafficLight;
+            bNewLightSignal = false;
         }
-      }
+
+        if (bNewLightStatus) {
+            bNewLightStatus = false;
+            for (unsigned int itls = 0; itls < m_PrevTrafficLight.size(); itls++)
+                m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus;
+        }
+
+        m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch;
+        m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0);
+
+        // std_msgs::Int32 curr_state_msg;
+        // curr_state_msg.data = m_CurrentBehavior.state;
+        // pub_currentState.publish(curr_state_msg);
+
+        CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint);
+        m_BehaviorGenerator.m_turnAngle = m_turnAngle;
+
+        std_msgs::Float64 turn_angle_msg;
+        turn_angle_msg.data = m_turnAngle;
+        pub_turnAngle.publish(turn_angle_msg);
+
+        // emergency_stop_msg.data = false;
+        // if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop!
+        //   emergency_stop_msg.data = true;
+        // pub_EmergencyStop.publish(emergency_stop_msg);
+        if (m_PlanningParams.pedestrianAppearence) {
+            emergency_stop_msg.data = true;
+            pub_EmergencyStop.publish(emergency_stop_msg);
+        }
+
+        SendLocalPlanningTopics(msg);
+        VisualizeLocalPlanner();
+        LogLocalPlanningInfo(dt);
+
+        // Publish turn_marker
+        visualization_msgs::MarkerArray turn_marker;
+        visualization_msgs::Marker marker;
+        marker.header.frame_id = "map";
+        marker.type = 2;
+        marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x;
+        marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y;
+        marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z;
+        marker.scale.x = 3;
+        marker.scale.y = 3;
+        marker.scale.z = 3;
+        marker.color.r = 0.0f;
+        marker.color.g = 1.0f;
+        marker.color.b = 0.0f;
+        marker.color.a = 1.0f;
+        marker.header.stamp = ros::Time::now();
+        marker.header.frame_id = "map";
+        turn_marker.markers.push_back(marker);
+
+        pub_turnMarker.publish(turn_marker);
+    } else {
+        sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this);
     }
 
-    m_BehaviorGenerator.m_RollOuts = m_RollOuts;
-    bRollOuts = true;
-  }
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
+/*
 void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg)
 {
   bNewLightSignal = true;
@@ -450,489 +500,315 @@ void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTr
 
   m_CurrTrafficLight = simulatedLights;
 }
-
-void BehaviorGen::VisualizeLocalPlanner()
-{
-  visualization_msgs::Marker behavior_rviz;
-  int iDirection = 0;
-  if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory > m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
-    iDirection = 1;
-  else if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory < m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
-    iDirection = -1;
-  PlannerHNS::ROSHelpers::VisualizeBehaviorState(m_CurrentPos, m_CurrentBehavior, !m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bTrafficIsRed , iDirection, behavior_rviz, "beh_state");
-  //pub_BehaviorStateRviz.publish(behavior_rviz);
-
-  visualization_msgs::MarkerArray markerArray;
-
-  //PlannerHNS::ROSHelpers::GetIndicatorArrows(m_CurrentPos, m_CarInfo.width, m_CarInfo.length, m_CurrentBehavior.indicator, 0, markerArray);
-
-  markerArray.markers.push_back(behavior_rviz);
-
-  pub_BehaviorStateRviz.publish(markerArray);
-
-  //To Test Synchronization Problem
-//  visualization_msgs::MarkerArray selected_path;
-//  std::vector<std::vector<std::vector<PlannerHNS::WayPoint> > > paths;
-//  paths.push_back(std::vector<std::vector<PlannerHNS::WayPoint> >());
-//  paths.at(0).push_back(m_BehaviorGenerator.m_Path);
-//  paths.push_back(m_GlobalPathsToUse);
-//  paths.push_back(m_RollOuts);
-//  PlannerHNS::ROSHelpers::TrajectoriesToMarkers(paths, selected_path);
-//  pub_SelectedPathRviz.publish(selected_path);
-}
-
-void BehaviorGen::SendLocalPlanningTopics()
-{
-  //Send Behavior State
-  geometry_msgs::Twist t;
-  geometry_msgs::TwistStamped behavior;
-  t.linear.x = m_CurrentBehavior.bNewPlan;
-  t.linear.y = m_CurrentBehavior.followDistance;
-  t.linear.z = m_CurrentBehavior.followVelocity;
-  t.angular.x = (int)m_CurrentBehavior.indicator;
-  t.angular.y = (int)m_CurrentBehavior.state;
-  t.angular.z = m_CurrentBehavior.iTrajectory;
-  behavior.twist = t;
-  behavior.header.stamp = ros::Time::now();
-  pub_BehaviorState.publish(behavior);
-
-  //Send Ego Vehicle Simulation Pose Data
-  geometry_msgs::PoseArray sim_data;
-  geometry_msgs::Pose p_id, p_pose, p_box;
-
-  sim_data.header.frame_id = "map";
-  sim_data.header.stamp = ros::Time();
-  p_id.position.x = 0;
-  p_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a));
-
-  PlannerHNS::WayPoint pose_center = PlannerHNS::PlanningHelpers::GetRealCenter(m_BehaviorGenerator.state, m_CarInfo.wheel_base);
-
-  p_pose.position.x = pose_center.pos.x;
-  p_pose.position.y = pose_center.pos.y;
-  p_pose.position.z = pose_center.pos.z;
-  p_box.position.x = m_BehaviorGenerator.m_CarInfo.width;
-  p_box.position.y = m_BehaviorGenerator.m_CarInfo.length;
-  p_box.position.z = 2.2;
-  sim_data.poses.push_back(p_id);
-  sim_data.poses.push_back(p_pose);
-  sim_data.poses.push_back(p_box);
-  pub_SimuBoxPose.publish(sim_data);
-
-  //Send Trajectory Data to path following nodes
-  std_msgs::Int32 closest_waypoint;
-  PlannerHNS::RelativeInfo info;
-  PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info);
-  PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack);
-  //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl;
-
-  closest_waypoint.data = 1;
-  pub_ClosestIndex.publish(closest_waypoint);
-  pub_LocalBasePath.publish(m_CurrentTrajectoryToSend);
-  pub_LocalPath.publish(m_CurrentTrajectoryToSend);
-  rubis::sched::task_state_ = TASK_STATE_DONE;
-}
-
-void BehaviorGen::LogLocalPlanningInfo(double dt)
-{
-  timespec log_t;
-  UtilityHNS::UtilityH::GetTickCount(log_t);
-  std::ostringstream dataLine;
-  dataLine << UtilityHNS::UtilityH::GetLongTime(log_t) <<"," << dt << "," << m_CurrentBehavior.state << ","<< PlannerHNS::ROSHelpers::GetBehaviorNameFromCode(m_CurrentBehavior.state) << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->m_pParams->rollOutNumber << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bFullyBlock << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentStopSignID << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentTrafficLightID << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->minStoppingDistance << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->distanceToNext << "," <<
-      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->velocityOfNext << "," <<
-      m_CurrentBehavior.maxVelocity << "," <<
-      m_Twist_raw.twist.linear.x << "," <<
-      m_Twist_cmd.twist.linear.x << "," <<
-      m_Ctrl_cmd.linear_velocity << "," <<
-      m_VehicleStatus.speed << "," <<
-      m_VehicleStatus.steer << "," <<
-      m_BehaviorGenerator.state.pos.x << "," << m_BehaviorGenerator.state.pos.y << "," << m_BehaviorGenerator.state.pos.z << "," << UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)+M_PI << ",";
-  m_LogData.push_back(dataLine.str());
-
-
-  if(bWayGlobalPathLogs)
-  {
-    for(unsigned int i=0; i < m_GlobalPaths.size(); i++)
-    {
-      std::ostringstream str_out;
-      str_out << UtilityHNS::UtilityH::GetHomeDirectory();
-      str_out << UtilityHNS::DataRW::LoggingMainfolderName;
-      str_out << UtilityHNS::DataRW::PathLogFolderName;
-      str_out << "Global_Vel";
-      str_out << i;
-      str_out << "_";
-      PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), m_GlobalPaths.at(i));
+*/
+
+void BehaviorGen::VisualizeLocalPlanner() {
+    visualization_msgs::Marker behavior_rviz;
+    int iDirection = 0;
+    if (m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory >
+        m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+        iDirection = 1;
+    else if (m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory <
+             m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+        iDirection = -1;
+    PlannerHNS::ROSHelpers::VisualizeBehaviorState(m_CurrentPos, m_CurrentBehavior,
+                                                   !m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bTrafficIsRed, iDirection,
+                                                   behavior_rviz, "beh_state");
+
+    visualization_msgs::MarkerArray markerArray;
+    behavior_rviz.scale.x = 5.0;
+    behavior_rviz.scale.y = 5.0;
+    behavior_rviz.scale.z = 5.0;
+
+    markerArray.markers.push_back(behavior_rviz);
+
+    pub_BehaviorStateRviz.publish(markerArray);
+
+    // To Test Synchronization Problem
+    //  visualization_msgs::MarkerArray selected_path;
+    //  std::vector<std::vector<std::vector<PlannerHNS::WayPoint> > > paths;
+    //  paths.push_back(std::vector<std::vector<PlannerHNS::WayPoint> >());
+    //  paths.at(0).push_back(m_BehaviorGenerator.m_Path);
+    //  paths.push_back(m_GlobalPathsToUse);
+    //  paths.push_back(m_RollOuts);
+    //  PlannerHNS::ROSHelpers::TrajectoriesToMarkers(paths, selected_path);
+    //  pub_SelectedPathRviz.publish(selected_path);
+}
+
+void BehaviorGen::SendLocalPlanningTopics(const rubis_msgs::PlanningInfoConstPtr &msg) {
+    // Send Behavior State
+    //  geometry_msgs::Twist t;
+    //  geometry_msgs::TwistStamped behavior;
+    //  t.linear.x = m_CurrentBehavior.bNewPlan;
+    //  t.linear.y = m_CurrentBehavior.followDistance;
+    //  t.linear.z = m_CurrentBehavior.followVelocity;
+    //  t.angular.x = (int)m_CurrentBehavior.indicator;
+    //  t.angular.y = (int)m_CurrentBehavior.state;
+    //  t.angular.z = m_CurrentBehavior.iTrajectory;
+    //  behavior.twist = t;
+    //  behavior.header.stamp = msg->header.stamp;
+    //  pub_BehaviorState.publish(behavior);
+
+    // Send Ego Vehicle Simulation Pose Data
+    geometry_msgs::PoseArray sim_data;
+    geometry_msgs::Pose p_id, p_pose, p_box;
+
+    sim_data.header.frame_id = "map";
+    sim_data.header.stamp = msg->header.stamp;
+    p_id.position.x = 0;
+    p_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a));
+
+    PlannerHNS::WayPoint pose_center = PlannerHNS::PlanningHelpers::GetRealCenter(m_BehaviorGenerator.state, m_CarInfo.wheel_base);
+
+    p_pose.position.x = pose_center.pos.x;
+    p_pose.position.y = pose_center.pos.y;
+    p_pose.position.z = pose_center.pos.z;
+    p_box.position.x = m_BehaviorGenerator.m_CarInfo.width;
+    p_box.position.y = m_BehaviorGenerator.m_CarInfo.length;
+    p_box.position.z = 2.2;
+    sim_data.poses.push_back(p_id);
+    sim_data.poses.push_back(p_pose);
+    sim_data.poses.push_back(p_box);
+    pub_SimuBoxPose.publish(sim_data);
+
+    // Send Trajectory Data to path following nodes
+    std_msgs::Int32 closest_waypoint;
+    PlannerHNS::RelativeInfo info;
+    PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info);
+    PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack);
+    // std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl;
+
+    closest_waypoint.data = 1;
+    pub_ClosestIndex.publish(closest_waypoint);
+    // m_CurrentTrajectoryToSend.header.stamp = msg->header.stamp;
+    // pub_LocalBasePath.publish(m_CurrentTrajectoryToSend);
+
+    rubis_msgs::LaneWithPoseTwist final_waypoints_with_pose_twist_msg;
+    final_waypoints_with_pose_twist_msg.instance = rubis::instance_;
+    final_waypoints_with_pose_twist_msg.lidar_instance = rubis::lidar_instance_;
+    final_waypoints_with_pose_twist_msg.vision_instance = rubis::vision_instance_;
+    final_waypoints_with_pose_twist_msg.lane = m_CurrentTrajectoryToSend;
+    final_waypoints_with_pose_twist_msg.pose = msg->pose;
+    final_waypoints_with_pose_twist_msg.twist = msg->twist;
+
+    final_waypoints_with_pose_twist_msg.header = msg->header;
+    pub_LocalPathWithPosePub.publish(final_waypoints_with_pose_twist_msg);
+
+    // m_CurrentTrajectoryToSend.header.stamp = msg->header.stamp;
+    // pub_LocalPath.publish(m_CurrentTrajectoryToSend);
+}
+
+void BehaviorGen::LogLocalPlanningInfo(double dt) {
+    timespec log_t;
+    UtilityHNS::UtilityH::GetTickCount(log_t);
+    std::ostringstream dataLine;
+    dataLine << UtilityHNS::UtilityH::GetLongTime(log_t) << "," << dt << "," << m_CurrentBehavior.state << ","
+             << PlannerHNS::ROSHelpers::GetBehaviorNameFromCode(m_CurrentBehavior.state) << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->m_pParams->rollOutNumber << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bFullyBlock << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentStopSignID << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentTrafficLightID << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->minStoppingDistance << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->distanceToNext << ","
+             << m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->velocityOfNext << "," << m_CurrentBehavior.maxVelocity << ","
+             << m_VehicleStatus.speed << "," << m_VehicleStatus.steer << "," << m_BehaviorGenerator.state.pos.x << ","
+             << m_BehaviorGenerator.state.pos.y << "," << m_BehaviorGenerator.state.pos.z << ","
+             << UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a) + M_PI << ",";
+    m_LogData.push_back(dataLine.str());
+
+    if (bWayGlobalPathLogs) {
+        for (unsigned int i = 0; i < m_GlobalPaths.size(); i++) {
+            std::ostringstream str_out;
+            str_out << UtilityHNS::UtilityH::GetHomeDirectory();
+            str_out << UtilityHNS::DataRW::LoggingMainfolderName;
+            str_out << UtilityHNS::DataRW::PathLogFolderName;
+            str_out << "Global_Vel";
+            str_out << i;
+            str_out << "_";
+            PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), m_GlobalPaths.at(i));
+        }
+        bWayGlobalPathLogs = false;
     }
-    bWayGlobalPathLogs = false;
-  }
-}
-
-void BehaviorGen::CalculateTurnAngle(PlannerHNS::WayPoint turn_point){
-  geometry_msgs::PoseStamped turn_pose;
-
-  if(GetBaseMapTF()){
-    // std::cout<<"BEFORE:"<<turn_point.pos.x<<" "<<turn_point.pos.y<<" "<<turn_point.rot.x<<" "<<turn_point.rot.y<<" "<<turn_point.rot.z<<std::endl;
-    turn_pose.pose.position.x = turn_point.pos.x;
-    turn_pose.pose.position.y = turn_point.pos.y;
-    turn_pose.pose.position.z = turn_point.pos.z;
-    turn_pose.pose.orientation.x = turn_point.rot.x;
-    turn_pose.pose.orientation.y = turn_point.rot.y;
-    turn_pose.pose.orientation.z = turn_point.rot.z;
-    turn_pose.pose.orientation.w = turn_point.rot.w;
-    TransformPose(turn_pose, turn_pose, m_map_base_transform);
-    // std::cout<<"AFTER:"<<turn_pose.pose.position.x<<" "<<turn_pose.pose.position.y<<" "<<turn_pose.pose.orientation.x<<" "<<turn_pose.pose.orientation.y<<" "<<turn_pose.pose.orientation.z<<std::endl;
-
-    double hypot_length = hypot(turn_pose.pose.position.x, turn_pose.pose.position.y);
-
-    if(hypot_length <= 0)
-      m_turnAngle = 0;
-    else
-      m_turnAngle = acos(abs(turn_pose.pose.position.x)/hypot_length)*180.0/PI;
-    if(turn_pose.pose.position.y < 0)
-      m_turnAngle = -1 * m_turnAngle;
-  }
-
-  return;
 }
 
-void BehaviorGen::MainLoop()
-{
-  ros::NodeHandle private_nh("~");
-
-  timespec planningTimer;
-  UtilityHNS::UtilityH::GetTickCount(planningTimer);
-  std_msgs::Bool emergency_stop_msg;
-
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
-
-  m_BehaviorGenerator.m_turnThreshold = m_turnThreshold;
-
-m_sprintSwitch = false;
-
-  private_nh.param<int>("/op_behavior_selector/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/op_behavior_selector/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/op_behavior_selector/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv");
-  private_nh.param<int>("/op_behavior_selector/rate", rate, 10);
-  private_nh.param("/op_behavior_selector/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/op_behavior_selector/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/op_behavior_selector/task_relative_deadline", task_relative_deadline, (double)10);
-
-  /* For Task scheduling */
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-  ros::Rate loop_rate(rate);  
-  if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25);
+void BehaviorGen::CalculateTurnAngle(PlannerHNS::WayPoint turn_point) {
+    geometry_msgs::PoseStamped turn_pose;
 
-  struct timespec start_time, end_time;
+    if (GetBaseMapTF()) {
+        // std::cout<<"BEFORE:"<<turn_point.pos.x<<" "<<turn_point.pos.y<<" "<<turn_point.rot.x<<" "<<turn_point.rot.y<<"
+        // "<<turn_point.rot.z<<std::endl;
+        turn_pose.pose.position.x = turn_point.pos.x;
+        turn_pose.pose.position.y = turn_point.pos.y;
+        turn_pose.pose.position.z = turn_point.pos.z;
+        turn_pose.pose.orientation.x = turn_point.rot.x;
+        turn_pose.pose.orientation.y = turn_point.rot.y;
+        turn_pose.pose.orientation.z = turn_point.rot.z;
+        turn_pose.pose.orientation.w = turn_point.rot.w;
+        TransformPose(turn_pose, turn_pose, m_map_base_transform);
+        // std::cout<<"AFTER:"<<turn_pose.pose.position.x<<" "<<turn_pose.pose.position.y<<" "<<turn_pose.pose.orientation.x<<"
+        // "<<turn_pose.pose.orientation.y<<" "<<turn_pose.pose.orientation.z<<std::endl;
 
-  m_sprintSwitch = false;
+        double hypot_length = hypot(turn_pose.pose.position.x, turn_pose.pose.position.y);
 
-  while (ros::ok())
-  {
-    if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){
-      if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-      rubis::sched::task_state_ = TASK_STATE_RUNNING;     
+        if (hypot_length <= 0)
+            m_turnAngle = 0;
+        else
+            m_turnAngle = acos(abs(turn_pose.pose.position.x) / hypot_length) * 180.0 / PI;
+        if (turn_pose.pose.position.y < 0)
+            m_turnAngle = -1 * m_turnAngle;
     }
 
-    ros::spinOnce();
+    return;
+}
 
-    // Check Pedestrian is Appeared
-    double dt  = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer);
-    UtilityHNS::UtilityH::GetTickCount(planningTimer);
+void BehaviorGen::MainLoop() {
+    ros::NodeHandle private_nh("~");
 
-    if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap)
-    {
-      bMap = true;
-      PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map);
-    }
-    else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap)
-    {
-      bMap = true;
-      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true);
+    m_BehaviorGenerator.m_turnThreshold = m_turnThreshold;
+    m_sprintSwitch = false;
 
-    }
-    else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap)
-    {
-      std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;;
+    // Scheduling & Profiling Setup
+    std::string node_name = ros::this_node::getName();
+    std::string task_response_time_filename;
+    private_nh.param<std::string>(node_name + "/task_response_time_filename", task_response_time_filename,
+                                  "~/Documents/profiling/response_time/op_behavior_selector.csv");
 
-      if(m_MapRaw.GetVersion()==2)
-      {
-        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
-            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
-            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
-            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
-            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,
-            m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false);
-
-        try{
-          // Add Traffic Signal Info from yaml file
-          XmlRpc::XmlRpcValue traffic_light_list;
-          nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list);
-
-          // Add Stop Line Info from yaml file
-          XmlRpc::XmlRpcValue stop_line_list;
-          nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list);
-
-          // Add Crossing Info from yaml file
-          // XmlRpc::XmlRpcValue intersection_list;
-          // nh.getParam("/op_behavior_selector/intersection_list", intersection_list);
-
-          PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list);
-        }
-        catch(XmlRpc::XmlRpcException& e){
-          ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str());
-          exit(1);
-        }
+    int rate;
+    private_nh.param<int>(node_name + "/rate", rate, 10);
 
-        m_BehaviorGenerator.m_Map = m_Map;
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time, deadline, period;
 
-        if(m_Map.roadSegments.size() > 0)
-        {
-          bMap = true;
-          std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl;
-        }
-      }
-      else if(m_MapRaw.GetVersion()==1)
-      {
-        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
-            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
-            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
-            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
-            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,  PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false);
-
-        if(m_Map.roadSegments.size() > 0)
-        {
-          bMap = true;
-          std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl;
-        }
-      }
-    }
+    private_nh.param(node_name + "/task_scheduling_configs/policy", policy, std::string("NONE"));
+    private_nh.param(node_name + "/task_scheduling_configs/priority", priority, 99);
+    private_nh.param(node_name + "/task_scheduling_configs/exec_time", exec_time, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/deadline", deadline, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/period", period, 0);
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);
+    rubis::init_task_scheduling(policy, attr);
 
-    if(bNewCurrentPos && m_GlobalPaths.size()>0)
-    {
-      if(bNewLightSignal)
-      {
-        m_PrevTrafficLight = m_CurrTrafficLight;
-        bNewLightSignal = false;
-      }
+    rubis::init_task_profiling(task_response_time_filename);
 
-      if(bNewLightStatus)
-      {
-        bNewLightStatus = false;
-        for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++)
-          m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus;
-      }
-      
-      m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch;
-      m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0);
-      std_msgs::Int32 curr_state_msg;
-      curr_state_msg.data = m_CurrentBehavior.state;
-
-      pub_currentState.publish(curr_state_msg);
-
-      CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint);
-      m_BehaviorGenerator.m_turnAngle = m_turnAngle;
-
-      std_msgs::Float64 turn_angle_msg;
-      turn_angle_msg.data = m_turnAngle;
-      pub_turnAngle.publish(turn_angle_msg);
-
-      emergency_stop_msg.data = false;
-      if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop!
-        emergency_stop_msg.data = true;
-      pub_EmergencyStop.publish(emergency_stop_msg);
-
-      SendLocalPlanningTopics();
-      VisualizeLocalPlanner();
-      LogLocalPlanningInfo(dt);
-
-      // Publish turn_marker
-      visualization_msgs::MarkerArray turn_marker;
-      visualization_msgs::Marker marker;
-      marker.header.frame_id = "map";
-      marker.type = 2;
-      marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x;
-      marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y;
-      marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z;
-      marker.scale.x = 3;
-      marker.scale.y = 3;
-      marker.scale.z = 3;
-      marker.color.r = 0.0f;
-      marker.color.g = 1.0f;
-      marker.color.b = 0.0f;
-      marker.color.a = 1.0f;
-      marker.header.stamp = ros::Time::now();
-      marker.header.frame_id = "map";
-      turn_marker.markers.push_back(marker);
-
-      pub_turnMarker.publish(turn_marker);
-    }
-    else
-      sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array",   1,    &BehaviorGen::callbackGetGlobalPlannerPath,   this);
+    ros::spin();
+}
 
-    if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
+bool BehaviorGen::GetBaseMapTF() {
 
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){
-      if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-      rubis::sched::task_state_ = TASK_STATE_READY;
+    try {
+        m_map_base_listener.waitForTransform("base_link", "map", ros::Time(0), ros::Duration(0.001));
+        m_map_base_listener.lookupTransform("base_link", "map", ros::Time(0), m_map_base_transform);
+        return true;
+    } catch (tf::TransformException &ex) {
+        return false;
     }
-
-    loop_rate.sleep();
-  }
 }
 
-bool BehaviorGen::GetBaseMapTF(){
-  
-  try{
-    m_map_base_listener.waitForTransform("base_link", "map", ros::Time(0), ros::Duration(0.001));
-    m_map_base_listener.lookupTransform("base_link", "map", ros::Time(0), m_map_base_transform);
-    return true;
-  }
-  catch(tf::TransformException& ex)
-  {
-    return false;
-  }
-  
-}
+void BehaviorGen::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped &out_pose,
+                                const tf::StampedTransform &in_transform) {
 
-void BehaviorGen::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform)
-{
+    tf::Vector3 in_pos(in_pose.pose.position.x, in_pose.pose.position.y, in_pose.pose.position.z);
+    tf::Quaternion in_quat(in_pose.pose.orientation.x, in_pose.pose.orientation.y, in_pose.pose.orientation.w, in_pose.pose.orientation.z);
 
-  tf::Vector3 in_pos(in_pose.pose.position.x,
-                     in_pose.pose.position.y,
-                     in_pose.pose.position.z);
-  tf::Quaternion in_quat(in_pose.pose.orientation.x,
-                         in_pose.pose.orientation.y,
-                         in_pose.pose.orientation.w,
-                         in_pose.pose.orientation.z);
-
-  tf::Vector3 in_pos_t = in_transform * in_pos;
-  tf::Quaternion in_quat_t = in_transform * in_quat;
-  
-  out_pose.header = in_pose.header;
-  out_pose.pose.position.x = in_pos_t.x();
-  out_pose.pose.position.y = in_pos_t.y();
-  out_pose.pose.position.z = in_pos_t.z();
-  out_pose.pose.orientation.x = in_quat_t.x();
-  out_pose.pose.orientation.y = in_quat_t.y();
-  out_pose.pose.orientation.z = in_quat_t.z();
-
-  return;
-}
+    tf::Vector3 in_pos_t = in_transform * in_pos;
+    tf::Quaternion in_quat_t = in_transform * in_quat;
 
-//Mapping Section
+    out_pose.header = in_pose.header;
+    out_pose.pose.position.x = in_pos_t.x();
+    out_pose.pose.position.y = in_pos_t.y();
+    out_pose.pose.position.z = in_pos_t.z();
+    out_pose.pose.orientation.x = in_quat_t.x();
+    out_pose.pose.orientation.y = in_quat_t.y();
+    out_pose.pose.orientation.z = in_quat_t.z();
 
-void BehaviorGen::callbackGetVMLanes(const vector_map_msgs::LaneArray& msg)
-{
-  std::cout << "Received Lanes" << endl;
-  if(m_MapRaw.pLanes == nullptr)
-    m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg);
+    return;
 }
 
-void BehaviorGen::callbackGetVMPoints(const vector_map_msgs::PointArray& msg)
-{
-  std::cout << "Received Points" << endl;
-  if(m_MapRaw.pPoints  == nullptr)
-    m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg);
+// Mapping Section
+
+void BehaviorGen::callbackGetVMLanes(const vector_map_msgs::LaneArray &msg) {
+    std::cout << "Received Lanes" << endl;
+    if (m_MapRaw.pLanes == nullptr)
+        m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray& msg)
-{
-  std::cout << "Received dtLanes" << endl;
-  if(m_MapRaw.pCenterLines == nullptr)
-    m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg);
+void BehaviorGen::callbackGetVMPoints(const vector_map_msgs::PointArray &msg) {
+    std::cout << "Received Points" << endl;
+    if (m_MapRaw.pPoints == nullptr)
+        m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray& msg)
-{
-  std::cout << "Received CrossRoads" << endl;
-  if(m_MapRaw.pIntersections == nullptr)
-    m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg);
+void BehaviorGen::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray &msg) {
+    std::cout << "Received dtLanes" << endl;
+    if (m_MapRaw.pCenterLines == nullptr)
+        m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMAreas(const vector_map_msgs::AreaArray& msg)
-{
-  std::cout << "Received Areas" << endl;
-  if(m_MapRaw.pAreas == nullptr)
-    m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg);
+void BehaviorGen::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray &msg) {
+    std::cout << "Received CrossRoads" << endl;
+    if (m_MapRaw.pIntersections == nullptr)
+        m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMLines(const vector_map_msgs::LineArray& msg)
-{
-  std::cout << "Received Lines" << endl;
-  if(m_MapRaw.pLines == nullptr)
-    m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg);
+void BehaviorGen::callbackGetVMAreas(const vector_map_msgs::AreaArray &msg) {
+    std::cout << "Received Areas" << endl;
+    if (m_MapRaw.pAreas == nullptr)
+        m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg)
-{
-  std::cout << "Received StopLines" << endl;
-  if(m_MapRaw.pStopLines == nullptr)
-    m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg);
+void BehaviorGen::callbackGetVMLines(const vector_map_msgs::LineArray &msg) {
+    std::cout << "Received Lines" << endl;
+    if (m_MapRaw.pLines == nullptr)
+        m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMSignal(const vector_map_msgs::SignalArray& msg)
-{
-  std::cout << "Received Signals" << endl;
-  if(m_MapRaw.pSignals  == nullptr)
-    m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg);
+void BehaviorGen::callbackGetVMStopLines(const vector_map_msgs::StopLineArray &msg) {
+    std::cout << "Received StopLines" << endl;
+    if (m_MapRaw.pStopLines == nullptr)
+        m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMVectors(const vector_map_msgs::VectorArray& msg)
-{
-  std::cout << "Received Vectors" << endl;
-  if(m_MapRaw.pVectors  == nullptr)
-    m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg);
+void BehaviorGen::callbackGetVMSignal(const vector_map_msgs::SignalArray &msg) {
+    std::cout << "Received Signals" << endl;
+    if (m_MapRaw.pSignals == nullptr)
+        m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMCurbs(const vector_map_msgs::CurbArray& msg)
-{
-  std::cout << "Received Curbs" << endl;
-  if(m_MapRaw.pCurbs == nullptr)
-    m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg);
+void BehaviorGen::callbackGetVMVectors(const vector_map_msgs::VectorArray &msg) {
+    std::cout << "Received Vectors" << endl;
+    if (m_MapRaw.pVectors == nullptr)
+        m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray& msg)
-{
-  std::cout << "Received Edges" << endl;
-  if(m_MapRaw.pRoadedges  == nullptr)
-    m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg);
+void BehaviorGen::callbackGetVMCurbs(const vector_map_msgs::CurbArray &msg) {
+    std::cout << "Received Curbs" << endl;
+    if (m_MapRaw.pCurbs == nullptr)
+        m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray& msg)
-{
-  std::cout << "Received Wayareas" << endl;
-  if(m_MapRaw.pWayAreas  == nullptr)
-    m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg);
+void BehaviorGen::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray &msg) {
+    std::cout << "Received Edges" << endl;
+    if (m_MapRaw.pRoadedges == nullptr)
+        m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray& msg)
-{
-  std::cout << "Received CrossWalks" << endl;
-  if(m_MapRaw.pCrossWalks == nullptr)
-    m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg);
+void BehaviorGen::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray &msg) {
+    std::cout << "Received Wayareas" << endl;
+    if (m_MapRaw.pWayAreas == nullptr)
+        m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg);
 }
 
-void BehaviorGen::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg)
-{
-  std::cout << "Received Nodes" << endl;
-  if(m_MapRaw.pNodes == nullptr)
-    m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg);
+void BehaviorGen::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray &msg) {
+    std::cout << "Received CrossWalks" << endl;
+    if (m_MapRaw.pCrossWalks == nullptr)
+        m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg);
 }
+
+void BehaviorGen::callbackGetVMNodes(const vector_map_msgs::NodeArray &msg) {
+    std::cout << "Received Nodes" << endl;
+    if (m_MapRaw.pNodes == nullptr)
+        m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg);
 }
+} // namespace BehaviorGeneratorNS
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp
new file mode 100644
index 00000000..97b0a4cd
--- /dev/null
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp
@@ -0,0 +1,905 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "op_behavior_selector_core.h"
+#include "op_ros_helpers/op_ROSHelpers.h"
+#include "op_planner/MappingHelpers.h"
+#include <rubis_lib/sched.hpp>
+
+namespace BehaviorGeneratorNS
+{
+
+BehaviorGen::BehaviorGen()
+{
+  sleep(2);
+  bNewCurrentPos = false;
+  bVehicleStatus = false;
+  bWayGlobalPath = false;
+  bWayGlobalPathLogs = false;
+  bNewLightStatus = false;
+  bNewLightSignal = false;
+  bBestCost = false;
+  bMap = false;
+  bRollOuts = false;
+  UtilityHNS::UtilityH::GetTickCount(planningTimer);
+
+  ros::NodeHandle _nh;
+  UpdatePlanningParams(_nh);
+
+  // RUBIS driving parameter
+  nh.getParam("/op_behavior_selector/distanceToPedestrianThreshold", m_distanceToPedestrianThreshold);
+  nh.param("/op_behavior_selector/turnThreshold", m_turnThreshold, 20.0);
+
+
+  tf::StampedTransform transform;
+  PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
+  m_OriginPos.position.x  = transform.getOrigin().x();
+  m_OriginPos.position.y  = transform.getOrigin().y();
+  m_OriginPos.position.z  = transform.getOrigin().z();
+
+  pub_LocalPath = nh.advertise<autoware_msgs::Lane>("final_waypoints", 1,true);
+  pub_LocalPathWithPosePub = nh.advertise<rubis_msgs::LaneWithPoseTwist>("final_waypoints_with_pose_twist", 1,true);
+  pub_LocalBasePath = nh.advertise<autoware_msgs::Lane>("base_waypoints", 1,true);
+  pub_ClosestIndex = nh.advertise<std_msgs::Int32>("closest_waypoint", 1,true);
+  pub_BehaviorState = nh.advertise<geometry_msgs::TwistStamped>("current_behavior", 1);
+  pub_SimuBoxPose    = nh.advertise<geometry_msgs::PoseArray>("sim_box_pose_ego", 1);
+  pub_BehaviorStateRviz = nh.advertise<visualization_msgs::MarkerArray>("behavior_state", 1);
+  pub_SelectedPathRviz = nh.advertise<visualization_msgs::MarkerArray>("local_selected_trajectory_rviz", 1);
+  pub_EmergencyStop = nh.advertise<std_msgs::Bool>("emergency_stop", 1);
+  pub_turnAngle = nh.advertise<std_msgs::Float64>("turn_angle", 1);
+  pub_turnMarker = nh.advertise<visualization_msgs::MarkerArray>("turn_marker", 1);
+  pub_currentState = nh.advertise<std_msgs::Int32>("current_state", 1);
+
+  int bVelSource = 1;
+  _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
+  if(bVelSource == 0)
+    sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this);
+  else if(bVelSource == 2)
+    sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this);
+
+  sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this);
+  sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories_with_pose_twist", 1, &BehaviorGen::callbackGetLocalPlannerPath, this);
+  // sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this);
+  // sub_TrafficLightSignals  = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this);
+  sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this);
+
+  sub_TrafficLightSignals  = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this);
+
+  sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this);
+  sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this);
+  //sub_ctrl_cmd = nh.subscribe("/ctrl_cmd", 1, &BehaviorGen::callbackGetCommandCMD, this);
+  sub_DistanceToPedestrian = nh.subscribe("/distance_to_pedestrian", 1, &BehaviorGen::callbackDistanceToPedestrian, this);
+  sub_IntersectionCondition = nh.subscribe("/intersection_condition", 1, &BehaviorGen::callbackIntersectionCondition, this);
+  sub_SprintSwitch = nh.subscribe("/sprint_switch", 1, &BehaviorGen::callbackSprintSwitch, this);
+
+  //Mapping Section
+  sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &BehaviorGen::callbackGetVMLanes,  this);
+  sub_points = nh.subscribe("/vector_map_info/point", 1, &BehaviorGen::callbackGetVMPoints,  this);
+  sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &BehaviorGen::callbackGetVMdtLanes,  this);
+  sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &BehaviorGen::callbackGetVMIntersections,  this);
+  sup_area = nh.subscribe("/vector_map_info/area", 1, &BehaviorGen::callbackGetVMAreas,  this);
+  sub_lines = nh.subscribe("/vector_map_info/line", 1, &BehaviorGen::callbackGetVMLines,  this);
+  sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &BehaviorGen::callbackGetVMStopLines,  this);
+  sub_signals = nh.subscribe("/vector_map_info/signal", 1, &BehaviorGen::callbackGetVMSignal,  this);
+  sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &BehaviorGen::callbackGetVMVectors,  this);
+  sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &BehaviorGen::callbackGetVMCurbs,  this);
+  sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &BehaviorGen::callbackGetVMRoadEdges,  this);
+  sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &BehaviorGen::callbackGetVMWayAreas,  this);
+  sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &BehaviorGen::callbackGetVMCrossWalks,  this);
+  sub_nodes = nh.subscribe("/vector_map_info/node", 1, &BehaviorGen::callbackGetVMNodes,  this);
+}
+
+BehaviorGen::~BehaviorGen()
+{
+  UtilityHNS::DataRW::WriteLogData(UtilityHNS::UtilityH::GetHomeDirectory()+UtilityHNS::DataRW::LoggingMainfolderName+UtilityHNS::DataRW::StatesLogFolderName, "MainLog",
+        "time,dt, Behavior_i, Behavior_str, RollOuts_n, Blocked_i, Central_i, Selected_i, StopSign_id, Light_id, Stop_Dist, Follow_Dist, Follow_Vel,"
+        "Target_Vel, PID_Vel, T_cmd_Vel, C_cmd_Vel, Vel, Steer, X, Y, Z, Theta,"
+        , m_LogData);
+}
+
+void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh)
+{
+  _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
+  if(m_PlanningParams.enableSwerving)
+    m_PlanningParams.enableFollowing = true;
+  else
+    _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
+
+  _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
+  _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
+
+  _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
+  _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
+  _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
+
+  _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
+
+  _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
+  if(m_PlanningParams.enableSwerving)
+    _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
+  else
+    m_PlanningParams.rollOutNumber = 0;
+
+  _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
+  _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
+  _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
+  _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
+  _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
+
+  _nh.getParam("/op_trajectory_evaluator/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
+  _nh.getParam("/op_trajectory_evaluator/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
+
+  _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
+
+  _nh.getParam("/op_common_params/width", m_CarInfo.width);
+  _nh.getParam("/op_common_params/length", m_CarInfo.length);
+  _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
+  _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
+  _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
+  _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
+  _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
+  m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
+  m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
+
+  _nh.getParam("/op_common_params/stopLineMargin", m_PlanningParams.stopLineMargin);
+  _nh.getParam("/op_common_params/stopLineDetectionDistance", m_PlanningParams.stopLineDetectionDistance);
+
+  PlannerHNS::ControllerParams controlParams;
+  controlParams.Steering_Gain = PlannerHNS::PID_CONST(0.07, 0.02, 0.01);
+  controlParams.Velocity_Gain = PlannerHNS::PID_CONST(0.1, 0.005, 0.1);
+  nh.getParam("/op_common_params/steeringDelay", controlParams.SteeringDelay);
+  nh.getParam("/op_common_params/minPursuiteDistance", controlParams.minPursuiteDistance );
+  nh.getParam("/op_common_params/additionalBrakingDistance", m_PlanningParams.additionalBrakingDistance );
+  nh.getParam("/op_common_params/giveUpDistance", m_PlanningParams.giveUpDistance );
+  nh.getParam("/op_common_params/enableSlowDownOnCurve", m_PlanningParams.enableSlowDownOnCurve );
+  nh.getParam("/op_common_params/curveVelocityRatio", m_PlanningParams.curveVelocityRatio );
+
+  int iSource = 0;
+  _nh.getParam("/op_common_params/mapSource" , iSource);
+  if(iSource == 0)
+    m_MapType = PlannerHNS::MAP_AUTOWARE;
+  else if (iSource == 1)
+    m_MapType = PlannerHNS::MAP_FOLDER;
+  else if(iSource == 2)
+    m_MapType = PlannerHNS::MAP_KML_FILE;
+
+  _nh.getParam("/op_common_params/mapFileName" , m_MapPath);
+
+  _nh.getParam("/op_behavior_selector/evidence_tust_number", m_PlanningParams.nReliableCount);
+
+  //std::cout << "nReliableCount: " << m_PlanningParams.nReliableCount << std::endl;
+  
+  _nh.param("/op_behavior_selector/sprintSpeed", m_sprintSpeed, 13.5);
+  _nh.param("/op_behavior_selector/obstacleWaitingTimeinIntersection", m_obstacleWaitingTimeinIntersection, 1.0);
+
+  m_BehaviorGenerator.Init(controlParams, m_PlanningParams, m_CarInfo, m_sprintSpeed);  
+
+  m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior = PlannerHNS::INITIAL_STATE;
+  
+  m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection;
+}
+
+void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){
+  double distance = msg.data;
+  if(distance < m_distanceToPedestrianThreshold){
+    m_PlanningParams.pedestrianAppearence = true;
+  }
+  else
+  {
+    m_PlanningParams.pedestrianAppearence = false;
+  }
+  m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence);
+  // m_BehaviorGenerator.printPedestrianAppearence();
+}
+
+void BehaviorGen::callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg){
+  m_BehaviorGenerator.m_isInsideIntersection = msg.isIntersection;
+  m_BehaviorGenerator.m_closestIntersectionDistance = msg.intersectionDistance;
+  m_BehaviorGenerator.m_riskyLeft = msg.riskyLeftTurn;
+  m_BehaviorGenerator.m_riskyRight = msg.riskyRightTurn;
+}
+
+void BehaviorGen::callbackSprintSwitch(const std_msgs::Bool& msg){
+  m_sprintSwitch = msg.data;
+}
+
+void BehaviorGen::callbackGetTwistRaw(const geometry_msgs::TwistStampedConstPtr& msg)
+{
+  m_Twist_raw = *msg;
+}
+
+void BehaviorGen::callbackGetTwistCMD(const geometry_msgs::TwistStampedConstPtr& msg)
+{
+  m_Twist_cmd = *msg;
+}
+
+void BehaviorGen::callbackGetCommandCMD(const autoware_msgs::ControlCommandConstPtr& msg)
+{
+  m_Ctrl_cmd = *msg;
+}
+
+void BehaviorGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
+{
+  m_VehicleStatus.speed = msg->speed/3.6;
+  m_CurrentPos.v = m_VehicleStatus.speed;
+  m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value;
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+}
+
+void BehaviorGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
+{
+  m_VehicleStatus.speed = msg->twist.twist.linear.x;
+  m_CurrentPos.v = m_VehicleStatus.speed ;
+  if(msg->twist.twist.linear.x != 0)
+    m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+}
+
+void BehaviorGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
+{
+  if(msg->lanes.size() > 0 && bMap)
+  {
+
+    bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
+
+    m_GlobalPaths.clear();
+
+    for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
+    {
+      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
+      PlannerHNS::Lane* pPrevValid = 0;
+      for(unsigned int j = 0 ; j < m_temp_path.size(); j++)
+      {
+        PlannerHNS::Lane* pLane = 0;
+        pLane = PlannerHNS::MappingHelpers::GetLaneById(m_temp_path.at(j).laneId, m_Map);
+        if(!pLane)
+        {
+          pLane = PlannerHNS::MappingHelpers::GetClosestLaneFromMapDirectionBased(m_temp_path.at(j), m_Map, 1);
+
+          if(!pLane && !pPrevValid)
+          {
+            ROS_ERROR("Map inconsistency between Global Path and Local Planer Map, Can't identify current lane.");
+            return;
+          }
+
+          if(!pLane)
+            m_temp_path.at(j).pLane = pPrevValid;
+          else
+          {
+            m_temp_path.at(j).pLane = pLane;
+            pPrevValid = pLane ;
+          }
+
+          m_temp_path.at(j).laneId = m_temp_path.at(j).pLane->id;
+        }
+        else
+          m_temp_path.at(j).pLane = pLane;
+
+
+        //std::cout << "StopLineInGlobalPath: " << m_temp_path.at(j).stopLineID << std::endl;
+      }
+
+      PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
+      m_GlobalPaths.push_back(m_temp_path);
+
+      if(bOldGlobalPath)
+      {
+        bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
+      }
+    }
+
+    if(!bOldGlobalPath)
+    {
+      bWayGlobalPath = true;
+      bWayGlobalPathLogs = true;
+      for(unsigned int i = 0; i < m_GlobalPaths.size(); i++)
+      {
+        PlannerHNS::PlanningHelpers::FixPathDensity(m_GlobalPaths.at(i), m_PlanningParams.pathDensity);
+        PlannerHNS::PlanningHelpers::SmoothPath(m_GlobalPaths.at(i), 0.35, 0.4, 0.05);
+
+        PlannerHNS::PlanningHelpers::GenerateRecommendedSpeed(m_GlobalPaths.at(i), m_CarInfo.max_speed_forward, m_PlanningParams.speedProfileFactor);
+        m_GlobalPaths.at(i).at(m_GlobalPaths.at(i).size()-1).v = 0;
+      }
+
+      std::cout << "Received New Global Path Selector! " << std::endl;
+    }
+    else
+    {
+      m_GlobalPaths.clear();
+    }
+  }
+}
+
+void BehaviorGen::callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg)
+{
+  if(m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior == PlannerHNS::INTERSECTION_STATE){
+    bBestCost = true;
+    m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance;
+    m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity;
+    return;
+  }
+  bBestCost = true;
+  m_TrajectoryBestCost.bBlocked = msg->is_blocked;
+  m_TrajectoryBestCost.index = msg->lane_index;
+  m_TrajectoryBestCost.cost = msg->cost;
+  m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance;
+  m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity;
+}
+
+void BehaviorGen::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg)
+{  
+  rubis::instance_ = msg->instance;
+  lane_array_with_pose_twist_msg_ = msg;
+
+  // Callback
+  m_VehicleStatus.speed = msg->twist.twist.linear.x;
+  m_CurrentPos.v = m_VehicleStatus.speed;
+  if(fabs(msg->twist.twist.linear.x) > 0.25)
+    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+
+  m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation));
+  bNewCurrentPos = true;
+
+  if(msg->lane_array.lanes.size() > 0)
+  {
+    m_RollOuts.clear();
+    int globalPathId_roll_outs = -1;
+
+    for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++)
+    {
+      std::vector<PlannerHNS::WayPoint> path;
+      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path);
+      m_RollOuts.push_back(path);
+
+      if(path.size() > 0)
+        globalPathId_roll_outs = path.at(0).gid;
+    }
+
+    if(bWayGlobalPath && m_GlobalPaths.size() > 0)
+    {
+      if(m_GlobalPaths.at(0).size() > 0)
+      {
+        int globalPathId = m_GlobalPaths.at(0).at(0).gid;
+        std::cout << "Before Synchronization At Behavior Selector: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
+
+        if(globalPathId_roll_outs == globalPathId)
+        {
+          bWayGlobalPath = false;
+          m_GlobalPathsToUse = m_GlobalPaths;
+          m_BehaviorGenerator.SetNewGlobalPath(m_GlobalPathsToUse);
+          std::cout << "Synchronization At Behavior Selector: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
+        }
+      }
+    }
+
+    m_BehaviorGenerator.m_RollOuts = m_RollOuts;
+    bRollOuts = true;
+  }
+}
+
+void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg)
+{
+  bNewLightSignal = true;
+  std::vector<PlannerHNS::TrafficLight> simulatedLights;
+  for(unsigned int i = 0 ; i < msg.signals.size() ; i++)
+  {
+    PlannerHNS::TrafficLight tl;
+    tl.id = msg.signals.at(i).id;
+    tl.remainTime = msg.signals.at(i).time;
+
+    for(unsigned int k = 0; k < m_Map.trafficLights.size(); k++)
+    {
+      if(m_Map.trafficLights.at(k).id == tl.id)
+      {
+        tl.pos = m_Map.trafficLights.at(k).pos;
+        tl.routine = m_Map.trafficLights.at(k).routine;
+        break;
+      }
+    }
+
+    if(msg.signals.at(i).type == 0)
+    {
+      tl.lightState = PlannerHNS::RED_LIGHT;
+    }
+    else if(msg.signals.at(i).type == 1)
+    {
+      tl.lightState = PlannerHNS::YELLOW_LIGHT;
+    }
+    else
+    {
+      tl.lightState = PlannerHNS::GREEN_LIGHT;
+    }
+
+    simulatedLights.push_back(tl);
+  }
+
+  m_CurrTrafficLight = simulatedLights;
+}
+
+void BehaviorGen::VisualizeLocalPlanner()
+{
+  visualization_msgs::Marker behavior_rviz;
+  int iDirection = 0;
+  if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory > m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+    iDirection = 1;
+  else if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory < m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+    iDirection = -1;
+  PlannerHNS::ROSHelpers::VisualizeBehaviorState(m_CurrentPos, m_CurrentBehavior, !m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bTrafficIsRed , iDirection, behavior_rviz, "beh_state");
+  //pub_BehaviorStateRviz.publish(behavior_rviz);
+
+  visualization_msgs::MarkerArray markerArray;
+
+  //PlannerHNS::ROSHelpers::GetIndicatorArrows(m_CurrentPos, m_CarInfo.width, m_CarInfo.length, m_CurrentBehavior.indicator, 0, markerArray);
+
+  markerArray.markers.push_back(behavior_rviz);
+
+  pub_BehaviorStateRviz.publish(markerArray);
+
+  //To Test Synchronization Problem
+//  visualization_msgs::MarkerArray selected_path;
+//  std::vector<std::vector<std::vector<PlannerHNS::WayPoint> > > paths;
+//  paths.push_back(std::vector<std::vector<PlannerHNS::WayPoint> >());
+//  paths.at(0).push_back(m_BehaviorGenerator.m_Path);
+//  paths.push_back(m_GlobalPathsToUse);
+//  paths.push_back(m_RollOuts);
+//  PlannerHNS::ROSHelpers::TrajectoriesToMarkers(paths, selected_path);
+//  pub_SelectedPathRviz.publish(selected_path);
+}
+
+void BehaviorGen::SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg)
+{
+  //Send Behavior State
+  geometry_msgs::Twist t;
+  geometry_msgs::TwistStamped behavior;
+  t.linear.x = m_CurrentBehavior.bNewPlan;
+  t.linear.y = m_CurrentBehavior.followDistance;
+  t.linear.z = m_CurrentBehavior.followVelocity;
+  t.angular.x = (int)m_CurrentBehavior.indicator;
+  t.angular.y = (int)m_CurrentBehavior.state;
+  t.angular.z = m_CurrentBehavior.iTrajectory;
+  behavior.twist = t;
+  behavior.header.stamp = ros::Time::now();
+  pub_BehaviorState.publish(behavior);
+
+  //Send Ego Vehicle Simulation Pose Data
+  geometry_msgs::PoseArray sim_data;
+  geometry_msgs::Pose p_id, p_pose, p_box;
+
+  sim_data.header.frame_id = "map";
+  sim_data.header.stamp = ros::Time();
+  p_id.position.x = 0;
+  p_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a));
+
+  PlannerHNS::WayPoint pose_center = PlannerHNS::PlanningHelpers::GetRealCenter(m_BehaviorGenerator.state, m_CarInfo.wheel_base);
+
+  p_pose.position.x = pose_center.pos.x;
+  p_pose.position.y = pose_center.pos.y;
+  p_pose.position.z = pose_center.pos.z;
+  p_box.position.x = m_BehaviorGenerator.m_CarInfo.width;
+  p_box.position.y = m_BehaviorGenerator.m_CarInfo.length;
+  p_box.position.z = 2.2;
+  sim_data.poses.push_back(p_id);
+  sim_data.poses.push_back(p_pose);
+  sim_data.poses.push_back(p_box);
+  pub_SimuBoxPose.publish(sim_data);
+
+  //Send Trajectory Data to path following nodes
+  std_msgs::Int32 closest_waypoint;
+  PlannerHNS::RelativeInfo info;
+  PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info);
+  PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack);
+  //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl;  
+
+  closest_waypoint.data = 1;
+  pub_ClosestIndex.publish(closest_waypoint);
+  pub_LocalBasePath.publish(m_CurrentTrajectoryToSend);
+
+  rubis_msgs::LaneWithPoseTwist final_waypoints_with_pose_twist_msg;
+  final_waypoints_with_pose_twist_msg.instance = rubis::instance_;
+  final_waypoints_with_pose_twist_msg.lane = m_CurrentTrajectoryToSend;  
+  final_waypoints_with_pose_twist_msg.pose = msg->pose;
+  final_waypoints_with_pose_twist_msg.twist = msg->twist;
+
+  pub_LocalPathWithPosePub.publish(final_waypoints_with_pose_twist_msg);
+  pub_LocalPath.publish(m_CurrentTrajectoryToSend);
+
+  
+}
+
+void BehaviorGen::LogLocalPlanningInfo(double dt)
+{
+  timespec log_t;
+  UtilityHNS::UtilityH::GetTickCount(log_t);
+  std::ostringstream dataLine;
+  dataLine << UtilityHNS::UtilityH::GetLongTime(log_t) <<"," << dt << "," << m_CurrentBehavior.state << ","<< PlannerHNS::ROSHelpers::GetBehaviorNameFromCode(m_CurrentBehavior.state) << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->m_pParams->rollOutNumber << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bFullyBlock << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentStopSignID << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentTrafficLightID << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->minStoppingDistance << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->distanceToNext << "," <<
+      m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->velocityOfNext << "," <<
+      m_CurrentBehavior.maxVelocity << "," <<
+      m_Twist_raw.twist.linear.x << "," <<
+      m_Twist_cmd.twist.linear.x << "," <<
+      m_Ctrl_cmd.linear_velocity << "," <<
+      m_VehicleStatus.speed << "," <<
+      m_VehicleStatus.steer << "," <<
+      m_BehaviorGenerator.state.pos.x << "," << m_BehaviorGenerator.state.pos.y << "," << m_BehaviorGenerator.state.pos.z << "," << UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)+M_PI << ",";
+  m_LogData.push_back(dataLine.str());
+
+
+  if(bWayGlobalPathLogs)
+  {
+    for(unsigned int i=0; i < m_GlobalPaths.size(); i++)
+    {
+      std::ostringstream str_out;
+      str_out << UtilityHNS::UtilityH::GetHomeDirectory();
+      str_out << UtilityHNS::DataRW::LoggingMainfolderName;
+      str_out << UtilityHNS::DataRW::PathLogFolderName;
+      str_out << "Global_Vel";
+      str_out << i;
+      str_out << "_";
+      PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), m_GlobalPaths.at(i));
+    }
+    bWayGlobalPathLogs = false;
+  }
+}
+
+void BehaviorGen::CalculateTurnAngle(PlannerHNS::WayPoint turn_point){
+  geometry_msgs::PoseStamped turn_pose;
+
+  if(GetBaseMapTF()){
+    // std::cout<<"BEFORE:"<<turn_point.pos.x<<" "<<turn_point.pos.y<<" "<<turn_point.rot.x<<" "<<turn_point.rot.y<<" "<<turn_point.rot.z<<std::endl;
+    turn_pose.pose.position.x = turn_point.pos.x;
+    turn_pose.pose.position.y = turn_point.pos.y;
+    turn_pose.pose.position.z = turn_point.pos.z;
+    turn_pose.pose.orientation.x = turn_point.rot.x;
+    turn_pose.pose.orientation.y = turn_point.rot.y;
+    turn_pose.pose.orientation.z = turn_point.rot.z;
+    turn_pose.pose.orientation.w = turn_point.rot.w;
+    TransformPose(turn_pose, turn_pose, m_map_base_transform);
+    // std::cout<<"AFTER:"<<turn_pose.pose.position.x<<" "<<turn_pose.pose.position.y<<" "<<turn_pose.pose.orientation.x<<" "<<turn_pose.pose.orientation.y<<" "<<turn_pose.pose.orientation.z<<std::endl;
+
+    double hypot_length = hypot(turn_pose.pose.position.x, turn_pose.pose.position.y);
+
+    if(hypot_length <= 0)
+      m_turnAngle = 0;
+    else
+      m_turnAngle = acos(abs(turn_pose.pose.position.x)/hypot_length)*180.0/PI;
+    if(turn_pose.pose.position.y < 0)
+      m_turnAngle = -1 * m_turnAngle;
+  }
+
+  return;
+}
+
+void BehaviorGen::MainLoop()
+{
+  ros::NodeHandle private_nh("~");
+
+  // Scheduling Setup
+  std::string task_response_time_filename;
+  int rate;
+  double task_minimum_inter_release_time;
+  double task_execution_time;
+  double task_relative_deadline; 
+
+  m_BehaviorGenerator.m_turnThreshold = m_turnThreshold;
+
+  m_sprintSwitch = false;
+
+  private_nh.param<std::string>("/op_behavior_selector/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv");
+  private_nh.param<int>("/op_behavior_selector/rate", rate, 10);
+  private_nh.param("/op_behavior_selector/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
+  private_nh.param("/op_behavior_selector/task_execution_time", task_execution_time, (double)10);
+  private_nh.param("/op_behavior_selector/task_relative_deadline", task_relative_deadline, (double)10);
+
+  /* For Task scheduling */
+  rubis::init_task_profiling(task_response_time_filename);
+
+  m_sprintSwitch = false;
+
+  ros::Rate r(100);
+  while(ros::ok()){
+    rubis::start_task_profiling();
+
+    ros::spinOnce();
+
+    // Check Pedestrian is Appeared
+    double dt  = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer);
+    UtilityHNS::UtilityH::GetTickCount(planningTimer);
+
+    if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap)
+    {
+      bMap = true;
+      PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map);
+    }
+    else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap)
+    {
+      bMap = true;
+      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true);
+
+    }
+    else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap)
+    {
+      std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;;
+
+      if(m_MapRaw.GetVersion()==2)
+      {
+        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
+            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
+            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
+            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,
+            m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false);
+
+        try{
+          // Add Traffic Signal Info from yaml file
+          XmlRpc::XmlRpcValue traffic_light_list;
+          nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list);
+
+          // Add Stop Line Info from yaml file
+          XmlRpc::XmlRpcValue stop_line_list;
+          nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list);
+
+          // Add Crossing Info from yaml file
+          // XmlRpc::XmlRpcValue intersection_list;
+          // nh.getParam("/op_behavior_selector/intersection_list", intersection_list);
+
+          PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list);
+        }
+        catch(XmlRpc::XmlRpcException& e){
+          ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str());
+          exit(1);
+        }
+
+        m_BehaviorGenerator.m_Map = m_Map;
+
+        if(m_Map.roadSegments.size() > 0)
+        {
+          bMap = true;
+          std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl;
+        }
+      }
+      else if(m_MapRaw.GetVersion()==1)
+      {
+        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
+            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
+            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
+            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,  PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false);
+
+        if(m_Map.roadSegments.size() > 0)
+        {
+          bMap = true;
+          std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl;
+        }
+      }
+    }
+
+    if(bNewCurrentPos && m_GlobalPaths.size()>0)
+    {
+      if(bNewLightSignal)
+      {
+        m_PrevTrafficLight = m_CurrTrafficLight;
+        bNewLightSignal = false;
+      }
+
+      if(bNewLightStatus)
+      {
+        bNewLightStatus = false;
+        for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++)
+          m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus;
+      }
+
+      m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch;
+      m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0);
+      std_msgs::Int32 curr_state_msg;
+      curr_state_msg.data = m_CurrentBehavior.state;
+
+      pub_currentState.publish(curr_state_msg);
+
+      CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint);
+      m_BehaviorGenerator.m_turnAngle = m_turnAngle;
+
+      std_msgs::Float64 turn_angle_msg;
+      turn_angle_msg.data = m_turnAngle;
+      pub_turnAngle.publish(turn_angle_msg);
+
+      emergency_stop_msg.data = false;
+      if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop!
+        emergency_stop_msg.data = true;
+      pub_EmergencyStop.publish(emergency_stop_msg);
+
+      SendLocalPlanningTopics(lane_array_with_pose_twist_msg_);
+      VisualizeLocalPlanner();
+      LogLocalPlanningInfo(dt);
+
+      // Publish turn_marker
+      visualization_msgs::MarkerArray turn_marker;
+      visualization_msgs::Marker marker;
+      marker.header.frame_id = "map";
+      marker.type = 2;
+      marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x;
+      marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y;
+      marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z;
+      marker.scale.x = 3;
+      marker.scale.y = 3;
+      marker.scale.z = 3;
+      marker.color.r = 0.0f;
+      marker.color.g = 1.0f;
+      marker.color.b = 0.0f;
+      marker.color.a = 1.0f;
+      marker.header.stamp = ros::Time::now();
+      marker.header.frame_id = "map";
+      turn_marker.markers.push_back(marker);
+
+      pub_turnMarker.publish(turn_marker);
+    }
+    else
+      sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array",   1,    &BehaviorGen::callbackGetGlobalPlannerPath,   this);
+
+    rubis::stop_task_profiling(0, 0, 0);    
+    r.sleep();
+  }
+}
+
+bool BehaviorGen::GetBaseMapTF(){
+  
+  try{
+    m_map_base_listener.waitForTransform("base_link", "map", ros::Time(0), ros::Duration(0.001));
+    m_map_base_listener.lookupTransform("base_link", "map", ros::Time(0), m_map_base_transform);
+    return true;
+  }
+  catch(tf::TransformException& ex)
+  {
+    return false;
+  }
+  
+}
+
+void BehaviorGen::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform)
+{
+
+  tf::Vector3 in_pos(in_pose.pose.position.x,
+                     in_pose.pose.position.y,
+                     in_pose.pose.position.z);
+  tf::Quaternion in_quat(in_pose.pose.orientation.x,
+                         in_pose.pose.orientation.y,
+                         in_pose.pose.orientation.w,
+                         in_pose.pose.orientation.z);
+
+  tf::Vector3 in_pos_t = in_transform * in_pos;
+  tf::Quaternion in_quat_t = in_transform * in_quat;
+  
+  out_pose.header = in_pose.header;
+  out_pose.pose.position.x = in_pos_t.x();
+  out_pose.pose.position.y = in_pos_t.y();
+  out_pose.pose.position.z = in_pos_t.z();
+  out_pose.pose.orientation.x = in_quat_t.x();
+  out_pose.pose.orientation.y = in_quat_t.y();
+  out_pose.pose.orientation.z = in_quat_t.z();
+
+  return;
+}
+
+//Mapping Section
+
+void BehaviorGen::callbackGetVMLanes(const vector_map_msgs::LaneArray& msg)
+{
+  std::cout << "Received Lanes" << endl;
+  if(m_MapRaw.pLanes == nullptr)
+    m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMPoints(const vector_map_msgs::PointArray& msg)
+{
+  std::cout << "Received Points" << endl;
+  if(m_MapRaw.pPoints  == nullptr)
+    m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray& msg)
+{
+  std::cout << "Received dtLanes" << endl;
+  if(m_MapRaw.pCenterLines == nullptr)
+    m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray& msg)
+{
+  std::cout << "Received CrossRoads" << endl;
+  if(m_MapRaw.pIntersections == nullptr)
+    m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMAreas(const vector_map_msgs::AreaArray& msg)
+{
+  std::cout << "Received Areas" << endl;
+  if(m_MapRaw.pAreas == nullptr)
+    m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMLines(const vector_map_msgs::LineArray& msg)
+{
+  std::cout << "Received Lines" << endl;
+  if(m_MapRaw.pLines == nullptr)
+    m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg)
+{
+  std::cout << "Received StopLines" << endl;
+  if(m_MapRaw.pStopLines == nullptr)
+    m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMSignal(const vector_map_msgs::SignalArray& msg)
+{
+  std::cout << "Received Signals" << endl;
+  if(m_MapRaw.pSignals  == nullptr)
+    m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMVectors(const vector_map_msgs::VectorArray& msg)
+{
+  std::cout << "Received Vectors" << endl;
+  if(m_MapRaw.pVectors  == nullptr)
+    m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMCurbs(const vector_map_msgs::CurbArray& msg)
+{
+  std::cout << "Received Curbs" << endl;
+  if(m_MapRaw.pCurbs == nullptr)
+    m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray& msg)
+{
+  std::cout << "Received Edges" << endl;
+  if(m_MapRaw.pRoadedges  == nullptr)
+    m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray& msg)
+{
+  std::cout << "Received Wayareas" << endl;
+  if(m_MapRaw.pWayAreas  == nullptr)
+    m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray& msg)
+{
+  std::cout << "Received CrossWalks" << endl;
+  if(m_MapRaw.pCrossWalks == nullptr)
+    m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg);
+}
+
+void BehaviorGen::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg)
+{
+  std::cout << "Received Nodes" << endl;
+  if(m_MapRaw.pNodes == nullptr)
+    m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg);
+}
+}
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp
index 8488fd1d..15f6bc66 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp
@@ -43,6 +43,7 @@ MotionPrediction::MotionPrediction()
   m_OriginPos.position.z  = transform.getOrigin().z();
 
   pub_predicted_objects_trajectories = nh.advertise<autoware_msgs::DetectedObjectArray>("/predicted_objects", 1);
+  pub_rubis_predicted_objects_trajectories = nh.advertise<rubis_msgs::DetectedObjectArray>("/rubis_predicted_objects", 1);
   pub_PredictedTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("/predicted_trajectories_rviz", 1);
   pub_CurbsRviz          = nh.advertise<visualization_msgs::MarkerArray>("/map_curbs_rviz", 1);
   pub_ParticlesRviz = nh.advertise<visualization_msgs::MarkerArray>("prediction_particles", 1);
@@ -62,37 +63,45 @@ MotionPrediction::MotionPrediction()
   std::vector<std::string> input_object_list = ParseInputStr(input_object_list_str);
 
 
-  for(auto it = input_object_list.begin(); it != input_object_list.end(); ++it){
-    std::string topic = *it;
-    objects_subs_.push_back(nh.subscribe(topic.c_str(), 1, &MotionPrediction::callbackGetTrackedObjects, this));
-    autoware_msgs::DetectedObjectArray msg;
-    object_msg_list_.push_back(msg);
-  }
-  
-  sub_current_pose   = nh.subscribe("/current_pose", 10,  &MotionPrediction::callbackGetCurrentPose,     this);
-
-  int bVelSource = 1;
-  _nh.getParam("/op_motion_predictor/velocitySource", bVelSource);
-  if(bVelSource == 0)
-    sub_robot_odom = nh.subscribe("/odom", 10, &MotionPrediction::callbackGetRobotOdom, this);
-  else if(bVelSource == 1)
-    sub_current_velocity = nh.subscribe("/current_velocity", 10, &MotionPrediction::callbackGetVehicleStatus, this);
-  else if(bVelSource == 2)
-    sub_can_info = nh.subscribe("/can_info", 10, &MotionPrediction::callbackGetCANInfo, this);
   
+  bool use_svl_sensing = false;
+  _nh.param<bool>("/op_motion_predictor/use_svl_sensing", use_svl_sensing, false);
 
-  /*  RT Scheduling setup  */
-  // sub_current_pose   = nh.subscribe("/current_pose", 1,  &MotionPrediction::callbackGetCurrentPose,     this); //origin 10
+  if(use_svl_sensing){
+    svl_objects_sub_.subscribe(_nh, "/detection/lidar_detector/rubis_objects_center", 10);
+    pose_twist_sub_.subscribe(_nh, "/rubis_current_pose_twist", 10);
+    sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), svl_objects_sub_, pose_twist_sub_));
+	  sync_->registerCallback(boost::bind(&MotionPrediction::callbackSvl, this, _1, _2));
+
+    autoware_msgs::DetectedObjectArray msg;    
+    object_msg_list_.push_back(msg);
+  }
+  else{
+    for(auto it = input_object_list.begin(); it != input_object_list.end(); ++it){    
+      std::string topic = *it;
+      std::cout<<topic<<std::endl;
 
-  // int bVelSource = 1;
-  // _nh.getParam("/op_motion_predictor/velocitySource", bVelSource);
-  // if(bVelSource == 0)
-  //   sub_robot_odom = nh.subscribe("/odom", 1, &MotionPrediction::callbackGetRobotOdom, this); //origin 10
-  // else if(bVelSource == 1)
-  //   sub_current_velocity = nh.subscribe("/current_velocity", 1, &MotionPrediction::callbackGetVehicleStatus, this); //origin 10
-  // else if(bVelSource == 2)
-  //   sub_can_info = nh.subscribe("/can_info", 1, &MotionPrediction::callbackGetCANInfo, this); //origin 10
+      if(topic.find(std::string("rubis")) != std::string::npos){ // For rubis topic
+        objects_subs_.push_back(nh.subscribe(topic.c_str(), 1, &MotionPrediction::callbackGetRubisTrackedObjects, this));  
+      }
+      else{// For normal topic      
+        objects_subs_.push_back(nh.subscribe(topic.c_str(), 1, &MotionPrediction::callbackGetTrackedObjects, this));      
+      }
+      autoware_msgs::DetectedObjectArray msg;    
+      object_msg_list_.push_back(msg);
+    }
 
+    sub_current_pose   = nh.subscribe("/current_pose", 1,  &MotionPrediction::callbackGetCurrentPose,     this); // Def: 1
+    int bVelSource = 1;
+    _nh.getParam("/op_motion_predictor/velocitySource", bVelSource);
+    if(bVelSource == 0)
+      sub_robot_odom = nh.subscribe("/odom", 1, &MotionPrediction::callbackGetRobotOdom, this); // Def: 1
+    else if(bVelSource == 1)
+      sub_current_velocity = nh.subscribe("/current_velocity", 1, &MotionPrediction::callbackGetVehicleStatus, this); // Def: 1
+    else if(bVelSource == 2)
+      sub_can_info = nh.subscribe("/can_info", 1, &MotionPrediction::callbackGetCANInfo, this); // Def: 1
+  }
+  
   UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer);
   PlannerHNS::ROSHelpers::InitPredMarkers(100, m_PredictedTrajectoriesDummy);
   PlannerHNS::ROSHelpers::InitCurbsMarkers(100, m_CurbsDummy);
@@ -251,8 +260,6 @@ void MotionPrediction::callbackGetVehicleStatus(const geometry_msgs::TwistStampe
     m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x);
   UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
   bVehicleStatus = true;
-
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();  
 }
 
 void MotionPrediction::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
@@ -320,7 +327,7 @@ autoware_msgs::DetectedObjectArray MotionPrediction::TrasformObjAryToVeldoyne(co
 
 void MotionPrediction::callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& in_msg)
 {
-  
+  // std::cout<<"ERROR non rubis"<<std::endl;
   UtilityHNS::UtilityH::GetTickCount(m_SensingTimer);
   m_TrackedObjects.clear();
   bTrackedObjects = true;
@@ -408,7 +415,164 @@ void MotionPrediction::callbackGetTrackedObjects(const autoware_msgs::DetectedOb
     m_PredictedResultsResults.header.frame_id = "velodyne";
     
     pub_predicted_objects_trajectories.publish(m_PredictedResultsResults);
-    rubis::sched::task_state_ = TASK_STATE_DONE;
+  }
+}
+
+void MotionPrediction::callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& in_msg){
+  rubis::start_task_profiling();
+
+  rubis_msgs::DetectedObjectArray msg = *in_msg;
+  _callbackGetRubisTrackedObjects(msg);
+
+  if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap)
+  {
+    bMap = true;
+    PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map);
+  }
+  else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap)
+  {
+    bMap = true;
+    PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true);
+  }
+  else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap)
+  {
+    std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;;
+
+    if(m_MapRaw.GetVersion()==2)
+    {
+      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
+          m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
+          m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
+          m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+          m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,
+          m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, true);
+
+      if(m_Map.roadSegments.size() > 0)
+      {
+        bMap = true;
+        std::cout << " ******* Map V2 Is Loaded successfully from the Motion Predictor !! " << std::endl;
+      }
+    }
+    else if(m_MapRaw.GetVersion()==1)
+    {
+      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
+          m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
+          m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
+          m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+          m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,  PlannerHNS::GPSPoint(), m_Map, true);
+
+      if(m_Map.roadSegments.size() > 0)
+      {
+        bMap = true;
+        std::cout << " ******* Map V1 Is Loaded successfully from the Motion Predictor !! " << std::endl;
+      }
+    }
+  }
+
+  if(UtilityHNS::UtilityH::GetTimeDiffNow(m_VisualizationTimer) > m_VisualizationTime)
+  {
+    VisualizePrediction();
+    UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer);
+  }
+
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+}
+
+void MotionPrediction::_callbackGetRubisTrackedObjects(rubis_msgs::DetectedObjectArray& objects_msg)
+{
+  rubis::instance_ = objects_msg.instance;
+  rubis::lidar_instance_ = objects_msg.lidar_instance;
+
+  UtilityHNS::UtilityH::GetTickCount(m_SensingTimer);
+  m_TrackedObjects.clear();
+  bTrackedObjects = true;
+
+  // Check frame id of the object is valid
+  std::string target_frame = objects_msg.object_array.header.frame_id;
+  int obj_idx = getIndex(tf_str_list_, target_frame);
+  if(obj_idx == -1){      
+    std::cout<<target_frame<<std::endl;
+    ROS_ERROR("Cannot find index by frame id");      
+    exit(0);
+  }  
+
+  autoware_msgs::DetectedObjectArray msg; 
+  if(target_frame != "velodyne"){
+    msg = TrasformObjAryToVeldoyne(objects_msg.object_array, transform_list_[obj_idx]);  
+  }
+  else{
+    msg = objects_msg.object_array;
+  }
+  
+  PlannerHNS::DetectedObject obj;
+
+  for(unsigned int i = 0 ; i <msg.objects.size(); i++)
+  {
+    // if(msg->objects.at(i).id > 0)
+    // {
+      PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg.objects.at(i), obj);
+      m_TrackedObjects.push_back(obj);
+    // }
+  }
+  
+  if(bMap)
+  {
+    if(m_PredictBeh.m_bStepByStep && m_bGoNextStep)
+    {
+      m_bGoNextStep = false;
+      m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration,  m_Map);
+    }
+    else if(!m_PredictBeh.m_bStepByStep)
+    {
+      m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration,  m_Map);
+    }
+    object_msg_list_[obj_idx].objects.clear();
+    autoware_msgs::DetectedObject pred_obj;
+    for(unsigned int i = 0 ; i <m_PredictBeh.m_ParticleInfo_II.size(); i++)
+    {
+      PlannerHNS::ROSHelpers::ConvertFromOpenPlannerDetectedObjectToAutowareDetectedObject(m_PredictBeh.m_ParticleInfo_II.at(i)->obj, false, pred_obj);
+      if(m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track)
+        pred_obj.behavior_state = m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track->best_beh;
+      // pred_obj = TransformObjToVeldoyne(pred_obj, transform_list_[obj_idx]);
+      object_msg_list_[obj_idx].objects.push_back(pred_obj);
+    }
+    
+    if(m_bEnableCurbObstacles)
+    {
+      curr_curbs_obstacles.clear();
+      GenerateCurbsObstacles(curr_curbs_obstacles);
+      //std::cout << "Curbs No: " << curr_curbs_obstacles.size() << endl;
+      for(unsigned int i = 0 ; i <curr_curbs_obstacles.size(); i++)
+      {
+        PlannerHNS::ROSHelpers::ConvertFromOpenPlannerDetectedObjectToAutowareDetectedObject(curr_curbs_obstacles.at(i), false, pred_obj);
+        // pred_obj = TransformObjToVeldoyne(pred_obj, transform_list_[obj_idx]);
+        object_msg_list_[obj_idx].objects.push_back(pred_obj);
+      }
+    }
+
+    m_PredictedResultsResults.objects.clear();
+
+    for(auto it1 = object_msg_list_.begin(); it1 != object_msg_list_.end(); ++it1){
+  
+      auto object_msg_entry = *it1;
+      for(auto it2 = object_msg_entry.objects.begin(); it2 != object_msg_entry.objects.end(); ++it2){
+        auto object_entry = *it2;
+        object_entry.header.frame_id = "velodyne";
+        object_entry.convex_hull.header.frame_id = "velodyne";
+        object_entry.valid=true;
+        m_PredictedResultsResults.objects.push_back(object_entry);
+      }
+    }
+
+    m_PredictedResultsResults.header.stamp = ros::Time().now();
+    m_PredictedResultsResults.header.frame_id = "velodyne";
+
+    rubis_msgs::DetectedObjectArray output_msg;
+    output_msg.instance = rubis::instance_;
+    output_msg.lidar_instance = rubis::lidar_instance_;
+    output_msg.object_array = m_PredictedResultsResults;
+    pub_rubis_predicted_objects_trajectories.publish(output_msg);
+    
   }
 }
 
@@ -552,99 +716,29 @@ void MotionPrediction::MainLoop()
 
   ros::NodeHandle private_nh("~");
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
+  // Scheduling & Profiling Setup
+  std::string node_name = ros::this_node::getName();
   std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
-
-  private_nh.param<int>("/op_motion_predictor/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/op_motion_predictor/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/op_motion_predictor/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_motion_predictor.csv");
-  private_nh.param<int>("/op_motion_predictor/rate", rate, 10);
-  private_nh.param("/op_motion_predictor/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/op_motion_predictor/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/op_motion_predictor/task_relative_deadline", task_relative_deadline, (double)10);
-
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-  ros::Rate loop_rate(rate);
-  if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25);
-
-  while (ros::ok())
-  {
-    if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){
-      if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-      rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-    }
-
-    ros::spinOnce();
-
-    if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap)
-    {
-      bMap = true;
-      PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map);
-    }
-    else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap)
-    {
-      bMap = true;
-      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true);
-    }
-    else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap)
-    {
-      std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;;
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_motion_predictor.csv");
 
-      if(m_MapRaw.GetVersion()==2)
-      {
-        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
-            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
-            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
-            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
-            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,
-            m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, true);
-
-        if(m_Map.roadSegments.size() > 0)
-        {
-          bMap = true;
-          std::cout << " ******* Map V2 Is Loaded successfully from the Motion Predictor !! " << std::endl;
-        }
-      }
-      else if(m_MapRaw.GetVersion()==1)
-      {
-        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
-            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
-            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
-            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
-            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,  PlannerHNS::GPSPoint(), m_Map, true);
-
-        if(m_Map.roadSegments.size() > 0)
-        {
-          bMap = true;
-          std::cout << " ******* Map V1 Is Loaded successfully from the Motion Predictor !! " << std::endl;
-        }
-      }
-    }
-
-    if(UtilityHNS::UtilityH::GetTimeDiffNow(m_VisualizationTimer) > m_VisualizationTime)
-    {
-      VisualizePrediction();
-      UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer);
-    }
+  int rate;
+  private_nh.param<int>(node_name+"/rate", rate, 10);
 
-    if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
 
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){
-      if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-      rubis::sched::task_state_ = TASK_STATE_READY;
-    }
+  rubis::init_task_profiling(task_response_time_filename);
 
-    loop_rate.sleep();
-  }
+  ros::spin();
 }
 
 void MotionPrediction::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform)
@@ -810,4 +904,75 @@ void MotionPrediction::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg)
     m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg);
 }
 
+void MotionPrediction::callbackSvl(const rubis_msgs::DetectedObjectArray::ConstPtr& objects_msg, const rubis_msgs::PoseTwistStamped::ConstPtr& pose_twist_msg){
+  rubis::start_task_profiling();
+
+  // Update pose and velocity
+  m_CurrentPos = PlannerHNS::WayPoint(pose_twist_msg->pose.pose.position.x, pose_twist_msg->pose.pose.position.y, pose_twist_msg->pose.pose.position.z, tf::getYaw(pose_twist_msg->pose.pose.orientation));
+  bNewCurrentPos = true;
+
+  m_VehicleStatus.speed = pose_twist_msg->twist.twist.linear.x;
+  m_CurrentPos.v = m_VehicleStatus.speed;
+  if(fabs(pose_twist_msg->twist.twist.linear.x) > 0.25)
+    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * pose_twist_msg->twist.twist.angular.z/pose_twist_msg->twist.twist.linear.x);
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+
+  // Process objects
+  rubis_msgs::DetectedObjectArray msg = *objects_msg;
+  _callbackGetRubisTrackedObjects(msg);
+
+  if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap)
+  {
+    bMap = true;
+    PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map);
+  }
+  else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap)
+  {
+    bMap = true;
+    PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true);
+  }
+  else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap)
+  {
+    std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;;
+
+    if(m_MapRaw.GetVersion()==2)
+    {
+      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
+          m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
+          m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
+          m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+          m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,
+          m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, true);
+
+      if(m_Map.roadSegments.size() > 0)
+      {
+        bMap = true;
+        std::cout << " ******* Map V2 Is Loaded successfully from the Motion Predictor !! " << std::endl;
+      }
+    }
+    else if(m_MapRaw.GetVersion()==1)
+    {
+      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
+          m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
+          m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
+          m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
+          m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,  PlannerHNS::GPSPoint(), m_Map, true);
+
+      if(m_Map.roadSegments.size() > 0)
+      {
+        bMap = true;
+        std::cout << " ******* Map V1 Is Loaded successfully from the Motion Predictor !! " << std::endl;
+      }
+    }
+  }
+
+  if(UtilityHNS::UtilityH::GetTimeDiffNow(m_VisualizationTimer) > m_VisualizationTime)
+  {
+    VisualizePrediction();
+    UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer);
+  }
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+}
+
 }
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp
index 466b858e..95f6caa3 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp
@@ -15,600 +15,532 @@
  */
 
 #include "op_trajectory_evaluator_core.h"
-#include "op_ros_helpers/op_ROSHelpers.h"
 #include "op_planner/MappingHelpers.h"
+#include "op_ros_helpers/op_ROSHelpers.h"
 #include <rubis_lib/sched.hpp>
 
-namespace TrajectoryEvaluatorNS
-{
-
-TrajectoryEval::TrajectoryEval()
-{
-  bNewCurrentPos = false;
-  bVehicleStatus = false;
-  bWayGlobalPath = false;
-  bWayGlobalPathToUse = false;
-  m_bUseMoveingObjectsPrediction = false;
-  m_noVehicleCnt = 0;
-
-  ros::NodeHandle _nh;
-  UpdatePlanningParams(_nh);
-
-  tf::StampedTransform transform;
-  PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
-  m_OriginPos.position.x  = transform.getOrigin().x();
-  m_OriginPos.position.y  = transform.getOrigin().y();
-  m_OriginPos.position.z  = transform.getOrigin().z();
-
-  pub_CollisionPointsRviz = nh.advertise<visualization_msgs::MarkerArray>("dynamic_collision_points_rviz", 1);
-  pub_LocalWeightedTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_eval_rviz", 1);
-  pub_LocalWeightedTrajectories = nh.advertise<autoware_msgs::LaneArray>("local_weighted_trajectories", 1);
-  pub_TrajectoryCost = nh.advertise<autoware_msgs::Lane>("local_trajectory_cost", 1);
-  pub_SafetyBorderRviz = nh.advertise<visualization_msgs::Marker>("safety_border", 1);
-  pub_DistanceToPedestrian = nh.advertise<std_msgs::Float64>("distance_to_pedestrian", 1);
-  pub_IntersectionCondition = nh.advertise<autoware_msgs::IntersectionCondition>("intersection_condition", 1);
-  pub_SprintSwitch = nh.advertise<std_msgs::Bool>("sprint_switch", 1);
-
-  sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this);
-  sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this);
-
-  int bVelSource = 1;
-  _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
-  if(bVelSource == 0)
-    sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this);
-  else if(bVelSource == 1)
-    sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this);
-  else if(bVelSource == 2)
-    sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this);
-
-  /*  RT Scheduling setup  */
-  // sub_current_pose = nh.subscribe("/current_pose", 1, &TrajectoryEval::callbackGetCurrentPose, this); //origin 10
-  // sub_current_state = nh.subscribe("/current_state", 1, &TrajectoryEval::callbackGetCurrentState, this); //origin 10
-
-  // int bVelSource = 1;
-  // _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
-  // if(bVelSource == 0)
-  //   sub_robot_odom = nh.subscribe("/odom", 1, &TrajectoryEval::callbackGetRobotOdom, this); //origin 10
-  // else if(bVelSource == 1)
-  //   sub_current_velocity = nh.subscribe("/current_velocity", 1, &TrajectoryEval::callbackGetVehicleStatus, this); //origin 10
-  // else if(bVelSource == 2)
-  //   sub_can_info = nh.subscribe("/can_info", 1, &TrajectoryEval::callbackGetCANInfo, this); //origin 10
-
-  sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this);
-  sub_LocalPlannerPaths = nh.subscribe("/local_trajectories", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this);
-  sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this);
-  sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this);
-
-  PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy);
-
-  while(1){
-    if(UpdateTf() == true)
-      break;
-  }
-}
+namespace TrajectoryEvaluatorNS {
 
-TrajectoryEval::~TrajectoryEval()
-{
+TrajectoryEval::TrajectoryEval() {
+    bNewCurrentPos = false;
+    bVehicleStatus = false;
+    bWayGlobalPath = false;
+    bWayGlobalPathToUse = false;
+    m_bUseMoveingObjectsPrediction = false;
+    m_noVehicleCnt = 0;
+    is_objects_updated_ = false;
+
+    ros::NodeHandle _nh;
+    UpdatePlanningParams(_nh);
+
+    tf::StampedTransform transform;
+    PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
+    m_OriginPos.position.x = transform.getOrigin().x();
+    m_OriginPos.position.y = transform.getOrigin().y();
+    m_OriginPos.position.z = transform.getOrigin().z();
+
+    pub_CollisionPointsRviz = nh.advertise<visualization_msgs::MarkerArray>("dynamic_collision_points_rviz", 1);
+    pub_LocalWeightedTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_eval_rviz", 1);
+    // pub_LocalWeightedTrajectories =
+    // nh.advertise<rubis_msgs::LaneArrayWithPoseTwist>("local_weighted_trajectories",
+    // 1); pub_LocalWeightedTrajectoriesWithPoseTwist =
+    // nh.advertise<rubis_msgs::LaneArrayWithPoseTwist>("local_weighted_trajectories_with_pose_twist",
+    // 1); pub_TrajectoryCost =
+    // nh.advertise<autoware_msgs::Lane>("local_trajectory_cost", 1);
+    pub_SafetyBorderRviz = nh.advertise<visualization_msgs::Marker>("safety_border", 1);
+    // pub_DistanceToPedestrian =
+    // nh.advertise<std_msgs::Float64>("distance_to_pedestrian", 1);
+    // pub_IntersectionCondition =
+    // nh.advertise<autoware_msgs::IntersectionCondition>("intersection_condition",
+    // 1); pub_SprintSwitch = nh.advertise<std_msgs::Bool>("sprint_switch", 1);
+    pub_PlanningInfo = nh.advertise<rubis_msgs::PlanningInfo>("planning_info", 10);
+
+    sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this);
+
+    PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy);
+
+    while (1) {
+        if (UpdateTf() == true)
+            break;
+    }
+
+    typedef message_filters::sync_policies::ExactTime<rubis_msgs::LaneArrayWithPoseTwist, rubis_msgs::DetectedObjectArray> SyncPolicy;
+    trajectories_sub_.subscribe(nh, "/local_trajectories_with_pose_twist", 200);
+    objects_sub_.subscribe(nh, "/detection/lidar_detector/rubis_objects_center", 200);
+
+    if (m_LaneTopic == "None") {
+        sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(50), trajectories_sub_, objects_sub_));
+        sync_->registerCallback(boost::bind(&TrajectoryEval::callback, this, _1, _2));
+    } else {
+        lane_sub_.subscribe(nh, m_LaneTopic, 10);
+        lane_sync_.reset(new message_filters::Synchronizer<LaneSyncPolicy>(LaneSyncPolicy(50), trajectories_sub_, objects_sub_, lane_sub_));
+        lane_sync_->registerCallback(boost::bind(&TrajectoryEval::callbackWithLane, this, _1, _2, _3));
+    }
 }
 
-void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh)
-{
-  _nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction);
-
-  _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
-  _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
-  _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
-  if(m_PlanningParams.enableSwerving)
-    m_PlanningParams.enableFollowing = true;
-  else
-    _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
-
-  _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
-  _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
-
-  _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
-  _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
-  _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
-
-  _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
-
-  _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
-  if(m_PlanningParams.enableSwerving)
-    _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
-  else
-    m_PlanningParams.rollOutNumber = 0;
-
-  std::cout << "Rolls Number: " << m_PlanningParams.rollOutNumber << std::endl;
-
-  _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
-  _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
-  _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
-  _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
-  _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
-
-  _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
-
-  _nh.getParam("/op_common_params/width", m_CarInfo.width);
-  _nh.getParam("/op_common_params/length", m_CarInfo.length);
-  _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
-  _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
-  _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
-  _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
-  _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
-  m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
-  m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
-
-  _nh.param("/op_trajectory_evaluator/PedestrianRightThreshold", m_PedestrianRightThreshold, 7.0);
-  _nh.param("/op_trajectory_evaluator/PedestrianLeftThreshold", m_PedestrianLeftThreshold, 2.0);
-  _nh.param("/op_trajectory_evaluator/PedestrianImageDetectionRange", m_PedestrianImageDetectionRange, 0.7);
-  _nh.param("/op_trajectory_evaluator/PedestrianStopImgHeightThreshold", m_pedestrian_stop_img_height_threshold, 120);
-  _nh.param("/op_trajectory_evaluator/ImageWidth", m_ImageWidth, 1920);
-  _nh.param("/op_trajectory_evaluator/ImageHeight", m_ImageHeight, 1080);
-  _nh.param("/op_trajectory_evaluator/VehicleImageDetectionRange", m_VehicleImageDetectionRange, 0.3);
-  _nh.param("/op_trajectory_evaluator/VehicleImageWidthThreshold", m_VehicleImageWidthThreshold, 0.05);
-  _nh.param("/op_trajectory_evaluator/SprintDecisionTime", m_SprintDecisionTime, 5.0);
-  
-  
-  m_VehicleImageWidthThreshold = m_VehicleImageWidthThreshold * m_ImageWidth;
-  m_PedestrianRightThreshold *= -1;
+TrajectoryEval::~TrajectoryEval() {}
 
-}
+void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle &_nh) {
+    _nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction);
 
-void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
-{
-  m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation));
-  bNewCurrentPos = true;
-}
+    _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
+    _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
+    _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
+    if (m_PlanningParams.enableSwerving)
+        m_PlanningParams.enableFollowing = true;
+    else
+        _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
 
-void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  m_VehicleStatus.speed = msg->twist.linear.x;
-  m_CurrentPos.v = m_VehicleStatus.speed;
-  if(fabs(msg->twist.linear.x) > 0.25)
-    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x);
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
+    _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
+    _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
 
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-}
+    _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
+    _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
+    _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
 
-void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
-{
-  m_VehicleStatus.speed = msg->speed/3.6;
-  m_CurrentPos.v = m_VehicleStatus.speed;
-  m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value;
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
-}
+    _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
 
-void TrajectoryEval::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
-{
-  m_VehicleStatus.speed = msg->twist.twist.linear.x;
-  m_CurrentPos.v = m_VehicleStatus.speed;
-  if(fabs(msg->twist.twist.linear.x) > 0.25)
-    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
+    _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
+    if (m_PlanningParams.enableSwerving)
+        _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
+    else
+        m_PlanningParams.rollOutNumber = 0;
+
+    std::cout << "Rolls Number: " << m_PlanningParams.rollOutNumber << std::endl;
+
+    _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
+    _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
+    _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
+    _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
+    _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
+
+    _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
+    _nh.getParam("/op_common_params/blockIdx", m_PlanningParams.blockIdx);
+
+    _nh.getParam("/op_common_params/width", m_CarInfo.width);
+    _nh.getParam("/op_common_params/length", m_CarInfo.length);
+    _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
+    _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
+    _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
+    _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
+    _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
+    m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
+    m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
+
+    _nh.param("/op_trajectory_evaluator/PedestrianRightThreshold", m_PedestrianRightThreshold, 7.0);
+    _nh.param("/op_trajectory_evaluator/PedestrianLeftThreshold", m_PedestrianLeftThreshold, 2.0);
+    _nh.param("/op_trajectory_evaluator/PedestrianImageDetectionRange", m_PedestrianImageDetectionRange, 0.7);
+    _nh.param("/op_trajectory_evaluator/PedestrianStopImgHeightThreshold", m_pedestrian_stop_img_height_threshold, 120);
+    _nh.param("/op_trajectory_evaluator/ImageWidth", m_ImageWidth, 1920);
+    _nh.param("/op_trajectory_evaluator/ImageHeight", m_ImageHeight, 1080);
+    _nh.param("/op_trajectory_evaluator/VehicleImageDetectionRange", m_VehicleImageDetectionRange, 0.3);
+    _nh.param("/op_trajectory_evaluator/VehicleImageWidthThreshold", m_VehicleImageWidthThreshold, 0.05);
+    _nh.param("/op_trajectory_evaluator/SprintDecisionTime", m_SprintDecisionTime, 5.0);
+
+    _nh.param("/op_trajectory_evaluator/laneTopic", m_LaneTopic, std::string("None"));
+
+    m_VehicleImageWidthThreshold = m_VehicleImageWidthThreshold * m_ImageWidth;
+    m_PedestrianRightThreshold *= -1;
+
+    // m_PlanningParams.enableStop = false;
 }
 
-void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
-{
-  if(msg->lanes.size() > 0)
-  {
+void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr &msg) {
+    if (msg->lanes.size() > 0) {
 
-    bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
+        bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
 
-    m_GlobalPaths.clear();
+        m_GlobalPaths.clear();
 
-    for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
-    {
-      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
+        for (unsigned int i = 0; i < msg->lanes.size(); i++) {
+            PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
 
-      PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
-      m_GlobalPaths.push_back(m_temp_path);
+            PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
+            m_GlobalPaths.push_back(m_temp_path);
 
-      if(bOldGlobalPath)
-      {
-        bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
-      }
-    }
+            if (bOldGlobalPath) {
+                bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
+            }
+        }
 
-    if(!bOldGlobalPath)
-    {
-      bWayGlobalPath = true;
-      std::cout << "Received New Global Path Evaluator! " << std::endl;
-    }
-    else
-    {
-      m_GlobalPaths.clear();
+        if (!bOldGlobalPath) {
+            bWayGlobalPath = true;
+            std::cout << "Received New Global Path Evaluator! " << std::endl;
+        } else {
+            m_GlobalPaths.clear();
+        }
     }
-  }
 }
 
-void TrajectoryEval::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
-{
-  if(msg->lanes.size() > 0)
-  {
-    m_GeneratedRollOuts.clear();
-    int globalPathId_roll_outs = -1;
-
-    for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
-    {
-      std::vector<PlannerHNS::WayPoint> path;
-      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path);
-      m_GeneratedRollOuts.push_back(path);
-      if(path.size() > 0)
-        globalPathId_roll_outs = path.at(0).gid;
-    }
+void TrajectoryEval::_callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr &msg) {
+    rubis_msgs::PlanningInfo planning_info_msg;
+    planning_info_msg.header = msg->header;
+    planning_info_msg.instance = rubis::instance_;
+    planning_info_msg.lidar_instance = rubis::lidar_instance_;
+
+    // Before spin
+    UpdateMyParams();
+    UpdateTf();
+
+    static double prev_x = 0.0, prev_y = 0.0, prev_speed = 0.0;
 
-    if(bWayGlobalPath && m_GlobalPaths.size() > 0 && m_GlobalPaths.at(0).size() > 0)
-    {
-      int globalPathId = m_GlobalPaths.at(0).at(0).gid;
-      std::cout << "Before Synchronization At Trajectory Evaluator: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
-
-      if(globalPathId_roll_outs == globalPathId)
-      {
-        bWayGlobalPath = false;
-        m_GlobalPathsToUse = m_GlobalPaths;
-        std::cout << "Synchronization At Trajectory Evaluator: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
-      }
+    // callback for current pose
+    if (prev_x != msg->pose.pose.position.x || prev_y != msg->pose.pose.position.y) {
+        m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z,
+                                            tf::getYaw(msg->pose.pose.orientation));
+        bNewCurrentPos = true;
+        prev_x = msg->pose.pose.position.x;
+        prev_y = msg->pose.pose.position.y;
     }
 
-    bRollOuts = true;
-  }
-}
+    // callback for vehicle status
+    if (prev_speed != msg->twist.twist.linear.x) {
+        m_VehicleStatus.speed = msg->twist.twist.linear.x;
+        m_CurrentPos.v = m_VehicleStatus.speed;
+        if (fabs(msg->twist.twist.linear.x) > 0.25)
+            m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z / msg->twist.twist.linear.x);
+        UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+        bVehicleStatus = true;
+        prev_speed = msg->twist.twist.linear.x;
+    }
 
-void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg)
-{  
-  m_PredictedObjects.clear();
-  bPredictedObjects = true;
-  double distance_to_pedestrian = 1000;
-  int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2;
-  int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2;
-  
-  int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2;
-  int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2;
-
-  int vehicle_cnt = 0;
-
-  PlannerHNS::DetectedObject obj;
-  for(unsigned int i = 0 ; i <msg->objects.size(); i++)
-  {    
-    if(msg->objects.at(i).pose.position.y < -20 || msg->objects.at(i).pose.position.y > 20)
-      continue;    
-      
-    if(msg->objects.at(i).pose.position.z > 1 || msg->objects.at(i).pose.position.z < -1.5)
-      continue;
-
-    autoware_msgs::DetectedObject msg_obj = msg->objects.at(i);     
-
-    // #### Decison making for objects
-    
-    if(msg_obj.id > 0) // If fusion object is detected
-    {
-      // calculate distance to person first
-      // if(msg_obj.label == "person"){        
-      //   std::cout<<"Pedestrian box size(width x height):"<<msg_obj.width<<" "<<msg_obj.height<<std::endl;
-      //   geometry_msgs::PoseStamped pose;
-      //   pose.header = msg_obj.header;
-      //   pose.pose = msg_obj.pose;
-      //   try{
-      //     m_vtob_listener.transformPose("/base_link", pose, pose);
-      //     double temp_x_distance = pose.pose.position.x;
-      //     double temp_y_distance = pose.pose.position.y;
-      //     // y-axis: Left + / Right -          
-      //     if(temp_y_distance > m_PedestrianLeftThreshold || temp_y_distance < m_PedestrianRightThreshold ) continue;
-      //     if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance;          
-      //   }
-      //   catch(tf::TransformException& ex){
-      //     // ROS_ERROR("Cannot transform person pose: %s", ex.what());
-
-      //   }
-      // }
-
-      if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){
-        vehicle_cnt += 1;
-      }
-
-      PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj);
-
-
-      // transform center pose into map frame
-      geometry_msgs::PoseStamped pose_in_map;
-      pose_in_map.header = msg_obj.header;
-      pose_in_map.pose = msg_obj.pose;
-      try{
-        m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map);
-      }
-      catch(tf::TransformException& ex)
-      {
-        // ROS_ERROR("Cannot transform object pose: %s", ex.what());
-        continue;
-      }
-      // msg_obj.header.frame_id = "map";
-      obj.center.pos.x = pose_in_map.pose.position.x;
-      obj.center.pos.y = pose_in_map.pose.position.y;
-      obj.center.pos.z = pose_in_map.pose.position.z;
-
-      // transform contour into map frame
-      for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){
-        geometry_msgs::PoseStamped contour_point_in_map;
-        contour_point_in_map.header = msg_obj.header;
-        contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x;
-        contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y;
-        contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z;
-
-        // For resolve TF malform, set orientation w to 1
-        contour_point_in_map.pose.orientation.w = 1;
-
-        try{
-          m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map);
+    // callback for local planner path
+    if (msg->lane_array.lanes.size() > 0) {
+        m_GeneratedRollOuts.clear();
+        int globalPathId_roll_outs = -1;
+
+        for (unsigned int i = 0; i < msg->lane_array.lanes.size(); i++) {
+            std::vector<PlannerHNS::WayPoint> path;
+            PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path);
+            m_GeneratedRollOuts.push_back(path);
+            if (path.size() > 0)
+                globalPathId_roll_outs = path.at(0).gid;
         }
-        catch(tf::TransformException& ex){
-          // ROS_ERROR("Cannot transform contour pose: %s", ex.what());
-          continue;
+
+        if (bWayGlobalPath && m_GlobalPaths.size() > 0 && m_GlobalPaths.at(0).size() > 0) {
+            int globalPathId = m_GlobalPaths.at(0).at(0).gid;
+            std::cout << "Before Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs
+                      << std::endl;
+
+            if (globalPathId_roll_outs == globalPathId) {
+                bWayGlobalPath = false;
+                m_GlobalPathsToUse = m_GlobalPaths;
+                std::cout << "Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs
+                          << std::endl;
+            }
         }
 
-        obj.contour.at(j).x = contour_point_in_map.pose.position.x;
-        obj.contour.at(j).y = contour_point_in_map.pose.position.y;
-        obj.contour.at(j).z = contour_point_in_map.pose.position.z;
-      }
+        bRollOuts = true;
+    }
 
-      msg_obj.header.frame_id = "map";
+    // After spin
+    PlannerHNS::TrajectoryCost tc;
 
-      m_PredictedObjects.push_back(obj);
-    }
-    /*
-    else{ // If object is only detected at vision
-      int image_obj_center_x = msg_obj.x+msg_obj.width/2;
-      int image_obj_center_y = msg_obj.y+msg_obj.height/2;
-      // if (msg_obj.label == "person"){// If person is detected only in image
-      //   // TO ERASE
-      //   std::cout<<"object height:" << msg_obj.height << " / threshold:" << m_pedestrian_stop_img_height_threshold << std::endl;
-      //   if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ 
-      //     double temp_x_distance = 1000;
-      //     if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10;
-      //     if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance;
-      //   }
-      // }                    
-      // else 
-      if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){            
-        if((msg_obj.width > m_VehicleImageWidthThreshold) 
-              && (image_obj_center_x > image_vehicle_detection_range_left) 
-              && (image_obj_center_x < image_vehicle_detection_range_right)
-        )
-        {          
-          vehicle_cnt+=1;        
+    if (bNewCurrentPos && m_GlobalPaths.size() > 0) {
+        m_GlobalPathSections.clear();
+
+        for (unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) {
+            t_centerTrajectorySmoothed.clear();
+            PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(
+                m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance, m_PlanningParams.pathDensity, t_centerTrajectorySmoothed);
+            m_GlobalPathSections.push_back(t_centerTrajectorySmoothed);
         }
-      }
-    }
-    */
-
-    int image_obj_center_x = msg_obj.x+msg_obj.width/2;
-    int image_obj_center_y = msg_obj.y+msg_obj.height/2;
-    if (msg_obj.label == "person"){// If person is detected only in image
-      
-      // TO ERASE
-      // ROS_WARN("object height:%d // thr: %d\n", msg_obj.height, m_pedestrian_stop_img_height_threshold);
-      printf("center_x %d \n left: %d \n right %d\n\n\n", image_obj_center_x, image_person_detection_range_left, image_person_detection_range_right);
-      if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ 
-        double temp_x_distance = 1000;
-        if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10;
-        if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance;
-      }
-    }
-    
-  }
 
-  // Publish Sprint Switch
-  std_msgs::Bool sprint_switch_msg;
+        autoware_msgs::IntersectionCondition intersection_condition;
+        intersection_condition.header = msg->header;
+
+        if (m_GlobalPathSections.size() > 0) {
+            tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams,
+                                                             m_CarInfo, m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state);
+
+            autoware_msgs::Lane l;
+            l.closest_object_distance = tc.closest_obj_distance;
+            l.closest_object_velocity = tc.closest_obj_velocity;
+            l.cost = tc.cost;
+            l.is_blocked = tc.bBlocked;
+            l.lane_index = tc.index;
+            planning_info_msg.trajectory_cost = l;
+
+            // hjw added : Check if ego is on intersection and obstacles are in
+            // risky area
+            int intersectionID = -1;
+            double closestIntersectionDistance = -1;
+            bool isInsideIntersection = false;
+            bool riskyLeftTurn = false;
+            bool riskyRightTurn = false;
+
+            PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list_, m_PredictedObjects, intersectionID,
+                                                                  closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn);
+
+            intersection_condition.intersectionID = intersectionID;
+            intersection_condition.intersectionDistance = closestIntersectionDistance;
+            intersection_condition.isIntersection = isInsideIntersection;
+            intersection_condition.riskyLeftTurn = riskyLeftTurn;
+            intersection_condition.riskyRightTurn = riskyRightTurn;
+        }
 
-  if(vehicle_cnt != 0){
-    m_noVehicleCnt = 0;
-    sprint_switch_msg.data = false;
-  }
-  else{ // No vehicle is exist in front of the car
-    if(m_noVehicleCnt < m_SprintDecisionTime*10) {
-      m_noVehicleCnt +=1;
-      sprint_switch_msg.data = false;
+        if (m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) {
+            for (unsigned int i = 0; i < m_GeneratedRollOuts.size(); i++) {
+                autoware_msgs::Lane lane;
+                PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane);
+                lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance;
+                lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity;
+                lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost;
+                lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked;
+                lane.lane_index = i;
+                planning_info_msg.lane_array.lanes.push_back(lane);
+            }
+
+            planning_info_msg.pose = msg->pose;
+            planning_info_msg.twist = msg->twist;
+            planning_info_msg.sprint_switch = m_sprint_switch;
+            planning_info_msg.intersection_condition = intersection_condition;
+
+            if (m_PlanningParams.enableStop) {
+                m_distance_to_pedestrian.data = 1.0;
+            }
+
+            planning_info_msg.distance_to_pedestrian = m_distance_to_pedestrian;
+
+            pub_PlanningInfo.publish(planning_info_msg);
+        } else {
+            ROS_ERROR("m_TrajectoryCosts.size() Not Equal "
+                      "m_GeneratedRollOuts.size()");
+        }
+
+        if (m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() > 0) {
+            visualization_msgs::MarkerArray all_rollOuts;
+            PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts,
+                                                                 m_CurrentBehavior.iTrajectory, all_rollOuts);
+            pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts);
+
+            PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual,
+                                                                  m_CollisionsDummy);
+            pub_CollisionPointsRviz.publish(m_CollisionsActual);
+
+            // Visualize Safety Box
+            visualization_msgs::Marker safety_box;
+            PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box);
+            pub_SafetyBorderRviz.publish(safety_box);
+        }
+    } else {
+        sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this);
     }
-    else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true;
-  }  
-  pub_SprintSwitch.publish(sprint_switch_msg);
-
-  // ROS_INFO("object # : %d", m_PredictedObjects.size());
-  
-  std_msgs::Float64 distanceToPedestrianMsg; 
-  distanceToPedestrianMsg.data = distance_to_pedestrian;
-  pub_DistanceToPedestrian.publish(distanceToPedestrianMsg);
 }
 
-void TrajectoryEval::callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  m_CurrentBehavior.iTrajectory = msg->twist.angular.z;
+void TrajectoryEval::callbackGetPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr &msg) {
+    object_msg_ = msg->object_array;
+    is_objects_updated_ = true;
+    // _callbackGetPredictedObjects(object_msg_);
 }
 
-void TrajectoryEval::callbackGetCurrentState(const std_msgs::Int32 & msg)
-{
-  m_CurrentBehavior.state = static_cast<PlannerHNS::STATE_TYPE>(msg.data);
-}
+void TrajectoryEval::_callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArray &objects_msg) {
+    m_PredictedObjects.clear();
+    // ROS_WARN("callbackGetPredictedObjects Called");
+    bPredictedObjects = true;
+    int image_person_detection_range_left = m_ImageWidth / 2 - m_ImageWidth * m_PedestrianImageDetectionRange / 2;
+    int image_person_detection_range_right = m_ImageWidth / 2 + m_ImageWidth * m_PedestrianImageDetectionRange / 2;
+    int image_vehicle_detection_range_left = m_ImageWidth / 2 - m_ImageWidth * m_VehicleImageDetectionRange / 2;
+    int image_vehicle_detection_range_right = m_ImageWidth / 2 + m_ImageWidth * m_VehicleImageDetectionRange / 2;
+    int vehicle_cnt = 0;
+    double distance_to_pedestrian = 1000;
+
+    PlannerHNS::DetectedObject obj;
+    for (unsigned int i = 0; i < objects_msg.objects.size(); i++) {
+        if (objects_msg.objects.at(i).pose.position.y < -20 || objects_msg.objects.at(i).pose.position.y > 20)
+            continue;
+
+        if (objects_msg.objects.at(i).pose.position.z > 1 || objects_msg.objects.at(i).pose.position.z < -1.5)
+            continue;
+
+        autoware_msgs::DetectedObject msg_obj = objects_msg.objects.at(i);
+
+        if (msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus") {
+            vehicle_cnt += 1;
+        }
 
-void TrajectoryEval::UpdateMyParams()
-{
-  ros::NodeHandle _nh;
-  _nh.getParam("/op_trajectory_evaluator/weightPriority", m_PlanningParams.weightPriority);
-  _nh.getParam("/op_trajectory_evaluator/weightTransition", m_PlanningParams.weightTransition);
-  _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong);
-  _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat);
-  _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance);
-}
+        PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(objects_msg.objects.at(i), obj);
+        geometry_msgs::PoseStamped pose_in_map;
+        pose_in_map.header = msg_obj.header;
+        pose_in_map.pose = msg_obj.pose;
+        while (1) {
+            try {
+                m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map);
+                break;
+            } catch (tf::TransformException &ex) {
+                // ROS_ERROR("Cannot transform object pose: %s", ex.what());
+                continue;
+            }
+        }
+        // msg_obj.header.frame_id = "map";
+        obj.center.pos.x = pose_in_map.pose.position.x;
+        obj.center.pos.y = pose_in_map.pose.position.y;
+        obj.center.pos.z = pose_in_map.pose.position.z;
+
+        // transform contour into map frame
+        for (unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++) {
+            geometry_msgs::PoseStamped contour_point_in_map;
+            contour_point_in_map.header = msg_obj.header;
+            contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x;
+            contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y;
+            contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z;
+
+            // For resolve TF malform, set orientation w to 1
+            contour_point_in_map.pose.orientation.w = 1;
+
+            for (int i = 0; i < 1000; i++) {
+                try {
+                    m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map);
+                    break;
+                } catch (tf::TransformException &ex) {
+                    // ROS_ERROR("Cannot transform contour pose: %s",
+                    // ex.what());
+                    continue;
+                }
+            }
+
+            obj.contour.at(j).x = contour_point_in_map.pose.position.x;
+            obj.contour.at(j).y = contour_point_in_map.pose.position.y;
+            obj.contour.at(j).z = contour_point_in_map.pose.position.z;
+        }
+
+        msg_obj.header.frame_id = "map";
+
+        m_PredictedObjects.push_back(obj);
+
+        int image_obj_center_x = msg_obj.x + msg_obj.width / 2;
+        int image_obj_center_y = msg_obj.y + msg_obj.height / 2;
+        if (msg_obj.label == "person") { // If person is detected only in image
+            ROS_WARN("==========================================");
+            ROS_WARN("person detected!");
+            ROS_WARN("==========================================");
+            if (image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right) {
+                double temp_x_distance = 1000;
+                if (msg_obj.height >= m_pedestrian_stop_img_height_threshold)
+                    temp_x_distance = 10;
+                if (abs(temp_x_distance) < abs(distance_to_pedestrian))
+                    distance_to_pedestrian = temp_x_distance;
+            }
+            ROS_WARN("==========================================");
+            ROS_WARN("distance_to_pedestrian: %lf", distance_to_pedestrian);
+            ROS_WARN("==========================================");
+        }
+    }
 
-bool TrajectoryEval::UpdateTf()
-{
-  try{
-    m_vtob_listener.waitForTransform("/velodyne", "/base_link", ros::Time(0), ros::Duration(0.001));
-    m_vtob_listener.lookupTransform("/velodyne", "/base_link", ros::Time(0), m_velodyne_to_base_link);
-
-    m_vtom_listener.waitForTransform("/velodyne", "/map", ros::Time(0), ros::Duration(0.001));
-    m_vtom_listener.lookupTransform("/velodyne", "/map", ros::Time(0), m_velodyne_to_map);
-    return true;
-  }
-  catch(tf::TransformException& ex){
-    if(TF_DEBUG)
-      ROS_ERROR("%s", ex.what());
-    return false;
-  }
+    m_distance_to_pedestrian.data = distance_to_pedestrian;
+
+    if (vehicle_cnt != 0) {
+        m_noVehicleCnt = 0;
+        m_sprint_switch.data = false;
+    } else { // No vehicle is exist in front of the car
+        if (m_noVehicleCnt < m_SprintDecisionTime * 10) {
+            m_noVehicleCnt += 1;
+            m_sprint_switch.data = false;
+        } else if (m_noVehicleCnt >= 5)
+            m_sprint_switch.data = true;
+    }
 }
 
-void TrajectoryEval::MainLoop()
-{
-  ros::NodeHandle private_nh("~");
+void TrajectoryEval::callback(const rubis_msgs::LaneArrayWithPoseTwist::ConstPtr &trajectories_msg,
+                              const rubis_msgs::DetectedObjectArray::ConstPtr &objects_msg) {
+    rubis::start_task_profiling();
+
+    rubis::instance_ = trajectories_msg->instance;
+    rubis::lidar_instance_ = objects_msg->lidar_instance;
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
+    _callbackGetPredictedObjects(objects_msg->object_array);
 
-  private_nh.param<int>("/op_trajectory_evaluator/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/op_trajectory_evaluator/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/op_trajectory_evaluator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv");
-  private_nh.param<int>("/op_trajectory_evaluator/rate", rate, 10);
-  private_nh.param("/op_trajectory_evaluator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/op_trajectory_evaluator/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/op_trajectory_evaluator/task_relative_deadline", task_relative_deadline, (double)10);  
+    rubis_msgs::LaneArrayWithPoseTwist::ConstPtr input = boost::make_shared<const rubis_msgs::LaneArrayWithPoseTwist>(*trajectories_msg);
+    _callbackGetLocalPlannerPath(input);
 
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+}
+
+void TrajectoryEval::callbackWithLane(const rubis_msgs::LaneArrayWithPoseTwist::ConstPtr &trajectories_msg,
+                                      const rubis_msgs::DetectedObjectArray::ConstPtr &objects_msg, const rubis_msgs::Bool::ConstPtr &lane_msg) {
+    rubis::start_task_profiling();
 
-  PlannerHNS::WayPoint prevState, state_change;
+    rubis::instance_ = trajectories_msg->instance;
+    rubis::lidar_instance_ = objects_msg->lidar_instance;
+    rubis::vision_instance_ = lane_msg->instance;
 
-  // Add Crossing Info from yaml file
-  XmlRpc::XmlRpcValue intersection_xml;
-  std::vector<PlannerHNS::Crossing> intersection_list;
-  nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml);
-  PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list, intersection_xml);
+    _callbackGetPredictedObjects(objects_msg->object_array);
 
-  
+    rubis_msgs::LaneArrayWithPoseTwist::ConstPtr input = boost::make_shared<const rubis_msgs::LaneArrayWithPoseTwist>(*trajectories_msg);
+    _callbackGetLocalPlannerPath(input);
 
-  ros::Rate loop_rate(rate);
-  if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(100);
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+}
 
-  struct timespec start_time, end_time;
+void TrajectoryEval::callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr &msg) {
+    m_CurrentBehavior.iTrajectory = msg->twist.angular.z;
+}
 
-  while (ros::ok())
-  {
-    if(task_profiling_flag) rubis::sched::start_task_profiling();
+void TrajectoryEval::callbackGetCurrentState(const std_msgs::Int32 &msg) { m_CurrentBehavior.state = static_cast<PlannerHNS::STATE_TYPE>(msg.data); }
+
+void TrajectoryEval::UpdateMyParams() {
+    ros::NodeHandle _nh;
+    _nh.getParam("/op_trajectory_evaluator/weightPriority", m_PlanningParams.weightPriority);
+    _nh.getParam("/op_trajectory_evaluator/weightTransition", m_PlanningParams.weightTransition);
+    _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong);
+    _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat);
+    _nh.param("/op_trajectory_evaluator/blockThreshold", m_PlanningParams.blockThreshold, 2.0);
+    _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance);
+
+    _nh.getParam("/op_trajectory_evaluator/lateralBlockingThreshold", m_PlanningParams.lateralBlockingThreshold);
+    _nh.getParam("/op_trajectory_evaluator/frontLongitudinalBlockingThreshold", m_PlanningParams.frontLongitudinalBlockingThreshold);
+    _nh.getParam("/op_trajectory_evaluator/rearLongitudinalBlockingThreshold", m_PlanningParams.rearLongitudinalBlockingThreshold);
+    _nh.getParam("/op_trajectory_evaluator/enableDebug", m_PlanningParams.enableDebug);
+}
 
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){
-      if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-      rubis::sched::task_state_ = TASK_STATE_RUNNING;     
+bool TrajectoryEval::UpdateTf() {
+    try {
+        m_vtob_listener.waitForTransform("/velodyne", "/base_link", ros::Time(0), ros::Duration(0.001));
+        m_vtob_listener.lookupTransform("/velodyne", "/base_link", ros::Time(0), m_velodyne_to_base_link);
+
+        m_vtom_listener.waitForTransform("/velodyne", "/map", ros::Time(0), ros::Duration(0.001));
+        m_vtom_listener.lookupTransform("/velodyne", "/map", ros::Time(0), m_velodyne_to_map);
+        return true;
+    } catch (tf::TransformException &ex) {
+        if (TF_DEBUG)
+            ROS_ERROR("%s", ex.what());
+        return false;
     }
+}
 
-    UpdateMyParams();
-    UpdateTf();
-    
+void TrajectoryEval::MainLoop() {
+    ros::NodeHandle private_nh("~");
 
-    ros::spinOnce();
-    PlannerHNS::TrajectoryCost tc;
+    // Scheduling & Profiling Setup
+    std::string node_name = ros::this_node::getName();
+    std::string task_response_time_filename;
+    private_nh.param<std::string>(node_name + "/task_response_time_filename", task_response_time_filename,
+                                  "~/Documents/profiling/response_time/op_trajectory_evaluator.csv");
 
-    if(bNewCurrentPos && m_GlobalPaths.size()>0)
-    {
-      m_GlobalPathSections.clear();
-
-      for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++)
-      {
-        t_centerTrajectorySmoothed.clear();
-        PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed);
-        m_GlobalPathSections.push_back(t_centerTrajectorySmoothed);
-      }
-
-      if(m_GlobalPathSections.size()>0)
-      {
-        if(m_bUseMoveingObjectsPrediction)
-          tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams,  m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory);
-        else
-          tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,  m_PlanningParams,  m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state);
-
-        autoware_msgs::Lane l;
-        l.closest_object_distance = tc.closest_obj_distance;
-        l.closest_object_velocity = tc.closest_obj_velocity;
-        l.cost = tc.cost;
-        l.is_blocked = tc.bBlocked;
-        l.lane_index = tc.index;
-        pub_TrajectoryCost.publish(l);
-
-        // hjw added : Check if ego is on intersection and obstacles are in risky area 
-        int intersectionID = -1;
-        double closestIntersectionDistance = -1;
-        bool isInsideIntersection = false;
-        bool riskyLeftTurn = false;
-        bool riskyRightTurn = false;
-
-        PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn);
-
-        autoware_msgs::IntersectionCondition ic_msg;
-        ic_msg.intersectionID = intersectionID;
-        ic_msg.intersectionDistance = closestIntersectionDistance;
-        ic_msg.isIntersection = isInsideIntersection;
-        ic_msg.riskyLeftTurn = riskyLeftTurn;
-        ic_msg.riskyRightTurn = riskyRightTurn;
-        
-        pub_IntersectionCondition.publish(ic_msg);
-
-      }
-
-      if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size())
-      {
-        autoware_msgs::LaneArray local_lanes;
-        for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++)
-        {
-          autoware_msgs::Lane lane;
-          PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane);
-          lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance;
-          lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity;
-          lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost;
-          lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked;
-          lane.lane_index = i;
-          local_lanes.lanes.push_back(lane);
-        }
+    int rate;
+    private_nh.param<int>(node_name + "/rate", rate, 10);
 
-        pub_LocalWeightedTrajectories.publish(local_lanes);
-        rubis::sched::task_state_ = TASK_STATE_DONE;
-      }
-      else
-      {
-        ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()");
-      }
-
-      if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0)
-      {
-        visualization_msgs::MarkerArray all_rollOuts;
-        PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts);
-        pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts);
-
-        PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy);
-        pub_CollisionPointsRviz.publish(m_CollisionsActual);
-
-        //Visualize Safety Box
-        visualization_msgs::Marker safety_box;
-        PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box);
-        pub_SafetyBorderRviz.publish(safety_box);
-      }
-    }
-    else
-      sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array",   1,    &TrajectoryEval::callbackGetGlobalPlannerPath,   this);
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time, deadline, period;
 
-    if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
+    private_nh.param(node_name + "/task_scheduling_configs/policy", policy, std::string("NONE"));
+    private_nh.param(node_name + "/task_scheduling_configs/priority", priority, 99);
+    private_nh.param(node_name + "/task_scheduling_configs/exec_time", exec_time, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/deadline", deadline, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/period", period, 0);
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);
+    rubis::init_task_scheduling(policy, attr);
 
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){
-      if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-      rubis::sched::task_state_ = TASK_STATE_READY;
-    }
-    loop_rate.sleep();
-  }
-}
+    rubis::init_task_profiling(task_response_time_filename);
+
+    PlannerHNS::WayPoint prevState, state_change;
+
+    // Add Crossing Info from yaml file
+    XmlRpc::XmlRpcValue intersection_xml;
+    nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml);
+    PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list_, intersection_xml);
 
+    ros::spin();
 }
+} // namespace TrajectoryEvaluatorNS
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp
new file mode 100644
index 00000000..853c2b47
--- /dev/null
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp
@@ -0,0 +1,596 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "op_trajectory_evaluator_core.h"
+#include "op_ros_helpers/op_ROSHelpers.h"
+#include "op_planner/MappingHelpers.h"
+#include <rubis_lib/sched.hpp>
+
+namespace TrajectoryEvaluatorNS
+{
+
+TrajectoryEval::TrajectoryEval()
+{
+  bNewCurrentPos = false;
+  bVehicleStatus = false;
+  bWayGlobalPath = false;
+  bWayGlobalPathToUse = false;
+  m_bUseMoveingObjectsPrediction = false;
+  m_noVehicleCnt = 0;
+
+  ros::NodeHandle _nh;
+  UpdatePlanningParams(_nh);
+
+  tf::StampedTransform transform;
+  PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
+  m_OriginPos.position.x  = transform.getOrigin().x();
+  m_OriginPos.position.y  = transform.getOrigin().y();
+  m_OriginPos.position.z  = transform.getOrigin().z();
+
+  pub_CollisionPointsRviz = nh.advertise<visualization_msgs::MarkerArray>("dynamic_collision_points_rviz", 1);
+  pub_LocalWeightedTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_eval_rviz", 1);
+  pub_LocalWeightedTrajectories = nh.advertise<rubis_msgs::LaneArrayWithPoseTwist>("local_weighted_trajectories", 1);
+  pub_LocalWeightedTrajectoriesWithPoseTwist = nh.advertise<rubis_msgs::LaneArrayWithPoseTwist>("local_weighted_trajectories_with_pose_twist", 1);
+  pub_TrajectoryCost = nh.advertise<autoware_msgs::Lane>("local_trajectory_cost", 1);
+  pub_SafetyBorderRviz = nh.advertise<visualization_msgs::Marker>("safety_border", 1);
+  pub_DistanceToPedestrian = nh.advertise<std_msgs::Float64>("distance_to_pedestrian", 1);
+  pub_IntersectionCondition = nh.advertise<autoware_msgs::IntersectionCondition>("intersection_condition", 1);
+  pub_SprintSwitch = nh.advertise<std_msgs::Bool>("sprint_switch", 1);
+
+  // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this);
+  // sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this);
+
+  int bVelSource = 1;
+  _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
+  if(bVelSource == 0)
+    sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this);
+  // else if(bVelSource == 1)
+  //   sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this);
+  else if(bVelSource == 2)
+    sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this);
+
+  sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this);
+  sub_LocalPlannerPaths = nh.subscribe("/local_trajectories_with_pose_twist", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this);
+  sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this);
+  sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this);
+
+  PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy);
+
+  while(1){
+    if(UpdateTf() == true)
+      break;
+  }
+}
+
+TrajectoryEval::~TrajectoryEval()
+{
+}
+
+void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh)
+{
+  _nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction);
+
+  _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
+  _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
+  _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
+  if(m_PlanningParams.enableSwerving)
+    m_PlanningParams.enableFollowing = true;
+  else
+    _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
+
+  _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
+  _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
+
+  _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
+  _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
+  _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
+
+  _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
+
+  _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
+  if(m_PlanningParams.enableSwerving)
+    _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
+  else
+    m_PlanningParams.rollOutNumber = 0;
+
+  std::cout << "Rolls Number: " << m_PlanningParams.rollOutNumber << std::endl;
+
+  _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
+  _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
+  _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
+  _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
+  _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
+
+  _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
+
+  _nh.getParam("/op_common_params/width", m_CarInfo.width);
+  _nh.getParam("/op_common_params/length", m_CarInfo.length);
+  _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
+  _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
+  _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
+  _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
+  _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
+  m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
+  m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
+
+  _nh.param("/op_trajectory_evaluator/PedestrianRightThreshold", m_PedestrianRightThreshold, 7.0);
+  _nh.param("/op_trajectory_evaluator/PedestrianLeftThreshold", m_PedestrianLeftThreshold, 2.0);
+  _nh.param("/op_trajectory_evaluator/PedestrianImageDetectionRange", m_PedestrianImageDetectionRange, 0.7);
+  _nh.param("/op_trajectory_evaluator/PedestrianStopImgHeightThreshold", m_pedestrian_stop_img_height_threshold, 120);
+  _nh.param("/op_trajectory_evaluator/ImageWidth", m_ImageWidth, 1920);
+  _nh.param("/op_trajectory_evaluator/ImageHeight", m_ImageHeight, 1080);
+  _nh.param("/op_trajectory_evaluator/VehicleImageDetectionRange", m_VehicleImageDetectionRange, 0.3);
+  _nh.param("/op_trajectory_evaluator/VehicleImageWidthThreshold", m_VehicleImageWidthThreshold, 0.05);
+  _nh.param("/op_trajectory_evaluator/SprintDecisionTime", m_SprintDecisionTime, 5.0);
+  
+  
+  m_VehicleImageWidthThreshold = m_VehicleImageWidthThreshold * m_ImageWidth;
+  m_PedestrianRightThreshold *= -1;
+
+}
+
+// void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
+// {
+//   m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation));
+//   bNewCurrentPos = true;
+// }
+
+// void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg)
+// {
+//   m_VehicleStatus.speed = msg->twist.linear.x;
+//   m_CurrentPos.v = m_VehicleStatus.speed;
+//   if(fabs(msg->twist.linear.x) > 0.25)
+//     m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x);
+//   UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+//   bVehicleStatus = true;
+// }
+
+void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
+{
+  m_VehicleStatus.speed = msg->speed/3.6;
+  m_CurrentPos.v = m_VehicleStatus.speed;
+  m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value;
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+}
+
+void TrajectoryEval::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
+{
+  m_VehicleStatus.speed = msg->twist.twist.linear.x;
+  m_CurrentPos.v = m_VehicleStatus.speed;
+  if(fabs(msg->twist.twist.linear.x) > 0.25)
+    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+}
+
+void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
+{
+  if(msg->lanes.size() > 0)
+  {
+
+    bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
+
+    m_GlobalPaths.clear();
+
+    for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
+    {
+      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
+
+      PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
+      m_GlobalPaths.push_back(m_temp_path);
+
+      if(bOldGlobalPath)
+      {
+        bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
+      }
+    }
+
+    if(!bOldGlobalPath)
+    {
+      bWayGlobalPath = true;
+      std::cout << "Received New Global Path Evaluator! " << std::endl;
+    }
+    else
+    {
+      m_GlobalPaths.clear();
+    }
+  }
+}
+
+void TrajectoryEval::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg)
+{
+  rubis::instance_ = msg->instance;
+  // Callback
+  m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation));
+  bNewCurrentPos = true;
+
+  m_VehicleStatus.speed = msg->twist.twist.linear.x;
+  m_CurrentPos.v = m_VehicleStatus.speed;
+  if(fabs(msg->twist.twist.linear.x) > 0.25)
+    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;  
+
+  if(msg->lane_array.lanes.size() > 0)
+  {
+    m_GeneratedRollOuts.clear();
+    int globalPathId_roll_outs = -1;
+
+    for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++)
+    {
+      std::vector<PlannerHNS::WayPoint> path;
+      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path);
+      m_GeneratedRollOuts.push_back(path);
+      if(path.size() > 0)
+        globalPathId_roll_outs = path.at(0).gid;
+    }
+
+    if(bWayGlobalPath && m_GlobalPaths.size() > 0 && m_GlobalPaths.at(0).size() > 0)
+    {
+      int globalPathId = m_GlobalPaths.at(0).at(0).gid;
+      std::cout << "Before Synchronization At Trajectory Evaluator: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
+
+      if(globalPathId_roll_outs == globalPathId)
+      {
+        bWayGlobalPath = false;
+        m_GlobalPathsToUse = m_GlobalPaths;
+        std::cout << "Synchronization At Trajectory Evaluator: GlobalID: " <<  globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
+      }
+    }
+
+    bRollOuts = true;
+  }
+}
+
+void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg)
+{  
+  m_PredictedObjects.clear();
+  bPredictedObjects = true;
+  double distance_to_pedestrian = 1000;
+  int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2;
+  int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2;
+  
+  int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2;
+  int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2;
+
+  int vehicle_cnt = 0;
+
+  PlannerHNS::DetectedObject obj;
+  for(unsigned int i = 0 ; i <msg->objects.size(); i++)
+  {    
+    if(msg->objects.at(i).pose.position.y < -20 || msg->objects.at(i).pose.position.y > 20)
+      continue;    
+      
+    if(msg->objects.at(i).pose.position.z > 1 || msg->objects.at(i).pose.position.z < -1.5)
+      continue;
+
+    autoware_msgs::DetectedObject msg_obj = msg->objects.at(i);     
+
+    // #### Decison making for objects
+    
+    if(msg_obj.id > 0) // If fusion object is detected
+    {
+      // calculate distance to person first
+      // if(msg_obj.label == "person"){        
+      //   std::cout<<"Pedestrian box size(width x height):"<<msg_obj.width<<" "<<msg_obj.height<<std::endl;
+      //   geometry_msgs::PoseStamped pose;
+      //   pose.header = msg_obj.header;
+      //   pose.pose = msg_obj.pose;
+      //   try{
+      //     m_vtob_listener.transformPose("/base_link", pose, pose);
+      //     double temp_x_distance = pose.pose.position.x;
+      //     double temp_y_distance = pose.pose.position.y;
+      //     // y-axis: Left + / Right -          
+      //     if(temp_y_distance > m_PedestrianLeftThreshold || temp_y_distance < m_PedestrianRightThreshold ) continue;
+      //     if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance;          
+      //   }
+      //   catch(tf::TransformException& ex){
+      //     // ROS_ERROR("Cannot transform person pose: %s", ex.what());
+
+      //   }
+      // }
+
+      if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){
+        vehicle_cnt += 1;
+      }
+
+      PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj);
+
+
+      // transform center pose into map frame
+      geometry_msgs::PoseStamped pose_in_map;
+      pose_in_map.header = msg_obj.header;
+      pose_in_map.pose = msg_obj.pose;
+      try{
+        m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map);
+      }
+      catch(tf::TransformException& ex)
+      {
+        // ROS_ERROR("Cannot transform object pose: %s", ex.what());
+        continue;
+      }
+      // msg_obj.header.frame_id = "map";
+      obj.center.pos.x = pose_in_map.pose.position.x;
+      obj.center.pos.y = pose_in_map.pose.position.y;
+      obj.center.pos.z = pose_in_map.pose.position.z;
+
+      // transform contour into map frame
+      for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){
+        geometry_msgs::PoseStamped contour_point_in_map;
+        contour_point_in_map.header = msg_obj.header;
+        contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x;
+        contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y;
+        contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z;
+
+        // For resolve TF malform, set orientation w to 1
+        contour_point_in_map.pose.orientation.w = 1;
+
+        try{
+          m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map);
+        }
+        catch(tf::TransformException& ex){
+          // ROS_ERROR("Cannot transform contour pose: %s", ex.what());
+          continue;
+        }
+
+        obj.contour.at(j).x = contour_point_in_map.pose.position.x;
+        obj.contour.at(j).y = contour_point_in_map.pose.position.y;
+        obj.contour.at(j).z = contour_point_in_map.pose.position.z;
+      }
+
+      msg_obj.header.frame_id = "map";
+
+      m_PredictedObjects.push_back(obj);
+    }
+    /*
+    else{ // If object is only detected at vision
+      int image_obj_center_x = msg_obj.x+msg_obj.width/2;
+      int image_obj_center_y = msg_obj.y+msg_obj.height/2;
+      // if (msg_obj.label == "person"){// If person is detected only in image
+      //   // TO ERASE
+      //   std::cout<<"object height:" << msg_obj.height << " / threshold:" << m_pedestrian_stop_img_height_threshold << std::endl;
+      //   if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ 
+      //     double temp_x_distance = 1000;
+      //     if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10;
+      //     if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance;
+      //   }
+      // }                    
+      // else 
+      if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){            
+        if((msg_obj.width > m_VehicleImageWidthThreshold) 
+              && (image_obj_center_x > image_vehicle_detection_range_left) 
+              && (image_obj_center_x < image_vehicle_detection_range_right)
+        )
+        {          
+          vehicle_cnt+=1;        
+        }
+      }
+    }
+    */
+
+    int image_obj_center_x = msg_obj.x+msg_obj.width/2;
+    int image_obj_center_y = msg_obj.y+msg_obj.height/2;
+    if (msg_obj.label == "person"){// If person is detected only in image
+      
+      // TO ERASE
+      // ROS_WARN("object height:%d // thr: %d\n", msg_obj.height, m_pedestrian_stop_img_height_threshold);
+      printf("center_x %d \n left: %d \n right %d\n\n\n", image_obj_center_x, image_person_detection_range_left, image_person_detection_range_right);
+      if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ 
+        double temp_x_distance = 1000;
+        if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10;
+        if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance;
+      }
+    }
+    
+  }
+
+  // Publish Sprint Switch
+  std_msgs::Bool sprint_switch_msg;
+
+  if(vehicle_cnt != 0){
+    m_noVehicleCnt = 0;
+    sprint_switch_msg.data = false;
+  }
+  else{ // No vehicle is exist in front of the car
+    if(m_noVehicleCnt < m_SprintDecisionTime*10) {
+      m_noVehicleCnt +=1;
+      sprint_switch_msg.data = false;
+    }
+    else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true;
+  }  
+  pub_SprintSwitch.publish(sprint_switch_msg);
+
+  // ROS_INFO("object # : %d", m_PredictedObjects.size());
+  
+  std_msgs::Float64 distanceToPedestrianMsg; 
+  distanceToPedestrianMsg.data = distance_to_pedestrian;
+  pub_DistanceToPedestrian.publish(distanceToPedestrianMsg);
+}
+
+void TrajectoryEval::callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr& msg)
+{
+  m_CurrentBehavior.iTrajectory = msg->twist.angular.z;
+}
+
+void TrajectoryEval::callbackGetCurrentState(const std_msgs::Int32 & msg)
+{
+  m_CurrentBehavior.state = static_cast<PlannerHNS::STATE_TYPE>(msg.data);
+}
+
+void TrajectoryEval::UpdateMyParams()
+{
+  ros::NodeHandle _nh;
+  _nh.getParam("/op_trajectory_evaluator/weightPriority", m_PlanningParams.weightPriority);
+  _nh.getParam("/op_trajectory_evaluator/weightTransition", m_PlanningParams.weightTransition);
+  _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong);
+  _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat);
+  _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance);
+}
+
+bool TrajectoryEval::UpdateTf()
+{
+  try{
+    m_vtob_listener.waitForTransform("/velodyne", "/base_link", ros::Time(0), ros::Duration(0.001));
+    m_vtob_listener.lookupTransform("/velodyne", "/base_link", ros::Time(0), m_velodyne_to_base_link);
+
+    m_vtom_listener.waitForTransform("/velodyne", "/map", ros::Time(0), ros::Duration(0.001));
+    m_vtom_listener.lookupTransform("/velodyne", "/map", ros::Time(0), m_velodyne_to_map);
+    return true;
+  }
+  catch(tf::TransformException& ex){
+    if(TF_DEBUG)
+      ROS_ERROR("%s", ex.what());
+    return false;
+  }
+}
+
+void TrajectoryEval::MainLoop()
+{
+  ros::NodeHandle private_nh("~");
+
+  // Scheduling Setup
+  std::string task_response_time_filename;
+  int rate;
+  double task_minimum_inter_release_time;
+  double task_execution_time;
+  double task_relative_deadline; 
+
+  private_nh.param<std::string>("/op_trajectory_evaluator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv");
+  private_nh.param<int>("/op_trajectory_evaluator/rate", rate, 10);
+  private_nh.param("/op_trajectory_evaluator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
+  private_nh.param("/op_trajectory_evaluator/task_execution_time", task_execution_time, (double)10);
+  private_nh.param("/op_trajectory_evaluator/task_relative_deadline", task_relative_deadline, (double)10);  
+
+  rubis::init_task_profiling(task_response_time_filename);
+
+  PlannerHNS::WayPoint prevState, state_change;
+
+  // Add Crossing Info from yaml file
+  XmlRpc::XmlRpcValue intersection_xml;  
+  nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml);
+  PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list_, intersection_xml);
+
+  ros::Rate r(100);
+
+  while(ros::ok()){
+    // Before spin
+    UpdateMyParams();
+    UpdateTf();
+
+    ros::spinOnce();
+
+    PlannerHNS::TrajectoryCost tc;
+
+    if(bNewCurrentPos && m_GlobalPaths.size()>0)
+    {
+      m_GlobalPathSections.clear();
+
+      for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++)
+      {
+        t_centerTrajectorySmoothed.clear();
+        PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed);
+        m_GlobalPathSections.push_back(t_centerTrajectorySmoothed);
+      }
+
+      if(m_GlobalPathSections.size()>0)
+      {
+        if(m_bUseMoveingObjectsPrediction)
+          tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams,  m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory);
+        else
+          tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,  m_PlanningParams,  m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state);
+
+        autoware_msgs::Lane l;
+        l.closest_object_distance = tc.closest_obj_distance;
+        l.closest_object_velocity = tc.closest_obj_velocity;
+        l.cost = tc.cost;
+        l.is_blocked = tc.bBlocked;
+        l.lane_index = tc.index;`
+        pub_TrajectoryCost.publish(l);
+
+        // hjw added : Check if ego is on intersection and obstacles are in risky area 
+        int intersectionID = -1;
+        double closestIntersectionDistance = -1;
+        bool isInsideIntersection = false;
+        bool riskyLeftTurn = false;
+        bool riskyRightTurn = false;
+
+        PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list_, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn);
+
+        autoware_msgs::IntersectionCondition ic_msg;
+        ic_msg.intersectionID = intersectionID;
+        ic_msg.intersectionDistance = closestIntersectionDistance;
+        ic_msg.isIntersection = isInsideIntersection;
+        ic_msg.riskyLeftTurn = riskyLeftTurn;
+        ic_msg.riskyRightTurn = riskyRightTurn;
+
+        pub_IntersectionCondition.publish(ic_msg);
+
+      }
+
+      if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size())
+      {
+        rubis_msgs::LaneArrayWithPoseTwist local_lanes;
+        for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++)
+        {
+          autoware_msgs::Lane lane;
+          PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane);
+          lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance;
+          lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity;
+          lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost;
+          lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked;
+          lane.lane_index = i;
+          local_lanes.lane_array.lanes.push_back(lane);
+        }
+        
+        local_lanes.instance = rubis::instance_;
+        local_lanes.pose = msg->pose;
+        local_lanes.twist = msg->twist;
+
+        pub_LocalWeightedTrajectoriesWithPoseTwist.publish(local_lanes);
+        pub_LocalWeightedTrajectories.publish(local_lanes.lane_array);
+        
+      }
+      else
+      {
+        ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()");
+      }
+
+      if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0)
+      {
+        visualization_msgs::MarkerArray all_rollOuts;
+        PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts);
+        pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts);
+
+        PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy);
+        pub_CollisionPointsRviz.publish(m_CollisionsActual);
+
+        //Visualize Safety Box
+        visualization_msgs::Marker safety_box;
+        PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box);
+        pub_SafetyBorderRviz.publish(safety_box);
+      }
+    } 
+    else
+      sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array",   1,    &TrajectoryEval::callbackGetGlobalPlannerPath,   this);
+
+    rubis::stop_task_profiling(0, 0, 0);  
+
+    r.sleep();
+  }
+}
+
+}
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp
index 10549a37..3250819e 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp
@@ -39,37 +39,16 @@ TrajectoryGen::TrajectoryGen()
   m_OriginPos.position.y  = transform.getOrigin().y();
   m_OriginPos.position.z  = transform.getOrigin().z();
 
-  pub_LocalTrajectories = nh.advertise<autoware_msgs::LaneArray>("local_trajectories", 1);
+  // pub_LocalTrajectories = nh.advertise<autoware_msgs::LaneArray>("local_trajectories", 1);
+  pub_LocalTrajectoriesWithPoseTwist = nh.advertise<rubis_msgs::LaneArrayWithPoseTwist>("local_trajectories_with_pose_twist", 1);
   pub_LocalTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_gen_rviz", 1);
 
   sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this);
-  sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this);
 
   int bVelSource = 1;
   _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource);
-  if(bVelSource == 0)
-    sub_robot_odom = nh.subscribe("/odom", 10,  &TrajectoryGen::callbackGetRobotOdom, this);
-  else if(bVelSource == 1)
-    sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this);
-  else if(bVelSource == 2)
-    sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this);
-
-  sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this);
-  
-  /*  RT Scheduling setup  */
-  // sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this);
-  // sub_current_pose = nh.subscribe("/current_pose", 1, &TrajectoryGen::callbackGetCurrentPose, this); //origin 10
-
-  // int bVelSource = 1;
-  // _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource);
-  // if(bVelSource == 0)
-  //   sub_robot_odom = nh.subscribe("/odom", 1,  &TrajectoryGen::callbackGetRobotOdom, this); //origin 10
-  // else if(bVelSource == 1)
-  //   sub_current_velocity = nh.subscribe("/current_velocity", 1, &TrajectoryGen::callbackGetVehicleStatus, this); //origin 10
-  // else if(bVelSource == 2)
-  //   sub_can_info = nh.subscribe("/can_info", 1, &TrajectoryGen::callbackGetCANInfo, this); //origin 10
-
-  // sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this);
+  sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 10, &TrajectoryGen::callbackGetGlobalPlannerPath, this);
+  sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 10, &TrajectoryGen::callbackGetCurrentPoseTwist, this); // Def: 10
 }
 
 TrajectoryGen::~TrajectoryGen()
@@ -143,40 +122,95 @@ void TrajectoryGen::callbackGetInitPose(const geometry_msgs::PoseWithCovarianceS
   }
 }
 
-void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
-{
-  m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation));
+void TrajectoryGen::callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg){
+  rubis::start_task_profiling();
+
+  // Callback
+  rubis::instance_ = msg->instance;
+  
+  m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation));
   m_InitPos = m_CurrentPos;
   bNewCurrentPos = true;
   bInitPos = true;
-}
 
-void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  m_VehicleStatus.speed = msg->twist.linear.x;
+  m_VehicleStatus.speed = msg->twist.twist.linear.x;
   m_CurrentPos.v = m_VehicleStatus.speed;
-  if(fabs(msg->twist.linear.x) > 0.25)
-    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x);
+  if(fabs(msg->twist.twist.linear.x) > 0.25)
+    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
   UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
   bVehicleStatus = true;
 
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();  
-}
+  current_pose_ = msg->pose;
+  current_twist_ = msg->twist;
 
-void TrajectoryGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
-{
-  m_VehicleStatus.speed = msg->speed/3.6;
-  m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value;
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
-}
+  if(bInitPos && m_GlobalPaths.size()>0){
+    m_GlobalPathSections.clear();
 
-void TrajectoryGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
-{
-  m_VehicleStatus.speed = msg->twist.twist.linear.x;
-  m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
-  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
-  bVehicleStatus = true;
+    for(unsigned int i = 0; i < m_GlobalPaths.size(); i++)
+    {
+      t_centerTrajectorySmoothed.clear();
+      PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPaths.at(i), m_CurrentPos, m_PlanningParams.horizonDistance ,
+          m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed);
+
+      m_GlobalPathSections.push_back(t_centerTrajectorySmoothed);
+    }
+
+    std::vector<PlannerHNS::WayPoint> sampledPoints_debug;
+    m_Planner.GenerateRunoffTrajectory(m_GlobalPathSections, m_CurrentPos,
+              m_PlanningParams.enableLaneChange,
+              m_VehicleStatus.speed,
+              m_PlanningParams.microPlanDistance,
+              m_PlanningParams.maxSpeed,
+              m_PlanningParams.minSpeed,
+              m_PlanningParams.carTipMargin,
+              m_PlanningParams.rollInMargin,
+              m_PlanningParams.rollInSpeedFactor,
+              m_PlanningParams.pathDensity,
+              m_PlanningParams.rollOutDensity,
+              m_PlanningParams.rollOutNumber,
+              m_PlanningParams.smoothingDataWeight,
+              m_PlanningParams.smoothingSmoothWeight,
+              m_PlanningParams.smoothingToleranceError,
+              m_PlanningParams.speedProfileFactor,
+              m_PlanningParams.enableHeadingSmoothing,
+              -1 , -1,
+              m_RollOuts, sampledPoints_debug);
+
+    rubis_msgs::LaneArrayWithPoseTwist local_lanes;
+    for(unsigned int i=0; i < m_RollOuts.size(); i++)
+    {
+      for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++)
+      {
+        autoware_msgs::Lane lane;
+        PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_RollOuts.at(i).at(j), m_CurrentPos, m_PlanningParams.minSpeed, m_PlanningParams.microPlanDistance);
+        PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_RollOuts.at(i).at(j), lane);
+        lane.closest_object_distance = 0;
+        lane.closest_object_velocity = 0;
+        lane.cost = 0;
+        lane.is_blocked = false;
+        lane.lane_index = i;
+        local_lanes.lane_array.lanes.push_back(lane);
+      }
+    }
+
+    local_lanes.header = msg->header;
+    local_lanes.instance = rubis::instance_;
+    local_lanes.lidar_instance = 0;
+    local_lanes.pose = current_pose_;
+    local_lanes.twist = current_twist_;
+
+    pub_LocalTrajectoriesWithPoseTwist.publish(local_lanes);
+
+    // Visualize generated local trajectories
+    visualization_msgs::MarkerArray all_rollOuts;
+    PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts);
+    pub_LocalTrajectoriesRviz.publish(all_rollOuts);
+  }
+  else{
+    sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array",   1,    &TrajectoryGen::callbackGetGlobalPlannerPath,   this);    
+  }
+  
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
 void TrajectoryGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
@@ -216,113 +250,29 @@ void TrajectoryGen::MainLoop()
 {
   ros::NodeHandle private_nh("~");
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
+  // Scheduling & Profiling Setup
+  std::string node_name = ros::this_node::getName();
   std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline; 
-
-  private_nh.param<int>("/op_trajectory_generator/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/op_trajectory_generator/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/op_trajectory_generator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv");
-  private_nh.param<int>("/op_trajectory_generator/rate", rate, 10);
-  private_nh.param("/op_trajectory_generator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/op_trajectory_generator/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/op_trajectory_generator/task_relative_deadline", task_relative_deadline, (double)10);
-
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-  PlannerHNS::WayPoint prevState, state_change;
-
-
-  ros::Rate loop_rate(rate);
-  if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(100);
-
-  struct timespec start_time, end_time;
-
-  while (ros::ok())
-  {
-    if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){
-      if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-      rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-    }
-
-    ros::spinOnce();
-
-    if(bInitPos && m_GlobalPaths.size()>0)
-    {
-      m_GlobalPathSections.clear();
-
-      for(unsigned int i = 0; i < m_GlobalPaths.size(); i++)
-      {
-        t_centerTrajectorySmoothed.clear();
-        PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPaths.at(i), m_CurrentPos, m_PlanningParams.horizonDistance ,
-            m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed);
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv");
 
-        m_GlobalPathSections.push_back(t_centerTrajectorySmoothed);
-      }
-
-      std::vector<PlannerHNS::WayPoint> sampledPoints_debug;
-      m_Planner.GenerateRunoffTrajectory(m_GlobalPathSections, m_CurrentPos,
-                m_PlanningParams.enableLaneChange,
-                m_VehicleStatus.speed,
-                m_PlanningParams.microPlanDistance,
-                m_PlanningParams.maxSpeed,
-                m_PlanningParams.minSpeed,
-                m_PlanningParams.carTipMargin,
-                m_PlanningParams.rollInMargin,
-                m_PlanningParams.rollInSpeedFactor,
-                m_PlanningParams.pathDensity,
-                m_PlanningParams.rollOutDensity,
-                m_PlanningParams.rollOutNumber,
-                m_PlanningParams.smoothingDataWeight,
-                m_PlanningParams.smoothingSmoothWeight,
-                m_PlanningParams.smoothingToleranceError,
-                m_PlanningParams.speedProfileFactor,
-                m_PlanningParams.enableHeadingSmoothing,
-                -1 , -1,
-                m_RollOuts, sampledPoints_debug);
-
-      autoware_msgs::LaneArray local_lanes;
-      for(unsigned int i=0; i < m_RollOuts.size(); i++)
-      {
-        for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++)
-        {
-          autoware_msgs::Lane lane;
-          PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_RollOuts.at(i).at(j), m_CurrentPos, m_PlanningParams.minSpeed, m_PlanningParams.microPlanDistance);
-          PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_RollOuts.at(i).at(j), lane);
-          lane.closest_object_distance = 0;
-          lane.closest_object_velocity = 0;
-          lane.cost = 0;
-          lane.is_blocked = false;
-          lane.lane_index = i;
-          local_lanes.lanes.push_back(lane);
-        }
-      }
-      pub_LocalTrajectories.publish(local_lanes);
-      rubis::sched::task_state_ = TASK_STATE_DONE;
-    }
-    else
-      sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array",   1,    &TrajectoryGen::callbackGetGlobalPlannerPath,   this);
-
-    visualization_msgs::MarkerArray all_rollOuts;
-    PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts);
-    pub_LocalTrajectoriesRviz.publish(all_rollOuts);
-
-    if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
-
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){
-      if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-      rubis::sched::task_state_ = TASK_STATE_READY;
-    }
-
-    loop_rate.sleep();
-  } 
+  int rate;
+  private_nh.param<int>(node_name+"/rate", rate, 10);
+
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
+
+  rubis::init_task_profiling(task_response_time_filename);
+
+  ros::spin();
 }
 
-}
+}
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp
new file mode 100644
index 00000000..91b1f6ab
--- /dev/null
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp
@@ -0,0 +1,315 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "op_trajectory_generator_core.h"
+#include "op_ros_helpers/op_ROSHelpers.h"
+#include <rubis_lib/sched.hpp>
+
+#define SPIN_PROFILING
+
+namespace TrajectoryGeneratorNS
+{
+
+TrajectoryGen::TrajectoryGen()
+{
+  bInitPos = false;
+  bNewCurrentPos = false;
+  bVehicleStatus = false;
+  bWayGlobalPath = false;
+
+  ros::NodeHandle _nh;
+  UpdatePlanningParams(_nh);
+
+  tf::StampedTransform transform;
+  PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
+  m_OriginPos.position.x  = transform.getOrigin().x();
+  m_OriginPos.position.y  = transform.getOrigin().y();
+  m_OriginPos.position.z  = transform.getOrigin().z();
+
+  pub_LocalTrajectories = nh.advertise<autoware_msgs::LaneArray>("local_trajectories", 1);
+  pub_LocalTrajectoriesWithPoseTwist = nh.advertise<rubis_msgs::LaneArrayWithPoseTwist>("local_trajectories_with_pose_twist", 1);
+  pub_LocalTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_gen_rviz", 1);
+
+  sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this);
+  // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this);
+
+  int bVelSource = 1;
+  _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource);
+  if(bVelSource == 0)
+    sub_robot_odom = nh.subscribe("/odom", 10,  &TrajectoryGen::callbackGetRobotOdom, this);
+  // else if(bVelSource == 1)
+  //   sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this);
+  else if(bVelSource == 2)
+    sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this);
+
+  sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this);
+  
+  sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 10, &TrajectoryGen::callbackGetCurrentPoseTwist, this);
+}
+
+TrajectoryGen::~TrajectoryGen()
+{
+}
+
+void TrajectoryGen::UpdatePlanningParams(ros::NodeHandle& _nh)
+{
+  _nh.getParam("/op_trajectory_generator/samplingTipMargin", m_PlanningParams.carTipMargin);
+  _nh.getParam("/op_trajectory_generator/samplingOutMargin", m_PlanningParams.rollInMargin);
+  _nh.getParam("/op_trajectory_generator/samplingSpeedFactor", m_PlanningParams.rollInSpeedFactor);
+  _nh.getParam("/op_trajectory_generator/enableHeadingSmoothing", m_PlanningParams.enableHeadingSmoothing);
+
+  _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
+  if(m_PlanningParams.enableSwerving)
+    m_PlanningParams.enableFollowing = true;
+  else
+    _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
+
+  _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
+  _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
+
+  _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
+  _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
+  _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
+
+  _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
+  _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
+  if(m_PlanningParams.enableSwerving)
+    _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
+  else
+    m_PlanningParams.rollOutNumber = 0;
+
+  _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
+  _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
+  _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
+  _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
+  _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
+
+  _nh.getParam("/op_common_params/smoothingDataWeight", m_PlanningParams.smoothingDataWeight);
+  _nh.getParam("/op_common_params/smoothingSmoothWeight", m_PlanningParams.smoothingSmoothWeight);
+
+  _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
+  _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
+
+  _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
+
+  _nh.getParam("/op_common_params/width", m_CarInfo.width);
+  _nh.getParam("/op_common_params/length", m_CarInfo.length);
+  _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
+  _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
+  _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
+  _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
+  _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
+
+  m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
+  m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
+
+}
+
+void TrajectoryGen::callbackGetInitPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
+{
+  if(!bInitPos)
+  {
+    m_InitPos = PlannerHNS::WayPoint(msg->pose.pose.position.x+m_OriginPos.position.x,
+        msg->pose.pose.position.y+m_OriginPos.position.y,
+        msg->pose.pose.position.z+m_OriginPos.position.z,
+        tf::getYaw(msg->pose.pose.orientation));
+    m_CurrentPos = m_InitPos;
+    bInitPos = true;
+  }
+}
+
+// void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
+// {
+//   m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation));
+//   m_InitPos = m_CurrentPos;
+//   bNewCurrentPos = true;
+//   bInitPos = true;
+// }
+
+// void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg)
+// {
+//   m_VehicleStatus.speed = msg->twist.linear.x;
+//   m_CurrentPos.v = m_VehicleStatus.speed;
+//   if(fabs(msg->twist.linear.x) > 0.25)
+//     m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x);
+//   UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+//   bVehicleStatus = true;
+// }
+
+void TrajectoryGen::callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg){
+  // Before spinOnce
+  rubis::start_task_profiling();
+
+  // Callback
+  m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation));
+  m_InitPos = m_CurrentPos;
+  bNewCurrentPos = true;
+  bInitPos = true;
+
+  m_VehicleStatus.speed = msg->twist.twist.linear.x;
+  m_CurrentPos.v = m_VehicleStatus.speed;
+  if(fabs(msg->twist.twist.linear.x) > 0.25)
+    m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+  
+  // After spinOnce
+  if(bInitPos && m_GlobalPaths.size()>0)
+  {
+    m_GlobalPathSections.clear();
+
+    for(unsigned int i = 0; i < m_GlobalPaths.size(); i++)
+    {
+      t_centerTrajectorySmoothed.clear();
+      PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPaths.at(i), m_CurrentPos, m_PlanningParams.horizonDistance ,
+          m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed);
+
+      m_GlobalPathSections.push_back(t_centerTrajectorySmoothed);
+    }
+
+    std::vector<PlannerHNS::WayPoint> sampledPoints_debug;
+    m_Planner.GenerateRunoffTrajectory(m_GlobalPathSections, m_CurrentPos,
+              m_PlanningParams.enableLaneChange,
+              m_VehicleStatus.speed,
+              m_PlanningParams.microPlanDistance,
+              m_PlanningParams.maxSpeed,
+              m_PlanningParams.minSpeed,
+              m_PlanningParams.carTipMargin,
+              m_PlanningParams.rollInMargin,
+              m_PlanningParams.rollInSpeedFactor,
+              m_PlanningParams.pathDensity,
+              m_PlanningParams.rollOutDensity,
+              m_PlanningParams.rollOutNumber,
+              m_PlanningParams.smoothingDataWeight,
+              m_PlanningParams.smoothingSmoothWeight,
+              m_PlanningParams.smoothingToleranceError,
+              m_PlanningParams.speedProfileFactor,
+              m_PlanningParams.enableHeadingSmoothing,
+              -1 , -1,
+              m_RollOuts, sampledPoints_debug);
+
+    rubis_msgs::LaneArrayWithPoseTwist local_lanes;
+    for(unsigned int i=0; i < m_RollOuts.size(); i++)
+    {
+      for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++)
+      {
+        autoware_msgs::Lane lane;
+        PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_RollOuts.at(i).at(j), m_CurrentPos, m_PlanningParams.minSpeed, m_PlanningParams.microPlanDistance);
+        PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_RollOuts.at(i).at(j), lane);
+        lane.closest_object_distance = 0;
+        lane.closest_object_velocity = 0;
+        lane.cost = 0;
+        lane.is_blocked = false;
+        lane.lane_index = i;
+        local_lanes.lane_array.lanes.push_back(lane);
+      }
+    }
+
+    rubis::instance_ = msg->instance;
+    local_lanes.instance = rubis::instance_;
+    local_lanes.pose = msg->pose;
+    local_lanes.twist = msg->twist;
+
+    pub_LocalTrajectoriesWithPoseTwist.publish(local_lanes);
+    pub_LocalTrajectories.publish(local_lanes.lane_array);
+    
+  }
+  else{
+    sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array",   1,    &TrajectoryGen::callbackGetGlobalPlannerPath,   this);
+
+    visualization_msgs::MarkerArray all_rollOuts;
+    PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts);
+    pub_LocalTrajectoriesRviz.publish(all_rollOuts);
+
+    rubis::stop_task_profiling(0, 0, 0);
+  }
+}
+
+
+void TrajectoryGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
+{
+  m_VehicleStatus.speed = msg->speed/3.6;
+  m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value;
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+}
+
+void TrajectoryGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
+{
+  m_VehicleStatus.speed = msg->twist.twist.linear.x;
+  m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
+  UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
+  bVehicleStatus = true;
+}
+
+void TrajectoryGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
+{
+  if(msg->lanes.size() > 0)
+  {
+    bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
+
+    m_GlobalPaths.clear();
+
+    for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
+    {
+      PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
+
+      PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
+      m_GlobalPaths.push_back(m_temp_path);
+
+      if(bOldGlobalPath)
+      {
+        bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
+      }
+    }
+
+    if(!bOldGlobalPath)
+    {
+      bWayGlobalPath = true;
+      std::cout << "Received New Global Path Generator ! " << std::endl;
+    }
+    else
+    {
+      m_GlobalPaths.clear();
+    }
+  }
+}
+
+void TrajectoryGen::MainLoop()
+{
+  ros::NodeHandle private_nh("~");
+
+  // Scheduling Setup
+  std::string task_response_time_filename;
+  int rate;
+  double task_minimum_inter_release_time;
+  double task_execution_time;
+  double task_relative_deadline; 
+
+  private_nh.param<std::string>("/op_trajectory_generator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv");
+  private_nh.param<int>("/op_trajectory_generator/rate", rate, 10);
+  private_nh.param("/op_trajectory_generator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
+  private_nh.param("/op_trajectory_generator/task_execution_time", task_execution_time, (double)10);
+  private_nh.param("/op_trajectory_generator/task_relative_deadline", task_relative_deadline, (double)10);
+
+  rubis::init_task_profiling(task_response_time_filename);
+
+  PlannerHNS::WayPoint prevState, state_change;
+
+  ros::spin();
+}
+
+}
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml b/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml
index e638fb94..146b4b59 100644
--- a/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml
+++ b/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml
@@ -7,7 +7,7 @@
   <author email="hatem.darweesh@g.sp.m.is.nagoya-u.ac.jp">Hatem Darweesh</author>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
   
   <depend>autoware_can_msgs</depend>
@@ -16,7 +16,6 @@
   <depend>map_file</depend>
   <depend>op_planner</depend>
   <depend>op_ros_helpers</depend>
-  <depend>op_simu</depend>
   <depend>op_utility</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
@@ -26,4 +25,6 @@
   <depend>libwaypoint_follower</depend>
   <depend>yaml-cpp</depend>
   <depend>rubis_lib</depend>
+  <depend>nav_msgs</depend>
+  <depend>rubis_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt
index 97cc58e4..6cd87c0e 100644
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt
@@ -5,18 +5,16 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 find_package(
   catkin REQUIRED COMPONENTS
     amathutils_lib
     autoware_config_msgs
-    autoware_health_checker
     autoware_msgs
     geometry_msgs
     libwaypoint_follower
     roscpp
     roslint
-    rostest
     rosunit
     std_msgs
     tf
@@ -24,6 +22,7 @@ find_package(
     rubis_lib
     rubis_msgs
     can_data_msgs
+    visualization_msgs
 )
 
 catkin_package(
@@ -61,19 +60,3 @@ install(
   DIRECTORY launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
 )
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-
-  add_rostest_gtest(
-    test-pure_pursuit
-    test/test_pure_pursuit.test
-    test/src/test_pure_pursuit.cpp
-    src/pure_pursuit_core.cpp
-    src/pure_pursuit.cpp src/pure_pursuit_viz.cpp
-  )
-  add_dependencies(test-pure_pursuit ${catkin_EXPORTED_TARGETS})
-  target_link_libraries(test-pure_pursuit ${catkin_LIBRARIES})
-
-  roslint_add_test()
-endif()
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h
index bab5c953..6a56fee1 100644
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h
@@ -24,6 +24,7 @@
 #include <geometry_msgs/TwistStamped.h>
 #include <rubis_msgs/PoseStamped.h>
 #include <rubis_msgs/TwistStamped.h>
+#include "autoware_msgs/Lane.h"
 
 // C++ includes
 #include <vector>
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h
index 6222e31d..bc535837 100755
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h
@@ -27,6 +27,7 @@
 #include <geometry_msgs/Point.h>
 #include <rubis_msgs/PoseStamped.h>
 #include <rubis_msgs/TwistStamped.h>
+#include <rubis_msgs/PoseTwistStamped.h>
 
 #include <ros/ros.h>
 #include <std_msgs/Float32.h>
@@ -39,13 +40,14 @@
 #include <pure_pursuit/pure_pursuit.h>
 #include <pure_pursuit/pure_pursuit_viz.h>
 
-#include <autoware_health_checker/health_checker/health_checker.h>
-
 #include <vector>
 #include <memory>
 
 #include <XmlRpcException.h>
 
+#include "rubis_msgs/LaneWithPoseTwist.h"
+#include "rubis_msgs/PurePursuitOutput.h"
+
 #ifdef IONIC
 #include <can_data_msgs/Car_ctrl_output.h>
 #endif
@@ -91,17 +93,15 @@ class PurePursuitNode
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
 
-  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
-
   // class
   PurePursuit pp_;
 
   // publisher
-  ros::Publisher twist_pub_, rubis_twist_pub_, pub2_,
+  ros::Publisher twist_pub_, pure_pursuit_output_pub_, pub2_,
     pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_, pub18_;
 
   // subscriber
-  ros::Subscriber sub1_, pose_sub_, rubis_pose_sub_, sub3_, velocity_sub_, car_ctrl_output_sub;
+  ros::Subscriber sub1_, sub3_, pose_twist_sub_, final_waypoints_with_pose_twist_sub, car_ctrl_output_sub;
 
   // constant
   const int LOOP_RATE_;  // processing frequency
@@ -127,19 +127,14 @@ class PurePursuitNode
   double minimum_lookahead_distance_from_param;
 
   // Added by PHY
-  bool dynamic_param_flag_;
+  bool dynamic_param_flag_, wait_until_reaching_max_velocity_;
+  double max_velocity_;
   std::vector<DynamicParams> dynamic_params;
+  autoware_msgs::Lane lane_;
 
   // callbacks
-  void callbackFromConfig(
-    const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config);
-  void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
-  void callbackFromRubisCurrentPose(const rubis_msgs::PoseStampedConstPtr& _msg);
-  void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg);
-
-  #ifdef SVL
-  void callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg);
-  #endif
+  void callbackFromConfig(const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config);
+  void CallbackFinalWaypointsWithPoseTwist(const rubis_msgs::LaneWithPoseTwistConstPtr& msg);
 
   #ifdef IONIC
   void callbackCtrlOutput(const can_data_msgs::Car_ctrl_output::ConstPtr &msg);
@@ -155,16 +150,16 @@ class PurePursuitNode
   void initForROS();
 
   // functions
-  void publishTwistStamped(
+  geometry_msgs::TwistStamped calculateTwistStamped(
     const bool& can_get_curvature, const double& kappa) const;
-  void publishControlCommandStamped(
+  autoware_msgs::ControlCommandStamped calculateControlCommandStamped(
     const bool& can_get_curvature, const double& kappa) const;
   void publishDeviationCurrentPosition(
     const geometry_msgs::Point& point,
     const std::vector<autoware_msgs::Waypoint>& waypoints) const;
   void connectVirtualLastWaypoints(
     autoware_msgs::Lane* expand_lane, LaneDirection direction);
-  inline void updateCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
+  inline void updateCurrentPose(geometry_msgs::PoseStampedConstPtr& msg);
   
   // Added by PHY
   void setLookaheadParamsByVel();
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch
index 359db355..2b5c99c4 100644
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch
@@ -7,7 +7,7 @@
   <arg name="const_velocity" default="5.0"/>
   <arg name="lookahead_ratio" default="2.0"/>
   <arg name="minimum_lookahead_distance" default="6.0"/>
-  <arg name="instance_mode" default="0"/>
+  <arg name="wait_until_getting_velocity_10" default="False" /> 
 
 
   <!-- 0 = waypoints, 1 = provided constant velocity -->
@@ -15,7 +15,6 @@
   
   <!-- rosrun waypoint_follower pure_pursuit -->
   <node pkg="pure_pursuit" type="pure_pursuit" name="pure_pursuit" >
-    <param name="instance_mode" value="$(arg instance_mode)" />
     <param name="is_linear_interpolation" value="$(arg is_linear_interpolation)"/>
     <param name="publishes_for_steering_robot" value="$(arg publishes_for_steering_robot)"/>
     <param name="add_virtual_end_waypoints" value="$(arg add_virtual_end_waypoints)"/>
@@ -24,5 +23,6 @@
     <param name="lookahead_ratio" value="$(arg lookahead_ratio)"/>
     <param name="minimum_lookahead_distance" value="$(arg minimum_lookahead_distance)"/>
     <param name="velocity_source" value="$(arg velocity_source)"/>
+    <param name="wait_until_getting_velocity_10" value="$(arg wait_until_getting_velocity_10)" /> 
   </node>
 </launch>
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch
index f9212a6a..0859fae1 100644
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch
@@ -7,7 +7,7 @@
   <arg name="const_velocity" default="5.0"/>
   <arg name="lookahead_ratio" default="2.0"/>
   <arg name="minimum_lookahead_distance" default="6.0"/>
-  <!-- <arg name="instance_mode" default="0"/> -->
+  <arg name="wait_until_reaching_max_velocity" default="False"/>
 
 
   <!-- 0 = waypoints, 1 = provided constant velocity -->
@@ -15,7 +15,6 @@
   
   <!-- rosrun waypoint_follower pure_pursuit -->
   <node pkg="pure_pursuit" type="pure_pursuit" name="pure_pursuit" output="screen">
-    <!-- <param name="instance_mode" value="$(arg instance_mode)" /> -->
     <param name="is_linear_interpolation" value="$(arg is_linear_interpolation)"/>
     <param name="publishes_for_steering_robot" value="$(arg publishes_for_steering_robot)"/>
     <param name="add_virtual_end_waypoints" value="$(arg add_virtual_end_waypoints)"/>
@@ -24,5 +23,6 @@
     <param name="lookahead_ratio" value="$(arg lookahead_ratio)"/>
     <param name="minimum_lookahead_distance" value="$(arg minimum_lookahead_distance)"/>
     <param name="velocity_source" value="$(arg velocity_source)"/>
+    <param name="wait_until_reaching_max_velocity" value="$(arg wait_until_reaching_max_velocity)" />
   </node>
 </launch>
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/package.xml b/autoware.ai/src/autoware/core_planning/pure_pursuit/package.xml
index 3894e2c9..641a4500 100644
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/package.xml
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/package.xml
@@ -6,17 +6,15 @@
   <maintainer email="h_ohta@ertl.jp">h_ohta</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>amathutils_lib</depend>
   <depend>autoware_config_msgs</depend>
-  <depend>autoware_health_checker</depend>
   <depend>autoware_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>libwaypoint_follower</depend>
   <depend>roscpp</depend>
-  <depend>rostest</depend>
   <depend>rosunit</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
@@ -25,5 +23,6 @@
   <depend>rubis_lib</depend>
   <depend>rubis_msgs</depend>
   <depend>can_data_msgs</depend>
+  <depend>visualization_msgs</depend>
 
 </package>
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp
old mode 100755
new mode 100644
index 1fa981fd..9854919a
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp
@@ -45,9 +45,6 @@ PurePursuitNode::PurePursuitNode()
   , minimum_lookahead_distance_(6.0)
 {
   initForROS();
-  health_checker_ptr_ =
-    std::make_shared<autoware_health_checker::HealthChecker>(nh_, private_nh_);
-  health_checker_ptr_->ENABLE();
   // initialize for PurePursuit
   pp_.setLinearInterpolationParameter(is_linear_interpolation_);
 }
@@ -145,7 +142,8 @@ void PurePursuitNode::initForROS()
   nh_.param("vehicle_info/wheel_base", wheel_base_, 2.7);
 
   private_nh_.param("/pure_pursuit/dynamic_params_flag", dynamic_param_flag_, false);
-  private_nh_.param("/pure_pursuit/instance_mode", rubis::instance_mode_, 0);
+  private_nh_.param("/pure_pursuit/wait_until_reaching_max_velocity", wait_until_reaching_max_velocity_, false);
+  private_nh_.param("/op_common_params/maxVelocity", max_velocity_, 10.0);
   
   if(dynamic_param_flag_){
     XmlRpc::XmlRpcValue xml_list;
@@ -170,28 +168,14 @@ void PurePursuitNode::initForROS()
 
 
   // setup subscriber
-  sub1_ = nh_.subscribe("final_waypoints", 10,
-    &PurePursuitNode::callbackFromWayPoints, this);
-
-  if(rubis::instance_mode_) rubis_pose_sub_ = nh_.subscribe("rubis_current_pose", 10, &PurePursuitNode::callbackFromRubisCurrentPose, this);
-  else pose_sub_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this);
+  final_waypoints_with_pose_twist_sub = nh_.subscribe("/final_waypoints_with_pose_twist", 1, &PurePursuitNode::CallbackFinalWaypointsWithPoseTwist, this); // Def: 10
 
   sub3_ = nh_.subscribe("config/waypoint_follower", 10,
     &PurePursuitNode::callbackFromConfig, this);
   
-  #ifdef SVL
-  velocity_sub_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this);
-  #endif
-
-  #ifdef IONIC
-  car_ctrl_output_sub = nh_.subscribe("car_ctrl_output", 10, &PurePursuitNode::callbackCtrlOutput, this);
-  #endif
-
   // setup publisher
   twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("twist_raw", 10);
-  if(rubis::instance_mode_) rubis_twist_pub_ = nh_.advertise<rubis_msgs::TwistStamped>("rubis_twist_raw", 10);
-
-  pub2_ = nh_.advertise<autoware_msgs::ControlCommandStamped>("ctrl_raw", 10);
+  pure_pursuit_output_pub_ = nh_.advertise<rubis_msgs::PurePursuitOutput>("pure_pursuit_output", 10);
   pub11_ = nh_.advertise<visualization_msgs::Marker>("next_waypoint_mark", 0);
   pub12_ = nh_.advertise<visualization_msgs::Marker>("next_target_mark", 0);
   pub13_ = nh_.advertise<visualization_msgs::Marker>("search_circle_mark", 0);
@@ -211,138 +195,71 @@ void PurePursuitNode::run()
 {
   ros::NodeHandle private_nh("~");
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
+  // Scheduling & Profiling Setup
+  std::string node_name = ros::this_node::getName();
   std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline;
-
-  private_nh.param<int>("/pure_pursuit/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/pure_pursuit/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/pure_pursuit/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pure_pursuit.csv");
-  private_nh.param<int>("/pure_pursuit/rate", rate, 10);
-  private_nh.param("/pure_pursuit/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/pure_pursuit/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/pure_pursuit/task_relative_deadline", task_relative_deadline, (double)10);
-
-  ROS_INFO_STREAM("pure pursuit start");
-
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-  ros::Rate loop_rate(LOOP_RATE_);
-  if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(rate);
-
-  while (ros::ok())
-  {
-    if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){
-      if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-      rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-    }
-
-    ros::spinOnce();
-    if (!is_pose_set_ || !is_waypoint_set_ || !is_velocity_set_)
-    {
-      // ROS_WARN("Necessary topics are not subscribed yet ... ");
-      
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
-
-      if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pure_pursuit.csv");
 
-      loop_rate.sleep();
-      continue;
-    }
-
-    pp_.setLookaheadDistance(computeLookaheadDistance());
-    pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);
-
-    double kappa = 0;
-    bool can_get_curvature = pp_.canGetCurvature(&kappa);
-
-    publishTwistStamped(can_get_curvature, kappa);
-    publishControlCommandStamped(can_get_curvature, kappa);
-    health_checker_ptr_->NODE_ACTIVATE();
-    health_checker_ptr_->CHECK_RATE("topic_rate_vehicle_cmd_slow", 8, 5, 1,
-      "topic vehicle_cmd publish rate slow.");
-    // for visualization with Rviz
-    pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint()));
-    pub13_.publish(displaySearchRadius(
-      pp_.getCurrentPose().position, pp_.getLookaheadDistance()));
-    pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget()));
-    pub15_.publish(displayTrajectoryCircle(
-        waypoint_follower::generateTrajectoryCircle(
-          pp_.getPoseOfNextTarget(), pp_.getCurrentPose())));
-    if (add_virtual_end_waypoints_)
-    {
-      pub18_.publish(
-        displayExpandWaypoints(pp_.getCurrentWaypoints(), expand_size_));
-    }
-    std_msgs::Float32 angular_gravity_msg;
-    angular_gravity_msg.data =
-      computeAngularGravity(computeCommandVelocity(), kappa);
-    pub16_.publish(angular_gravity_msg);
-
-    publishDeviationCurrentPosition(
-      pp_.getCurrentPose().position, pp_.getCurrentWaypoints());
-
-    is_pose_set_ = false;
-    is_velocity_set_ = false;
-    is_waypoint_set_ = false;
-
-    if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
-
-    if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){
-      if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-      rubis::sched::task_state_ = TASK_STATE_READY;
-    }
-
-    loop_rate.sleep();
-  }
+  int rate;
+  private_nh.param<int>(node_name+"/rate", rate, 10);
+
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
+
+  rubis::init_task_profiling(task_response_time_filename);
+
+  ros::spin();
 }
 
-void PurePursuitNode::publishTwistStamped(
-  const bool& can_get_curvature, const double& kappa) const
+
+geometry_msgs::TwistStamped PurePursuitNode::calculateTwistStamped(
+    const bool& can_get_curvature, const double& kappa) const
 {
+  static bool is_started = false;
   geometry_msgs::TwistStamped ts;
   ts.header.stamp = ros::Time::now();
   ts.twist.linear.x = can_get_curvature ? computeCommandVelocity() : 0;
   ts.twist.angular.z = can_get_curvature ? kappa * ts.twist.linear.x : 0;
-  twist_pub_.publish(ts);
-
-  if(rubis::instance_mode_ && rubis::instance_mode_ != RUBIS_NO_INSTANCE){
-    rubis_msgs::TwistStamped rubis_ts;
-    rubis_ts.instance = rubis::instance_;
-    rubis_ts.msg = ts;
-    rubis_twist_pub_.publish(rubis_ts);
+  
+  // Wait until velocity is fast enough
+  if(!is_started){
+    if(wait_until_reaching_max_velocity_ && ts.twist.linear.x < max_velocity_){
+      ts.twist.linear.x = 0.0;    
+      return ts;
+    }
+    else{
+      is_started = true;
+    }
   }
 
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+  return ts;
 }
 
-void PurePursuitNode::publishControlCommandStamped(
+autoware_msgs::ControlCommandStamped PurePursuitNode::calculateControlCommandStamped(
   const bool& can_get_curvature, const double& kappa) const
 {
+  autoware_msgs::ControlCommandStamped ccs;
+
   if (!publishes_for_steering_robot_)
   {
-    return;
+    return ccs;
   }
-
-  autoware_msgs::ControlCommandStamped ccs;
-  ccs.header.stamp = ros::Time::now();
+  
   ccs.cmd.linear_velocity = can_get_curvature ? computeCommandVelocity() : 0;
   ccs.cmd.linear_acceleration = can_get_curvature ? computeCommandAccel() : 0;
   ccs.cmd.steering_angle =
     can_get_curvature ? convertCurvatureToSteeringAngle(wheel_base_, kappa) : 0;
 
-  pub2_.publish(ccs);
+  return ccs;
 }
 
 double PurePursuitNode::computeLookaheadDistance() const
@@ -401,6 +318,7 @@ double PurePursuitNode::computeCommandVelocity() const
 
 double PurePursuitNode::computeCommandAccel() const
 {
+  if(pp_.getCurrentWaypoints().size() < 2) return 0.0;
   const geometry_msgs::Pose current_pose = pp_.getCurrentPose();
   const geometry_msgs::Pose target_pose =
     pp_.getCurrentWaypoints().at(1).pose.pose;
@@ -412,6 +330,7 @@ double PurePursuitNode::computeCommandAccel() const
   const double v0 = current_linear_velocity_;
   const double v = computeCommandVelocity();
   const double a = getSgn() * (v * v - v0 * v0) / (2 * x);
+
   return a;
 }
 
@@ -455,7 +374,7 @@ void PurePursuitNode::publishDeviationCurrentPosition(
   pub17_.publish(msg);
 }
 
-inline void PurePursuitNode::updateCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg){
+inline void PurePursuitNode::updateCurrentPose(geometry_msgs::PoseStampedConstPtr& msg){
 #ifndef USE_WAYPOINT_ORIENTATION
   pp_.setCurrentPose(msg);
 #else
@@ -468,27 +387,6 @@ inline void PurePursuitNode::updateCurrentPose(const geometry_msgs::PoseStampedC
   is_pose_set_ = true;
 }
 
-void PurePursuitNode::callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
-{
-  updateCurrentPose(msg);
-}
-
-void PurePursuitNode::callbackFromRubisCurrentPose(const rubis_msgs::PoseStampedConstPtr& _msg)
-{
-  geometry_msgs::PoseStampedConstPtr msg = boost::make_shared<const geometry_msgs::PoseStamped>(_msg->msg);
-  rubis::instance_ = _msg->instance;
-  updateCurrentPose(msg);
-}
-
-#ifdef SVL
-void PurePursuitNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  current_linear_velocity_ = msg->twist.linear.x;
-  pp_.setCurrentVelocity(current_linear_velocity_);
-  is_velocity_set_ = true;
-}
-#endif
-
 #ifdef IONIC
 void PurePursuitNode::callbackCtrlOutput(const can_data_msgs::Car_ctrl_output::ConstPtr &msg)
 {
@@ -532,19 +430,39 @@ double PurePursuitNode::findWayPointVelocity(autoware_msgs::Waypoint msg){
   return way_points_velocity_[idx];
 }
 
-void PurePursuitNode::callbackFromWayPoints(
-  const autoware_msgs::LaneConstPtr& msg)
-{
+void PurePursuitNode::CallbackFinalWaypointsWithPoseTwist(const rubis_msgs::LaneWithPoseTwistConstPtr& msg)
+{ 
+  rubis::start_task_profiling();
+  rubis::instance_ = msg->instance;
+  rubis::lidar_instance_ = msg->lidar_instance;
+
+  // Update waypoints (lane)
+  lane_ = msg->lane;
+
+  // Update pose
+  geometry_msgs::PoseStampedConstPtr pose_ptr(new geometry_msgs::PoseStamped(msg->pose));
+  updateCurrentPose(pose_ptr);
+
+  // Update twist
+  current_linear_velocity_ = msg->twist.twist.linear.x;
+  pp_.setCurrentVelocity(current_linear_velocity_);
+  is_velocity_set_ = true;
+
+  if(lane_.waypoints.size() < 1){
+    std::cout<<"[Pure Pursuit] No input trajectory"<<std::endl;
+    return;
+  }
+
   if(use_algorithm_){
-    command_linear_velocity_ = findWayPointVelocity(msg->waypoints.at(0));
+    command_linear_velocity_ = findWayPointVelocity(lane_.waypoints.at(0));
   }
   else{
-    command_linear_velocity_ = (!msg->waypoints.empty()) ? msg->waypoints.at(0).twist.twist.linear.x : 0;
+    command_linear_velocity_ = (!lane_.waypoints.empty()) ? lane_.waypoints.at(0).twist.twist.linear.x : 0;
   }
 
-  geometry_msgs::Point curr_point = msg->waypoints.at(0).pose.pose.position;
-  geometry_msgs::Point near_point = msg->waypoints.at(std::min(3, (int)msg->waypoints.size() - 1)).pose.pose.position;
-  geometry_msgs::Point far_point = msg->waypoints.at(std::min(30, (int)msg->waypoints.size() - 1)).pose.pose.position;
+  geometry_msgs::Point curr_point = lane_.waypoints.at(0).pose.pose.position;
+  geometry_msgs::Point near_point = lane_.waypoints.at(std::min(3, (int)lane_.waypoints.size() - 1)).pose.pose.position;
+  geometry_msgs::Point far_point = lane_.waypoints.at(std::min(30, (int)lane_.waypoints.size() - 1)).pose.pose.position;
 
   double deg_1 = atan2((near_point.y - curr_point.y), (near_point.x - curr_point.x)) / 3.14 * 180;
   double deg_2 = atan2((far_point.y - curr_point.y), (far_point.x - curr_point.x)) / 3.14 * 180;
@@ -561,9 +479,9 @@ void PurePursuitNode::callbackFromWayPoints(
   
   if (add_virtual_end_waypoints_)
   {
-    const LaneDirection solved_dir = getLaneDirection(*msg);
+    const LaneDirection solved_dir = getLaneDirection(lane_);
     direction_ = (solved_dir != LaneDirection::Error) ? solved_dir : direction_;
-    autoware_msgs::Lane expanded_lane(*msg);
+    autoware_msgs::Lane expanded_lane(lane_);
     expand_size_ = -expanded_lane.waypoints.size();
     connectVirtualLastWaypoints(&expanded_lane, direction_);
     expand_size_ += expanded_lane.waypoints.size();
@@ -572,13 +490,58 @@ void PurePursuitNode::callbackFromWayPoints(
   }
   else
   {
-    pp_.setCurrentWaypoints(msg->waypoints);
+    pp_.setCurrentWaypoints(lane_.waypoints);    
   }
   is_waypoint_set_ = true;
 
-#ifdef USE_WAYPOINT_ORIENTATION
-  waypoint_pose_ = msg->waypoints[0].pose;
-#endif
+  #ifdef USE_WAYPOINT_ORIENTATION
+    waypoint_pose_ = lane_.waypoints[0].pose;
+  #endif
+
+  // After spinOnce
+  pp_.setLookaheadDistance(computeLookaheadDistance());
+  pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);
+
+  double kappa = 0;
+  bool can_get_curvature = pp_.canGetCurvature(&kappa);
+
+  rubis_msgs::PurePursuitOutput pure_pursuit_output_msg;
+  pure_pursuit_output_msg.instance = rubis::instance_;
+  pure_pursuit_output_msg.lidar_instance = rubis::lidar_instance_;
+  pure_pursuit_output_msg.lidar_instance = rubis::vision_instance_;
+  pure_pursuit_output_msg.header = msg->header;
+  pure_pursuit_output_msg.twist = calculateTwistStamped(can_get_curvature, kappa);
+  pure_pursuit_output_msg.twist.header = msg->header;
+  pure_pursuit_output_msg.ctrl = calculateControlCommandStamped(can_get_curvature, kappa);
+  pure_pursuit_output_msg.ctrl.header = msg->header;
+  
+  // for visualization with Rviz
+  pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint()));
+  pub13_.publish(displaySearchRadius(
+    pp_.getCurrentPose().position, pp_.getLookaheadDistance()));
+  pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget()));
+  pub15_.publish(displayTrajectoryCircle(
+      waypoint_follower::generateTrajectoryCircle(
+        pp_.getPoseOfNextTarget(), pp_.getCurrentPose())));
+  if (add_virtual_end_waypoints_)
+  {
+    pub18_.publish(
+      displayExpandWaypoints(pp_.getCurrentWaypoints(), expand_size_));
+  }
+  std_msgs::Float32 angular_gravity_msg;
+  angular_gravity_msg.data =
+    computeAngularGravity(computeCommandVelocity(), kappa);
+  pub16_.publish(angular_gravity_msg);
+
+  publishDeviationCurrentPosition(
+    pp_.getCurrentPose().position, pp_.getCurrentWaypoints());
+
+  is_pose_set_ = false;
+  is_velocity_set_ = false;
+  is_waypoint_set_ = false;
+  
+  pure_pursuit_output_pub_.publish(pure_pursuit_output_msg);
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
 void PurePursuitNode::connectVirtualLastWaypoints(
@@ -611,3 +574,4 @@ double convertCurvatureToSteeringAngle(
 }
 
 }  // namespace waypoint_follower
+
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_viz.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_viz.cpp
index 8e31fbbe..08a4718a 100644
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_viz.cpp
+++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_viz.cpp
@@ -60,9 +60,9 @@ visualization_msgs::Marker displayNextTarget(geometry_msgs::Point target)
   green.r = 0.0;
   green.g = 1.0;
   marker.color = green;
-  marker.scale.x = 1.0;
-  marker.scale.y = 1.0;
-  marker.scale.z = 1.0;
+  marker.scale.x = 3.0;
+  marker.scale.y = 3.0;
+  marker.scale.z = 3.0;
   marker.frame_locked = true;
   return marker;
 }
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp
deleted file mode 100644
index 5df8b48e..00000000
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- * Copyright 2018-2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <ros/ros.h>
-#include <gtest/gtest.h>
-#include <pure_pursuit/pure_pursuit_core.h>
-
-namespace waypoint_follower
-{
-class PurePursuitNodeTestSuite : public ::testing::Test
-{
-protected:
-  std::unique_ptr<PurePursuitNode> obj_;
-  virtual void SetUp()
-  {
-    obj_ = std::unique_ptr<PurePursuitNode>(new PurePursuitNode());
-    obj_->add_virtual_end_waypoints_ = true;
-  }
-  virtual void TearDown()
-  {
-    obj_.reset();
-  }
-
-public:
-  PurePursuitNodeTestSuite() {}
-  ~PurePursuitNodeTestSuite() {}
-  LaneDirection getDirection()
-  {
-    return obj_->direction_;
-  }
-  void ppCallbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg)
-  {
-    obj_->callbackFromWayPoints(msg);
-  }
-  void ppConnectVirtualLastWaypoints(
-    autoware_msgs::Lane* expand_lane, LaneDirection direction)
-  {
-    obj_->connectVirtualLastWaypoints(expand_lane, direction);
-  }
-};
-
-TEST_F(PurePursuitNodeTestSuite, inputPositivePath)
-{
-  autoware_msgs::Lane original_lane;
-  original_lane.waypoints.resize(3, autoware_msgs::Waypoint());
-  for (int i = 0; i < 3; i++)
-  {
-    original_lane.waypoints[i].pose.pose.position.x = i;
-    original_lane.waypoints[i].pose.pose.orientation =
-      tf::createQuaternionMsgFromYaw(0.0);
-  }
-  const autoware_msgs::LaneConstPtr
-    lp(boost::make_shared<autoware_msgs::Lane>(original_lane));
-  ppCallbackFromWayPoints(lp);
-  ASSERT_EQ(getDirection(), LaneDirection::Forward)
-    << "direction is not matching to positive lane.";
-}
-
-TEST_F(PurePursuitNodeTestSuite, inputNegativePath)
-{
-  autoware_msgs::Lane original_lane;
-  original_lane.waypoints.resize(3, autoware_msgs::Waypoint());
-  for (int i = 0; i < 3; i++)
-  {
-    original_lane.waypoints[i].pose.pose.position.x = -i;
-    original_lane.waypoints[i].pose.pose.orientation =
-      tf::createQuaternionMsgFromYaw(0.0);
-  }
-  const autoware_msgs::LaneConstPtr
-    lp(boost::make_shared<autoware_msgs::Lane>(original_lane));
-  ppCallbackFromWayPoints(lp);
-  ASSERT_EQ(getDirection(), LaneDirection::Backward)
-    << "direction is not matching to negative lane.";
-}
-// If original lane is empty, new lane is also empty.
-TEST_F(PurePursuitNodeTestSuite, inputEmptyLane)
-{
-  autoware_msgs::Lane original_lane, new_lane;
-  ppConnectVirtualLastWaypoints(&new_lane, LaneDirection::Forward);
-  ASSERT_EQ(original_lane.waypoints.size(), new_lane.waypoints.size())
-    << "Input empty lane, and output is not empty";
-}
-
-// If the original lane exceeds 2 points,
-// the additional part will be updated at
-// the interval of the first 2 points.
-TEST_F(PurePursuitNodeTestSuite, inputNormalLane)
-{
-  autoware_msgs::Lane original_lane;
-  original_lane.waypoints.resize(2, autoware_msgs::Waypoint());
-  for (int i = 0; i < 2; i++)
-  {
-    original_lane.waypoints[i].pose.pose.position.x = i;
-  }
-  autoware_msgs::Lane new_lane(original_lane);
-  ppConnectVirtualLastWaypoints(&new_lane, LaneDirection::Forward);
-
-  ASSERT_LT(original_lane.waypoints.size(), new_lane.waypoints.size())
-    << "Fail to expand waypoints";
-}
-}  // namespace waypoint_follower
-
-int main(int argc, char** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  ros::init(argc, argv, "PurePursuitTest");
-  return RUN_ALL_TESTS();
-}
diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test b/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test
deleted file mode 100644
index caba2948..00000000
--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
-  <test test-name="test-pure_pursuit" pkg="pure_pursuit" type="test-pure_pursuit" name="test"/>
-</launch>
diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/twist_filter/CMakeLists.txt
index 80cc5778..455f3bbf 100644
--- a/autoware.ai/src/autoware/core_planning/twist_filter/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_planning/twist_filter/CMakeLists.txt
@@ -5,12 +5,11 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 
 find_package(
   catkin REQUIRED COMPONENTS
     autoware_config_msgs
-    autoware_health_checker
     autoware_msgs
     geometry_msgs
     roscpp
@@ -70,11 +69,3 @@ install(
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
 )
-
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(test_twist_filter_node test/test_twist_filter_node.test test/src/test_twist_filter_node.cpp src/twist_filter_node.cpp src/twist_filter.cpp)
-  add_dependencies(test_twist_filter_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME} ${PROJECT_NAME}_core)
-  target_link_libraries(test_twist_filter_node ${catkin_LIBRARIES} ${PROJECT_NAME}_core)
-  roslint_add_test()
-endif ()
diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h
index 1355c302..f95d59ed 100644
--- a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h
+++ b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h
@@ -17,16 +17,18 @@
 #ifndef TWIST_FILTER_TWIST_FILTER_NODE_H
 #define TWIST_FILTER_TWIST_FILTER_NODE_H
 
+#include <ros/ros.h>
 #include "twist_filter/twist_filter.h"
 #include <memory>
 #include <std_msgs/Float32.h>
 #include <geometry_msgs/TwistStamped.h>
-#include <autoware_health_checker/health_checker/health_checker.h>
 #include <autoware_msgs/ControlCommandStamped.h>
 #include <autoware_config_msgs/ConfigTwistFilter.h>
 #include <std_msgs/Bool.h>
 #include <rubis_msgs/TwistStamped.h>
+#include <rubis_msgs/PurePursuitOutput.h>
 #include <rubis_lib/sched.hpp>
+#include <autoware_msgs/VehicleCmd.h>
 
 namespace twist_filter_node
 {
@@ -38,7 +40,6 @@ class TwistFilterNode
 private:
   ros::NodeHandle nh_, private_nh_;
   std::shared_ptr<twist_filter::TwistFilter> twist_filter_ptr_;
-  autoware_health_checker::HealthChecker health_checker_;
 
   // publishers
   ros::Publisher twist_pub_, rubis_twist_pub_, ctrl_pub_;
@@ -46,28 +47,31 @@ class TwistFilterNode
   ros::Publisher ctrl_lacc_limit_debug_pub_, ctrl_ljerk_limit_debug_pub_;
   ros::Publisher twist_lacc_result_pub_, twist_ljerk_result_pub_;
   ros::Publisher ctrl_lacc_result_pub_, ctrl_ljerk_result_pub_;
+  ros::Publisher rubis_vehicle_cmd_pub_, vehicle_cmd_pub_;
 
   // subscribers
-  ros::Subscriber twist_sub_, rubis_twist_sub_, ctrl_sub_, config_sub_;
+  ros::Subscriber pure_pursuit_output_sub_, config_sub_;
   // Added by PHY
   ros::Subscriber emergency_stop_sub_;
   
-  bool emergency_stop_;
+  bool emergency_stop_, current_emergency_stop_;
   int max_stop_count_;
   int current_stop_count_;
 
   void configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr& config);
-  void twistCmdCallback(const geometry_msgs::TwistStampedConstPtr& msg);
-  void rubisTwistCmdCallback(const rubis_msgs::TwistStampedConstPtr& _msg);
-  inline void publishTwist(const geometry_msgs::TwistStampedConstPtr& msg);
+  void purePursuitOutputCallback(const rubis_msgs::PurePursuitOutputConstPtr& msg);
+  inline geometry_msgs::TwistStamped calculateTwist(const geometry_msgs::TwistStampedConstPtr& msg);
   void ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg);
+  void _ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg);
   void checkTwist(const twist_filter::Twist twist, const twist_filter::Twist twist_prev, const double& dt);
   void checkCtrl(const twist_filter::Ctrl ctrl, const twist_filter::Ctrl ctrl_prev, const double& dt);
 
   //Added by PHY
   void emergencyStopCallback(const std_msgs::Bool& msg);
+  void _emergencyStopCallback();
 };
 
 }  // namespace twist_filter_node
 
 #endif  // TWIST_FILTER_TWIST_FILTER_NODE_H
+
diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch
index 81dc1fed..9b3b8b61 100644
--- a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch
+++ b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch
@@ -8,7 +8,6 @@
   <arg name="lowpass_gain_angular_z" default="0.0" />
   <arg name="lowpass_gain_steering_angle" default="0.0" />
   <arg name="max_stop_count" default="40" /> <!-- 10 = 1 second -->
-  <arg name="instance_mode" defaut="0" />
 
   <param name="vehicle_info/wheel_base" value="$(arg wheel_base)" />
 
@@ -24,12 +23,10 @@
     <param name="lowpass_gain_angular_z" value="$(arg lowpass_gain_angular_z)" />
     <param name="lowpass_gain_steering_angle" value="$(arg lowpass_gain_steering_angle)" />
     <param name="max_stop_count" value="$(arg max_stop_count)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <node pkg="twist_gate" type="twist_gate" name="twist_gate">
     <param name="loop_rate" value="$(arg loop_rate)" />
     <param name="use_decision_maker" value="$(arg use_decision_maker)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 </launch>
diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch
index 0b470a12..a4bfdcbf 100644
--- a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch
+++ b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch
@@ -8,7 +8,6 @@
   <arg name="lowpass_gain_angular_z" default="0.0" />
   <arg name="lowpass_gain_steering_angle" default="0.0" />
   <arg name="max_stop_count" default="40" /> <!-- 10 = 1 second -->
-  <!-- <arg name="instance_mode" defaut="0" /> -->
 
   <param name="vehicle_info/wheel_base" value="$(arg wheel_base)" />
 
@@ -17,19 +16,17 @@
   <arg name="use_decision_maker" default="false" />
 
   <!-- rosrun waypoint_follower twist_filter -->
-  <node pkg="twist_filter" type="twist_filter" name="twist_filter">
+  <node pkg="twist_filter" type="twist_filter" name="twist_filter" output="screen">
     <param name="lateral_accel_limit" value="$(arg lateral_accel_limit)" />
     <param name="lateral_jerk_limit" value="$(arg lateral_jerk_limit)" />
     <param name="lowpass_gain_linear_x" value="$(arg lowpass_gain_linear_x)" />
     <param name="lowpass_gain_angular_z" value="$(arg lowpass_gain_angular_z)" />
     <param name="lowpass_gain_steering_angle" value="$(arg lowpass_gain_steering_angle)" />
     <param name="max_stop_count" value="$(arg max_stop_count)" />
-    <!-- <param name="instance_mode" value="$(arg instance_mode)" /> -->
   </node>
 
-  <node pkg="twist_gate" type="twist_gate" name="twist_gate">
+  <!-- <node pkg="twist_gate" type="twist_gate" name="twist_gate">
     <param name="loop_rate" value="$(arg loop_rate)" />
     <param name="use_decision_maker" value="$(arg use_decision_maker)" />
-    <!-- <param name="instance_mode" value="$(arg instance_mode)" /> -->
-  </node>
+  </node> -->
 </launch>
diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/package.xml b/autoware.ai/src/autoware/core_planning/twist_filter/package.xml
index b41c77fe..95156be0 100644
--- a/autoware.ai/src/autoware/core_planning/twist_filter/package.xml
+++ b/autoware.ai/src/autoware/core_planning/twist_filter/package.xml
@@ -6,11 +6,10 @@
   <maintainer email="h_ohta@ertl.jp">h_ohta</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_config_msgs</depend>
-  <depend>autoware_health_checker</depend>
   <depend>autoware_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>roscpp</depend>
diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp
index f6e797fd..0e6d8813 100644
--- a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp
+++ b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp
@@ -16,298 +16,266 @@
 
 #include "twist_filter/twist_filter_node.h"
 
-namespace twist_filter_node
-{
-TwistFilterNode::TwistFilterNode() : nh_(), private_nh_("~"), health_checker_(nh_, private_nh_)
-{
-  // Parameters
-  twist_filter::Configuration twist_filter_config;
-  nh_.param("vehicle_info/wheel_base", twist_filter_config.wheel_base, 2.7);
-  nh_.param("twist_filter/lateral_accel_limit", twist_filter_config.lateral_accel_limit, 5.0);
-  nh_.param("twist_filter/lateral_jerk_limit", twist_filter_config.lateral_jerk_limit, 5.0);
-  nh_.param("twist_filter/lowpass_gain_linear_x", twist_filter_config.lowpass_gain_linear_x, 0.0);
-  nh_.param("twist_filter/lowpass_gain_angular_z", twist_filter_config.lowpass_gain_angular_z, 0.0);
-  nh_.param("twist_filter/lowpass_gain_steering_angle", twist_filter_config.lowpass_gain_steering_angle, 0.0);
-  nh_.param("twist_filter/max_stop_count", max_stop_count_, 30);
-  nh_.param("twist_filter/instance_mode", rubis::instance_mode_, 0);
-  twist_filter_ptr_ = std::make_shared<twist_filter::TwistFilter>(twist_filter_config);
-  emergency_stop_ = false;
-  current_stop_count_ = 0;
-
-  // Enable health checker
-  health_checker_.ENABLE();
-
-  // Subscribe
-  if(rubis::instance_mode_) rubis_twist_sub_ = nh_.subscribe("rubis_twist_raw", 1, &TwistFilterNode::rubisTwistCmdCallback, this);
-  else twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this);
-  ctrl_sub_ = nh_.subscribe("ctrl_raw", 1, &TwistFilterNode::ctrlCmdCallback, this);
-  config_sub_ = nh_.subscribe("config/twist_filter", 10, &TwistFilterNode::configCallback, this);
-  emergency_stop_sub_ = nh_.subscribe("emergency_stop", 1 ,&TwistFilterNode::emergencyStopCallback, this);
-
-  /*  RT Scheduling setup  */
-  // twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this);
-  // ctrl_sub_ = nh_.subscribe("ctrl_raw", 1, &TwistFilterNode::ctrlCmdCallback, this);
-  // config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistFilterNode::configCallback, this); //origin 10
-  // emergency_stop_sub_ = nh_.subscribe("emergency_stop", 1 ,&TwistFilterNode::emergencyStopCallback, this);
-
-  // Publish
-  twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("twist_cmd", 5);
-  if(rubis::instance_mode_) rubis_twist_pub_ = nh_.advertise<rubis_msgs::TwistStamped>("rubis_twist_cmd", 5);
-  ctrl_pub_ = nh_.advertise<autoware_msgs::ControlCommandStamped>("ctrl_cmd", 5);
-  twist_lacc_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/twist/lateral_accel", 5);
-  twist_ljerk_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/twist/lateral_jerk", 5);
-  ctrl_lacc_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/ctrl/lateral_accel", 5);
-  ctrl_ljerk_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/ctrl/lateral_jerk", 5);
-  twist_lacc_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/twist/lateral_accel", 5);
-  twist_ljerk_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/twist/lateral_jerk", 5);
-  ctrl_lacc_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/ctrl/lateral_accel", 5);
-  ctrl_ljerk_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/ctrl/lateral_jerk", 5);
+namespace twist_filter_node {
+TwistFilterNode::TwistFilterNode() : nh_(), private_nh_("~") {
+    // Parameters
+    twist_filter::Configuration twist_filter_config;
+    nh_.param("vehicle_info/wheel_base", twist_filter_config.wheel_base, 2.7);
+    nh_.param("twist_filter/lateral_accel_limit", twist_filter_config.lateral_accel_limit, 5.0);
+    nh_.param("twist_filter/lateral_jerk_limit", twist_filter_config.lateral_jerk_limit, 5.0);
+    nh_.param("twist_filter/lowpass_gain_linear_x", twist_filter_config.lowpass_gain_linear_x, 0.0);
+    nh_.param("twist_filter/lowpass_gain_angular_z", twist_filter_config.lowpass_gain_angular_z, 0.0);
+    nh_.param("twist_filter/lowpass_gain_steering_angle", twist_filter_config.lowpass_gain_steering_angle, 0.0);
+    nh_.param("twist_filter/max_stop_count", max_stop_count_, 30);
+    twist_filter_ptr_ = std::make_shared<twist_filter::TwistFilter>(twist_filter_config);
+    emergency_stop_ = false;
+    current_emergency_stop_ = false;
+    current_stop_count_ = 0;
 
+    // Subscribe
+    pure_pursuit_output_sub_ = nh_.subscribe("pure_pursuit_output", 1, &TwistFilterNode::purePursuitOutputCallback, this);
+    config_sub_ = nh_.subscribe("config/twist_filter", 10, &TwistFilterNode::configCallback, this);
+    emergency_stop_sub_ = nh_.subscribe("emergency_stop", 1, &TwistFilterNode::emergencyStopCallback, this);
+
+    // Publish
+    // twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("twist_cmd", 5);
+    // rubis_twist_pub_ = nh_.advertise<rubis_msgs::TwistStamped>("rubis_twist_cmd", 5);
+    vehicle_cmd_pub_ = nh_.advertise<autoware_msgs::VehicleCmd>("vehicle_cmd", 5);
+    // ctrl_pub_ = nh_.advertise<autoware_msgs::ControlCommandStamped>("ctrl_cmd", 5);
+    twist_lacc_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/twist/lateral_accel", 5);
+    twist_ljerk_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/twist/lateral_jerk", 5);
+    ctrl_lacc_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/ctrl/lateral_accel", 5);
+    ctrl_ljerk_limit_debug_pub_ = private_nh_.advertise<std_msgs::Float32>("limitation_debug/ctrl/lateral_jerk", 5);
+    twist_lacc_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/twist/lateral_accel", 5);
+    twist_ljerk_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/twist/lateral_jerk", 5);
+    ctrl_lacc_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/ctrl/lateral_accel", 5);
+    ctrl_ljerk_result_pub_ = private_nh_.advertise<std_msgs::Float32>("result/ctrl/lateral_jerk", 5);
 }
 
-void TwistFilterNode::configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr& config_msg)
-{
-  twist_filter::Configuration twist_filter_config;
-  twist_filter_config.lateral_accel_limit = config_msg->lateral_accel_limit;
-  twist_filter_config.lateral_jerk_limit = config_msg->lateral_jerk_limit;
-  twist_filter_config.lowpass_gain_linear_x = config_msg->lowpass_gain_linear_x;
-  twist_filter_config.lowpass_gain_angular_z = config_msg->lowpass_gain_angular_z;
-  twist_filter_config.lowpass_gain_steering_angle = config_msg->lowpass_gain_steering_angle;
-  twist_filter_ptr_->setConfiguration(twist_filter_config);
+void TwistFilterNode::configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr &config_msg) {
+    twist_filter::Configuration twist_filter_config;
+    twist_filter_config.lateral_accel_limit = config_msg->lateral_accel_limit;
+    twist_filter_config.lateral_jerk_limit = config_msg->lateral_jerk_limit;
+    twist_filter_config.lowpass_gain_linear_x = config_msg->lowpass_gain_linear_x;
+    twist_filter_config.lowpass_gain_angular_z = config_msg->lowpass_gain_angular_z;
+    twist_filter_config.lowpass_gain_steering_angle = config_msg->lowpass_gain_steering_angle;
+    twist_filter_ptr_->setConfiguration(twist_filter_config);
 }
 
-inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConstPtr& msg){
-  const twist_filter::Twist twist = { msg->twist.linear.x, msg->twist.angular.z };
-  ros::Time current_time = ros::Time::now();
-
-  static ros::Time last_callback_time = current_time;
-  static twist_filter::Twist twist_prev = twist;
-
-  double time_elapsed = (current_time - last_callback_time).toSec();
-
-  health_checker_.NODE_ACTIVATE();
-  checkTwist(twist, twist_prev, time_elapsed);
-
-  twist_filter::Twist twist_out = twist;
-
-  // Apply lateral limit
-  auto twist_limit_result = twist_filter_ptr_->lateralLimitTwist(twist, twist_prev, time_elapsed);
-  if (twist_limit_result)
-  {
-    twist_out = twist_limit_result.get();
-  }
-
-  // Publish lateral accel and jerk before smoothing
-  auto lacc_no_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist);
-  if (lacc_no_smoothed_result)
-  {
-    std_msgs::Float32 lacc_msg_debug;
-    lacc_msg_debug.data = lacc_no_smoothed_result.get();
-    twist_lacc_limit_debug_pub_.publish(lacc_msg_debug);
-  }
-  auto ljerk_no_smoothed_result = twist_filter_ptr_->calcLjerkWithAngularZ(twist, twist_prev, time_elapsed);
-  if (ljerk_no_smoothed_result)
-  {
-    std_msgs::Float32 ljerk_msg_debug;
-    ljerk_msg_debug.data = ljerk_no_smoothed_result.get();
-    twist_ljerk_limit_debug_pub_.publish(ljerk_msg_debug);
-  }
-
-  // Smoothing
-  twist_out = twist_filter_ptr_->smoothTwist(twist_out);
-
-  // Smoothed value publish
-  geometry_msgs::TwistStamped out_msg = *msg;
-  if(emergency_stop_ == false){
-    out_msg.twist.linear.x = twist_out.lx;
-    out_msg.twist.angular.z = twist_out.az;
-  }
-  else{
-    out_msg.twist.linear.x = -1000;
-    out_msg.twist.angular.z = 0;
-  }
-  twist_pub_.publish(out_msg);
-  if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){
-    rubis_msgs::TwistStamped rubis_out_msg;
-    rubis_out_msg.instance = rubis::instance_;
-    rubis_out_msg.msg = out_msg;
-    rubis_twist_pub_.publish(rubis_out_msg);
-  }
-
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
-
-  // Publish lateral accel and jerk after smoothing
-  auto lacc_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist_out);
-  if (lacc_smoothed_result)
-  {
-    std_msgs::Float32 lacc_msg;
-    lacc_msg.data = lacc_smoothed_result.get();
-    twist_lacc_result_pub_.publish(lacc_msg);
-  }
-  auto ljerk_smoothed_result = twist_filter_ptr_->calcLjerkWithAngularZ(twist_out, twist_prev, time_elapsed);
-  if (ljerk_smoothed_result)
-  {
-    std_msgs::Float32 ljerk_msg;
-    ljerk_msg.data = ljerk_smoothed_result.get();
-    twist_ljerk_result_pub_.publish(ljerk_msg);
-  }
-
-  // Preserve value and time
-  twist_prev = twist_out;
-  last_callback_time = current_time;
+inline geometry_msgs::TwistStamped TwistFilterNode::calculateTwist(const geometry_msgs::TwistStampedConstPtr &msg) {
+    const twist_filter::Twist twist = {msg->twist.linear.x, msg->twist.angular.z};
+    ros::Time current_time = msg->header.stamp;
+
+    static ros::Time last_callback_time = current_time;
+    static twist_filter::Twist twist_prev = twist;
+
+    double time_elapsed = (current_time - last_callback_time).toSec();
+
+    twist_filter::Twist twist_out = twist;
+
+    // Apply lateral limit
+    auto twist_limit_result = twist_filter_ptr_->lateralLimitTwist(twist, twist_prev, time_elapsed);
+    if (twist_limit_result) {
+        twist_out = twist_limit_result.get();
+    }
+
+    // Publish lateral accel and jerk before smoothing
+    auto lacc_no_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist);
+    if (lacc_no_smoothed_result) {
+        std_msgs::Float32 lacc_msg_debug;
+        lacc_msg_debug.data = lacc_no_smoothed_result.get();
+        twist_lacc_limit_debug_pub_.publish(lacc_msg_debug);
+    }
+    auto ljerk_no_smoothed_result = twist_filter_ptr_->calcLjerkWithAngularZ(twist, twist_prev, time_elapsed);
+    if (ljerk_no_smoothed_result) {
+        std_msgs::Float32 ljerk_msg_debug;
+        ljerk_msg_debug.data = ljerk_no_smoothed_result.get();
+        twist_ljerk_limit_debug_pub_.publish(ljerk_msg_debug);
+    }
+
+    // Smoothing
+    twist_out = twist_filter_ptr_->smoothTwist(twist_out);
+
+    // Smoothed value publish
+    geometry_msgs::TwistStamped out_msg = *msg;
+
+    if (emergency_stop_ == false) {
+        out_msg.twist.linear.x = twist_out.lx;
+        out_msg.twist.angular.z = twist_out.az;
+    } else {
+        out_msg.twist.linear.x = 0;
+        out_msg.twist.angular.z = 0;
+    }
+
+    // Publish lateral accel and jerk after smoothing
+    auto lacc_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist_out);
+    if (lacc_smoothed_result) {
+        std_msgs::Float32 lacc_msg;
+        lacc_msg.data = lacc_smoothed_result.get();
+        twist_lacc_result_pub_.publish(lacc_msg);
+    }
+    auto ljerk_smoothed_result = twist_filter_ptr_->calcLjerkWithAngularZ(twist_out, twist_prev, time_elapsed);
+    if (ljerk_smoothed_result) {
+        std_msgs::Float32 ljerk_msg;
+        ljerk_msg.data = ljerk_smoothed_result.get();
+        twist_ljerk_result_pub_.publish(ljerk_msg);
+    }
+
+    // Preserve value and time
+    twist_prev = twist_out;
+    last_callback_time = current_time;
+
+    out_msg.header = msg->header;
+
+    return out_msg;
 }
 
-void TwistFilterNode::rubisTwistCmdCallback(const rubis_msgs::TwistStampedConstPtr& _msg){
-  geometry_msgs::TwistStampedConstPtr msg = boost::make_shared<const geometry_msgs::TwistStamped>(_msg-> msg);
-  rubis::instance_ = _msg->instance;
-  publishTwist(msg);
+void TwistFilterNode::purePursuitOutputCallback(const rubis_msgs::PurePursuitOutputConstPtr &msg) {
+    static ros::Time previous_time = msg->header.stamp;
+    static double previous_target_velocity = 0.0;
+    static double previous_target_accel = 0.0;
+    static bool is_current_time_changed = false;
+
+    // Before spin
+    rubis::start_task_profiling();
+    rubis::instance_ = msg->instance;
+    rubis::lidar_instance_ = msg->lidar_instance;
+    ros::Time current_time = msg->header.stamp;
+
+    if (current_time == previous_time) {
+        previous_time = current_time;
+        rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+        return;
+    }
+
+    // Handle emergency stop
+    _emergencyStopCallback();
+
+    // Handle ctrl_cmd
+    auto ctrl_cmd_ptr = boost::make_shared<autoware_msgs::ControlCommandStamped const>(msg->ctrl);
+    _ctrlCmdCallback(ctrl_cmd_ptr);
+
+    // Filter twist
+    geometry_msgs::TwistStampedConstPtr twist_ptr = boost::make_shared<const geometry_msgs::TwistStamped>(msg->twist);
+    geometry_msgs::TwistStamped filtered_twist = calculateTwist(twist_ptr);
+
+    // Calculate vehicle cmd
+    double diff_time = (current_time - previous_time).toSec();
+    double current_target_velocity = 0.0, current_target_accel = 0.0;
+
+    current_target_velocity = filtered_twist.twist.linear.x;
+    current_target_accel = (diff_time > 0) ? ((current_target_velocity - previous_target_velocity) / diff_time) : previous_target_accel;
+
+    previous_time = current_time;
+    previous_target_velocity = current_target_velocity;
+    previous_target_accel = current_target_accel;
+
+    autoware_msgs::VehicleCmd vehicle_cmd_msg;
+    vehicle_cmd_msg.ctrl_cmd.linear_acceleration = current_target_accel;
+    vehicle_cmd_msg.ctrl_cmd.linear_velocity = current_target_velocity;
+    vehicle_cmd_msg.twist_cmd = filtered_twist;
+    
+    // After spin
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+
+    vehicle_cmd_pub_.publish(vehicle_cmd_msg);
 }
 
+void TwistFilterNode::_ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr &msg) {
+    if (msg == NULL)
+        return;
+    const twist_filter::Ctrl ctrl = {msg->cmd.linear_velocity, msg->cmd.steering_angle};
+    ros::Time current_time = ros::Time::now();
+
+    static ros::Time last_callback_time = current_time;
+    static twist_filter::Ctrl ctrl_prev = ctrl;
+
+    double time_elapsed = (current_time - last_callback_time).toSec();
+
+    checkCtrl(ctrl, ctrl_prev, time_elapsed);
+
+    twist_filter::Ctrl ctrl_out = ctrl;
+
+    // Apply lateral limit
+    auto ctrl_limit_result = twist_filter_ptr_->lateralLimitCtrl(ctrl, ctrl_prev, time_elapsed);
+    if (ctrl_limit_result) {
+        ctrl_out = ctrl_limit_result.get();
+    }
 
-void TwistFilterNode::twistCmdCallback(const geometry_msgs::TwistStampedConstPtr& msg)
-{
-  rubis::instance_ = RUBIS_NO_INSTANCE;
-  publishTwist(msg);
+    // Publish lateral accel and jerk before smoothing
+    auto lacc_no_smoothed_result = twist_filter_ptr_->calcLaccWithSteeringAngle(ctrl);
+    if (lacc_no_smoothed_result) {
+        std_msgs::Float32 lacc_msg_debug;
+        lacc_msg_debug.data = lacc_no_smoothed_result.get();
+        ctrl_lacc_limit_debug_pub_.publish(lacc_msg_debug);
+    }
+    auto ljerk_no_smoothed_result = twist_filter_ptr_->calcLjerkWithSteeringAngle(ctrl, ctrl_prev, time_elapsed);
+    if (ljerk_no_smoothed_result) {
+        std_msgs::Float32 ljerk_msg_debug;
+        ljerk_msg_debug.data = ljerk_no_smoothed_result.get();
+        ctrl_ljerk_limit_debug_pub_.publish(ljerk_msg_debug);
+    }
+
+    // Smoothing
+    ctrl_out = twist_filter_ptr_->smoothCtrl(ctrl_out);
+
+    // Publish lateral accel and jerk after smoothing
+    auto lacc_smoothed_result = twist_filter_ptr_->calcLaccWithSteeringAngle(ctrl_out);
+    if (lacc_smoothed_result) {
+        std_msgs::Float32 lacc_msg;
+        lacc_msg.data = lacc_smoothed_result.get();
+        ctrl_lacc_result_pub_.publish(lacc_msg);
+    }
+    auto ljerk_smoothed_result = twist_filter_ptr_->calcLjerkWithSteeringAngle(ctrl_out, ctrl_prev, time_elapsed);
+    if (ljerk_smoothed_result) {
+        std_msgs::Float32 ljerk_msg;
+        ljerk_msg.data = ljerk_smoothed_result.get();
+        ctrl_ljerk_result_pub_.publish(ljerk_msg);
+    }
+
+    // Preserve value and time
+    ctrl_prev = ctrl_out;
+    last_callback_time = current_time;
 }
 
-void TwistFilterNode::ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg)
-{
-  const twist_filter::Ctrl ctrl = { msg->cmd.linear_velocity, msg->cmd.steering_angle };
-  ros::Time current_time = ros::Time::now();
-
-  static ros::Time last_callback_time = current_time;
-  static twist_filter::Ctrl ctrl_prev = ctrl;
-
-  double time_elapsed = (current_time - last_callback_time).toSec();
-
-  health_checker_.NODE_ACTIVATE();
-  checkCtrl(ctrl, ctrl_prev, time_elapsed);
-
-  twist_filter::Ctrl ctrl_out = ctrl;
-
-  // Apply lateral limit
-  auto ctrl_limit_result = twist_filter_ptr_->lateralLimitCtrl(ctrl, ctrl_prev, time_elapsed);
-  if (ctrl_limit_result)
-  {
-    ctrl_out = ctrl_limit_result.get();
-  }
-
-  // Publish lateral accel and jerk before smoothing
-  auto lacc_no_smoothed_result = twist_filter_ptr_->calcLaccWithSteeringAngle(ctrl);
-  if (lacc_no_smoothed_result)
-  {
-    std_msgs::Float32 lacc_msg_debug;
-    lacc_msg_debug.data = lacc_no_smoothed_result.get();
-    ctrl_lacc_limit_debug_pub_.publish(lacc_msg_debug);
-  }
-  auto ljerk_no_smoothed_result = twist_filter_ptr_->calcLjerkWithSteeringAngle(ctrl, ctrl_prev, time_elapsed);
-  if (ljerk_no_smoothed_result)
-  {
-    std_msgs::Float32 ljerk_msg_debug;
-    ljerk_msg_debug.data = ljerk_no_smoothed_result.get();
-    ctrl_ljerk_limit_debug_pub_.publish(ljerk_msg_debug);
-  }
-
-  // Smoothing
-  ctrl_out = twist_filter_ptr_->smoothCtrl(ctrl_out);
-
-  // Smoothed value publish
-  autoware_msgs::ControlCommandStamped out_msg = *msg;
-  out_msg.cmd.linear_velocity = ctrl_out.lv;
-  out_msg.cmd.steering_angle = ctrl_out.sa;
-
-  if(emergency_stop_) out_msg.cmd.linear_velocity = -1000;
-
-  ctrl_pub_.publish(out_msg);
-
-  // Publish lateral accel and jerk after smoothing
-  auto lacc_smoothed_result = twist_filter_ptr_->calcLaccWithSteeringAngle(ctrl_out);
-  if (lacc_smoothed_result)
-  {
-    std_msgs::Float32 lacc_msg;
-    lacc_msg.data = lacc_smoothed_result.get();
-    ctrl_lacc_result_pub_.publish(lacc_msg);
-  }
-  auto ljerk_smoothed_result = twist_filter_ptr_->calcLjerkWithSteeringAngle(ctrl_out, ctrl_prev, time_elapsed);
-  if (ljerk_smoothed_result)
-  {
-    std_msgs::Float32 ljerk_msg;
-    ljerk_msg.data = ljerk_smoothed_result.get();
-    ctrl_ljerk_result_pub_.publish(ljerk_msg);
-  }
-
-  // Preserve value and time
-  ctrl_prev = ctrl_out;
-  last_callback_time = current_time;
+void TwistFilterNode::emergencyStopCallback(const std_msgs::Bool &msg) {
+    current_emergency_stop_ = msg.data;
+    return;
 }
 
-void TwistFilterNode::emergencyStopCallback(const std_msgs::Bool& msg){
-  bool current_emergency_stop = msg.data;
-  static std::string state("none");
-  
-  if(current_emergency_stop == true){
-    state = std::string("object is detected");
-    emergency_stop_ = true;
-    current_stop_count_ = max_stop_count_;
-  }
-  else if(current_emergency_stop == false && emergency_stop_ == true){ // Emergency Stop event is finished or wait
-    current_stop_count_--;
-    if(current_stop_count_ > 0){
-      state = std::string("Wait for go");
-      emergency_stop_ = true;
+void TwistFilterNode::_emergencyStopCallback() {
+    static std::string state("none");
+
+    if (current_emergency_stop_ == true) {
+        state = std::string("object is detected");
+        emergency_stop_ = true;
+        current_stop_count_ = max_stop_count_;
+    } else if (current_emergency_stop_ == false && emergency_stop_ == true) { // Emergency Stop event is finished or wait
+        current_stop_count_--;
+        if (current_stop_count_ > 0) {
+            state = std::string("Wait for go");
+            emergency_stop_ = true;
+        } else
+            emergency_stop_ = false;
+    } else if (current_emergency_stop_ == false && emergency_stop_ == false) { // No event
+        state = std::string("No object");
+        emergency_stop_ = false;
+        current_stop_count_ = 0;
     }
-    else
-      emergency_stop_ = false;
-  }
-  else if(current_emergency_stop == false && emergency_stop_ == false){ // No event
-    state = std::string("No object");
-    emergency_stop_ = false;
-    current_stop_count_ = 0;
-  }
 }
 
-void TwistFilterNode::checkTwist(const twist_filter::Twist twist, const twist_filter::Twist twist_prev,
-                                 const double& dt)
-{
-  const auto lacc = twist_filter_ptr_->calcLaccWithAngularZ(twist);
-  const auto ljerk = twist_filter_ptr_->calcLjerkWithAngularZ(twist, twist_prev, dt);
-
-  const twist_filter::Configuration& config = twist_filter_ptr_->getConfiguration();
-
-  if (lacc)
-  {
-    health_checker_.CHECK_MAX_VALUE("twist_lateral_accel_high", lacc.get(), config.lateral_accel_limit,
-                                    2 * config.lateral_accel_limit, DBL_MAX,
-                                    "lateral_accel is too high in twist filtering");
-  }
-  if (ljerk)
-  {
-    health_checker_.CHECK_MAX_VALUE("twist_lateral_jerk_high", lacc.get(), config.lateral_jerk_limit,
-                                    2 * config.lateral_jerk_limit, DBL_MAX,
-                                    "lateral_jerk is too high in twist filtering");
-  }
+void TwistFilterNode::checkTwist(const twist_filter::Twist twist, const twist_filter::Twist twist_prev, const double &dt) {
+    const auto lacc = twist_filter_ptr_->calcLaccWithAngularZ(twist);
+    const auto ljerk = twist_filter_ptr_->calcLjerkWithAngularZ(twist, twist_prev, dt);
+
+    const twist_filter::Configuration &config = twist_filter_ptr_->getConfiguration();
 }
 
-void TwistFilterNode::checkCtrl(const twist_filter::Ctrl ctrl, const twist_filter::Ctrl ctrl_prev, const double& dt)
-{
-  const auto lacc = twist_filter_ptr_->calcLaccWithSteeringAngle(ctrl);
-  const auto ljerk = twist_filter_ptr_->calcLjerkWithSteeringAngle(ctrl, ctrl_prev, dt);
-
-  const twist_filter::Configuration& config = twist_filter_ptr_->getConfiguration();
-
-  if (lacc)
-  {
-    health_checker_.CHECK_MAX_VALUE("ctrl_lateral_accel_high", lacc.get(), config.lateral_accel_limit,
-                                    3 * config.lateral_accel_limit, DBL_MAX,
-                                    "lateral_accel is too high in ctrl filtering");
-  }
-  if (ljerk)
-  {
-    health_checker_.CHECK_MAX_VALUE("ctrl_lateral_jerk_high", lacc.get(), config.lateral_jerk_limit,
-                                    3 * config.lateral_jerk_limit, DBL_MAX,
-                                    "lateral_jerk is too high in ctrl filtering");
-  }
+void TwistFilterNode::checkCtrl(const twist_filter::Ctrl ctrl, const twist_filter::Ctrl ctrl_prev, const double &dt) {
+    const auto lacc = twist_filter_ptr_->calcLaccWithSteeringAngle(ctrl);
+    const auto ljerk = twist_filter_ptr_->calcLjerkWithSteeringAngle(ctrl, ctrl_prev, dt);
+
+    const twist_filter::Configuration &config = twist_filter_ptr_->getConfiguration();
 }
 
-}  // namespace twist_filter_node
+} // namespace twist_filter_node
diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp
index eb82a63b..6a1da019 100644
--- a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp
+++ b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp
@@ -17,67 +17,38 @@
 #include "twist_filter/twist_filter_node.h"
 
 extern unsigned long rubis::instance_;
+extern 
 
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "twist_filter");
   twist_filter_node::TwistFilterNode node;
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline;
-
   ros::NodeHandle private_nh("~");
-  private_nh.param<int>("/twist_filter/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/twist_filter/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/twist_filter/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_filter.csv");
-  private_nh.param<int>("/twist_filter/rate", rate, 10);
-  private_nh.param("/twist_filter/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/twist_filter/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/twist_filter/task_relative_deadline", task_relative_deadline, (double)10);
-
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-  
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{
-    ros::Rate r(rate);
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){
-      if(rubis::sched::is_task_ready_ == TASK_READY) break;
-      ros::spinOnce();
-      r.sleep();      
-    }
-
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();
 
-      if(rubis::sched::task_state_ == TASK_STATE_READY){
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
-
-      ros::spinOnce();
-
-      if(task_profiling_flag){
-        rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
-      }
+  // Scheduling & Profiling Setup
+  std::string node_name = ros::this_node::getName();
+  std::string task_response_time_filename;
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_filter.csv");
 
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
-  }
+  int rate;
+  private_nh.param<int>(node_name+"/rate", rate, 10);
+
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
+
+  rubis::init_task_profiling(task_response_time_filename);
+  
+  ros::spin();
   
   return 0;
 }
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt
index 3073a8b7..05ae66ec 100644
--- a/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt
+++ b/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt
@@ -5,11 +5,10 @@ if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
   add_definitions(-D__aarch64__)
 endif()
 
-find_package(autoware_build_flags REQUIRED)
+
 find_package(
   catkin REQUIRED COMPONENTS
     autoware_config_msgs
-    autoware_health_checker
     autoware_msgs
     geometry_msgs
     ros_observer
@@ -45,16 +44,3 @@ install(
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
-
-if(CATKIN_ENABLE_TESTING)
-  roslint_add_test()
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(
-    test-twist_gate
-    test/test_twist_gate.test
-    test/src/test_twist_gate.cpp
-    src/twist_gate.cpp
-  )
-  add_dependencies(test-twist_gate ${catkin_EXPORTED_TARGETS})
-  target_link_libraries(test-twist_gate ${catkin_LIBRARIES})
-endif()
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h
index a5883ecb..b6254f84 100644
--- a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h
+++ b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h
@@ -42,10 +42,6 @@
 #include "tablet_socket_msgs/gear_cmd.h"
 #include "tablet_socket_msgs/mode_cmd.h"
 
-// headers in Autowae Health Checker
-#include <autoware_health_checker/health_checker/health_checker.h>
-
-
 extern int zero_flag_;
 
 class TwistGate
@@ -64,7 +60,8 @@ class TwistGate
   void watchdogTimer();
   void remoteCmdCallback(const remote_msgs_t::ConstPtr& input_msg);
   void autoCmdTwistCmdCallback(const geometry_msgs::TwistStamped::ConstPtr& input_msg);
-  void autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& _input_msg);
+  void autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg);
+  void _autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg);
   void modeCmdCallback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg);
   void gearCmdCallback(const tablet_socket_msgs::gear_cmd::ConstPtr& input_msg);
   void accelCmdCallback(const autoware_msgs::AccelCmd::ConstPtr& input_msg);
@@ -86,7 +83,6 @@ class TwistGate
 
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
-  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
   ros::Publisher control_command_pub_;
   ros::Publisher vehicle_cmd_pub_;
   ros::Publisher rubis_vehicle_cmd_pub_;
@@ -94,6 +90,9 @@ class TwistGate
   ros::Subscriber config_sub_;
   std::map<std::string, ros::Subscriber> auto_cmd_sub_stdmap_;
   ros::Timer timer_;
+  ros::Time current_time_;
+
+  bool is_current_time_changed_;
 
   vehicle_cmd_msg_t twist_gate_msg_;
   rubis_msgs::VehicleCmd rubis_twist_gate_msg_;
@@ -106,6 +105,8 @@ class TwistGate
   std::thread watchdog_timer_thread_;
   bool is_alive;
 
+  rubis_msgs::TwistStamped::ConstPtr rubis_twist_cmd_ptr_;
+
   enum class CommandMode
   {
     AUTO = 1,
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/package.xml b/autoware.ai/src/autoware/core_planning/twist_gate/package.xml
index 8c03545a..cab1aa01 100644
--- a/autoware.ai/src/autoware/core_planning/twist_gate/package.xml
+++ b/autoware.ai/src/autoware/core_planning/twist_gate/package.xml
@@ -6,11 +6,10 @@
   <maintainer email="h_ohta@ertl.jp">h_ohta</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_config_msgs</depend>
-  <depend>autoware_health_checker</depend>
   <depend>autoware_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>roscpp</depend>
@@ -22,4 +21,5 @@
   <depend>ros_observer</depend>
   <depend>rubis_lib</depend>
   <depend>rubis_msgs</depend>
+  <depend>autoware_system_msgs</depend>
 </package>
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp
index 14324122..d0b7bad7 100644
--- a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp
+++ b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp
@@ -45,39 +45,37 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n
   , timeout_period_(10.0)
   , command_mode_(CommandMode::AUTO)
   , previous_command_mode_(CommandMode::AUTO)
+  , rubis_twist_cmd_ptr_(NULL)
+  , current_time_(0)
+  , is_current_time_changed_(false)
 {
   private_nh_.param<double>("loop_rate", loop_rate_, 30.0);
   private_nh_.param<bool>("use_decision_maker", use_decision_maker_, false);
-  private_nh_.param<int>("instance_mode", rubis::instance_mode_, 0);
 
-  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh_, private_nh_);
   control_command_pub_ = nh_.advertise<std_msgs::String>("/ctrl_mode", 1);
   vehicle_cmd_pub_ = nh_.advertise<vehicle_cmd_msg_t>("/vehicle_cmd", 1, true);
-  if(rubis::instance_mode_) rubis_vehicle_cmd_pub_ = nh_.advertise<rubis_msgs::VehicleCmd>("/rubis_vehicle_cmd", 1, true);
-  remote_cmd_sub_ = nh_.subscribe("/remote_cmd", 1, &TwistGate::remoteCmdCallback, this);
-  config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistGate::configCallback, this);
-
+  rubis_vehicle_cmd_pub_ = nh_.advertise<rubis_msgs::VehicleCmd>("/rubis_vehicle_cmd", 1, true);
+  
   timer_ = nh_.createTimer(ros::Duration(1.0 / loop_rate_), &TwistGate::timerCallback, this);
 
-  if(rubis::instance_mode_) auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/rubis_twist_cmd", 1, &TwistGate::autoCmdRubisTwistCmdCallback, this);
-  else auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/twist_cmd", 1, &TwistGate::autoCmdTwistCmdCallback, this);
+  // remote_cmd_sub_ = nh_.subscribe("/remote_cmd", 1, &TwistGate::remoteCmdCallback, this);
+  config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistGate::configCallback, this);
+  auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/rubis_twist_cmd", 1, &TwistGate::autoCmdRubisTwistCmdCallback, this);
   
-  auto_cmd_sub_stdmap_["mode_cmd"] = nh_.subscribe("/mode_cmd", 1, &TwistGate::modeCmdCallback, this);
-  auto_cmd_sub_stdmap_["gear_cmd"] = nh_.subscribe("/gear_cmd", 1, &TwistGate::gearCmdCallback, this);
-  auto_cmd_sub_stdmap_["accel_cmd"] = nh_.subscribe("/accel_cmd", 1, &TwistGate::accelCmdCallback, this);
-  auto_cmd_sub_stdmap_["steer_cmd"] = nh_.subscribe("/steer_cmd", 1, &TwistGate::steerCmdCallback, this);
-  auto_cmd_sub_stdmap_["brake_cmd"] = nh_.subscribe("/brake_cmd", 1, &TwistGate::brakeCmdCallback, this);
-  auto_cmd_sub_stdmap_["lamp_cmd"] = nh_.subscribe("/lamp_cmd", 1, &TwistGate::lampCmdCallback, this);
-  auto_cmd_sub_stdmap_["ctrl_cmd"] = nh_.subscribe("/ctrl_cmd", 1, &TwistGate::ctrlCmdCallback, this);
-  auto_cmd_sub_stdmap_["state"] = nh_.subscribe("/decision_maker/state", 1, &TwistGate::stateCallback, this);
-  auto_cmd_sub_stdmap_["emergency_velocity"] =
-      nh_.subscribe("emergency_velocity", 1, &TwistGate::emergencyCmdCallback, this);
+  // auto_cmd_sub_stdmap_["mode_cmd"] = nh_.subscribe("/mode_cmd", 1, &TwistGate::modeCmdCallback, this);
+  // auto_cmd_sub_stdmap_["gear_cmd"] = nh_.subscribe("/gear_cmd", 1, &TwistGate::gearCmdCallback, this);
+  // auto_cmd_sub_stdmap_["accel_cmd"] = nh_.subscribe("/accel_cmd", 1, &TwistGate::accelCmdCallback, this);
+  // auto_cmd_sub_stdmap_["steer_cmd"] = nh_.subscribe("/steer_cmd", 1, &TwistGate::steerCmdCallback, this);
+  // auto_cmd_sub_stdmap_["brake_cmd"] = nh_.subscribe("/brake_cmd", 1, &TwistGate::brakeCmdCallback, this);
+  // auto_cmd_sub_stdmap_["lamp_cmd"] = nh_.subscribe("/lamp_cmd", 1, &TwistGate::lampCmdCallback, this);
+  // auto_cmd_sub_stdmap_["ctrl_cmd"] = nh_.subscribe("/ctrl_cmd", 1, &TwistGate::ctrlCmdCallback, this);
+  // auto_cmd_sub_stdmap_["state"] = nh_.subscribe("/decision_maker/state", 1, &TwistGate::stateCallback, this);
+  // auto_cmd_sub_stdmap_["emergency_velocity"] =
+  //     nh_.subscribe("emergency_velocity", 1, &TwistGate::emergencyCmdCallback, this);
 
 
   twist_gate_msg_.header.seq = 0;
   emergency_stop_msg_.data = false;
-  health_checker_ptr_->ENABLE();
-  health_checker_ptr_->NODE_ACTIVATE();
 
   remote_cmd_time_ = ros::Time::now();
   emergency_handling_time_ = ros::Time::now();
@@ -154,14 +152,10 @@ void TwistGate::watchdogTimer()
     if (command_mode_ == CommandMode::REMOTE)
     {
       const double dt = (now_time - remote_cmd_time_).toSec() * 1000;
-      health_checker_ptr_->CHECK_MAX_VALUE("remote_cmd_interval", dt, 700, 1000, 1500, "remote cmd interval is too "
-                                                                                       "long.");
     }
 
     // check push emergency stop button
     const int level = (emergency_stop_msg_.data) ? AwDiagStatus::ERROR : AwDiagStatus::OK;
-    health_checker_ptr_->CHECK_TRUE("emergency_stop_button", emergency_stop_msg_.data, level, "emergency stop button "
-                                                                                              "is pushed.");
 
     // if no emergency message received for more than timeout_period_
     if ((now_time - emergency_handling_time_) > timeout_period_)
@@ -199,22 +193,29 @@ void TwistGate::remoteCmdCallback(const remote_msgs_t::ConstPtr& input_msg)
 
 void TwistGate::autoCmdTwistCmdCallback(const geometry_msgs::TwistStamped::ConstPtr& input_msg)
 {
-  rubis::instance_ = RUBIS_NO_INSTANCE;
+  rubis::instance_ = 0;
   updateTwistGateMsg(input_msg); 
 }
 
-void TwistGate::autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& _input_msg)
+void TwistGate::_autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg)
 {
-  geometry_msgs::TwistStamped::ConstPtr input_msg = boost::make_shared<const geometry_msgs::TwistStamped>(_input_msg->msg);
-  rubis::instance_ = _input_msg->instance;
-  updateTwistGateMsg(input_msg); 
+  if(rubis_twist_cmd_ptr_ == NULL) return;
+  if(!is_current_time_changed_){
+    current_time_ = input_msg->msg.header.stamp;  
+    is_current_time_changed_ = true;
+  }
+  rubis::instance_ = input_msg->instance;  
+  rubis::lidar_instance_ = input_msg->lidar_instance;  
+  geometry_msgs::TwistStamped::ConstPtr _input_msg = boost::make_shared<const geometry_msgs::TwistStamped>(input_msg->msg);
+  updateTwistGateMsg(_input_msg); 
 }
 
-inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::ConstPtr& input_msg){
-  health_checker_ptr_->CHECK_RATE("topic_rate_twist_cmd_slow", 8, 5, 1, "topic twist_cmd subscribe rate slow.");
-  health_checker_ptr_->CHECK_MAX_VALUE("twist_cmd_linear_high", input_msg->twist.linear.x,
-    DBL_MAX, DBL_MAX, DBL_MAX, "linear twist_cmd is too high");
+void TwistGate::autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg)
+{
+  rubis_twist_cmd_ptr_ = boost::make_shared<rubis_msgs::TwistStamped const>(*input_msg);
+}
 
+inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::ConstPtr& input_msg){
   if (command_mode_ == CommandMode::AUTO && !emergency_handling_active_)
   {
     twist_gate_msg_.header.frame_id = input_msg->header.frame_id;
@@ -222,14 +223,9 @@ inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::Con
     twist_gate_msg_.header.seq++;
     twist_gate_msg_.twist_cmd.twist = input_msg->twist;
     
-    if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){
-      rubis_twist_gate_msg_.instance = rubis::instance_;
-    }
+    rubis_twist_gate_msg_.instance = rubis::instance_;
     checkState();
   }
-
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
 }
 
 void TwistGate::modeCmdCallback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg)
@@ -358,15 +354,45 @@ void TwistGate::emergencyCmdCallback(const vehicle_cmd_msg_t::ConstPtr& input_ms
 
 void TwistGate::timerCallback(const ros::TimerEvent& e)
 {
+  rubis::start_task_profiling();
+  if(rubis_twist_cmd_ptr_ == NULL){
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+    return;
+  }
+  
+
+  static ros::Time previous_time = ros::Time::now(); 
+  static double previous_target_velocity = 0.0;
+  static double previous_target_accel = 0.0;
+
+  // Callback
+  _autoCmdRubisTwistCmdCallback(rubis_twist_cmd_ptr_);  
+
+  double diff_time = (current_time_ - previous_time).toSec();
+  double current_target_velocity = 0.0, current_target_accel = 0.0;
+
+  current_target_velocity = rubis_twist_cmd_ptr_->msg.twist.linear.x;
+  current_target_accel = (diff_time > 0) ? ((current_target_velocity - previous_target_velocity) / diff_time) : previous_target_accel;
+  
   if(zero_flag_ == 1)
     resetVehicleCmdMsg();
+  twist_gate_msg_.ctrl_cmd.linear_acceleration = current_target_accel;
+  twist_gate_msg_.ctrl_cmd.linear_velocity = current_target_velocity;
 
   vehicle_cmd_pub_.publish(twist_gate_msg_);
-  if(rubis::instance_mode_){
-    rubis_twist_gate_msg_.msg = twist_gate_msg_;
-    rubis_vehicle_cmd_pub_.publish(rubis_twist_gate_msg_);
+  rubis_twist_gate_msg_.msg = twist_gate_msg_;
+  rubis_vehicle_cmd_pub_.publish(rubis_twist_gate_msg_);
+  
+  
+  
+  if(is_current_time_changed_){
+    previous_time = current_time_;
+    previous_target_velocity = current_target_velocity;
+    previous_target_accel = current_target_accel;
+    is_current_time_changed_ = false;    
   }
-  rubis::sched::task_state_ = TASK_STATE_DONE;
+
+  rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
 }
 
 void TwistGate::configCallback(const autoware_config_msgs::ConfigTwistFilter& msg)
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp
index e3f3e429..9e0bc827 100644
--- a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp
+++ b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp
@@ -28,61 +28,31 @@ int main(int argc, char** argv)
   ros::NodeHandle nh;
   ros::NodeHandle private_nh("~");
 
-  // Scheduling Setup
-  int task_scheduling_flag;
-  int task_profiling_flag;
-  std::string task_response_time_filename;
-  int rate;
-  double task_minimum_inter_release_time;
-  double task_execution_time;
-  double task_relative_deadline;
-
-  private_nh.param<int>("/twist_gate/task_scheduling_flag", task_scheduling_flag, 0);
-  private_nh.param<int>("/twist_gate/task_profiling_flag", task_profiling_flag, 0);
-  private_nh.param<std::string>("/twist_gate/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_gate.csv");
-  private_nh.param<int>("/twist_gate/rate", rate, 10);
-  private_nh.param("/twist_gate/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-  private_nh.param("/twist_gate/task_execution_time", task_execution_time, (double)10);
-  private_nh.param("/twist_gate/task_relative_deadline", task_relative_deadline, (double)10);
-  private_nh.param<int>("/twist_gate/zero_flag", zero_flag_, 0);
-
   TwistGate twist_gate(nh, private_nh);
 
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{
-    ros::Rate r(rate);
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){
-      if(rubis::sched::is_task_ready_ == TASK_READY) break;
-      ros::spinOnce();
-      r.sleep();      
-    }
-
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-      if(rubis::sched::task_state_ == TASK_STATE_READY){
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
-
-      ros::spinOnce();
-
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
+  // Scheduling & Profiling Setup
+  std::string node_name = ros::this_node::getName();
+  std::string task_response_time_filename;
+  private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_gate.csv");
 
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
-  }
+  int rate;
+  private_nh.param<int>(node_name+"/rate", rate, 10);
+
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
+
+  rubis::init_task_profiling(task_response_time_filename);
+
+  ros::spin();
 
   return 0;
 }
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp
deleted file mode 100644
index cf8882a2..00000000
--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp
+++ /dev/null
@@ -1,351 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <gtest/gtest.h>
-#include <ros/ros.h>
-
-#include "test_twist_gate.hpp"
-
-class TwistGateTestSuite : public ::testing::Test
-{
-public:
-  TwistGateTestSuite() {}
-  ~TwistGateTestSuite() {}
-
-  TwistGateTestClass test_obj_;
-
-protected:
-  virtual void SetUp()
-  {
-    ros::NodeHandle nh;
-    ros::NodeHandle private_nh("~");
-    test_obj_.tg = new TwistGate(nh, private_nh);
-  };
-  virtual void TearDown() { delete test_obj_.tg; }
-};
-
-TEST_F(TwistGateTestSuite, resetVehicelCmd)
-{
-  double d_value = 1.5;
-  int i_value = 1;
-
-  autoware_msgs::VehicleCmd msg = test_obj_.setTgTwistGateMsg(d_value, i_value);
-
-  ASSERT_EQ(d_value, msg.twist_cmd.twist.linear.x);
-  ASSERT_EQ(d_value, msg.twist_cmd.twist.angular.z);
-  ASSERT_EQ(i_value, msg.mode);
-  ASSERT_EQ(i_value, msg.gear_cmd.gear);
-  ASSERT_EQ(i_value, msg.lamp_cmd.l);
-  ASSERT_EQ(i_value, msg.lamp_cmd.r);
-  ASSERT_EQ(i_value, msg.accel_cmd.accel);
-  ASSERT_EQ(i_value, msg.brake_cmd.brake);
-  ASSERT_EQ(i_value, msg.steer_cmd.steer);
-  ASSERT_EQ(i_value, msg.ctrl_cmd.linear_velocity);
-  ASSERT_EQ(i_value, msg.ctrl_cmd.steering_angle);
-  ASSERT_EQ(i_value, msg.emergency);
-
-  test_obj_.tgResetVehicleCmdMsg();
-  msg = test_obj_.getTgTwistGateMsg();
-
-  ASSERT_EQ(0, msg.twist_cmd.twist.linear.x);
-  ASSERT_EQ(0, msg.twist_cmd.twist.angular.z);
-  ASSERT_EQ(0, msg.mode);
-  ASSERT_EQ(0, msg.gear_cmd.gear);
-  ASSERT_EQ(0, msg.lamp_cmd.l);
-  ASSERT_EQ(0, msg.lamp_cmd.r);
-  ASSERT_EQ(0, msg.accel_cmd.accel);
-  ASSERT_EQ(0, msg.brake_cmd.brake);
-  ASSERT_EQ(0, msg.steer_cmd.steer);
-  ASSERT_EQ(-1, msg.ctrl_cmd.linear_velocity);
-  ASSERT_EQ(0, msg.ctrl_cmd.steering_angle);
-  ASSERT_EQ(0, msg.emergency);
-}
-
-TEST_F(TwistGateTestSuite, twistCmdCallback)
-{
-  double linear_x = 5.0;
-  double angular_z = 1.5;
-  test_obj_.publishTwistCmd(linear_x, angular_z);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-  ASSERT_EQ(linear_x, test_obj_.cb_vehicle_cmd.twist_cmd.twist.linear.x);
-  ASSERT_EQ(angular_z, test_obj_.cb_vehicle_cmd.twist_cmd.twist.angular.z);
-}
-
-TEST_F(TwistGateTestSuite, controlCmdCallback)
-{
-  double linear_vel = 5.0;
-  double linear_acc = 1.5;
-  double steer_angle = 1.57;
-  test_obj_.publishControlCmd(linear_vel, linear_acc, steer_angle);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-
-  ASSERT_EQ(linear_vel, test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_velocity);
-  ASSERT_EQ(linear_acc, test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_acceleration);
-  ASSERT_EQ(steer_angle, test_obj_.cb_vehicle_cmd.ctrl_cmd.steering_angle);
-}
-
-TEST_F(TwistGateTestSuite, remoteCmdCallback)
-{
-  autoware_msgs::RemoteCmd remote_cmd;
-  remote_cmd.vehicle_cmd.header.frame_id = "/test_frame";
-  remote_cmd.vehicle_cmd.header.stamp = ros::Time::now();
-  remote_cmd.vehicle_cmd.twist_cmd.twist.linear.x = 5.0;
-  remote_cmd.vehicle_cmd.twist_cmd.twist.angular.z = 0.785;
-  remote_cmd.vehicle_cmd.ctrl_cmd.linear_velocity = 4.0;
-  remote_cmd.vehicle_cmd.ctrl_cmd.linear_acceleration = 3.0;
-  remote_cmd.vehicle_cmd.ctrl_cmd.steering_angle = 0.393;
-  remote_cmd.vehicle_cmd.accel_cmd.accel = 10;
-  remote_cmd.vehicle_cmd.brake_cmd.brake = 20;
-  remote_cmd.vehicle_cmd.steer_cmd.steer = 30;
-  remote_cmd.vehicle_cmd.gear_cmd.gear = autoware_msgs::Gear::PARK;
-  remote_cmd.vehicle_cmd.lamp_cmd.l = 1;
-  remote_cmd.vehicle_cmd.lamp_cmd.r = 1;
-  remote_cmd.vehicle_cmd.mode = 6;
-  remote_cmd.vehicle_cmd.emergency = 0;
-  remote_cmd.control_mode = 2;
-
-  test_obj_.publishRemoteCmd(remote_cmd);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-  ASSERT_EQ(remote_cmd.vehicle_cmd.header.frame_id
-      , test_obj_.cb_vehicle_cmd.header.frame_id);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.header.stamp
-      , test_obj_.cb_vehicle_cmd.header.stamp);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.twist_cmd.twist.linear.x
-      , test_obj_.cb_vehicle_cmd.twist_cmd.twist.linear.x);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.twist_cmd.twist.angular.z
-      , test_obj_.cb_vehicle_cmd.twist_cmd.twist.angular.z);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.linear_velocity
-      , test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_velocity);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.linear_acceleration
-      , test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_acceleration);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.steering_angle
-      , test_obj_.cb_vehicle_cmd.ctrl_cmd.steering_angle);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.accel_cmd.accel
-      , test_obj_.cb_vehicle_cmd.accel_cmd.accel);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.brake_cmd.brake
-      , test_obj_.cb_vehicle_cmd.brake_cmd.brake);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.steer_cmd.steer
-      , test_obj_.cb_vehicle_cmd.steer_cmd.steer);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.gear_cmd.gear
-      , test_obj_.cb_vehicle_cmd.gear_cmd.gear);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.lamp_cmd.l
-      , test_obj_.cb_vehicle_cmd.lamp_cmd.l);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.lamp_cmd.r
-      , test_obj_.cb_vehicle_cmd.lamp_cmd.r);
-  ASSERT_EQ(remote_cmd.vehicle_cmd.mode
-      , test_obj_.cb_vehicle_cmd.mode);
-}
-
-TEST_F(TwistGateTestSuite, modeCmdCallback)
-{
-  int mode = 8;
-
-  test_obj_.publishModeCmd(mode);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-
-  ASSERT_EQ(mode, test_obj_.cb_vehicle_cmd.mode);
-}
-
-TEST_F(TwistGateTestSuite, gearCmdCallback)
-{
-  int gear = autoware_msgs::Gear::DRIVE;
-
-  test_obj_.publishGearCmd(gear);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-
-  ASSERT_EQ(gear, test_obj_.cb_vehicle_cmd.gear_cmd.gear);
-}
-
-TEST_F(TwistGateTestSuite, accelCmdCallback)
-{
-  int accel = 100;
-
-  test_obj_.publishAccelCmd(accel);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-  ASSERT_EQ(accel, test_obj_.cb_vehicle_cmd.accel_cmd.accel);
-}
-
-TEST_F(TwistGateTestSuite, steerCmdCallback)
-{
-  int steer = 100;
-
-  test_obj_.publishSteerCmd(steer);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-
-  ASSERT_EQ(steer, test_obj_.cb_vehicle_cmd.steer_cmd.steer);
-}
-
-TEST_F(TwistGateTestSuite, brakeCmdCallback)
-{
-  int brake = 100;
-
-  test_obj_.publishBrakeCmd(brake);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-
-  ASSERT_EQ(brake, test_obj_.cb_vehicle_cmd.brake_cmd.brake);
-}
-
-TEST_F(TwistGateTestSuite, lampCmdCallback)
-{
-  int lamp_l = 1;
-  int lamp_r = 1;
-
-  test_obj_.publishLampCmd(lamp_l, lamp_r);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-  ASSERT_EQ(lamp_l, test_obj_.cb_vehicle_cmd.lamp_cmd.l);
-  ASSERT_EQ(lamp_r, test_obj_.cb_vehicle_cmd.lamp_cmd.r);
-}
-
-TEST_F(TwistGateTestSuite, emergencyVehicleCmdCallback)
-{
-  int lamp_l_pre = 0;
-  int lamp_r_pre = 0;
-  int emergency = 1;
-
-  test_obj_.publishLampCmd(lamp_l_pre, lamp_r_pre);
-  test_obj_.publishEmergencyVehicleCmd(emergency);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-  ASSERT_EQ(emergency, test_obj_.cb_vehicle_cmd.emergency);
-
-  int lamp_l = 1;
-  int lamp_r = 1;
-  test_obj_.publishLampCmd(lamp_l, lamp_r);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-  ASSERT_EQ(lamp_l_pre, test_obj_.cb_vehicle_cmd.lamp_cmd.l);
-  ASSERT_EQ(lamp_r_pre, test_obj_.cb_vehicle_cmd.lamp_cmd.r);
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(10000));
-
-  test_obj_.publishLampCmd(lamp_l, lamp_r);
-  ros::WallDuration(0.1).sleep();
-  test_obj_.tgSpinOnce();
-
-  for (int i = 0; i < 3; i++)
-  {
-    ros::WallDuration(0.1).sleep();
-    ros::spinOnce();
-  }
-
-  ASSERT_EQ(lamp_l, test_obj_.cb_vehicle_cmd.lamp_cmd.l);
-  ASSERT_EQ(lamp_r, test_obj_.cb_vehicle_cmd.lamp_cmd.r);
-}
-
-TEST_F(TwistGateTestSuite, stateCallback)
-{
-  test_obj_.publishDecisionMakerState("VehicleReady\nWaitOrder\nWaitEngage\n");
-  test_obj_.tgSpinOnce();
-  ros::WallDuration(0.1).sleep();
-  ros::spinOnce();
-
-  autoware_msgs::VehicleCmd tg_msg = test_obj_.getTwistGateMsg();
-  ASSERT_EQ(autoware_msgs::Gear::PARK, tg_msg.gear_cmd.gear);
-  ASSERT_EQ(false, test_obj_.getIsStateDriveFlag());
-
-  test_obj_.publishDecisionMakerState("VehicleReady\nDriving\nDrive\n");
-  test_obj_.tgSpinOnce();
-  ros::WallDuration(0.1).sleep();
-  ros::spinOnce();
-
-  tg_msg = test_obj_.getTwistGateMsg();
-  ASSERT_EQ(autoware_msgs::Gear::DRIVE, tg_msg.gear_cmd.gear);
-  ASSERT_EQ(true, test_obj_.getIsStateDriveFlag());
-}
-
-int main(int argc, char **argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  ros::init(argc, argv, "TwistGateTestNode");
-  return RUN_ALL_TESTS();
-}
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp b/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp
deleted file mode 100644
index bd4acca1..00000000
--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp
+++ /dev/null
@@ -1,168 +0,0 @@
-/*
- * Copyright 2019 Autoware Foundation. All rights reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <gtest/gtest.h>
-#include <ros/ros.h>
-
-#include "twist_gate/twist_gate.h"
-
-class TwistGateTestClass {
-public:
-  TwistGateTestClass() {
-    twist_cmd_publisher =
-        nh.advertise<geometry_msgs::TwistStamped>("twist_cmd", 0);
-    control_cmd_publisher =
-        nh.advertise<autoware_msgs::ControlCommandStamped>("ctrl_cmd", 0);
-    decision_maker_state_publisher =
-        nh.advertise<std_msgs::String>("decision_maker/state", 0);
-    remote_cmd_publisher = nh.advertise<autoware_msgs::RemoteCmd>("remote_cmd", 0);
-    mode_cmd_publisher = nh.advertise<tablet_socket_msgs::mode_cmd>("mode_cmd", 0);
-    gear_cmd_publisher = nh.advertise<tablet_socket_msgs::gear_cmd>("gear_cmd", 0);
-    accel_cmd_publisher = nh.advertise<autoware_msgs::AccelCmd>("accel_cmd", 0);
-    steer_cmd_publisher = nh.advertise<autoware_msgs::SteerCmd>("steer_cmd", 0);
-    brake_cmd_publisher = nh.advertise<autoware_msgs::BrakeCmd>("brake_cmd", 0);
-    lamp_cmd_publisher = nh.advertise<autoware_msgs::LampCmd>("lamp_cmd", 0);
-    emergency_vehicle_cmd_publisher = nh.advertise<autoware_msgs::VehicleCmd>("emergency_velocity", 0);
-    vehicle_cmd_subscriber = nh.subscribe(
-        "/vehicle_cmd", 1, &TwistGateTestClass::vehicleCmdCallback, this);
-  }
-
-  TwistGate *tg;
-  autoware_msgs::VehicleCmd cb_vehicle_cmd;
-
-  ros::NodeHandle nh;
-  ros::Publisher twist_cmd_publisher, control_cmd_publisher, remote_cmd_publisher, mode_cmd_publisher, gear_cmd_publisher, accel_cmd_publisher, steer_cmd_publisher, brake_cmd_publisher, lamp_cmd_publisher, decision_maker_state_publisher, emergency_vehicle_cmd_publisher;
-  ros::Subscriber vehicle_cmd_subscriber;
-
-  void tgSpinOnce() { tg->spinOnce(); }
-
-  void tgResetVehicleCmdMsg() { tg->resetVehicleCmdMsg(); }
-
-  autoware_msgs::VehicleCmd setTgTwistGateMsg(double d_value, int i_value) {
-    tg->twist_gate_msg_.twist_cmd.twist.linear.x = d_value;
-    tg->twist_gate_msg_.twist_cmd.twist.angular.z = d_value;
-    tg->twist_gate_msg_.mode = i_value;
-    tg->twist_gate_msg_.gear_cmd.gear = i_value;
-    tg->twist_gate_msg_.lamp_cmd.l = i_value;
-    tg->twist_gate_msg_.lamp_cmd.r = i_value;
-    tg->twist_gate_msg_.accel_cmd.accel = i_value;
-    tg->twist_gate_msg_.brake_cmd.brake = i_value;
-    tg->twist_gate_msg_.steer_cmd.steer = i_value;
-    tg->twist_gate_msg_.ctrl_cmd.linear_velocity = i_value;
-    tg->twist_gate_msg_.ctrl_cmd.steering_angle = i_value;
-    tg->twist_gate_msg_.emergency = i_value;
-
-    return tg->twist_gate_msg_;
-  }
-
-  autoware_msgs::VehicleCmd getTgTwistGateMsg() {return tg->twist_gate_msg_;}
-
-  void publishTwistCmd(double linear_x, double angular_z) {
-    geometry_msgs::TwistStamped msg;
-    msg.header.stamp = ros::Time::now();
-    msg.twist.linear.x = linear_x;
-    msg.twist.angular.z = angular_z;
-
-    twist_cmd_publisher.publish(msg);
-  }
-
-  void publishControlCmd(double linear_vel, double linear_acc,
-                         double steer_angle) {
-    autoware_msgs::ControlCommandStamped msg;
-    msg.header.stamp = ros::Time::now();
-    msg.cmd.linear_velocity = linear_vel;
-    msg.cmd.linear_acceleration = linear_acc;
-    msg.cmd.steering_angle = steer_angle;
-
-    control_cmd_publisher.publish(msg);
-  }
-
-  void publishRemoteCmd(autoware_msgs::RemoteCmd remote_cmd){
-    remote_cmd_publisher.publish(remote_cmd);
-  }
-
-  void publishModeCmd(int mode){
-    tablet_socket_msgs::mode_cmd msg;
-    msg.header.stamp = ros::Time::now();
-    msg.mode = mode;
-
-    mode_cmd_publisher.publish(msg);
-  }
-
-  void publishGearCmd(int gear){
-    tablet_socket_msgs::gear_cmd msg;
-    msg.header.stamp = ros::Time::now();
-    msg.gear = gear;
-
-    gear_cmd_publisher.publish(msg);
-  }
-
-  void publishAccelCmd(int accel){
-    autoware_msgs::AccelCmd msg;
-    msg.header.stamp = ros::Time::now();
-    msg.accel = accel;
-
-    accel_cmd_publisher.publish(msg);
-  }
-
-  void publishSteerCmd(int steer){
-    autoware_msgs::SteerCmd msg;
-    msg.header.stamp = ros::Time::now();
-    msg.steer = steer;
-
-    steer_cmd_publisher.publish(msg);
-  }
-
-  void publishBrakeCmd(int brake){
-    autoware_msgs::BrakeCmd msg;
-    msg.header.stamp = ros::Time::now();
-    msg.brake = brake;
-
-    brake_cmd_publisher.publish(msg);
-  }
-
-  void publishLampCmd(int lamp_l, int lamp_r){
-    autoware_msgs::LampCmd msg;
-    msg.header.stamp = ros::Time::now();
-    msg.l = lamp_l;
-    msg.r = lamp_r;
-
-    lamp_cmd_publisher.publish(msg);
-  }
-
-  void publishEmergencyVehicleCmd(int emergency){
-    autoware_msgs::VehicleCmd msg;
-    msg.header.stamp = ros::Time::now();
-    msg.emergency = emergency;
-
-    emergency_vehicle_cmd_publisher.publish(msg);
-  }
-
-  void publishDecisionMakerState(std::string states) {
-    std_msgs::String msg;
-    msg.data = states;
-
-    decision_maker_state_publisher.publish(msg);
-  }
-
-  void vehicleCmdCallback(autoware_msgs::VehicleCmd msg) {
-    cb_vehicle_cmd = msg;
-  }
-
-  autoware_msgs::VehicleCmd getTwistGateMsg() { return tg->twist_gate_msg_; }
-
-  bool getIsStateDriveFlag() { return tg->is_state_drive_; }
-};
diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test b/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test
deleted file mode 100644
index abe4934a..00000000
--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test
+++ /dev/null
@@ -1,5 +0,0 @@
-<launch>
-
-  <test test-name="test-twist_gate" pkg="twist_gate" type="test-twist_gate" name="test"/>
-
-</launch>
diff --git a/autoware.ai/src/autoware/messages/autoware_msgs/msg/Lane.msg b/autoware.ai/src/autoware/messages/autoware_msgs/msg/Lane.msg
index 67b75bdd..ef38b713 100644
--- a/autoware.ai/src/autoware/messages/autoware_msgs/msg/Lane.msg
+++ b/autoware.ai/src/autoware/messages/autoware_msgs/msg/Lane.msg
@@ -1,4 +1,4 @@
-Header header
+std_msgs/Header header
 int32 increment
 int32 lane_id
 Waypoint[] waypoints
diff --git a/autoware.ai/src/autoware/messages/can_data_msgs/package.xml b/autoware.ai/src/autoware/messages/can_data_msgs/package.xml
index 4e9c6b5d..a748a631 100644
--- a/autoware.ai/src/autoware/messages/can_data_msgs/package.xml
+++ b/autoware.ai/src/autoware/messages/can_data_msgs/package.xml
@@ -13,7 +13,7 @@
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
+  <license>Apache 2</license>
 
   <buildtool_depend>catkin</buildtool_depend>
 
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst
new file mode 100644
index 00000000..41c70f27
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst
@@ -0,0 +1,267 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nav_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.13.1 (2021-01-11)
+-------------------
+* Update package maintainers (`#168 <https://github.com/ros/common_msgs/issues/168>`_)
+* Add LoadMap service (`#164 <https://github.com/ros/common_msgs/issues/164>`_)
+* Contributors: David V. Lu!!, Michel Hidalgo
+
+1.13.0 (2020-05-21)
+-------------------
+* Bump CMake version to avoid CMP0048 warning (`#158 <https://github.com/ros/common_msgs/issues/158>`_)
+* Contributors: Shane Loretz
+
+1.12.7 (2018-11-06)
+-------------------
+
+1.12.6 (2018-05-03)
+-------------------
+
+1.12.5 (2016-09-30)
+-------------------
+
+1.12.4 (2016-02-22)
+-------------------
+
+1.12.3 (2015-04-20)
+-------------------
+
+1.12.2 (2015-03-21)
+-------------------
+* change type of initial_pose in SetMap service to PoseWithCovarianceStamped
+* Contributors: Stephan Wirth
+
+1.12.1 (2015-03-17)
+-------------------
+* updating outdated urls. fixes `#52 <https://github.com/ros/common_msgs/issues/52>`_.
+* Adds a SetMap service message to support swap maps functionality in amcl
+* Contributors: Tully Foote, liz-murphy
+
+1.12.0 (2014-12-29)
+-------------------
+
+1.11.6 (2014-11-04)
+-------------------
+
+1.11.5 (2014-10-27)
+-------------------
+
+1.11.4 (2014-06-19)
+-------------------
+
+1.11.3 (2014-05-07)
+-------------------
+* Export architecture_independent flag in package.xml
+* Contributors: Scott K Logan
+
+1.11.2 (2014-04-24)
+-------------------
+
+1.11.1 (2014-04-16)
+-------------------
+
+1.11.0 (2014-03-04)
+-------------------
+
+1.10.6 (2014-02-27)
+-------------------
+
+1.10.5 (2014-02-25)
+-------------------
+
+1.10.4 (2014-02-18)
+-------------------
+
+1.10.3 (2014-01-07)
+-------------------
+
+1.10.2 (2013-08-19)
+-------------------
+
+1.10.1 (2013-08-16)
+-------------------
+
+1.10.0 (2013-07-13)
+-------------------
+
+1.9.16 (2013-05-21)
+-------------------
+* added action definition for getting maps
+* update email in package.xml
+
+1.9.15 (2013-03-08)
+-------------------
+
+1.9.14 (2013-01-19)
+-------------------
+
+1.9.13 (2013-01-13)
+-------------------
+
+1.9.12 (2013-01-02)
+-------------------
+
+1.9.11 (2012-12-17)
+-------------------
+* modified dep type of catkin
+
+1.9.10 (2012-12-13)
+-------------------
+* add missing downstream depend
+* switched from langs to message_* packages
+
+1.9.9 (2012-11-22)
+------------------
+
+1.9.8 (2012-11-14)
+------------------
+
+1.9.7 (2012-10-30)
+------------------
+* fix catkin function order
+
+1.9.6 (2012-10-18)
+------------------
+* updated cmake min version to 2.8.3, use cmake_parse_arguments instead of custom macro
+
+1.9.5 (2012-09-28)
+------------------
+
+1.9.4 (2012-09-27 18:06)
+------------------------
+
+1.9.3 (2012-09-27 17:39)
+------------------------
+* cleanup
+* cleaned up package.xml files
+* updated to latest catkin
+* fixed dependencies and more
+* updated to latest catkin: created package.xmls, updated CmakeLists.txt
+
+1.9.2 (2012-09-05)
+------------------
+* updated pkg-config in manifest.xml
+
+1.9.1 (2012-09-04)
+------------------
+* use install destination variables, removed manual installation of manifests
+
+1.9.0 (2012-08-29)
+------------------
+* updated to current catkin
+
+1.8.13 (2012-07-26 18:34:15 +0000)
+----------------------------------
+
+1.8.8 (2012-06-12 22:36)
+------------------------
+* make find_package REQUIRED
+* removed obsolete catkin tag from manifest files
+* fixed package dependencies for several common messages (fixed `#3956 <https://github.com/ros/common_msgs/issues/3956>`_)
+* adding manifest exports
+* removed depend, added catkin
+* stripping depend and export tags from common_msgs manifests as msg dependencies are now declared in cmake and stack.yaml.  Also removed bag migration exports
+* common_msgs: removing migration rules as all are over a year old
+* bye bye vestigial MSG_DIRS
+* nav_msgs: getting rid of other build files and cleaning up
+* common_msgs: starting catkin conversion
+* adios rosbuild2 in manifest.xml
+* catkin updates
+* catkin_project
+* Updated to work with new message generation macros
+* More tweaking for standalone message generation
+* Getting standalone message generation working... w/o munging rosbuild2
+* more rosbuild2 hacking
+* missing dependencies
+* updating bagmigration exports
+* rosbuild2 taking shape
+* removing old exports ros`#2292 <https://github.com/ros/common_msgs/issues/2292>`_
+* Added Ubuntu platform tags to manifest
+* Adding a start pose to the GetPlan service
+* Remove use of deprecated rosbuild macros
+* Fixing migration rules for nav_msgs.
+* Changed byte to int8, in response to map_server doc review
+* changing review status
+* adding documentation for `#2997 <https://github.com/ros/common_msgs/issues/2997>`_
+* removing redundant range statements as per ticket:2997
+* Adding documentation to the Odometry message to make things more clear
+* manifest update
+* updated description and url
+* full migration rules
+* adding child_frame_id as per discussion about odometry message
+* Adding a header to Path
+* Adding a header to the GridCells message
+* Adding a new GridCells message for displaying obstacles in nav_view and rviz
+* clearing API reviews for they've been through a bunch of them recently.
+* fixing stack name
+* Adding comments to path
+* documenting messages
+* Making odometry migration fail until we have worked out appropriate way to handle covariances.
+* Changing naming of bag migration rules.
+* Modifying migration rules for Odometry and WrenchStamped change of field names.
+* Adding actual migration rules for all of the tested common_msgs migrations.
+* `#2250 <https://github.com/ros/common_msgs/issues/2250>`_ getting rid of _with_covariance in Odometry fields
+* nav_msgs: added missing srv export
+* Adding migration rules to get migration tests to pass.
+* removing last of robot_msgs and all dependencies on it
+* moving Path from robot_msgs to nav_msgs `#2281 <https://github.com/ros/common_msgs/issues/2281>`_
+* adding header to OccupancyGrid `#1906 <https://github.com/ros/common_msgs/issues/1906>`_
+* First half of the change from deprecated_msgs::RobotBase2DOdom to nav_msgs::Odometry, I think all the c++ compiles, can't speak for functionality yet, also... the python has yet to be run... this may break some things
+* moving PoseArray into geometry_msgs `#1907 <https://github.com/ros/common_msgs/issues/1907>`_
+* fixing names
+* Removing header since there's already one in the pose and fixing message definition to have variable names
+* adding Odometry message as per API review and ticket:2250
+* merging in the changes to messages see ros-users email.  THis is about half the common_msgs API changes
+* Forgot to check in the services I added.... shoot
+* Moving StaticMap.srv to GetMap.srv and moving Plan.srv to GetPlan.srv, also moving them to nav_msgs and removing the nav_srvs package
+* Merging tha actionlib branch back into trunk
+  r29135@att (orig r19792):  eitanme | 2009-07-27 18:30:30 -0700
+  Creating a branch for actionlib.... hopefully for the last time
+  r29137@att (orig r19794):  eitanme | 2009-07-27 18:32:49 -0700
+  Changing ParticleCloud to PoseArray
+  r29139@att (orig r19796):  eitanme | 2009-07-27 18:33:42 -0700
+  Adding action definition to the rep
+  r29148@att (orig r19805):  eitanme | 2009-07-27 18:47:39 -0700
+  Some fixes... almost compiling
+  r29165@att (orig r19822):  eitanme | 2009-07-27 20:41:07 -0700
+  Macro version of the typedefs that compiles
+  r29213@att (orig r19869):  eitanme | 2009-07-28 11:49:10 -0700
+  Compling version of the ActionServer re-write complete with garbage collection, be default it will keep goals without goal handles for 5 seconds
+  r29220@att (orig r19876):  eitanme | 2009-07-28 12:06:06 -0700
+  Fix to make sure that transitions into preempting and recalling states actually happen
+  r29254@att (orig r19888):  eitanme | 2009-07-28 13:27:40 -0700
+  Forgot to actually call the cancel callback... addind a subscriber on the cancel topic
+  r29267@att (orig r19901):  eitanme | 2009-07-28 14:41:09 -0700
+  Adding text field to GoalStatus to allow users to throw some debugging information into the GoalStatus messages
+  r29275@att (orig r19909):  eitanme | 2009-07-28 15:43:49 -0700
+  Using tf remapping as I should've been doing for awhile
+  r29277@att (orig r19911):  eitanme | 2009-07-28 15:48:48 -0700
+  The navigation stack can now handle goals that aren't in the global frame. However, these goals will be transformed to the global frame at the time of reception, so for achieving them accurately the global frame of move_base should really be set to match the goals.
+  r29299@att (orig r19933):  stuglaser | 2009-07-28 17:08:10 -0700
+  Created genaction.py script to create the various messages that an action needs
+  r29376@att (orig r20003):  vijaypradeep | 2009-07-29 02:45:24 -0700
+  ActionClient is running. MoveBase ActionServer seems to be crashing
+  r29409@att (orig r20033):  vijaypradeep | 2009-07-29 11:57:54 -0700
+  Fixing bug with adding status trackers
+  r29410@att (orig r20034):  vijaypradeep | 2009-07-29 11:58:18 -0700
+  Changing from Release to Debug
+  r29432@att (orig r20056):  vijaypradeep | 2009-07-29 14:07:30 -0700
+  No longer building goal_manager_test.cpp
+  r29472@att (orig r20090):  vijaypradeep | 2009-07-29 17:04:14 -0700
+  Lots of Client-Side doxygen
+  r29484@att (orig r20101):  vijaypradeep | 2009-07-29 18:35:01 -0700
+  Adding to mainpage.dox
+  r29487@att (orig r20104):  eitanme | 2009-07-29 18:55:06 -0700
+  Removing file to help resolve merge I hope
+  r29489@att (orig r20106):  eitanme | 2009-07-29 19:00:07 -0700
+  Removing another file to try to resolve the branch
+  r29492@att (orig r20108):  eitanme | 2009-07-29 19:14:25 -0700
+  Again removing a file to get the merge working
+  r29493@att (orig r20109):  eitanme | 2009-07-29 19:34:45 -0700
+  Removing yet another file on which ssl negotiation fails
+  r29500@att (orig r20116):  eitanme | 2009-07-29 19:54:18 -0700
+  Fixing bug in genaction
+* moving MapMetaData and OccGrid into nav_msgs `#1303 <https://github.com/ros/common_msgs/issues/1303>`_
+* created nav_msgs and moved ParticleCloud there `#1300 <https://github.com/ros/common_msgs/issues/1300>`_
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt
new file mode 100644
index 00000000..b4751819
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(nav_msgs)
+
+find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs actionlib_msgs)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  GridCells.msg
+  MapMetaData.msg
+  OccupancyGrid.msg
+  Odometry.msg
+  Path.msg)
+
+add_service_files(
+  DIRECTORY srv
+  FILES
+  GetMap.srv
+  GetPlan.srv
+  SetMap.srv
+  LoadMap.srv)
+
+add_action_files(
+  FILES
+  GetMap.action)
+
+generate_messages(DEPENDENCIES geometry_msgs std_msgs actionlib_msgs)
+
+catkin_package(CATKIN_DEPENDS geometry_msgs message_runtime std_msgs actionlib_msgs)
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action
new file mode 100644
index 00000000..080e54fe
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action
@@ -0,0 +1,5 @@
+# Get the map as a nav_msgs/OccupancyGrid
+---
+nav_msgs/OccupancyGrid map
+---
+# no feedback
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg
new file mode 100644
index 00000000..2c928941
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg
@@ -0,0 +1,5 @@
+#an array of cells in a 2D grid
+Header header
+float32 cell_width
+float32 cell_height
+geometry_msgs/Point[] cells
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg
new file mode 100644
index 00000000..398fbdd0
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg
@@ -0,0 +1,13 @@
+# This hold basic information about the characterists of the OccupancyGrid
+
+# The time at which the map was loaded
+time map_load_time
+# The map resolution [m/cell]
+float32 resolution
+# Map width [cells]
+uint32 width
+# Map height [cells]
+uint32 height
+# The origin of the map [m, m, rad].  This is the real-world pose of the
+# cell (0,0) in the map.
+geometry_msgs/Pose origin
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg
new file mode 100644
index 00000000..f2e79bda
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg
@@ -0,0 +1,11 @@
+# This represents a 2-D grid map, in which each cell represents the probability of
+# occupancy.
+
+Header header 
+
+#MetaData for the map
+MapMetaData info
+
+# The map data, in row-major order, starting with (0,0).  Occupancy
+# probabilities are in the range [0,100].  Unknown is -1.
+int8[] data
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg
new file mode 100644
index 00000000..73578ed8
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg
@@ -0,0 +1,7 @@
+# This represents an estimate of a position and velocity in free space.  
+# The pose in this message should be specified in the coordinate frame given by header.frame_id.
+# The twist in this message should be specified in the coordinate frame given by the child_frame_id
+Header header
+string child_frame_id
+geometry_msgs/PoseWithCovariance pose
+geometry_msgs/TwistWithCovariance twist
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg
new file mode 100644
index 00000000..979cb7d3
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg
@@ -0,0 +1,3 @@
+#An array of poses that represents a Path for a robot to follow
+Header header
+geometry_msgs/PoseStamped[] poses
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/package.xml b/autoware.ai/src/autoware/messages/nav_msgs/package.xml
new file mode 100644
index 00000000..f379061f
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/package.xml
@@ -0,0 +1,29 @@
+<package>
+  <name>nav_msgs</name>
+  <version>1.13.1</version>
+  <description>
+    nav_msgs defines the common messages used to interact with the
+    <a href="http://wiki.ros.org/navigation">navigation</a> stack.
+  </description>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
+  <license>BSD</license>
+
+  <url>http://wiki.ros.org/nav_msgs</url>
+  <author email="tfoote@osrfoundation.org">Tully Foote</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+
+  <export>
+    <architecture_independent/>
+  </export>
+</package>
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv
new file mode 100644
index 00000000..6bd8e4a6
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv
@@ -0,0 +1,3 @@
+# Get the map as a nav_msgs/OccupancyGrid
+---
+nav_msgs/OccupancyGrid map
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv
new file mode 100644
index 00000000..f5c23edb
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv
@@ -0,0 +1,13 @@
+# Get a plan from the current position to the goal Pose 
+
+# The start pose for the plan
+geometry_msgs/PoseStamped start
+
+# The final pose of the goal position
+geometry_msgs/PoseStamped goal
+
+# If the goal is obstructed, how many meters the planner can 
+# relax the constraint in x and y before failing. 
+float32 tolerance
+---
+nav_msgs/Path plan
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv
new file mode 100644
index 00000000..3b9caaad
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv
@@ -0,0 +1,15 @@
+# URL of map resource
+# Can be an absolute path to a file: file:///path/to/maps/floor1.yaml
+# Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml
+string map_url
+---
+# Result code defintions
+uint8 RESULT_SUCCESS=0
+uint8 RESULT_MAP_DOES_NOT_EXIST=1
+uint8 RESULT_INVALID_MAP_DATA=2
+uint8 RESULT_INVALID_MAP_METADATA=3
+uint8 RESULT_UNDEFINED_FAILURE=255
+
+# Returned map is only valid if result equals RESULT_SUCCESS
+nav_msgs/OccupancyGrid map
+uint8 result
diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv
new file mode 100644
index 00000000..55d3c24f
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv
@@ -0,0 +1,6 @@
+# Set a new map together with an initial pose
+nav_msgs/OccupancyGrid map
+geometry_msgs/PoseWithCovarianceStamped initial_pose
+---
+bool success
+
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt
index 5f862ea7..733f974b 100644
--- a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt
@@ -18,6 +18,14 @@ add_message_files(
     TwistStamped.msg
     VehicleCmd.msg
     InsStat.msg
+    Image.msg
+    PoseTwistStamped.msg
+    LaneWithPoseTwist.msg
+    LaneArrayWithPoseTwist.msg
+    DetectedObjectArray.msg
+    PlanningInfo.msg
+    PurePursuitOutput.msg
+    Bool.msg
 )
 
 generate_messages(
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/Bool.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/Bool.msg
new file mode 100644
index 00000000..b7dec684
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/Bool.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+bool data
+uint64 instance
+uint64 vision_instance
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg
new file mode 100644
index 00000000..71626188
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint64 instance
+autoware_msgs/DetectedObjectArray object_array
+uint64 lidar_instance
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/Image.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/Image.msg
new file mode 100644
index 00000000..3db669ae
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/Image.msg
@@ -0,0 +1,3 @@
+std_msgs/Header header
+uint64 instance
+sensor_msgs/Image msg
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg
new file mode 100644
index 00000000..6f6722cd
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg
@@ -0,0 +1,6 @@
+std_msgs/Header header
+uint64 instance
+autoware_msgs/LaneArray lane_array
+geometry_msgs/PoseStamped pose
+geometry_msgs/TwistStamped twist
+uint64 lidar_instance
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg
new file mode 100644
index 00000000..c2cf6d7f
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg
@@ -0,0 +1,7 @@
+std_msgs/Header header
+uint64 instance
+autoware_msgs/Lane lane
+geometry_msgs/PoseStamped pose
+geometry_msgs/TwistStamped twist
+uint64 lidar_instance
+uint64 vision_instance
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PlanningInfo.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PlanningInfo.msg
new file mode 100644
index 00000000..1fd6f37b
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PlanningInfo.msg
@@ -0,0 +1,11 @@
+std_msgs/Header header
+uint64 instance
+uint64 lidar_instance
+uint64 vision_instance
+autoware_msgs/LaneArray lane_array
+geometry_msgs/PoseStamped pose
+geometry_msgs/TwistStamped twist
+autoware_msgs/Lane trajectory_cost
+std_msgs/Bool sprint_switch
+autoware_msgs/IntersectionCondition intersection_condition
+std_msgs/Float64 distance_to_pedestrian
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PointCloud2.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PointCloud2.msg
index 5ab45e60..a38ef527 100644
--- a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PointCloud2.msg
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PointCloud2.msg
@@ -1,2 +1,3 @@
+std_msgs/Header header
 uint64 instance
 sensor_msgs/PointCloud2 msg
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg
new file mode 100644
index 00000000..b9055380
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint64 instance
+geometry_msgs/PoseStamped pose
+geometry_msgs/TwistStamped twist
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PurePursuitOutput.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PurePursuitOutput.msg
new file mode 100644
index 00000000..78eb1544
--- /dev/null
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PurePursuitOutput.msg
@@ -0,0 +1,6 @@
+std_msgs/Header header
+uint64 instance
+uint64 lidar_instance
+uint64 vision_instance
+geometry_msgs/TwistStamped twist
+autoware_msgs/ControlCommandStamped ctrl
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg
index b2cde46b..c28db1d4 100644
--- a/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg
+++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg
@@ -1,2 +1,4 @@
+std_msgs/Header header
 uint64 instance
+uint64 lidar_instance
 geometry_msgs/TwistStamped msg
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch b/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch
index 1e669ba1..9ba29def 100644
--- a/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch
+++ b/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch
@@ -1,5 +1,5 @@
 <launch>
-    <node name="image_decompresser" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw">
+    <node name="image_decompresser" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw">
     </node>
 
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
diff --git a/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch b/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch
index 08a39312..f5fa0f9a 100644
--- a/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch
+++ b/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch
@@ -10,7 +10,7 @@ roslaunch autoware_camera_lidar_calibrator camera_lidar_calibration.launch intri
     <arg name="target_frame" default="velodyne"/>
     <arg name="camera_frame" default="camera"/>
 
-    <node if="$(arg compressed_stream)" name="decompress" type="republish" pkg="image_transport" output="screen" args="compressed in:=/$(arg camera_id)/$(arg image_src) raw out:=/$(arg camera_id)/$(arg image_src)" />
+    <node if="$(arg compressed_stream)" name="decompress" type="republish" pkg="image_transport" args="compressed in:=/$(arg camera_id)/$(arg image_src) raw out:=/$(arg camera_id)/$(arg image_src)" />
 
     <node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher" ns="$(arg camera_id)">
         <param name="register_lidar2camera_tf" type="bool" value="false"/>
@@ -22,16 +22,16 @@ roslaunch autoware_camera_lidar_calibrator camera_lidar_calibration.launch intri
         <param name="camera_frame" type="str" value="$(arg camera_frame)"/>
     </node>
 
-    <node pkg="image_processor" type="image_rectifier" name="rectifier" ns="$(arg camera_id)" output="screen">
+    <node pkg="image_processor" type="image_rectifier" name="rectifier" ns="$(arg camera_id)">
         <param name="image_src" value="$(arg image_src)" />
         <param name="camera_info_src" value="$(arg camera_info_src)" />
     </node>
 
-    <node pkg="image_view2" type="image_view2" name="image_view2" ns="$(arg camera_id)" output="screen">
+    <node pkg="image_view2" type="image_view2" name="image_view2" ns="$(arg camera_id)">
         <remap from="image" to="image_rectified" />
     </node>
 
-    <node pkg="autoware_camera_lidar_calibrator" type="autoware_camera_lidar_calibration_node" name="autoware_camera_lidar_calibration_node" ns="$(arg camera_id)" output="screen">
+    <node pkg="autoware_camera_lidar_calibrator" type="autoware_camera_lidar_calibration_node" name="autoware_camera_lidar_calibration_node" ns="$(arg camera_id)">
         <param name="image_src" value="/image_rectified" />
         <param name="camera_info_src" value="$(arg camera_info_src)" />
     </node>
diff --git a/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml b/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml
index 908bcd40..e4d91a94 100644
--- a/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml
+++ b/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml
@@ -10,7 +10,7 @@
     <arg name="camera_info_topic"        default="/camera_info"/>
     <arg name="projection_matrix_topic"  default="/projection_matrix"/>
 
-    <node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher" ns="$(arg camera_id)" output="screen">
+    <node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher" ns="$(arg camera_id)">
         <param name="calibration_file"         type="str"  value="$(arg calibration_file)"/>
         <param name="register_lidar2camera_tf" type="bool" value="$(arg register_lidar2camera_tf)"/>
         <param name="publish_extrinsic_mat"    type="bool" value="$(arg publish_extrinsic_mat)"/>
diff --git a/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml b/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml
index 91fd8cc6..3ecd8e76 100644
--- a/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml
+++ b/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml
@@ -1,5 +1,5 @@
 <launch>
     <arg name="options"/>
     <arg name="bagfile"/>
-    <node pkg="rosbag" type="play" name="$(anon play)" output="screen" args="$(arg options) $(arg bagfile)"/>
+    <node pkg="rosbag" type="play" name="$(anon play)" args="$(arg options) $(arg bagfile)"/>
 </launch>
\ No newline at end of file
diff --git a/autoware.ai/src/autoware/utilities/calibration_publisher/src/calibration_publisher.cpp b/autoware.ai/src/autoware/utilities/calibration_publisher/src/calibration_publisher.cpp
index 17eb9694..b5f704fa 100644
--- a/autoware.ai/src/autoware/utilities/calibration_publisher/src/calibration_publisher.cpp
+++ b/autoware.ai/src/autoware/utilities/calibration_publisher/src/calibration_publisher.cpp
@@ -69,10 +69,10 @@ void load_calibration_file(std::string calibration_file){
   DistCoeff.rows = calib["DistCoeff"]["rows"].as<int>();
   DistCoeff.cols = calib["DistCoeff"]["cols"].as<int>();  
   ImgSize = calib["ImageSize"].as<std::vector<int>>();
-  assert((int)CameraExtrinsicMat.size() == 16);
-  assert((int)CameraMat.size() == 9);
-  assert((int)DistCoeff.size() == 5);
-  assert((int)ImgSize.size() == 2);
+  assert((int)sizeof(CameraExtrinsicMat) == 16);
+  assert((int)sizeof(CameraMat) == 9);
+  assert((int)sizeof(DistCoeff) == 5);
+  assert((int)sizeof(ImgSize) == 2);
   DistModel = "plumb_bob";  
 }
 
diff --git a/autoware.ai/src/autoware/utilities/map_tf_generator/CMakeLists.txt b/autoware.ai/src/autoware/utilities/map_tf_generator/CMakeLists.txt
index 70608645..874605e8 100644
--- a/autoware.ai/src/autoware/utilities/map_tf_generator/CMakeLists.txt
+++ b/autoware.ai/src/autoware/utilities/map_tf_generator/CMakeLists.txt
@@ -3,7 +3,7 @@ project(map_tf_generator)
 
 add_compile_options(-std=c++11)
 
-find_package(autoware_build_flags REQUIRED)
+
 find_package(catkin REQUIRED COMPONENTS
   pcl_ros
   roscpp
diff --git a/autoware.ai/src/autoware/utilities/map_tf_generator/package.xml b/autoware.ai/src/autoware/utilities/map_tf_generator/package.xml
index f181d0ec..b5b2c68e 100644
--- a/autoware.ai/src/autoware/utilities/map_tf_generator/package.xml
+++ b/autoware.ai/src/autoware/utilities/map_tf_generator/package.xml
@@ -6,7 +6,7 @@
   <maintainer email="azumi.suzuki@tier4.jp">azumi-suzuki</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>roscpp</depend>
diff --git a/autoware.ai/src/autoware/utilities/map_tools/CMakeLists.txt b/autoware.ai/src/autoware/utilities/map_tools/CMakeLists.txt
index bb9107da..05aa8452 100644
--- a/autoware.ai/src/autoware/utilities/map_tools/CMakeLists.txt
+++ b/autoware.ai/src/autoware/utilities/map_tools/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(map_tools)
 
-find_package(autoware_build_flags REQUIRED)
+
 find_package(catkin REQUIRED COMPONENTS
   pcl_conversions
   pcl_ros
diff --git a/autoware.ai/src/autoware/utilities/map_tools/package.xml b/autoware.ai/src/autoware/utilities/map_tools/package.xml
index 0b17bcf7..64383d72 100644
--- a/autoware.ai/src/autoware/utilities/map_tools/package.xml
+++ b/autoware.ai/src/autoware/utilities/map_tools/package.xml
@@ -6,7 +6,7 @@
   <maintainer email="yuki@ertl.jp">kitsukawa</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>libpcl-all-dev</depend>
diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch
index 96e93788..4425817b 100644
--- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch
+++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch
@@ -39,7 +39,7 @@
         <node name="image_proc" pkg="image_proc" type="image_proc"/>
     </group>
 
-    <node name="avt_camera" pkg="avt_vimba_camera" type="mono_camera_node" output="screen">
+    <node name="avt_camera" pkg="avt_vimba_camera" type="mono_camera_node">
         <param name="guid" value="$(arg guid)"/>
         <param name="ip" value="$(arg ip)"/>
         <param name="camera_info_url" value="$(arg camera_info_url)"/>
diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch
index ea2ec96e..f29ddd60 100755
--- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch
+++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch
@@ -11,7 +11,7 @@
     <arg name="projection_matrix_topic" default="/projection_matrix"/>
 
     <node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher" ns="$(arg camera_id)"
-          output="screen">
+         >
         <param name="register_lidar2camera_tf" type="bool" value="$(arg register_lidar2camera_tf)"/>
         <param name="publish_extrinsic_mat" type="bool" value="$(arg publish_extrinsic_mat)"/>
         <param name="publish_camera_info" type="bool" value="$(arg publish_camera_info)"/>
diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/launch_files/rubis_velodyne_vlp16_hires_FR_ddd.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/launch_files/rubis_velodyne_vlp16_hires_FR_ddd.launch
new file mode 100644
index 00000000..28a197ce
--- /dev/null
+++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/launch_files/rubis_velodyne_vlp16_hires_FR_ddd.launch
@@ -0,0 +1,33 @@
+<!-- -*- mode: XML -*- -->
+<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an VLP-16 HiRes
+
+arg: calibration = path to calibration file (default: standard VLP16_hires_db.yaml)
+pcap = path to packet capture file (default: use real device)
+
+$Id$
+-->
+
+<launch>
+  <!-- declare arguments with default values -->
+  <arg name="pcap" default="" />
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16_hires_db.yaml"/>
+  <arg name="min_range" default="0.4" />
+  <arg name="max_range" default="130.0" />
+  <arg name="model" default="VLP16"/>
+  <arg name="topic_name" default="points_raw_origin"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <node pkg="nodelet" type="nodelet" name="cloud_nodelet"
+        args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
+    <param name="calibration" value="$(arg calibration)"/>
+    <param name="min_range" value="$(arg min_range)"/>
+    <param name="max_range" value="$(arg max_range)"/>
+    <remap from="velodyne_points" to="$(arg topic_name)"/>
+  </node>
+</launch>
diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch
index bbd02004..d24c4a02 100644
--- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch
+++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch
@@ -4,7 +4,7 @@
     <arg name="projection_matrix_src" default="/projection_matrix"/>
     <arg name="sync" default="false" />
 
-    <node pkg="points2image" type="points2image" name="points2image" output="screen">
+    <node pkg="points2image" type="points2image" name="points2image">
         <param name="camera_info_topic" value="$(arg camera_id)$(arg camera_info_src)"/>
         <param name="projection_matrix_topic" value="$(arg camera_id)$(arg projection_matrix_src)"/>
         <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch
index 29657457..8d8ca68a 100755
--- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch
+++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch
@@ -23,7 +23,7 @@
     <arg name="sync" default="false" />
     <arg name="points_node" default="points_fused" />
 
-  <node pkg="points2image" type="points2vscan" name="points2vscan" output="screen" >
+  <node pkg="points2image" type="points2vscan" name="points2vscan" >
       <param name="points_node" type="string" value="$(arg points_node)" />
       <param name="BEAMNUM" type="int" value="$(arg beam_num)" />
       <param name="STEP" type="double" value="$(arg step)" />
@@ -44,9 +44,9 @@
       <param name="SYNC" type="bool" value="$(arg sync)" />
   </node>
 
-  <node pkg="points2image" type="vscan2image" name="vscan2image" output="screen">
+  <node pkg="points2image" type="vscan2image" name="vscan2image">
       <param name="camera_info_topic" value="$(arg camera_id)$(arg camera_info_src)" />
       <param name="projection_matrix_topic" value="$(arg camera_id)$(arg projection_matrix_src)" />
   </node>
-  <node pkg="points2image" type="vscan2linelist" name="vscan2linelist" output="screen" />
+  <node pkg="points2image" type="vscan2linelist" name="vscan2linelist" />
 </launch>
diff --git a/autoware.ai/src/autoware/visualization/detected_objects_visualizer/CMakeLists.txt b/autoware.ai/src/autoware/visualization/detected_objects_visualizer/CMakeLists.txt
index a187c77c..4bea3892 100644
--- a/autoware.ai/src/autoware/visualization/detected_objects_visualizer/CMakeLists.txt
+++ b/autoware.ai/src/autoware/visualization/detected_objects_visualizer/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(detected_objects_visualizer)
 
-find_package(autoware_build_flags REQUIRED)
+
 find_package(catkin REQUIRED COMPONENTS
   autoware_msgs
   cv_bridge
diff --git a/autoware.ai/src/autoware/visualization/detected_objects_visualizer/package.xml b/autoware.ai/src/autoware/visualization/detected_objects_visualizer/package.xml
index d6fa6f34..a8cc7f02 100644
--- a/autoware.ai/src/autoware/visualization/detected_objects_visualizer/package.xml
+++ b/autoware.ai/src/autoware/visualization/detected_objects_visualizer/package.xml
@@ -6,7 +6,7 @@
   <maintainer email="abrahammonrroy@yahoo.com">amc</maintainer>
   <license>Apache</license>
 
-  <buildtool_depend>autoware_build_flags</buildtool_depend>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>autoware_msgs</depend>
diff --git a/rubis_ws/deprecated/gnss_converter/cfg/ntrip_client.yaml b/rubis_ws/deprecated/gnss_converter/cfg/ntrip_client.yaml
deleted file mode 100644
index eb91d0f4..00000000
--- a/rubis_ws/deprecated/gnss_converter/cfg/ntrip_client.yaml
+++ /dev/null
@@ -1 +0,0 @@
-ntrip_file_path : "/home/rubis/Desktop/gps/SML/GPS/ntripclient/launch.sh"
diff --git a/rubis_ws/deprecated/gnss_converter/launch/ntrip_client.launch b/rubis_ws/deprecated/gnss_converter/launch/ntrip_client.launch
deleted file mode 100644
index 619699c1..00000000
--- a/rubis_ws/deprecated/gnss_converter/launch/ntrip_client.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
-    <rosparam file="$(find gnss_converter)/cfg/ntrip_client.yaml" />
-    <node pkg="gnss_converter" type="ntrip_client" name="ntrip_client" clear_params="true" output="screen" />
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/deprecated/gnss_converter/launch/position_pub.launch b/rubis_ws/deprecated/gnss_converter/launch/position_pub.launch
deleted file mode 100644
index 1392ec1d..00000000
--- a/rubis_ws/deprecated/gnss_converter/launch/position_pub.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-    <arg name="x_offset" default="29991.369" />
-    <arg name="y_offset" default="1404.013" />
-    <arg name="z_offset" default="-1463.76" />
-    <node pkg="gnss_converter" type="position_pub" name="position_pub">
-        <param name="x_offset" value="$(arg x_offset)"/>
-        <param name="y_offset" value="$(arg y_offset)"/>
-        <param name="z_offset" value="$(arg z_offset)"/>
-    </node>
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/deprecated/gnss_converter/src/gnss_pose_pub.cpp b/rubis_ws/deprecated/gnss_converter/src/gnss_pose_pub.cpp
deleted file mode 100644
index ef903139..00000000
--- a/rubis_ws/deprecated/gnss_converter/src/gnss_pose_pub.cpp
+++ /dev/null
@@ -1,118 +0,0 @@
-#include <gnss_converter/LLH2UTM.h>
-
-#include <geometry_msgs/PoseStamped.h>
-#include <inertiallabs_msgs/gps_data.h>
-#include <inertiallabs_msgs/ins_data.h>
-
-#include <message_filters/subscriber.h>
-#include <message_filters/time_synchronizer.h>
-#include <message_filters/sync_policies/exact_time.h>
-
-#include <tf/tf.h>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_listener.h>
-
-#define M_PI 3.14159265358979323846
-
-using namespace std;
-using namespace message_filters;
-
-typedef sync_policies::ExactTime<inertiallabs_msgs::gps_data, inertiallabs_msgs::ins_data> SyncPolicy_;
-
-static ros::Publisher gnss_pose_pub_;
-static geometry_msgs::PoseStamped gnss_pose_;
-static double x_offset_, y_offset_, z_offset_, yaw_offset_;
-static double roll_, pitch_, yaw_;
-
-void gnss_pose_pub_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins){
-    gnss_pose_.header = msg_gps->header;
-    gnss_pose_.header.frame_id = "/map";
-
-    /* coordinate transform (LLH2 to UTM) */ 
-    LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, gnss_pose_);
-
-    /* position offset calculation */ 
-    gnss_pose_.pose.position.x = gnss_pose_.pose.position.x - x_offset_;
-    gnss_pose_.pose.position.y = gnss_pose_.pose.position.y - y_offset_;
-    gnss_pose_.pose.position.z = gnss_pose_.pose.position.z - z_offset_;
-
-    /* orientation */ 
-    roll_ = msg_ins->YPR.z;
-    pitch_ = msg_ins->YPR.y;
-
-    /* yaw offset calculation */
-    yaw_ = -1 * (msg_ins->YPR.x) - yaw_offset_;
-    yaw_ = (yaw_ > 180.0) ? (yaw_ - 360) : ((yaw_ < -180) ? (yaw_ + 360) : yaw_);
-    
-    /* unit conversion */ 
-    roll_ = roll_ * M_PI/180.0; pitch_ = pitch_ * M_PI/180.0; yaw_ = yaw_ * M_PI/180.0;
-}
-
-int main(int argc, char *argv[]){
-    ros::init(argc, argv, "gnss_pose_pub");
-
-    ros::NodeHandle nh;
-
-    nh.param("/gnss_pose_pub/x_offset", x_offset_, 0.0);
-    nh.param("/gnss_pose_pub/y_offset", y_offset_, 0.0);
-    nh.param("/gnss_pose_pub/z_offset", z_offset_, 0.0);
-    nh.param("/ins_twist_generator/yaw_offset", yaw_offset_, 0.0);
-
-    gnss_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/gnss_pose", 2);
-
-    message_filters::Subscriber<inertiallabs_msgs::gps_data> gps_sub(nh, "/Inertial_Labs/gps_data", 2);
-    message_filters::Subscriber<inertiallabs_msgs::ins_data> ins_sub(nh, "/Inertial_Labs/ins_data", 2);
-    
-    Synchronizer<SyncPolicy_> sync(SyncPolicy_(2), gps_sub, ins_sub);
-
-    sync.registerCallback(boost::bind(&gnss_pose_pub_cb, _1, _2));
-
-    ros::Rate rate(10);
-
-    tf::TransformListener listener;
-    tf::TransformBroadcaster broadcaster;
-    tf::StampedTransform transform;
-    tf::Transform tf_gnss_to_base, tf_map_to_gnss, tf_map_to_base;
-    tf::Quaternion q;
-
-    while(nh.ok()){
-        try{
-            /* lookup /gnss to /base_link static transform */ 
-            listener.lookupTransform("/gnss", "/base_link", ros::Time(0), transform);
-            tf_gnss_to_base = transform;
-            break;
-        }
-        catch (tf::TransformException ex){
-            ROS_ERROR("%s", ex.what());
-            rate.sleep();
-        }
-    }
-
-    while(nh.ok()){
-        ros::spinOnce();
-        
-        /* /map to /gnss tf */ 
-        tf_map_to_gnss.setOrigin(tf::Vector3(gnss_pose_.pose.position.x, gnss_pose_.pose.position.y, gnss_pose_.pose.position.z));
-        q.setRPY(roll_, pitch_, yaw_);
-        tf_map_to_gnss.setRotation(q);
-
-        /* /map to /base tf calculation */ 
-        tf_map_to_base = tf_map_to_gnss * tf_gnss_to_base;
-
-        /* msg */  
-        gnss_pose_.pose.position.x = tf_map_to_base.getOrigin().x();
-        gnss_pose_.pose.position.y = tf_map_to_base.getOrigin().y();
-        gnss_pose_.pose.position.z = tf_map_to_base.getOrigin().z();
-
-        gnss_pose_.pose.orientation.w = tf_map_to_base.getRotation().w();
-        gnss_pose_.pose.orientation.x = tf_map_to_base.getRotation().x();
-        gnss_pose_.pose.orientation.y = tf_map_to_base.getRotation().y();
-        gnss_pose_.pose.orientation.z = tf_map_to_base.getRotation().z();
-
-        /* broadcast & publish */ 
-        broadcaster.sendTransform(tf::StampedTransform(tf_map_to_base, ros::Time::now(), "/map", "/base_link"));
-        gnss_pose_pub_.publish(gnss_pose_); 
-
-        rate.sleep();
-    }
-}
diff --git a/rubis_ws/deprecated/gnss_converter/src/ntrip_client.cpp b/rubis_ws/deprecated/gnss_converter/src/ntrip_client.cpp
deleted file mode 100644
index f7ec3548..00000000
--- a/rubis_ws/deprecated/gnss_converter/src/ntrip_client.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-#include <sys/wait.h>
-#include <unistd.h>
-#include <ros/ros.h>
-#include <string>
-#include <stdlib.h>
-#include <sys/types.h>
-
-static int pid;
-
-void INT_handler(int sig){
-    if (pid != 0){
-        kill(-1 * (pid), SIGINT);
-    }
-    exit(0);
-}
-
-int main(int argc, char *argv[]){
-    ros::init(argc, argv, "ntrip_client");
-    ros::NodeHandle nh;
-
-    //  register SIGINT handler
-    struct sigaction action;
-    action.sa_handler = INT_handler;
-    sigemptyset(&action.sa_mask);
-    action.sa_flags = SA_RESTART;
-    if (sigaction(SIGINT, &action, NULL) < 0){
-        ROS_ERROR("ntrip : cannot install signal handler");
-        exit(EXIT_FAILURE);
-    }
-
-    std::string file_path;
-    ros::param::get("/ntrip_file_path", file_path);
-
-    char ntrip_file_path_cstr[200];
-    int str_len = file_path.length();
-    if (str_len >= 200)
-    {
-        ROS_ERROR("ntrip : file path is too long");
-        exit(0);
-    }
-    strcpy(ntrip_file_path_cstr, file_path.c_str());
-    char *exe_argv[] = {"/bin/bash",
-                        "-c",
-                        ntrip_file_path_cstr,
-                        NULL};
-
-    while (ros::ok()){
-        if ((pid = fork()) < 0){
-            ROS_ERROR("ntrip : cannot create child process");
-        }
-
-        //  child process
-        if (pid == 0){
-            if (execvp(exe_argv[0], exe_argv) < 0){
-                ROS_ERROR("ntrip : cannot execute script file");
-            }
-            exit(EXIT_FAILURE);
-        }
-        //  parent process
-        else{
-            int wstatus;
-            while (waitpid(pid, &wstatus, WNOHANG) == 0){
-                sleep(2);
-            }
-        }
-    }
-}
\ No newline at end of file
diff --git a/rubis_ws/deprecated/gnss_converter/src/position_pub.cpp b/rubis_ws/deprecated/gnss_converter/src/position_pub.cpp
deleted file mode 100644
index c1926207..00000000
--- a/rubis_ws/deprecated/gnss_converter/src/position_pub.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-#include <ros/ros.h>
-#include <gnss_converter/LLH2UTM.h>
-
-#include <geometry_msgs/PoseStamped.h>
-#include <inertiallabs_msgs/gps_data.h>
-#include <inertiallabs_msgs/ins_data.h>
-
-#define M_PI 3.14159265358979323846
-
-static ros::Publisher gnss_pose_pub_;
-static geometry_msgs::PoseStamped gnss_pose_;
-static double x_offset_, y_offset_, z_offset_, yaw_offset_;
-static double roll_, pitch_, yaw_;
-
-void inertial_gps_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps){
-    gnss_pose_.header = msg_gps->header;
-    gnss_pose_.header.frame_id = "/map";
-
-    /* coordinate transform (LLH2 to UTM) */ 
-    LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, gnss_pose_);
-
-    /* position offset calculation */ 
-    gnss_pose_.pose.position.x = gnss_pose_.pose.position.x - x_offset_;
-    gnss_pose_.pose.position.y = gnss_pose_.pose.position.y - y_offset_;
-    gnss_pose_.pose.position.z = gnss_pose_.pose.position.z - z_offset_;
-
-    gnss_pose_pub_.publish(gnss_pose_);
-}
-
-int main(int argc, char *argv[]){
-    ros::init(argc, argv, "position_pub");
-
-    ros::NodeHandle nh;
-    ros::NodeHandle private_nh("~");
-
-    private_nh.param("x_offset", x_offset_, 0.0);
-    private_nh.param("y_offset", y_offset_, 0.0);
-    private_nh.param("z_offset", z_offset_, 0.0);
-
-    ros::Subscriber inertial_gps_sub;
-    inertial_gps_sub = nh.subscribe("/Inertial_Labs/gps_data", 10, inertial_gps_cb);
-
-    gnss_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/gnss_pose", 10);
-
-    ros::spin();
-
-    // ros::Rate rate(10);
-
-    // while(nh.ok()){
-    //     ros::spinOnce();
-    //     rate.sleep();
-    // }
-}
diff --git a/rubis_ws/src/image_common/image_transport/CHANGELOG.rst b/rubis_ws/deprecated/image_common/image_transport/CHANGELOG.rst
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/CHANGELOG.rst
rename to rubis_ws/deprecated/image_common/image_transport/CHANGELOG.rst
diff --git a/rubis_ws/src/image_common/image_transport/CMakeLists.txt b/rubis_ws/deprecated/image_common/image_transport/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/CMakeLists.txt
rename to rubis_ws/deprecated/image_common/image_transport/CMakeLists.txt
diff --git a/rubis_ws/src/image_common/image_transport/default_plugins.xml b/rubis_ws/deprecated/image_common/image_transport/default_plugins.xml
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/default_plugins.xml
rename to rubis_ws/deprecated/image_common/image_transport/default_plugins.xml
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/camera_common.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/camera_common.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/camera_common.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/camera_common.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/camera_publisher.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/camera_publisher.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/camera_publisher.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/camera_publisher.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/camera_subscriber.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/camera_subscriber.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/camera_subscriber.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/camera_subscriber.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/exception.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/exception.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/exception.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/exception.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/exports.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/exports.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/exports.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/exports.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/image_transport.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/image_transport.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/image_transport.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/image_transport.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/loader_fwds.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/loader_fwds.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/loader_fwds.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/loader_fwds.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/publisher.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/publisher.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/publisher.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/publisher.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/publisher_plugin.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/publisher_plugin.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/publisher_plugin.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/publisher_plugin.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/raw_publisher.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/raw_publisher.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/raw_publisher.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/raw_publisher.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/raw_subscriber.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/raw_subscriber.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/raw_subscriber.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/raw_subscriber.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/simple_publisher_plugin.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/simple_publisher_plugin.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/simple_publisher_plugin.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/simple_publisher_plugin.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/single_subscriber_publisher.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/single_subscriber_publisher.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/single_subscriber_publisher.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/single_subscriber_publisher.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/subscriber.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/subscriber.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/subscriber.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/subscriber.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/subscriber_filter.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/subscriber_filter.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/subscriber_filter.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/subscriber_filter.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/subscriber_plugin.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/subscriber_plugin.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/subscriber_plugin.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/subscriber_plugin.h
diff --git a/rubis_ws/src/image_common/image_transport/include/image_transport/transport_hints.h b/rubis_ws/deprecated/image_common/image_transport/include/image_transport/transport_hints.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/include/image_transport/transport_hints.h
rename to rubis_ws/deprecated/image_common/image_transport/include/image_transport/transport_hints.h
diff --git a/rubis_ws/src/image_common/image_transport/mainpage.dox b/rubis_ws/deprecated/image_common/image_transport/mainpage.dox
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/mainpage.dox
rename to rubis_ws/deprecated/image_common/image_transport/mainpage.dox
diff --git a/rubis_ws/src/image_common/image_transport/package.xml b/rubis_ws/deprecated/image_common/image_transport/package.xml
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/package.xml
rename to rubis_ws/deprecated/image_common/image_transport/package.xml
diff --git a/rubis_ws/src/image_common/image_transport/src/camera_common.cpp b/rubis_ws/deprecated/image_common/image_transport/src/camera_common.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/camera_common.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/camera_common.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/camera_publisher.cpp b/rubis_ws/deprecated/image_common/image_transport/src/camera_publisher.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/camera_publisher.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/camera_publisher.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/camera_subscriber.cpp b/rubis_ws/deprecated/image_common/image_transport/src/camera_subscriber.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/camera_subscriber.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/camera_subscriber.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/image_transport.cpp b/rubis_ws/deprecated/image_common/image_transport/src/image_transport.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/image_transport.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/image_transport.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/list_transports.cpp b/rubis_ws/deprecated/image_common/image_transport/src/list_transports.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/list_transports.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/list_transports.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/manifest.cpp b/rubis_ws/deprecated/image_common/image_transport/src/manifest.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/manifest.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/manifest.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/publisher.cpp b/rubis_ws/deprecated/image_common/image_transport/src/publisher.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/publisher.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/publisher.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/raw_publisher.cpp b/rubis_ws/deprecated/image_common/image_transport/src/raw_publisher.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/raw_publisher.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/raw_publisher.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/republish.cpp b/rubis_ws/deprecated/image_common/image_transport/src/republish.cpp
similarity index 66%
rename from rubis_ws/src/image_common/image_transport/src/republish.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/republish.cpp
index 589e351a..d0510ac0 100644
--- a/rubis_ws/src/image_common/image_transport/src/republish.cpp
+++ b/rubis_ws/deprecated/image_common/image_transport/src/republish.cpp
@@ -54,23 +54,19 @@ int main(int argc, char** argv)
 
   // scheduling
   ros::NodeHandle pnh("~");
-  int task_scheduling_flag = 0;
-  int task_profiling_flag = 0;
   std::string task_response_time_filename;
   int rate = 0;
   double task_minimum_inter_release_time = 0;
   double task_execution_time = 0;
   double task_relative_deadline = 0;
 
-  pnh.param<int>("/republish/task_scheduling_flag", task_scheduling_flag, 0);
-  pnh.param<int>("/republish/task_profiling_flag", task_profiling_flag, 0);
   pnh.param<std::string>("/republish/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/republish.csv");
   pnh.param<int>("/republish/rate", rate, 10);
   pnh.param("/republish/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000);
   pnh.param("/republish/task_execution_time", task_execution_time, (double)100000000);
   pnh.param("/republish/task_relative_deadline", task_relative_deadline, (double)100000000);
   
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
+  rubis::init_task_profiling(task_response_time_filename);
 
   if (argc < 3) {
     // Use all available transports for output
@@ -81,34 +77,17 @@ int main(int argc, char** argv)
     PublishMemFn pub_mem_fn = &image_transport::Publisher::publish;
     sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, _1), ros::VoidPtr(), in_transport);
 
-    rubis::sched::task_state_ = TASK_STATE_READY;
+    ros::Rate r(rate);
+    // Executing task
+    while(ros::ok()){
+      rubis::start_task_profiling();
 
-    if(!task_scheduling_flag && !task_profiling_flag){
-      ros::spin();
-    }
-    else{
-      ros::Rate r(rate);
-      // Executing task
-      while(ros::ok()){
-        if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-        if(rubis::sched::task_state_ == TASK_STATE_READY){          
-          if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-          rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-        }
+      ros::spinOnce();
+      
 
-        ros::spinOnce();
-        rubis::sched::task_state_ = TASK_STATE_DONE;
+      rubis::stop_task_profiling(0, 0, 0);
 
-        if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
-
-        if(rubis::sched::task_state_ == TASK_STATE_DONE){          
-          if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-          rubis::sched::task_state_ = TASK_STATE_READY;
-        }
-        
-        r.sleep();
-      }
+      r.sleep();
     }
   }
   else {
@@ -128,32 +107,14 @@ int main(int argc, char** argv)
     PublishMemFn pub_mem_fn = &Plugin::publish;
     sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), _1), pub, in_transport);
 
-    if(!task_scheduling_flag && !task_profiling_flag){
-      ros::spin();
-    }
-    else{      
-      ros::Rate r(rate);
-      // Executing task      
-      while(ros::ok()){
-        if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-        if(rubis::sched::task_state_ == TASK_STATE_READY){          
-          if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-          rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-        }
-
-        ros::spinOnce();
-        rubis::sched::task_state_ = TASK_STATE_DONE;
-
-        if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
-
-        if(rubis::sched::task_state_ == TASK_STATE_DONE){          
-          if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-          rubis::sched::task_state_ = TASK_STATE_READY;
-        }
-        
-        r.sleep();
-      }
+    ros::Rate r(rate);
+    // Executing task      
+    while(ros::ok()){
+      rubis::start_task_profiling();
+      ros::spinOnce();
+      
+      rubis::stop_task_profiling(0, 0, 0);
+      r.sleep();
     }
   }
 
diff --git a/rubis_ws/src/image_common/image_transport/src/single_subscriber_publisher.cpp b/rubis_ws/deprecated/image_common/image_transport/src/single_subscriber_publisher.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/single_subscriber_publisher.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/single_subscriber_publisher.cpp
diff --git a/rubis_ws/src/image_common/image_transport/src/subscriber.cpp b/rubis_ws/deprecated/image_common/image_transport/src/subscriber.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/src/subscriber.cpp
rename to rubis_ws/deprecated/image_common/image_transport/src/subscriber.cpp
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/CMakeLists.txt b/rubis_ws/deprecated/image_common/image_transport/tutorial/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/CMakeLists.txt
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/CMakeLists.txt
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h b/rubis_ws/deprecated/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h b/rubis_ws/deprecated/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/msg/ResizedImage.msg b/rubis_ws/deprecated/image_common/image_transport/tutorial/msg/ResizedImage.msg
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/msg/ResizedImage.msg
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/msg/ResizedImage.msg
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/package.xml b/rubis_ws/deprecated/image_common/image_transport/tutorial/package.xml
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/package.xml
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/package.xml
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/resized_plugins.xml b/rubis_ws/deprecated/image_common/image_transport/tutorial/resized_plugins.xml
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/resized_plugins.xml
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/resized_plugins.xml
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/src/manifest.cpp b/rubis_ws/deprecated/image_common/image_transport/tutorial/src/manifest.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/src/manifest.cpp
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/src/manifest.cpp
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/src/my_publisher.cpp b/rubis_ws/deprecated/image_common/image_transport/tutorial/src/my_publisher.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/src/my_publisher.cpp
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/src/my_publisher.cpp
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/src/my_subscriber.cpp b/rubis_ws/deprecated/image_common/image_transport/tutorial/src/my_subscriber.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/src/my_subscriber.cpp
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/src/my_subscriber.cpp
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/src/resized_publisher.cpp b/rubis_ws/deprecated/image_common/image_transport/tutorial/src/resized_publisher.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/src/resized_publisher.cpp
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/src/resized_publisher.cpp
diff --git a/rubis_ws/src/image_common/image_transport/tutorial/src/resized_subscriber.cpp b/rubis_ws/deprecated/image_common/image_transport/tutorial/src/resized_subscriber.cpp
similarity index 100%
rename from rubis_ws/src/image_common/image_transport/tutorial/src/resized_subscriber.cpp
rename to rubis_ws/deprecated/image_common/image_transport/tutorial/src/resized_subscriber.cpp
diff --git a/rubis_ws/src/image_common/polled_camera/CHANGELOG.rst b/rubis_ws/deprecated/image_common/polled_camera/CHANGELOG.rst
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/CHANGELOG.rst
rename to rubis_ws/deprecated/image_common/polled_camera/CHANGELOG.rst
diff --git a/rubis_ws/src/image_common/polled_camera/CMakeLists.txt b/rubis_ws/deprecated/image_common/polled_camera/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/CMakeLists.txt
rename to rubis_ws/deprecated/image_common/polled_camera/CMakeLists.txt
diff --git a/rubis_ws/src/image_common/polled_camera/include/polled_camera/publication_server.h b/rubis_ws/deprecated/image_common/polled_camera/include/polled_camera/publication_server.h
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/include/polled_camera/publication_server.h
rename to rubis_ws/deprecated/image_common/polled_camera/include/polled_camera/publication_server.h
diff --git a/rubis_ws/src/image_common/polled_camera/mainpage.dox b/rubis_ws/deprecated/image_common/polled_camera/mainpage.dox
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/mainpage.dox
rename to rubis_ws/deprecated/image_common/polled_camera/mainpage.dox
diff --git a/rubis_ws/src/image_common/polled_camera/package.xml b/rubis_ws/deprecated/image_common/polled_camera/package.xml
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/package.xml
rename to rubis_ws/deprecated/image_common/polled_camera/package.xml
diff --git a/rubis_ws/src/image_common/polled_camera/src/poller.cpp b/rubis_ws/deprecated/image_common/polled_camera/src/poller.cpp
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/src/poller.cpp
rename to rubis_ws/deprecated/image_common/polled_camera/src/poller.cpp
diff --git a/rubis_ws/src/image_common/polled_camera/src/publication_server.cpp b/rubis_ws/deprecated/image_common/polled_camera/src/publication_server.cpp
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/src/publication_server.cpp
rename to rubis_ws/deprecated/image_common/polled_camera/src/publication_server.cpp
diff --git a/rubis_ws/src/image_common/polled_camera/srv/GetPolledImage.srv b/rubis_ws/deprecated/image_common/polled_camera/srv/GetPolledImage.srv
similarity index 100%
rename from rubis_ws/src/image_common/polled_camera/srv/GetPolledImage.srv
rename to rubis_ws/deprecated/image_common/polled_camera/srv/GetPolledImage.srv
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt
similarity index 95%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt
index 66aa1a13..bf1bf2a9 100644
--- a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt
+++ b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt
@@ -15,7 +15,6 @@ inertiallabs_msgs
 
 include_directories(${catkin_INCLUDE_DIRS})
 
-find_package(OpenCV)
 
 set(DEPRECATION_FLAG "-Wsizeof-array-argument")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra ${DEPRECATION_FLAG}")
@@ -26,7 +25,6 @@ catkin_package(
   DEPENDS system_lib
 )
 
-include_directories(${Opencv_INCLUDE_DIRS})
 include_directories(${system_lib_INCLUDE_DIRS})
 
 include_directories(../inertiallabs_sdk/)
diff --git a/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch
new file mode 100644
index 00000000..baff29f4
--- /dev/null
+++ b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch
@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="inertiallabs_ins" type="il_ins" name="INS">
+   <param name="ins_url" type="string" value="serial:/dev/ttyUSB0:460800" />
+   <param name="ins_output_format" type="int" value="82"/>
+  </node>
+</launch>
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/package.xml b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/package.xml
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/package.xml
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/package.xml
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/src/il_ins.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/src/il_ins.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/src/il_ins.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_ins/src/il_ins.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/CMakeLists.txt b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/CMakeLists.txt
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/CMakeLists.txt
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/README.md b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/README.md
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/README.md
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/README.md
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/gnss_data.msg b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/gnss_data.msg
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/gnss_data.msg
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/gnss_data.msg
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/gps_data.msg b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/gps_data.msg
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/gps_data.msg
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/gps_data.msg
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/ins_data.msg b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/ins_data.msg
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/ins_data.msg
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/ins_data.msg
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/marine_data.msg b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/marine_data.msg
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/marine_data.msg
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/marine_data.msg
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/sensor_data.msg b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/sensor_data.msg
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/msg/sensor_data.msg
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/msg/sensor_data.msg
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/package.xml b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/package.xml
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_msgs/package.xml
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_msgs/package.xml
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.h b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.h
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.h
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/ILDriver.h
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/LICENSE.txt b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/LICENSE.txt
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/LICENSE.txt
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/LICENSE.txt
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/Makefile b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/Makefile
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/Makefile
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/Makefile
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/NetClient.h b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/NetClient.h
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/NetClient.h
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/NetClient.h
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/SerialPort.h b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/SerialPort.h
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/SerialPort.h
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/SerialPort.h
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/Transport.h b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/Transport.h
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/Transport.h
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/Transport.h
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.h b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.h
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.h
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/UDDParser.h
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/dataStructures.h b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/dataStructures.h
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/dataStructures.h
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/dataStructures.h
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/example.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/example.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/example.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/example.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/NetClient.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/NetClient.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/NetClient.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/NetClient.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/SerialPort.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/SerialPort.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/SerialPort.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/linux/SerialPort.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/NetClient.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/NetClient.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/NetClient.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/NetClient.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/SerialPort.cpp b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/SerialPort.cpp
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/SerialPort.cpp
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/platforms/windows/SerialPort.cpp
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj.filters b/rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj.filters
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj.filters
rename to rubis_ws/deprecated/inertiallabs_pkgs/inertiallabs_sdk/winsample.vcxproj.filters
diff --git a/rubis_ws/src/vesc/README.md b/rubis_ws/deprecated/vesc/README.md
similarity index 100%
rename from rubis_ws/src/vesc/README.md
rename to rubis_ws/deprecated/vesc/README.md
diff --git a/rubis_ws/src/vesc/vesc/CMakeLists.txt b/rubis_ws/deprecated/vesc/vesc/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/vesc/vesc/CMakeLists.txt
rename to rubis_ws/deprecated/vesc/vesc/CMakeLists.txt
diff --git a/rubis_ws/src/vesc/vesc/package.xml b/rubis_ws/deprecated/vesc/vesc/package.xml
similarity index 100%
rename from rubis_ws/src/vesc/vesc/package.xml
rename to rubis_ws/deprecated/vesc/vesc/package.xml
diff --git a/rubis_ws/src/vesc/vesc_ackermann/CMakeLists.txt b/rubis_ws/deprecated/vesc/vesc_ackermann/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/CMakeLists.txt
rename to rubis_ws/deprecated/vesc/vesc_ackermann/CMakeLists.txt
diff --git a/rubis_ws/src/vesc/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/rubis_ws/deprecated/vesc/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h
rename to rubis_ws/deprecated/vesc/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h
diff --git a/rubis_ws/src/vesc/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/rubis_ws/deprecated/vesc/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
rename to rubis_ws/deprecated/vesc/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
diff --git a/rubis_ws/deprecated/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch
new file mode 100644
index 00000000..7857f4e8
--- /dev/null
+++ b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch
@@ -0,0 +1,14 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <!-- Optionally launch in GDB, for debugging -->
+  <arg name="debug" default="false" />
+  <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+
+  <!-- Ackermann to VESC node -->
+  <node pkg="vesc_ackermann" type="ackermann_to_vesc_node" name="ackermann_to_vesc_node"
+        launch-prefix="$(arg launch_prefix)" >
+  </node>
+
+</launch>
diff --git a/rubis_ws/deprecated/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch
new file mode 100644
index 00000000..a5c3843c
--- /dev/null
+++ b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch
@@ -0,0 +1,29 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <arg name="start_manager" default="true" />
+  <arg name="manager" default="vesc_nodelet_manager" />
+
+  <!-- Optionally launch manager in GDB, for debugging -->
+  <arg name="debug" default="false" />
+  <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <!-- Worker threads -->
+  <arg name="num_worker_threads" default="4" />
+
+  <!-- Also globally disable bond heartbeat timeout in debug mode, so everything
+       doesn't die when you hit a break point -->
+  <param if="$(arg debug)" name="/bond_disable_heartbeat_timeout" value="true" />
+
+  <!-- Nodelet manager -->
+  <node if="$(arg start_manager)" pkg="nodelet" type="nodelet" name="$(arg manager)"
+        args="manager" launch-prefix="$(arg launch_prefix)">
+     <param name="num_worker_threads" value="$(arg num_worker_threads)" />
+  </node>
+
+  <!-- Ackermann to VESC nodelet -->
+  <node pkg="nodelet" type="nodelet" name="ackermann_to_vesc_nodelet"
+        args="load vesc_ackermann::AckermannToVescNodelet $(arg manager)" >
+  </node>    
+
+</launch>
diff --git a/rubis_ws/deprecated/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch
new file mode 100644
index 00000000..48644eeb
--- /dev/null
+++ b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch
@@ -0,0 +1,14 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <!-- Optionally launch in GDB, for debugging -->
+  <arg name="debug" default="false" />
+  <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+
+  <!-- VESC to Odom node -->
+  <node pkg="vesc_ackermann" type="vesc_to_odom_node" name="vesc_to_odom_node"
+        launch-prefix="$(arg launch_prefix)" >
+  </node>
+
+</launch>
diff --git a/rubis_ws/deprecated/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch
new file mode 100644
index 00000000..1db0a9cf
--- /dev/null
+++ b/rubis_ws/deprecated/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch
@@ -0,0 +1,29 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <arg name="start_manager" default="true" />
+  <arg name="manager" default="vesc_nodelet_manager" />
+
+  <!-- Optionally launch manager in GDB, for debugging -->
+  <arg name="debug" default="false" />
+  <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <!-- Worker threads -->
+  <arg name="num_worker_threads" default="4" />
+
+  <!-- Also globally disable bond heartbeat timeout in debug mode, so everything
+       doesn't die when you hit a break point -->
+  <param if="$(arg debug)" name="/bond_disable_heartbeat_timeout" value="true" />
+
+  <!-- Nodelet manager -->
+  <node if="$(arg start_manager)" pkg="nodelet" type="nodelet" name="$(arg manager)"
+        args="manager" launch-prefix="$(arg launch_prefix)">
+     <param name="num_worker_threads" value="$(arg num_worker_threads)" />
+  </node>
+
+  <!-- VESC to Odom nodelet -->
+  <node pkg="nodelet" type="nodelet" name="vesc_to_odom_nodelet"
+        args="load vesc_ackermann::VescToOdomNodelet $(arg manager)" >
+  </node>    
+
+</launch>
diff --git a/rubis_ws/src/vesc/vesc_ackermann/package.xml b/rubis_ws/deprecated/vesc/vesc_ackermann/package.xml
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/package.xml
rename to rubis_ws/deprecated/vesc/vesc_ackermann/package.xml
diff --git a/rubis_ws/src/vesc/vesc_ackermann/src/ackermann_to_vesc.cpp b/rubis_ws/deprecated/vesc/vesc_ackermann/src/ackermann_to_vesc.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/src/ackermann_to_vesc.cpp
rename to rubis_ws/deprecated/vesc/vesc_ackermann/src/ackermann_to_vesc.cpp
diff --git a/rubis_ws/src/vesc/vesc_ackermann/src/ackermann_to_vesc_node.cpp b/rubis_ws/deprecated/vesc/vesc_ackermann/src/ackermann_to_vesc_node.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/src/ackermann_to_vesc_node.cpp
rename to rubis_ws/deprecated/vesc/vesc_ackermann/src/ackermann_to_vesc_node.cpp
diff --git a/rubis_ws/src/vesc/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/rubis_ws/deprecated/vesc/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp
rename to rubis_ws/deprecated/vesc/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp
diff --git a/rubis_ws/src/vesc/vesc_ackermann/src/vesc_to_odom.cpp b/rubis_ws/deprecated/vesc/vesc_ackermann/src/vesc_to_odom.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/src/vesc_to_odom.cpp
rename to rubis_ws/deprecated/vesc/vesc_ackermann/src/vesc_to_odom.cpp
diff --git a/rubis_ws/src/vesc/vesc_ackermann/src/vesc_to_odom_node.cpp b/rubis_ws/deprecated/vesc/vesc_ackermann/src/vesc_to_odom_node.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/src/vesc_to_odom_node.cpp
rename to rubis_ws/deprecated/vesc/vesc_ackermann/src/vesc_to_odom_node.cpp
diff --git a/rubis_ws/src/vesc/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/rubis_ws/deprecated/vesc/vesc_ackermann/src/vesc_to_odom_nodelet.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/src/vesc_to_odom_nodelet.cpp
rename to rubis_ws/deprecated/vesc/vesc_ackermann/src/vesc_to_odom_nodelet.cpp
diff --git a/rubis_ws/src/vesc/vesc_ackermann/vesc_ackermann_nodelet.xml b/rubis_ws/deprecated/vesc/vesc_ackermann/vesc_ackermann_nodelet.xml
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/vesc_ackermann_nodelet.xml
rename to rubis_ws/deprecated/vesc/vesc_ackermann/vesc_ackermann_nodelet.xml
diff --git a/rubis_ws/src/vesc/vesc_driver/CMakeLists.txt b/rubis_ws/deprecated/vesc/vesc_driver/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/CMakeLists.txt
rename to rubis_ws/deprecated/vesc/vesc_driver/CMakeLists.txt
diff --git a/rubis_ws/src/vesc/vesc_driver/include/vesc_driver/datatypes.h b/rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/datatypes.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/include/vesc_driver/datatypes.h
rename to rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/datatypes.h
diff --git a/rubis_ws/src/vesc/vesc_driver/include/vesc_driver/v8stdint.h b/rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/v8stdint.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/include/vesc_driver/v8stdint.h
rename to rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/v8stdint.h
diff --git a/rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_driver.h b/rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_driver.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_driver.h
rename to rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_driver.h
diff --git a/rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_interface.h b/rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_interface.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_interface.h
rename to rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_interface.h
diff --git a/rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_packet.h b/rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_packet.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_packet.h
rename to rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_packet.h
diff --git a/rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_packet_factory.h
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/include/vesc_driver/vesc_packet_factory.h
rename to rubis_ws/deprecated/vesc/vesc_driver/include/vesc_driver/vesc_packet_factory.h
diff --git a/rubis_ws/deprecated/vesc/vesc_driver/launch/vesc_driver_node.launch b/rubis_ws/deprecated/vesc/vesc_driver/launch/vesc_driver_node.launch
new file mode 100644
index 00000000..ec15f0d2
--- /dev/null
+++ b/rubis_ws/deprecated/vesc/vesc_driver/launch/vesc_driver_node.launch
@@ -0,0 +1,20 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <arg name="node_name" default="vesc_driver_node" />
+
+  <!-- Optionally launch in GDB, for debugging -->
+  <arg name="debug" default="false" />
+  <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+
+  <!-- VESC driver parameters -->
+  <arg name="port" default="/dev/vesc" />
+
+  <!-- VESC driver node -->
+  <node pkg="vesc_driver" type="vesc_driver_node" name="$(arg node_name)"
+        launch-prefix="$(arg launch_prefix)" >
+    <param name="port" value="$(arg port)" />
+  </node>
+
+</launch>
diff --git a/rubis_ws/deprecated/vesc/vesc_driver/launch/vesc_driver_nodelet.launch b/rubis_ws/deprecated/vesc/vesc_driver/launch/vesc_driver_nodelet.launch
new file mode 100644
index 00000000..05c542b3
--- /dev/null
+++ b/rubis_ws/deprecated/vesc/vesc_driver/launch/vesc_driver_nodelet.launch
@@ -0,0 +1,34 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <arg name="node_name" default="vesc_driver_nodelet" />
+  <arg name="start_manager" default="true" />
+  <arg name="manager" default="vesc_nodelet_manager" />
+
+  <!-- Optionally launch manager in GDB, for debugging -->
+  <arg name="debug" default="false" />
+  <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <!-- Worker threads -->
+  <arg name="num_worker_threads" default="4" />
+
+  <!-- VESC driver parameters -->
+  <arg name="port" default="/dev/ttyUSB0" />
+  
+  <!-- Also globally disable bond heartbeat timeout in debug mode, so everything
+       doesn't die when you hit a break point -->
+  <param if="$(arg debug)" name="/bond_disable_heartbeat_timeout" value="true" />
+
+  <!-- Nodelet manager -->
+  <node if="$(arg start_manager)" pkg="nodelet" type="nodelet" name="$(arg manager)"
+        args="manager" launch-prefix="$(arg launch_prefix)">
+     <param name="num_worker_threads" value="$(arg num_worker_threads)" />
+  </node>
+
+  <!-- VESC driver nodelet -->
+  <node pkg="nodelet" type="nodelet" name="$(arg node_name)"
+        args="load vesc_driver::VescDriverNodelet $(arg manager)" >
+    <param name="port" value="$(arg port)" />
+  </node>    
+
+</launch>
diff --git a/rubis_ws/src/vesc/vesc_driver/package.xml b/rubis_ws/deprecated/vesc/vesc_driver/package.xml
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/package.xml
rename to rubis_ws/deprecated/vesc/vesc_driver/package.xml
diff --git a/rubis_ws/src/vesc/vesc_driver/src/vesc_driver.cpp b/rubis_ws/deprecated/vesc/vesc_driver/src/vesc_driver.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/src/vesc_driver.cpp
rename to rubis_ws/deprecated/vesc/vesc_driver/src/vesc_driver.cpp
diff --git a/rubis_ws/src/vesc/vesc_driver/src/vesc_driver_node.cpp b/rubis_ws/deprecated/vesc/vesc_driver/src/vesc_driver_node.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/src/vesc_driver_node.cpp
rename to rubis_ws/deprecated/vesc/vesc_driver/src/vesc_driver_node.cpp
diff --git a/rubis_ws/src/vesc/vesc_driver/src/vesc_driver_nodelet.cpp b/rubis_ws/deprecated/vesc/vesc_driver/src/vesc_driver_nodelet.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/src/vesc_driver_nodelet.cpp
rename to rubis_ws/deprecated/vesc/vesc_driver/src/vesc_driver_nodelet.cpp
diff --git a/rubis_ws/src/vesc/vesc_driver/src/vesc_interface.cpp b/rubis_ws/deprecated/vesc/vesc_driver/src/vesc_interface.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/src/vesc_interface.cpp
rename to rubis_ws/deprecated/vesc/vesc_driver/src/vesc_interface.cpp
diff --git a/rubis_ws/src/vesc/vesc_driver/src/vesc_packet.cpp b/rubis_ws/deprecated/vesc/vesc_driver/src/vesc_packet.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/src/vesc_packet.cpp
rename to rubis_ws/deprecated/vesc/vesc_driver/src/vesc_packet.cpp
diff --git a/rubis_ws/src/vesc/vesc_driver/src/vesc_packet_factory.cpp b/rubis_ws/deprecated/vesc/vesc_driver/src/vesc_packet_factory.cpp
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/src/vesc_packet_factory.cpp
rename to rubis_ws/deprecated/vesc/vesc_driver/src/vesc_packet_factory.cpp
diff --git a/rubis_ws/src/vesc/vesc_driver/vesc_driver_nodelet.xml b/rubis_ws/deprecated/vesc/vesc_driver/vesc_driver_nodelet.xml
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/vesc_driver_nodelet.xml
rename to rubis_ws/deprecated/vesc/vesc_driver/vesc_driver_nodelet.xml
diff --git a/rubis_ws/src/vesc/vesc_msgs/CMakeLists.txt b/rubis_ws/deprecated/vesc/vesc_msgs/CMakeLists.txt
similarity index 100%
rename from rubis_ws/src/vesc/vesc_msgs/CMakeLists.txt
rename to rubis_ws/deprecated/vesc/vesc_msgs/CMakeLists.txt
diff --git a/rubis_ws/src/vesc/vesc_msgs/msg/VescState.msg b/rubis_ws/deprecated/vesc/vesc_msgs/msg/VescState.msg
similarity index 100%
rename from rubis_ws/src/vesc/vesc_msgs/msg/VescState.msg
rename to rubis_ws/deprecated/vesc/vesc_msgs/msg/VescState.msg
diff --git a/rubis_ws/src/vesc/vesc_msgs/msg/VescStateStamped.msg b/rubis_ws/deprecated/vesc/vesc_msgs/msg/VescStateStamped.msg
similarity index 100%
rename from rubis_ws/src/vesc/vesc_msgs/msg/VescStateStamped.msg
rename to rubis_ws/deprecated/vesc/vesc_msgs/msg/VescStateStamped.msg
diff --git a/rubis_ws/src/vesc/vesc_msgs/package.xml b/rubis_ws/deprecated/vesc/vesc_msgs/package.xml
similarity index 100%
rename from rubis_ws/src/vesc/vesc_msgs/package.xml
rename to rubis_ws/deprecated/vesc/vesc_msgs/package.xml
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/CHANGELOG.rst b/rubis_ws/deprecated/vision_darknet_detect_opencl/CHANGELOG.rst
new file mode 100644
index 00000000..7b9a9e1d
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/CHANGELOG.rst
@@ -0,0 +1,357 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package vision_yolo3_detect
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* Removing CUDA dependencies for Darknet Yolov3 (`#1784 <https://github.com/CPFL/Autoware/issues/1784>`_)
+  * Removing CUDA dependencies for Darknet yolov3
+  If the host machine does not have CUDA, this will build the vision_darknet_detect package based on a pre-built darknet directory (which doesn't require CUDA as there are no CUDA dependencies for yolov3).
+  * Update ros/src/computing/perception/detection/vision_detector/packages/vision_darknet_detect/CMakeLists.txt
+  Co-Authored-By: K1504296 <greytrt@gmail.com>
+* Fix license notice in corresponding package.xml
+* Initial release of object filter
+* Contributors: Abraham Monrroy, Theodore, amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+* Fixes for catkin_make
+* [fix] SSD detector, cmake colcon (`#1837 <https://github.com/CPFL/Autoware/issues/1837>`_)
+  * Fixes for new colcon script on ssd cuda based node
+  * Fixed to RTM and darknet launch files
+  * catkin_fix
+  * * catkin & colcon build successfully
+  * reverted back run to devel space (for the time being)
+* Switch to Apache 2 license (develop branch) (`#1741 <https://github.com/CPFL/Autoware/issues/1741>`_)
+  * Switch to Apache 2
+  * Replace BSD-3 license header with Apache 2 and reassign copyright to the
+  Autoware Foundation.
+  * Update license on Python files
+  * Update copyright years
+  * Add #ifndef/define _POINTS_IMAGE_H\_
+  * Updated license comment
+* Use colcon as the build tool (`#1704 <https://github.com/CPFL/Autoware/issues/1704>`_)
+  * Switch to colcon as the build tool instead of catkin
+  * Added cmake-target
+  * Added note about the second colcon call
+  * Added warning about catkin* scripts being deprecated
+  * Fix COLCON_OPTS
+  * Added install targets
+  * Update Docker image tags
+  * Message packages fixes
+  * Fix missing dependency
+* Feature/perception visualization cleanup (`#1648 <https://github.com/CPFL/Autoware/issues/1648>`_)
+  * * Initial commit for visualization package
+  * Removal of all visualization messages from perception nodes
+  * Visualization dependency removal
+  * Launch file modification
+  * * Fixes to visualization
+  * Error on Clustering CPU
+  * Reduce verbosity on markers
+  * intial commit
+  * * Changed to 2 spaces indentation
+  * Added README
+  * Fixed README messages type
+  * 2 space indenting
+  * ros clang format
+  * Publish acceleration and velocity from ukf tracker
+  * Remove hardcoded path
+  * Updated README
+  * updated prototype
+  * Prototype update for header and usage
+  * Removed unknown label from being reported
+  * Updated publishing orientation to match develop
+  * * Published all the trackers
+  * Added valid field for visualization and future compatibility with ADAS ROI filtering
+  * Add simple functions
+  * Refacor code
+  * * Reversed back UKF node to develop
+  * Formatted speed
+  * Refactor codes
+  * Refactor codes
+  * Refactor codes
+  * Refacor codes
+  * Make tracking visualization work
+  * Relay class info in tracker node
+  * Remove dependency to jskbbox and rosmarker in ukf tracker
+  * apply rosclang to ukf tracker
+  * Refactor codes
+  * Refactor codes
+  * add comment
+  * refactor codes
+  * Revert "Refactor codes"
+  This reverts commit 135aaac46e49cb18d9b76611576747efab3caf9c.
+  * Revert "apply rosclang to ukf tracker"
+  This reverts commit 4f8d1cb5c8263a491f92ae5321e5080cb34b7b9c.
+  * Revert "Remove dependency to jskbbox and rosmarker in ukf tracker"
+  This reverts commit 4fa1dd40ba58065f7afacc5e478001078925b27d.
+  * Revert "Relay class info in tracker node"
+  This reverts commit 1637baac44c8d3d414cc069f3af12a79770439ae.
+  * delete dependency to jsk and remove pointcloud_frame
+  * get direction nis
+  * set velocity_reliable true in tracker node
+  * Add divided function
+  * add function
+  * Sanity checks
+  * Relay all the data from input DetectedObject
+  * Divided function work both for immukf and sukf
+  * Add comment
+  * Refactor codes
+  * Pass immukf test
+  * make direction assisted tracking work
+  * Visualization fixes
+  * Refacor codes
+  * Refactor codes
+  * Refactor codes
+  * refactor codes
+  * refactor codes
+  * Refactor codes
+  * refactor codes
+  * Tracker Merging step added
+  * Added launch file support for merging phase
+  * lane assisted with sukf
+  * Refactor codes
+  * Refactor codes
+  * * change only static objects
+  * keep label of the oldest tracker
+  * Static Object discrimination
+  * Non rotating bouding box
+  * no disappear if detector works
+  * Modify removeRedundant a bit
+  * Replacement of JSK visualization for RViz Native Markers
+  * Added Models namespace to visualization
+  * Naming change for matching the perception component graph
+  * * Added 3D Models for different classes in visualization
+  * 2D Rect node visualize_rects added to visualization_package
+* Fix Ros/ROS naming convention
+* Fix Ssd/SSD naming convention
+* Contributors: Abraham Monrroy Cano, Esteve Fernandez, amc-nu
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+* Fix compile error (vision_darknet_detect.h:52:37: fatal error: autoware_msgs/ConfigSsd.h: No such file or directory)
+* Moved configuration messages to autoware_config_msgs
+* include fstream header (`#1608 <https://github.com/CPFL/Autoware/issues/1608>`_)
+* Added support for custom class "names files" in darknet format. (`#1535 <https://github.com/CPFL/Autoware/issues/1535>`_)
+  * Added support for custom class "names files" in darknet format.
+  * Fixed launch file, not including source topic arg
+  * Fix the default path of coco.names (`#1550 <https://github.com/CPFL/Autoware/issues/1550>`_)
+* fixes two typos in yolo class name/id file (`#1484 <https://github.com/CPFL/Autoware/issues/1484>`_)
+* Contributors: Abraham Monrroy, Esteve Fernandez, Jacob Lambert, Kenji Funaoka
+
+1.8.0 (2018-08-31)
+------------------
+* fixes two typos in yolo class name/id file (`#1486 <https://github.com/CPFL/Autoware/pull/1486>`_)
+* [Fix] README.md of vision_darknet_detect (`#1437 <https://github.com/CPFL/Autoware/pull/1437>`_)
+* Feature/std perception msg (`#1418 <https://github.com/CPFL/Autoware/pull/1418>`_)
+  * New standard message definition for the perception nodes
+  * New Detected Object message applied to:
+  * SSD
+  * Integrated RVIZ viewer
+  * External Viewer
+  * modified yolo2 and yolo3, compiles but cuda issues, trying different PC
+  * Boiler plate for range vision fusion node
+  * Added GenColors for Kinetic
+  Typo fixes for yolo2
+  * testing colors in Yolo3
+  * Completed transformation, projection of 3D boxes
+  * Fixed error on negative assignation
+  * code clean up
+  * removed yolo2 and yolo3, replaced by single darknet node. GUI launches yolo3 for now, to change. Pushing to test code on other PC.
+  * Readme updated, added gitignore for data folder.
+  * *Added Runtime manager UI for yolo2, yolo3.
+  *Support tested for TinyYolo v2 and v3
+  * Fusion Vision Range
+  Icons for viewer
+  * Range Vision Fusion node
+  * Indigo cv im read
+  * Indigo compiation fix
+  * Topic renaming according to new spec
+  * Try to fix arm64 stuff
+  * * Added launch file
+  * Added Runtime manager entry
+  * * Added Publication of non fused objects
+  * Fixed topic names
+* Contributors: Abraham Monrroy, Kenji Funaoka
+
+1.7.0 (2018-05-16)
+------------------
+* Add  code in cmakelists
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* Remove history of sub-branches
+* Add automatically-generated CHANGELOG.rst
+* [Fix] rename packages (`#1269 <https://github.com/CPFL/Autoware/pull/1269>`_)
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * Rename road_wizard to trafficlight_recognizer
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * removed unnecessary comments
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename road_wizard to trafficlight_recognizer
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * Fix bug for not starting run time manager
+  * Remove invalid dependency
+* Contributors: Kenji Funaoka, Kosuke Murakami
+
+1.6.3 (2018-03-06)
+------------------
+
+1.6.2 (2018-02-27)
+------------------
+
+1.6.1 (2018-01-20)
+------------------
+
+1.6.0 (2017-12-11)
+------------------
+
+1.5.1 (2017-09-25)
+------------------
+
+1.5.0 (2017-09-21)
+------------------
+
+1.4.0 (2017-08-04)
+------------------
+
+1.3.1 (2017-07-16)
+------------------
+
+1.3.0 (2017-07-14)
+------------------
+
+1.2.0 (2017-06-07)
+------------------
+
+1.1.2 (2017-02-27 23:10)
+------------------------
+
+1.1.1 (2017-02-27 22:25)
+------------------------
+
+1.1.0 (2017-02-24)
+------------------
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/CMakeLists.txt b/rubis_ws/deprecated/vision_darknet_detect_opencl/CMakeLists.txt
new file mode 100644
index 00000000..a61abf5d
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/CMakeLists.txt
@@ -0,0 +1,162 @@
+cmake_minimum_required(VERSION 2.8.12)
+project(vision_darknet_detect_opencl)
+
+if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
+  add_definitions(-D__aarch64__)  
+endif()
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_config_msgs
+  autoware_msgs
+  cv_bridge
+  image_transport
+  roscpp
+  sensor_msgs
+  std_msgs
+  rubis_lib
+)
+
+find_package(OpenCV REQUIRED)
+find_package(OpenMP)
+
+catkin_package()
+
+set(CMAKE_CXX_FLAGS "-O3 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+add_compile_definitions(ARM)
+add_compile_definitions(GPU)
+add_compile_definitions(GPU_FAST)
+
+add_library(vision_darknet_detect_opencl_lib SHARED
+  darknet/src/activation_kernels.c
+  darknet/src/avgpool_layer_kernels.c
+  darknet/src/convolutional_kernels.c
+  darknet/src/crop_layer_kernels.c
+  darknet/src/col2im_kernels.c
+  darknet/src/blas_kernels.c
+  darknet/src/deconvolutional_kernels.c
+  darknet/src/dropout_layer_kernels.c
+  darknet/src/im2col_kernels.c
+  darknet/src/maxpool_layer_kernels.c
+  darknet/src/activation_layer.c
+  darknet/src/activations.c
+  darknet/src/avgpool_layer.c
+  darknet/src/batchnorm_layer.c
+  darknet/src/blas.c
+  darknet/src/box.c
+  darknet/src/col2im.c
+  darknet/src/compare.c
+  darknet/src/connected_layer.c
+  darknet/src/convolutional_layer.c
+  darknet/src/cost_layer.c
+  darknet/src/crnn_layer.c
+  darknet/src/crop_layer.c
+  darknet/src/data.c
+  darknet/src/deconvolutional_layer.c
+  darknet/src/demo.c
+  darknet/src/detection_layer.c
+  darknet/src/dropout_layer.c
+  darknet/src/gemm.c
+  darknet/src/gru_layer.c
+  darknet/src/im2col.c
+  darknet/src/image.c
+  darknet/src/iseg_layer.c
+  darknet/src/layer.c
+  darknet/src/list.c
+  darknet/src/local_layer.c
+  darknet/src/lstm_layer.c
+  darknet/src/matrix.c
+  darknet/src/maxpool_layer.c
+  darknet/src/network.c
+  darknet/src/normalization_layer.c
+  darknet/src/option_list.c
+  darknet/src/parser.c
+  darknet/src/region_layer.c
+  darknet/src/reorg_layer.c
+  darknet/src/rnn_layer.c
+  darknet/src/route_layer.c
+  darknet/src/shortcut_layer.c
+  darknet/src/softmax_layer.c
+  darknet/src/system.c
+  darknet/src/tree.c
+  darknet/src/utils.c
+  darknet/src/yolo_layer.c
+  darknet/src/yolo4_layer.c
+  darknet/src/upsample_layer.c
+  darknet/src/logistic_layer.c
+  darknet/src/l2norm_layer.c
+  darknet/src/opencl.c
+)
+
+target_compile_definitions(vision_darknet_detect_opencl_lib PUBLIC -DGPU -DOPENCL)
+
+target_include_directories(vision_darknet_detect_opencl_lib PRIVATE
+  ${OpenCV_INCLUDE_DIR}
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${PROJECT_SOURCE_DIR}/darknet
+  ${PROJECT_SOURCE_DIR}/darknet/src
+  ${PROJECT_SOURCE_DIR}/src
+  ${PROJECT_SOURCE_DIR}/CL
+)
+
+target_link_libraries(vision_darknet_detect_opencl_lib
+  ${OpenCV_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${Qt5Core_LIBRARIES}
+  pthread
+  OpenCL
+)
+
+add_dependencies(vision_darknet_detect_opencl_lib
+  ${catkin_EXPORTED_TARGETS}
+)
+
+#ros node
+add_executable(vision_darknet_detect_opencl
+  src/vision_darknet_detect_opencl_node.cpp
+  src/vision_darknet_detect_opencl.cpp
+  src/vision_darknet_detect_opencl.h
+)
+
+target_compile_definitions(vision_darknet_detect_opencl PUBLIC -DGPU -DOPENCL)
+
+target_include_directories(vision_darknet_detect_opencl PRIVATE
+  ${catkin_INCLUDE_DIRS}
+  ${PROJECT_SOURCE_DIR}/darknet
+  ${PROJECT_SOURCE_DIR}/darknet/src
+  ${PROJECT_SOURCE_DIR}/src
+  ${PROJECT_SOURCE_DIR}/CL
+)
+
+target_link_libraries(vision_darknet_detect_opencl
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}  
+  vision_darknet_detect_opencl_lib
+)
+
+add_dependencies(vision_darknet_detect_opencl
+  ${catkin_EXPORTED_TARGETS}
+)
+install(
+  TARGETS vision_darknet_detect_opencl_lib vision_darknet_detect_opencl
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY darknet/cfg/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/darknet/cfg/
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY darknet/data/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/darknet/data/
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/CMakeLists_origin.txt b/rubis_ws/deprecated/vision_darknet_detect_opencl/CMakeLists_origin.txt
new file mode 100644
index 00000000..54b25057
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/CMakeLists_origin.txt
@@ -0,0 +1,275 @@
+cmake_minimum_required(VERSION 2.8.12)
+project(vision_darknet_detect)
+
+if("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
+  add_definitions(-D__aarch64__)
+endif()
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_config_msgs
+  autoware_msgs
+  cv_bridge
+  image_transport
+  roscpp
+  sensor_msgs
+  std_msgs
+  rubis_lib
+)
+
+find_package(CUDA)
+find_package(OpenCV REQUIRED)
+find_package(OpenMP)
+
+catkin_package()
+
+set(CMAKE_CXX_FLAGS "-O3 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+AW_CHECK_CUDA()
+
+if(USE_CUDA)
+  list(APPEND CUDA_NVCC_FLAGS "--std=c++11 -I$${PROJECT_SOURCE_DIR}/darknet/src -I${PROJECT_SOURCE_DIR}/src -DGPU")
+  SET(CUDA_PROPAGATE_HOST_FLAGS OFF)
+
+  #darknet
+  cuda_add_library(vision_darknet_detect_lib SHARED
+    darknet/src/activation_kernels.cu
+    darknet/src/avgpool_layer_kernels.cu
+    darknet/src/convolutional_kernels.cu
+    darknet/src/crop_layer_kernels.cu
+    darknet/src/col2im_kernels.cu
+    darknet/src/blas_kernels.cu
+    darknet/src/deconvolutional_kernels.cu
+    darknet/src/dropout_layer_kernels.cu
+    darknet/src/im2col_kernels.cu
+    darknet/src/maxpool_layer_kernels.cu
+
+    darknet/src/gemm.c
+    darknet/src/utils.c
+    darknet/src/cuda.c
+    darknet/src/deconvolutional_layer.c
+    darknet/src/convolutional_layer.c
+    darknet/src/list.c
+    darknet/src/image.c
+    darknet/src/activations.c
+    darknet/src/im2col.c
+    darknet/src/col2im.c
+    darknet/src/blas.c
+    darknet/src/crop_layer.c
+    darknet/src/dropout_layer.c
+    darknet/src/maxpool_layer.c
+    darknet/src/softmax_layer.c
+    darknet/src/data.c
+    darknet/src/matrix.c
+    darknet/src/network.c
+    darknet/src/connected_layer.c
+    darknet/src/cost_layer.c
+    darknet/src/parser.c
+    darknet/src/option_list.c
+    darknet/src/detection_layer.c
+    darknet/src/route_layer.c
+    darknet/src/upsample_layer.c
+    darknet/src/box.c
+    darknet/src/normalization_layer.c
+    darknet/src/avgpool_layer.c
+    darknet/src/layer.c
+    darknet/src/local_layer.c
+    darknet/src/shortcut_layer.c
+    darknet/src/logistic_layer.c
+    darknet/src/activation_layer.c
+    darknet/src/rnn_layer.c
+    darknet/src/gru_layer.c
+    darknet/src/crnn_layer.c
+    darknet/src/batchnorm_layer.c
+    darknet/src/region_layer.c
+    darknet/src/reorg_layer.c
+    darknet/src/tree.c
+    darknet/src/lstm_layer.c
+    darknet/src/l2norm_layer.c
+    darknet/src/yolo_layer.c
+  )
+
+  target_compile_definitions(vision_darknet_detect_lib PUBLIC -DGPU)
+  cuda_add_cublas_to_target(vision_darknet_detect_lib)
+
+  if(OPENMP_FOUND)
+    set_target_properties(vision_darknet_detect_lib PROPERTIES
+      COMPILE_FLAGS ${OpenMP_CXX_FLAGS}
+      LINK_FLAGS ${OpenMP_CXX_FLAGS}
+    )
+  endif()
+
+  target_include_directories(vision_darknet_detect_lib PRIVATE
+    ${OpenCV_INCLUDE_DIR}
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+    ${CUDA_INCLUDE_DIRS}
+    ${PROJECT_SOURCE_DIR}/darknet
+    ${PROJECT_SOURCE_DIR}/darknet/src
+    ${PROJECT_SOURCE_DIR}/src
+  )
+
+  target_link_libraries(vision_darknet_detect_lib
+    ${OpenCV_LIBRARIES}
+    ${catkin_LIBRARIES}
+    ${PCL_LIBRARIES}
+    ${Qt5Core_LIBRARIES}
+    ${CUDA_LIBRARIES}
+    ${CUDA_CUBLAS_LIBRARIES}
+    ${CUDA_curand_LIBRARY}
+
+  )
+
+  add_dependencies(vision_darknet_detect_lib
+    ${catkin_EXPORTED_TARGETS}
+  )
+
+  #ros node
+  cuda_add_executable(vision_darknet_detect
+    src/vision_darknet_detect_node.cpp
+    src/vision_darknet_detect.cpp
+    src/vision_darknet_detect.h
+    darknet/src/cuda.h
+    darknet/src/cuda.c
+  )
+
+  target_compile_definitions(vision_darknet_detect PUBLIC -DGPU)
+
+  target_include_directories(vision_darknet_detect PRIVATE
+    ${CUDA_INCLUDE_DIRS}
+    ${catkin_INCLUDE_DIRS}
+    ${PROJECT_SOURCE_DIR}/darknet
+    ${PROJECT_SOURCE_DIR}/darknet/src
+    ${PROJECT_SOURCE_DIR}/src
+  )
+
+  target_link_libraries(vision_darknet_detect
+    ${catkin_LIBRARIES}
+    ${OpenCV_LIBS}
+    ${CUDA_LIBRARIES}
+    ${CUDA_CUBLAS_LIBRARIES}
+    ${CUDA_curand_LIBRARY}
+    vision_darknet_detect_lib
+  )
+  add_dependencies(vision_darknet_detect
+    ${catkin_EXPORTED_TARGETS}
+  )
+  install(
+    TARGETS vision_darknet_detect_lib vision_darknet_detect
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+else()
+  message(WARNING "Building vision_darknet_detect without CUDA")
+
+  add_library(vision_darknet_detect_lib SHARED
+    darknet/src/gemm.c
+    darknet/src/utils.c
+    darknet/src/cuda.c
+    darknet/src/deconvolutional_layer.c
+    darknet/src/convolutional_layer.c
+    darknet/src/list.c
+    darknet/src/image.c
+    darknet/src/activations.c
+    darknet/src/im2col.c
+    darknet/src/col2im.c
+    darknet/src/blas.c
+    darknet/src/crop_layer.c
+    darknet/src/dropout_layer.c
+    darknet/src/maxpool_layer.c
+    darknet/src/softmax_layer.c
+    darknet/src/data.c
+    darknet/src/matrix.c
+    darknet/src/network.c
+    darknet/src/connected_layer.c
+    darknet/src/cost_layer.c
+    darknet/src/parser.c
+    darknet/src/option_list.c
+    darknet/src/detection_layer.c
+    darknet/src/route_layer.c
+    darknet/src/upsample_layer.c
+    darknet/src/box.c
+    darknet/src/normalization_layer.c
+    darknet/src/avgpool_layer.c
+    darknet/src/layer.c
+    darknet/src/local_layer.c
+    darknet/src/shortcut_layer.c
+    darknet/src/logistic_layer.c
+    darknet/src/activation_layer.c
+    darknet/src/rnn_layer.c
+    darknet/src/gru_layer.c
+    darknet/src/crnn_layer.c
+    darknet/src/batchnorm_layer.c
+    darknet/src/region_layer.c
+    darknet/src/reorg_layer.c
+    darknet/src/tree.c
+    darknet/src/lstm_layer.c
+    darknet/src/l2norm_layer.c
+    darknet/src/yolo_layer.c
+  )
+
+  target_include_directories(vision_darknet_detect_lib PRIVATE
+    ${OpenCV_INCLUDE_DIR}
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+    ${PROJECT_SOURCE_DIR}/darknet
+    ${PROJECT_SOURCE_DIR}/darknet/src
+    ${PROJECT_SOURCE_DIR}/src
+  )
+
+  target_link_libraries(vision_darknet_detect_lib
+    ${OpenCV_LIBRARIES}
+    ${catkin_LIBRARIES}
+    ${PCL_LIBRARIES}
+    ${Qt5Core_LIBRARIES}
+    m
+    pthread
+  )
+
+  add_dependencies(vision_darknet_detect_lib
+    ${catkin_EXPORTED_TARGETS}
+  )
+
+  #ros node
+  add_executable(vision_darknet_detect
+    src/vision_darknet_detect_node.cpp
+    src/vision_darknet_detect.cpp
+    src/vision_darknet_detect.h
+    darknet/src/cuda.h
+    darknet/src/cuda.c
+  )
+
+  target_include_directories(vision_darknet_detect PRIVATE
+    ${catkin_INCLUDE_DIRS}
+    ${PROJECT_SOURCE_DIR}/darknet
+    ${PROJECT_SOURCE_DIR}/darknet/src
+    ${PROJECT_SOURCE_DIR}/src
+  )
+
+  target_link_libraries(vision_darknet_detect
+    ${catkin_LIBRARIES}
+    ${OpenCV_LIBS}
+    vision_darknet_detect_lib
+  )
+  add_dependencies(vision_darknet_detect
+    ${catkin_EXPORTED_TARGETS}
+  )
+  install(
+    TARGETS vision_darknet_detect_lib vision_darknet_detect
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+endif()
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY darknet/cfg/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/darknet/cfg/
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/README.md b/rubis_ws/deprecated/vision_darknet_detect_opencl/README.md
new file mode 100644
index 00000000..73bb6f64
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/README.md
@@ -0,0 +1,55 @@
+# Vision Darknet Detect
+
+Autoware package based on Darknet that supports Yolov3 and Yolov2 image detector.
+
+### Requirements
+
+* NVIDIA GPU with CUDA installed
+* Pretrained [YOLOv3](https://pjreddie.com/media/files/yolov3.weights) or
+ [YOLOv2](https://pjreddie.com/media/files/yolov2.weights) model on COCO dataset,
+ Models found on the [YOLO website](https://pjreddie.com/darknet/yolo/).
+* The weights file must be placed in `vision_darknet_detect/darknet/data/`.
+
+### How to launch
+
+* From a sourced terminal:
+
+    - `roslaunch vision_darknet_detect vision_yolo3_detect.launch`
+    - `roslaunch vision_darknet_detect vision_yolo2_detect.launch`
+
+* From Runtime Manager:
+
+Computing Tab -> Detection/ vision_detector -> `vision_darknet_detect`
+You can change the config and weights file, as well as other parameters, by clicking [app]
+
+### Parameters
+
+Launch file available parameters:
+
+|Parameter| Type| Description|
+----------|-----|--------
+|`score_threshold`|*Double* |Detections with a confidence value larger than this value will be displayed. Default `0.5`.|
+|`nms_threshold`|*Double*|Non-Maximum suppresion area threshold ratio to merge proposals. Default `0.45`.|
+|`network_definition_file`|*String*|Network architecture definition configuration file. Default `yolov3.cfg`.|
+|`pretrained_model_file`|*String*|Path to pretrained model. Default `yolov3.weights`.|
+|`camera_id`|*String*|Camera workspace. Default `/`.|
+|`image_src`|*String*|Image source topic. Default `/image_raw`.|
+|`names_file`|*String*|Path to pretrained model. Default `coco.names`.|
+
+
+### Subscribed topics
+
+|Topic|Type|Objective|
+------|----|---------
+|`/image_raw`|`sensor_msgs/Image`|Source image stream to perform detection.|
+|`/config/Yolo3`|`autoware_config_msgs/ConfigSSD`|Configuration adjustment for threshold.|
+
+### Published topics
+
+|Topic|Type|Objective|
+------|----|---------
+|`/detection/vision_objects`|`autoware_msgs::DetectedObjectArray`|Contains the coordinates of the bounding box in image coordinates for detected objects.|
+
+### Video
+
+[![Yolo v3 Autoware](https://img.youtube.com/vi/pO4vM4ehI98/0.jpg)](https://www.youtube.com/watch?v=pO4vM4ehI98)
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/cfg/coco.names b/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/cfg/coco.names
new file mode 100644
index 00000000..ca76c80b
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/cfg/coco.names
@@ -0,0 +1,80 @@
+person
+bicycle
+car
+motorbike
+aeroplane
+bus
+train
+truck
+boat
+traffic light
+fire hydrant
+stop sign
+parking meter
+bench
+bird
+cat
+dog
+horse
+sheep
+cow
+elephant
+bear
+zebra
+giraffe
+backpack
+umbrella
+handbag
+tie
+suitcase
+frisbee
+skis
+snowboard
+sports ball
+kite
+baseball bat
+baseball glove
+skateboard
+surfboard
+tennis racket
+bottle
+wine glass
+cup
+fork
+knife
+spoon
+bowl
+banana
+apple
+sandwich
+orange
+broccoli
+carrot
+hot dog
+pizza
+donut
+cake
+chair
+sofa
+pottedplant
+bed
+diningtable
+toilet
+tvmonitor
+laptop
+mouse
+remote
+keyboard
+cell phone
+microwave
+oven
+toaster
+sink
+refrigerator
+book
+clock
+vase
+scissors
+teddy bear
+hair drier
+toothbrush
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/cfg/voc.names b/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/cfg/voc.names
new file mode 100644
index 00000000..8420ab35
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/cfg/voc.names
@@ -0,0 +1,20 @@
+aeroplane
+bicycle
+bird
+boat
+bottle
+bus
+car
+cat
+chair
+cow
+diningtable
+dog
+horse
+motorbike
+person
+pottedplant
+sheep
+sofa
+train
+tvmonitor
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/data/yolov4-tiny.weights b/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/data/yolov4-tiny.weights
new file mode 100644
index 00000000..27edc5d3
Binary files /dev/null and b/rubis_ws/deprecated/vision_darknet_detect_opencl/darknet/data/yolov4-tiny.weights differ
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/interface.yaml b/rubis_ws/deprecated/vision_darknet_detect_opencl/interface.yaml
new file mode 100644
index 00000000..7a1b3c4c
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/interface.yaml
@@ -0,0 +1,3 @@
+- name: vision_darknet_detect
+  publish: [/detected_objects]
+  subscribe: [/config/Yolo3, /image_raw]
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/launch/vision_darknet_detect_opencl_param.launch b/rubis_ws/deprecated/vision_darknet_detect_opencl/launch/vision_darknet_detect_opencl_param.launch
new file mode 100644
index 00000000..4104366d
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/launch/vision_darknet_detect_opencl_param.launch
@@ -0,0 +1,26 @@
+<launch>
+  <arg name="gpu_device_id" default="0"/>
+  <arg name="score_threshold" default="0.30"/>
+  <arg name="nms_threshold" default="0.30"/>
+
+  <arg name="names_file" default="$(find vision_darknet_detect_opencl)/darknet/cfg/coco.names"/>
+  <arg name="camera_id" default="/"/>
+  <arg name="image_src" default="/image_raw"/>  
+
+  <node pkg="vision_darknet_detect_opencl" name="vision_darknet_detect_opencl" type="vision_darknet_detect_opencl" output="screen">
+    <param name="score_threshold" type="double" value="$(arg score_threshold)"/>
+    <param name="nms_threshold" type="double" value="$(arg nms_threshold)"/>
+    <param name="gpu_device_id" type="int" value="$(arg gpu_device_id)"/>
+    <param name="image_raw_node" type="str" value="$(arg camera_id)$(arg image_src)"/>
+    <param name="names_file" type="str" value="$(arg names_file)"/>
+  </node>
+
+  <node pkg="detected_objects_visualizer" type="visualize_rects" name="yolo4_rects"
+        output="screen">
+    <param name="image_src" value="$(arg image_src)"/>
+    <param name="image_out" value="/image_rects"/>
+    <param name="object_src" value="/detection/image_detector/objects"/> <!-- this is fixed by definition -->
+  </node>
+
+
+</launch>
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/launch/vision_yolo4_tiny_detect.launch b/rubis_ws/deprecated/vision_darknet_detect_opencl/launch/vision_yolo4_tiny_detect.launch
new file mode 100644
index 00000000..2c123582
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/launch/vision_yolo4_tiny_detect.launch
@@ -0,0 +1,30 @@
+<launch>
+  <arg name="gpu_device_id" default="0"/>
+  <arg name="score_threshold" default="0.30"/>
+  <arg name="nms_threshold" default="0.30"/>
+
+  <arg name="network_definition_file" default="$(find vision_darknet_detect_opencl)/darknet/cfg/yolov4-tiny.cfg"/>
+  <arg name="pretrained_model_file" default="$(find vision_darknet_detect_opencl)/darknet/data/yolov4-tiny.weights"/>
+  <arg name="names_file" default="$(find vision_darknet_detect_opencl)/darknet/cfg/coco.names"/>
+  <arg name="camera_id" default="/"/>
+  <arg name="image_src" default="/image_raw"/>  
+
+  <node pkg="vision_darknet_detect_opencl" name="vision_darknet_detect_opencl" type="vision_darknet_detect_opencl" output="screen">
+    <param name="network_definition_file" type="str" value="$(arg network_definition_file)"/>
+    <param name="pretrained_model_file" type="str" value="$(arg pretrained_model_file)"/>
+    <param name="score_threshold" type="double" value="$(arg score_threshold)"/>
+    <param name="nms_threshold" type="double" value="$(arg nms_threshold)"/>
+    <param name="gpu_device_id" type="int" value="$(arg gpu_device_id)"/>
+    <param name="image_raw_node" type="str" value="$(arg camera_id)$(arg image_src)"/>
+    <param name="names_file" type="str" value="$(arg names_file)"/>
+  </node>
+
+  <node pkg="detected_objects_visualizer" type="visualize_rects" name="yolo4_rects"
+        output="screen">
+    <param name="image_src" value="$(arg image_src)"/>
+    <param name="image_out" value="/image_rects"/>
+    <param name="object_src" value="/detection/image_detector/objects"/> <!-- this is fixed by definition -->
+  </node>
+
+
+</launch>
diff --git a/rubis_ws/deprecated/vision_darknet_detect_opencl/package.xml b/rubis_ws/deprecated/vision_darknet_detect_opencl/package.xml
new file mode 100644
index 00000000..8297e953
--- /dev/null
+++ b/rubis_ws/deprecated/vision_darknet_detect_opencl/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>vision_darknet_detect_opencl</name>
+  <version>1.12.0</version>
+  <description>darknet image detector</description>
+  <maintainer email="abrahammonrroy@yahoo.com">Abraham Monrroy</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>autoware_config_msgs</depend>
+  <depend>autoware_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>rubis_lib</depend>
+  <depend>libopencv-dev</depend>
+</package>
diff --git a/rubis_ws/output.txt b/rubis_ws/output.txt
new file mode 100644
index 00000000..5e2472a2
--- /dev/null
+++ b/rubis_ws/output.txt
@@ -0,0 +1,338 @@
+[  0%] Built target geometry_msgs_generate_messages_lisp
+[  0%] Built target std_msgs_generate_messages_lisp
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaTrafficLightInfoList
+[  0%] Built target geometry_msgs_generate_messages_py
+[  0%] Built target diagnostic_msgs_generate_messages_lisp
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaEgoVehicleInfo
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaCollisionEvent
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaTrafficLightInfo
+[  0%] Built target _carla_msgs_generate_messages_check_deps_GetBlueprints
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaLaneInvasionEvent
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaEgoVehicleStatus
+[  0%] Built target std_msgs_generate_messages_py
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaWorldInfo
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaWeatherParameters
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaBoundingBox
+[  0%] Built target diagnostic_msgs_generate_messages_py
+[  0%] Built target geometry_msgs_generate_messages_nodejs
+[  0%] Built target diagnostic_msgs_generate_messages_nodejs
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaWalkerControl
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaEgoVehicleInfoWheel
+[  0%] Built target _carla_msgs_generate_messages_check_deps_DestroyObject
+[  0%] Built target _carla_msgs_generate_messages_check_deps_SpawnObject
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaEgoVehicleControl
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaActorList
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaTrafficLightStatus
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaStatus
+[  0%] Built target diagnostic_msgs_generate_messages_eus
+[  0%] Built target std_msgs_generate_messages_nodejs
+[  0%] Built target geometry_msgs_generate_messages_eus
+[  0%] Built target std_msgs_generate_messages_eus
+[  0%] Built target std_msgs_generate_messages_cpp
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaTrafficLightStatusList
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaControl
+[  0%] Built target geometry_msgs_generate_messages_cpp
+[  0%] Built target _carla_msgs_generate_messages_check_deps_CarlaActorInfo
+[  0%] Built target actionlib_msgs_generate_messages_py
+[  0%] Built target actionlib_msgs_generate_messages_eus
+[  0%] Built target actionlib_msgs_generate_messages_cpp
+[  0%] Built target nav_msgs_generate_messages_lisp
+[  0%] Built target actionlib_msgs_generate_messages_nodejs
+[  0%] Built target _carla_ackermann_msgs_generate_messages_check_deps_EgoVehicleControlCurrent
+[  0%] Built target diagnostic_msgs_generate_messages_cpp
+[  0%] Built target _carla_ackermann_msgs_generate_messages_check_deps_EgoVehicleControlStatus
+[  0%] Built target _carla_ros_scenario_runner_types_generate_messages_check_deps_CarlaScenarioList
+[  0%] Built target actionlib_msgs_generate_messages_lisp
+[  0%] Built target _carla_ros_scenario_runner_types_generate_messages_check_deps_CarlaScenarioRunnerStatus
+[  0%] Built target _carla_ackermann_msgs_generate_messages_check_deps_EgoVehicleControlTarget
+[  0%] Built target _carla_ackermann_msgs_generate_messages_check_deps_EgoVehicleControlInfo
+[  0%] Built target _inertiallabs_msgs_generate_messages_check_deps_sensor_data
+[  0%] Built target nav_msgs_generate_messages_eus
+[  0%] Built target nav_msgs_generate_messages_nodejs
+[  0%] Built target nav_msgs_generate_messages_py
+[  0%] Built target _carla_ros_scenario_runner_types_generate_messages_check_deps_CarlaScenario
+[  0%] Built target _carla_ros_scenario_runner_types_generate_messages_check_deps_ExecuteScenario
+[  0%] Built target _inertiallabs_msgs_generate_messages_check_deps_gnss_data
+[  0%] Built target _carla_ackermann_msgs_generate_messages_check_deps_EgoVehicleControlMaxima
+[  0%] Built target _inertiallabs_msgs_generate_messages_check_deps_gps_data
+[  0%] Built target nav_msgs_generate_messages_cpp
+[  0%] Built target _carla_waypoint_types_generate_messages_check_deps_GetWaypoint
+[  0%] Built target _inertiallabs_msgs_generate_messages_check_deps_ins_data
+[  0%] Built target _carla_waypoint_types_generate_messages_check_deps_CarlaWaypoint
+[  0%] Built target _inertiallabs_msgs_generate_messages_check_deps_marine_data
+[  0%] Built target _carla_waypoint_types_generate_messages_check_deps_GetActorWaypoint
+[  0%] Built target carla_ackermann_control_gencfg
+[  0%] Built target sensor_msgs_generate_messages_lisp
+[  0%] Built target sensor_msgs_generate_messages_cpp
+[  0%] Built target sensor_msgs_generate_messages_eus
+[  0%] Built target roscpp_generate_messages_lisp
+[  0%] Built target _vesc_msgs_generate_messages_check_deps_VescState
+[  0%] Built target _topic_tools_generate_messages_check_deps_MuxSelect
+[  0%] Built target _vesc_msgs_generate_messages_check_deps_VescStateStamped
+[  0%] Built target roscpp_generate_messages_py
+[  0%] Built target _topic_tools_generate_messages_check_deps_DemuxDelete
+[  0%] Built target rosgraph_msgs_generate_messages_cpp
+[  0%] Built target sensor_msgs_generate_messages_py
+[  0%] Built target _topic_tools_generate_messages_check_deps_MuxDelete
+[  0%] Built target _topic_tools_generate_messages_check_deps_MuxList
+[  0%] Built target roscpp_generate_messages_eus
+[  0%] Built target rosgraph_msgs_generate_messages_py
+[  0%] Built target sensor_msgs_generate_messages_nodejs
+[  0%] Built target roscpp_generate_messages_cpp
+[  0%] Built target _rubis_logger_msgs_generate_messages_check_deps_rubis_log_handler
+[  0%] Built target rosgraph_msgs_generate_messages_lisp
+[  0%] Built target _topic_tools_generate_messages_check_deps_DemuxList
+[  0%] Built target rosgraph_msgs_generate_messages_nodejs
+[  0%] Built target roscpp_generate_messages_nodejs
+[  0%] Built target can_data_msgs_generate_messages_cpp
+[  0%] Built target can_data_msgs_generate_messages_lisp
+[  0%] Built target _topic_tools_generate_messages_check_deps_DemuxSelect
+[  0%] Built target rosgraph_msgs_generate_messages_eus
+[  0%] Built target _topic_tools_generate_messages_check_deps_MuxAdd
+[  0%] Built target _topic_tools_generate_messages_check_deps_DemuxAdd
+[  0%] Built target can_msgs_generate_messages_cpp
+[  0%] Built target can_data_msgs_generate_messages_eus
+[  0%] Built target can_msgs_generate_messages_nodejs
+[  0%] Built target can_data_msgs_generate_messages_nodejs
+[  0%] Built target jsk_footstep_msgs_generate_messages_nodejs
+[  0%] Built target jsk_footstep_msgs_generate_messages_cpp
+[  0%] Built target can_msgs_generate_messages_lisp
+[  0%] Built target jsk_footstep_msgs_generate_messages_eus
+[  0%] Built target can_msgs_generate_messages_py
+[  0%] Built target autoware_msgs_generate_messages_py
+[  0%] Built target can_data_msgs_generate_messages_py
+[  0%] Built target jsk_recognition_msgs_generate_messages_eus
+[  0%] Built target pcl_msgs_generate_messages_lisp
+[  0%] Built target autoware_msgs_generate_messages_cpp
+[  0%] Built target can_msgs_generate_messages_eus
+[  0%] Built target jsk_recognition_msgs_generate_messages_nodejs
+[  0%] Built target jsk_recognition_msgs_generate_messages_lisp
+[  0%] Built target jsk_footstep_msgs_generate_messages_lisp
+[  0%] Built target autoware_msgs_generate_messages_lisp
+[  0%] Built target jsk_recognition_msgs_generate_messages_py
+[  0%] Built target autoware_msgs_generate_messages_eus
+[  0%] Built target jsk_recognition_msgs_generate_messages_cpp
+[  0%] Built target autoware_msgs_generate_messages_nodejs
+[  0%] Built target pcl_msgs_generate_messages_cpp
+[  0%] Built target pcl_msgs_generate_messages_eus
+[  0%] Built target visualization_msgs_generate_messages_lisp
+[  0%] Built target jsk_footstep_msgs_generate_messages_py
+[  0%] Built target visualization_msgs_generate_messages_cpp
+[  0%] Built target pcl_msgs_generate_messages_py
+[  0%] Built target visualization_msgs_generate_messages_eus
+[  0%] Built target nmea_msgs_generate_messages_nodejs
+[  0%] Built target rubis_msgs_generate_messages_nodejs
+[  0%] Built target ros_autorunner_lib
+[  0%] Built target visualization_msgs_generate_messages_nodejs
+[  0%] Built target visualization_msgs_generate_messages_py
+[  0%] Built target rubis_msgs_generate_messages_py
+[  0%] Built target rubis_msgs_generate_messages_eus
+[  0%] Built target nmea_msgs_generate_messages_cpp
+[  0%] Built target nmea_msgs_generate_messages_lisp
+[  0%] Built target rubis_msgs_generate_messages_lisp
+[  0%] Built target nmea_msgs_generate_messages_py
+[  0%] Built target rubis_msgs_generate_messages_cpp
+[  0%] Built target nmea_msgs_generate_messages_eus
+[  0%] Built target pcl_msgs_generate_messages_nodejs
+[  0%] Built target tf_generate_messages_nodejs
+[  0%] Built target tf2_msgs_generate_messages_nodejs
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestUInt8
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestTimeArray
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestHeader
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestMultipleRequestFields
+[  0%] Built target tf_generate_messages_cpp
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_SendBytes
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestRequestAndResponse
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestEmpty
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestResponseOnly
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestHeaderArray
+[  0%] Built target tf_generate_messages_eus
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_AddTwoInts
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestDurationArray
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestChar
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestUInt8FixedSizeArray16
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_Num
+[  0%] Built target actionlib_generate_messages_lisp
+[  0%] Built target actionlib_generate_messages_eus
+[  0%] Built target tf_generate_messages_lisp
+[  0%] Built target tf2_msgs_generate_messages_lisp
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestNestedService
+[  0%] Built target actionlib_generate_messages_nodejs
+[  0%] Built target gicp_localizer_generate_messages_cpp
+[  0%] Built target twist_converter
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestMultipleResponseFields
+[  0%] Built target gicp_localizer_generate_messages_eus
+[  0%] Built target tf2_msgs_generate_messages_py
+[  0%] Built target actionlib_generate_messages_py
+[  0%] Built target actionlib_generate_messages_cpp
+[  0%] Built target gicp_localizer_generate_messages_lisp
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestHeaderTwo
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestArrayRequest
+[  0%] Built target _lgsvl_rosbridge_library_generate_messages_check_deps_TestRequestOnly
+[  0%] Built target tf_generate_messages_py
+[  0%] Built target tf2_msgs_generate_messages_eus
+[  0%] Built target camera_image_generate_messages_nodejs
+[  0%] Built target tf2_msgs_generate_messages_cpp
+[  1%] Built target camera_image_generate_messages_eus
+[  1%] Built target camera_image_generate_messages_lisp
+[  1%] Built target gtest
+[  1%] Built target gicp_localizer_generate_messages_py
+[  1%] Built target ackermann_msgs_generate_messages_lisp
+[  1%] Built target ackermann_msgs_generate_messages_nodejs
+[  1%] Built target camera_image_generate_messages_cpp
+[  2%] Built target fast_gicp
+[  2%] Built target ackermann_msgs_generate_messages_eus
+[  2%] Built target camera_image_generate_messages_py
+[  2%] Built target bond_generate_messages_cpp
+[  2%] Built target ackermann_msgs_generate_messages_cpp
+[  2%] Built target ackermann_msgs_generate_messages_py
+[  2%] Built target bond_generate_messages_nodejs
+[  2%] Built target gicp_localizer_generate_messages_nodejs
+[  2%] Built target bond_generate_messages_eus
+[  3%] Built target bond_generate_messages_lisp
+[  4%] Built target image_transport
+[  4%] Built target nodelet_generate_messages_eus
+[  4%] Built target nodelet_generate_messages_lisp
+[  4%] Built target nodelet_generate_messages_py
+[  4%] Built target lane_detector
+[  4%] Built target bond_generate_messages_py
+[  4%] Built target nodelet_generate_messages_cpp
+[  4%] Built target nodelet_generate_messages_nodejs
+[  8%] Built target carla_msgs_generate_messages_nodejs
+[ 12%] Built target carla_ros_scenario_runner_types_generate_messages_eus
+[ 13%] Built target carla_ros_scenario_runner_types_generate_messages_cpp
+[ 14%] Built target carla_msgs_generate_messages_eus
+[ 15%] Built target carla_ros_scenario_runner_types_generate_messages_lisp
+[ 19%] Built target carla_msgs_generate_messages_py
+[ 19%] Built target carla_ros_scenario_runner_types_generate_messages_nodejs
+[ 23%] Built target carla_msgs_generate_messages_cpp
+[ 26%] Built target carla_msgs_generate_messages_lisp
+[ 27%] Built target carla_ros_scenario_runner_types_generate_messages_py
+[ 28%] Built target inertiallabs_msgs_generate_messages_lisp
+[ 29%] Built target inertiallabs_msgs_generate_messages_py
+[ 30%] Built target inertiallabs_msgs_generate_messages_nodejs
+[ 30%] Built target carla_waypoint_types_generate_messages_lisp
+[ 31%] Built target carla_waypoint_types_generate_messages_nodejs
+[ 31%] Built target _polled_camera_generate_messages_check_deps_GetPolledImage
+[ 32%] Built target carla_waypoint_types_generate_messages_py
+[ 33%] Built target rubis_logger_msgs_generate_messages_cpp
+[ 33%] Built target inertiallabs_msgs_generate_messages_eus
+[ 34%] Built target carla_waypoint_types_generate_messages_cpp
+[ 34%] Built target rubis_logger_msgs_generate_messages_py
+[ 36%] Built target rubis_logger_msgs_generate_messages_lisp
+[ 36%] Built target inertiallabs_msgs_generate_messages_cpp
+[ 37%] Built target vesc_msgs_generate_messages_py
+[ 37%] Built target rubis_logger_msgs_generate_messages_eus
+[ 38%] Built target vesc_msgs_generate_messages_lisp
+[ 38%] Built target vesc_msgs_generate_messages_eus
+[ 39%] Built target carla_waypoint_types_generate_messages_eus
+[ 39%] Built target vesc_msgs_generate_messages_nodejs
+[ 41%] Built target topic_tools_generate_messages_lisp
+[ 41%] Built target rubis_logger_msgs_generate_messages_nodejs
+[ 42%] Built target vesc_msgs_generate_messages_cpp
+[ 44%] Built target topic_tools_generate_messages_nodejs
+[ 45%] Built target topic_tools_generate_messages_py
+[ 47%] Built target topic_tools_generate_messages_cpp
+[ 52%] Built target lgsvl_rosbridge_library_generate_messages_py
+[ 52%] Built target camera_calibration_parsers
+[ 53%] Built target rubis_testbed_autorunner
+[ 53%] Built target carla_lkas_autorunner
+[ 53%] Built target controller
+[ 56%] Built target lgsvl_rosbridge_library_generate_messages_eus
+[ 57%] Built target exynos_carla_full_autorunner
+[ 58%] Built target topic_tools_generate_messages_eus
+[ 58%] Built target exynos_carla_lkas_autorunner
+[ 58%] Built target can_translate
+[ 59%] Built target carla_full_autorunner
+[ 59%] Built target control_module
+[ 59%] Built target ins_twist_generator
+[ 63%] Built target lgsvl_rosbridge_library_generate_messages_cpp
+[ 63%] Built target gnss_pose_pub
+[ 64%] Built target il_ins
+[ 65%] Built target gnss_converter
+[ 65%] Built target cubetown_full_autorunner
+[ 66%] Built target cubetown_lkas_autorunner
+[ 69%] Built target lgsvl_rosbridge_library_generate_messages_nodejs
+[ 70%] Built target ins_sync_test
+[ 70%] Built target odom_converter
+[ 71%] Built target gnss_module
+[ 71%] Built target dynamic_ins_twist_generator
+[ 75%] Built target lgsvl_rosbridge_library_generate_messages_lisp
+[ 76%] Built target republish
+[ 76%] Built target image_transport_plugins
+[ 77%] Built target list_transports
+[ 77%] Built target gicp_localizer_generate_messages
+[ 77%] Built target polled_camera_generate_messages_nodejs
+[ 77%] Built target camera_info_manager
+[ 78%] Built target yaw_offset_calculator
+[ 78%] Built target camera_image_generate_messages
+[ 78%] Built target camera_image_sub
+[ 78%] Built target quaternion_to_rpy
+[ 78%] Built target polled_camera_generate_messages_eus
+[ 78%] Automatic MOC for target rviz_carla_plugin
+[ 78%] Built target polled_camera_generate_messages_cpp
+[ 79%] Built target gnss_republisher
+[ 80%] Built target polled_camera_generate_messages_lisp
+[ 81%] Built target camera_republisher
+[ 82%] Built target gnss_localizer
+[ 82%] Built target polled_camera_generate_messages_py
+[ 82%] Built target fake_current_pose
+Scanning dependencies of target svl_sensing
+[ 82%] Built target lidar_republisher
+[ 83%] Built target fake_traffic_signal_generator
+[ 84%] Built target rubis_pose_relay
+[ 84%] Built target camera_image
+[ 84%] Built target ackermann_to_vesc_node
+[ 84%] Built target vesc_to_odom_node
+[ 85%] Built target vesc_driver_node
+[ 87%] Built target carla_ackermann_msgs_generate_messages_eus
+[ 87%] Building CXX object svl_pkg/CMakeFiles/svl_sensing.dir/src/svl_sensing/svl_sensing_node.cpp.o
+[ 88%] Built target carla_ackermann_msgs_generate_messages_lisp
+[ 88%] Built target rviz_carla_plugin_autogen
+[ 88%] Building CXX object svl_pkg/CMakeFiles/svl_sensing.dir/src/svl_sensing/svl_sensing.cpp.o
+[ 89%] Built target vesc_ackermann_nodelet
+[ 90%] Built target carla_waypoint_types_generate_messages
+[ 90%] Built target carla_ackermann_msgs_generate_messages_nodejs
+[ 90%] Built target carla_msgs_generate_messages
+[ 90%] Built target carla_ros_scenario_runner_types_generate_messages
+[ 90%] Built target convert
+[ 90%] Built target rubis_logger_msgs_generate_messages
+[ 91%] Built target vesc_driver_nodelet
+[ 91%] Built target logger_brake_dist
+[ 92%] Built target inertiallabs_msgs_generate_messages
+[ 92%] Built target carla_ackermann_msgs_generate_messages_cpp
+[ 92%] Built target topic_tools
+[ 93%] Built target carla_ackermann_msgs_generate_messages_py
+[ 93%] Built target topic_tools_generate_messages
+[ 93%] Built target lgsvl_rosbridge_library_generate_messages
+[ 93%] Built target polled_camera
+[ 93%] Built target vesc_msgs_generate_messages
+[ 93%] Built target polled_camera_generate_messages
+[ 93%] Built target logger_topic
+[ 93%] Built target camera_calibration_parsers_wrapper
+[ 94%] Built target rviz_carla_plugin
+[ 94%] Built target unit_test
+[ 95%] Built target drop
+[ 96%] Built target mux
+[ 96%] Built target switch_mux
+[ 96%] Built target demux
+[ 96%] Built target carla_ackermann_msgs_generate_messages
+[ 97%] Built target poller
+[ 97%] Built target relay
+[100%] Built target pcl_recorder_node
+[100%] Built target throttle
+[100%] Built target gicp_localizer_node
+svl_pkg/CMakeFiles/svl_sensing.dir/build.make:86: recipe for target 'svl_pkg/CMakeFiles/svl_sensing.dir/src/svl_sensing/svl_sensing.cpp.o' failed
+CMakeFiles/Makefile2:21529: recipe for target 'svl_pkg/CMakeFiles/svl_sensing.dir/all' failed
+Makefile:140: recipe for target 'all' failed
+Base path: /home/hayeonp/git/Autoware_On_Embedded/rubis_ws
+Source space: /home/hayeonp/git/Autoware_On_Embedded/rubis_ws/src
+Build space: /home/hayeonp/git/Autoware_On_Embedded/rubis_ws/build
+Devel space: /home/hayeonp/git/Autoware_On_Embedded/rubis_ws/devel
+Install space: /home/hayeonp/git/Autoware_On_Embedded/rubis_ws/install
+####
+#### Running command: "make cmake_check_build_system" in "/home/hayeonp/git/Autoware_On_Embedded/rubis_ws/build"
+####
+####
+#### Running command: "make -j24 -l24" in "/home/hayeonp/git/Autoware_On_Embedded/rubis_ws/build"
+####
diff --git a/rubis_ws/scripts/carla.launch b/rubis_ws/scripts/carla.launch
new file mode 100755
index 00000000..48b3aae6
--- /dev/null
+++ b/rubis_ws/scripts/carla.launch
@@ -0,0 +1,69 @@
+<!-- -->
+<launch>
+  <arg name='host' default='localhost'/>
+  <arg name='port' default='2000'/>
+  <arg name='town' default='Town04'/>
+  <arg name='role_name' default='ego_vehicle'/>
+  
+  <!-- default -->
+  <!-- <arg name="spawn_point" default="258.5, 208.2, 0.3, 0, 0, 90"/> -->
+  
+  <!-- walker mission coordinate reference point 205.0, 297.0, 0.3, 0, 0, 87 -->
+  <!-- lr du height roll pitch yaw  -->
+  
+  <!-- default -->
+  <!-- <arg name="spawn_point" default="258.5, 208.2, 0.3, 0, 0, 90"/> -->
+  <!-- Straight -->
+  <!-- <arg name="spawn_point" default="258.5, 127.05, 0.3, 0, 0, 90"/> -->
+
+
+  <arg name="objects_definition_file" default='$(find carla_spawn_objects)/config/objects.json'/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/cfg/carla_autorunner/carla_spawn_points.yaml'/>
+
+  <arg name='role_name_obstacle' default='obstacle'/>
+  <!-- new -->
+  <!-- <arg name="spawn_point" default="314.065, 129.676, 0.3, 0, 0, 90"/> -->
+  <!-- <arg name="spawn_point_obstacle" default="324.4, 346.1, 0.3, 0, 0, -32"/> -->
+  <!-- <arg name="spawn_point_obstacle" default="315, 192, 0.3, 0, 0, 90"/> -->
+
+
+  <!-- <arg name="spawn_point_walker" default="212.0, 297.0, 0.3, 0, 0, 180"/> -->
+  <!-- <arg name="spawn_point_walker" default="-10, 214, 5, 0, 0, -90"/>
+  <arg name="walker_point1" default="206.0, 297.0, 0.3, 0, 0, 180"/>
+  <arg name="walker_point2" default="204.0, 297.0, 0.3, 0, 0, 180"/>
+  <arg name="walker_point3" default="198.0, 297.0, 0.3, 0, 0, 180"/>
+  <arg name="walker_target_speed" default="2"/> -->
+  
+  <!-- walker default spawn point 210.0, 297.0, 0.3, 0, 0, 180 -->
+  <!-- walker collision point 206.0 - 204.0, 297.0, 0.3, 0, 0, 180 -->
+
+  <arg name="enable_openplanner_simulation" default="false"/>
+  <arg name="enable_object_insert" default="false"/>
+  <arg name="spawn_sensors_only" default="false"/>
+
+  <include file="$(find carla_autoware_bridge)/launch/carla_autoware_bridge_with_manual_control.launch">
+    <arg name='host' value='$(arg host)'/>
+    <arg name='port' value='$(arg port)'/>
+    <arg name='town' value='$(arg town)'/>
+    <arg name="objects_definition_file" value='$(arg objects_definition_file)'/>
+    <arg name="spawn_points_file" value='$(arg spawn_points_file)'/>
+    <arg name='role_name' value='$(arg role_name)'/>
+    <arg name='role_name_obstacle' value='$(arg role_name_obstacle)'/>
+    <!-- <arg name="spawn_point" value="$(arg spawn_point)"/>
+    <arg name="spawn_point_obstacle" value="$(arg spawn_point_obstacle)"/>     
+    <arg name="spawn_point_walker" value="$(arg spawn_point_walker)"/>
+    <arg name="walker_point1" value="$(arg walker_point1)"/>
+    <arg name="walker_point2" value="$(arg walker_point2)"/>
+    <arg name="walker_point3" value="$(arg walker_point3)"/>
+    <arg name="walker_target_speed" value="$(arg walker_target_speed)"/> -->
+
+    <arg name='spawn_sensors_only' value='$(arg spawn_sensors_only)'/>
+  </include>
+</launch>
+
+
+
+<!-- original -->
+<!-- <arg name="spawn_point" default="-10,214,0.3,0,0,-90"/> -->
+<!-- legacy -->
+<!-- <arg name="spawn_point" default="202.5, 188.0, 0.3, 0, 0, 90"/> -->
\ No newline at end of file
diff --git a/rubis_ws/scripts/carla_keyboard_controller.py b/rubis_ws/scripts/carla_keyboard_controller.py
index 0db701e4..f6b6ff63 100644
--- a/rubis_ws/scripts/carla_keyboard_controller.py
+++ b/rubis_ws/scripts/carla_keyboard_controller.py
@@ -47,6 +47,7 @@ def on_press(key):
     # print('Key %s pressed' % current_pressed)
 
     if keyboard.KeyCode(char='w') in current_pressed:
+        reverse_toggle = 0
         current_thr = THR_MAX
 
     if keyboard.KeyCode(char='s') in current_pressed:
@@ -62,6 +63,10 @@ def on_press(key):
         current_steer = 0
         current_thr = 0
 
+    if keyboard.KeyCode(char='x') in current_pressed:
+        reverse_toggle = 1
+        current_thr = -1 * THR_MAX
+
 def keyboard_routine():
     print('W, A, S, D : Move, F : Stop')
     print('Press P to Quit')
diff --git a/rubis_ws/scripts/carla_test.launch b/rubis_ws/scripts/carla_test.launch
index 24a95b77..48b3aae6 100755
--- a/rubis_ws/scripts/carla_test.launch
+++ b/rubis_ws/scripts/carla_test.launch
@@ -11,18 +11,28 @@
   <!-- walker mission coordinate reference point 205.0, 297.0, 0.3, 0, 0, 87 -->
   <!-- lr du height roll pitch yaw  -->
   
-  <arg name="spawn_point" default="258.5, 208.2, 0.3, 0, 0, 90"/>
+  <!-- default -->
+  <!-- <arg name="spawn_point" default="258.5, 208.2, 0.3, 0, 0, 90"/> -->
+  <!-- Straight -->
+  <!-- <arg name="spawn_point" default="258.5, 127.05, 0.3, 0, 0, 90"/> -->
+
+
+  <arg name="objects_definition_file" default='$(find carla_spawn_objects)/config/objects.json'/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/cfg/carla_autorunner/carla_spawn_points.yaml'/>
+
   <arg name='role_name_obstacle' default='obstacle'/>
+  <!-- new -->
+  <!-- <arg name="spawn_point" default="314.065, 129.676, 0.3, 0, 0, 90"/> -->
   <!-- <arg name="spawn_point_obstacle" default="324.4, 346.1, 0.3, 0, 0, -32"/> -->
-  <arg name="spawn_point_obstacle" default="-10, 214, 0.3, 0, 0, -90"/>
+  <!-- <arg name="spawn_point_obstacle" default="315, 192, 0.3, 0, 0, 90"/> -->
 
 
   <!-- <arg name="spawn_point_walker" default="212.0, 297.0, 0.3, 0, 0, 180"/> -->
-  <arg name="spawn_point_walker" default="-10, 214, 5, 0, 0, -90"/>
+  <!-- <arg name="spawn_point_walker" default="-10, 214, 5, 0, 0, -90"/>
   <arg name="walker_point1" default="206.0, 297.0, 0.3, 0, 0, 180"/>
   <arg name="walker_point2" default="204.0, 297.0, 0.3, 0, 0, 180"/>
   <arg name="walker_point3" default="198.0, 297.0, 0.3, 0, 0, 180"/>
-  <arg name="walker_target_speed" default="2"/>
+  <arg name="walker_target_speed" default="2"/> -->
   
   <!-- walker default spawn point 210.0, 297.0, 0.3, 0, 0, 180 -->
   <!-- walker collision point 206.0 - 204.0, 297.0, 0.3, 0, 0, 180 -->
@@ -35,15 +45,17 @@
     <arg name='host' value='$(arg host)'/>
     <arg name='port' value='$(arg port)'/>
     <arg name='town' value='$(arg town)'/>
+    <arg name="objects_definition_file" value='$(arg objects_definition_file)'/>
+    <arg name="spawn_points_file" value='$(arg spawn_points_file)'/>
     <arg name='role_name' value='$(arg role_name)'/>
-    <arg name="spawn_point" value="$(arg spawn_point)"/>
     <arg name='role_name_obstacle' value='$(arg role_name_obstacle)'/>
+    <!-- <arg name="spawn_point" value="$(arg spawn_point)"/>
     <arg name="spawn_point_obstacle" value="$(arg spawn_point_obstacle)"/>     
     <arg name="spawn_point_walker" value="$(arg spawn_point_walker)"/>
     <arg name="walker_point1" value="$(arg walker_point1)"/>
     <arg name="walker_point2" value="$(arg walker_point2)"/>
     <arg name="walker_point3" value="$(arg walker_point3)"/>
-    <arg name="walker_target_speed" value="$(arg walker_target_speed)"/>
+    <arg name="walker_target_speed" value="$(arg walker_target_speed)"/> -->
 
     <arg name='spawn_sensors_only' value='$(arg spawn_sensors_only)'/>
   </include>
diff --git a/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch b/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch
index 273985d1..671bde01 100644
--- a/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch
+++ b/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch
@@ -1,5 +1,5 @@
 <launch>
     <rosparam command="load" file="$(find can_translate)/cfg/can_translate.yaml" />
-    <node pkg="can_translate" type="can_translate" name="can_translate" clear_params="true" output="screen" />
+    <node pkg="can_translate" type="can_translate" name="can_translate" clear_params="true" />
     
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/camera_image/CMakeLists.txt b/rubis_ws/src/camera_image/CMakeLists.txt
index a3579174..5334c1b8 100644
--- a/rubis_ws/src/camera_image/CMakeLists.txt
+++ b/rubis_ws/src/camera_image/CMakeLists.txt
@@ -4,40 +4,12 @@ project(camera_image)
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED)
 
 # add by Park
 find_package(OpenCV REQUIRED)
 
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   message_generation
@@ -45,66 +17,12 @@ find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   rubis_lib
 )
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
 
 ## Generate added messages and services with any dependencies listed here
 generate_messages(DEPENDENCIES
   std_msgs
 )
 
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   CATKIN_DEPENDS
   message_runtime
@@ -114,12 +32,6 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
 include_directories(
   include ${catkin_INCLUDE_DIRS}
   #add by Park
@@ -128,84 +40,6 @@ include_directories(
 # ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/camera_image.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/camera_image_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_camera_image.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
 
 add_executable(camera_image
   src/camera_image.cpp
@@ -225,4 +59,22 @@ target_link_libraries(camera_image_sub
   ${catkin_LIBRARIES}
   #add by Park
   ${OpenCV_LIBS}
-)
\ No newline at end of file
+)
+
+install(
+  TARGETS
+    camera_image
+    camera_image_sub
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+# install(DIRECTORY cfg/
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+# )
diff --git a/rubis_ws/src/camera_image/package.xml b/rubis_ws/src/camera_image/package.xml
index 261f6363..d80d314d 100644
--- a/rubis_ws/src/camera_image/package.xml
+++ b/rubis_ws/src/camera_image/package.xml
@@ -4,62 +4,21 @@
   <version>0.0.0</version>
   <description>The camera_image package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="kite9240@todo.todo">kite9240</maintainer>
 
+  <license>Apache 2</license>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
 
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/camera_image</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
 
 
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
 <build_depend>message_generation</build_depend>
 <build_depend>rubis_lib</build_depend>
 <build_export_depend>message_generation</build_export_depend>
 <build_export_depend>rubis_lib</build_export_depend>
 <exec_depend>message_runtime</exec_depend>
 <exec_depend>rubis_lib</exec_depend>
+<depend>libopencv-dev</depend>
+<depend>image_transport</depend>
+<depend>cv_bridge</depend>
 </package>
\ No newline at end of file
diff --git a/rubis_ws/src/camera_image/src/camera_image.cpp b/rubis_ws/src/camera_image/src/camera_image.cpp
index 0c747228..6031a09c 100644
--- a/rubis_ws/src/camera_image/src/camera_image.cpp
+++ b/rubis_ws/src/camera_image/src/camera_image.cpp
@@ -11,8 +11,6 @@ static const std::string OPENCV_WINDOW = "Raw Image Window";
 
 static int camera_id = 0;
 static int frequency = 0;
-static int task_scheduling_flag = 0;
-static int task_profiling_flag = 0;
 static std::string task_response_time_filename;
 // static int rate = 0; // Frequency replaces rate
 static double task_minimum_inter_release_time = 0;
@@ -65,15 +63,13 @@ int main(int argc, char** argv){
 
     pnh.param<int>("/camera_image/camera_id", camera_id, 0);
     pnh.param<int>("/camera_image/frequency", frequency, 10);
-    pnh.param<int>("/camera_image/task_scheduling_flag", task_scheduling_flag, 0);
-    pnh.param<int>("/camera_image/task_profiling_flag", task_profiling_flag, 0);
     pnh.param<std::string>("/camera_image/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/camera_image.csv");
     // pnh.param<int>("/camera_image/rate", rate, 10); // Frequency replaces rate
     pnh.param("/camera_image/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000);
     pnh.param("/camera_image/task_execution_time", task_execution_time, (double)100000000);
     pnh.param("/camera_image/task_relative_deadline", task_relative_deadline, (double)100000000);
     
-    if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
+    rubis::init_task_profiling(task_response_time_filename);
 
     ROS_INFO("camera_id : %d / frequency : %d",camera_id, frequency);
     if(!frequency){
@@ -104,12 +100,7 @@ void CameraImage::sendImage(){
         
     while(nh_.ok()){
 
-        if(task_profiling_flag) rubis::sched::start_task_profiling();   
-
-        if(rubis::sched::task_state_ == TASK_STATE_READY){            
-            if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline);
-            rubis::sched::task_state_ = TASK_STATE_RUNNING;
-        }
+        rubis::start_task_profiling();   
 
         cap >> frame;
         if(!frame.empty()){
@@ -118,17 +109,12 @@ void CameraImage::sendImage(){
             msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
             msg->header.frame_id="camera";
             camera_image_pub_.publish(msg);
-            rubis::sched::is_task_ready_ = TASK_STATE_DONE;            
         }                        
         // int ckey = cv::waitKey(1);
         // if(ckey == 27)break;
 
-        if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_);
+        rubis::stop_task_profiling(0, 0, 0);
 
-        if(rubis::sched::task_state_ == TASK_STATE_DONE){            
-            if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-            rubis::sched::task_state_ = TASK_STATE_READY;
-        }
         loop_rate.sleep();
     }
 }
diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge.launch
index 499d8a56..3454f53c 100755
--- a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge.launch
+++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge.launch
@@ -7,17 +7,18 @@
   <!-- <arg name="sensor_definition_file" default='$(find carla_autoware_bridge)/config/sensors.json'/> -->
   <arg name='role_name' default='ego_vehicle'/>
   <!-- use comma separated format "x,y,z,roll,pitch,yaw" -->
-  <arg name='spawn_point' default=''/>
   <arg name="spawn_sensors_only" default="false"/>
-
   <arg name='role_name_obstacle' default='obstacle'/>
-  <arg name="spawn_point_obstacle" default=""/>
+  <arg name="objects_definition_file" default='$(find carla_spawn_objects)/config/objects.json'/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/cfg/carla_autorunner/carla_spawn_points.yaml'/>
 
+  <!-- <arg name='spawn_point' default=''/>
+  <arg name="spawn_point_obstacle" default=""/>
   <arg name="spawn_point_walker" default=""/>
   <arg name="walker_point1" default=""/>
   <arg name="walker_point2" default=""/>
   <arg name="walker_point3" default=""/>
-  <arg name="walker_target_speed" default=""/>
+  <arg name="walker_target_speed" default=""/> -->
 
   <!--
     #################
@@ -34,21 +35,23 @@
 
   <!-- ++for 0.9.11++ the ego vehicle, that will be controlled by an agent (e.g. carla_ad_agent) -->
   <include file="$(find carla_spawn_objects)/launch/carla_example_ego_vehicle.launch">
-    <arg name="objects_definition_file" value='$(find carla_spawn_objects)/config/objects.json'/>
+    <arg name="objects_definition_file" value='$(arg objects_definition_file)'/>
+    <arg name="spawn_points_file" value='$(arg spawn_points_file)'/>
     <arg name='role_name' value='$(arg role_name)'/>
-    <arg name="spawn_point_ego_vehicle" value="$(arg spawn_point)"/>
     <arg name='role_name_obstacle' value='$(arg role_name_obstacle)'/>
+    <!-- <arg name="spawn_point_ego_vehicle" value="$(arg spawn_point)"/>
     <arg name='spawn_point_obstacle' value='$(arg spawn_point_obstacle)'/>
     <arg name="spawn_point_walker" value="$(arg spawn_point_walker)"/>    
-    <arg name="spawn_sensors_only" value="$(arg spawn_sensors_only)"/>
+    <arg name="spawn_sensors_only" value="$(arg spawn_sensors_only)"/> -->
   </include>
 
   <include file="$(find carla_walker_agent)/launch/carla_walker_agent.launch">
     <arg name="role_name" value="walker"/>
-    <arg name="walker_point1" value="$(arg walker_point1)"/>
+    <arg name="spawn_points_file" value='$(arg spawn_points_file)'/>
+    <!-- <arg name="walker_point1" value="$(arg walker_point1)"/>
     <arg name="walker_point2" value="$(arg walker_point2)"/>
     <arg name="walker_point3" value="$(arg walker_point3)"/>
-    <arg name="walker_target_speed" value="$(arg walker_target_speed)"/>
+    <arg name="walker_target_speed" value="$(arg walker_target_speed)"/> -->
   </include>
 
   <!-- mission planning -->
diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch
index 6a926615..7ea93364 100755
--- a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch
+++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch
@@ -28,14 +28,14 @@
     remap carla lidar to autoware.
     @todo: to reduce load, Autoware should directly use the Carla-topic.
   -->
-  <node pkg="topic_tools" type="relay" name="points_relay" args="/carla/$(arg role_name)/lidar /points_raw"/>
+  <node pkg="topic_tools" type="relay" name="points_relay" args="/carla/$(arg role_name)/lidar /points_raw_origin"/>
 
   <!-- 
     # CAMERA #
     remap carla front camera to autoware
     @todo: to reduce load, Autoware should directly use the Carla-topic.
   -->
-  <node pkg="topic_tools" type="relay" name="imag_relay" args="/carla/$(arg role_name)/rgb_front/image /image_raw"/>
+  <node pkg="topic_tools" type="relay" name="imag_relay" args="/carla/$(arg role_name)/rgb_front/image /image_raw_origin"/>
 
   <!-- 
     # GNSS #
@@ -45,10 +45,10 @@
   <node pkg="carla_autoware_bridge" type="odometry_to_posestamped" name="odometry_to_posestamped"/>
 
   <!-- convert the vehicle status from carla to autoware -->
-  <node pkg="carla_autoware_bridge" type="carla_to_autoware_vehicle_status" name="carla_to_autoware_vehicle_status" output="screen"/>
+  <node pkg="carla_autoware_bridge" type="carla_to_autoware_vehicle_status" name="carla_to_autoware_vehicle_status"/>
 
   <!-- convert waypoint path to autoware lane_array -->
-  <node pkg="carla_autoware_bridge" type="carla_to_autoware_waypoints" name="carla_to_autoware_waypoints" output="screen"/>
+  <node pkg="carla_autoware_bridge" type="carla_to_autoware_waypoints" name="carla_to_autoware_waypoints"/>
 
   <!-- 
     # ACTUATOR #
@@ -56,7 +56,7 @@
   -->
 
   <!-- convert twist to carla ackermann drive-->
-  <node pkg="carla_autoware_bridge" type="vehiclecmd_to_ackermanndrive" name="vehiclecmd_to_ackermanndrive" output="screen">
+  <node pkg="carla_autoware_bridge" type="vehiclecmd_to_ackermanndrive" name="vehiclecmd_to_ackermanndrive">
     <param name="wheelbase" value="$(arg wheelbase)"/> 
   </node>
 
diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_with_manual_control.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_with_manual_control.launch
index c6870425..2bfca10d 100755
--- a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_with_manual_control.launch
+++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_with_manual_control.launch
@@ -5,16 +5,16 @@
   <arg name='town' default=''/>
   <arg name='role_name' default='ego_vehicle'/>
   <!-- use comma separated format "x,y,z,roll,pitch,yaw" -->
-  <arg name='spawn_point' default=''/>
-
-
   <arg name='role_name_obstacle' default='obstacle'/>
+  <arg name="objects_definition_file" default='$(find carla_spawn_objects)/config/objects.json'/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/config/carla_spawn_points.yaml'/>
+  <!-- <arg name='spawn_point' default=''/>
   <arg name="spawn_point_obstacle" default=""/>
   <arg name="spawn_point_walker" default=""/>
   <arg name="walker_point1" default=""/>
   <arg name="walker_point2" default=""/>
   <arg name="walker_point3" default=""/>
-  <arg name="walker_target_speed" default=""/>
+  <arg name="walker_target_speed" default=""/> -->
 
   <arg name="spawn_sensors_only" default="false"/>
 
@@ -29,14 +29,16 @@
     <arg name='port' value='$(arg port)'/>
     <arg name='town' value='$(arg town)'/>
     <arg name='role_name' value='$(arg role_name)'/>
-    <arg name='spawn_point' value='$(arg spawn_point)'/>
     <arg name='role_name_obstacle' value='$(arg role_name_obstacle)'/>
+    <arg name="objects_definition_file" value='$(arg objects_definition_file)'/>
+    <arg name="spawn_points_file" value='$(arg spawn_points_file)'/>
+    <!-- <arg name='spawn_point' value='$(arg spawn_point)'/>
     <arg name='spawn_point_obstacle' value='$(arg spawn_point_obstacle)'/>
     <arg name="spawn_point_walker" value="$(arg spawn_point_walker)"/>
     <arg name="walker_point1" value="$(arg walker_point1)"/>
     <arg name="walker_point2" value="$(arg walker_point2)"/>
     <arg name="walker_point3" value="$(arg walker_point3)"/>
-    <arg name="walker_target_speed" value="$(arg walker_target_speed)"/>
+    <arg name="walker_target_speed" value="$(arg walker_target_speed)"/> -->
 
     <arg name='spawn_sensors_only' value='$(arg spawn_sensors_only)'/>
   </include>
diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_town04.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_town04.launch
new file mode 100755
index 00000000..f3392529
--- /dev/null
+++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_town04.launch
@@ -0,0 +1,28 @@
+<!-- -->
+<launch>
+  <arg name='host' default='localhost'/>
+  <arg name='port' default='2000'/>
+  <arg name='town' default='Town04'/>
+  <arg name='role_name' default='ego_vehicle'/>
+  
+  <arg name="objects_definition_file" default='$(find carla_spawn_objects)/config/objects.json'/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/cfg/carla_autorunner/carla_spawn_points.yaml'/>
+
+  <arg name='role_name_obstacle' default='obstacle'/>
+
+  <arg name="enable_openplanner_simulation" default="false"/>
+  <arg name="enable_object_insert" default="false"/>
+  <arg name="spawn_sensors_only" default="false"/>
+
+  <include file="$(find carla_autoware_bridge)/launch/carla_autoware_bridge_with_manual_control.launch">
+    <arg name='host' value='$(arg host)'/>
+    <arg name='port' value='$(arg port)'/>
+    <arg name='town' value='$(arg town)'/>
+    <arg name="objects_definition_file" value='$(arg objects_definition_file)'/>
+    <arg name="spawn_points_file" value='$(arg spawn_points_file)'/>
+    <arg name='role_name' value='$(arg role_name)'/>
+    <arg name='role_name_obstacle' value='$(arg role_name_obstacle)'/>
+
+    <arg name='spawn_sensors_only' value='$(arg spawn_sensors_only)'/>
+  </include>
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/carla_autoware_bridge/requirements.txt b/rubis_ws/src/carla_autoware_bridge/requirements.txt
index 47d4dae7..06dd1b8c 100644
--- a/rubis_ws/src/carla_autoware_bridge/requirements.txt
+++ b/rubis_ws/src/carla_autoware_bridge/requirements.txt
@@ -9,4 +9,7 @@ PyYAML
 rospkg
 transforms3d
 opencv-python==4.2.0.32
-simple-pid
\ No newline at end of file
+simple-pid
+ipywidgets==7.5.1
+widgetsnbextension==3.5.1
+
diff --git a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive
index 3685e92d..69b53989 100755
--- a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive
+++ b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive
@@ -34,9 +34,10 @@ def callback(data):
     """
     global wheelbase
     msg = AckermannDrive()
-    msg.speed = data.twist_cmd.twist.linear.x
+    msg.speed = data.ctrl_cmd.linear_velocity
     msg.steering_angle = convert_trans_rot_vel_to_steering_angle(
         msg.speed, data.twist_cmd.twist.angular.z, wheelbase)
+    msg.acceleration = data.ctrl_cmd.linear_acceleration
     pub.publish(msg)
 
 def twist_to_ackermanndrive():
diff --git a/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch b/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch
index bb22dcb3..0cbd18ab 100755
--- a/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch
+++ b/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch
@@ -4,6 +4,6 @@
 
   <param name="/carla/maps_path" value="$(arg maps_path)" />
 
-  <node pkg="carla_points_map_loader" type="carla_points_map_loader" name="carla_points_map_loader" output="screen"/>
+  <node pkg="carla_points_map_loader" type="carla_points_map_loader" name="carla_points_map_loader"/>
 </launch>
 
diff --git a/rubis_ws/src/control_module/CMakeLists.txt b/rubis_ws/src/control_module/CMakeLists.txt
index c2f80b33..6304340e 100644
--- a/rubis_ws/src/control_module/CMakeLists.txt
+++ b/rubis_ws/src/control_module/CMakeLists.txt
@@ -4,9 +4,6 @@ project(control_module)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   tf
@@ -15,97 +12,6 @@ find_package(catkin REQUIRED COMPONENTS
   visualization_msgs  
 )
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag forg each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   isFound.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# 	DEPENDENCIES
-# 	std_msgs  # Or other packages containing msgs
-# 	geometry_msgs
-#   visualization_msgs
-#   lane_following
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES contorl_module
@@ -113,45 +19,12 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
 include_directories(
 # include
   ${PROJECT_SOURCE_DIR}/include/control_module
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/contorl_module.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-
 add_executable(twist_converter src/twist_converter.cpp)
 target_link_libraries(twist_converter
    ${catkin_LIBRARIES}
@@ -168,50 +41,21 @@ target_link_libraries(control_module
 # add_dependencies(control_module ${catkin_EXPORTED_TARGETS} ${${my_package}_EXPORTED_TARGETS})
 add_dependencies(control_module ${${my_package}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
 
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
+install(
+  TARGETS
+    twist_converter
+    control_module
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_contorl_module.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
 
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
\ No newline at end of file
diff --git a/rubis_ws/src/control_module/package.xml b/rubis_ws/src/control_module/package.xml
index ba629703..10e9c40c 100644
--- a/rubis_ws/src/control_module/package.xml
+++ b/rubis_ws/src/control_module/package.xml
@@ -4,50 +4,10 @@
   <version>0.0.0</version>
   <description>The control_module package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="kite9240@gmail.com">nvidia</maintainer>
 
+  <license>Apache 2</license>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/contorl_module</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>tf</build_depend>
@@ -65,9 +25,5 @@
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
 
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
 
-  </export>
 </package>
diff --git a/rubis_ws/src/controller/CMakeLists.txt b/rubis_ws/src/controller/CMakeLists.txt
index 2687612f..2e90e9b2 100644
--- a/rubis_ws/src/controller/CMakeLists.txt
+++ b/rubis_ws/src/controller/CMakeLists.txt
@@ -10,36 +10,8 @@ set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED)
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   autoware_msgs
@@ -48,69 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   nav_msgs
 )
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
 
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES controller
@@ -118,103 +28,12 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
 include_directories(
   include ${catkin_INCLUDE_DIRS}
 # include
 # ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/controller.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/controller_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_controller.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
 
 add_executable(controller
   include/controller/controller.h
@@ -223,4 +42,25 @@ add_executable(controller
 add_dependencies(controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(controller
   ${catkin_LIBRARIES}
+)
+
+install(
+  TARGETS
+    controller
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY cfg/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
 )
\ No newline at end of file
diff --git a/rubis_ws/src/controller/package.xml b/rubis_ws/src/controller/package.xml
index 96c9d1e9..9f22cafe 100644
--- a/rubis_ws/src/controller/package.xml
+++ b/rubis_ws/src/controller/package.xml
@@ -3,59 +3,14 @@
   <name>controller</name>
   <version>0.0.0</version>
   <description>The controller package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="hypark@todo.todo">hypark</maintainer>
 
+  <license>Apache 2</license>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/controller</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <depend>autoware_msgs</depend>
   <depend>geometry_msgs</depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
+  <depend>nav_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>can_data_msgs</depend>
 </package>
diff --git a/rubis_ws/src/gicp_localizer/launch/vgicp.launch b/rubis_ws/src/gicp_localizer/launch/vgicp.launch
index af122572..7334a2df 100644
--- a/rubis_ws/src/gicp_localizer/launch/vgicp.launch
+++ b/rubis_ws/src/gicp_localizer/launch/vgicp.launch
@@ -23,7 +23,7 @@
   <arg name="numthreads" default="4" doc="vgicp threads num" />
   <arg name="leafsize" default="0.05" doc="point map downsample leafsize" />
 
-  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node" output="screen">
+  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node">
     <remap from="points_raw" to="$(arg input_sensor_points_topic)" />
     <remap from="pointcloud_map" to="$(arg input_map_points_topic)" />
     <remap from="gicp_pose" to="$(arg output_pose_topic)" />
diff --git a/rubis_ws/deprecated/gnss_converter/CMakeLists.txt b/rubis_ws/src/gnss_converter/CMakeLists.txt
similarity index 62%
rename from rubis_ws/deprecated/gnss_converter/CMakeLists.txt
rename to rubis_ws/src/gnss_converter/CMakeLists.txt
index 263ec354..8c7a4f57 100755
--- a/rubis_ws/deprecated/gnss_converter/CMakeLists.txt
+++ b/rubis_ws/src/gnss_converter/CMakeLists.txt
@@ -10,9 +10,6 @@ set(CMAKE_CXX_FLAGS_RELEASE "-Ofast")
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
@@ -22,8 +19,6 @@ find_package(catkin REQUIRED COMPONENTS
   tf
 )
 
-find_package(OpenCV REQUIRED)
-
 find_package(Eigen3 REQUIRED)
 set(EIGEN_PACKAGE EIGEN3)
 if(NOT EIGEN3_FOUND)
@@ -34,8 +29,7 @@ if(NOT EIGEN3_FOUND)
   set(EIGEN_PACKAGE Eigen)
 endif()
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(OpenCV REQUIRED)
 
 catkin_package(
 #  INCLUDE_DIRS include
@@ -44,19 +38,13 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
-  ${EIGEN3_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
 )
 
+
 add_executable(gnss_converter
   include/gnss_converter/gnss_converter.h
   include/gnss_converter/LLH2UTM.h
@@ -86,26 +74,24 @@ target_link_libraries(gnss_pose_pub
   ${OpenCV_LIBS}
 )
 
-add_executable(ntrip_client
-  src/ntrip_client.cpp
-)
-add_dependencies(ntrip_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(ntrip_client
-  ${catkin_LIBRARIES}
-  ${EIGEN3_LIBRARIES}
-  ${OpenCV_LIBS}
+install(
+  TARGETS
+    gnss_converter
+    gnss_pose_pub
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-add_executable(position_pub
-  include/gnss_converter/LLH2UTM.h
-  include/gnss_converter/quaternion_euler.h
-  src/position_pub.cpp
-  src/LLH2UTM.cpp
-  src/quaternion_euler.cpp
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
 )
-add_dependencies(position_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(position_pub
-  ${catkin_LIBRARIES}
-  ${EIGEN3_LIBRARIES}
-  ${OpenCV_LIBS}
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY cfg/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
 )
diff --git a/rubis_ws/deprecated/gnss_converter/README.md b/rubis_ws/src/gnss_converter/README.md
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/README.md
rename to rubis_ws/src/gnss_converter/README.md
diff --git a/rubis_ws/deprecated/gnss_converter/cfg/gnss_converter.yaml b/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/cfg/gnss_converter.yaml
rename to rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml
diff --git a/rubis_ws/deprecated/gnss_converter/include/gnss_converter/LLH2UTM.h b/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/include/gnss_converter/LLH2UTM.h
rename to rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h
diff --git a/rubis_ws/deprecated/gnss_converter/include/gnss_converter/gnss_converter.h b/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/include/gnss_converter/gnss_converter.h
rename to rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h
diff --git a/rubis_ws/deprecated/gnss_converter/include/gnss_converter/quaternion_euler.h b/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/include/gnss_converter/quaternion_euler.h
rename to rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h
diff --git a/rubis_ws/deprecated/gnss_converter/launch/gnss_converter.launch b/rubis_ws/src/gnss_converter/launch/gnss_converter.launch
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/launch/gnss_converter.launch
rename to rubis_ws/src/gnss_converter/launch/gnss_converter.launch
diff --git a/rubis_ws/deprecated/gnss_converter/launch/gnss_pose_pub.launch b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch
similarity index 82%
rename from rubis_ws/deprecated/gnss_converter/launch/gnss_pose_pub.launch
rename to rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch
index 6a3a3d5b..8cdf1375 100644
--- a/rubis_ws/deprecated/gnss_converter/launch/gnss_pose_pub.launch
+++ b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch
@@ -5,10 +5,8 @@
     
     <arg name="x_offset" default="0.0" />
     <arg name="y_offset" default="0.0" />
-    <arg name="z_offset" default="0.0" />
     <node pkg="gnss_converter" type="gnss_pose_pub" name="gnss_pose_pub" output="screen">
         <param name="x_offset" value="$(arg x_offset)"/>
         <param name="y_offset" value="$(arg y_offset)"/>
-        <param name="z_offset" value="$(arg z_offset)"/>
     </node>
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/gnss_converter/package.xml b/rubis_ws/src/gnss_converter/package.xml
new file mode 100644
index 00000000..51cf2b79
--- /dev/null
+++ b/rubis_ws/src/gnss_converter/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gnss_converter</name>
+  <version>0.0.0</version>
+  <description>The gnss_converter package</description>
+
+  <maintainer email="sunhokim@todo.todo">sunhokim</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <depend>inertiallabs_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>tf</depend>
+  <depend>eigen</depend>
+  <depend>libopencv-dev</depend>
+</package>
diff --git a/rubis_ws/deprecated/gnss_converter/src/LLH2UTM.cpp b/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/src/LLH2UTM.cpp
rename to rubis_ws/src/gnss_converter/src/LLH2UTM.cpp
diff --git a/rubis_ws/deprecated/gnss_converter/src/gnss_converter.cpp b/rubis_ws/src/gnss_converter/src/gnss_converter.cpp
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/src/gnss_converter.cpp
rename to rubis_ws/src/gnss_converter/src/gnss_converter.cpp
diff --git a/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp
new file mode 100644
index 00000000..ad248edd
--- /dev/null
+++ b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp
@@ -0,0 +1,91 @@
+#include <gnss_converter/LLH2UTM.h>
+#include <gnss_converter/quaternion_euler.h>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <inertiallabs_msgs/gps_data.h>
+#include <inertiallabs_msgs/ins_data.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+#define M_PI 3.14159265358979323846
+
+using namespace std;
+using namespace message_filters;
+
+typedef sync_policies::ExactTime<inertiallabs_msgs::gps_data, inertiallabs_msgs::ins_data> SyncPolicy_;
+
+static ros::Publisher gnss_pose_pub_;
+static double x_offset_, y_offset_, yaw_offset_;
+static double roll_, pitch_, yaw_;
+static geometry_msgs::PoseStamped gnss_pose_;
+
+void gnss_pose_pub_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins){
+    gnss_pose_.header = msg_gps->header;
+    gnss_pose_.header.frame_id = "/map";
+
+    LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, gnss_pose_);
+
+    gnss_pose_.pose.position.x = gnss_pose_.pose.position.x - x_offset_;
+    gnss_pose_.pose.position.y = gnss_pose_.pose.position.y - y_offset_;
+
+    roll_ = msg_ins->YPR.z;
+    pitch_ = msg_ins->YPR.y;
+    
+    yaw_ = msg_ins->YPR.x;
+    yaw_ *= -1;
+    yaw_ -= yaw_offset_;
+    if(yaw_ > 180.0) yaw_ -= 360.0;
+    if(yaw_ < -180.0) yaw_ += 360.0;
+
+    roll_ = roll_ * M_PI/180.0;
+    pitch_ = pitch_ * M_PI/180.0;
+    yaw_ = yaw_ * M_PI/180.0;
+}
+
+int main(int argc, char *argv[]){
+    ros::init(argc, argv, "gnss_pose_pub");
+
+    ros::NodeHandle nh;
+
+    nh.param("/gnss_pose_pub/x_offset", x_offset_, 0.0);
+    nh.param("/gnss_pose_pub/y_offset", y_offset_, 0.0);
+    nh.param("/ins_twist_generator/yaw_offset", yaw_offset_, 0.0);
+
+    gnss_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 2);
+
+    message_filters::Subscriber<inertiallabs_msgs::gps_data> gps_sub(nh, "/Inertial_Labs/gps_data", 2);
+    message_filters::Subscriber<inertiallabs_msgs::ins_data> ins_sub(nh, "/Inertial_Labs/ins_data", 2);
+    
+    Synchronizer<SyncPolicy_> sync(SyncPolicy_(2), gps_sub, ins_sub);
+
+    sync.registerCallback(boost::bind(&gnss_pose_pub_cb, _1, _2));
+
+    tf::TransformBroadcaster br;
+    ros::Rate r(10);
+    while(nh.ok()){
+        ros::spinOnce();
+
+        ToQuaternion(yaw_, pitch_, roll_, gnss_pose_.pose.orientation);
+
+        tf::Quaternion q;
+        q.setRPY(roll_, pitch_, yaw_);
+
+        tf::StampedTransform transform;
+        transform.setOrigin(tf::Vector3(gnss_pose_.pose.position.x, gnss_pose_.pose.position.y, 0.0));
+        transform.setRotation(q);
+        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/map", "/base_link"));
+
+        gnss_pose_pub_.publish(gnss_pose_); 
+
+
+        r.sleep();
+    }
+}
\ No newline at end of file
diff --git a/rubis_ws/deprecated/gnss_converter/src/quaternion_euler.cpp b/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp
similarity index 100%
rename from rubis_ws/deprecated/gnss_converter/src/quaternion_euler.cpp
rename to rubis_ws/src/gnss_converter/src/quaternion_euler.cpp
diff --git a/rubis_ws/src/gnss_module/launch/gnss_module.launch b/rubis_ws/src/gnss_module/launch/gnss_module.launch
index bd06f459..e00550d9 100644
--- a/rubis_ws/src/gnss_module/launch/gnss_module.launch
+++ b/rubis_ws/src/gnss_module/launch/gnss_module.launch
@@ -19,7 +19,7 @@
     <rosparam command="load" file="$(arg kalman_filter_params)" />
     <rosparam command="load" file="$(arg transformation_params)" />
 
-    <node pkg="gnss_module" type="gnss_module" name="gnss_module" output="screen">
+    <node pkg="gnss_module" type="gnss_module" name="gnss_module">
         <param name="pose_topic_name" value="$(arg pose_topic_name)"/>
         <param name="x_offset" value="$(arg x_offset)"/>
         <param name="y_offset" value="$(arg y_offset)"/>
diff --git a/rubis_ws/src/image_transport/CHANGELOG.rst b/rubis_ws/src/image_transport/CHANGELOG.rst
new file mode 100644
index 00000000..191e9aa4
--- /dev/null
+++ b/rubis_ws/src/image_transport/CHANGELOG.rst
@@ -0,0 +1,217 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package image_transport
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.0 (2020-04-03)
+-------------------
+* Noetic release image_common (`#155 <https://github.com/ros-perception/image_common/issues/155>`_)
+* Contributors: Alejandro Hernández Cordero
+
+1.11.14 (2020-04-03)
+--------------------
+* export runtime binaries correctly on Windows (`#116 <https://github.com/ros-perception/image_common/issues/116>`_)
+* add DLL import/export macro (`#118 <https://github.com/ros-perception/image_common/issues/118>`_)
+* Contributors: James Xu
+
+1.11.13 (2017-11-05)
+--------------------
+* Disable image publisher plugins by name (`#60 <https://github.com/ros-perception/image_common/issues/60>`_)
+  * Disable publisher plugins by name
+  * Now have per publisher blacklist instead of image_transport wide.
+* update to use non deprecated pluginlib macro
+* Extend documentation of `getCameraInfoTopic`
+  Document the fact that the `base_topic` argument must be resolved in order to build the correct camera info topic.
+* Added cv::waitkey(10) for blank popup
+  Without the cv::waitkey(10), it results in a blank popup which crashes/ leads to a black popup. This change corrects that problem.
+  ROS Kinetic, Ubuntu 16.04.3
+* Contributors: Aaditya Saraiya, Lucas Walter, Mikael Arguedas, Thibaud Chupin, Vincent Rabaud
+
+1.11.12 (2017-01-29)
+--------------------
+* Fix CMake of image_transport/tutorial and polled_camera
+  Fix loads of problems with the CMakeLists.
+* image_transport/tutorial: Add dependency on generated msg
+  Without this, build fails on Kinetic because ResizedImage.h has not been
+  generated yet.
+* image_transport/tutorial: Add missing catkin_INCLUDE_DIRS
+  Without this, compilation files on Kinetic because ros.h cannot be found.
+* 1.11.11
+* update changelogs
+* Contributors: Martin Guenther, Vincent Rabaud
+
+1.11.11 (2016-09-24)
+--------------------
+
+1.11.10 (2016-01-19)
+--------------------
+
+1.11.9 (2016-01-17)
+-------------------
+* fix linkage in tutorials
+* Use $catkin_EXPORTED_TARGETS
+* Contributors: Jochen Sprickerhof, Vincent Rabaud
+
+1.11.8 (2015-11-29)
+-------------------
+
+1.11.7 (2015-07-28)
+-------------------
+
+1.11.6 (2015-07-16)
+-------------------
+
+1.11.5 (2015-05-14)
+-------------------
+* image_transport: fix CameraSubscriber shutdown (circular shared_ptr ref)
+  CameraSubscriber uses a private boost::shared_ptr to share an impl object
+  between copied instances. In CameraSubscriber::CameraSubscriber(), it
+  handed this shared_ptr to boost::bind() and saved the created wall timer
+  in the impl object, thus creating a circular reference. The impl object
+  was therefore never freed.
+  Fix that by passing a plain pointer to boost::bind().
+* avoid a memory copy for the raw publisher
+* add a way to publish an image with only the data pointer
+* Make function inline to avoid duplicated names when linking statically
+* add plugin examples for the tutorial
+* update instructions for catkin
+* remove uselessly linked library
+  fixes `#28 <https://github.com/ros-perception/image_common/issues/28>`_
+* add a tutorial for image_transport
+* Contributors: Gary Servin, Max Schwarz, Vincent Rabaud
+
+1.11.4 (2014-09-21)
+-------------------
+
+1.11.3 (2014-05-19)
+-------------------
+
+1.11.2 (2014-02-13)
+-------------------
+
+1.11.1 (2014-01-26 02:33)
+-------------------------
+
+1.11.0 (2013-07-20 12:23)
+-------------------------
+
+1.10.5 (2014-01-26 02:34)
+-------------------------
+
+1.10.4 (2013-07-20 11:42)
+-------------------------
+* add Jack as maintainer
+* update my email address
+* Contributors: Vincent Rabaud
+
+1.10.3 (2013-02-21 05:33)
+-------------------------
+
+1.10.2 (2013-02-21 04:48)
+-------------------------
+
+1.10.1 (2013-02-21 04:16)
+-------------------------
+
+1.10.0 (2013-01-13)
+-------------------
+* fix the urls
+* use the pluginlib script to remove some warnings
+* added license headers to various cpp and h files
+* Contributors: Aaron Blasdel, Vincent Rabaud
+
+1.9.22 (2012-12-16)
+-------------------
+* get rid of the deprecated class_loader interface
+* Contributors: Vincent Rabaud
+
+1.9.21 (2012-12-14)
+-------------------
+* CMakeLists.txt clean up
+* Updated package.xml file(s) to handle new catkin buildtool_depend
+  requirement
+* Contributors: William Woodall, mirzashah
+
+1.9.20 (2012-12-04)
+-------------------
+
+1.9.19 (2012-11-08)
+-------------------
+* add the right link libraries
+* Contributors: Vincent Rabaud
+
+1.9.18 (2012-11-06)
+-------------------
+* Isolated plugins into their own library to follow new
+  class_loader/pluginlib guidelines.
+* remove the brief attribute
+* Contributors: Mirza Shah, Vincent Rabaud
+
+1.9.17 (2012-10-30 19:32)
+-------------------------
+
+1.9.16 (2012-10-30 09:10)
+-------------------------
+* add xml file
+* Contributors: Vincent Rabaud
+
+1.9.15 (2012-10-13 08:43)
+-------------------------
+* fix bad folder/libraries
+* Contributors: Vincent Rabaud
+
+1.9.14 (2012-10-13 01:07)
+-------------------------
+
+1.9.13 (2012-10-06)
+-------------------
+
+1.9.12 (2012-10-04)
+-------------------
+
+1.9.11 (2012-10-02 02:56)
+-------------------------
+
+1.9.10 (2012-10-02 02:42)
+-------------------------
+
+1.9.9 (2012-10-01)
+------------------
+* fix dependencies
+* Contributors: Vincent Rabaud
+
+1.9.8 (2012-09-30)
+------------------
+* add catkin as a dependency
+* comply to the catkin API
+* Contributors: Vincent Rabaud
+
+1.9.7 (2012-09-18 11:39)
+------------------------
+
+1.9.6 (2012-09-18 11:07)
+------------------------
+
+1.9.5 (2012-09-13)
+------------------
+* install the include directories
+* Contributors: Vincent Rabaud
+
+1.9.4 (2012-09-12 23:37)
+------------------------
+
+1.9.3 (2012-09-12 20:44)
+------------------------
+
+1.9.2 (2012-09-10)
+------------------
+
+1.9.1 (2012-09-07 15:33)
+------------------------
+* make the libraries public
+* Contributors: Vincent Rabaud
+
+1.9.0 (2012-09-07 13:03)
+------------------------
+* catkinize for Groovy
+* Initial image_common stack check-in, containing image_transport.
+* Contributors: Vincent Rabaud, gerkey, kwc, mihelich, pmihelich, straszheim, vrabaud
diff --git a/rubis_ws/src/image_transport/CMakeLists.txt b/rubis_ws/src/image_transport/CMakeLists.txt
new file mode 100644
index 00000000..1c887df1
--- /dev/null
+++ b/rubis_ws/src/image_transport/CMakeLists.txt
@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(image_transport)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    message_filters
+    pluginlib
+    rosconsole
+    roscpp
+    roslib
+    sensor_msgs
+    rubis_lib
+)
+
+find_package(Boost REQUIRED)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  DEPENDS message_filters pluginlib rosconsole roscpp roslib sensor_msgs
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+# Build libimage_transport
+add_library(${PROJECT_NAME}
+  src/camera_common.cpp
+  src/camera_publisher.cpp
+  src/camera_subscriber.cpp
+  src/image_transport.cpp
+  src/publisher.cpp
+  src/single_subscriber_publisher.cpp
+  src/subscriber.cpp
+)
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+# Build libimage_transport_plugins
+add_library(${PROJECT_NAME}_plugins src/manifest.cpp src/raw_publisher.cpp)
+target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME})
+
+# Install plugin DLL to ${CATKIN_PACKAGE_LIB_DESTINATION} according to the path in default_plugins.xml
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        COMPONENT main
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+# add two execs
+
+add_executable(republish src/republish.cpp)
+target_link_libraries(republish ${PROJECT_NAME})
+
+add_executable(list_transports src/list_transports.cpp)
+target_link_libraries(list_transports
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+install(TARGETS list_transports republish
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# add xml file
+install(FILES default_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
\ No newline at end of file
diff --git a/rubis_ws/src/image_transport/default_plugins.xml b/rubis_ws/src/image_transport/default_plugins.xml
new file mode 100644
index 00000000..f3500ecb
--- /dev/null
+++ b/rubis_ws/src/image_transport/default_plugins.xml
@@ -0,0 +1,13 @@
+<library path="lib/libimage_transport_plugins">
+  <class name="image_transport/raw_pub" type="image_transport::RawPublisher" base_class_type="image_transport::PublisherPlugin">
+    <description>
+      This is the default publisher. It publishes the Image as-is on the base topic.
+    </description>
+  </class>
+
+  <class name="image_transport/raw_sub" type="image_transport::RawSubscriber" base_class_type="image_transport::SubscriberPlugin">
+    <description>
+      This is the default pass-through subscriber for topics of type sensor_msgs/Image.
+    </description>
+  </class>
+</library>
diff --git a/rubis_ws/src/image_transport/include/image_transport/camera_common.h b/rubis_ws/src/image_transport/include/image_transport/camera_common.h
new file mode 100644
index 00000000..b82692c6
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/camera_common.h
@@ -0,0 +1,53 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_CAMERA_COMMON_H
+#define IMAGE_TRANSPORT_CAMERA_COMMON_H
+
+#include <string>
+#include "exports.h"
+
+namespace image_transport {
+
+/**
+ * \brief Form the camera info topic name, sibling to the base topic.
+ *
+ * \note This function assumes that the name is completely resolved. If the \c 
+ * base_topic is remapped the resulting camera info topic will be incorrect. 
+ */
+IMAGE_TRANSPORT_DECL std::string getCameraInfoTopic(const std::string& base_topic);
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/camera_publisher.h b/rubis_ws/src/image_transport/include/image_transport/camera_publisher.h
new file mode 100644
index 00000000..d71809d9
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/camera_publisher.h
@@ -0,0 +1,136 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_CAMERA_PUBLISHER_H
+#define IMAGE_TRANSPORT_CAMERA_PUBLISHER_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include "image_transport/single_subscriber_publisher.h"
+#include "exports.h"
+
+namespace image_transport {
+
+class ImageTransport;
+
+/**
+ * \brief Manages advertisements for publishing camera images.
+ *
+ * CameraPublisher is a convenience class for publishing synchronized image and
+ * camera info topics using the standard topic naming convention, where the info
+ * topic name is "camera_info" in the same namespace as the base image topic.
+ *
+ * On the client side, CameraSubscriber simplifies subscribing to camera images.
+ *
+ * A CameraPublisher should always be created through a call to
+ * ImageTransport::advertiseCamera(), or copied from one that was.
+ * Once all copies of a specific CameraPublisher go out of scope, any subscriber callbacks
+ * associated with that handle will stop being called. Once all CameraPublisher for a
+ * given base topic go out of scope the topic (and all subtopics) will be unadvertised.
+ */
+class IMAGE_TRANSPORT_DECL CameraPublisher
+{
+public:
+  CameraPublisher() {}
+
+  /*!
+   * \brief Returns the number of subscribers that are currently connected to
+   * this CameraPublisher.
+   *
+   * Returns max(image topic subscribers, info topic subscribers).
+   */
+  uint32_t getNumSubscribers() const;
+
+  /*!
+   * \brief Returns the base (image) topic of this CameraPublisher.
+   */
+  std::string getTopic() const;
+
+  /**
+   * \brief Returns the camera info topic of this CameraPublisher.
+   */
+  std::string getInfoTopic() const;
+
+  /*!
+   * \brief Publish an (image, info) pair on the topics associated with this CameraPublisher.
+   */
+  void publish(const sensor_msgs::Image& image, const sensor_msgs::CameraInfo& info) const;
+
+  /*!
+   * \brief Publish an (image, info) pair on the topics associated with this CameraPublisher.
+   */
+  void publish(const sensor_msgs::ImageConstPtr& image,
+               const sensor_msgs::CameraInfoConstPtr& info) const;
+
+  /*!
+   * \brief Publish an (image, info) pair with given timestamp on the topics associated with
+   * this CameraPublisher.
+   *
+   * Convenience version, which sets the timestamps of both image and info to stamp before
+   * publishing.
+   */
+  void publish(sensor_msgs::Image& image, sensor_msgs::CameraInfo& info, ros::Time stamp) const;
+
+  /*!
+   * \brief Shutdown the advertisements associated with this Publisher.
+   */
+  void shutdown();
+
+  operator void*() const;
+  bool operator< (const CameraPublisher& rhs) const { return impl_ <  rhs.impl_; }
+  bool operator!=(const CameraPublisher& rhs) const { return impl_ != rhs.impl_; }
+  bool operator==(const CameraPublisher& rhs) const { return impl_ == rhs.impl_; }
+
+private:
+  CameraPublisher(ImageTransport& image_it, ros::NodeHandle& info_nh,
+                  const std::string& base_topic, uint32_t queue_size,
+                  const SubscriberStatusCallback& image_connect_cb,
+                  const SubscriberStatusCallback& image_disconnect_cb,
+                  const ros::SubscriberStatusCallback& info_connect_cb,
+                  const ros::SubscriberStatusCallback& info_disconnect_cb,
+                  const ros::VoidPtr& tracked_object, bool latch);
+  
+  struct Impl;
+  typedef boost::shared_ptr<Impl> ImplPtr;
+  typedef boost::weak_ptr<Impl> ImplWPtr;
+  
+  ImplPtr impl_;
+
+  friend class ImageTransport;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/camera_subscriber.h b/rubis_ws/src/image_transport/include/image_transport/camera_subscriber.h
new file mode 100644
index 00000000..e769ab69
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/camera_subscriber.h
@@ -0,0 +1,119 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_CAMERA_SUBSCRIBER_H
+#define IMAGE_TRANSPORT_CAMERA_SUBSCRIBER_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
+#include "image_transport/transport_hints.h"
+#include "exports.h"
+
+namespace image_transport {
+
+class ImageTransport;
+
+/**
+ * \brief Manages a subscription callback on synchronized Image and CameraInfo topics.
+ *
+ * CameraSubscriber is the client-side counterpart to CameraPublisher, and assumes the
+ * same topic naming convention. The callback is of type:
+\verbatim
+void callback(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
+\endverbatim
+ *
+ * A CameraSubscriber should always be created through a call to
+ * ImageTransport::subscribeCamera(), or copied from one that was.
+ * Once all copies of a specific CameraSubscriber go out of scope, the subscription callback
+ * associated with that handle will stop being called. Once all CameraSubscriber for a given
+ * topic go out of scope the topic will be unsubscribed.
+ */
+class IMAGE_TRANSPORT_DECL CameraSubscriber
+{
+public:
+  typedef boost::function<void(const sensor_msgs::ImageConstPtr&,
+                               const sensor_msgs::CameraInfoConstPtr&)> Callback;
+  
+  CameraSubscriber() {}
+
+  /**
+   * \brief Get the base topic (on which the raw image is published).
+   */
+  std::string getTopic() const;
+
+  /**
+   * \brief Get the camera info topic.
+   */
+  std::string getInfoTopic() const;
+
+  /**
+   * \brief Returns the number of publishers this subscriber is connected to.
+   */
+  uint32_t getNumPublishers() const;
+
+  /**
+   * \brief Returns the name of the transport being used.
+   */
+  std::string getTransport() const;
+
+  /**
+   * \brief Unsubscribe the callback associated with this CameraSubscriber.
+   */
+  void shutdown();
+
+  operator void*() const;
+  bool operator< (const CameraSubscriber& rhs) const { return impl_ <  rhs.impl_; }
+  bool operator!=(const CameraSubscriber& rhs) const { return impl_ != rhs.impl_; }
+  bool operator==(const CameraSubscriber& rhs) const { return impl_ == rhs.impl_; }
+  
+private:
+  CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh,
+                   const std::string& base_topic, uint32_t queue_size,
+                   const Callback& callback,
+                   const ros::VoidPtr& tracked_object = ros::VoidPtr(),
+                   const TransportHints& transport_hints = TransportHints());
+  
+  struct Impl;
+  typedef boost::shared_ptr<Impl> ImplPtr;
+  typedef boost::weak_ptr<Impl> ImplWPtr;
+  
+  ImplPtr impl_;
+
+  friend class ImageTransport;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/exception.h b/rubis_ws/src/image_transport/include/image_transport/exception.h
new file mode 100644
index 00000000..1b3e2f92
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/exception.h
@@ -0,0 +1,71 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_EXCEPTION_H
+#define IMAGE_TRANSPORT_EXCEPTION_H
+
+#include <stdexcept>
+
+namespace image_transport {
+
+/**
+ * \brief A base class for all image_transport exceptions inheriting from std::runtime_error.
+ */
+class Exception : public std::runtime_error
+{
+public:
+  Exception(const std::string& message) : std::runtime_error(message) {}
+};
+
+/**
+ * \brief An exception class thrown when image_transport is unable to load a requested transport.
+ */
+class TransportLoadException : public Exception
+{
+public:
+  TransportLoadException(const std::string& transport, const std::string& message)
+    : Exception("Unable to load plugin for transport '" + transport + "', error string:\n" + message),
+      transport_(transport.c_str())
+  {
+  }
+
+  std::string getTransport() const { return transport_; }
+
+protected:
+  const char* transport_;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/exports.h b/rubis_ws/src/image_transport/include/image_transport/exports.h
new file mode 100644
index 00000000..d3dae203
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/exports.h
@@ -0,0 +1,18 @@
+#ifndef IMAGE_TRANSPORT_EXPORTS_H
+#define IMAGE_TRANSPORT_EXPORTS_H
+
+#include <ros/macros.h>
+
+// Import/export for windows dll's and visibility for gcc shared libraries.
+
+#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
+  #ifdef image_transport_EXPORTS // we are building a shared lib/dll
+    #define IMAGE_TRANSPORT_DECL ROS_HELPER_EXPORT
+  #else // we are using shared lib/dll
+    #define IMAGE_TRANSPORT_DECL ROS_HELPER_IMPORT
+  #endif
+#else // ros is being built around static libraries
+  #define IMAGE_TRANSPORT_DECL
+#endif
+
+#endif // IMAGE_TRANSPORT_EXPORTS_H
diff --git a/rubis_ws/src/image_transport/include/image_transport/image_transport.h b/rubis_ws/src/image_transport/include/image_transport/image_transport.h
new file mode 100644
index 00000000..9a542e62
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/image_transport.h
@@ -0,0 +1,205 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_IMAGE_TRANSPORT_H
+#define IMAGE_TRANSPORT_IMAGE_TRANSPORT_H
+
+#include "image_transport/publisher.h"
+#include "image_transport/subscriber.h"
+#include "image_transport/camera_publisher.h"
+#include "image_transport/camera_subscriber.h"
+#include "exports.h"
+
+namespace image_transport {
+
+/**
+ * \brief Advertise and subscribe to image topics.
+ *
+ * ImageTransport is analogous to ros::NodeHandle in that it contains advertise() and
+ * subscribe() functions for creating advertisements and subscriptions of image topics.
+ */
+class IMAGE_TRANSPORT_DECL ImageTransport
+{
+public:
+  explicit ImageTransport(const ros::NodeHandle& nh);
+
+  ~ImageTransport();
+
+  /*!
+   * \brief Advertise an image topic, simple version.
+   */
+  Publisher advertise(const std::string& base_topic, uint32_t queue_size, bool latch = false);
+
+  /*!
+   * \brief Advertise an image topic with subcriber status callbacks.
+   */
+  Publisher advertise(const std::string& base_topic, uint32_t queue_size,
+                      const SubscriberStatusCallback& connect_cb,
+                      const SubscriberStatusCallback& disconnect_cb = SubscriberStatusCallback(),
+                      const ros::VoidPtr& tracked_object = ros::VoidPtr(), bool latch = false);
+
+  /**
+   * \brief Subscribe to an image topic, version for arbitrary boost::function object.
+   */
+  Subscriber subscribe(const std::string& base_topic, uint32_t queue_size,
+                       const boost::function<void(const sensor_msgs::ImageConstPtr&)>& callback,
+                       const ros::VoidPtr& tracked_object = ros::VoidPtr(),
+                       const TransportHints& transport_hints = TransportHints());
+
+  /**
+   * \brief Subscribe to an image topic, version for bare function.
+   */
+  Subscriber subscribe(const std::string& base_topic, uint32_t queue_size,
+                       void(*fp)(const sensor_msgs::ImageConstPtr&),
+                       const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribe(base_topic, queue_size,
+                     boost::function<void(const sensor_msgs::ImageConstPtr&)>(fp),
+                     ros::VoidPtr(), transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to an image topic, version for class member function with bare pointer.
+   */
+  template<class T>
+  Subscriber subscribe(const std::string& base_topic, uint32_t queue_size,
+                       void(T::*fp)(const sensor_msgs::ImageConstPtr&), T* obj,
+                       const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribe(base_topic, queue_size, boost::bind(fp, obj, _1), ros::VoidPtr(), transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to an image topic, version for class member function with shared_ptr.
+   */
+  template<class T>
+  Subscriber subscribe(const std::string& base_topic, uint32_t queue_size,
+                       void(T::*fp)(const sensor_msgs::ImageConstPtr&),
+                       const boost::shared_ptr<T>& obj,
+                       const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribe(base_topic, queue_size, boost::bind(fp, obj.get(), _1), obj, transport_hints);
+  }
+
+  /*!
+   * \brief Advertise a synchronized camera raw image + info topic pair, simple version.
+   */
+  CameraPublisher advertiseCamera(const std::string& base_topic, uint32_t queue_size, bool latch = false);
+
+  /*!
+   * \brief Advertise a synchronized camera raw image + info topic pair with subscriber status
+   * callbacks.
+   */
+  CameraPublisher advertiseCamera(const std::string& base_topic, uint32_t queue_size,
+                                  const SubscriberStatusCallback& image_connect_cb,
+                                  const SubscriberStatusCallback& image_disconnect_cb = SubscriberStatusCallback(),
+                                  const ros::SubscriberStatusCallback& info_connect_cb = ros::SubscriberStatusCallback(),
+                                  const ros::SubscriberStatusCallback& info_disconnect_cb = ros::SubscriberStatusCallback(),
+                                  const ros::VoidPtr& tracked_object = ros::VoidPtr(), bool latch = false);
+
+  /**
+   * \brief Subscribe to a synchronized image & camera info topic pair, version for arbitrary
+   * boost::function object.
+   *
+   * This version assumes the standard topic naming scheme, where the info topic is
+   * named "camera_info" in the same namespace as the base image topic.
+   */
+  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
+                                   const CameraSubscriber::Callback& callback,
+                                   const ros::VoidPtr& tracked_object = ros::VoidPtr(),
+                                   const TransportHints& transport_hints = TransportHints());
+
+  /**
+   * \brief Subscribe to a synchronized image & camera info topic pair, version for bare function.
+   */
+  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
+                                   void(*fp)(const sensor_msgs::ImageConstPtr&,
+                                             const sensor_msgs::CameraInfoConstPtr&),
+                                   const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribeCamera(base_topic, queue_size, CameraSubscriber::Callback(fp), ros::VoidPtr(),
+                           transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to a synchronized image & camera info topic pair, version for class member
+   * function with bare pointer.
+   */
+  template<class T>
+  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
+                                   void(T::*fp)(const sensor_msgs::ImageConstPtr&,
+                                                const sensor_msgs::CameraInfoConstPtr&), T* obj,
+                                   const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribeCamera(base_topic, queue_size, boost::bind(fp, obj, _1, _2), ros::VoidPtr(),
+                           transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to a synchronized image & camera info topic pair, version for class member
+   * function with shared_ptr.
+   */
+  template<class T>
+  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
+                                   void(T::*fp)(const sensor_msgs::ImageConstPtr&,
+                                                const sensor_msgs::CameraInfoConstPtr&),
+                                   const boost::shared_ptr<T>& obj,
+                                   const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribeCamera(base_topic, queue_size, boost::bind(fp, obj.get(), _1, _2), obj,
+                           transport_hints);
+  }
+
+  /**
+   * \brief Returns the names of all transports declared in the system. Declared
+   * transports are not necessarily built or loadable.
+   */
+  std::vector<std::string> getDeclaredTransports() const;
+
+  /**
+   * \brief Returns the names of all transports that are loadable in the system.
+   */
+  std::vector<std::string> getLoadableTransports() const;
+
+private:
+  struct Impl;
+  typedef boost::shared_ptr<Impl> ImplPtr;
+  typedef boost::weak_ptr<Impl> ImplWPtr;
+
+  ImplPtr impl_;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/loader_fwds.h b/rubis_ws/src/image_transport/include/image_transport/loader_fwds.h
new file mode 100644
index 00000000..2d2dd311
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/loader_fwds.h
@@ -0,0 +1,56 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_LOADER_FWDS_H
+#define IMAGE_TRANSPORT_LOADER_FWDS_H
+
+// Forward-declare some classes most users shouldn't care about so that
+// image_transport.h doesn't bring them in.
+
+namespace pluginlib {
+  template<class T> class ClassLoader;
+}
+
+namespace image_transport {
+  class PublisherPlugin;
+  class SubscriberPlugin;
+
+  typedef pluginlib::ClassLoader<PublisherPlugin> PubLoader;
+  typedef boost::shared_ptr<PubLoader> PubLoaderPtr;
+
+  typedef pluginlib::ClassLoader<SubscriberPlugin> SubLoader;
+  typedef boost::shared_ptr<SubLoader> SubLoaderPtr;
+}
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/publisher.h b/rubis_ws/src/image_transport/include/image_transport/publisher.h
new file mode 100644
index 00000000..6a6c1153
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/publisher.h
@@ -0,0 +1,126 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_PUBLISHER_H
+#define IMAGE_TRANSPORT_PUBLISHER_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include "image_transport/single_subscriber_publisher.h"
+#include "image_transport/exception.h"
+#include "image_transport/loader_fwds.h"
+#include "exports.h"
+
+namespace image_transport {
+
+/**
+ * \brief Manages advertisements of multiple transport options on an Image topic.
+ *
+ * Publisher is a drop-in replacement for ros::Publisher when publishing
+ * Image topics. In a minimally built environment, they behave the same; however,
+ * Publisher is extensible via plugins to publish alternative representations of
+ * the image on related subtopics. This is especially useful for limiting bandwidth and
+ * latency over a network connection, when you might (for example) use the theora plugin
+ * to transport the images as streamed video. All topics are published only on demand
+ * (i.e. if there are subscribers).
+ *
+ * A Publisher should always be created through a call to ImageTransport::advertise(),
+ * or copied from one that was.
+ * Once all copies of a specific Publisher go out of scope, any subscriber callbacks
+ * associated with that handle will stop being called. Once all Publisher for a
+ * given base topic go out of scope the topic (and all subtopics) will be unadvertised.
+ */
+class IMAGE_TRANSPORT_DECL Publisher
+{
+public:
+  Publisher() {}
+
+  /*!
+   * \brief Returns the number of subscribers that are currently connected to
+   * this Publisher.
+   *
+   * Returns the total number of subscribers to all advertised topics.
+   */
+  uint32_t getNumSubscribers() const;
+
+  /*!
+   * \brief Returns the base topic of this Publisher.
+   */
+  std::string getTopic() const;
+
+  /*!
+   * \brief Publish an image on the topics associated with this Publisher.
+   */
+  void publish(const sensor_msgs::Image& message) const;
+
+  /*!
+   * \brief Publish an image on the topics associated with this Publisher.
+   */
+  void publish(const sensor_msgs::ImageConstPtr& message) const;
+
+  /*!
+   * \brief Shutdown the advertisements associated with this Publisher.
+   */
+  void shutdown();
+
+  operator void*() const;
+  bool operator< (const Publisher& rhs) const { return impl_ <  rhs.impl_; }
+  bool operator!=(const Publisher& rhs) const { return impl_ != rhs.impl_; }
+  bool operator==(const Publisher& rhs) const { return impl_ == rhs.impl_; }
+
+private:
+  Publisher(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+            const SubscriberStatusCallback& connect_cb,
+            const SubscriberStatusCallback& disconnect_cb,
+            const ros::VoidPtr& tracked_object, bool latch,
+            const PubLoaderPtr& loader);
+
+  struct Impl;
+  typedef boost::shared_ptr<Impl> ImplPtr;
+  typedef boost::weak_ptr<Impl> ImplWPtr;
+  
+  ImplPtr impl_;
+
+  static void weakSubscriberCb(const ImplWPtr& impl_wptr,
+                               const SingleSubscriberPublisher& plugin_pub,
+                               const SubscriberStatusCallback& user_cb);
+  
+  SubscriberStatusCallback rebindCB(const SubscriberStatusCallback& user_cb);
+
+  friend class ImageTransport;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/publisher_plugin.h b/rubis_ws/src/image_transport/include/image_transport/publisher_plugin.h
new file mode 100644
index 00000000..cc9c0869
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/publisher_plugin.h
@@ -0,0 +1,150 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_PUBLISHER_PLUGIN_H
+#define IMAGE_TRANSPORT_PUBLISHER_PLUGIN_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include "image_transport/single_subscriber_publisher.h"
+
+namespace image_transport {
+
+/**
+ * \brief Base class for plugins to Publisher.
+ */
+class PublisherPlugin : boost::noncopyable
+{
+public:
+  virtual ~PublisherPlugin() {}
+
+  /**
+   * \brief Get a string identifier for the transport provided by
+   * this plugin.
+   */
+  virtual std::string getTransportName() const = 0;
+
+  /**
+   * \brief Advertise a topic, simple version.
+   */
+  void advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                 bool latch = true)
+  {
+    advertiseImpl(nh, base_topic, queue_size, SubscriberStatusCallback(),
+                  SubscriberStatusCallback(), ros::VoidPtr(), latch);
+  }
+  
+  /**
+   * \brief Advertise a topic with subscriber status callbacks.
+   */
+  void advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                 const SubscriberStatusCallback& connect_cb,
+                 const SubscriberStatusCallback& disconnect_cb = SubscriberStatusCallback(),
+                 const ros::VoidPtr& tracked_object = ros::VoidPtr(), bool latch = true)
+  {
+    advertiseImpl(nh, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch);
+  }
+
+  /**
+   * \brief Returns the number of subscribers that are currently connected to
+   * this PublisherPlugin.
+   */
+  virtual uint32_t getNumSubscribers() const = 0;
+
+  /**
+   * \brief Returns the communication topic that this PublisherPlugin will publish on.
+   */
+  virtual std::string getTopic() const = 0;
+
+  /**
+   * \brief Publish an image using the transport associated with this PublisherPlugin.
+   */
+  virtual void publish(const sensor_msgs::Image& message) const = 0;
+
+  /**
+   * \brief Publish an image using the transport associated with this PublisherPlugin.
+   */
+  virtual void publish(const sensor_msgs::ImageConstPtr& message) const
+  {
+    publish(*message);
+  }
+
+  /**
+   * \brief Publish an image using the transport associated with this PublisherPlugin.
+   * This version of the function can be used to optimize cases where you don't want to
+   * fill a ROS message first to avoid useless copies.
+   * @param message an image message to use information from (but not data)
+   * @param data a pointer to the image data to use to fill the Image message
+   */
+  virtual void publish(const sensor_msgs::Image& message, const uint8_t* data) const
+  {
+    sensor_msgs::Image msg;
+    msg.header = message.header;
+    msg.height = message.height;
+    msg.width = message.width;
+    msg.encoding = message.encoding;
+    msg.is_bigendian = message.is_bigendian;
+    msg.step = message.step;
+    msg.data = std::vector<uint8_t>(data, data + msg.step*msg.height);
+
+    publish(msg);
+  }
+
+  /**
+   * \brief Shutdown any advertisements associated with this PublisherPlugin.
+   */
+  virtual void shutdown() = 0;
+
+  /**
+   * \brief Return the lookup name of the PublisherPlugin associated with a specific
+   * transport identifier.
+   */
+  static std::string getLookupName(const std::string& transport_name)
+  {
+    return "image_transport/" + transport_name + "_pub";
+  }
+
+protected:
+  /**
+   * \brief Advertise a topic. Must be implemented by the subclass.
+   */
+  virtual void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                             const SubscriberStatusCallback& connect_cb,
+                             const SubscriberStatusCallback& disconnect_cb,
+                             const ros::VoidPtr& tracked_object, bool latch) = 0;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/raw_publisher.h b/rubis_ws/src/image_transport/include/image_transport/raw_publisher.h
new file mode 100644
index 00000000..4ba36e3f
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/raw_publisher.h
@@ -0,0 +1,82 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_RAW_PUBLISHER_H
+#define IMAGE_TRANSPORT_RAW_PUBLISHER_H
+
+#include "image_transport/simple_publisher_plugin.h"
+
+namespace image_transport {
+
+/**
+ * \brief The default PublisherPlugin.
+ *
+ * RawPublisher is a simple wrapper for ros::Publisher, publishing unaltered Image
+ * messages on the base topic.
+ */
+class RawPublisher : public SimplePublisherPlugin<sensor_msgs::Image>
+{
+public:
+  virtual ~RawPublisher() {}
+
+  virtual std::string getTransportName() const
+  {
+    return "raw";
+  }
+
+  // Override the default implementation because publishing the message pointer allows
+  // the no-copy intraprocess optimization.
+  virtual void publish(const sensor_msgs::ImageConstPtr& message) const
+  {
+    getPublisher().publish(message);
+  }
+
+  // Override the default implementation to not copy data to a sensor_msgs::Image first
+  virtual void publish(const sensor_msgs::Image& message, const uint8_t* data) const;
+
+protected:
+  virtual void publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const
+  {
+    publish_fn(message);
+  }
+
+  virtual std::string getTopicToAdvertise(const std::string& base_topic) const
+  {
+    return base_topic;
+  }
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/raw_subscriber.h b/rubis_ws/src/image_transport/include/image_transport/raw_subscriber.h
new file mode 100644
index 00000000..82411916
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/raw_subscriber.h
@@ -0,0 +1,72 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_RAW_SUBSCRIBER_H
+#define IMAGE_TRANSPORT_RAW_SUBSCRIBER_H
+
+#include "image_transport/simple_subscriber_plugin.h"
+
+namespace image_transport {
+
+/**
+ * \brief The default SubscriberPlugin.
+ *
+ * RawSubscriber is a simple wrapper for ros::Subscriber which listens for Image messages
+ * and passes them through to the callback.
+ */
+class RawSubscriber : public SimpleSubscriberPlugin<sensor_msgs::Image>
+{
+public:
+  virtual ~RawSubscriber() {}
+
+  virtual std::string getTransportName() const
+  {
+    return "raw";
+  }
+
+protected:
+  virtual void internalCallback(const sensor_msgs::ImageConstPtr& message, const Callback& user_cb)
+  {
+    user_cb(message);
+  }
+
+  virtual std::string getTopicToSubscribe(const std::string& base_topic) const
+  {
+    return base_topic;
+  }
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/simple_publisher_plugin.h b/rubis_ws/src/image_transport/include/image_transport/simple_publisher_plugin.h
new file mode 100644
index 00000000..691d2ed5
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/simple_publisher_plugin.h
@@ -0,0 +1,240 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H
+#define IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H
+
+#include "image_transport/publisher_plugin.h"
+#include <boost/scoped_ptr.hpp>
+
+namespace image_transport {
+
+/**
+ * \brief Base class to simplify implementing most plugins to Publisher.
+ *
+ * This base class vastly simplifies implementing a PublisherPlugin in the common
+ * case that all communication with the matching SubscriberPlugin happens over a
+ * single ROS topic using a transport-specific message type. SimplePublisherPlugin
+ * is templated on the transport-specific message type.
+ *
+ * A subclass need implement only two methods:
+ * - getTransportName() from PublisherPlugin
+ * - publish() with an extra PublishFn argument
+ *
+ * For access to the parameter server and name remappings, use nh().
+ *
+ * getTopicToAdvertise() controls the name of the internal communication topic.
+ * It defaults to \<base topic\>/\<transport name\>.
+ */
+template <class M>
+class SimplePublisherPlugin : public PublisherPlugin
+{
+public:
+  virtual ~SimplePublisherPlugin() {}
+
+  virtual uint32_t getNumSubscribers() const
+  {
+    if (simple_impl_) return simple_impl_->pub_.getNumSubscribers();
+    return 0;
+  }
+
+  virtual std::string getTopic() const
+  {
+    if (simple_impl_) return simple_impl_->pub_.getTopic();
+    return std::string();
+  }
+
+  virtual void publish(const sensor_msgs::Image& message) const
+  {
+    if (!simple_impl_ || !simple_impl_->pub_) {
+      ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::SimplePublisherPlugin");
+      return;
+    }
+    
+    publish(message, bindInternalPublisher(simple_impl_->pub_));
+  }
+
+  virtual void shutdown()
+  {
+    if (simple_impl_) simple_impl_->pub_.shutdown();
+  }
+
+protected:
+  virtual void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                             const SubscriberStatusCallback& user_connect_cb,
+                             const SubscriberStatusCallback& user_disconnect_cb,
+                             const ros::VoidPtr& tracked_object, bool latch)
+  {
+    std::string transport_topic = getTopicToAdvertise(base_topic);
+    ros::NodeHandle param_nh(transport_topic);
+    simple_impl_.reset(new SimplePublisherPluginImpl(param_nh));
+    simple_impl_->pub_ = nh.advertise<M>(transport_topic, queue_size,
+                                         bindCB(user_connect_cb, &SimplePublisherPlugin::connectCallback),
+                                         bindCB(user_disconnect_cb, &SimplePublisherPlugin::disconnectCallback),
+                                         tracked_object, latch);
+  }
+
+  //! Generic function for publishing the internal message type.
+  typedef boost::function<void(const M&)> PublishFn;
+
+  /**
+   * \brief Publish an image using the specified publish function. Must be implemented by
+   * the subclass.
+   *
+   * The PublishFn publishes the transport-specific message type. This indirection allows
+   * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and
+   * single subscriber publishing (in subscription callbacks).
+   */
+  virtual void publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const = 0;
+
+  /**
+   * \brief Return the communication topic name for a given base topic.
+   *
+   * Defaults to \<base topic\>/\<transport name\>.
+   */
+  virtual std::string getTopicToAdvertise(const std::string& base_topic) const
+  {
+    return base_topic + "/" + getTransportName();
+  }
+
+  /**
+   * \brief Function called when a subscriber connects to the internal publisher.
+   *
+   * Defaults to noop.
+   */
+  virtual void connectCallback(const ros::SingleSubscriberPublisher& pub) {}
+
+  /**
+   * \brief Function called when a subscriber disconnects from the internal publisher.
+   *
+   * Defaults to noop.
+   */
+  virtual void disconnectCallback(const ros::SingleSubscriberPublisher& pub) {}
+
+  /**
+   * \brief Returns the ros::NodeHandle to be used for parameter lookup.
+   */
+  const ros::NodeHandle& nh() const
+  {
+    return simple_impl_->param_nh_;
+  }
+
+  /**
+   * \brief Returns the internal ros::Publisher.
+   *
+   * This really only exists so RawPublisher can implement no-copy intraprocess message
+   * passing easily.
+   */
+  const ros::Publisher& getPublisher() const
+  {
+    ROS_ASSERT(simple_impl_);
+    return simple_impl_->pub_;
+  }
+
+private:
+  struct SimplePublisherPluginImpl
+  {
+    SimplePublisherPluginImpl(const ros::NodeHandle& nh)
+      : param_nh_(nh)
+    {
+    }
+    
+    const ros::NodeHandle param_nh_;
+    ros::Publisher pub_;
+  };
+  
+  boost::scoped_ptr<SimplePublisherPluginImpl> simple_impl_;
+
+  typedef void (SimplePublisherPlugin::*SubscriberStatusMemFn)(const ros::SingleSubscriberPublisher& pub);
+  
+  /**
+   * Binds the user callback to subscriberCB(), which acts as an intermediary to expose
+   * a publish(Image) interface to the user while publishing to an internal topic.
+   */
+  ros::SubscriberStatusCallback bindCB(const SubscriberStatusCallback& user_cb,
+                                       SubscriberStatusMemFn internal_cb_fn)
+  {
+    ros::SubscriberStatusCallback internal_cb = boost::bind(internal_cb_fn, this, _1);
+    if (user_cb)
+      return boost::bind(&SimplePublisherPlugin::subscriberCB, this, _1, user_cb, internal_cb);
+    else
+      return internal_cb;
+  }
+  
+  /**
+   * Forms the ros::SingleSubscriberPublisher for the internal communication topic into
+   * an image_transport::SingleSubscriberPublisher for Image messages and passes it
+   * to the user subscriber status callback.
+   */
+  void subscriberCB(const ros::SingleSubscriberPublisher& ros_ssp,
+                    const SubscriberStatusCallback& user_cb,
+                    const ros::SubscriberStatusCallback& internal_cb)
+  {
+    // First call the internal callback (for sending setup headers, etc.)
+    internal_cb(ros_ssp);
+    
+    // Construct a function object for publishing sensor_msgs::Image through the
+    // subclass-implemented publish() using the ros::SingleSubscriberPublisher to send
+    // messages of the transport-specific type.
+    typedef void (SimplePublisherPlugin::*PublishMemFn)(const sensor_msgs::Image&, const PublishFn&) const;
+    PublishMemFn pub_mem_fn = &SimplePublisherPlugin::publish;
+    ImagePublishFn image_publish_fn = boost::bind(pub_mem_fn, this, _1, bindInternalPublisher(ros_ssp));
+    
+    SingleSubscriberPublisher ssp(ros_ssp.getSubscriberName(), getTopic(),
+                                  boost::bind(&SimplePublisherPlugin::getNumSubscribers, this),
+                                  image_publish_fn);
+    user_cb(ssp);
+  }
+
+  typedef boost::function<void(const sensor_msgs::Image&)> ImagePublishFn;
+
+  /**
+   * Returns a function object for publishing the transport-specific message type
+   * through some ROS publisher type.
+   *
+   * @param pub An object with method void publish(const M&)
+   */
+  template <class PubT>
+  PublishFn bindInternalPublisher(const PubT& pub) const
+  {
+    // Bind PubT::publish(const Message&) as PublishFn
+    typedef void (PubT::*InternalPublishMemFn)(const M&) const;
+    InternalPublishMemFn internal_pub_mem_fn = &PubT::publish;
+    return boost::bind(internal_pub_mem_fn, &pub, _1);
+  }
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/simple_subscriber_plugin.h b/rubis_ws/src/image_transport/include/image_transport/simple_subscriber_plugin.h
new file mode 100644
index 00000000..dd6ea3c7
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/simple_subscriber_plugin.h
@@ -0,0 +1,141 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_SIMPLE_SUBSCRIBER_PLUGIN_H
+#define IMAGE_TRANSPORT_SIMPLE_SUBSCRIBER_PLUGIN_H
+
+#include "image_transport/subscriber_plugin.h"
+#include <boost/scoped_ptr.hpp>
+
+namespace image_transport {
+
+/**
+ * \brief Base class to simplify implementing most plugins to Subscriber.
+ *
+ * The base class simplifies implementing a SubscriberPlugin in the common case that
+ * all communication with the matching PublisherPlugin happens over a single ROS
+ * topic using a transport-specific message type. SimpleSubscriberPlugin is templated
+ * on the transport-specific message type.
+ *
+ * A subclass need implement only two methods:
+ * - getTransportName() from SubscriberPlugin
+ * - internalCallback() - processes a message and invoked the user Image callback if
+ * appropriate.
+ *
+ * For access to the parameter server and name remappings, use nh().
+ *
+ * getTopicToSubscribe() controls the name of the internal communication topic. It
+ * defaults to \<base topic\>/\<transport name\>.
+ */
+template <class M>
+class SimpleSubscriberPlugin : public SubscriberPlugin
+{
+public:
+  virtual ~SimpleSubscriberPlugin() {}
+
+  virtual std::string getTopic() const
+  {
+    if (simple_impl_) return simple_impl_->sub_.getTopic();
+    return std::string();
+  }
+
+  virtual uint32_t getNumPublishers() const
+  {
+    if (simple_impl_) return simple_impl_->sub_.getNumPublishers();
+    return 0;
+  }
+
+  virtual void shutdown()
+  {
+    if (simple_impl_) simple_impl_->sub_.shutdown();
+  }
+
+protected:
+  /**
+   * \brief Process a message. Must be implemented by the subclass.
+   *
+   * @param message A message from the PublisherPlugin.
+   * @param user_cb The user Image callback to invoke, if appropriate.
+   */
+  virtual void internalCallback(const typename M::ConstPtr& message, const Callback& user_cb) = 0;
+
+  /**
+   * \brief Return the communication topic name for a given base topic.
+   *
+   * Defaults to \<base topic\>/\<transport name\>.
+   */
+  virtual std::string getTopicToSubscribe(const std::string& base_topic) const
+  {
+    return base_topic + "/" + getTransportName();
+  }
+  
+  virtual void subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                             const Callback& callback, const ros::VoidPtr& tracked_object,
+                             const TransportHints& transport_hints)
+  {
+    // Push each group of transport-specific parameters into a separate sub-namespace
+    ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName());
+    simple_impl_.reset(new SimpleSubscriberPluginImpl(param_nh));
+
+    simple_impl_->sub_ = nh.subscribe<M>(getTopicToSubscribe(base_topic), queue_size,
+                                         boost::bind(&SimpleSubscriberPlugin::internalCallback, this, _1, callback),
+                                         tracked_object, transport_hints.getRosHints());
+  }
+
+  /**
+   * \brief Returns the ros::NodeHandle to be used for parameter lookup.
+   */
+  const ros::NodeHandle& nh() const
+  {
+    return simple_impl_->param_nh_;
+  }
+
+private:
+  struct SimpleSubscriberPluginImpl
+  {
+    SimpleSubscriberPluginImpl(const ros::NodeHandle& nh)
+      : param_nh_(nh)
+    {
+    }
+    
+    const ros::NodeHandle param_nh_;
+    ros::Subscriber sub_;
+  };
+  
+  boost::scoped_ptr<SimpleSubscriberPluginImpl> simple_impl_;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/single_subscriber_publisher.h b/rubis_ws/src/image_transport/include/image_transport/single_subscriber_publisher.h
new file mode 100644
index 00000000..4168cd78
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/single_subscriber_publisher.h
@@ -0,0 +1,81 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_SINGLE_SUBSCRIBER_PUBLISHER
+#define IMAGE_TRANSPORT_SINGLE_SUBSCRIBER_PUBLISHER
+
+#include <boost/noncopyable.hpp>
+#include <boost/function.hpp>
+#include <sensor_msgs/Image.h>
+#include "exports.h"
+
+namespace image_transport {
+
+/**
+ * \brief Allows publication of an image to a single subscriber. Only available inside
+ * subscriber connection callbacks.
+ */
+class IMAGE_TRANSPORT_DECL SingleSubscriberPublisher : boost::noncopyable
+{
+public:
+  typedef boost::function<uint32_t()> GetNumSubscribersFn;
+  typedef boost::function<void(const sensor_msgs::Image&)> PublishFn;
+  
+  SingleSubscriberPublisher(const std::string& caller_id, const std::string& topic,
+                            const GetNumSubscribersFn& num_subscribers_fn,
+                            const PublishFn& publish_fn);
+  
+  std::string getSubscriberName() const;
+
+  std::string getTopic() const;
+
+  uint32_t getNumSubscribers() const;
+
+  void publish(const sensor_msgs::Image& message) const;
+  void publish(const sensor_msgs::ImageConstPtr& message) const;
+
+private:
+  std::string caller_id_;
+  std::string topic_;
+  GetNumSubscribersFn num_subscribers_fn_;
+  PublishFn publish_fn_;
+
+  friend class Publisher; // to get publish_fn_ directly
+};
+
+typedef boost::function<void(const SingleSubscriberPublisher&)> SubscriberStatusCallback;
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/subscriber.h b/rubis_ws/src/image_transport/include/image_transport/subscriber.h
new file mode 100644
index 00000000..22f27223
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/subscriber.h
@@ -0,0 +1,112 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_SUBSCRIBER_H
+#define IMAGE_TRANSPORT_SUBSCRIBER_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include "image_transport/transport_hints.h"
+#include "image_transport/exception.h"
+#include "image_transport/loader_fwds.h"
+#include "exports.h"
+
+namespace image_transport {
+
+/**
+ * \brief Manages a subscription callback on a specific topic that can be interpreted
+ * as an Image topic.
+ *
+ * Subscriber is the client-side counterpart to Publisher. By loading the
+ * appropriate plugin, it can subscribe to a base image topic using any available
+ * transport. The complexity of what transport is actually used is hidden from the user,
+ * who sees only a normal Image callback.
+ *
+ * A Subscriber should always be created through a call to ImageTransport::subscribe(),
+ * or copied from one that was.
+ * Once all copies of a specific Subscriber go out of scope, the subscription callback
+ * associated with that handle will stop being called. Once all Subscriber for a given
+ * topic go out of scope the topic will be unsubscribed.
+ */
+class IMAGE_TRANSPORT_DECL Subscriber
+{
+public:
+  Subscriber() {}
+
+  /**
+   * \brief Returns the base image topic.
+   *
+   * The Subscriber may actually be subscribed to some transport-specific topic that
+   * differs from the base topic.
+   */
+  std::string getTopic() const;
+
+  /**
+   * \brief Returns the number of publishers this subscriber is connected to.
+   */
+  uint32_t getNumPublishers() const;
+
+  /**
+   * \brief Returns the name of the transport being used.
+   */
+  std::string getTransport() const;
+  
+  /**
+   * \brief Unsubscribe the callback associated with this Subscriber.
+   */
+  void shutdown();
+
+  operator void*() const;
+  bool operator< (const Subscriber& rhs) const { return impl_ <  rhs.impl_; }
+  bool operator!=(const Subscriber& rhs) const { return impl_ != rhs.impl_; }
+  bool operator==(const Subscriber& rhs) const { return impl_ == rhs.impl_; }
+  
+private:
+  Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+             const boost::function<void(const sensor_msgs::ImageConstPtr&)>& callback,
+             const ros::VoidPtr& tracked_object, const TransportHints& transport_hints,
+             const SubLoaderPtr& loader);
+  
+  struct Impl;
+  typedef boost::shared_ptr<Impl> ImplPtr;
+  typedef boost::weak_ptr<Impl> ImplWPtr;
+  
+  ImplPtr impl_;
+
+  friend class ImageTransport;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/subscriber_filter.h b/rubis_ws/src/image_transport/include/image_transport/subscriber_filter.h
new file mode 100644
index 00000000..3d5be7bd
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/subscriber_filter.h
@@ -0,0 +1,163 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_SUBSCRIBER_FILTER_H
+#define IMAGE_TRANSPORT_SUBSCRIBER_FILTER_H
+
+#include <ros/ros.h>
+#include <message_filters/simple_filter.h>
+
+#include "image_transport/image_transport.h"
+
+namespace image_transport {
+
+/**
+ * \brief Image subscription filter.
+ *
+ * This class wraps Subscriber as a "filter" compatible with the message_filters
+ * package. It acts as a highest-level filter, simply passing messages from an image
+ * transport subscription through to the filters which have connected to it.
+ *
+ * When this object is destroyed it will unsubscribe from the ROS subscription.
+ *
+ * \section connections CONNECTIONS
+ *
+ * SubscriberFilter has no input connection.
+ *
+ * The output connection for the SubscriberFilter object is the same signature as for roscpp
+ * subscription callbacks, ie.
+\verbatim
+void callback(const boost::shared_ptr<const sensor_msgs::Image>&);
+\endverbatim
+ */
+class SubscriberFilter : public message_filters::SimpleFilter<sensor_msgs::Image>
+{
+public:
+  /**
+   * \brief Constructor
+   *
+   * See the ros::NodeHandle::subscribe() variants for more information on the parameters
+   *
+   * \param nh The ros::NodeHandle to use to subscribe.
+   * \param base_topic The topic to subscribe to.
+   * \param queue_size The subscription queue size
+   * \param transport_hints The transport hints to pass along
+   */
+  SubscriberFilter(ImageTransport& it, const std::string& base_topic, uint32_t queue_size,
+                   const TransportHints& transport_hints = TransportHints())
+  {
+    subscribe(it, base_topic, queue_size, transport_hints);
+  }
+
+  /**
+   * \brief Empty constructor, use subscribe() to subscribe to a topic
+   */
+  SubscriberFilter()
+  {
+  }
+
+  ~SubscriberFilter()
+  {
+    unsubscribe();
+  }
+
+  /**
+   * \brief Subscribe to a topic.
+   *
+   * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.
+   *
+   * \param nh The ros::NodeHandle to use to subscribe.
+   * \param base_topic The topic to subscribe to.
+   * \param queue_size The subscription queue size
+   * \param transport_hints The transport hints to pass along
+   */
+  void subscribe(ImageTransport& it, const std::string& base_topic, uint32_t queue_size,
+                 const TransportHints& transport_hints = TransportHints())
+  {
+    unsubscribe();
+
+    sub_ = it.subscribe(base_topic, queue_size, boost::bind(&SubscriberFilter::cb, this, _1),
+                        ros::VoidPtr(), transport_hints);
+  }
+
+  /**
+   * \brief Force immediate unsubscription of this subscriber from its topic
+   */
+  void unsubscribe()
+  {
+    sub_.shutdown();
+  }
+
+  std::string getTopic() const
+  {
+    return sub_.getTopic();
+  }
+
+  /**
+   * \brief Returns the number of publishers this subscriber is connected to.
+   */
+  uint32_t getNumPublishers() const
+  {
+    return sub_.getNumPublishers();
+  }
+
+  /**
+   * \brief Returns the name of the transport being used.
+   */
+  std::string getTransport() const
+  {
+    return sub_.getTransport();
+  }
+
+  /**
+   * \brief Returns the internal image_transport::Subscriber object.
+   */
+  const Subscriber& getSubscriber() const
+  {
+    return sub_;
+  }
+
+private:
+
+  void cb(const sensor_msgs::ImageConstPtr& m)
+  {
+    signalMessage(m);
+  }
+
+  Subscriber sub_;
+};
+
+}
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/subscriber_plugin.h b/rubis_ws/src/image_transport/include/image_transport/subscriber_plugin.h
new file mode 100644
index 00000000..4601dc9b
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/subscriber_plugin.h
@@ -0,0 +1,141 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_SUBSCRIBER_PLUGIN_H
+#define IMAGE_TRANSPORT_SUBSCRIBER_PLUGIN_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <boost/noncopyable.hpp>
+#include "image_transport/transport_hints.h"
+
+namespace image_transport {
+
+/**
+ * \brief Base class for plugins to Subscriber.
+ */
+class SubscriberPlugin : boost::noncopyable
+{
+public:
+  typedef boost::function<void(const sensor_msgs::ImageConstPtr&)> Callback;
+  
+  virtual ~SubscriberPlugin() {}
+
+  /**
+   * \brief Get a string identifier for the transport provided by
+   * this plugin.
+   */
+  virtual std::string getTransportName() const = 0;
+
+  /**
+   * \brief Subscribe to an image topic, version for arbitrary boost::function object.
+   */
+  void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                 const Callback& callback, const ros::VoidPtr& tracked_object = ros::VoidPtr(),
+                 const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribeImpl(nh, base_topic, queue_size, callback, tracked_object, transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to an image topic, version for bare function.
+   */
+  void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                 void(*fp)(const sensor_msgs::ImageConstPtr&),
+                 const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribe(nh, base_topic, queue_size,
+                     boost::function<void(const sensor_msgs::ImageConstPtr&)>(fp),
+                     ros::VoidPtr(), transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to an image topic, version for class member function with bare pointer.
+   */
+  template<class T>
+  void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                 void(T::*fp)(const sensor_msgs::ImageConstPtr&), T* obj,
+                 const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribe(nh, base_topic, queue_size, boost::bind(fp, obj, _1), ros::VoidPtr(), transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to an image topic, version for class member function with shared_ptr.
+   */
+  template<class T>
+  void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                 void(T::*fp)(const sensor_msgs::ImageConstPtr&),
+                 const boost::shared_ptr<T>& obj,
+                 const TransportHints& transport_hints = TransportHints())
+  {
+    return subscribe(nh, base_topic, queue_size, boost::bind(fp, obj.get(), _1), obj, transport_hints);
+  }
+
+  /**
+   * \brief Get the transport-specific communication topic.
+   */
+  virtual std::string getTopic() const = 0;
+
+  /**
+   * \brief Returns the number of publishers this subscriber is connected to.
+   */
+  virtual uint32_t getNumPublishers() const = 0;
+
+  /**
+   * \brief Unsubscribe the callback associated with this SubscriberPlugin.
+   */
+  virtual void shutdown() = 0;
+
+  /**
+   * \brief Return the lookup name of the SubscriberPlugin associated with a specific
+   * transport identifier.
+   */
+  static std::string getLookupName(const std::string& transport_type)
+  {
+    return "image_transport/" + transport_type + "_sub";
+  }
+
+protected:
+  /**
+   * \brief Subscribe to an image transport topic. Must be implemented by the subclass.
+   */
+  virtual void subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                             const Callback& callback, const ros::VoidPtr& tracked_object,
+                             const TransportHints& transport_hints) = 0;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/include/image_transport/transport_hints.h b/rubis_ws/src/image_transport/include/image_transport/transport_hints.h
new file mode 100644
index 00000000..1524d180
--- /dev/null
+++ b/rubis_ws/src/image_transport/include/image_transport/transport_hints.h
@@ -0,0 +1,94 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef IMAGE_TRANSPORT_TRANSPORT_HINTS_H
+#define IMAGE_TRANSPORT_TRANSPORT_HINTS_H
+
+#include <ros/ros.h>
+
+namespace image_transport {
+
+/**
+ * \brief Stores transport settings for an image topic subscription.
+ */
+class TransportHints
+{
+public:
+  /**
+   * \brief Constructor.
+   *
+   * The default transport can be overridden by setting a certain parameter to the
+   * name of the desired transport. By default this parameter is named "image_transport"
+   * in the node's local namespace. For consistency across ROS applications, the
+   * name of this parameter should not be changed without good reason.
+   *
+   * @param default_transport Preferred transport to use
+   * @param ros_hints Hints to pass through to ROS subscriptions
+   * @param parameter_nh Node handle to use when looking up the transport parameter.
+   * Defaults to the local namespace.
+   * @param parameter_name The name of the transport parameter
+   */
+  TransportHints(const std::string& default_transport = "raw",
+                 const ros::TransportHints& ros_hints = ros::TransportHints(),
+                 const ros::NodeHandle& parameter_nh = ros::NodeHandle("~"),
+                 const std::string& parameter_name = "image_transport")
+    : ros_hints_(ros_hints), parameter_nh_(parameter_nh)
+  {
+    parameter_nh_.param(parameter_name, transport_, default_transport);
+  }
+
+  const std::string& getTransport() const
+  {
+    return transport_;
+  }
+
+  const ros::TransportHints& getRosHints() const
+  {
+    return ros_hints_;
+  }
+
+  const ros::NodeHandle& getParameterNH() const
+  {
+    return parameter_nh_;
+  }
+  
+private:
+  std::string transport_;
+  ros::TransportHints ros_hints_;
+  ros::NodeHandle parameter_nh_;
+};
+
+} //namespace image_transport
+
+#endif
diff --git a/rubis_ws/src/image_transport/mainpage.dox b/rubis_ws/src/image_transport/mainpage.dox
new file mode 100644
index 00000000..2df6f5b5
--- /dev/null
+++ b/rubis_ws/src/image_transport/mainpage.dox
@@ -0,0 +1,45 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\section codeapi Code API
+When transporting images, you should use image_transport's classes as drop-in
+replacements for ros::Publisher and ros::Subscriber.
+- image_transport::ImageTransport - use this to create a Publisher or Subscriber
+- image_transport::Publisher - manage advertisements for an image topic, using all available transport options
+- image_transport::Subscriber - manage an Image subscription callback using a particular transport
+
+Camera drivers publish a "camera_info" sibling topic containing important metadata on how to
+interpret an image for vision applications. image_transport included helper classes to
+publish (image, info) message pairs and re-synchronize them on the client side:
+- image_transport::CameraPublisher - manage advertisements for camera images
+- image_transport::CameraSubscriber - manage a single subscription callback to synchronized image (using any transport) and CameraInfo topics
+
+For other synchronization or filtering needs, see the low-level filter class:
+- image_transport::SubscriberFilter - a wrapper for image_transport::Subscriber compatible with message_filters
+
+\subsection writing_plugin Writing a plugin
+If you are an advanced user implementing your own image transport option, you will need to
+implement these base-level interfaces:
+- image_transport::PublisherPlugin
+- image_transport::SubscriberPlugin
+
+In the common case that all communication between PublisherPlugin and SubscriberPlugin happens
+over a single ROS topic using a transport-specific message type, writing the plugins is vastly
+simplified by using these base classes instead:
+- image_transport::SimplePublisherPlugin - see image_transport::RawPublisher for a trivial example
+- image_transport::SimpleSubscriberPlugin - see image_transport::RawSubscriber for a trivial example
+
+\section rosapi ROS API
+
+\subsection pub_sub_rosapi Publishers and Subscribers
+
+Because they encapsulate complicated communication behavior, image_transport publisher
+and subscriber classes have a public ROS API as well as a code API. See the wiki
+documentation for details.
+
+Although image_transport::Publisher may publish many topics, in all code interfaces you should
+use only the name of the "base topic." The image transport classes will figure out the name of
+the dedicated ROS topic to use for the desired transport.
+
+*/
diff --git a/rubis_ws/src/image_transport/package.xml b/rubis_ws/src/image_transport/package.xml
new file mode 100644
index 00000000..5ad26c30
--- /dev/null
+++ b/rubis_ws/src/image_transport/package.xml
@@ -0,0 +1,41 @@
+<package>
+  <name>image_transport</name>
+  <version>1.12.0</version>
+  <description>
+
+     image_transport should always be used to subscribe to and publish images. It provides transparent
+     support for transporting images in low-bandwidth compressed formats. Examples (provided by separate
+     plugin packages) include JPEG/PNG compression and Theora streaming video.
+
+  </description>
+  <author>Patrick Mihelich</author>
+  <maintainer email="jack.oquin@gmail.com">Jack O'Quin</maintainer>
+  <maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/image_transport</url>
+  <url type="bugtracker">https://github.com/ros-perception/image_common/issues</url>
+  <url type="repository">https://github.com/ros-perception/image_common</url>
+
+  <export>
+    <image_transport plugin="${prefix}/default_plugins.xml" />
+  </export>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  
+  <build_depend>message_filters</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>rosconsole</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>rubis_lib</build_depend>
+
+  <run_depend>message_filters</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>rosconsole</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>rubis_lib</run_depend>
+</package>
diff --git a/rubis_ws/src/image_transport/src/camera_common.cpp b/rubis_ws/src/image_transport/src/camera_common.cpp
new file mode 100644
index 00000000..a09f764b
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/camera_common.cpp
@@ -0,0 +1,58 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/camera_common.h"
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/join.hpp>
+#include <vector>
+
+namespace image_transport {
+
+std::string getCameraInfoTopic(const std::string& base_topic)
+{
+  // Split into separate names
+  std::vector<std::string> names;
+  boost::algorithm::split(names, base_topic, boost::algorithm::is_any_of("/"),
+                          boost::algorithm::token_compress_on);
+  // Get rid of empty tokens from trailing slashes
+  while (names.back().empty())
+    names.pop_back();
+  // Replace image name with "camera_info"
+  names.back() = "camera_info";
+  // Join back together into topic name
+  return boost::algorithm::join(names, "/");
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/src/camera_publisher.cpp b/rubis_ws/src/image_transport/src/camera_publisher.cpp
new file mode 100644
index 00000000..232905a5
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/camera_publisher.cpp
@@ -0,0 +1,160 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/image_transport.h"
+#include "image_transport/camera_common.h"
+
+namespace image_transport {
+
+struct CameraPublisher::Impl
+{
+  Impl()
+    : unadvertised_(false)
+  {
+  }
+
+  ~Impl()
+  {
+    shutdown();
+  }
+
+  bool isValid() const
+  {
+    return !unadvertised_;
+  }
+  
+  void shutdown()
+  {
+    if (!unadvertised_) {
+      unadvertised_ = true;
+      image_pub_.shutdown();
+      info_pub_.shutdown();
+    }
+  }
+
+  Publisher image_pub_;
+  ros::Publisher info_pub_;
+  bool unadvertised_;
+  //double constructed_;
+};
+
+CameraPublisher::CameraPublisher(ImageTransport& image_it, ros::NodeHandle& info_nh,
+                                 const std::string& base_topic, uint32_t queue_size,
+                                 const SubscriberStatusCallback& image_connect_cb,
+                                 const SubscriberStatusCallback& image_disconnect_cb,
+                                 const ros::SubscriberStatusCallback& info_connect_cb,
+                                 const ros::SubscriberStatusCallback& info_disconnect_cb,
+                                 const ros::VoidPtr& tracked_object, bool latch)
+  : impl_(new Impl)
+{
+  // Explicitly resolve name here so we compute the correct CameraInfo topic when the
+  // image topic is remapped (#4539).
+  std::string image_topic = info_nh.resolveName(base_topic);
+  std::string info_topic = getCameraInfoTopic(image_topic);
+
+  impl_->image_pub_ = image_it.advertise(image_topic, queue_size, image_connect_cb,
+                                         image_disconnect_cb, tracked_object, latch);
+  impl_->info_pub_ = info_nh.advertise<sensor_msgs::CameraInfo>(info_topic, queue_size, info_connect_cb,
+                                                                info_disconnect_cb, tracked_object, latch);
+}
+
+uint32_t CameraPublisher::getNumSubscribers() const
+{
+  if (impl_ && impl_->isValid())
+    return std::max(impl_->image_pub_.getNumSubscribers(), impl_->info_pub_.getNumSubscribers());
+  return 0;
+}
+
+std::string CameraPublisher::getTopic() const
+{
+  if (impl_) return impl_->image_pub_.getTopic();
+  return std::string();
+}
+
+std::string CameraPublisher::getInfoTopic() const
+{
+  if (impl_) return impl_->info_pub_.getTopic();
+  return std::string();
+}
+
+void CameraPublisher::publish(const sensor_msgs::Image& image, const sensor_msgs::CameraInfo& info) const
+{
+  if (!impl_ || !impl_->isValid()) {
+    ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
+    return;
+  }
+  
+  impl_->image_pub_.publish(image);
+  impl_->info_pub_.publish(info);
+}
+
+void CameraPublisher::publish(const sensor_msgs::ImageConstPtr& image,
+                              const sensor_msgs::CameraInfoConstPtr& info) const
+{
+  if (!impl_ || !impl_->isValid()) {
+    ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
+    return;
+  }
+  
+  impl_->image_pub_.publish(image);
+  impl_->info_pub_.publish(info);
+}
+
+void CameraPublisher::publish(sensor_msgs::Image& image, sensor_msgs::CameraInfo& info,
+                              ros::Time stamp) const
+{
+  if (!impl_ || !impl_->isValid()) {
+    ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
+    return;
+  }
+  
+  image.header.stamp = stamp;
+  info.header.stamp = stamp;
+  publish(image, info);
+}
+
+void CameraPublisher::shutdown()
+{
+  if (impl_) {
+    impl_->shutdown();
+    impl_.reset();
+  }
+}
+
+CameraPublisher::operator void*() const
+{
+  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/src/camera_subscriber.cpp b/rubis_ws/src/image_transport/src/camera_subscriber.cpp
new file mode 100644
index 00000000..a91a14ea
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/camera_subscriber.cpp
@@ -0,0 +1,160 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/camera_subscriber.h"
+#include "image_transport/subscriber_filter.h"
+#include "image_transport/camera_common.h"
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+
+inline void increment(int* value)
+{
+  ++(*value);
+}
+
+namespace image_transport {
+
+struct CameraSubscriber::Impl
+{
+  Impl(uint32_t queue_size)
+    : sync_(queue_size),
+      unsubscribed_(false),
+      image_received_(0), info_received_(0), both_received_(0)
+  {}
+
+  ~Impl()
+  {
+    shutdown();
+  }
+
+  bool isValid() const
+  {
+    return !unsubscribed_;
+  }
+  
+  void shutdown()
+  {
+    if (!unsubscribed_) {
+      unsubscribed_ = true;
+      image_sub_.unsubscribe();
+      info_sub_.unsubscribe();
+    }
+  }
+
+  void checkImagesSynchronized()
+  {
+    int threshold = 3 * both_received_;
+    if (image_received_ > threshold || info_received_ > threshold) {
+      ROS_WARN_NAMED("sync", // Can suppress ros.image_transport.sync independent of anything else
+                     "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. "
+                     "In the last 10s:\n"
+                     "\tImage messages received:      %d\n"
+                     "\tCameraInfo messages received: %d\n"
+                     "\tSynchronized pairs:           %d",
+                     image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(),
+                     image_received_, info_received_, both_received_);
+    }
+    image_received_ = info_received_ = both_received_ = 0;
+  }
+  
+  SubscriberFilter image_sub_;
+  message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+  message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
+  bool unsubscribed_;
+  // For detecting when the topics aren't synchronized
+  ros::WallTimer check_synced_timer_;
+  int image_received_, info_received_, both_received_;
+};
+
+CameraSubscriber::CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh,
+                                   const std::string& base_topic, uint32_t queue_size,
+                                   const Callback& callback, const ros::VoidPtr& tracked_object,
+                                   const TransportHints& transport_hints)
+  : impl_(new Impl(queue_size))
+{
+  // Must explicitly remap the image topic since we then do some string manipulation on it
+  // to figure out the sibling camera_info topic.
+  std::string image_topic = info_nh.resolveName(base_topic);
+  std::string info_topic = getCameraInfoTopic(image_topic);
+  impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints);
+  impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints());
+  impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
+  // need for Boost.Bind here is kind of broken
+  impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
+
+  // Complain every 10s if it appears that the image and info topics are not synchronized
+  impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_));
+  impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_));
+  impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_));
+  impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0),
+                                                       boost::bind(&Impl::checkImagesSynchronized, impl_.get()));
+}
+
+std::string CameraSubscriber::getTopic() const
+{
+  if (impl_) return impl_->image_sub_.getTopic();
+  return std::string();
+}
+
+std::string CameraSubscriber::getInfoTopic() const
+{
+  if (impl_) return impl_->info_sub_.getTopic();
+  return std::string();
+}
+
+uint32_t CameraSubscriber::getNumPublishers() const
+{
+  /// @todo Fix this when message_filters::Subscriber has getNumPublishers()
+  //if (impl_) return std::max(impl_->image_sub_.getNumPublishers(), impl_->info_sub_.getNumPublishers());
+  if (impl_) return impl_->image_sub_.getNumPublishers();
+  return 0;
+}
+
+std::string CameraSubscriber::getTransport() const
+{
+  if (impl_) return impl_->image_sub_.getTransport();
+  return std::string();
+}
+
+void CameraSubscriber::shutdown()
+{
+  if (impl_) impl_->shutdown();
+}
+
+CameraSubscriber::operator void*() const
+{
+  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/src/image_transport.cpp b/rubis_ws/src/image_transport/src/image_transport.cpp
new file mode 100644
index 00000000..88ba1254
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/image_transport.cpp
@@ -0,0 +1,148 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/image_transport.h"
+#include "image_transport/publisher_plugin.h"
+#include "image_transport/subscriber_plugin.h"
+#include <pluginlib/class_loader.h>
+#include <boost/make_shared.hpp>
+#include <boost/foreach.hpp>
+#include <boost/algorithm/string/erase.hpp>
+
+namespace image_transport {
+
+struct ImageTransport::Impl
+{
+  ros::NodeHandle nh_;
+  PubLoaderPtr pub_loader_;
+  SubLoaderPtr sub_loader_;
+  
+  Impl(const ros::NodeHandle& nh)
+    : nh_(nh),
+      pub_loader_( boost::make_shared<PubLoader>("image_transport", "image_transport::PublisherPlugin") ),
+      sub_loader_( boost::make_shared<SubLoader>("image_transport", "image_transport::SubscriberPlugin") )
+  {
+  }
+};
+
+ImageTransport::ImageTransport(const ros::NodeHandle& nh)
+  : impl_(new Impl(nh))
+{
+}
+
+ImageTransport::~ImageTransport()
+{
+}
+
+Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size, bool latch)
+{
+  return advertise(base_topic, queue_size, SubscriberStatusCallback(),
+                   SubscriberStatusCallback(), ros::VoidPtr(), latch);
+}
+
+Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size,
+                                    const SubscriberStatusCallback& connect_cb,
+                                    const SubscriberStatusCallback& disconnect_cb,
+                                    const ros::VoidPtr& tracked_object, bool latch)
+{
+  return Publisher(impl_->nh_, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch, impl_->pub_loader_);
+}
+
+Subscriber ImageTransport::subscribe(const std::string& base_topic, uint32_t queue_size,
+                                     const boost::function<void(const sensor_msgs::ImageConstPtr&)>& callback,
+                                     const ros::VoidPtr& tracked_object, const TransportHints& transport_hints)
+{
+  return Subscriber(impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints, impl_->sub_loader_);
+}
+
+CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size, bool latch)
+{
+  return advertiseCamera(base_topic, queue_size,
+                         SubscriberStatusCallback(), SubscriberStatusCallback(),
+                         ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
+                         ros::VoidPtr(), latch);
+}
+
+CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size,
+                                                const SubscriberStatusCallback& image_connect_cb,
+                                                const SubscriberStatusCallback& image_disconnect_cb,
+                                                const ros::SubscriberStatusCallback& info_connect_cb,
+                                                const ros::SubscriberStatusCallback& info_disconnect_cb,
+                                                const ros::VoidPtr& tracked_object, bool latch)
+{
+  return CameraPublisher(*this, impl_->nh_, base_topic, queue_size, image_connect_cb, image_disconnect_cb,
+                         info_connect_cb, info_disconnect_cb, tracked_object, latch);
+}
+
+CameraSubscriber ImageTransport::subscribeCamera(const std::string& base_topic, uint32_t queue_size,
+                                                 const CameraSubscriber::Callback& callback,
+                                                 const ros::VoidPtr& tracked_object,
+                                                 const TransportHints& transport_hints)
+{
+  return CameraSubscriber(*this, impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints);
+}
+
+std::vector<std::string> ImageTransport::getDeclaredTransports() const
+{
+  std::vector<std::string> transports = impl_->sub_loader_->getDeclaredClasses();
+  // Remove the "_sub" at the end of each class name.
+  BOOST_FOREACH(std::string& transport, transports) {
+    transport = boost::erase_last_copy(transport, "_sub");
+  }
+  return transports;
+}
+
+std::vector<std::string> ImageTransport::getLoadableTransports() const
+{
+  std::vector<std::string> loadableTransports;
+
+  BOOST_FOREACH( const std::string& transportPlugin, impl_->sub_loader_->getDeclaredClasses() )
+  {
+    // If the plugin loads without throwing an exception, add its
+    // transport name to the list of valid plugins, otherwise ignore
+    // it.
+    try
+    {
+      boost::shared_ptr<image_transport::SubscriberPlugin> sub = impl_->sub_loader_->createInstance(transportPlugin);
+      loadableTransports.push_back(boost::erase_last_copy(transportPlugin, "_sub")); // Remove the "_sub" at the end of each class name.
+    }
+    catch (const pluginlib::LibraryLoadException& e) {}
+    catch (const pluginlib::CreateClassException& e) {}
+  }
+
+  return loadableTransports;
+
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/src/list_transports.cpp b/rubis_ws/src/image_transport/src/list_transports.cpp
new file mode 100644
index 00000000..dba88127
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/list_transports.cpp
@@ -0,0 +1,141 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/publisher_plugin.h"
+#include "image_transport/subscriber_plugin.h"
+#include <pluginlib/class_loader.h>
+#include <boost/foreach.hpp>
+#include <boost/algorithm/string/erase.hpp>
+#include <map>
+
+using namespace image_transport;
+using namespace pluginlib;
+
+enum PluginStatus {SUCCESS, CREATE_FAILURE, LIB_LOAD_FAILURE, DOES_NOT_EXIST};
+
+/// \cond
+struct TransportDesc
+{
+  TransportDesc()
+    : pub_status(DOES_NOT_EXIST), sub_status(DOES_NOT_EXIST)
+  {}
+
+  std::string package_name;
+  std::string pub_name;
+  PluginStatus pub_status;
+  std::string sub_name;
+  PluginStatus sub_status;
+};
+/// \endcond
+
+int main(int argc, char** argv)
+{
+  ClassLoader<PublisherPlugin> pub_loader("image_transport", "image_transport::PublisherPlugin");
+  ClassLoader<SubscriberPlugin> sub_loader("image_transport", "image_transport::SubscriberPlugin");
+  typedef std::map<std::string, TransportDesc> StatusMap;
+  StatusMap transports;
+
+  BOOST_FOREACH(const std::string& lookup_name, pub_loader.getDeclaredClasses()) {
+    std::string transport_name = boost::erase_last_copy(lookup_name, "_pub");
+    transports[transport_name].pub_name = lookup_name;
+    transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name);
+    try {
+      boost::shared_ptr<PublisherPlugin> pub = pub_loader.createInstance(lookup_name);
+      transports[transport_name].pub_status = SUCCESS;
+    }
+    catch (const LibraryLoadException& e) {
+      transports[transport_name].pub_status = LIB_LOAD_FAILURE;
+    }
+    catch (const CreateClassException& e) {
+      transports[transport_name].pub_status = CREATE_FAILURE;
+    }
+  }
+
+  BOOST_FOREACH(const std::string& lookup_name, sub_loader.getDeclaredClasses()) {
+    std::string transport_name = boost::erase_last_copy(lookup_name, "_sub");
+    transports[transport_name].sub_name = lookup_name;
+    transports[transport_name].package_name = sub_loader.getClassPackage(lookup_name);
+    try {
+      boost::shared_ptr<SubscriberPlugin> sub = sub_loader.createInstance(lookup_name);
+      transports[transport_name].sub_status = SUCCESS;
+    }
+    catch (const LibraryLoadException& e) {
+      transports[transport_name].sub_status = LIB_LOAD_FAILURE;
+    }
+    catch (const CreateClassException& e) {
+      transports[transport_name].sub_status = CREATE_FAILURE;
+    }
+  }
+
+  bool problem_package = false;
+  printf("Declared transports:\n");
+  BOOST_FOREACH(const StatusMap::value_type& value, transports) {
+    const TransportDesc& td = value.second;
+    printf("%s", value.first.c_str());
+    if ((td.pub_status == CREATE_FAILURE || td.pub_status == LIB_LOAD_FAILURE) ||
+        (td.sub_status == CREATE_FAILURE || td.sub_status == LIB_LOAD_FAILURE)) {
+      printf(" (*): Not available. Try 'catkin_make --pkg %s'.", td.package_name.c_str());
+      problem_package = true;
+    }
+    printf("\n");
+  }
+#if 0
+  if (problem_package)
+    printf("(*) \n");
+#endif
+
+  printf("\nDetails:\n");
+  BOOST_FOREACH(const StatusMap::value_type& value, transports) {
+    const TransportDesc& td = value.second;
+    printf("----------\n");
+    printf("\"%s\"\n", value.first.c_str());
+    if (td.pub_status == CREATE_FAILURE || td.sub_status == CREATE_FAILURE) {
+      printf("*** Plugins are built, but could not be loaded. The package may need to be rebuilt or may not be compatible with this release of image_common. ***\n");
+    }
+    else if (td.pub_status == LIB_LOAD_FAILURE || td.sub_status == LIB_LOAD_FAILURE) {
+      printf("*** Plugins are not built. ***\n");
+    }
+    printf(" - Provided by package: %s\n", td.package_name.c_str());
+    if (td.pub_status == DOES_NOT_EXIST)
+      printf(" - No publisher provided\n");
+    else
+      printf(" - Publisher: %s\n", pub_loader.getClassDescription(td.pub_name).c_str());
+    if (td.sub_status == DOES_NOT_EXIST)
+      printf(" - No subscriber provided\n");
+    else
+      printf(" - Subscriber: %s\n", sub_loader.getClassDescription(td.sub_name).c_str());
+  }
+  
+  return 0;
+}
diff --git a/rubis_ws/src/image_transport/src/manifest.cpp b/rubis_ws/src/image_transport/src/manifest.cpp
new file mode 100644
index 00000000..40a8acdd
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/manifest.cpp
@@ -0,0 +1,41 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <pluginlib/class_list_macros.h>
+#include "image_transport/raw_publisher.h"
+#include "image_transport/raw_subscriber.h"
+
+PLUGINLIB_EXPORT_CLASS( image_transport::RawPublisher, image_transport::PublisherPlugin)
+
+PLUGINLIB_EXPORT_CLASS( image_transport::RawSubscriber, image_transport::SubscriberPlugin)
diff --git a/rubis_ws/src/image_transport/src/publisher.cpp b/rubis_ws/src/image_transport/src/publisher.cpp
new file mode 100644
index 00000000..ebf44bd7
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/publisher.cpp
@@ -0,0 +1,217 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/publisher.h"
+#include "image_transport/publisher_plugin.h"
+#include <pluginlib/class_loader.h>
+#include <boost/foreach.hpp>
+#include <boost/algorithm/string/erase.hpp>
+
+namespace image_transport {
+
+struct Publisher::Impl
+{
+  Impl()
+    : unadvertised_(false)
+  {
+  }
+
+  ~Impl()
+  {
+    shutdown();
+  }
+
+  uint32_t getNumSubscribers() const
+  {
+    uint32_t count = 0;
+    BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, publishers_)
+      count += pub->getNumSubscribers();
+    return count;
+  }
+
+  std::string getTopic() const
+  {
+    return base_topic_;
+  }
+
+  bool isValid() const
+  {
+    return !unadvertised_;
+  }
+  
+  void shutdown()
+  {
+    if (!unadvertised_) {
+      unadvertised_ = true;
+      BOOST_FOREACH(boost::shared_ptr<PublisherPlugin>& pub, publishers_)
+        pub->shutdown();
+      publishers_.clear();
+    }
+  }
+
+  void subscriberCB(const SingleSubscriberPublisher& plugin_pub,
+                    const SubscriberStatusCallback& user_cb)
+  {
+    SingleSubscriberPublisher ssp(plugin_pub.getSubscriberName(), getTopic(),
+                                  boost::bind(&Publisher::Impl::getNumSubscribers, this),
+                                  plugin_pub.publish_fn_);
+    user_cb(ssp);
+  }
+  
+  std::string base_topic_;
+  PubLoaderPtr loader_;
+  std::vector<boost::shared_ptr<PublisherPlugin> > publishers_;
+  bool unadvertised_;
+  //double constructed_;
+};
+
+
+Publisher::Publisher(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                     const SubscriberStatusCallback& connect_cb,
+                     const SubscriberStatusCallback& disconnect_cb,
+                     const ros::VoidPtr& tracked_object, bool latch,
+                     const PubLoaderPtr& loader)
+  : impl_(new Impl)
+{
+  // Resolve the name explicitly because otherwise the compressed topics don't remap
+  // properly (#3652).
+  impl_->base_topic_ = nh.resolveName(base_topic);
+  impl_->loader_ = loader;
+
+  std::vector<std::string> blacklist_vec;
+  std::set<std::string> blacklist;
+  nh.getParam(impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec);
+  for (size_t i = 0; i < blacklist_vec.size(); ++i)
+  {
+    blacklist.insert(blacklist_vec[i]);
+  }
+
+  BOOST_FOREACH(const std::string& lookup_name, loader->getDeclaredClasses()) {
+    const std::string transport_name = boost::erase_last_copy(lookup_name, "_pub");
+    if (blacklist.count(transport_name))
+    {
+      continue;
+    }
+
+    try {
+      boost::shared_ptr<PublisherPlugin> pub = loader->createInstance(lookup_name);
+      impl_->publishers_.push_back(pub);
+      pub->advertise(nh, impl_->base_topic_, queue_size, rebindCB(connect_cb),
+                     rebindCB(disconnect_cb), tracked_object, latch);
+    }
+    catch (const std::runtime_error& e) {
+      ROS_DEBUG("Failed to load plugin %s, error string: %s",
+                lookup_name.c_str(), e.what());
+    }
+  }
+
+  if (impl_->publishers_.empty())
+    throw Exception("No plugins found! Does `rospack plugins --attrib=plugin "
+                    "image_transport` find any packages?");
+}
+
+uint32_t Publisher::getNumSubscribers() const
+{
+  if (impl_ && impl_->isValid()) return impl_->getNumSubscribers();
+  return 0;
+}
+
+std::string Publisher::getTopic() const
+{
+  if (impl_) return impl_->getTopic();
+  return std::string();
+}
+
+void Publisher::publish(const sensor_msgs::Image& message) const
+{
+  if (!impl_ || !impl_->isValid()) {
+    ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher");
+    return;
+  }
+  
+  BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, impl_->publishers_) {
+    if (pub->getNumSubscribers() > 0)
+      pub->publish(message);
+  }
+}
+
+void Publisher::publish(const sensor_msgs::ImageConstPtr& message) const
+{
+  if (!impl_ || !impl_->isValid()) {
+    ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher");
+    return;
+  }
+  
+  BOOST_FOREACH(const boost::shared_ptr<PublisherPlugin>& pub, impl_->publishers_) {
+    if (pub->getNumSubscribers() > 0)
+      pub->publish(message);
+  }
+}
+
+void Publisher::shutdown()
+{
+  if (impl_) {
+    impl_->shutdown();
+    impl_.reset();
+  }
+}
+
+Publisher::operator void*() const
+{
+  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
+}
+
+void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr,
+                                 const SingleSubscriberPublisher& plugin_pub,
+                                 const SubscriberStatusCallback& user_cb)
+{
+  if (ImplPtr impl = impl_wptr.lock())
+    impl->subscriberCB(plugin_pub, user_cb);
+}
+
+SubscriberStatusCallback Publisher::rebindCB(const SubscriberStatusCallback& user_cb)
+{
+  // Note: the subscriber callback must be bound to the internal Impl object, not
+  // 'this'. Due to copying behavior the Impl object may outlive the original Publisher
+  // instance. But it should not outlive the last Publisher, so we use a weak_ptr.
+  if (user_cb)
+  {
+    ImplWPtr impl_wptr(impl_);
+    return boost::bind(&Publisher::weakSubscriberCb, impl_wptr, _1, user_cb);
+  }
+  else
+    return SubscriberStatusCallback();
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/src/raw_publisher.cpp b/rubis_ws/src/image_transport/src/raw_publisher.cpp
new file mode 100644
index 00000000..c1eda750
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/raw_publisher.cpp
@@ -0,0 +1,140 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <image_transport/raw_publisher.h>
+#include <ros/static_assert.h>
+#include <sensor_msgs/Image.h>
+
+/** The code in the following namespace copies a lof of code from cv_bridge
+ * It does not depend on cv_bridge to not depend on OpenCV
+ * It re-defines a CvImage so that we can publish that object and not a
+ * sensor_msgs::Image, which requires a memory copy
+ */
+
+class ImageTransportImage
+{
+public:
+  sensor_msgs::Image image_; //!< ROS header
+  const uint8_t* data_;           //!< Image data for use with OpenCV
+
+  /**
+   * \brief Empty constructor.
+   */
+  ImageTransportImage() {}
+
+  /**
+   * \brief Constructor.
+   */
+  ImageTransportImage(const sensor_msgs::Image& image, const uint8_t* data)
+    : image_(image), data_(data)
+  {
+  }
+};
+
+/// @cond DOXYGEN_IGNORE
+namespace ros {
+
+namespace message_traits {
+
+template<> struct MD5Sum<ImageTransportImage>
+{
+  static const char* value() { return MD5Sum<sensor_msgs::Image>::value(); }
+  static const char* value(const ImageTransportImage&) { return value(); }
+
+  static const uint64_t static_value1 = MD5Sum<sensor_msgs::Image>::static_value1;
+  static const uint64_t static_value2 = MD5Sum<sensor_msgs::Image>::static_value2;
+  
+  // If the definition of sensor_msgs/Image changes, we'll get a compile error here.
+  ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value1 == 0x060021388200f6f0ULL);
+  ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value2 == 0xf447d0fcd9c64743ULL);
+};
+
+template<> struct DataType<ImageTransportImage>
+{
+  static const char* value() { return DataType<sensor_msgs::Image>::value(); }
+  static const char* value(const ImageTransportImage&) { return value(); }
+};
+
+template<> struct Definition<ImageTransportImage>
+{
+  static const char* value() { return Definition<sensor_msgs::Image>::value(); }
+  static const char* value(const ImageTransportImage&) { return value(); }
+};
+
+template<> struct HasHeader<ImageTransportImage> : TrueType {};
+
+} // namespace ros::message_traits
+
+namespace serialization {
+
+template<> struct Serializer<ImageTransportImage>
+{
+  /// @todo Still ignoring endianness...
+  
+  template<typename Stream>
+  inline static void write(Stream& stream, const ImageTransportImage& m)
+  {
+    stream.next(m.image_.header);
+    stream.next((uint32_t)m.image_.height); // height
+    stream.next((uint32_t)m.image_.width); // width
+    stream.next(m.image_.encoding);
+    uint8_t is_bigendian = 0;
+    stream.next(is_bigendian);
+    stream.next((uint32_t)m.image_.step);
+    size_t data_size = m.image_.step*m.image_.height;
+    stream.next((uint32_t)data_size);
+    if (data_size > 0)
+      memcpy(stream.advance(data_size), m.data_, data_size);
+  }
+
+  inline static uint32_t serializedLength(const ImageTransportImage& m)
+  {
+    size_t data_size = m.image_.step*m.image_.height;
+    return serializationLength(m.image_.header) + serializationLength(m.image_.encoding) + 17 + data_size;
+  }
+};
+
+} // namespace ros::serialization
+
+} // namespace ros
+
+
+namespace image_transport {
+
+void RawPublisher::publish(const sensor_msgs::Image& message, const uint8_t* data) const
+{
+   getPublisher().publish(ImageTransportImage(message, data));
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/src/republish.cpp b/rubis_ws/src/image_transport/src/republish.cpp
new file mode 100644
index 00000000..0e890c99
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/republish.cpp
@@ -0,0 +1,96 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/image_transport.h"
+#include "image_transport/publisher_plugin.h"
+#include <pluginlib/class_loader.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "image_republisher", ros::init_options::AnonymousName);
+  if (argc < 2) {
+    printf("Usage: %s in_transport in:=<in_base_topic> [out_transport] out:=<out_base_topic>\n", argv[0]);
+    return 0;
+  }
+  ros::NodeHandle nh;
+  std::string in_topic  = nh.resolveName("in");
+  std::string in_transport = argv[1];
+  std::string out_topic = nh.resolveName("out");
+
+  image_transport::ImageTransport it(nh);
+  image_transport::Subscriber sub;
+
+  // scheduling
+  ros::NodeHandle pnh("~");
+  int task_scheduling_flag = 0;
+  int task_profiling_flag = 0;
+  std::string task_response_time_filename;
+  int rate = 0;
+  double task_minimum_inter_release_time = 0;
+  double task_execution_time = 0;
+  double task_relative_deadline = 0;
+
+  if (argc < 3) {
+    // Use all available transports for output
+    image_transport::Publisher pub = it.advertise(out_topic, 1);
+    
+    // Use Publisher::publish as the subscriber callback
+    typedef void (image_transport::Publisher::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const;
+    PublishMemFn pub_mem_fn = &image_transport::Publisher::publish;
+    sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, _1), ros::VoidPtr(), in_transport);
+
+    ros::spin();
+  }
+  else {
+    // Use one specific transport for output
+    std::string out_transport = argv[2];
+
+    // Load transport plugin
+    typedef image_transport::PublisherPlugin Plugin;
+    pluginlib::ClassLoader<Plugin> loader("image_transport", "image_transport::PublisherPlugin");
+    std::string lookup_name = Plugin::getLookupName(out_transport);
+    boost::shared_ptr<Plugin> pub( loader.createInstance(lookup_name) );
+    pub->advertise(nh, out_topic, 1, image_transport::SubscriberStatusCallback(),
+                   image_transport::SubscriberStatusCallback(), ros::VoidPtr(), false);
+
+    // Use PublisherPlugin::publish as the subscriber callback
+    typedef void (Plugin::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const;
+    PublishMemFn pub_mem_fn = &Plugin::publish;
+    sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), _1), pub, in_transport);
+
+    ros::spin();
+  }
+
+  return 0;
+}
diff --git a/rubis_ws/src/image_transport/src/single_subscriber_publisher.cpp b/rubis_ws/src/image_transport/src/single_subscriber_publisher.cpp
new file mode 100644
index 00000000..10fc2943
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/single_subscriber_publisher.cpp
@@ -0,0 +1,74 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/single_subscriber_publisher.h"
+#include "image_transport/publisher.h"
+
+namespace image_transport {
+
+SingleSubscriberPublisher::SingleSubscriberPublisher(const std::string& caller_id, const std::string& topic,
+                                                     const GetNumSubscribersFn& num_subscribers_fn,
+                                                     const PublishFn& publish_fn)
+  : caller_id_(caller_id), topic_(topic),
+    num_subscribers_fn_(num_subscribers_fn),
+    publish_fn_(publish_fn)
+{
+}
+
+std::string SingleSubscriberPublisher::getSubscriberName() const
+{
+  return caller_id_;
+}
+
+std::string SingleSubscriberPublisher::getTopic() const
+{
+  return topic_;
+}
+
+uint32_t SingleSubscriberPublisher::getNumSubscribers() const
+{
+  return num_subscribers_fn_();
+}
+
+void SingleSubscriberPublisher::publish(const sensor_msgs::Image& message) const
+{
+  publish_fn_(message);
+}
+
+void SingleSubscriberPublisher::publish(const sensor_msgs::ImageConstPtr& message) const
+{
+  publish_fn_(*message);
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/src/subscriber.cpp b/rubis_ws/src/image_transport/src/subscriber.cpp
new file mode 100644
index 00000000..b7ef654b
--- /dev/null
+++ b/rubis_ws/src/image_transport/src/subscriber.cpp
@@ -0,0 +1,142 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "image_transport/subscriber.h"
+#include "image_transport/subscriber_plugin.h"
+#include <ros/names.h>
+#include <pluginlib/class_loader.h>
+#include <boost/scoped_ptr.hpp>
+
+namespace image_transport {
+
+struct Subscriber::Impl
+{
+  Impl()
+    : unsubscribed_(false)
+  {
+  }
+
+  ~Impl()
+  {
+    shutdown();
+  }
+
+  bool isValid() const
+  {
+    return !unsubscribed_;
+  }
+
+  void shutdown()
+  {
+    if (!unsubscribed_) {
+      unsubscribed_ = true;
+      if (subscriber_)
+        subscriber_->shutdown();
+    }
+  }
+  
+  SubLoaderPtr loader_;
+  boost::shared_ptr<SubscriberPlugin> subscriber_;
+  bool unsubscribed_;
+  //double constructed_;
+};
+
+Subscriber::Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
+                       const boost::function<void(const sensor_msgs::ImageConstPtr&)>& callback,
+                       const ros::VoidPtr& tracked_object, const TransportHints& transport_hints,
+                       const SubLoaderPtr& loader)
+  : impl_(new Impl)
+{
+  // Load the plugin for the chosen transport.
+  std::string lookup_name = SubscriberPlugin::getLookupName(transport_hints.getTransport());
+  try {
+    impl_->subscriber_ = loader->createInstance(lookup_name);
+  }
+  catch (pluginlib::PluginlibException& e) {
+    throw TransportLoadException(transport_hints.getTransport(), e.what());
+  }
+  // Hang on to the class loader so our shared library doesn't disappear from under us.
+  impl_->loader_ = loader;
+
+  // Try to catch if user passed in a transport-specific topic as base_topic.
+  std::string clean_topic = ros::names::clean(base_topic);
+  size_t found = clean_topic.rfind('/');
+  if (found != std::string::npos) {
+    std::string transport = clean_topic.substr(found+1);
+    std::string plugin_name = SubscriberPlugin::getLookupName(transport);
+    std::vector<std::string> plugins = loader->getDeclaredClasses();
+    if (std::find(plugins.begin(), plugins.end(), plugin_name) != plugins.end()) {
+      std::string real_base_topic = clean_topic.substr(0, found);
+      ROS_WARN("[image_transport] It looks like you are trying to subscribe directly to a "
+               "transport-specific image topic '%s', in which case you will likely get a connection "
+               "error. Try subscribing to the base topic '%s' instead with parameter ~image_transport "
+               "set to '%s' (on the command line, _image_transport:=%s). "
+               "See http://ros.org/wiki/image_transport for details.",
+               clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str());
+    }
+  }
+
+  // Tell plugin to subscribe.
+  impl_->subscriber_->subscribe(nh, base_topic, queue_size, callback, tracked_object, transport_hints);
+}
+
+std::string Subscriber::getTopic() const
+{
+  if (impl_) return impl_->subscriber_->getTopic();
+  return std::string();
+}
+
+uint32_t Subscriber::getNumPublishers() const
+{
+  if (impl_) return impl_->subscriber_->getNumPublishers();
+  return 0;
+}
+
+std::string Subscriber::getTransport() const
+{
+  if (impl_) return impl_->subscriber_->getTransportName();
+  return std::string();
+}
+
+void Subscriber::shutdown()
+{
+  if (impl_) impl_->shutdown();
+}
+
+Subscriber::operator void*() const
+{
+  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
+}
+
+} //namespace image_transport
diff --git a/rubis_ws/src/image_transport/tutorial/CMakeLists.txt b/rubis_ws/src/image_transport/tutorial/CMakeLists.txt
new file mode 100644
index 00000000..191edc45
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/CMakeLists.txt
@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(image_transport_tutorial)
+
+find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport message_generation sensor_msgs)
+
+# add the resized image message
+add_message_files(DIRECTORY msg
+   FILES ResizedImage.msg
+)
+generate_messages(DEPENDENCIES sensor_msgs)
+
+catkin_package(CATKIN_DEPENDS cv_bridge image_transport message_runtime sensor_msgs)
+
+find_package(OpenCV)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+
+# add the publisher example
+add_executable(my_publisher src/my_publisher.cpp)
+add_dependencies(my_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(my_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+# add the subscriber example
+add_executable(my_subscriber src/my_subscriber.cpp)
+add_dependencies(my_subscriber ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(my_subscriber ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+# add the plugin example
+add_library(resized_publisher src/manifest.cpp src/resized_publisher.cpp src/resized_subscriber.cpp)
+add_dependencies(resized_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(resized_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+
+# Mark executables and/or libraries for installation
+install(TARGETS my_publisher my_subscriber resized_publisher
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES resized_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/rubis_ws/src/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h b/rubis_ws/src/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h
new file mode 100644
index 00000000..63ab3eb7
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h
@@ -0,0 +1,15 @@
+#include <image_transport/simple_publisher_plugin.h>
+#include <image_transport_tutorial/ResizedImage.h>
+
+class ResizedPublisher : public image_transport::SimplePublisherPlugin<image_transport_tutorial::ResizedImage>
+{
+public:
+  virtual std::string getTransportName() const
+  {
+    return "resized";
+  }
+
+protected:
+  virtual void publish(const sensor_msgs::Image& message,
+                       const PublishFn& publish_fn) const;
+};
diff --git a/rubis_ws/src/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h b/rubis_ws/src/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h
new file mode 100644
index 00000000..94b55f54
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h
@@ -0,0 +1,17 @@
+#include <image_transport/simple_subscriber_plugin.h>
+#include <image_transport_tutorial/ResizedImage.h>
+
+class ResizedSubscriber : public image_transport::SimpleSubscriberPlugin<image_transport_tutorial::ResizedImage>
+{
+public:
+  virtual ~ResizedSubscriber() {}
+
+  virtual std::string getTransportName() const
+  {
+    return "resized";
+  }
+
+protected:
+  virtual void internalCallback(const typename image_transport_tutorial::ResizedImage::ConstPtr& message,
+                                const Callback& user_cb);
+};
diff --git a/rubis_ws/src/image_transport/tutorial/msg/ResizedImage.msg b/rubis_ws/src/image_transport/tutorial/msg/ResizedImage.msg
new file mode 100644
index 00000000..d8c8fadb
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/msg/ResizedImage.msg
@@ -0,0 +1,3 @@
+uint32 original_height
+uint32 original_width
+sensor_msgs/Image image
diff --git a/rubis_ws/src/image_transport/tutorial/package.xml b/rubis_ws/src/image_transport/tutorial/package.xml
new file mode 100644
index 00000000..668cd0ac
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/package.xml
@@ -0,0 +1,26 @@
+<package>
+  <name>image_transport_tutorial</name>
+  <version>0.0.0</version>
+  <description>Tutorial for image_transport. This is useful for the tutorials at http://wiki.ros.org/image_transport/Tutorials/</description>
+  <author>Vincent Rabaud</author>
+  <maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
+  <license>Apache 2.0</license>
+
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>opencv2</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>opencv2</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <export>
+    <image_transport plugin="${prefix}/resized_plugins.xml"/>
+  </export>
+</package>
diff --git a/rubis_ws/src/image_transport/tutorial/resized_plugins.xml b/rubis_ws/src/image_transport/tutorial/resized_plugins.xml
new file mode 100644
index 00000000..bd2dff1a
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/resized_plugins.xml
@@ -0,0 +1,13 @@
+<library path="lib/libresized_image_transport">
+  <class name="resized_pub" type="ResizedPublisher" base_class_type="image_transport::PublisherPlugin">
+    <description>
+      This plugin publishes a decimated version of the image.
+    </description>
+  </class>
+
+  <class name="resized_sub" type="ResizedSubscriber" base_class_type="image_transport::SubscriberPlugin">
+    <description>
+      This plugin rescales a decimated image to its original size.
+    </description>
+  </class>
+</library>
diff --git a/rubis_ws/src/image_transport/tutorial/src/manifest.cpp b/rubis_ws/src/image_transport/tutorial/src/manifest.cpp
new file mode 100644
index 00000000..ab208194
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/src/manifest.cpp
@@ -0,0 +1,7 @@
+#include <pluginlib/class_list_macros.h>
+#include <image_transport_tutorial/resized_publisher.h>
+#include <image_transport_tutorial/resized_subscriber.h>
+
+PLUGINLIB_EXPORT_CLASS(ResizedPublisher, image_transport::PublisherPlugin)
+
+PLUGINLIB_EXPORT_CLASS(ResizedSubscriber, image_transport::SubscriberPlugin)
diff --git a/rubis_ws/src/image_transport/tutorial/src/my_publisher.cpp b/rubis_ws/src/image_transport/tutorial/src/my_publisher.cpp
new file mode 100644
index 00000000..acb45ed6
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/src/my_publisher.cpp
@@ -0,0 +1,23 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "image_publisher");
+  ros::NodeHandle nh;
+  image_transport::ImageTransport it(nh);
+  image_transport::Publisher pub = it.advertise("camera/image", 1);
+
+  cv::Mat image = cv::imread(argv[1], cv::IMREAD_COLOR);
+  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
+
+  ros::Rate loop_rate(5);
+  while (nh.ok()) {
+    pub.publish(msg);
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+}
+
diff --git a/rubis_ws/src/image_transport/tutorial/src/my_subscriber.cpp b/rubis_ws/src/image_transport/tutorial/src/my_subscriber.cpp
new file mode 100644
index 00000000..b6310f48
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/src/my_subscriber.cpp
@@ -0,0 +1,29 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+{
+  try
+  {
+    cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
+    cv::waitKey(10);
+  }
+  catch (cv_bridge::Exception& e)
+  {
+    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
+  }
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "image_listener");
+  ros::NodeHandle nh;
+  cv::namedWindow("view");
+  cv::startWindowThread();
+  image_transport::ImageTransport it(nh);
+  image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
+  ros::spin();
+  cv::destroyWindow("view");
+}
diff --git a/rubis_ws/src/image_transport/tutorial/src/resized_publisher.cpp b/rubis_ws/src/image_transport/tutorial/src/resized_publisher.cpp
new file mode 100644
index 00000000..178f4821
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/src/resized_publisher.cpp
@@ -0,0 +1,37 @@
+#include <image_transport_tutorial/resized_publisher.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+void ResizedPublisher::publish(const sensor_msgs::Image& message,
+                               const PublishFn& publish_fn) const
+{
+  cv::Mat cv_image;
+  boost::shared_ptr<void const> tracked_object;
+  try
+  {
+    cv_image = cv_bridge::toCvShare(message, tracked_object, message.encoding)->image;
+  }
+  catch (cv::Exception &e)
+  {
+    ROS_ERROR("Could not convert from '%s' to '%s'.", message.encoding.c_str(), message.encoding.c_str());
+    return;
+  }
+
+  // Retrieve subsampling factor from the parameter server
+  double subsampling_factor;
+  std::string param_name;
+  nh().param<double>("resized_image_transport_subsampling_factor", subsampling_factor, 2.0);
+
+  // Rescale image
+  int new_width = cv_image.cols / subsampling_factor + 0.5;
+  int new_height = cv_image.rows / subsampling_factor + 0.5;
+  cv::Mat buffer;
+  cv::resize(cv_image, buffer, cv::Size(new_width, new_height));
+
+  // Set up ResizedImage and publish
+  image_transport_tutorial::ResizedImage resized_image;
+  resized_image.original_height = cv_image.rows;
+  resized_image.original_width = cv_image.cols;
+  resized_image.image = *(cv_bridge::CvImage(message.header, "bgr8", cv_image).toImageMsg());
+  publish_fn(resized_image);
+}
diff --git a/rubis_ws/src/image_transport/tutorial/src/resized_subscriber.cpp b/rubis_ws/src/image_transport/tutorial/src/resized_subscriber.cpp
new file mode 100644
index 00000000..299dc83d
--- /dev/null
+++ b/rubis_ws/src/image_transport/tutorial/src/resized_subscriber.cpp
@@ -0,0 +1,18 @@
+#include <image_transport_tutorial/resized_subscriber.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+void ResizedSubscriber::internalCallback(const image_transport_tutorial::ResizedImage::ConstPtr& msg,
+                                         const Callback& user_cb)
+{
+  // This is only for optimization, not to copy the image
+  boost::shared_ptr<void const> tracked_object_tmp;
+  cv::Mat img_rsz = cv_bridge::toCvShare(msg->image, tracked_object_tmp)->image;
+  // Resize the image to its original size
+  cv::Mat img_restored;
+  cv::resize(img_rsz, img_restored, cv::Size(msg->original_width, msg->original_height));
+
+  // Call the user callback with the restored image
+  cv_bridge::CvImage cv_img(msg->image.header, msg->image.encoding, img_restored);
+  user_cb(cv_img.toImageMsg());
+};
diff --git a/rubis_ws/src/inertiallabs_ins/CMakeLists.txt b/rubis_ws/src/inertiallabs_ins/CMakeLists.txt
new file mode 100644
index 00000000..8deccdec
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/CMakeLists.txt
@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(inertiallabs_ins)
+
+find_package(catkin REQUIRED COMPONENTS
+tf
+rospy
+roscpp
+geometry_msgs
+sensor_msgs
+std_msgs
+dynamic_reconfigure
+actionlib_msgs
+inertiallabs_msgs
+)
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+find_package(OpenCV)
+
+set(DEPRECATION_FLAG "-Wsizeof-array-argument")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra ${DEPRECATION_FLAG}")
+
+catkin_package(
+#  INCLUDE_DIRS include
+  CATKIN_DEPENDS roscpp rospy sensor_msgs geometry_msgs tf dynamic_reconfigure  actionlib_msgs inertiallabs_msgs
+  DEPENDS system_lib
+)
+
+include_directories(${Opencv_INCLUDE_DIRS})
+include_directories(${system_lib_INCLUDE_DIRS})
+
+include_directories(src/inertiallabs_sdk/)
+
+if(WIN32)
+	set(IL_PLATFORM "windows")
+else()
+	set(IL_PLATFORM "linux")
+endif()
+
+add_executable(il_ins src/il_ins.cpp
+                      src/inertiallabs_sdk/ILDriver.cpp
+                      src/inertiallabs_sdk/UDDParser.cpp
+                      src/inertiallabs_sdk/platforms/${IL_PLATFORM}/SerialPort.cpp
+                      src/inertiallabs_sdk/platforms/${IL_PLATFORM}/NetClient.cpp
+)
+add_dependencies(il_ins inertiallabs_msgs_generate_messages_cpp)
+if(WIN32)
+	target_link_libraries(il_ins ${catkin_LIBRARIES} )
+else()
+	target_link_libraries(il_ins ${catkin_LIBRARIES} pthread )
+endif()
+
+install(
+  TARGETS
+    il_ins
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch b/rubis_ws/src/inertiallabs_ins/launch/ins.launch
similarity index 100%
rename from rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch
rename to rubis_ws/src/inertiallabs_ins/launch/ins.launch
diff --git a/rubis_ws/src/inertiallabs_ins/package.xml b/rubis_ws/src/inertiallabs_ins/package.xml
new file mode 100644
index 00000000..2908c2a6
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/package.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<package>
+  <name>inertiallabs_ins</name>
+  <version>1.0.0</version>
+  <description>The Inertial Labs INS package</description>
+
+  <maintainer email="omprakashpatro@gmail.com">Omprakash Patra</maintainer>
+  <url type="website">http://exmaple.com</url>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>inertiallabs_msgs</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>inertiallabs_msgs</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+
+  <export>
+
+  </export>
+</package>
diff --git a/rubis_ws/src/inertiallabs_ins/src/il_ins.cpp b/rubis_ws/src/inertiallabs_ins/src/il_ins.cpp
new file mode 100644
index 00000000..db867f88
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/il_ins.cpp
@@ -0,0 +1,208 @@
+#include<iostream>
+#include<unistd.h>
+#include<math.h>
+#include<stdlib.h>
+#include <ros/ros.h>
+
+//Inertial Labs source header
+#include "ILDriver.h"
+
+//adding message type headers
+#include <inertiallabs_msgs/sensor_data.h>
+#include <inertiallabs_msgs/ins_data.h>
+#include <inertiallabs_msgs/gps_data.h>
+#include <inertiallabs_msgs/gnss_data.h>
+#include <inertiallabs_msgs/marine_data.h>
+
+//Publishers
+
+struct Context {
+	ros::Publisher publishers[5];
+	std::string imu_frame_id;
+};
+
+void publish_device(IL::INSDataStruct *data, void* contextPtr)
+{
+	Context * context = reinterpret_cast<Context*>(contextPtr);
+	static int seq=0;
+	seq++;
+
+	inertiallabs_msgs::sensor_data msg_sensor_data;
+	inertiallabs_msgs::ins_data msg_ins_data;
+	inertiallabs_msgs::gps_data msg_gps_data;
+	inertiallabs_msgs::gnss_data msg_gnss_data;
+	inertiallabs_msgs::marine_data msg_marine_data;
+
+	ros::Time timestamp = ros::Time::now();
+
+	if (context->publishers[0].getNumSubscribers() > 0)
+	{
+		msg_sensor_data.header.seq = seq;
+		msg_sensor_data.header.stamp = timestamp;
+		msg_sensor_data.header.frame_id = context->imu_frame_id;
+		msg_sensor_data.Mag.x = data->Mag[0];
+		msg_sensor_data.Mag.y = data->Mag[0];
+		msg_sensor_data.Mag.z = data->Mag[0];
+		msg_sensor_data.Accel.x = data->Acc[0];
+		msg_sensor_data.Accel.y = data->Acc[1];
+		msg_sensor_data.Accel.z = data->Acc[2];
+		msg_sensor_data.Gyro.x = data->Gyro[0];
+		msg_sensor_data.Gyro.y = data->Gyro[1];
+		msg_sensor_data.Gyro.z = data->Gyro[2];
+		msg_sensor_data.Temp = data->Temp;
+		msg_sensor_data.Vinp = data->VSup;
+		msg_sensor_data.Pressure = data->hBar;
+		msg_sensor_data.Barometric_Height = data->pBar;
+		context->publishers[0].publish(msg_sensor_data);
+	}
+
+	if (context->publishers[1].getNumSubscribers() > 0)
+	{
+		msg_ins_data.header.seq = seq;
+		msg_ins_data.header.stamp = timestamp;
+		msg_ins_data.header.frame_id = context->imu_frame_id;
+		msg_ins_data.YPR.x = data->Heading;
+		msg_ins_data.YPR.y = data->Pitch;
+		msg_ins_data.YPR.z = data->Roll;
+		msg_ins_data.OriQuat.w = data->Quat[0];
+		msg_ins_data.OriQuat.x = data->Quat[1];
+		msg_ins_data.OriQuat.y = data->Quat[2];
+		msg_ins_data.OriQuat.z = data->Quat[3];
+		msg_ins_data.LLH.x = data->Latitude;
+		msg_ins_data.LLH.y = data->Longitude;
+		msg_ins_data.LLH.z = data->Altitude;
+		msg_ins_data.Vel_ENU.x = data->VelENU[0];
+		msg_ins_data.Vel_ENU.y = data->VelENU[1];
+		msg_ins_data.Vel_ENU.z = data->VelENU[2];
+		msg_ins_data.GPS_INS_Time = data->GPS_INS_Time;
+		msg_ins_data.GPS_IMU_Time = data->GPS_IMU_Time;
+		msg_ins_data.GPS_mSOW.data = data->ms_gps;
+		msg_ins_data.Solution_Status.data = data->INSSolStatus;
+		msg_ins_data.USW = data->USW;
+		msg_ins_data.Pos_STD.x = data->KFLatStd;
+		msg_ins_data.Pos_STD.y = data->KFLonStd;
+		msg_ins_data.Pos_STD.z = data->KFAltStd;
+		msg_ins_data.Heading_STD = data->KFHdgStd;
+		context->publishers[1].publish(msg_ins_data);
+	}
+
+	if (context->publishers[2].getNumSubscribers() > 0)
+	{
+		msg_gps_data.header.seq = seq;
+		msg_gps_data.header.stamp = timestamp;
+		msg_gps_data.header.frame_id = context->imu_frame_id;
+		msg_gps_data.LLH.x = data->LatGNSS;
+		msg_gps_data.LLH.y = data->LonGNSS;
+		msg_gps_data.LLH.z = data->AltGNSS;
+		msg_gps_data.HorSpeed = data->V_Hor;
+		msg_gps_data.SpeedDir = data->Trk_gnd;
+		msg_gps_data.VerSpeed = data->V_ver;
+		context->publishers[2].publish(msg_gps_data);
+	}
+
+	if (context->publishers[3].getNumSubscribers() > 0)
+	{
+		msg_gnss_data.header.seq = seq;
+		msg_gnss_data.header.stamp = timestamp;
+		msg_gnss_data.header.frame_id = context->imu_frame_id;
+		msg_gnss_data.GNSS_info_1 = data->GNSSInfo1;
+		msg_gnss_data.GNSS_info_2 = data->GNSSInfo2;
+		msg_gnss_data.Number_Sat = data->SVsol;
+		msg_gnss_data.GNSS_Velocity_Latency = data->GNSSVelLatency;
+		msg_gnss_data.GNSS_Angles_Position_Type = data->AnglesType;
+		msg_gnss_data.GNSS_Heading = data->Heading_GNSS;
+		msg_gnss_data.GNSS_Pitch = data->Pitch_GNSS;
+		msg_gnss_data.GNSS_GDOP = data->GDOP;
+		msg_gnss_data.GNSS_PDOP = data->PDOP;
+		msg_gnss_data.GNSS_HDOP = data->HDOP;
+		msg_gnss_data.GNSS_VDOP = data->VDOP;
+		msg_gnss_data.GNSS_TDOP = data->TDOP;
+		msg_gnss_data.New_GNSS_Flags = data->NewGPS;
+		msg_gnss_data.Diff_Age = data->DiffAge;
+		msg_gnss_data.Pos_STD.x = data->LatGNSSStd;
+		msg_gnss_data.Pos_STD.y = data->LonGNSSStd;
+		msg_gnss_data.Pos_STD.z = data->AltGNSSStd;
+		msg_gnss_data.Heading_STD = data->HeadingGNSSStd;
+		msg_gnss_data.Pitch_STD = data->PitchGNSSStd;
+		context->publishers[3].publish(msg_gnss_data);
+	}
+
+	if (context->publishers[4].getNumSubscribers() > 0)
+	{
+		msg_marine_data.header.seq = seq;
+		msg_marine_data.header.stamp = timestamp;
+		msg_marine_data.header.frame_id = context->imu_frame_id;
+		msg_marine_data.Heave = data->Heave;
+		msg_marine_data.Surge = data->Surge;
+		msg_marine_data.Sway = data->Sway;
+		msg_marine_data.Heave_velocity = data->Heave_velocity;
+		msg_marine_data.Surge_velocity = data->Surge_velocity;
+		msg_marine_data.Sway_velocity = data->Sway_velocity;
+		msg_marine_data.Significant_wave_height = data->significant_wave_height;
+		context->publishers[4].publish(msg_marine_data);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "il_ins");
+	ros::NodeHandle n;
+	ros::NodeHandle np("~");
+	ros::Rate r(100); // 100 hz
+	std::string port;
+	IL::Driver ins;
+	int ins_output_format;
+	std::string imu_frame_id;
+	Context context;
+
+	//command line varibales
+
+	np.param<std::string>("ins_url", port, "serial:/dev/ttyUSB0:460800");
+	np.param<int>("ins_output_format", ins_output_format, 0x52);
+
+	//Initializing Publishers
+	context.publishers[0] = np.advertise<inertiallabs_msgs::sensor_data>("/Inertial_Labs/sensor_data", 1);
+	context.publishers[1] = np.advertise<inertiallabs_msgs::ins_data>("/Inertial_Labs/ins_data", 1);
+	context.publishers[2] = np.advertise<inertiallabs_msgs::gps_data>("/Inertial_Labs/gps_data", 1);
+	context.publishers[3] = np.advertise<inertiallabs_msgs::gnss_data>("/Inertial_Labs/gnss_data", 1);
+	context.publishers[4] = np.advertise<inertiallabs_msgs::marine_data>("/Inertial_Labs/marine_data", 1);
+
+
+	ROS_INFO("connecting to INS at URL %s\n",port.c_str());
+
+	auto il_err = ins.connect(port.c_str());
+	if (il_err != 0)
+	{
+		ROS_FATAL("Could not connect to the INS on this URL %s\n",
+				  port.c_str()
+		);
+		exit(EXIT_FAILURE);
+	}
+
+	if (ins.isStarted())
+	{
+		ins.stop();
+	}
+	auto devInfo = ins.getDeviceInfo();
+	auto devParams = ins.getDeviceParams();
+	std::string SN(reinterpret_cast<const char *>(devInfo.IDN), 8);
+	ROS_INFO("Found INS S/N %s\n", SN.c_str());
+	context.imu_frame_id = SN;
+	il_err = ins.start(ins_output_format);
+	if (il_err != 0)
+	{
+		ROS_FATAL("Could not start the INS: %i\n", il_err);
+		ins.disconnect();
+		exit(EXIT_FAILURE);
+	}
+	ins.setCallback(&publish_device, &context);
+	ROS_INFO("publishing at %d Hz\n", devParams.dataRate);
+	ROS_INFO("rostopic echo the topics to see the data");
+	ros::spin();
+	std::cout << "Stopping INS... " << std::flush;
+	ins.stop();
+	std::cout << "Disconnecting... " << std::flush;
+	ins.disconnect();
+	std::cout << "Done." << std::endl;
+	return 0;
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/ILDriver.cpp b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/ILDriver.cpp
new file mode 100644
index 00000000..6a30eafc
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/ILDriver.cpp
@@ -0,0 +1,430 @@
+#include "ILDriver.h"
+#include "UDDParser.h"
+#include "SerialPort.h"
+#include "NetClient.h"
+#include <iostream>
+#include <string>
+#include <errno.h>
+
+namespace IL {
+
+	Driver::Driver()
+		: latestData()
+		, deviceInfo()
+		, deviceParam()
+		, port(nullptr)
+		, workerThread(nullptr)
+		, quit(false)
+		, devInfoRead(false)
+		, onRequestMode(false)
+		, sessionState(Off)
+		, callback(nullptr)
+	{
+		port = nullptr;
+	}
+
+	Driver::~Driver()
+	{
+		disconnect();
+	}
+
+	int Driver::connect(const char* url)
+	{
+		if (workerThread)
+			return 1;
+		std::string urlStr(url);
+		std::string pathStr;
+		size_t pos = urlStr.find(':');
+		if (pos != std::string::npos) {
+			std::string typeStr = urlStr.substr(0, pos);
+			if ("serial" == typeStr) {
+				port = new SerialPort;
+				pathStr = urlStr.substr(pos + 1);
+			}
+			else if ("tcp" == typeStr || "udp" == typeStr) {
+				port = new NetClient;
+				pathStr = urlStr;
+			}
+			else
+				return 256;
+		}
+		else {
+			return 256;
+		}
+		int result = port->open(pathStr.c_str());
+		if (result) {
+			disconnect();
+			return result;
+		}
+		sessionState = Off;
+		workerThread = new std::thread(threadFunc, this);
+		readDevInfo();
+		if (sessionState < GotDevParams)
+		{
+			disconnect();
+			return 512;
+		}
+		return 0;
+	}
+
+	void Driver::disconnect()
+	{
+		if (workerThread) {
+			quit = true;
+			workerThread->join();
+			delete workerThread;
+			workerThread = nullptr;
+		}
+		if (port) {
+			if (port->isOpen()) {
+				port->close();
+			}
+			delete port;
+			port = nullptr;
+		}
+	}
+
+	int Driver::start(unsigned char mode, bool onRequest, const char* logname)
+	{
+		char command = onRequest ? '\xC1' : mode;
+		if (sessionState != GotDevParams) {
+			return 1;
+		}
+
+		onRequestMode = onRequest;
+		sendPacket(0, &command, 1);
+		for (int repeat = 0; repeat < 10; ++repeat) {
+			std::this_thread::sleep_for(std::chrono::milliseconds(100));
+			if (sessionState >= GetIntinialReport) {
+				break;
+            }
+		}
+		if (sessionState < GetIntinialReport) {
+			return 2;
+		}
+		if (sessionState < Processing) {
+			std::this_thread::sleep_for(std::chrono::seconds(deviceParam.initAlignmentTime));
+			for (int repeat = 0; repeat < 10; ++repeat) {
+				if (sessionState == Processing) {
+					break;
+				}
+				std::this_thread::sleep_for(std::chrono::milliseconds(100));
+			}
+			if (sessionState < Processing) {
+				return 3;
+			}
+		}
+		if (logname)
+			log.open(logname);
+		return 0;
+	}
+
+	int Driver::request(unsigned char mode, int timeout)
+	{
+		if (!onRequestMode)
+			return -1;
+		if (sessionState < GetIntinialReport)
+			return -2;
+		requestFulfilled = false;
+		requestCode = mode;
+		sendPacket(0, &requestCode, 1);
+		for (int i = 0; i < timeout; ++i)
+		{
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
+			if (requestFulfilled)
+				return 0;
+		}
+		return 1;
+	}
+
+	int Driver::stop()
+	{
+		sessionState = Closing;
+		for (int trial = 0; trial < 5; ++trial)
+		{
+			sendPacket(0, "\xFE", 1);
+			std::this_thread::sleep_for(std::chrono::milliseconds(100));
+		}
+		std::this_thread::sleep_for(std::chrono::milliseconds(500));
+		if (log) {
+			log.close();
+		}
+		sessionState = Off;
+		if (!devInfoRead)					// In case the device was in auto-start mode or already started when we connected
+			return readDevInfo();
+		sessionState = GotDevParams;
+		return 0;
+	}
+
+	INSDeviceInfo Driver::getDeviceInfo()
+	{
+		return deviceInfo;
+	}
+
+	INSDevicePar Driver::getDeviceParams()
+	{
+		return deviceParam;
+	}
+
+	void Driver::setCallback(void (*newCallback)(INSDataStruct*, void*), void * userContext)
+	{
+		callback = newCallback;
+		callbackContext = userContext;
+	}
+
+	int Driver::sendPacket(char type, const char* payload, unsigned int size)
+	{
+		uint8_t buf[65536] = "\xAA\x55\x00\x00";
+		uint16_t checksum = buf[2] = type;
+		checksum += buf[4] = (size + 6) & 0xFF;
+		checksum += buf[5] = (size + 6) >> 8;
+		for (unsigned int i = 0; i < size; ++i)
+			checksum += buf[6 + i] = payload[i];
+		buf[6 + size] = checksum & 0xFF;
+		buf[7 + size] = checksum >> 8;
+		return port->write(reinterpret_cast<char*>(buf), 8 + size);
+	}
+
+	int Driver::readDevInfo()
+	{
+		int result = sendPacket(0, "\x12", 1);(void) result;
+		for (int sec = 0; sec < 30; ++sec) {
+			std::this_thread::sleep_for(std::chrono::seconds(1));
+			if (sessionState)
+				break;
+		}
+		if (!sessionState)
+		{
+			disconnect();
+			return 2;
+		}
+		sendPacket(0, "\x41", 1);
+		for (int sec = 0; sec < 10; ++sec) {
+			std::this_thread::sleep_for(std::chrono::seconds(1));
+			if (GotDevParams == sessionState || Processing == sessionState)
+				break;
+		}
+		return 0;
+	}
+
+	void Driver::readerLoop()
+	{
+		enum TrafficType
+		{
+			Invalid,
+			Binary,
+			RawImu,
+			Nmea,
+			Nmea_A,
+		};
+		int state = 0;
+		int checksum = 0;
+		char buf[65536];
+		UDDParser parser;
+		uint16_t len = 0;
+		std::string NMEA;
+		std::string RawIMU;
+		uint32_t RawIMUcounter;
+		uint8_t header[3];
+		TrafficType trafficType = Invalid;
+		uint8_t byte = 0, prevByte = 0;
+		while (!quit)
+		{
+			int readBytes = port->read(buf, sizeof(buf));
+			if (readBytes < 0)
+				quit = true;
+			else
+			{
+				for (int i = 0; i < readBytes; ++i)
+				{
+					prevByte = byte;
+					byte = buf[i];
+					if (state) checksum += byte;
+					switch (state)
+					{
+					case 0:
+						for (int i = 0; i < 2; ++i)
+							header[i] = header[i + 1];
+						header[2] = byte;
+						if (0xAA == header[0] && 0x55 == header[1] && 0x01 == header[2])
+						{
+							state = 3;
+							trafficType = Binary;
+							checksum = 1;
+						}
+						else if (0xAA == header[0] && 0x44 == header[1] && 0x12 == header[2])
+						{
+							state = 3;
+							trafficType = RawImu;
+						}
+						else if (0x0D == header[0] && 0x0A == header[1] && '$' == header[2])
+						{
+							state = 3;
+							trafficType = Nmea;
+						}
+						else if (0x0D == header[0] && 0x0A == header[1] && 'A' == header[2])
+						{
+							state = 3;
+							trafficType = Nmea_A;
+						}
+						break;
+					case 3:
+						switch (trafficType)
+						{
+						case Binary:
+							parser.code = byte;
+							++state;
+							break;
+						case RawImu:
+							RawIMU = "\xAA\x44\x12";
+							RawIMU += (char)byte;
+							RawIMUcounter = 4;
+							++state;
+							break;
+						case Nmea:
+							NMEA = "$";
+							NMEA += (char)byte;
+							++state;
+							break;
+						case Nmea_A:
+							NMEA = "A";
+							NMEA += (char)byte;
+							++state;
+							break;
+						default:
+							break;
+						}
+						break;
+					case 4:
+						switch (trafficType)
+						{
+						case Binary:
+							parser.payloadLen = byte;
+							++state;
+							break;
+						case RawImu:
+							if (72 == ++RawIMUcounter)
+							{
+								state = 0;
+								{
+									// TODO: RAWIMUB data parser
+								}
+							}
+							break;
+						case Nmea:
+							if (0x0a == byte && 0x0d == prevByte)
+							{
+								header[1] = prevByte; header[2] = byte;
+								state = 0;
+								{
+									// We do not parse NMEA
+								}
+							}
+							break;
+						case Nmea_A:
+							if (0x0a == byte && 0x0d == prevByte)
+							{
+								header[1] = prevByte; header[2] = byte;
+								state = 0;
+								{
+									// We do not parse COBHAM
+								}
+							}
+							break;
+						default:
+							break;
+						}
+						break;
+					case 5:
+						parser.payloadLen += static_cast<uint16_t>(byte) << 8;
+						++state;
+						parser.payloadInd = 0;
+						if (6 == parser.payloadLen)
+							++state; 		// no payload;
+						if (parser.payloadLen < 6)
+							state = 0; 		// The packet length value cannot be less than 6
+						parser.payloadLen -= 6;
+						break;
+					case 6:
+						parser.payloadBuf[parser.payloadInd] = byte;
+						if (++parser.payloadInd == parser.payloadLen)
+							++state;
+						break;
+					case 7:
+						checksum -= byte;
+						checksum -= byte;
+						if (checksum & 0xFF)
+							state = 0;
+						else
+							++state;
+						checksum >>= 8;
+						break;
+					case 8:
+						checksum -= byte;
+						checksum -= byte;
+						if (!(checksum & 0xFF))
+						{
+							switch (parser.code) {
+							case 0x12:
+								if (parser.payloadLen == sizeof(INSDeviceInfo))
+								{
+									sessionState = GotDevInfo;
+									deviceInfo = *reinterpret_cast<INSDeviceInfo*>(parser.payloadBuf);
+								}
+								break;
+							case 0x41:
+								if (parser.payloadLen == sizeof(INSDevicePar))
+								{
+									sessionState = GotDevParams;
+									devInfoRead = true;
+									deviceParam = *reinterpret_cast<INSDevicePar*>(parser.payloadBuf);
+									sendPacket(0, "\xB1\x6C", 2);
+								}
+								break;
+							case 0xB1:
+								if (parser.payloadLen == 2)
+								{
+									if (parser.payloadBuf[0] == 0x6C)
+									{
+										parser.high_precision_heave = (parser.payloadBuf[1] == 0x01);
+									}
+								}
+								break;
+							default:
+								if ((!sessionState || GetIntinialReport == sessionState) && (0x32 == parser.payloadLen || 0x80 == parser.payloadLen))
+								{
+									// Initial alignment report
+									sessionState = Processing;
+								}
+								else if (!sessionState ||
+										 GotDevParams == sessionState ||
+										 GetIntinialReport == sessionState || // Ignore missing initial alignment report
+										 Processing == sessionState)
+								{
+									if (parser.parse()) {
+										sessionState = GetIntinialReport;	// ACK received
+									} else {
+										sessionState = Processing;
+										if (log) {
+											if (parser.hdrStream.str().size()) {
+												log << parser.hdrStream.str() << std::endl;
+											}
+											log << parser.txtStream.str() << std::endl;
+										}
+										latestData = parser.outData;
+										if (onRequestMode && parser.code == requestCode)
+											requestFulfilled = true;
+										if (callback) callback(&latestData, callbackContext);
+									}
+								}
+							}
+						}
+					/* fall through */
+					default:
+						state = 0;
+					}
+				}
+			}
+		}
+	}
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/ILDriver.h b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/ILDriver.h
new file mode 100644
index 00000000..980288e2
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/ILDriver.h
@@ -0,0 +1,56 @@
+#pragma once
+#include <thread>
+#include <fstream>
+#include "dataStructures.h"
+#include "Transport.h"
+
+namespace IL {
+	class Driver
+	{
+	public:
+
+        enum SessionState
+        {
+            Off,
+            GotDevInfo,
+            GotDevParams,
+            GetIntinialReport,
+            Processing,
+            Closing
+        };
+
+		Driver();
+		~Driver();
+		int connect(const char* url);
+		void disconnect();
+		int start(unsigned char mode, bool onRequest = false, const char* logname = nullptr);
+		int request(unsigned char mode, int timeout);
+		int stop();
+		INSDeviceInfo getDeviceInfo();
+		INSDevicePar getDeviceParams();
+		bool isStarted() { return sessionState == Processing; }
+		void setCallback(void (*newCallback)(INSDataStruct*, void*), void* userContext);
+
+	private:
+		INSDataStruct latestData;
+		INSDeviceInfo deviceInfo;
+		INSDevicePar deviceParam;
+		Transport* port;
+		std::thread* workerThread;
+		bool quit;
+		bool devInfoRead;
+		bool onRequestMode;
+		char requestCode;
+		bool requestFulfilled;
+		SessionState sessionState;
+		int sendPacket(char type, const char* payload, unsigned int size);
+		int readDevInfo();
+		void readerLoop();
+		static void threadFunc(Driver* instance) { instance->readerLoop(); }
+		std::ofstream log;
+		void (*callback)(INSDataStruct*, void*);
+		void* callbackContext;
+	};
+
+}
+
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/LICENSE.txt b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/LICENSE.txt
new file mode 100644
index 00000000..4e2c31e6
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/LICENSE.txt
@@ -0,0 +1,9 @@
+The MIT License (MIT)
+
+Copyright (c) 2018 Inertial Labs, Inc(TM)
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/Makefile b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/Makefile
new file mode 100644
index 00000000..764bff50
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/Makefile
@@ -0,0 +1,19 @@
+CC      = g++ 
+CCFLAGS = -std=c++17
+LDFLAGS = -lpthread
+
+default: all
+
+all: debug release
+
+debug: 
+	mkdir -p debug
+	$(CC) $(CCFLAGS) -Og -ggdb3 -o debug/example example.cpp ILDriver.cpp UDDParser.cpp platforms/linux/SerialPort.cpp platforms/linux/NetClient.cpp $(LDFLAGS)
+
+release: 
+	mkdir -p release
+	$(CC) $(CCFLAGS) -O3 -g0 -o release/example example.cpp ILDriver.cpp UDDParser.cpp platforms/linux/SerialPort.cpp platforms/linux/NetClient.cpp $(LDFLAGS)
+
+clean:
+	rm debug -rf
+	rm release -rf
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/NetClient.h b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/NetClient.h
new file mode 100644
index 00000000..3bbf5521
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/NetClient.h
@@ -0,0 +1,32 @@
+#pragma once
+#include "Transport.h"
+
+struct sockaddr_in;
+
+namespace IL {
+    class NetClient :
+        public Transport
+    {
+    public:
+        enum Type {
+            None,
+            Tcp,
+            Udp,
+        };
+        NetClient();
+        virtual ~NetClient();
+        virtual int open(const char* url);
+        virtual bool isOpen();
+        virtual void close();
+        virtual int read(char* buf, unsigned int size);
+        virtual int write(char* buf, unsigned int size);
+
+    private:
+        int fd;
+        int64_t hCom;
+        int timeout;
+        Type type;
+        sockaddr_in *addr;
+    };
+}
+
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/SerialPort.h b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/SerialPort.h
new file mode 100644
index 00000000..c22048ea
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/SerialPort.h
@@ -0,0 +1,23 @@
+#pragma once
+#include "Transport.h"
+
+namespace IL {
+
+	class SerialPort : public Transport
+	{
+	public:
+		SerialPort();
+		virtual ~SerialPort();
+		virtual int open(const char* url);
+		virtual bool isOpen();
+		virtual void close();
+		virtual int read(char* buf, unsigned int size);
+		virtual int write(char* buf, unsigned int size);
+
+	private:
+		int fd;
+		void* hCom;
+		int timeout;
+	};
+}
+
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/Transport.h b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/Transport.h
new file mode 100644
index 00000000..d6a501a5
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/Transport.h
@@ -0,0 +1,16 @@
+#pragma once
+
+namespace IL {
+	class Transport
+	{
+	public:
+		Transport() {};
+		virtual ~Transport() {};
+		virtual int open(const char* path) { (void)path;return 1; }
+		virtual bool isOpen() { return false; }
+		virtual void close() {}
+		virtual int read(char* buf, unsigned int size) { (void)buf; (void)size; return 0; }
+		virtual int write(char* buf, unsigned int size) { (void)buf; (void)size; return 0; }
+	};
+}
+
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/UDDParser.cpp b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/UDDParser.cpp
new file mode 100644
index 00000000..8ec1474d
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/UDDParser.cpp
@@ -0,0 +1,769 @@
+#include "UDDParser.h"
+#include "dataStructures.h"
+#include <forward_list>
+#include <iostream>
+#include <chrono>
+
+using namespace std;
+
+namespace IL {
+
+	UDDParser::UDDParser()
+		: KA(2000)
+		, KG(50)
+		, code(0)
+		, payloadLen(0)
+		, payloadInd(0)
+		, dataSet("")
+		, oldDataSet("")
+		, high_precision_heave(false)
+	{
+	}
+
+
+	UDDParser::~UDDParser()
+	{
+	}
+
+	int UDDParser::parse()
+	{
+		txtStream.str("");
+		hdrStream.str("");
+		statusStream.str("");
+		if (payloadLen <= 2)
+			return 1;				// ACK message
+		payloadInd = 0;
+		statusStream << "INS data: ";
+		SA = 1e6; SG = 1e5; SO = 1e3; SV = 1e2;
+		switch (code)
+		{
+			using namespace PacketType;
+		case IL_Sensors:
+			statusStream << "Sensors";
+			dataSet = "\x07\x20\x22\x24\xFF\xFF\x53\x50\x52\x30\x34\x32\x01\xF3\xF0\xF4\x41\xFE";
+			break;
+		case IL_OPVT:
+			statusStream << "OPVT";
+			dataSet = "\x07\x20\x22\x24\x53\x50\x52\x10\x12\x30\x32\x01\x36\x3B\xF0\x25\x41";
+			break;
+		case IL_min:
+			statusStream << "Minimal";
+			dataSet = "\x07\x53\x50\x52\x10\x12\x01\xF5\x3B";
+			break;
+		case IL_QPVT:
+			statusStream << "QPVT";
+			dataSet = "\x09\x20\x22\x24\x53\x50\x52\x10\x12\x30\x32\x01\x36\x3B\xF0\x25\x41";
+			break;
+		case IL_OPVT2A:
+			statusStream << "OPVT2A";
+			dataSet = "\x07\x20\x22\x24\x53\x50\x52\x10\x12\x30\x32\x01\x36\x3B\x3D\x3A\xF2\xF1\xF7\x25\x41";
+			break;
+		case IL_OPVT2AHR:
+			statusStream << "OPVT2AHR";
+			dataSet = "\x07\x21\x23\x24\x53\x50\x52\x11\x12\x31\x32\x01\x36\x3B"
+					  "\x3D\x3A\xF2\xF1\xF7\x25\x41";
+			break;
+		case MRU_OPVTHSSHR:
+			statusStream << "OPVTHSSHR";
+			dataSet = "\x08\x21\x23\x24\x53\x50\x52\x11\x12\x13\x16\x15\x18\x31"
+					  "\x32\x01\x3c\x36\x3B\x3D\x3A\xF2\xF1\xF7\x25\x41";
+			if (high_precision_heave) {
+				dataSet[9] = '\x14';
+				dataSet[10] = '\x17';
+			}
+			break;
+		case IL_OPVT2AW:
+			statusStream << "OPVT2AW";
+			dataSet = "\x07\x20\x22\x24\x53\x50\x52\x10\x12\x30\x32\x01\x3C\x36\x3B\xF0\x3A\x33\x35\x25\x41";
+			break;
+		case IL_OPVTAD:
+			statusStream << "OPVTAD";
+			dataSet = "\x07\x21\x23\x24\x53\x50\x52\x11\x12\x31\x32\x01\x36\x3B\xF0\x3A\xF2\xF1\x25\x41\x60\x61\x62\x63\x64\x65";
+			break;
+		case IL_OPVT_rawIMU:
+			statusStream << "OPVTRawIMU";
+			dataSet = "\x02\x03\x21\x23\x53\x08\x11\x12\x36\x3B\x41";
+			SA = SG = 1e4;
+			break;
+		case IL_OPVT_GNSSext:
+			statusStream << "OPVTGNSSExt";
+			dataSet = "\x01\x08\x11\x12\x21\x23\x24\xF6\x3D\xF7\x41";
+			SO = SA = SG = SV = 1e6;
+			break;
+		case IL_UDD:
+			statusStream << "UDD";
+			dataSet.clear();
+			dataSet.append(reinterpret_cast<const char*>(&payloadBuf[1]), payloadBuf[0]);
+			payloadInd = payloadBuf[0] + 1;
+			break;
+		default:
+			statusStream << "0x" << hex << setw(2) << setfill('0') << static_cast<uint32_t>(code);
+			break;
+		}
+		//	if (dataSet != oldDataSet)
+		{
+			writeHeader();
+			oldDataSet = dataSet;
+		}
+		writeTxtAndData();
+		return 0;
+	}
+
+	void UDDParser::writeHeader()
+	{
+		for (int i = 0; i < dataSet.size(); ++i)
+		{
+			switch (static_cast<uint8_t>(dataSet[i]))
+			{
+			case 0x01:
+				hdrStream << "ms_gps";
+				break;
+			case 0x02:
+				hdrStream << "GPS_INS_Time";
+				break;
+			case 0x03:
+				hdrStream << "GPS_IMU_Time";
+				break;
+			case 0x04:
+				hdrStream << "UTC_Hour\tUTC_Minute\tUTC_Second\tUTC_DecSec\tUTC_Day\tUTC_Month\tUTC_Year";
+				break;
+			case 0x07:
+			case 0x08:
+				hdrStream << "Heading\tPitch\tRoll";
+				break;
+			case 0x09:
+				hdrStream << "QW\tQX\tQY\tQZ";
+				break;
+			case 0x10:
+			case 0x11:
+				hdrStream << "Latitude\tLongitude\tAltitude";
+				break;
+			case 0x12:
+				hdrStream << "V_East\tV_North\tV_Up";
+				break;
+			case 0x13:
+			case 0x14:
+				hdrStream << "Heave";
+				break;
+			case 0x15:
+				hdrStream << "Heave_Velocity";
+				break;
+			case 0x16:
+			case 0x17:
+				hdrStream << "Surve\tSway";
+				break;
+			case 0x18:
+				hdrStream << "Surve_Velocity\tSway_Velocity";
+				break;
+			case 0x19:
+				hdrStream << "Significant_Wave_Height";
+				break;
+			case 0x1B:
+				hdrStream << "V_East\tV_North\tV_Up";
+				break;
+			case 0x20:
+			case 0x21:
+				hdrStream << "GX\tGY\tGZ";
+				break;
+			case 0x22:
+			case 0x23:
+				hdrStream << "AX\tAY\tAZ";
+				break;
+			case 0x24:
+				hdrStream << "MX\tMY\tMZ";
+				break;
+			case 0x25:
+				hdrStream << "pBar\thBar";
+				break;
+			case 0x26:
+				hdrStream << "GBX\tGBY\tGBZ\tABX\tABY\tABZ\tBReserved";
+				break;
+			case 0x27:
+				hdrStream << "APVPX\tAPVPY\tAPVPZ";
+				break;
+			case 0x30:
+			case 0x31:
+				hdrStream << "LatGNSS\tLongGNSS\tAltGNSS";
+				break;
+			case 0x32:
+				hdrStream << "V_Hor\tTrk_gnd\tV_ver";
+				break;
+			case 0x33:
+				hdrStream << "Heading_GNSS\tPitch_GNSS";
+				break;
+			case 0x34:
+				hdrStream << "LatGNSSStd\tLongGNSSStd\tAltGNSSStd";
+				break;
+			case 0x35:
+				hdrStream << "HeadingGNSSStd\tPitchGNSSStd";
+				break;
+			case 0x36:
+				hdrStream << "GNSSInfo1\tGNSSInfo2";
+				break;
+			case 0x37:
+				hdrStream << "SVtrack\tSVsol\tSVsolL1\tSVSolMulti\tGalBD\tGPSGlo\tTimeStatus\tExtSolStatus";
+				break;
+			case 0x38:
+				hdrStream << "GNSSSolStatus";
+				break;
+			case 0x39:
+				hdrStream << "GNSSSolType";
+				break;
+			case 0x3A:
+				hdrStream << "AnglesType";
+				break;
+			case 0x3B:
+				hdrStream << "SVSol";
+				break;
+			case 0x3C:
+				hdrStream << "Week";
+				break;
+			case 0x3D:
+				hdrStream << "GNSSVelLatency";
+				break;
+			case 0x3E:
+				hdrStream << "GNSSPosMs";
+				break;
+			case 0x3F:
+				hdrStream << "GNSSVelMs";
+				break;
+			case 0x40:
+				hdrStream << "GNSSHdgMs";
+				break;
+			case 0x41:
+				hdrStream << "NewGPS";
+				break;
+			case 0x42:
+				hdrStream << "GDOP\tPDOP\tHDOP\tVDOP\tTDOP";
+				break;
+			case 0x43:
+				hdrStream << "GNSS_PACC\tGNSS_VACC";
+				break;
+			case 0x44:
+				hdrStream << "GDOP\tPDOP";
+				break;
+			case 0x45:
+				hdrStream << "Trk_gnd";
+				break;
+			case 0x47:
+				hdrStream << "DiffAge";
+				break;
+			case 0x48:
+				hdrStream << "GNSS_ECEF_VXStsd\tGNSS_ECEF_VYStd\tGNSS_ECEF_VZStd";
+				break;
+			case 0x49:
+				hdrStream << "PPPApp\tPPPStore";
+				break;
+			case 0x50:
+				hdrStream << "VSup";
+				break;
+			case 0x51:
+				hdrStream << "VStab";
+				break;
+			case 0x52:
+				hdrStream << "Temp";
+				break;
+			case 0x53:
+				hdrStream << "USW";
+				break;
+			case 0x54:
+				hdrStream << "INSSolStatus";
+				break;
+			case 0x55:
+				hdrStream << "KFLatStd\tKFLonStd\tKFAltStd";
+				break;
+			case 0x56:
+				hdrStream << "KFHdgStd";
+				break;
+			case 0x57:
+				hdrStream << "KFLatStd\tKFLonStd\tKFAltStd";
+				break;
+			case 0x58:
+				hdrStream << "KFVEStd\tKFVNStd\tKFVUStd";
+				break;
+			case 0x60:
+				hdrStream << "Odometer";
+				break;
+			case 0x61:
+				hdrStream << "AirSpeed";
+				break;
+			case 0x62:
+				hdrStream << "WindN\tWindE\tWindNStd\tWindEStd";
+				break;
+			case 0x63:
+				hdrStream << "LatExt\tLonExt\tAltExt\tLatExtStd\tLonExtStd\tAltExtStd\tExtPosLatency";
+				break;
+			case 0x64:
+				hdrStream << "LocLat\tLocLon\tLocAlt\tLocDopplerShift\tLocDopplerShiftStd";
+				break;
+			case 0x65:
+				hdrStream << "NewAiding";
+				break;
+			case 0x66:
+				hdrStream << "HgdExt\tHgdExtStd\tHdgExtLatency";
+				break;
+			case 0x67:
+				hdrStream << "DVLRight\tDVLFwd\tDVLUp\tDVLRightStd\tDVLFwdStd\tDVLUpStd\tDVLLatency\tDVLPressure";
+				break;
+			case 0x68:
+				hdrStream << "GBXExt\tGBYExt\tGBZExt\tABXExt\tABYExt\tABZExt\tBExtReserved";
+				break;
+			case 0x69:
+				hdrStream << "PitchExt\tRollExt";
+				break;
+			case 0x6A:
+				hdrStream << "PriAntRightExt\tPriAntForwardExt\tPriAntUpExt\tSecAntRightExt\tSecAntForwardExt\tSecAntUpExt";
+				break;
+			case 0xF0:
+			case 0xF7:
+				hdrStream << "Latency_ms_pos\tLatency_ms_vel";
+				break;
+			case 0xF1:
+				hdrStream << "Latency_ms_head";
+				break;
+			case 0xF2:
+				hdrStream << "HeadingGNSS";
+				break;
+			case 0xF3:
+				hdrStream << "TimeStatus\tGNSSSolStatus\tGNSSPosType\tSVtrack\tSVsol\tSVsolL1\tSVSolMulti\tExtSolStatus\tGalBD\tGPSGlo";
+				break;
+			case 0xF4:
+				hdrStream << "UP\tUT";
+				break;
+			case 0xF5:
+				hdrStream << "GNSSInfo1";
+				break;
+			case 0xF6:
+				hdrStream << "pBar\tTemp\tUSW\tPosType\tGNSS_ECEF_X\tGNSS_ECEF_Y\tGNSS_ECEF_Z\tGNSS_PACC\tGNSS_ECEF_VX\tGNSS_ECEF_VY\tGNSS_ECEF_VZ\tGNSS_SACC\
+\tLatGNSS\tLongGNSS\tAltGNSS\tV_Hor\tTrk_gnd\tV_ver\tAnglesType\tHeadingGNSS\tSVSol\tGNSSInfo1\tGNSSInfo2\tGDOP\tPDOP\tHDOP\tVDOP\tTDOP\tDiffAge\
+\tUTC_Hour\tUTC_Minute\tUTC_Second\tUTC_DecSec\tUTC_Day\tUTC_Month\tUTC_Year\tUTC\tLatencyECEF";
+				break;
+			case 0xFE:
+			case 0xFF:
+				hdrStream << "Reserved";
+				break;
+			default:
+				break;
+			}
+			if (i < dataSet.size() - 1)
+				hdrStream << "\t";
+		}
+	}
+
+	void UDDParser::writeTxtAndData()
+	{
+		uint8_t IMRdataPresent = 0;
+		bool GPSTimePresent = false;
+		double UTCTOD = 0; 			// UTC Time of Day
+		uint32_t UTCDOW = 0; 		// Day of Week computed from UTC date;
+		int UTCMonth;
+		int UTCYear;
+		static int startOfMonthDaysToAdd[] = { 0, 3, 2, 5, 0, 3, 5, 1, 4, 6, 2, 4 }; 	// To compute day of week
+		double val;
+		for (int i = 0; i < dataSet.size(); ++i)
+		{
+			switch (static_cast<uint8_t>(dataSet[i]))
+			{
+			case 0x01:
+				outData.ms_gps = readScaled<uint32_t>(1, false);
+				break;
+			case 0x02:
+				outData.GPS_INS_Time = readScaled<uint64_t>(1e9, false);
+				break;
+			case 0x03:
+				outData.GPS_IMU_Time = readScaled<uint64_t>(1e9, false);
+				break;
+			case 0x04:
+				outData.UTC_Hour = readScaled<uint8_t>(1, true);
+				outData.UTC_Minute = readScaled<uint8_t>(1, true);
+				outData.UTC_Second = readScaled<uint8_t>(1, true);
+				outData.UTC_DecSec = readScaled<uint16_t>(1e3, true);
+				outData.UTC_Day = readScaled<uint8_t>(1, true);
+				outData.UTC_Month = readScaled<uint8_t>(1, true);
+				outData.UTC_Year = readScaled<uint16_t>(1, false);
+				break;
+			case 0x07:
+				outData.Heading = readScaled<uint16_t>(100, true);
+				outData.Pitch = readScaled<int16_t>(100, true);
+				outData.Roll = readScaled<int16_t>(100, false);
+				break;
+			case 0x08:
+				outData.Heading = readScaled<uint32_t>(SO, true);
+				outData.Pitch = readScaled<int32_t>(SO, true);
+				outData.Roll = readScaled<int32_t>(SO, false);
+				break;
+			case 0x09:
+				for (int ind = 0; ind < 4; ++ind)
+					outData.Quat[ind] = readScaled<int16_t>(1e4, ind < 3);
+				break;
+			case 0x10:
+				outData.Latitude = readScaled<int32_t>(1e7, true);
+				outData.Longitude = readScaled<int32_t>(1e7, true);
+				outData.Altitude = readScaled<int32_t>(1e2, false);
+				break;
+			case 0x11:
+				outData.Latitude = readScaled<int64_t>(1e9, true);
+				outData.Longitude = readScaled<int64_t>(1e9, true);
+				outData.Altitude = readScaled<int32_t>(1e3, false);
+				break;
+			case 0x12:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.VelENU[ind] = readScaled<int32_t>(SV, ind < 2);
+				break;
+			case 0x13:
+				outData.Heave = readScaled<int32_t>(100.0, false);
+				break;
+			case 0x14:
+				outData.Heave = readScaled<int32_t>(10000.0, false);
+				break;
+			case 0x15:
+				outData.Heave_velocity = readScaled<int16_t>(100.0,false);
+				break;
+			case 0x16:
+				outData.Surge = readScaled<int16_t>(100.0, true);
+				outData.Sway = readScaled<int16_t>(100.0, false);
+				break;
+			case 0x17:
+				outData.Surge = readScaled<int16_t>(1000.0, true);
+				outData.Sway = readScaled<int16_t>(1000.0, false);
+				break;
+			case 0x18:
+				outData.Surge_velocity = readScaled<int16_t>(100.0,true);
+				outData.Sway_velocity = readScaled<int16_t>(100.0,false);
+				break;
+			case 0x19:
+				outData.significant_wave_height = readScaled<uint16_t>(100.0, false);
+				break;
+			case 0x1B:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.VelENU[ind] = readScaled<int32_t>(1e6, ind < 2);
+				break;
+			case 0x20:
+				for (int ind = 0; ind < 3; ++ind)
+				{
+					outData.Gyro[ind] = readScaled<int16_t>(KG, ind < 2);
+				}
+				break;
+			case 0x21:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.Gyro[ind] = readScaled<int32_t>(SG, ind < 2);
+				break;
+			case 0x22:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.Acc[ind] = readScaled<int16_t>(KA, ind < 2);
+				break;
+			case 0x23:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.Acc[ind] = readScaled<int32_t>(SA, ind < 2);
+				break;
+			case 0x24:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.Mag[ind] = readScaled<int16_t>(0.1, ind < 2);
+				break;
+			case 0x25:
+				outData.pBar = readScaled<uint16_t>(0.5, true);
+				outData.hBar = readScaled<int32_t>(100, false);
+				break;
+			case 0x26:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.GBias[ind] = readScaled<int8_t>(5e3, true);
+				for (int ind = 0; ind < 3; ++ind)
+					outData.ABias[ind] = readScaled<int8_t>(5e4, true);
+				readScaled<uint8_t>(1, false);
+				break;
+			case 0x27:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.AccPVPoint[ind] = readScaled<int32_t>(1e5, ind < 2);
+				break;
+			case 0x30:
+				outData.LatGNSS = readScaled<int32_t>(1e7, true);
+				outData.LonGNSS = readScaled<int32_t>(1e7, true);
+				outData.AltGNSS = readScaled<int32_t>(1e2, false);
+				break;
+			case 0x31:
+				outData.LatGNSS = readScaled<int64_t>(1e9, true);
+				outData.LonGNSS = readScaled<int64_t>(1e9, true);
+				outData.AltGNSS = readScaled<int32_t>(1e3, false);
+				break;
+			case 0x32:
+				outData.V_Hor = readScaled<int32_t>(100, true);
+				outData.Trk_gnd = readScaled<uint16_t>(100, true);
+				outData.V_ver = readScaled<int32_t>(100, false);
+				break;
+			case 0x33:
+				outData.Heading_GNSS = readScaled<uint16_t>(100, true);
+				outData.Pitch_GNSS = readScaled<int16_t>(100, false);
+				break;
+			case 0x34:
+				outData.LatGNSSStd = readScaled<uint16_t>(1000, true);
+				outData.LonGNSSStd = readScaled<uint16_t>(1000, true);
+				outData.AltGNSSStd = readScaled<uint16_t>(1000, false);
+				break;
+			case 0x35:
+				outData.HeadingGNSSStd = readScaled<uint16_t>(100, true);
+				outData.PitchGNSSStd = readScaled<uint16_t>(100, false);
+				break;
+			case 0x36:
+				outData.GNSSInfo1 = readScaled<uint8_t>(1, true, 16);
+				outData.GNSSInfo2 = readScaled<uint8_t>(1, false, 16);
+				break;
+			case 0x37:
+				outData.SVtrack = readScaled<uint8_t>(1, true);
+				outData.SVsol = readScaled<uint8_t>(1, true);
+				outData.SVsolL1 = readScaled<uint8_t>(1, true);
+				outData.SVSolMulti = readScaled<uint8_t>(1, true);
+				outData.GalBD = readScaled<uint8_t>(1, true, 16);
+				outData.GPSGlo = readScaled<uint8_t>(1, true, 16);
+				outData.TimeStatus = readScaled<uint8_t>(1, true);
+				outData.ExtSolStatus = readScaled<uint8_t>(1, false, 16);
+				break;
+			case 0x38:
+				outData.GNSSSolStatus = readScaled<uint8_t>(1, false, 16);
+				break;
+			case 0x39:
+				outData.GNSSSolType = readScaled<uint8_t>(1, false);
+				break;
+			case 0x3A:
+				outData.AnglesType = readScaled<uint8_t>(1, false);
+				break;
+			case 0x3B:
+				outData.SVsol = readScaled<uint8_t>(1, false);
+				break;
+			case 0x3C:
+				outData.Week = readScaled<uint16_t>(1, false);
+				break;
+			case 0x3D:
+				outData.GNSSVelLatency = readScaled<uint16_t>(1, false);
+				break;
+			case 0x3E:
+				outData.GNSSPosMs = readScaled<uint32_t>(1, false);
+				break;
+			case 0x3F:
+				outData.GNSSVelMs = readScaled<uint32_t>(1, false);
+				break;
+			case 0x40:
+				outData.GNSSHdgMs = readScaled<uint32_t>(1, false);
+				break;
+			case 0x41:
+				outData.NewGPS = readScaled<uint8_t>(1, false, 16);
+				break;
+			case 0x42:
+				outData.GDOP = readScaled<uint16_t>(1000, true);
+				outData.PDOP = readScaled<uint16_t>(1000, true);
+				outData.HDOP = readScaled<uint16_t>(1000, true);
+				outData.VDOP = readScaled<uint16_t>(1000, true);
+				outData.TDOP = readScaled<uint16_t>(1000, false);
+				break;
+			case 0x43:
+				outData.GNSS_PACC = readScaled<uint16_t>(100, true);
+				outData.GNSS_VACC = readScaled<uint16_t>(100, false);
+				break;
+			case 0x44:
+				outData.GDOP = readScaled<uint16_t>(1000, true);
+				outData.PDOP = readScaled<uint16_t>(1000, false);
+				break;
+			case 0x45:
+				outData.Trk_gnd = readScaled<uint16_t>(100, false);
+				break;
+			case 0x47:
+				outData.DiffAge = readScaled<uint16_t>(10, false);
+				break;
+			case 0x48:
+				outData.GNSS_ECEF_VXStd = readScaled<uint16_t>(1000, true);
+				outData.GNSS_ECEF_VYStd = readScaled<uint16_t>(1000, true);
+				outData.GNSS_ECEF_VZStd = readScaled<uint16_t>(1000, true);
+				break;
+			case 0x49:
+				outData.PPPApp = readScaled<uint8_t>(1, true);
+				outData.PPPStore = readScaled<uint8_t>(1, false);
+				break;
+			case 0x50:
+				outData.VSup = readScaled<uint16_t>(100, false);
+				break;
+			case 0x51:
+				outData.VStab = readScaled<uint16_t>(1000, false);
+				break;
+			case 0x52:
+				outData.Temp = readScaled<int16_t>(10, false);
+				break;
+			case 0x53:
+				outData.USW = readScaled<uint16_t>(1, false, 16);
+				break;
+			case 0x54:
+				outData.INSSolStatus = readScaled<uint8_t>(1, false);
+				break;
+			case 0x55:
+				outData.KFLatStd = readScaled<uint8_t>(100, true);
+				outData.KFLonStd = readScaled<uint8_t>(100, true);
+				outData.KFAltStd = readScaled<uint8_t>(100, false);
+				break;
+			case 0x56:
+				outData.KFHdgStd = readScaled<uint8_t>(100, false);
+				break;
+			case 0x57:
+				outData.KFLatStd = readScaled<uint16_t>(1000, true);
+				outData.KFLonStd = readScaled<uint16_t>(1000, true);
+				outData.KFAltStd = readScaled<uint16_t>(1000, false);
+				break;
+			case 0x58:
+				outData.KFVelStd[0] = readScaled<uint8_t>(1000, true);
+				outData.KFVelStd[1] = readScaled<uint8_t>(1000, true);
+				outData.KFVelStd[2] = readScaled<uint8_t>(1000, false);
+				break;
+			case 0x60:
+				outData.Odometer = readScaled<int32_t>(1000, false);
+				break;
+			case 0x61:
+				outData.AirSpeed = readScaled<int16_t>(1000, false);
+				break;
+			case 0x62:
+				outData.WindN = readScaled<int16_t>(100, true);
+				outData.WindE = readScaled<int16_t>(100, true);
+				outData.WindNStd = readScaled<uint16_t>(100, true);
+				outData.WindNStd = readScaled<uint16_t>(100, false);
+				break;
+			case 0x63:
+				outData.LatExt = readScaled<int32_t>(1e7, true);
+				outData.LonExt = readScaled<int32_t>(1e7, true);
+				outData.AltExt = readScaled<int32_t>(1e3, true);
+				outData.LatExtStd = readScaled<uint16_t>(100, true);
+				outData.LonExtStd = readScaled<uint16_t>(100, true);
+				outData.AltExtStd = readScaled<uint16_t>(100, true);
+				outData.ExtPosLatency = readScaled<uint16_t>(1000, false);
+				break;
+			case 0x64:
+				outData.LocLat = readScaled<int32_t>(1e7, true);
+				outData.LocLon = readScaled<int32_t>(1e7, true);
+				outData.LocAlt = readScaled<int32_t>(1e3, true);
+				outData.LocDopplerShift = readScaled<int16_t>(100, true);
+				outData.LocDopplerShiftStd = readScaled<uint16_t>(100, false);
+				break;
+			case 0x65:
+				outData.NewAiding = readScaled<uint16_t>(1, false, 16);
+				break;
+			case 0x66:
+				outData.HdgExt = readScaled<uint16_t>(100, true);
+				outData.HdgExtStd = readScaled<uint16_t>(100, true);
+				outData.HdgExtLatency = readScaled<uint16_t>(1000, false);
+				break;
+			case 0x67:
+				outData.DVLRight = readScaled<int32_t>(1e3, true);
+				outData.DVLFwd = readScaled<int32_t>(1e3, true);
+				outData.DVLUp = readScaled<int32_t>(1e3, true);
+				outData.DVLRightStd = readScaled<uint16_t>(1000, true);
+				outData.DVLFwdStd = readScaled<uint16_t>(1000, true);
+				outData.DVLUpStd = readScaled<uint16_t>(1000, true);
+				outData.DVLLatency = readScaled<uint16_t>(1000, false);
+				outData.DVLPressure = readScaled<uint32_t>(0.1, true);
+				break;
+			case 0x68:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.GBExt[ind] = readScaled<int8_t>(5e3, true);
+				for (int ind = 0; ind < 3; ++ind)
+					outData.ABExt[ind] = readScaled<int8_t>(5e4, true);
+				readScaled<uint8_t>(1, false);
+				break;
+			case 0x69:
+				outData.PitchExt = readScaled<int16_t>(100, true);
+				outData.RollExt = readScaled<int16_t>(100, false);
+				break;
+			case 0x6A:
+				for (int ind = 0; ind < 3; ++ind)
+					outData.ExtAntPri[ind] = readScaled<int16_t>(1e3, true);
+				for (int ind = 0; ind < 3; ++ind)
+					outData.ExtAntSec[ind] = readScaled<int16_t>(1e3, ind < 2);
+				break;
+			case 0xF0:
+				outData.Latency_ms_pos = readScaled<uint8_t>(1, true);
+				outData.Latency_ms_vel = readScaled<uint8_t>(1, false);
+				break;
+			case 0xF1:
+				outData.Latency_ms_head = readScaled<uint16_t>(1, false);
+				break;
+			case 0xF2:
+				outData.Heading_GNSS = readScaled<uint16_t>(100, false);
+				break;
+			case 0xF3:
+				outData.TimeStatus = readScaled<uint8_t>(1, true);
+				outData.GNSSSolStatus = readScaled<uint8_t>(1, true);
+				outData.GNSSSolType = readScaled<uint8_t>(1, true);
+				outData.SVtrack = readScaled<uint8_t>(1, true);
+				outData.SVsol = readScaled<uint8_t>(1, true);
+				outData.SVsolL1 = readScaled<uint8_t>(1, true);
+				outData.SVSolMulti = readScaled<uint8_t>(1, true);
+				outData.ExtSolStatus = readScaled<uint8_t>(1, true, 16);
+				outData.GalBD = readScaled<uint8_t>(1, true);
+				outData.GPSGlo = readScaled<uint8_t>(1, false);
+				break;
+			case 0xF4:
+				outData.UP = readScaled<uint16_t>(1, true);
+				outData.UT = readScaled<uint16_t>(1, false);
+				break;
+			case 0xF5:
+				outData.GNSSInfo1 = readScaled<uint8_t>(1, false, 16);
+				break;
+			case 0xF6:
+				outData.pBar = readScaled<uint16_t>(0.5, true);
+				outData.Temp = readScaled<uint16_t>(100, true);
+				outData.USW = readScaled<uint16_t>(1, true, 16);
+				outData.GNSSSolType = readScaled<uint8_t>(1, true);
+				outData.GNSS_ECEF_X = readScaled<int32_t>(100, true);
+				outData.GNSS_ECEF_Y = readScaled<int32_t>(100, true);
+				outData.GNSS_ECEF_Z = readScaled<int32_t>(100, true);
+				outData.GNSS_PACC = readScaled<uint16_t>(100, true);
+				outData.GNSS_ECEF_VX = readScaled<int32_t>(1e6, true);
+				outData.GNSS_ECEF_VY = readScaled<int32_t>(1e6, true);
+				outData.GNSS_ECEF_VZ = readScaled<int32_t>(1e6, true);
+				outData.GNSS_VACC = readScaled<uint16_t>(100, true);
+				outData.LatGNSS = readScaled<int64_t>(1e9, true);
+				outData.LonGNSS = readScaled<int64_t>(1e9, true);
+				outData.AltGNSS = readScaled<int32_t>(1e3, true);
+				outData.V_Hor = readScaled<int32_t>(1e6, true);
+				outData.Trk_gnd = readScaled<int32_t>(1e6, true);
+				outData.V_ver = readScaled<int32_t>(1e6, true);
+				outData.AnglesType = readScaled<uint8_t>(1, true);
+				outData.Heading_GNSS = readScaled<uint16_t>(100, true);
+				outData.SVsol = readScaled<uint8_t>(1, true);
+				outData.GNSSInfo1 = readScaled<uint8_t>(1, true, 16);
+				outData.GNSSInfo2 = readScaled<uint8_t>(1, true, 16);
+				outData.GDOP = readScaled<uint16_t>(1e3, true);
+				outData.PDOP = readScaled<uint16_t>(1e3, true);
+				outData.HDOP = readScaled<uint16_t>(1e3, true);
+				outData.VDOP = readScaled<uint16_t>(1e3, true);
+				outData.TDOP = readScaled<uint16_t>(1e3, true);
+				outData.DiffAge = readScaled<uint16_t>(10, true);
+				outData.UTC_Hour = readScaled<uint8_t>(1, true);
+				outData.UTC_Minute = readScaled<uint8_t>(1, true);
+				outData.UTC_Second = readScaled<uint8_t>(1, true);
+				outData.UTC_DecSec = readScaled<uint16_t>(1e3, true);
+				outData.UTC_Day = readScaled<uint8_t>(1, true);
+				outData.UTC_Month = readScaled<uint8_t>(1, true);
+				outData.UTC_Year = readScaled<uint16_t>(1, true);
+				outData.UTCSecSinceEpoch = readScaled<int64_t>(1, true);
+				outData.LatencyECEF = readScaled<uint8_t>(1, false);
+				break;
+			case 0xF7:
+				outData.Latency_ms_pos = readScaled<int16_t>(1, true);
+				outData.Latency_ms_vel = readScaled<int16_t>(1, false);
+				break;
+			case 0xFE:
+				readScaled<uint8_t>(1, false, 16);
+				break;
+			case 0xFF:
+				readScaled<uint16_t>(1, false, 16);
+				break;
+			default:
+				//			cout << "Unknown data!" << endl;
+				break;
+			}
+			if (i < dataSet.size() - 1)
+				txtStream << "\t";
+			else
+				txtStream << "\n";
+		}
+	}
+
+	void UDDParser::finish()
+	{
+	}
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/UDDParser.h b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/UDDParser.h
new file mode 100644
index 00000000..86d74dbb
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/UDDParser.h
@@ -0,0 +1,90 @@
+#pragma once
+#include <cstdint>
+#include <string>
+#include <fstream>
+#include <sstream>
+#include <iomanip>
+#include <vector>
+#include <cmath>
+#include <string.h>
+#include "dataStructures.h"
+
+
+namespace IL {
+
+	class UDDParser
+	{
+	public:
+		UDDParser();
+		~UDDParser();
+
+		int parse();
+		void writeHeader();
+		void writeTxtAndData();
+		void finish();
+		int KA;
+		double SA;
+		int KG;
+		double SG;
+		double SO;
+		double SV;
+		uint8_t code;
+		uint16_t payloadLen;
+		int payloadInd;
+		uint8_t payloadBuf[65530];
+		std::string dataSet;
+		std::string oldDataSet;
+		std::string fileName;
+		std::stringstream txtStream;
+		std::stringstream hdrStream;
+		std::stringstream statusStream;
+		double dVal;
+		float fVal;
+		int64_t i64val;
+		uint64_t ui64val;
+		int32_t i32val;
+		uint32_t ui32val;
+		int16_t i16val;
+		uint16_t ui16val;
+		int8_t i8val;
+		uint8_t ui8val;
+		bool parseError;
+		bool high_precision_heave;
+		INSDataStruct outData;
+
+		template<typename T>
+		void readVal(T& val) {
+			parseError = (payloadLen - payloadInd < sizeof(val));
+			if (!parseError)
+			{
+				memcpy(&val, &payloadBuf[payloadInd], sizeof(val));
+				payloadInd += sizeof(val);
+			}
+		}
+
+		template<typename T>
+		double readScaled(double scale, bool tab = false, int base = 10)
+		{
+			T val;
+			parseError = (payloadLen - payloadInd < sizeof(val));
+			if (!parseError)
+			{
+				memcpy(&val, &payloadBuf[payloadInd], sizeof(val));
+				//			val = *reinterpret_cast<T*>(&payloadBuf[payloadInd]);
+				payloadInd += sizeof(val);
+			}
+			double res = static_cast<double>(val) / scale;
+			int precision = static_cast<int>(std::log10(scale) + 0.5);
+			if (precision < 0) precision = 0;
+			if (8 != base && 16 != base)
+				base = 10;
+			if (10 == base)
+				txtStream << std::fixed << std::setprecision(precision) << res;
+			else
+				txtStream << std::setprecision(0) << std::setbase(base) << std::setw(sizeof(T) * 8 / std::log2(base)) << std::setfill('0') << static_cast<int>(res);
+			if (tab) txtStream << "\t";
+			return res;
+		}
+	};
+
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/dataStructures.h b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/dataStructures.h
new file mode 100644
index 00000000..f157bc10
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/dataStructures.h
@@ -0,0 +1,218 @@
+#pragma once
+#include <cstdint>
+
+namespace IL {
+
+	namespace PacketType {
+		const uint8_t Cobham_UAV200_Satcom = 0x46;
+		const uint8_t IL_Sensors = 0x50;
+		const uint8_t IL_OPVT = 0x52;
+		const uint8_t IL_min = 0x53;
+		const uint8_t IL_NMEA = 0x54;
+		const uint8_t IL_Sensors_NMEA = 0x55;
+		const uint8_t IL_QPVT = 0x56;
+		const uint8_t IL_OPVT2A = 0x57;
+		const uint8_t IL_OPVT2AHR = 0x58;
+		const uint8_t IL_OPVT2AW = 0x59;
+		const uint8_t IL_OPVTAD = 0x61;
+		const uint8_t MRU_OPVTHSSHR = 0x64;
+		const uint8_t IL_OPVT_rawIMU = 0x66;
+		const uint8_t IL_OPVT_GNSSext = 0x67;
+		const uint8_t SPAN_rawIMU = 0x68;
+		const uint8_t IL_UDD = 0x95;
+	};
+
+	struct INSDataStruct
+	{
+		unsigned int ms_gps;
+		double GPS_INS_Time;
+		double GPS_IMU_Time;
+		int UTC_Hour;
+		int UTC_Minute;
+		int UTC_Second;
+		double UTC_DecSec;
+		int UTC_Day;
+		int UTC_Month;
+		int UTC_Year;
+		uint64_t UTCSecSinceEpoch;
+		double Heading;
+		double Pitch;
+		double Roll;
+		double Quat[4];
+		double Latitude;
+		double Longitude;
+		double Altitude;
+		double VelENU[3];
+		double Gyro[3];
+		double Acc[3];
+		double AccPVPoint[3];
+		double Mag[3];
+		double pBar;
+		double hBar;
+		double GBias[3];
+		double ABias[3];
+		double LatGNSS;
+		double LonGNSS;
+		double AltGNSS;
+		double V_Hor;
+		double Trk_gnd;
+		double V_ver;
+		double Heading_GNSS;
+		double Pitch_GNSS;
+		double LatGNSSStd;
+		double LonGNSSStd;
+		double AltGNSSStd;
+		double HeadingGNSSStd;
+		double PitchGNSSStd;
+		int GNSSInfo1;
+		int GNSSInfo2;
+		int SVtrack;
+		int SVsol;
+		int SVsolL1;
+		int SVSolMulti;
+		int GalBD;
+		int GPSGlo;
+		int TimeStatus;
+		int ExtSolStatus;
+		int GNSSSolStatus;
+		int GNSSSolType;
+		int AnglesType;
+		int Week;
+		int GNSSVelLatency;
+		int GNSSPosMs;
+		int GNSSVelMs;
+		int GNSSHdgMs;
+		int NewGPS;
+		double GDOP;
+		double PDOP;
+		double HDOP;
+		double VDOP;
+		double TDOP;
+		double GNSS_PACC;
+		double GNSS_VACC;
+		double VSup;
+		double VStab;
+		double Temp;
+		int USW;
+		int INSSolStatus;
+		double KFLatStd;
+		double KFLonStd;
+		double KFAltStd;
+		double KFHdgStd;
+		double KFVelStd[3];
+		double Odometer;
+		double AirSpeed;
+		double WindN;
+		double WindE;
+		double WindNStd;
+		double WindEStd;
+		double LatExt;
+		double LonExt;
+		double AltExt;
+		double LatExtStd;
+		double LonExtStd;
+		double AltExtStd;
+		double ExtPosLatency;
+		double LocLat;
+		double LocLon;
+		double LocAlt;
+		double LocDopplerShift;
+		double LocDopplerShiftStd;
+		double ExtAntPri[3];
+		double ExtAntSec[3];
+		int NewAiding;
+		double HdgExt;
+		double HdgExtStd;
+		double HdgExtLatency;
+		double DVLRight;
+		double DVLFwd;
+		double DVLUp;
+		double DVLRightStd;
+		double DVLFwdStd;
+		double DVLUpStd;
+		double DVLLatency;
+		double DVLPressure;
+		double GBExt[3];
+		double ABExt[3];
+		double PitchExt;
+		double RollExt;
+		int Latency_ms_pos;
+		int Latency_ms_vel;
+		int Latency_ms_head;
+		int UP;
+		int UT;
+		double GNSS_ECEF_X;
+		double GNSS_ECEF_Y;
+		double GNSS_ECEF_Z;
+		double GNSS_ECEF_VX;
+		double GNSS_ECEF_VY;
+		double GNSS_ECEF_VZ;
+		double GNSS_ECEF_VXStd;
+		double GNSS_ECEF_VYStd;
+		double GNSS_ECEF_VZStd;
+		double DiffAge;
+		int LatencyECEF;
+		int PPPStore;
+		int PPPApp;
+		uint64_t dataPresent[8];
+
+		double Heave;
+		double Surge;
+		double Sway;
+		double Heave_velocity;
+		double Surge_velocity;
+		double Sway_velocity;
+		double significant_wave_height;
+	};
+
+
+#ifdef _MSC_VER
+#pragma pack(push,1)
+#endif
+	struct INSDeviceInfo {
+		char IDN[8];
+		char FW[40];
+		uint8_t pressSensor;
+		uint8_t imuType;
+		char imuSN[8];
+		char imuFW[40];
+		char GNSSmodel[16];
+		char GNSSsn[16];
+		char GNSShw[16];
+		char GNSSfw[16];
+		uint16_t week;
+		uint8_t GNSSmaxRate;
+		uint8_t reserved;
+#ifdef __GNUC__
+	} __attribute__((packed));
+#else
+};
+#endif
+	struct INSDevicePar {
+		uint16_t dataRate;
+		uint16_t initAlignmentTime;
+		int32_t magDeclination;
+		int32_t Lat;
+		int32_t Lon;
+		int32_t Alt;
+		uint8_t YearSince1900;
+		uint8_t Month;
+		uint8_t Day;
+		int16_t AlignmentAngles[3];
+		int16_t COGtoINSleverArm[3];
+		int16_t AntennaToINSleverArm[3];
+		uint8_t GeoidAltitude;
+		char reserved1[8];
+		char IDN[8];
+		uint8_t enableBaroAltimeter;
+		char reserved2;
+#ifdef __GNUC__
+	} __attribute__((packed));
+#else
+};
+#endif
+#ifdef _MSC_VER
+#pragma pack(pop)
+#endif
+
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/example.cpp b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/example.cpp
new file mode 100644
index 00000000..fdda75bd
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/example.cpp
@@ -0,0 +1,68 @@
+#include "ILDriver.h"
+#include <iostream>
+
+
+void callback(IL::INSDataStruct* data, void* context)
+{
+    std::cout << *reinterpret_cast<std::string*>(context) << " Time: " << data->GPS_INS_Time << ", USW = " << data->USW << std::endl;
+}
+
+int main()
+{
+    IL::Driver driver;
+    uint8_t packetType = IL::PacketType::IL_OPVT;
+    std::cout << "Enter INS URL ([serial/tcp/udp]:[com port path / hostname or IP]:[baud rate or TCP/UDP port]: ";
+    std::string URL;
+    std::cin >> URL;
+    if (driver.connect(URL.c_str()) != 0)
+    {
+        std::cout << "IL Driver failed to connect!" << std::endl;
+        return 1;
+    }
+    if (driver.isStarted()) {
+        driver.stop();
+    }
+    IL::INSDeviceInfo devInfo = driver.getDeviceInfo();
+    IL::INSDevicePar devParams = driver.getDeviceParams();
+    std::cout << "Found INS " << std::string(devInfo.IDN,8) << " providing data at " << devParams.dataRate << " Hz" << std::endl;
+    std::string context(devInfo.IDN, 8);
+    int res;
+    driver.setCallback(&callback, &context);
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    std::cout << "Starting device in polling mode" << std::endl;
+    if ((res = driver.start(packetType, true)))
+    {
+        std::cout << "INS failed to start! " << res << std::endl;
+        driver.disconnect();
+        return 2;
+    }
+    for (int i = 0; i < 5; ++i)
+    {
+        if (driver.request(0x52, 100) == 0) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        else
+        {
+            std::cout << "INS failed to fulfill request!" << std::endl;
+            break;
+        }
+    }
+    std::cout << "Stopping device" << std::endl;
+    driver.stop();
+    std::cout << "Starting device in continuous mode" << std::endl;
+    if ((res = driver.start(packetType, false)))
+    {
+        std::cout << "INS failed to start! " << res << std::endl;
+        driver.disconnect();
+        return 2;
+    }
+    for (int i = 0; i < 5; ++i)
+    {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+    std::cout << "Stopping device" << std::endl;
+    driver.stop();
+    std::cout << "Closing device" << std::endl;
+    driver.disconnect();
+    return 0;
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/linux/NetClient.cpp b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/linux/NetClient.cpp
new file mode 100644
index 00000000..f85ce61d
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/linux/NetClient.cpp
@@ -0,0 +1,126 @@
+#include <unistd.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <cerrno>
+#include <string>
+#include <cstdio>
+#include <cstring>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+#include "../../NetClient.h"
+
+namespace IL {
+	NetClient::NetClient()
+		: fd(-1)
+		, timeout(1000)
+		, type(None)
+		, addr(nullptr)
+	{
+	}
+
+	NetClient::~NetClient()
+	{
+		close();
+		if (addr) {
+			delete addr;
+		}
+	}
+
+	int NetClient::open(const char* url)
+	{
+		if (!addr) {
+			addr = new sockaddr_in;
+			memset(addr, 0, sizeof(sockaddr_in));
+		}
+		std::string urlStr(url);
+		size_t pos = urlStr.find(':');
+		if (pos == std::string::npos) return 1;
+		std::string addrStr = urlStr.substr(pos + 1);
+		std::string protoStr = urlStr.substr(0, pos);
+		pos = addrStr.find(':');
+		if (pos == std::string::npos) return 1;
+		std::string portStr = addrStr.substr(pos + 1);
+		std::string hostStr = addrStr.substr(0, pos);
+		struct addrinfo hints = {};
+		struct addrinfo* result, * rp;
+		hints.ai_family = AF_INET;
+		if (protoStr == "tcp") {
+			hints.ai_socktype = SOCK_STREAM;
+			type = Tcp;
+		} else if (protoStr == "udp") {
+			hints.ai_socktype = SOCK_DGRAM;
+			type = Udp;
+		}
+		else return 1;
+		hints.ai_flags = 0;
+		hints.ai_protocol = 0;
+		int res = getaddrinfo(hostStr.c_str(), portStr.c_str(), &hints, &result);
+		if (res != 0) return -res;
+		for (rp = result; rp != NULL; rp = rp->ai_next) {
+			fd = socket(rp->ai_family, rp->ai_socktype,	rp->ai_protocol);
+			if (fd == -1) continue;
+
+			if (type == Tcp) {
+				if (connect(fd, rp->ai_addr, rp->ai_addrlen) != -1)	break;
+				::close(fd);
+				fd = -1;
+			}
+		}
+		freeaddrinfo(result);
+
+		if (type == Udp) {
+			addr->sin_family = AF_INET;
+			addr->sin_port = htons(stoi(portStr));
+			addr->sin_addr.s_addr = INADDR_ANY;
+		}
+		if (fd < 0)	return 2;
+		return 0;
+	}
+
+	bool NetClient::isOpen()
+	{
+		return fd >= 0;
+	}
+
+	void NetClient::close()
+	{
+		if (isOpen()) {
+			::close(fd);
+			fd = -1;
+		}
+	}
+
+	int NetClient::read(char* buf, unsigned int size)
+	{
+		if (fd < 0) return -1;
+		pollfd fds;
+		fds.fd = fd;
+		fds.events = POLLIN  | POLLHUP | POLLERR;
+		fds.revents = 0;
+		int result = poll(&fds, 1, timeout);
+		if (result < 0) return -errno;
+		if (!result) return 0;
+		if (type == Udp) {
+			socklen_t slen = sizeof(sockaddr_in);
+			result = ::recvfrom(fd, buf, size, 0, (sockaddr*)addr, &slen);
+		} else {
+			result = ::read(fd, buf, size);
+		}
+		if (result < 0)	return -errno;
+		return result;
+	}
+
+	int NetClient::write(char* buf, unsigned int size)
+	{
+		int result = -1;
+		if (type == Udp) {
+			socklen_t slen = sizeof(sockaddr_in);
+			result = ::sendto(fd, buf, size, 0 , (struct sockaddr *) addr, slen);
+		} else {
+			result = ::write(fd, buf, size);
+		}
+		if (result < 0)	return -errno;
+		return result;
+	}
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/linux/SerialPort.cpp b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/linux/SerialPort.cpp
new file mode 100644
index 00000000..36e62c6d
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/linux/SerialPort.cpp
@@ -0,0 +1,95 @@
+#include <unistd.h>
+#include <poll.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <cerrno>
+#include <string>
+#include <cstdio>
+#include "../../SerialPort.h"
+
+namespace IL {
+
+	SerialPort::SerialPort()
+		: fd(-1)
+		, timeout(1000)
+	{
+	}
+
+	SerialPort::~SerialPort()
+	{
+		close();
+	}
+
+	int SerialPort::open(const char* url)
+	{
+		std::string urlStr(url);
+		std::string pathStr;
+		tcflag_t baudrate;
+		size_t pos = urlStr.find(':');
+		if (pos != std::string::npos) {
+			std::string param = urlStr.substr(pos + 1);
+			if (param == "4800") baudrate = B4800;
+			else if (param == "9600") baudrate = B9600;
+			else if (param == "19200") baudrate = B19200;
+			else if (param == "38400") baudrate = B38400;
+			else if (param == "57600") baudrate = B57600;
+			else if (param == "115200") baudrate = B115200;
+			else if (param == "230400") baudrate = B230400;
+			else if (param == "460800") baudrate = B460800;
+			else if (param == "921600") baudrate = B921600;
+			else if (param == "2000000") baudrate = B2000000;
+			else  return 2;
+			pathStr = urlStr.substr(0, pos);
+		}
+		else {
+			baudrate = B115200;
+			pathStr = urlStr;
+		}
+		fd = ::open(pathStr.c_str(), O_NOCTTY | O_RDWR);
+		if (fd < 0) return -errno;
+		struct termios config;
+		if (tcgetattr(fd, &config) < 0)	return 1;
+		cfmakeraw(&config);
+		config.c_cflag = baudrate & ~(CSIZE | PARENB);
+		config.c_cflag |= CS8 | CREAD | CLOCAL;
+		config.c_lflag |= IEXTEN;
+		if (tcsetattr(fd, TCSANOW, &config) < 0) return 3;
+		return 0;
+	}
+
+	bool SerialPort::isOpen()
+	{
+		return fd >= 0;
+	}
+
+	void SerialPort::close()
+	{
+		if (isOpen())
+		{
+			::close(fd);
+			fd = -1;
+		}
+	}
+
+	int SerialPort::read(char* buf, unsigned int size)
+	{
+		if (!isatty(fd)) return -1;
+		pollfd fds;
+		fds.fd = fd;
+		fds.events = POLLIN;
+		fds.revents = 0;
+		int result = poll(&fds, 1, timeout);
+		if (result < 0)	return -errno;
+		if (!result) return 0;
+		result = ::read(fd, buf, size);
+		if (result < 0)	return -errno;
+		return result;
+	}
+
+	int SerialPort::write(char* buf, unsigned int size)
+	{
+		int result = ::write(fd, buf, size);
+		if (result < 0)	return -errno;
+		return result;
+	}
+}
\ No newline at end of file
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/windows/NetClient.cpp b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/windows/NetClient.cpp
new file mode 100644
index 00000000..1e914fca
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/windows/NetClient.cpp
@@ -0,0 +1,123 @@
+#include <WS2tcpip.h>
+#include <string>
+#include <cstring>
+#include "../../NetClient.h"
+
+#pragma comment(lib, "Ws2_32.lib")
+
+namespace IL {
+	NetClient::NetClient()
+		: timeout(1000)
+		, hCom(INVALID_SOCKET)
+		, type(None)
+		, addr(nullptr)
+	{
+		WSADATA data;
+		WSAStartup(MAKEWORD(2, 2), &data);
+	}
+
+	NetClient::~NetClient()
+	{
+		close();
+		WSACleanup();
+		if (addr) {
+			delete addr;
+		}
+	}
+
+	int NetClient::open(const char* url)
+	{
+		if (!addr) {
+			addr = new sockaddr_in;
+			memset(addr, 0, sizeof(sockaddr_in));
+		}
+		std::string urlStr(url);
+		size_t pos = urlStr.find(':');
+		if (pos == std::string::npos) return 1;
+		std::string addrStr = urlStr.substr(pos + 1);
+		std::string protoStr = urlStr.substr(0, pos);
+		pos = addrStr.find(':');
+		if (pos == std::string::npos) return 1;
+		std::string portStr = addrStr.substr(pos + 1);
+		std::string hostStr = addrStr.substr(0, pos);
+		struct addrinfo hints = {};
+		struct addrinfo* result, * rp;
+		hints.ai_family = AF_INET;
+		if (protoStr == "tcp") {
+			hints.ai_socktype = SOCK_STREAM;
+			type = Tcp;
+		} else if (protoStr == "udp") {
+			hints.ai_socktype = SOCK_DGRAM;
+			type = Udp;
+		}
+		else return 1;
+		hints.ai_flags = 0;
+		hints.ai_protocol = 0;
+		int res = getaddrinfo(hostStr.c_str(), portStr.c_str(), &hints, &result);
+		if (res != 0) return -res;
+		for (rp = result; rp != NULL; rp = rp->ai_next) {
+			hCom = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
+			if (hCom == INVALID_SOCKET) continue;
+
+			if (type == Tcp) {
+				if (connect(hCom, rp->ai_addr, rp->ai_addrlen) != SOCKET_ERROR)	break;
+				closesocket(hCom);
+				hCom = INVALID_SOCKET;
+			}
+		}
+		freeaddrinfo(result);
+
+		if (type == Udp) {
+			addr->sin_family = AF_INET;
+			addr->sin_port = htons(stoi(portStr));
+			addr->sin_addr.s_addr = INADDR_ANY;
+		}
+		if (hCom == INVALID_SOCKET) return 2;
+		return 0;
+	}
+
+	bool NetClient::isOpen()
+	{
+		return hCom != INVALID_SOCKET;
+	}
+
+	void NetClient::close()
+	{
+		if (isOpen()) {
+			closesocket(hCom);
+			hCom = INVALID_SOCKET;
+		}
+	}
+
+	int NetClient::read(char* buf, unsigned int size)
+	{
+		if (hCom == INVALID_SOCKET) return -1;
+		fd_set readfds = { 1, {hCom} };
+		TIMEVAL tv_timeout = {timeout / 1000, (timeout % 1000) * 1000};
+		int result = select(1, &readfds, nullptr, nullptr, &tv_timeout);
+		if (result == SOCKET_ERROR) return -WSAGetLastError();
+		if (!result) return 0;
+		if (type == Udp) {
+			socklen_t slen = sizeof(sockaddr_in);
+			result = ::recvfrom(fd, buf, size, 0, (sockaddr*)addr, &slen);
+		} else {
+			result = ::recv(fd, buf, size);
+		}
+		int err;
+		if (result == SOCKET_ERROR && (err = WSAGetLastError()) != WSAEMSGSIZE)	return -err;
+		return result;
+	}
+
+	int NetClient::write(char* buf, unsigned int size)
+	{
+		int result = -1;
+		if (type == Udp) {
+			socklen_t slen = sizeof(sockaddr_in);
+			result = ::sendto(hCom, buf, size, 0 , (struct sockaddr *) addr, slen);
+		} else {
+			result = ::send(hCom, buf, size);
+		}
+		if (result == SOCKET_ERROR)	return -WSAGetLastError();
+		return result;
+	}
+}
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/windows/SerialPort.cpp b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/windows/SerialPort.cpp
new file mode 100644
index 00000000..b55d2482
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/platforms/windows/SerialPort.cpp
@@ -0,0 +1,76 @@
+#include "../../SerialPort.h"
+#include <windows.h>
+#include <string>
+#include <cstdio>
+
+namespace IL {
+
+	SerialPort::SerialPort()
+		: hCom(INVALID_HANDLE_VALUE)
+		, timeout(1000)
+	{
+	}
+
+	SerialPort::~SerialPort()
+	{
+		close();
+	}
+
+	int SerialPort::open(const char* url)
+	{
+		std::string urlStr(url);
+		std::string pathStr;
+		std::string baudrate;
+		size_t pos = urlStr.find(':');
+		if (pos != std::string::npos) {
+			baudrate = urlStr.substr(pos + 1);
+			pathStr = urlStr.substr(0, pos);
+		}
+		else {
+			baudrate = "115200";
+			pathStr = urlStr;
+		}
+		hCom = CreateFileA(pathStr.c_str(), GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, nullptr);
+		if (hCom == INVALID_HANDLE_VALUE) return -static_cast<int>(GetLastError());
+		COMMTIMEOUTS timeouts = { MAXDWORD,MAXDWORD,MAXDWORD,1,1 };
+		timeouts.ReadTotalTimeoutConstant = timeout;
+		if (!SetCommTimeouts(hCom, &timeouts)) return 1;
+		COMMPROP commProp = {};
+		if (!GetCommProperties(hCom, &commProp)) return 2;
+		if (!(commProp.dwSettableBaud & BAUD_USER))	return 3;
+		_DCB config;
+		config.DCBlength = sizeof(config);
+		if (!GetCommState(hCom, &config)) return 4;
+		if (!BuildCommDCBA(("baud=" + baudrate + " parity=N data=8 stop=1 to=off xon=off odsr=off octs=off dtr=off rts=off idsr=off").c_str(), &config)) return 5;
+		if (!SetCommState(hCom, &config)) return -static_cast<int>(GetLastError());
+		return 0;
+	}
+
+	bool SerialPort::isOpen()
+	{
+		return hCom != INVALID_HANDLE_VALUE;
+	}
+
+	void SerialPort::close()
+	{
+		if (isOpen())
+		{
+			CloseHandle(hCom);
+			hCom = INVALID_HANDLE_VALUE;
+		}
+	}
+
+	int SerialPort::read(char* buf, unsigned int size)
+	{
+		DWORD bytesRead = 0;
+		if (!ReadFile(hCom, buf, size, &bytesRead, nullptr)) return -static_cast<int>(GetLastError());
+		return bytesRead;
+	}
+
+	int SerialPort::write(char* buf, unsigned int size)
+	{
+		DWORD bytesWritten = 0;
+		if (!WriteFile(hCom, buf, size, reinterpret_cast<LPDWORD>(&bytesWritten), nullptr)) return -static_cast<int>(GetLastError());
+		return bytesWritten;
+	}
+}
\ No newline at end of file
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/winsample.vcxproj b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/winsample.vcxproj
new file mode 100644
index 00000000..a1cafd11
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/winsample.vcxproj
@@ -0,0 +1,159 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup Label="ProjectConfigurations">
+    <ProjectConfiguration Include="Debug|Win32">
+      <Configuration>Debug</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|Win32">
+      <Configuration>Release</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Debug|x64">
+      <Configuration>Debug</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|x64">
+      <Configuration>Release</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+  </ItemGroup>
+  <PropertyGroup Label="Globals">
+    <VCProjectVersion>16.0</VCProjectVersion>
+    <Keyword>Win32Proj</Keyword>
+    <ProjectGuid>{26fb76ac-8d6a-479b-a661-9e3efb8aeca6}</ProjectGuid>
+    <RootNamespace>winsample</RootNamespace>
+    <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
+  <ImportGroup Label="ExtensionSettings">
+  </ImportGroup>
+  <ImportGroup Label="Shared">
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <PropertyGroup Label="UserMacros" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemGroup>
+    <ClCompile Include="ILDriver.cpp" />
+    <ClCompile Include="example.cpp" />
+    <ClCompile Include="platforms\windows\SerialPort.cpp" />
+    <ClCompile Include="platforms\windows\NetClient.cpp" />
+    <ClCompile Include="UDDParser.cpp" />
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="dataStructures.h" />
+    <ClInclude Include="ILDriver.h" />
+    <ClInclude Include="NetClient.h" />
+    <ClInclude Include="SerialPort.h" />
+    <ClInclude Include="Transport.h" />
+    <ClInclude Include="UDDParser.h" />
+  </ItemGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>
\ No newline at end of file
diff --git a/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/winsample.vcxproj.filters b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/winsample.vcxproj.filters
new file mode 100644
index 00000000..4cba42ed
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_ins/src/inertiallabs_sdk/winsample.vcxproj.filters
@@ -0,0 +1,54 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup>
+    <Filter Include="Source Files">
+      <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
+      <Extensions>cpp;c;cc;cxx;c++;def;odl;idl;hpj;bat;asm;asmx</Extensions>
+    </Filter>
+    <Filter Include="Header Files">
+      <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
+      <Extensions>h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd</Extensions>
+    </Filter>
+    <Filter Include="Resource Files">
+      <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
+      <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
+    </Filter>
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="ILDriver.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="main.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="UDDParser.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="platforms\windows\SerialPort.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="platforms\windows\NetClient.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="ILDriver.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="SerialPort.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="UDDParser.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="NetClient.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="dataStructures.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Transport.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+  </ItemGroup>
+</Project>
\ No newline at end of file
diff --git a/rubis_ws/src/inertiallabs_msgs/CMakeLists.txt b/rubis_ws/src/inertiallabs_msgs/CMakeLists.txt
new file mode 100644
index 00000000..9c749297
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/CMakeLists.txt
@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(inertiallabs_msgs)
+
+find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs actionlib_msgs message_generation)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  ins_data.msg
+  sensor_data.msg
+  gnss_data.msg
+  gps_data.msg
+  marine_data.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+  actionlib_msgs
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+  CATKIN_DEPENDS geometry_msgs std_msgs actionlib_msgs message_runtime)
\ No newline at end of file
diff --git a/rubis_ws/src/inertiallabs_msgs/README.md b/rubis_ws/src/inertiallabs_msgs/README.md
new file mode 100644
index 00000000..7e29754d
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/README.md
@@ -0,0 +1,82 @@
+# inertiallabs_msgs
+
+  ins_data
+
+  ```
+std_msgs/Header             header
+float64                     GPS_INS_Time
+float64                     GPS_IMU_Time
+std_msgs/UInt32             GPS_mSOW
+geometry_msgs/Vector3       LLH
+geometry_msgs/Vector3       YPR
+geometry_msgs/Quaternion    OriQuat
+geometry_msgs/Vector3       Vel_ENU
+std_msgs/Int8               Solution_Status
+geometry_msgs/Vector3       Pos_STD
+float32                     Heading_STD
+uint16                      USW
+
+  ```
+
+  gnss_data
+
+  ```
+std_msgs/Header          header
+int8                     GNSS_info_1
+int8                     GNSS_info_2
+int8                     Number_Sat
+float32                  GNSS_Velocity_Latency
+int8                     GNSS_Angles_Position_Type
+float32                  GNSS_Heading
+float32                  GNSS_Pitch
+float32                  GNSS_GDOP
+float32                  GNSS_PDOP
+float32                  GNSS_HDOP
+float32                  GNSS_VDOP
+float32                  GNSS_TDOP
+uint8                    New_GNSS_Flags
+float64                  Diff_Age
+
+  ```
+
+  gps_data
+
+  ```
+
+std_msgs/Header           header
+geometry_msgs/Vector3     LLH
+float32                   HorSpeed
+float32                   SpeedDir
+float32                   VerSpeed
+
+  ```
+
+  sensor_data
+
+  ```
+
+std_msgs/Header           header
+geometry_msgs/Vector3     Mag
+geometry_msgs/Vector3     Accel
+geometry_msgs/Vector3     Gyro
+float32                   Temp
+float32                   Vinp
+float32                   Pressure
+float32                   Barometric_Height
+
+  ```
+
+  marine_data
+
+  ```
+
+  std_msgs/Header           header
+float64                   Heave
+float64                   Surge
+float64                   Sway
+float32                   Heave_velocity
+float32                   Surge_velocity
+float32                   Sway_velocity
+float32                   Significant_wave_height
+
+  ```
diff --git a/rubis_ws/src/inertiallabs_msgs/msg/gnss_data.msg b/rubis_ws/src/inertiallabs_msgs/msg/gnss_data.msg
new file mode 100644
index 00000000..42df8119
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/msg/gnss_data.msg
@@ -0,0 +1,18 @@
+std_msgs/Header          header
+int8                     GNSS_info_1
+int8                     GNSS_info_2
+int8                     Number_Sat
+float32                  GNSS_Velocity_Latency
+int8                     GNSS_Angles_Position_Type
+float32                  GNSS_Heading
+float32                  GNSS_Pitch
+float32                  GNSS_GDOP
+float32                  GNSS_PDOP
+float32                  GNSS_HDOP
+float32                  GNSS_VDOP
+float32                  GNSS_TDOP
+uint8                    New_GNSS_Flags
+float64                  Diff_Age
+geometry_msgs/Vector3    Pos_STD
+float32                  Heading_STD
+float32                  Pitch_STD
diff --git a/rubis_ws/src/inertiallabs_msgs/msg/gps_data.msg b/rubis_ws/src/inertiallabs_msgs/msg/gps_data.msg
new file mode 100644
index 00000000..89fee5f5
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/msg/gps_data.msg
@@ -0,0 +1,6 @@
+std_msgs/Header           header
+geometry_msgs/Vector3     LLH
+float32                   HorSpeed
+float32                   SpeedDir
+float32                   VerSpeed
+
diff --git a/rubis_ws/src/inertiallabs_msgs/msg/ins_data.msg b/rubis_ws/src/inertiallabs_msgs/msg/ins_data.msg
new file mode 100644
index 00000000..33d1ec5f
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/msg/ins_data.msg
@@ -0,0 +1,12 @@
+std_msgs/Header             header
+float64                     GPS_INS_Time
+float64                     GPS_IMU_Time
+std_msgs/UInt32             GPS_mSOW
+geometry_msgs/Vector3       LLH
+geometry_msgs/Vector3       YPR
+geometry_msgs/Quaternion    OriQuat
+geometry_msgs/Vector3       Vel_ENU
+std_msgs/Int8               Solution_Status
+geometry_msgs/Vector3       Pos_STD
+float32                     Heading_STD
+uint16                      USW
diff --git a/rubis_ws/src/inertiallabs_msgs/msg/marine_data.msg b/rubis_ws/src/inertiallabs_msgs/msg/marine_data.msg
new file mode 100644
index 00000000..e0c3c7df
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/msg/marine_data.msg
@@ -0,0 +1,8 @@
+std_msgs/Header           header
+float64                   Heave
+float64                   Surge
+float64                   Sway
+float32                   Heave_velocity
+float32                   Surge_velocity
+float32                   Sway_velocity
+float32                   Significant_wave_height
diff --git a/rubis_ws/src/inertiallabs_msgs/msg/sensor_data.msg b/rubis_ws/src/inertiallabs_msgs/msg/sensor_data.msg
new file mode 100644
index 00000000..458518c3
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/msg/sensor_data.msg
@@ -0,0 +1,8 @@
+std_msgs/Header           header
+geometry_msgs/Vector3     Mag
+geometry_msgs/Vector3     Accel
+geometry_msgs/Vector3     Gyro
+float32                   Temp
+float32                   Vinp
+float32                   Pressure
+float32                   Barometric_Height
diff --git a/rubis_ws/src/inertiallabs_msgs/package.xml b/rubis_ws/src/inertiallabs_msgs/package.xml
new file mode 100644
index 00000000..216b215e
--- /dev/null
+++ b/rubis_ws/src/inertiallabs_msgs/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package>
+  <name>inertiallabs_msgs</name>
+  <version>1.0.0</version>
+  <description>The Inertial Labs Message package</description>
+
+ <maintainer email="omprakashpatro@gmail.com">Omprakash Patra</maintainer>
+  <url type="website">http://exmaple.com</url>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>message_generation</run_depend>
+  <run_depend>message_runtime</run_depend>
+</package>
diff --git a/rubis_ws/src/ins_twist_generator/CMakeLists.txt b/rubis_ws/src/ins_twist_generator/CMakeLists.txt
index 68a5b16c..97f9d208 100644
--- a/rubis_ws/src/ins_twist_generator/CMakeLists.txt
+++ b/rubis_ws/src/ins_twist_generator/CMakeLists.txt
@@ -24,107 +24,6 @@ find_package(catkin REQUIRED COMPONENTS
   tf
 )
 
-find_package(OpenCV REQUIRED)
-
-find_package(Eigen3 REQUIRED)
-set(EIGEN_PACKAGE EIGEN3)
-if(NOT EIGEN3_FOUND)
-  find_package(cmake_modules REQUIRED)
-  find_package(Eigen REQUIRED)
-  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
-  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})
-  set(EIGEN_PACKAGE Eigen)
-endif()
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES gnss_converter
@@ -132,104 +31,11 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
-  EIGEN3_INCLUDE_DIRS
-  ${OpenCV_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/gnss_converter.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/gnss_converter_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_gnss_converter.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
 add_executable(ins_twist_generator
   src/ins_twist_generator.cpp
 )
@@ -246,3 +52,16 @@ target_link_libraries(dynamic_ins_twist_generator
   ${catkin_LIBRARIES}
 )
 
+install(
+  TARGETS
+    ins_twist_generator
+    dynamic_ins_twist_generator
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/rubis_ws/src/ins_twist_generator/package.xml b/rubis_ws/src/ins_twist_generator/package.xml
index 2f12fc11..fe1ac622 100644
--- a/rubis_ws/src/ins_twist_generator/package.xml
+++ b/rubis_ws/src/ins_twist_generator/package.xml
@@ -4,72 +4,20 @@
   <version>0.0.0</version>
   <description>The ins_twist_generator package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="kite9240@gmail.com">HayeonP</maintainer>
 
+  <license>Apache 2</license>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/gnss_converter</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>rubis_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>can_data_msgs</build_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
-  <build_export_depend>rubis_msgs</build_export_depend>
-  <build_export_depend>tf</build_export_depend>
-  <build_export_depend>can_data_msgs</build_export_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>rubis_msgs</exec_depend>
-  <exec_depend>tf</exec_depend>
-  <exec_depend>can_data_msg</exec_depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>rubis_msgs</depend>
+  <depend>tf</depend>
+  <depend>can_data_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>geometry_msgs</depend>
+  <depend>inertiallabs_msgs</depend>
+  <depend>eigen</depend>
 
-  </export>
 </package>
 
diff --git a/rubis_ws/src/polled_camera/CHANGELOG.rst b/rubis_ws/src/polled_camera/CHANGELOG.rst
new file mode 100644
index 00000000..345e1a34
--- /dev/null
+++ b/rubis_ws/src/polled_camera/CHANGELOG.rst
@@ -0,0 +1,200 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package polled_camera
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.0 (2020-04-03)
+-------------------
+* Noetic release image_common (`#155 <https://github.com/ros-perception/image_common/issues/155>`_)
+* Contributors: Alejandro Hernández Cordero
+
+1.11.14 (2020-04-03)
+--------------------
+* export runtime binaries correctly on Windows (`#116 <https://github.com/ros-perception/image_common/issues/116>`_)
+* add DLL import/export macro (`#118 <https://github.com/ros-perception/image_common/issues/118>`_)
+* Contributors: James Xu
+
+1.11.13 (2017-11-05)
+--------------------
+
+1.11.12 (2017-01-29)
+--------------------
+* Fix CMake of image_transport/tutorial and polled_camera
+  Fix loads of problems with the CMakeLists.
+* 1.11.11
+* update changelogs
+* address gcc6 build error in polled_camera
+  With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+  as including '-isystem /usr/include' breaks with gcc6, cf.,
+  https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+  This commit addresses this issue for this package in the same way
+  it was addressed in various other ROS packages. A list of related
+  commits and pull requests is at:
+  https://github.com/ros/rosdistro/issues/12783
+  Signed-off-by: Lukas Bulwahn <lukas.bulwahn@oss.bmw-carit.de>
+* Contributors: Lukas Bulwahn, Martin Guenther, Vincent Rabaud
+
+1.11.11 (2016-09-24)
+--------------------
+* address gcc6 build error in polled_camera
+  With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+  as including '-isystem /usr/include' breaks with gcc6, cf.,
+  https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+  This commit addresses this issue for this package in the same way
+  it was addressed in various other ROS packages. A list of related
+  commits and pull requests is at:
+  https://github.com/ros/rosdistro/issues/12783
+  Signed-off-by: Lukas Bulwahn <lukas.bulwahn@oss.bmw-carit.de>
+* Contributors: Lukas Bulwahn
+
+1.11.10 (2016-01-19)
+--------------------
+
+1.11.9 (2016-01-17)
+-------------------
+
+1.11.8 (2015-11-29)
+-------------------
+
+1.11.7 (2015-07-28)
+-------------------
+
+1.11.6 (2015-07-16)
+-------------------
+
+1.11.5 (2015-05-14)
+-------------------
+
+1.11.4 (2014-09-21)
+-------------------
+
+1.11.3 (2014-05-19)
+-------------------
+
+1.11.2 (2014-02-13)
+-------------------
+
+1.11.1 (2014-01-26 02:33)
+-------------------------
+
+1.11.0 (2013-07-20 12:23)
+-------------------------
+
+1.10.5 (2014-01-26 02:34)
+-------------------------
+
+1.10.4 (2013-07-20 11:42)
+-------------------------
+* add Jack as maintainer
+* Contributors: Vincent Rabaud
+
+1.10.3 (2013-02-21 05:33)
+-------------------------
+
+1.10.2 (2013-02-21 04:48)
+-------------------------
+
+1.10.1 (2013-02-21 04:16)
+-------------------------
+
+1.10.0 (2013-01-13)
+-------------------
+* use CATKIN_DEVEL_PREFIX instead of obsolete CATKIN_BUILD_PREFIX
+* fix the urls
+* Missing link flags exposed on OS X
+* added license headers to various cpp and h files
+* Contributors: Aaron Blasdel, Dirk Thomas, Vincent Rabaud, William Woodall
+
+1.9.22 (2012-12-16)
+-------------------
+* replace genmsg by message_generation
+* Contributors: Vincent Rabaud
+
+1.9.21 (2012-12-14)
+-------------------
+* Updated package.xml file(s) to handle new catkin buildtool_depend requirement
+* Contributors: mirzashah
+
+1.9.20 (2012-12-04)
+-------------------
+
+1.9.19 (2012-11-08)
+-------------------
+
+1.9.18 (2012-11-06)
+-------------------
+* remove the brief attribute
+* Contributors: Vincent Rabaud
+
+1.9.17 (2012-10-30 19:32)
+-------------------------
+* comlpy to the new catkin API
+* Contributors: Vincent Rabaud
+
+1.9.16 (2012-10-30 09:10)
+-------------------------
+
+1.9.15 (2012-10-13 08:43)
+-------------------------
+* fix bad folder/libraries
+* Contributors: Vincent Rabaud
+
+1.9.14 (2012-10-13 01:07)
+-------------------------
+* fix typo that resulted in bad installation of include folder
+* Contributors: Vincent Rabaud
+
+1.9.13 (2012-10-06)
+-------------------
+
+1.9.12 (2012-10-04)
+-------------------
+
+1.9.11 (2012-10-02 02:56)
+-------------------------
+
+1.9.10 (2012-10-02 02:42)
+-------------------------
+
+1.9.9 (2012-10-01)
+------------------
+* fix dependencies
+* Contributors: Vincent Rabaud
+
+1.9.8 (2012-09-30)
+------------------
+* add catkin as a dependency
+* comply to the catkin API
+* Contributors: Vincent Rabaud
+
+1.9.7 (2012-09-18 11:39)
+------------------------
+
+1.9.6 (2012-09-18 11:07)
+------------------------
+
+1.9.5 (2012-09-13)
+------------------
+* install the include directories
+* Contributors: Vincent Rabaud
+
+1.9.4 (2012-09-12 23:37)
+------------------------
+* make sure we depend on the server
+* Contributors: Vincent Rabaud
+
+1.9.3 (2012-09-12 20:44)
+------------------------
+
+1.9.2 (2012-09-10)
+------------------
+
+1.9.1 (2012-09-07 15:33)
+------------------------
+
+1.9.0 (2012-09-07 13:03)
+------------------------
+* catkinize for Groovy
+* polled_camera (rep0104): Changed callback to allow filling
+  status_message field.
+* polled_camera (rep0104): Applied changes to GetPolledImage service.
+* Contributors: Vincent Rabaud, eitan, gerkey, kwc, mihelich
diff --git a/rubis_ws/src/polled_camera/CMakeLists.txt b/rubis_ws/src/polled_camera/CMakeLists.txt
new file mode 100644
index 00000000..4ace7af6
--- /dev/null
+++ b/rubis_ws/src/polled_camera/CMakeLists.txt
@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(polled_camera)
+
+# generate the server
+find_package(catkin REQUIRED COMPONENTS image_transport message_generation roscpp sensor_msgs std_msgs)
+
+add_service_files(DIRECTORY srv FILES GetPolledImage.srv)
+
+generate_messages(DEPENDENCIES sensor_msgs std_msgs)
+
+# define the project
+catkin_package(CATKIN_DEPENDS image_transport message_runtime roscpp sensor_msgs std_msgs
+               INCLUDE_DIRS include
+               LIBRARIES ${PROJECT_NAME}
+)
+
+
+# create some library and exe
+include_directories(include
+                    ${catkin_INCLUDE_DIRS} 
+                    ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+)
+
+add_library(${PROJECT_NAME} src/publication_server.cpp)
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+add_executable(poller src/poller.cpp)
+add_dependencies(poller ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(poller ${PROJECT_NAME}
+                             ${catkin_LIBRARIES}
+)
+
+install(TARGETS poller
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
\ No newline at end of file
diff --git a/rubis_ws/src/polled_camera/include/polled_camera/publication_server.h b/rubis_ws/src/polled_camera/include/polled_camera/publication_server.h
new file mode 100644
index 00000000..4bc3619e
--- /dev/null
+++ b/rubis_ws/src/polled_camera/include/polled_camera/publication_server.h
@@ -0,0 +1,150 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef POLLED_CAMERA_PUBLICATION_SERVER_H
+#define POLLED_CAMERA_PUBLICATION_SERVER_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include "polled_camera/GetPolledImage.h"
+
+#include <ros/macros.h>
+
+// Import/export for windows dll's and visibility for gcc shared libraries.
+
+#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
+  #ifdef polled_camera_EXPORTS // we are building a shared lib/dll
+    #define POLLED_CAMERA_DECL ROS_HELPER_EXPORT
+  #else // we are using shared lib/dll
+    #define POLLED_CAMERA_DECL ROS_HELPER_IMPORT
+  #endif
+#else // ros is being built around static libraries
+  #define POLLED_CAMERA_DECL
+#endif
+
+namespace polled_camera {
+
+/**
+ * \brief Manage image requests from one or more clients.
+ *
+ * Instances of polled_camera::PublicationServer should be created using one of
+ * the overloads of polled_camera::advertise(). You must specify a driver
+ * callback that populates the requested data:
+\code
+void callback(polled_camera::GetPolledImage::Request& req,
+              polled_camera::GetPolledImage::Response& rsp,
+              sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
+{
+  // Capture an image and fill in the Image and CameraInfo messages here.
+  
+  // On success, set rsp.success = true. rsp.timestamp will be filled in
+  // automatically.
+  
+  // On failure, set rsp.success = false and fill rsp.status_message with an
+  // informative error message.
+}
+\endcode
+ */
+class POLLED_CAMERA_DECL PublicationServer
+{
+public:
+  typedef boost::function<void (polled_camera::GetPolledImage::Request&,
+                                polled_camera::GetPolledImage::Response&,
+                                sensor_msgs::Image&,
+                                sensor_msgs::CameraInfo&)> DriverCallback;
+  
+  PublicationServer() {}
+
+  /**
+   * \brief Unadvertise the request service and shut down all published topics.
+   */
+  void shutdown();
+
+  std::string getService() const;
+
+  operator void*() const;
+  bool operator< (const PublicationServer& rhs) const { return impl_ <  rhs.impl_; }
+  bool operator==(const PublicationServer& rhs) const { return impl_ == rhs.impl_; }
+  bool operator!=(const PublicationServer& rhs) const { return impl_ != rhs.impl_; }
+
+private:
+  PublicationServer(const std::string& service, ros::NodeHandle& nh,
+                    const DriverCallback& cb, const ros::VoidPtr& tracked_object);
+
+  class Impl;
+
+  boost::shared_ptr<Impl> impl_;
+
+  friend
+  PublicationServer advertise(ros::NodeHandle&, const std::string&, const DriverCallback&,
+                              const ros::VoidPtr&);
+};
+
+/**
+ * \brief Advertise a polled image service, version for arbitrary boost::function object.
+ */
+PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
+                            const PublicationServer::DriverCallback& cb,
+                            const ros::VoidPtr& tracked_object = ros::VoidPtr());
+
+/**
+ * \brief Advertise a polled image service, version for class member function with bare pointer.
+ */
+template<class T>
+PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
+                            void(T::*fp)(polled_camera::GetPolledImage::Request&,
+                                         polled_camera::GetPolledImage::Response&,
+                                         sensor_msgs::Image&, sensor_msgs::CameraInfo&),
+                            T* obj)
+{
+  return advertise(nh, service, boost::bind(fp, obj, _1, _2, _3, _4));
+}
+
+/**
+ * \brief Advertise a polled image service, version for class member function with bare pointer.
+ */
+template<class T>
+PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
+                            void(T::*fp)(polled_camera::GetPolledImage::Request&,
+                                         polled_camera::GetPolledImage::Response&,
+                                         sensor_msgs::Image&, sensor_msgs::CameraInfo&),
+                            const boost::shared_ptr<T>& obj)
+{
+  return advertise(nh, service, boost::bind(fp, obj.get(), _1, _2, _3, _4), obj);
+}
+
+} //namespace polled_camera
+
+#endif
diff --git a/rubis_ws/src/polled_camera/mainpage.dox b/rubis_ws/src/polled_camera/mainpage.dox
new file mode 100644
index 00000000..fc6fa6bd
--- /dev/null
+++ b/rubis_ws/src/polled_camera/mainpage.dox
@@ -0,0 +1,48 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+NOTE: This package's API is not yet released. It may change from its current form.
+
+\b polled_camera contains a service definition for requesting polled images,
+as well as a C++ server class to simplify publishing polled images to clients.
+
+The protocol for polling images from a camera driver node that supports it is as
+follows:
+ - The camera driver advertises a service call \c \<camera\>/request_image.
+ - The client calls the service, specifying an output namespace in the request.
+ - On receiving a request, the driver captures an image and returns its time stamp
+in the service response.
+ - The \c Image and \c CameraInfo are published to \c \<response_namespace\>/image_raw
+and \c \<response_namespace\>/camera_info, latched.
+ - Clients subscribe to the response topics just like any other camera image stream.
+
+\section codeapi Code API
+Use polled_camera::PublicationServer in camera driver nodes (or similar) to
+track client connections and respond to image requests.
+
+There is not currently a matching client class, but receiving polled images
+is identical to subscribing to any other image topic. The only additional
+step is using a ros::ServiceClient to make explicit requests:
+
+\code
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <polled_camera/GetPolledImage.h>
+
+void callback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& info);
+
+ros::NodeHandle nh;
+image_transport::ImageTransport it(nh);
+
+image_transport::CameraSubscriber sub = it.subscribeCamera("output_ns/image_raw", 1, callback);
+ros::ServiceClient client = nh.serviceClient<polled_camera::GetPolledImage>("my_camera/request_image");
+polled_camera::GetPolledImage srv;
+srv.request.response_namespace = "output_ns";
+if (client.call(srv))
+{
+  ROS_INFO_STREAM("Image captured with timestamp " << srv.response.stamp);
+}
+\endcode
+
+*/
diff --git a/rubis_ws/src/polled_camera/package.xml b/rubis_ws/src/polled_camera/package.xml
new file mode 100644
index 00000000..3cee36cd
--- /dev/null
+++ b/rubis_ws/src/polled_camera/package.xml
@@ -0,0 +1,33 @@
+<package>
+  <name>polled_camera</name>
+  <version>1.12.0</version>
+  <description>
+
+     polled_camera contains a service and C++ helper classes for implementing a polled
+     camera driver node and requesting images from it. The package is currently for
+     internal use as the API is still under development.
+
+  </description>
+  <author>Patrick Mihelich</author>
+  <maintainer email="jack.oquin@gmail.com">Jack O'Quin</maintainer>
+  <maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/polled_camera</url>
+  <url type="bugtracker">https://github.com/ros-perception/image_common/issues</url>
+  <url type="repository">https://github.com/ros-perception/image_common</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>image_transport</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <run_depend>image_transport</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+</package>
diff --git a/rubis_ws/src/polled_camera/src/poller.cpp b/rubis_ws/src/polled_camera/src/poller.cpp
new file mode 100644
index 00000000..74263242
--- /dev/null
+++ b/rubis_ws/src/polled_camera/src/poller.cpp
@@ -0,0 +1,68 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <cstdio>
+#include <ros/ros.h>
+#include <boost/lexical_cast.hpp>
+#include "polled_camera/GetPolledImage.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "poller", ros::init_options::AnonymousName);
+  if (argc < 2) {
+    printf("Usage: %s <Hz> camera:=<namespace> output:=<namespace>\n", argv[0]);
+    return 0;
+  }
+  double hz = boost::lexical_cast<double>(argv[1]);
+  
+  ros::NodeHandle nh;
+  std::string service_name = nh.resolveName("camera") + "/request_image";
+  ros::ServiceClient client = nh.serviceClient<polled_camera::GetPolledImage>(service_name);
+
+  polled_camera::GetPolledImage::Request req;
+  polled_camera::GetPolledImage::Response rsp;
+  req.response_namespace = nh.resolveName("output");
+
+  ros::Rate loop_rate(hz);
+  while (nh.ok()) {
+    if (client.call(req, rsp)) {
+      std::cout << "Timestamp: " << rsp.stamp << std::endl;
+      loop_rate.sleep();
+    }
+    else {
+      ROS_ERROR("Service call failed");
+      client.waitForExistence();
+    }
+  }
+}
diff --git a/rubis_ws/src/polled_camera/src/publication_server.cpp b/rubis_ws/src/polled_camera/src/publication_server.cpp
new file mode 100644
index 00000000..99097ea8
--- /dev/null
+++ b/rubis_ws/src/polled_camera/src/publication_server.cpp
@@ -0,0 +1,161 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "polled_camera/publication_server.h"
+#include <image_transport/image_transport.h>
+
+namespace polled_camera {
+
+/// \cond
+
+class PublicationServer::Impl
+{
+public:
+  ros::ServiceServer srv_server_;
+  DriverCallback driver_cb_;
+  ros::VoidPtr tracked_object_;
+  image_transport::ImageTransport it_;
+  std::map<std::string, image_transport::CameraPublisher> client_map_;
+  bool unadvertised_;
+  double constructed_;
+  
+  Impl(const ros::NodeHandle& nh)
+    : it_(nh),
+      unadvertised_(false),
+      constructed_(ros::WallTime::now().toSec())
+  {
+  }
+  
+  ~Impl()
+  {
+    if (ros::WallTime::now().toSec() - constructed_ < 0.001)
+      ROS_WARN("PublicationServer destroyed immediately after creation. Did you forget to store the handle?");
+    unadvertise();
+  }
+
+  bool isValid() const
+  {
+    return !unadvertised_;
+  }
+
+  void unadvertise()
+  {
+    if (!unadvertised_) {
+      unadvertised_ = true;
+      srv_server_.shutdown();
+      client_map_.clear();
+    }
+  }
+
+  bool requestCallback(polled_camera::GetPolledImage::Request& req,
+                       polled_camera::GetPolledImage::Response& rsp)
+  {
+    std::string image_topic = req.response_namespace + "/image_raw";
+    image_transport::CameraPublisher& pub = client_map_[image_topic];
+    if (!pub) {
+      // Create a latching camera publisher.
+      pub = it_.advertiseCamera(image_topic, 1, image_transport::SubscriberStatusCallback(),
+                                boost::bind(&Impl::disconnectCallback, this, _1),
+                                ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
+                                ros::VoidPtr(), true /*latch*/);
+      ROS_INFO("Advertising %s", pub.getTopic().c_str());
+    }
+
+    // Correct zero binning values to one for convenience
+    req.binning_x = std::max(req.binning_x, (uint32_t)1);
+    req.binning_y = std::max(req.binning_y, (uint32_t)1);
+
+    /// @todo Use pointers in prep for nodelet drivers?
+    sensor_msgs::Image image;
+    sensor_msgs::CameraInfo info;
+    driver_cb_(req, rsp, image, info);
+    
+    if (rsp.success) {
+      assert(image.header.stamp == info.header.stamp);
+      rsp.stamp = image.header.stamp;
+      pub.publish(image, info);
+    }
+    else {
+      ROS_ERROR("Failed to capture requested image, status message: '%s'",
+                rsp.status_message.c_str());
+    }
+    
+    return true; // Success/failure indicated by rsp.success
+  }
+
+  void disconnectCallback(const image_transport::SingleSubscriberPublisher& ssp)
+  {
+    // Shut down the publication when the subscription count drops to zero.
+    if (ssp.getNumSubscribers() == 0) {
+      ROS_INFO("Shutting down %s", ssp.getTopic().c_str());
+      client_map_.erase(ssp.getTopic());
+    }
+  }
+};
+
+/// \endcond
+
+PublicationServer::PublicationServer(const std::string& service, ros::NodeHandle& nh,
+                                     const DriverCallback& cb, const ros::VoidPtr& tracked_object)
+  : impl_(new Impl(nh))
+{
+  impl_->driver_cb_ = cb;
+  impl_->tracked_object_ = tracked_object;
+  impl_->srv_server_ = nh.advertiseService<>(service, &Impl::requestCallback, impl_);
+}
+
+void PublicationServer::shutdown()
+{
+  if (impl_) impl_->unadvertise();
+}
+
+std::string PublicationServer::getService() const
+{
+  if (impl_) return impl_->srv_server_.getService();
+  return std::string();
+}
+
+PublicationServer::operator void*() const
+{
+  return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
+}
+
+PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
+                            const PublicationServer::DriverCallback& cb,
+                            const ros::VoidPtr& tracked_object)
+{
+  return PublicationServer(service, nh, cb, tracked_object);
+}
+
+} //namespace polled_camera
diff --git a/rubis_ws/src/polled_camera/srv/GetPolledImage.srv b/rubis_ws/src/polled_camera/srv/GetPolledImage.srv
new file mode 100644
index 00000000..f73d825b
--- /dev/null
+++ b/rubis_ws/src/polled_camera/srv/GetPolledImage.srv
@@ -0,0 +1,22 @@
+# Namespace to publish response topics in. A polled camera driver node
+# should publish:
+#   <response_namespace>/image_raw
+#   <response_namespace>/camera_info
+string response_namespace
+
+# Timeout for attempting to capture data from the device. This does not
+# include latency from ROS communication, post-processing of raw camera
+# data, etc. A zero duration indicates no time limit.
+duration timeout
+
+# Binning settings, if supported by the camera.
+uint32 binning_x
+uint32 binning_y
+
+# Region of interest, if supported by the camera.
+sensor_msgs/RegionOfInterest roi
+---
+bool success          # Could the image be captured?
+string status_message # Error message in case of failure
+time stamp            # Timestamp of the captured image. Can be matched
+                      # against incoming sensor_msgs/Image header.
diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml
index c49e3f72..40654dad 100755
--- a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml
+++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml
@@ -15,4 +15,4 @@ carla_ackermann_control:
     # at more or less constant speed.
     # If the absolute value of the ackermann drive target acceleration exceeds this value,
     # directly the input acceleration is controlled 
-    min_accel: 1.0
+    min_accel: 0.005
diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup
new file mode 100755
index 00000000..c49e3f72
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup
@@ -0,0 +1,18 @@
+carla_ackermann_control:
+  ros__parameters:
+    # override the default values of the pid speed controller
+    # (only relevant for ackermann control mode) 
+    speed_Kp: 0.05  # min: 0, max: 1.
+    speed_Ki: 0.00  # min: 0, max: 1.
+    speed_Kd: 0.50  # min: 0, max: 10. 
+    # override the default values of the pid acceleration controller
+    # (only relevant for ackermann control mode)
+    accel_Kp: 0.05  # min: 0, max: 10.
+    accel_Ki: 0.00  # min: 0, max: 10.
+    accel_Kd: 0.05  # min: 0, max: 10.
+    # set the minimum acceleration in (m/s^2)
+    # This border value is used to enable the speed controller which is used to control driving 
+    # at more or less constant speed.
+    # If the absolute value of the ackermann drive target acceleration exceeds this value,
+    # directly the input acceleration is controlled 
+    min_accel: 1.0
diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch b/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch
index 6428956c..61bebdb3 100755
--- a/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch
+++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch
@@ -3,15 +3,24 @@
   <arg name="role_name" default="ego_vehicle" />
   <arg name="control_loop_rate" default="0.05" />
 
-  <arg name="speed_Kp" default="0.04" />
+  <!-- <arg name="speed_Kp" default="0.04" />
   <arg name="speed_Ki" default="0.00" />
   <arg name="speed_Kd" default="0.001" />
 
   <arg name="accel_Kp" default="0.3" />
   <arg name="accel_Ki" default="0.08" />
-  <arg name="accel_Kd" default="0.001" />
+  <arg name="accel_Kd" default="0.001" /> -->
 
-  <node pkg="carla_ackermann_control" type="carla_ackermann_control_node.py" name="carla_ackermann_control_$(arg role_name)" output="screen">
+  <arg name="speed_Kp" default="0.80" />
+  <arg name="speed_Ki" default="0.00" />
+  <arg name="speed_Kd" default="0.00" />
+
+<!-- 0.4 -->
+  <arg name="accel_Kp" default="0.40" /> 
+  <arg name="accel_Ki" default="0.02" />
+  <arg name="accel_Kd" default="0.05" />
+
+  <node pkg="carla_ackermann_control" type="carla_ackermann_control_node.py" name="carla_ackermann_control_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)" />
     <param name="control_loop_rate" value="$(arg control_loop_rate)" />
     <param name="speed_Kp" value="$(arg speed_Kp)" />
@@ -23,5 +32,5 @@
     <rosparam file="$(find carla_ackermann_control)/config/settings.ros1.yaml" command="load" />
   </node>
 
-  <node pkg="carla_ros_bridge" type="carla_current_velocity_publisher.py" name="carla_current_velocity_publisher" output="screen" />
+  <node pkg="carla_ros_bridge" type="carla_current_velocity_publisher.py" name="carla_current_velocity_publisher" />
 </launch>
diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py
index 944f70fd..8ab0b4bd 100755
--- a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py
+++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py
@@ -304,7 +304,7 @@ def set_target_steering_angle(self, target_steering_angle):
         set target sterring angle
         """
         # hardcoding
-        self.info.target.steering_angle = -1.15*target_steering_angle
+        self.info.target.steering_angle = -1.0*target_steering_angle
         if abs(self.info.target.steering_angle) > self.info.restrictions.max_steering_angle:
             self.logerr("Max steering angle reached, clipping value")
             self.info.target.steering_angle = numpy.clip(
@@ -442,6 +442,7 @@ def run_speed_control_loop(self):
         else:
             if self.info.status.speed_control_activation_count > 0:
                 self.info.status.speed_control_activation_count -= 1
+        
         # set the auto_mode of the controller accordingly
         self.speed_controller.auto_mode = self.info.status.speed_control_activation_count >= 5
 
diff --git a/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch b/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch
index 71b78102..81ae04df 100755
--- a/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch
+++ b/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch
@@ -10,12 +10,12 @@
   <arg name="Kd_longitudinal" default="0.515"/>
   <arg name="control_time_step" default="0.05"/>
 
-  <node pkg="carla_ad_agent" type="ad_agent.py" name="carla_ad_agent_$(arg role_name)" output="screen">
+  <node pkg="carla_ad_agent" type="ad_agent.py" name="carla_ad_agent_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)" />
     <param name="avoid_risk" value="$(arg avoid_risk)" />
   </node>
 
-  <node pkg="carla_ad_agent" type="local_planner.py" name="local_planner_$(arg role_name)" output="screen">
+  <node pkg="carla_ad_agent" type="local_planner.py" name="local_planner_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)" />
     <param name="Kp_lateral" value="$(arg Kp_lateral)" />
     <param name="Ki_lateral" value="$(arg Ki_lateral)" />
diff --git a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py
index 19336f89..e382bef2 100755
--- a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py
+++ b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py
@@ -29,7 +29,7 @@ def launch_target_speed_publisher(context, *args, **kwargs):
     data_string = "{'data': " + launch.substitutions.LaunchConfiguration('target_speed').perform(context) + "}"
     return [
         launch.actions.ExecuteProcess(
-            output="screen",
+           ,
             cmd=["ros2", "topic", "pub", topic_name,
                  "std_msgs/msg/Float64", data_string, "--qos-durability", "transient_local"],
             name='topic_pub_target_speed')]
diff --git a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch
index b1ade608..924ec473 100755
--- a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch
+++ b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch
@@ -70,7 +70,7 @@
   </include>
 
   <!-- Rviz visualization-->
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find carla_ad_demo)/config/carla_ad_demo.rviz" required="true" output="screen">
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find carla_ad_demo)/config/carla_ad_demo.rviz" required="true">
     <remap from="carla/ego_vehicle/spectator_pose" to="/carla/ego_vehicle/rgb_view/control/set_transform"/>
   </node>
 
diff --git a/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch b/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch
index 438dc6f3..efe346d2 100755
--- a/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch
+++ b/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch
@@ -2,7 +2,7 @@
 <launch>
   <arg name="role_name" default="ego_vehicle" />
   
-  <node pkg="carla_fake_control" type="carla_fake_control_node.py" name="carla_fake_control_$(arg role_name)" output="screen">
+  <node pkg="carla_fake_control" type="carla_fake_control_node.py" name="carla_fake_control_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)" />
   </node>
 </launch>
diff --git a/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch b/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch
index 6a38db62..f8273a34 100755
--- a/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch
+++ b/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch
@@ -2,7 +2,7 @@
 <launch>
   <arg name="role_name" default="ego_vehicle"/>
 
-  <node pkg="carla_manual_control" type="carla_manual_control.py" name="carla_manual_control_$(arg role_name)" output="screen">
+  <node pkg="carla_manual_control" type="carla_manual_control.py" name="carla_manual_control_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)"/>
   </node>
 
diff --git a/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch b/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch
index 4bc9aa82..1cddf412 100755
--- a/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch
+++ b/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch
@@ -27,7 +27,7 @@
   -->
   <arg name='ego_vehicle_role_name' default='["hero", "ego_vehicle", "hero0", "hero1", "hero2", "hero3", "hero4", "hero5", "hero6", "hero7", "hero8", "hero9"]'/>
       
-  <node pkg="carla_ros_bridge" name="carla_ros_bridge" type="bridge.py" output="screen" required="true">
+  <node pkg="carla_ros_bridge" name="carla_ros_bridge" type="bridge.py" required="true">
     <param name="host" value="$(arg host)" unless="$(eval host == '')"/>
     <param name="port" value="$(arg port)" unless="$(eval port == '')"/>
     <param name="timeout" value="$(arg timeout)" unless="$(eval timeout == '')"/>
diff --git a/rubis_ws/src/ros-bridge/carla_ros_bridge/src/carla_ros_bridge/bridge.py b/rubis_ws/src/ros-bridge/carla_ros_bridge/src/carla_ros_bridge/bridge.py
index 9e34f499..88e23cb4 100755
--- a/rubis_ws/src/ros-bridge/carla_ros_bridge/src/carla_ros_bridge/bridge.py
+++ b/rubis_ws/src/ros-bridge/carla_ros_bridge/src/carla_ros_bridge/bridge.py
@@ -12,6 +12,7 @@
 """
 
 import os
+import time
 import pkg_resources
 try:
     import queue
@@ -89,7 +90,9 @@ def initialize_bridge(self, carla_world, params):
             self.loginfo("fixed_delta_seconds: {}".format(
                 self.parameters["fixed_delta_seconds"]))
             self.carla_settings.fixed_delta_seconds = self.parameters["fixed_delta_seconds"]
+            
             carla_world.apply_settings(self.carla_settings)
+            
 
         self.loginfo("Parameters:")
         for key in self.parameters:
@@ -264,6 +267,8 @@ def _synchronous_mode_update(self):
 
             self.actor_factory.update_available_objects()
             frame = self.carla_world.tick()
+            time.sleep(0.05)
+            
 
             world_snapshot = self.carla_world.get_snapshot()
 
diff --git a/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch b/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch
index 0c465164..093b97c3 100755
--- a/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch
+++ b/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch
@@ -6,7 +6,7 @@
   <arg name="scenario_runner_path"/>
   <arg name="wait_for_ego" default="True"/>
 
-  <node pkg="carla_ros_scenario_runner" type="carla_ros_scenario_runner_node.py" name="carla_ros_scenario_runner" output="screen" required="true">
+  <node pkg="carla_ros_scenario_runner" type="carla_ros_scenario_runner_node.py" name="carla_ros_scenario_runner" required="true">
     <param name="host" value="$(arg host)"/>
     <param name='port' value='$(arg port)'/>
     <param name="scenario_runner_path" value="$(arg scenario_runner_path)"/>
diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json
index 9aef8118..fe0cb3b4 100755
--- a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json
+++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json
@@ -30,10 +30,10 @@
                     "type": "sensor.camera.rgb",
                     "id": "rgb_front",
                     "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
-                    "image_size_x": 400,
-                    "image_size_y": 400,
+                    "image_size_x": 1400,
+                    "image_size_y": 1200,
                     "fov": 90.0,
-                    "sensor_tick": 0.1
+                    "sensor_tick": 0.2
                 },
                 {
                     "type": "sensor.camera.rgb",
@@ -42,7 +42,7 @@
                     "image_size_x": 800,
                     "image_size_y": 600,
                     "fov": 90.0,
-                    "sensor_tick": 0.1,
+                    "sensor_tick": 0.2,
                     "attached_objects":
                     [
                         {
@@ -55,13 +55,14 @@
                     "type": "sensor.lidar.ray_cast",
                     "id": "lidar",
                     "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
-                    "range": 50,
-                    "channels": 32,
+                    "range": 100,
+                    "channels": 16,
                     "points_per_second": 320000,
                     "upper_fov": 2.0,
                     "lower_fov": -26.8,
                     "rotation_frequency": 20,
-                    "noise_stddev": 0.0
+                    "noise_stddev": 0.0,
+                    "sensor_tick": 0.2
                 },
                 {
                     "type": "sensor.other.gnss",
@@ -77,7 +78,6 @@
                     "noise_accel_stddev_x": 0.0, "noise_accel_stddev_y": 0.0, "noise_accel_stddev_z": 0.0,
                     "noise_gyro_stddev_x": 0.0, "noise_gyro_stddev_y": 0.0, "noise_gyro_stddev_z": 0.0,
                     "noise_gyro_bias_x": 0.0, "noise_gyro_bias_y": 0.0, "noise_gyro_bias_z": 0.0
-
                 },
                 {
                     "type": "sensor.other.collision",
diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects_backup.json b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects_backup.json
new file mode 100755
index 00000000..f36d9f1a
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects_backup.json
@@ -0,0 +1,160 @@
+{   
+    "objects": 
+    [
+        {
+            "type": "sensor.pseudo.traffic_lights",
+            "id": "traffic_lights"
+        },
+        {
+            "type": "sensor.pseudo.objects",
+            "id": "objects"
+        },
+        {
+            "type": "sensor.pseudo.actor_list",
+            "id": "actor_list"
+        },
+        {
+            "type": "sensor.pseudo.markers",
+            "id": "markers"
+        },
+        {
+            "type": "sensor.pseudo.opendrive_map",
+            "id": "map"
+        },
+        {
+            "type": "vehicle.toyota.prius",
+            "id": "ego_vehicle",
+            "sensors": 
+            [
+                {
+                    "type": "sensor.camera.rgb",
+                    "id": "rgb_front",
+                    "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
+                    "image_size_x": 400,
+                    "image_size_y": 400,
+                    "fov": 90.0,
+                    "sensor_tick": 0.1
+                },
+                {
+                    "type": "sensor.camera.rgb",
+                    "id": "rgb_view",
+                    "spawn_point": {"x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": 20.0, "yaw": 0.0},
+                    "image_size_x": 800,
+                    "image_size_y": 600,
+                    "fov": 90.0,
+                    "sensor_tick": 0.1,
+                    "attached_objects":
+                    [
+                        {
+                            "type": "actor.pseudo.control",
+                            "id": "control"
+                        }
+                    ]
+                },
+                {
+                    "type": "sensor.lidar.ray_cast",
+                    "id": "lidar",
+                    "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
+                    "range": 100,
+                    "channels": 16,
+                    "points_per_second": 320000,
+                    "upper_fov": 2.0,
+                    "lower_fov": -26.8,
+                    "rotation_frequency": 20,
+                    "noise_stddev": 0.0,
+                    "sensor_tick": 0.2
+                },
+                {
+                    "type": "sensor.other.gnss",
+                    "id": "gnss",
+                    "spawn_point": {"x": 1.0, "y": 0.0, "z": 2.0},
+                    "noise_alt_stddev": 0.0, "noise_lat_stddev": 0.0, "noise_lon_stddev": 0.0,
+                    "noise_alt_bias": 0.0, "noise_lat_bias": 0.0, "noise_lon_bias": 0.0
+                },
+                {
+                    "type": "sensor.other.imu",
+                    "id": "imu",
+                    "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
+                    "noise_accel_stddev_x": 0.0, "noise_accel_stddev_y": 0.0, "noise_accel_stddev_z": 0.0,
+                    "noise_gyro_stddev_x": 0.0, "noise_gyro_stddev_y": 0.0, "noise_gyro_stddev_z": 0.0,
+                    "noise_gyro_bias_x": 0.0, "noise_gyro_bias_y": 0.0, "noise_gyro_bias_z": 0.0
+
+                },
+                {
+                    "type": "sensor.other.collision",
+                    "id": "collision",
+                    "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0}
+                },
+                {
+                    "type": "sensor.other.lane_invasion",
+                    "id": "lane_invasion",
+                    "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0}
+                },
+                {
+                    "type": "sensor.pseudo.objects",
+                    "id": "objects"
+                },
+                {
+                    "type": "sensor.pseudo.odom",
+                    "id": "odometry"
+                },
+                {
+                    "type": "sensor.pseudo.speedometer",
+                    "id": "speedometer"
+                },
+                {
+                    "type": "actor.pseudo.control",
+                    "id": "control"
+                }
+            ]
+        },
+        {
+            "type": "vehicle.tesla.cybertruck",
+            "id": "obstacle",
+            "sensors": 
+            [
+                {
+                    "type": "sensor.other.collision",
+                    "id": "collision",
+                    "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0}
+                },
+                {
+                    "type": "sensor.pseudo.tf",
+                    "id": "tf"
+                },
+                {
+                    "type": "sensor.pseudo.objects",
+                    "id": "objects"
+                },
+                {
+                    "type": "sensor.pseudo.odom",
+                    "id": "odometry"
+                }
+            ]
+        },
+        {
+            "type": "walker.pedestrian.0001",
+            "id": "walker",
+            "sensors": 
+            [
+                {
+                    "type": "sensor.other.collision",
+                    "id": "collision",
+                    "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0}
+                },
+                {
+                    "type": "sensor.pseudo.tf",
+                    "id": "tf"
+                },
+                {
+                    "type": "sensor.pseudo.objects",
+                    "id": "objects"
+                },
+                {
+                    "type": "sensor.pseudo.odom",
+                    "id": "odometry"
+                }
+            ]
+        }
+    ]
+}
diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_example_ego_vehicle.launch b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_example_ego_vehicle.launch
index 84ad384f..7239e41e 100755
--- a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_example_ego_vehicle.launch
+++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_example_ego_vehicle.launch
@@ -2,24 +2,25 @@
 <launch>
   <arg name="role_name" default="ego_vehicle"/>
   <arg name="objects_definition_file" default='$(find carla_spawn_objects)/config/objects.json'/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/cfg/carla_autorunner/carla_spawn_points.yaml'/>
   <!-- use comma separated format "x,y,z,roll,pitch,yaw", 
     and parameter name spawn_point_<vehicle_name>. You can add
     as many spawn_point as vehicles defined in objects_definition_file-->
   <!-- <arg name="spawn_point_ego_vehicle" default='390.0,-87.0,0.3,0.0,0.0,90.0'/> -->
-  <arg name="spawn_point_ego_vehicle" default=''/>
-
   <arg name='role_name_obstacle' default='obstacle'/>
-  <arg name='spawn_point_obstacle' default=''/>
-  <arg name="spawn_point_walker" default=""/>
+  <arg name="spawn_point_ego_vehicle" default=''/>
+  <!-- <arg name='spawn_point_obstacle' default=''/>
+  <arg name="spawn_point_walker" default=""/> -->
   <arg name="spawn_sensors_only" default="false"/>
 
   <arg name="control_id" default="control"/>
 
   <include file="$(find carla_spawn_objects)/launch/carla_spawn_objects.launch">
     <arg name="objects_definition_file" value='$(arg objects_definition_file)'/>
+    <arg name="spawn_points_file" value='$(arg spawn_points_file)'/>
     <arg name="spawn_point_ego_vehicle" value="$(arg spawn_point_ego_vehicle)"/>
-    <arg name='spawn_point_obstacle' value='$(arg spawn_point_obstacle)'/>
-    <arg name="spawn_point_walker" value="$(arg spawn_point_walker)"/>    
+    <!-- <arg name='spawn_point_obstacle' value='$(arg spawn_point_obstacle)'/>
+    <arg name="spawn_point_walker" value="$(arg spawn_point_walker)"/>     -->
     <arg name="spawn_sensors_only" value="$(arg spawn_sensors_only)" />
   </include>
 
diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch
index 885ffdde..2c811cd1 100755
--- a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch
+++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch
@@ -1,19 +1,19 @@
 <!-- -->
 <launch>
   <arg name="objects_definition_file" default='$(find carla_spawn_objects)/config/objects.json'/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/cfg/carla_autorunner/carla_spawn_points.yaml'/>
+  <arg name="spawn_point_ego_vehicle" default="314.065, 129.676, 0.3, 0, 0, 90"/>
+  <arg name="spawn_point_obstacle" default="324.4, 346.1, 0.3, 0, 0, -32"/>
   <!-- use comma separated format "x,y,z,roll,pitch,yaw", 
     and parameter name spawn_point_<vehicle_name>. You can add
     as many spawn_point as vehicles defined in objects_definition_file-->
-  <arg name="spawn_point_ego_vehicle" default=""/>
-  <arg name="spawn_point_obstacle" default=""/>
-  <arg name="spawn_point_walker" default=""/>
   <arg name="spawn_sensors_only" default="false"/>
 
-  <node pkg="carla_spawn_objects" type="carla_spawn_objects.py" name="$(anon carla_spawn_objects)" output="screen">
+  <node pkg="carla_spawn_objects" type="carla_spawn_objects.py" name="$(anon carla_spawn_objects)">
     <param name="objects_definition_file" value="$(arg objects_definition_file)" />
-    <param name="spawn_point_ego_vehicle" value="$(arg spawn_point_ego_vehicle)" />
-    <param name="spawn_point_obstacle" value="$(arg spawn_point_obstacle)" />
-    <param name="spawn_point_walker" value="$(arg spawn_point_walker)"/>  
+    <param name="spawn_points_file" value="$(arg spawn_points_file)"/>
+    <param name="spawn_point_ego_vehicle" value="$(arg spawn_point_ego_vehicle)"/>
+    <param name="spawn_point_obstacle" value="$(arg spawn_point_obstacle)"/>  
     <param name="spawn_sensors_only" value="$(arg spawn_sensors_only)" />
   </node>
 </launch>
diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch
index ba4647b6..ffc62e27 100755
--- a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch
+++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch
@@ -4,7 +4,7 @@
   <!-- id of the actor.pseudo.control actor-->
   <arg name='control_id' default='control'/>
   
-  <node pkg="carla_spawn_objects" type="set_initial_pose.py" name="set_initial_pose_$(arg role_name)" output="screen">
+  <node pkg="carla_spawn_objects" type="set_initial_pose.py" name="set_initial_pose_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)" />
     <param name="control_id" value="$(arg control_id)" />
   </node>
diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py b/rubis_ws/src/ros-bridge/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py
index 0160d6a3..3a862ad1 100755
--- a/rubis_ws/src/ros-bridge/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py
+++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py
@@ -14,7 +14,7 @@
 finally ask for a random one to the spawn service.
 
 """
-
+import yaml
 import json
 import math
 import os
@@ -47,6 +47,7 @@ def __init__(self):
         super(CarlaSpawnObjects, self).__init__('carla_spawn_objects')
 
         self.objects_definition_file = self.get_param('objects_definition_file', '')
+        self.spawn_points_file = self.get_param('spawn_points_file','')
         self.spawn_sensors_only = self.get_param('spawn_sensors_only', False)
 
         self.players = []
@@ -56,6 +57,9 @@ def __init__(self):
         self.spawn_object_service = self.new_client(SpawnObject, "/carla/spawn_object")
         self.destroy_object_service = self.new_client(DestroyObject, "/carla/destroy_object")
 
+        self.enable_walker = False
+        self.enable_obstacle = False
+
     def spawn_object(self, spawn_object_request):
         response_id = -1
         response = self.call_service(self.spawn_object_service, spawn_object_request, spin_until_response_received=True)
@@ -87,7 +91,7 @@ def spawn_objects(self):
         global_sensors = []
         vehicles = []
         found_sensor_actor_list = False
-
+        
         for actor in json_actors["objects"]:
             actor_type = actor["type"].split('.')[0]
             if actor["type"] == "sensor.pseudo.actor_list" and self.spawn_sensors_only:
@@ -139,7 +143,55 @@ def setup_vehicles(self, vehicles):
                 spawn_point = None
 
                 # check if there's a spawn_point corresponding to this vehicle
-                spawn_point_param = self.get_param("spawn_point_" + vehicle["id"], None)
+
+                #Read points from file
+                if not self.spawn_points_file or not os.path.exists(self.spawn_points_file):
+                    raise RuntimeError(
+                        "Could not read spawn points from {}".format(self.spawn_points_file))
+                with open(self.spawn_points_file) as handle:
+                    loaded = yaml.load(handle, Loader=yaml.FullLoader)
+                
+                if loaded['obstacle']['enable']: self.enable_obstacle = True
+                if loaded['walker']['enable']: self.enable_walker = True
+
+                if vehicle['id'] == 'walker':
+                    if not self.enable_walker: continue
+                if vehicle['id'] == 'obstacle':
+                    if not self.enable_obstacle: continue
+
+                
+                # print("==========================================================")
+                # print("==========================================================")
+                # print("==========================================================")
+                # print(loaded)
+                # print("==========================================================")
+                # print("==========================================================")
+                # print("==========================================================")
+
+                # print("==========================================================")
+                # print("==========================================================")
+                # print("==========================================================")
+                # print(vehicle["id"])
+                # print("==========================================================")
+                # print("==========================================================")
+                # print("==========================================================")
+                if loaded[vehicle["id"]].has_key("spawn_points"):
+                    spawn_point_param = str(loaded[vehicle["id"]]["spawn_points"]["x"])+","+ \
+                                        str(loaded[vehicle["id"]]["spawn_points"]["y"])+","+ \
+                                        str(loaded[vehicle["id"]]["spawn_points"]["z"])+","+ \
+                                        str(loaded[vehicle["id"]]["spawn_points"]["roll"])+","+ \
+                                        str(loaded[vehicle["id"]]["spawn_points"]["pitch"])+","+ \
+                                        str(loaded[vehicle["id"]]["spawn_points"]["yaw"])
+                #Never
+                else:
+                    spawn_point_param = self.get_param("spawn_point_" + vehicle["id"], None)
+                # print("==========================================================")
+                # print("==========================================================")
+                # print("==========================================================")
+                # print(spawn_point_param)
+                # print("==========================================================")
+                # print("==========================================================")
+                # print("==========================================================")
                 spawn_param_used = False
                 if (spawn_point_param is not None):
                     # try to use spawn_point from parameters
diff --git a/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch b/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch
index d3694776..71b74e99 100755
--- a/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch
+++ b/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch
@@ -2,7 +2,7 @@
 <launch>
   <arg name="role_name" default="ego_vehicle"/>
 
-  <node pkg="carla_twist_to_control" type="carla_twist_to_control.py" name="carla_twist_to_control_$(arg role_name)" output="screen">
+  <node pkg="carla_twist_to_control" type="carla_twist_to_control.py" name="carla_twist_to_control_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)"/>
   </node>
 
diff --git a/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch b/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch
index a107b701..a647366e 100644
--- a/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch
+++ b/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch
@@ -2,19 +2,21 @@
 <launch>
   <arg name="role_name" default="walker"/>
   <arg name="mode" default="vehicle"/>
-  <arg name="walker_point1" default=""/>
+  <arg name="spawn_points_file" default='$(find rubis_autorunner)/cfg/carla_autorunner/carla_spawn_points.yaml'/>
+  <!-- <arg name="walker_point1" default=""/>
   <arg name="walker_point2" default=""/>
   <arg name="walker_point3" default=""/>
-  <arg name="walker_target_speed" default=""/>
+  <arg name="walker_target_speed" default=""/> -->
 
 
-  <node pkg="carla_walker_agent" type="carla_walker_agent.py" name="carla_walker_agent_$(arg role_name)" output="screen">
+  <node pkg="carla_walker_agent" type="carla_walker_agent.py" name="carla_walker_agent_$(arg role_name)">
     <param name="role_name" value="$(arg role_name)" />
     <param name="mode" value="$(arg mode)" />
-    <param name="walker_point1" value="$(arg walker_point1)"/>
+    <param name="spawn_points_file" value='$(arg spawn_points_file)'/>
+    <!-- <param name="walker_point1" value="$(arg walker_point1)"/>
     <param name="walker_point2" value="$(arg walker_point2)"/>
     <param name="walker_point3" value="$(arg walker_point3)"/>
-    <param name="walker_target_speed" value="$(arg walker_target_speed)"/>
+    <param name="walker_target_speed" value="$(arg walker_target_speed)"/> -->
   </node>
 </launch>
 
diff --git a/rubis_ws/src/ros-bridge/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py b/rubis_ws/src/ros-bridge/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py
index d6c88daa..0fcf93c9 100755
--- a/rubis_ws/src/ros-bridge/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py
+++ b/rubis_ws/src/ros-bridge/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py
@@ -10,6 +10,8 @@
 """
 
 import math
+import yaml
+import os
 
 import ros_compatibility as roscomp
 from ros_compatibility.exceptions import ROSInterruptException
@@ -38,24 +40,65 @@ def __init__(self):
         super(CarlaWalkerAgent, self).__init__('carla_walker_agent')
 
         role_name = self.get_param("role_name", "walker")
-        self._walker_target_speed = float(self.get_param("walker_target_speed", 2.0))
+        # self._walker_target_speed = float(self.get_param("walker_target_speed", 2.0))
         # self._fisrt_waypoint_x = self.get_param("fisrt_waypoint_x", 205.4)
         # self._fisrt_waypoint_y = self.get_param("fisrt_waypoint_y", 311.1)
         # self._second_waypoint_x = self.get_param("second_waypoint_x", 190.4)
         # self._second_waypoint_y = self.get_param("second_waypoint_y", 311.1)
         # self._third_waypoint_x = self.get_param("third_waypoint_x", 190.4)
         # self._third_waypoint_y = self.get_param("third_waypoint_y", 311.1)
+        self.spawn_points_file = self.get_param('spawn_points_file','')
+        # print("==================================================")
+        # print(self.spawn_points_file)
+        # print(os.path.exists(self.spawn_points_file))
+        # print("==================================================")
         self._waypoint_params = []
         self._waypoints = []
         
+        # Read points from file
+        if not self.spawn_points_file or not os.path.exists(self.spawn_points_file):
+            raise RuntimeError(
+                "Could not read spawn points from {}".format(self.spawn_points_file))
+        with open(self.spawn_points_file) as handle:
+            loaded = yaml.load(handle, Loader=yaml.FullLoader)
+            # print("==================================================")
+            # print(loaded)
+            # print("==================================================")
+        
+        self._walker_target_speed = float(loaded["walker"]["walker_target_speed"])
+        # not Use
+        # self._fisrt_waypoint_x = float(loaded["walker"]["walker_point1"]["x"])
+        # self._fisrt_waypoint_y = float(loaded["walker"]["walker_point1"]["y"])
+        # self._fisrt_waypoint_x = float(loaded["walker"]["walker_point2"]["x"])
+        # self._fisrt_waypoint_y = float(loaded["walker"]["walker_point2"]["y"])
+        # self._fisrt_waypoint_x = float(loaded["walker"]["walker_point3"]["x"])
+        # self._fisrt_waypoint_y = float(loaded["walker"]["walker_point3"]["y"])
+
         waypoint_iter=1
         while waypoint_iter is not 0:
-            waypoint_param=self.get_param("walker_point"+str(waypoint_iter))
-            if waypoint_param:
+            # waypoint_param=self.get_param("walker_point"+str(waypoint_iter))
+            if loaded["walker"].has_key("walker_point"+str(waypoint_iter)):
+                waypoint_param = str(loaded["walker"]["walker_point"+str(waypoint_iter)]["x"])+","+ \
+                    str(loaded["walker"]["walker_point"+str(waypoint_iter)]["y"])+","+ \
+                    str(loaded["walker"]["walker_point"+str(waypoint_iter)]["z"])+","+ \
+                    str(loaded["walker"]["walker_point"+str(waypoint_iter)]["roll"])+","+ \
+                    str(loaded["walker"]["walker_point"+str(waypoint_iter)]["pitch"])+","+ \
+                    str(loaded["walker"]["walker_point"+str(waypoint_iter)]["yaw"])
+                
                 self._waypoint_params.append(waypoint_param)
                 waypoint_iter += 1
             else:
                 waypoint_iter = 0
+            # if waypoint_param:
+            #     self._waypoint_params.append(waypoint_param)
+            #     waypoint_iter += 1
+            # else:
+            #     waypoint_iter = 0
+
+        # print("==================================================")
+        # print(self._waypoint_params)
+        # print("==================================================")
+
 
         
         rospy.logwarn(self._waypoint_params)
diff --git a/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch b/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch
index 1fdbedf3..e3ef70a1 100644
--- a/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch
+++ b/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch
@@ -5,7 +5,7 @@
   <arg name='timeout' default='10'/>
   <arg name="role_name" default="ego_vehicle" />
 
-  <node pkg="carla_waypoint_publisher" type="carla_waypoint_publisher.py" name="carla_waypoint_publisher" output="screen">
+  <node pkg="carla_waypoint_publisher" type="carla_waypoint_publisher.py" name="carla_waypoint_publisher">
     <param name="role_name" value="$(arg role_name)" />
     <param name="host" value="$(arg host)" />
     <param name="port" value="$(arg port)" />
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/CMakeLists.txt b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/CMakeLists.txt
new file mode 100644
index 00000000..34fd188f
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/CMakeLists.txt
@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(lgsvl_ros_bridge)
+
+catkin_python_setup()
+
+catkin_package()
+
+find_package(catkin REQUIRED COMPONENTS
+  rosapi
+  rosauth
+  lgsvl_rosbridge_library
+  rosbridge_msgs
+  roscpp
+  rospy
+  rostest
+  rubis_msgs
+  std_msgs
+)
+
+catkin_install_python(PROGRAMS
+  scripts/rosbridge_websocket.py
+  scripts/rosbridge_websocket
+  scripts/rosbridge_tcp.py
+  scripts/rosbridge_tcp
+  scripts/rosbridge_udp
+  scripts/rosbridge_udp.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+install(
+  DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/websocket/test_smoke.test)
+endif()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/launch/rosbridge_websocket.launch b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/launch/rosbridge_websocket.launch
new file mode 100644
index 00000000..4c6e632c
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/launch/rosbridge_websocket.launch
@@ -0,0 +1,88 @@
+<launch>
+  <arg name="port" default="9090" />
+  <arg name="address" default="127.0.0.1" />
+  <arg name="ssl" default="false" />
+  <arg name="certfile" default=""/>
+  <arg name="keyfile" default="" />
+
+  <arg name="retry_startup_delay" default="5" />
+
+  <arg name="fragment_timeout" default="600" />
+  <arg name="delay_between_messages" default="0" />
+  <arg name="max_message_size" default="None" />
+  <arg name="unregister_timeout" default="10" />
+  <arg name="websocket_external_port" default="None" />
+
+  <arg name="use_compression" default="false" />
+
+  <arg name="authenticate" default="false" />
+
+  <arg name="websocket_ping_interval" default="0" />
+  <arg name="websocket_ping_timeout" default="30" />
+  <arg name="websocket_null_origin" default="true" />
+
+  <arg name="topics_glob" default="[*]" />
+  <arg name="services_glob" default="[*]" />
+  <arg name="params_glob" default="[*]" />
+  <arg name="bson_only_mode" default="false" />
+  <!-- Output: screen, log -->
+  <arg name="output" default="screen" />
+
+  <!-- Valid options for binary_encoder are "default", "b64" and "bson". -->
+  <arg unless="$(arg bson_only_mode)" name="binary_encoder" default="default"/>
+
+  <group if="$(arg ssl)">
+    <node name="rosbridge_websocket" pkg="lgsvl_ros_bridge" type="rosbridge_websocket" output="$(arg output)">
+      <param name="certfile" value="$(arg certfile)" />
+      <param name="keyfile" value="$(arg keyfile)" />
+      <param name="authenticate" value="$(arg authenticate)" />
+      <param name="port" value="$(arg port)"/>
+      <param name="address" value="$(arg address)"/>
+      <param name="retry_startup_delay" value="$(arg retry_startup_delay)"/>
+      <param name="fragment_timeout" value="$(arg fragment_timeout)"/>
+      <param name="delay_between_messages" value="$(arg delay_between_messages)"/>
+      <param name="max_message_size" value="$(arg max_message_size)"/>
+      <param name="unregister_timeout" value="$(arg unregister_timeout)"/>
+      <param name="use_compression" value="$(arg use_compression)"/>
+
+      <param name="websocket_ping_interval" value="$(arg websocket_ping_interval)" />
+      <param name="websocket_ping_timeout" value="$(arg websocket_ping_timeout)" />
+      <param name="websocket_external_port" value="$(arg websocket_external_port)" />
+      <param name="websocket_null_origin" value="$(arg websocket_null_origin)" />
+
+      <param name="topics_glob" value="$(arg topics_glob)"/>
+      <param name="services_glob" value="$(arg services_glob)"/>
+      <param name="params_glob" value="$(arg params_glob)"/>
+    </node>
+  </group>
+  <group unless="$(arg ssl)">
+    <node name="rosbridge_websocket" pkg="lgsvl_ros_bridge" type="rosbridge_websocket" output="screen">
+      <param name="authenticate" value="$(arg authenticate)" />
+      <param name="port" value="$(arg port)"/>
+      <param name="address" value="$(arg address)"/>
+      <param name="retry_startup_delay" value="$(arg retry_startup_delay)"/>
+      <param name="fragment_timeout" value="$(arg fragment_timeout)"/>
+      <param name="delay_between_messages" value="$(arg delay_between_messages)"/>
+      <param name="max_message_size" value="$(arg max_message_size)"/>
+      <param name="unregister_timeout" value="$(arg unregister_timeout)"/>
+      <param name="use_compression" value="$(arg use_compression)"/>
+
+      <param name="websocket_ping_interval" value="$(arg websocket_ping_interval)" />
+      <param name="websocket_ping_timeout" value="$(arg websocket_ping_timeout)" />
+      <param name="websocket_external_port" value="$(arg websocket_external_port)" />
+
+      <param name="topics_glob" value="$(arg topics_glob)"/>
+      <param name="services_glob" value="$(arg services_glob)"/>
+      <param name="params_glob" value="$(arg params_glob)"/>
+
+      <param name="bson_only_mode" value="$(arg bson_only_mode)"/>
+      <param name="log_level" value="debug"/>
+    </node>
+  </group>
+
+  <node name="rosapi" pkg="rosapi" type="rosapi_node" output="screen">
+    <param name="topics_glob" value="$(arg topics_glob)"/>
+    <param name="services_glob" value="$(arg services_glob)"/>
+    <param name="params_glob" value="$(arg params_glob)"/>
+  </node>
+</launch>
diff --git a/rubis_ws/deprecated/gnss_converter/package.xml b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/package.xml
similarity index 69%
rename from rubis_ws/deprecated/gnss_converter/package.xml
rename to rubis_ws/src/ros-bridge/lgsvl_ros_bridge/package.xml
index 5af6b27f..4fcec6a0 100644
--- a/rubis_ws/deprecated/gnss_converter/package.xml
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/package.xml
@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>gnss_converter</name>
+  <name>lgsvl_ros_bridge</name>
   <version>0.0.0</version>
-  <description>The gnss_converter package</description>
+  <description>The lgsvl_ros_bridge package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="sunhokim@todo.todo">sunhokim</maintainer>
+  <maintainer email="lee@todo.todo">lee</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
@@ -19,7 +19,7 @@
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/gnss_converter</url> -->
+  <!-- <url type="website">http://wiki.ros.org/lgsvl_ros_bridge</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->
@@ -49,12 +49,28 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rosapi</build_depend>
+  <build_depend>rosauth</build_depend>
+  <build_depend>lgsvl_rosbridge_library</build_depend>
+  <build_depend>rosbridge_msgs</build_depend>
   <build_depend>roscpp</build_depend>
-  <build_depend>std_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_export_depend>rosapi</build_export_depend>
+  <build_export_depend>rosauth</build_export_depend>
+  <build_export_depend>lgsvl_rosbridge_library</build_export_depend>
+  <build_export_depend>rosbridge_msgs</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>rostest</build_export_depend>
+  <exec_depend>rosapi</exec_depend>
+  <exec_depend>rosauth</exec_depend>
+  <exec_depend>lgsvl_rosbridge_library</exec_depend>
+  <exec_depend>rosbridge_msgs</exec_depend>
   <exec_depend>roscpp</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>rostest</exec_depend>
+
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_tcp b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_tcp
new file mode 120000
index 00000000..d7a2d32a
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_tcp
@@ -0,0 +1 @@
+./rosbridge_tcp.py
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_tcp.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_tcp.py
new file mode 100755
index 00000000..8506be99
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_tcp.py
@@ -0,0 +1,244 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+from rospy import init_node, get_param, loginfo, logerr, on_shutdown, Publisher
+from lgsvl_ros_bridge import RosbridgeTcpSocket
+
+from lgsvl_rosbridge_library.capabilities.advertise import Advertise
+from lgsvl_rosbridge_library.capabilities.publish import Publish
+from lgsvl_rosbridge_library.capabilities.subscribe import Subscribe
+from lgsvl_rosbridge_library.capabilities.advertise_service import AdvertiseService
+from lgsvl_rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
+from lgsvl_rosbridge_library.capabilities.call_service import CallService
+
+from functools import partial
+from signal import signal, SIGINT, SIG_DFL
+from std_msgs.msg import Int32
+
+try:
+    import SocketServer
+except ImportError:
+    import socketserver as SocketServer
+
+import sys
+import time
+
+#TODO: take care of socket timeouts and make sure to close sockets after killing programm to release network ports
+
+#TODO: add new parameters to websocket version! those of rosbridge_tcp.py might not be needed, but the others should work well when adding them to .._websocket.py
+
+
+def shutdown_hook(server):
+        server.shutdown()
+
+if __name__ == "__main__":
+    loaded = False
+    retry_count = 0
+    while not loaded:
+        retry_count += 1
+        print("trying to start rosbridge TCP server..")
+        try:
+            print("")
+            init_node("rosbridge_tcp")
+            signal(SIGINT, SIG_DFL)
+
+            """
+            Parameter handling:
+             - try to get parameter from parameter server (..define those via launch-file)
+             - overwrite value if given as commandline-parameter
+
+            BEGIN...
+            """
+
+#TODO: ensure types get cast correctly after getting from parameter server
+#TODO: check if ROS parameter server uses None string for 'None-value' or Null or something else, then change code accordingly
+
+            # update parameters from parameter server or use default value ( second parameter of get_param )
+            port = get_param('~port', 9090)
+            host = get_param('~host', '')
+            incoming_buffer = get_param('~incoming_buffer', RosbridgeTcpSocket.incoming_buffer)
+            socket_timeout = get_param('~socket_timeout', RosbridgeTcpSocket.socket_timeout)
+            retry_startup_delay = get_param('~retry_startup_delay', 5.0)  # seconds
+            fragment_timeout = get_param('~fragment_timeout', RosbridgeTcpSocket.fragment_timeout)
+            delay_between_messages = get_param('~delay_between_messages', RosbridgeTcpSocket.delay_between_messages)
+            max_message_size = get_param('~max_message_size', RosbridgeTcpSocket.max_message_size)
+            unregister_timeout = get_param('~unregister_timeout', RosbridgeTcpSocket.unregister_timeout)
+            bson_only_mode = get_param('~bson_only_mode', False)
+
+            if max_message_size == "None":
+                max_message_size = None
+
+            # Get the glob strings and parse them as arrays.
+            RosbridgeTcpSocket.topics_glob = [
+                    element.strip().strip("'")
+                    for element in get_param('~topics_glob', '')[1:-1].split(',')
+                    if len(element.strip().strip("'")) > 0]
+            RosbridgeTcpSocket.services_glob = [
+                    element.strip().strip("'")
+                    for element in get_param('~services_glob', '')[1:-1].split(',')
+                    if len(element.strip().strip("'")) > 0]
+            RosbridgeTcpSocket.params_glob = [
+                    element.strip().strip("'")
+                    for element in get_param('~params_glob', '')[1:-1].split(',')
+                    if len(element.strip().strip("'")) > 0]
+            
+            # Publisher for number of connected clients
+            RosbridgeTcpSocket.client_count_pub = Publisher('client_count', Int32, queue_size=10, latch=True)
+            RosbridgeTcpSocket.client_count_pub.publish(0)
+
+            # update parameters if provided via commandline
+            # .. could implemented 'better' (value/type checking, etc.. )
+            if "--port" in sys.argv:
+                idx = sys.argv.index("--port") + 1
+                if idx < len(sys.argv):
+                    port = int(sys.argv[idx])
+                else:
+                    print("--port argument provided without a value.")
+                    sys.exit(-1)
+
+            if "--host" in sys.argv:
+                idx = sys.argv.index("--host") + 1
+                if idx < len(sys.argv):
+                    host = str(sys.argv[idx])
+                else:
+                    print("--host argument provided without a value.")
+                    sys.exit(-1)
+
+            if "--incoming_buffer" in sys.argv:
+                idx = sys.argv.index("--incoming_buffer") + 1
+                if idx < len(sys.argv):
+                    incoming_buffer = int(sys.argv[idx])
+                else:
+                    print("--incoming_buffer argument provided without a value.")
+                    sys.exit(-1)
+
+            if "--socket_timeout" in sys.argv:
+                idx = sys.argv.index("--socket_timeout") + 1
+                if idx < len(sys.argv):
+                    socket_timeout = int(sys.argv[idx])
+                else:
+                    print("--socket_timeout argument provided without a value.")
+                    sys.exit(-1)
+
+            if "--retry_startup_delay" in sys.argv:
+                idx = sys.argv.index("--retry_startup_delay") + 1
+                if idx < len(sys.argv):
+                    retry_startup_delay = int(sys.argv[idx])
+                else:
+                    print("--retry_startup_delay argument provided without a value.")
+                    sys.exit(-1)
+
+            if "--fragment_timeout" in sys.argv:
+                idx = sys.argv.index("--fragment_timeout") + 1
+                if idx < len(sys.argv):
+                    fragment_timeout = int(sys.argv[idx])
+                else:
+                    print("--fragment_timeout argument provided without a value.")
+                    sys.exit(-1)
+
+            if "--delay_between_messages" in sys.argv:
+                idx = sys.argv.index("--delay_between_messages") + 1
+                if idx < len(sys.argv):
+                    delay_between_messages = float(sys.argv[idx])
+                else:
+                    print("--delay_between_messages argument provided without a value.")
+                    sys.exit(-1)
+
+            if "--max_message_size" in sys.argv:
+                idx = sys.argv.index("--max_message_size") + 1
+                if idx < len(sys.argv):
+                    value = sys.argv[idx]
+                    if value == "None":
+                        max_message_size = None
+                    else:
+                        max_message_size = int(value)
+                else:
+                    print("--max_message_size argument provided without a value. (can be None or <Integer>)")
+                    sys.exit(-1)
+
+            if "--unregister_timeout" in sys.argv:
+                idx = sys.argv.index("--unregister_timeout") + 1
+                if idx < len(sys.argv):
+                    unregister_timeout = float(sys.argv[idx])
+                else:
+                    print("--unregister_timeout argument provided without a value.")
+                    sys.exit(-1)
+
+            # export parameters to handler class
+            RosbridgeTcpSocket.incoming_buffer = incoming_buffer
+            RosbridgeTcpSocket.socket_timeout = socket_timeout
+            RosbridgeTcpSocket.fragment_timeout = fragment_timeout
+            RosbridgeTcpSocket.delay_between_messages = delay_between_messages
+            RosbridgeTcpSocket.max_message_size = max_message_size
+            RosbridgeTcpSocket.unregister_timeout = unregister_timeout
+            RosbridgeTcpSocket.bson_only_mode = bson_only_mode
+
+
+            if "--topics_glob" in sys.argv:
+                idx = sys.argv.index("--topics_glob") + 1
+                if idx < len(sys.argv):
+                    value = sys.argv[idx]
+                    if value == "None":
+                        RosbridgeTcpSocket.topics_glob = []
+                    else:
+                        RosbridgeTcpSocket.topics_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+                else:
+                    print("--topics_glob argument provided without a value. (can be None or a list)")
+                    sys.exit(-1)
+
+            if "--services_glob" in sys.argv:
+                idx = sys.argv.index("--services_glob") + 1
+                if idx < len(sys.argv):
+                    value = sys.argv[idx]
+                    if value == "None":
+                        RosbridgeTcpSocket.services_glob = []
+                    else:
+                        RosbridgeTcpSocket.services_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+                else:
+                    print("--services_glob argument provided without a value. (can be None or a list)")
+                    sys.exit(-1)
+
+            if "--params_glob" in sys.argv:
+                idx = sys.argv.index("--params_glob") + 1
+                if idx < len(sys.argv):
+                    value = sys.argv[idx]
+                    if value == "None":
+                        RosbridgeTcpSocket.params_glob = []
+                    else:
+                        RosbridgeTcpSocket.params_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+                else:
+                    print("--params_glob argument provided without a value. (can be None or a list)")
+                    sys.exit(-1)
+
+            if "--bson_only_mode" in sys.argv:
+                bson_only_mode = True
+
+            # To be able to access the list of topics and services, you must be able to access the rosapi services.
+            if RosbridgeTcpSocket.services_glob:
+                RosbridgeTcpSocket.services_glob.append("/rosapi/*")
+
+            Subscribe.topics_glob = RosbridgeTcpSocket.topics_glob
+            Advertise.topics_glob = RosbridgeTcpSocket.topics_glob
+            Publish.topics_glob = RosbridgeTcpSocket.topics_glob
+            AdvertiseService.services_glob = RosbridgeTcpSocket.services_glob
+            UnadvertiseService.services_glob = RosbridgeTcpSocket.services_glob
+            CallService.services_glob = RosbridgeTcpSocket.services_glob
+
+            """
+            ...END (parameter handling)
+            """
+
+
+            # Server host is a tuple ('host', port)
+            # empty string for host makes server listen on all available interfaces
+            SocketServer.ThreadingTCPServer.allow_reuse_address = True
+            server = SocketServer.ThreadingTCPServer((host, port), RosbridgeTcpSocket)
+            on_shutdown(partial(shutdown_hook, server))
+
+            loginfo("Rosbridge TCP server started on port %d", port)
+
+            server.serve_forever()
+            loaded = True
+        except Exception as e:
+            time.sleep(retry_startup_delay)
+    print("server loaded")
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_udp b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_udp
new file mode 120000
index 00000000..9c844a3b
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_udp
@@ -0,0 +1 @@
+rosbridge_udp.py
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_udp.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_udp.py
new file mode 100755
index 00000000..fb10aa04
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_udp.py
@@ -0,0 +1,200 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from __future__ import print_function
+import rospy
+import sys
+
+from socket import error
+from twisted.internet import reactor
+from lgsvl_ros_bridge import RosbridgeUdpSocket,RosbridgeUdpFactory
+
+from lgsvl_rosbridge_library.capabilities.advertise import Advertise
+from lgsvl_rosbridge_library.capabilities.publish import Publish
+from lgsvl_rosbridge_library.capabilities.subscribe import Subscribe
+from lgsvl_rosbridge_library.capabilities.advertise_service import AdvertiseService
+from lgsvl_rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
+from lgsvl_rosbridge_library.capabilities.call_service import CallService
+
+from std_msgs.msg import Int32
+
+def shutdown_hook():
+    reactor.stop()
+
+if __name__ == "__main__":
+    rospy.init_node("rosbridge_websocket")
+    rospy.on_shutdown(shutdown_hook)    # register shutdown hook to stop the server
+
+    ##################################################
+    # Parameter handling                             #
+    ##################################################
+     # get RosbridgeProtocol parameters
+    RosbridgeUdpSocket.fragment_timeout = rospy.get_param('~fragment_timeout',
+                                                          RosbridgeUdpSocket.fragment_timeout)
+    RosbridgeUdpSocket.delay_between_messages = rospy.get_param('~delay_between_messages',
+                                                                RosbridgeUdpSocket.delay_between_messages)
+    RosbridgeUdpSocket.max_message_size = rospy.get_param('~max_message_size',
+                                                          RosbridgeUdpSocket.max_message_size)
+    RosbridgeUdpSocket.unregister_timeout = rospy.get_param('~unregister_timeout',
+                                                          RosbridgeUdpSocket.unregister_timeout)
+    if RosbridgeUdpSocket.max_message_size == "None":
+        RosbridgeUdpSocket.max_message_size = None
+
+    # Get the glob strings and parse them as arrays.
+    RosbridgeUdpSocket.topics_glob = [
+        element.strip().strip("'")
+        for element in rospy.get_param('~topics_glob', '')[1:-1].split(',')
+        if len(element.strip().strip("'")) > 0]
+    RosbridgeUdpSocket.services_glob = [
+        element.strip().strip("'")
+        for element in rospy.get_param('~services_glob', '')[1:-1].split(',')
+        if len(element.strip().strip("'")) > 0]
+    RosbridgeUdpSocket.params_glob = [
+        element.strip().strip("'")
+        for element in rospy.get_param('~params_glob', '')[1:-1].split(',')
+        if len(element.strip().strip("'")) > 0]
+
+    # if authentication should be used
+    RosbridgeUdpSocket.authenticate = rospy.get_param('~authenticate', False)
+    port = rospy.get_param('~port', 9090)
+    interface = rospy.get_param('~interface', "")
+    # Publisher for number of connected clients
+    RosbridgeUdpSocket.client_count_pub = rospy.Publisher('client_count', Int32, queue_size=10, latch=True)
+    RosbridgeUdpSocket.client_count_pub.publish(0)
+
+    if "--port" in sys.argv:
+        idx = sys.argv.index("--port")+1
+        if idx < len(sys.argv):
+            port = int(sys.argv[idx])
+        else:
+            print("--port argument provided without a value.")
+            sys.exit(-1)
+
+    if "--interface" in sys.argv:
+        idx = sys.argv.index("--interface")+1
+        if idx < len(sys.argv):
+            interface = int(sys.argv[idx])
+        else:
+            print("--interface argument provided without a value.")
+            sys.exit(-1)
+
+    if "--fragment_timeout" in sys.argv:
+        idx = sys.argv.index("--fragment_timeout") + 1
+        if idx < len(sys.argv):
+            RosbridgeUdpSocket.fragment_timeout = int(sys.argv[idx])
+        else:
+            print("--fragment_timeout argument provided without a value.")
+            sys.exit(-1)
+
+    if "--delay_between_messages" in sys.argv:
+        idx = sys.argv.index("--delay_between_messages") + 1
+        if idx < len(sys.argv):
+            RosbridgeUdpSocket.delay_between_messages = float(sys.argv[idx])
+        else:
+            print("--delay_between_messages argument provided without a value.")
+            sys.exit(-1)
+
+    if "--max_message_size" in sys.argv:
+        idx = sys.argv.index("--max_message_size") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeUdpSocket.max_message_size = None
+            else:
+                RosbridgeUdpSocket.max_message_size = int(value)
+        else:
+            print("--max_message_size argument provided without a value. (can be None or <Integer>)")
+            sys.exit(-1)
+
+    if "--unregister_timeout" in sys.argv:
+        idx = sys.argv.index("--unregister_timeout") + 1
+        if idx < len(sys.argv):
+            unregister_timeout = float(sys.argv[idx])
+        else:
+            print("--unregister_timeout argument provided without a value.")
+            sys.exit(-1)
+
+    if "--topics_glob" in sys.argv:
+        idx = sys.argv.index("--topics_glob") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeUdpSocket.topics_glob = []
+            else:
+                RosbridgeUdpSocket.topics_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+        else:
+            print("--topics_glob argument provided without a value. (can be None or a list)")
+            sys.exit(-1)
+
+    if "--services_glob" in sys.argv:
+        idx = sys.argv.index("--services_glob") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeUdpSocket.services_glob = []
+            else:
+                RosbridgeUdpSocket.services_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+        else:
+            print("--services_glob argument provided without a value. (can be None or a list)")
+            sys.exit(-1)
+
+    if "--params_glob" in sys.argv:
+        idx = sys.argv.index("--params_glob") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeUdpSocket.params_glob = []
+            else:
+                RosbridgeUdpSocket.params_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+        else:
+            print("--params_glob argument provided without a value. (can be None or a list)")
+            sys.exit(-1)
+
+    # To be able to access the list of topics and services, you must be able to access the rosapi services.
+    if RosbridgeUdpSocket.services_glob:
+        RosbridgeUdpSocket.services_glob.append("/rosapi/*")
+
+    Subscribe.topics_glob = RosbridgeUdpSocket.topics_glob
+    Advertise.topics_glob = RosbridgeUdpSocket.topics_glob
+    Publish.topics_glob = RosbridgeUdpSocket.topics_glob
+    AdvertiseService.services_glob = RosbridgeUdpSocket.services_glob
+    UnadvertiseService.services_glob = RosbridgeUdpSocket.services_glob
+    CallService.services_glob = RosbridgeUdpSocket.services_glob
+
+    ##################################################
+    # Done with parameter handling                   #
+    ##################################################
+
+    rospy.loginfo("Rosbridge UDP server started on port %d", port)
+    reactor.listenUDP(port, RosbridgeUdpFactory(), interface=interface)
+    reactor.run()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_websocket b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_websocket
new file mode 120000
index 00000000..64706944
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_websocket
@@ -0,0 +1 @@
+rosbridge_websocket.py
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_websocket.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_websocket.py
new file mode 100755
index 00000000..dde2a410
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/scripts/rosbridge_websocket.py
@@ -0,0 +1,320 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from __future__ import print_function
+import rospy
+import sys
+
+from twisted.python import log
+from twisted.internet import reactor, ssl
+from twisted.internet.error import CannotListenError, ReactorNotRunning
+from distutils.version import LooseVersion
+import autobahn #to check version
+from autobahn.twisted.websocket import WebSocketServerFactory, listenWS
+from autobahn.websocket.compress import (PerMessageDeflateOffer,
+                                         PerMessageDeflateOfferAccept)
+log.startLogging(sys.stdout)
+
+from lgsvl_ros_bridge import ClientManager
+from lgsvl_ros_bridge.autobahn_websocket import RosbridgeWebSocket
+from lgsvl_ros_bridge.util import get_ephemeral_port
+
+from lgsvl_rosbridge_library.capabilities.advertise import Advertise
+from lgsvl_rosbridge_library.capabilities.publish import Publish
+from lgsvl_rosbridge_library.capabilities.subscribe import Subscribe
+from lgsvl_rosbridge_library.capabilities.advertise_service import AdvertiseService
+from lgsvl_rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
+from lgsvl_rosbridge_library.capabilities.call_service import CallService
+
+
+def shutdown_hook():
+    try:
+        reactor.stop()
+    except ReactorNotRunning:
+        rospy.logwarn("Can't stop the reactor, it wasn't running")
+
+
+if __name__ == "__main__":
+    rospy.init_node("rosbridge_websocket")
+
+    ##################################################
+    # Parameter handling                             #
+    ##################################################
+    retry_startup_delay = rospy.get_param('~retry_startup_delay', 2.0)  # seconds
+
+    use_compression = rospy.get_param('~use_compression', False)
+
+    # get RosbridgeProtocol parameters
+    RosbridgeWebSocket.fragment_timeout = rospy.get_param('~fragment_timeout',
+                                                          RosbridgeWebSocket.fragment_timeout)
+    RosbridgeWebSocket.delay_between_messages = rospy.get_param('~delay_between_messages',
+                                                                RosbridgeWebSocket.delay_between_messages)
+    RosbridgeWebSocket.max_message_size = rospy.get_param('~max_message_size',
+                                                          RosbridgeWebSocket.max_message_size)
+    RosbridgeWebSocket.unregister_timeout = rospy.get_param('~unregister_timeout',
+                                                          RosbridgeWebSocket.unregister_timeout)
+    bson_only_mode = rospy.get_param('~bson_only_mode', False)
+
+    if RosbridgeWebSocket.max_message_size == "None":
+        RosbridgeWebSocket.max_message_size = None
+
+    ping_interval = float(rospy.get_param('~websocket_ping_interval', 0))
+    ping_timeout = float(rospy.get_param('~websocket_ping_timeout', 30))
+    null_origin = rospy.get_param('~websocket_null_origin', True) #default to original behaviour
+
+    # SSL options
+    certfile = rospy.get_param('~certfile', None)
+    keyfile = rospy.get_param('~keyfile', None)
+    # if authentication should be used
+    RosbridgeWebSocket.authenticate = rospy.get_param('~authenticate', False)
+    port = rospy.get_param('~port', 9090)
+    address = rospy.get_param('~address', "0.0.0.0")
+
+    external_port = rospy.get_param('~websocket_external_port', None)
+    if external_port:
+        try:
+            external_port = int(external_port)
+        except ValueError:
+            external_port = None
+
+    RosbridgeWebSocket.client_manager = ClientManager()
+
+    # Get the glob strings and parse them as arrays.
+    RosbridgeWebSocket.topics_glob = [
+        element.strip().strip("'")
+        for element in rospy.get_param('~topics_glob', '')[1:-1].split(',')
+        if len(element.strip().strip("'")) > 0]
+    RosbridgeWebSocket.services_glob = [
+        element.strip().strip("'")
+        for element in rospy.get_param('~services_glob', '')[1:-1].split(',')
+        if len(element.strip().strip("'")) > 0]
+    RosbridgeWebSocket.params_glob = [
+        element.strip().strip("'")
+        for element in rospy.get_param('~params_glob', '')[1:-1].split(',')
+        if len(element.strip().strip("'")) > 0]
+
+    if "--port" in sys.argv:
+        idx = sys.argv.index("--port")+1
+        if idx < len(sys.argv):
+            port = int(sys.argv[idx])
+        else:
+            print("--port argument provided without a value.")
+            sys.exit(-1)
+
+    if "--address" in sys.argv:
+        idx = sys.argv.index("--address")+1
+        if idx < len(sys.argv):
+            address = str(sys.argv[idx])
+        else:
+            print("--address argument provided without a value.")
+            sys.exit(-1)
+
+    if "--retry_startup_delay" in sys.argv:
+        idx = sys.argv.index("--retry_startup_delay") + 1
+        if idx < len(sys.argv):
+            retry_startup_delay = int(sys.argv[idx])
+        else:
+            print("--retry_startup_delay argument provided without a value.")
+            sys.exit(-1)
+
+    if "--fragment_timeout" in sys.argv:
+        idx = sys.argv.index("--fragment_timeout") + 1
+        if idx < len(sys.argv):
+            RosbridgeWebSocket.fragment_timeout = int(sys.argv[idx])
+        else:
+            print("--fragment_timeout argument provided without a value.")
+            sys.exit(-1)
+
+    if "--delay_between_messages" in sys.argv:
+        idx = sys.argv.index("--delay_between_messages") + 1
+        if idx < len(sys.argv):
+            RosbridgeWebSocket.delay_between_messages = float(sys.argv[idx])
+        else:
+            print("--delay_between_messages argument provided without a value.")
+            sys.exit(-1)
+
+    if "--max_message_size" in sys.argv:
+        idx = sys.argv.index("--max_message_size") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeWebSocket.max_message_size = None
+            else:
+                RosbridgeWebSocket.max_message_size = int(value)
+        else:
+            print("--max_message_size argument provided without a value. (can be None or <Integer>)")
+            sys.exit(-1)
+
+    if "--unregister_timeout" in sys.argv:
+        idx = sys.argv.index("--unregister_timeout") + 1
+        if idx < len(sys.argv):
+            unregister_timeout = float(sys.argv[idx])
+        else:
+            print("--unregister_timeout argument provided without a value.")
+            sys.exit(-1)
+
+    if "--topics_glob" in sys.argv:
+        idx = sys.argv.index("--topics_glob") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeWebSocket.topics_glob = []
+            else:
+                RosbridgeWebSocket.topics_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+        else:
+            print("--topics_glob argument provided without a value. (can be None or a list)")
+            sys.exit(-1)
+
+    if "--services_glob" in sys.argv:
+        idx = sys.argv.index("--services_glob") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeWebSocket.services_glob = []
+            else:
+                RosbridgeWebSocket.services_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+        else:
+            print("--services_glob argument provided without a value. (can be None or a list)")
+            sys.exit(-1)
+
+    if "--params_glob" in sys.argv:
+        idx = sys.argv.index("--params_glob") + 1
+        if idx < len(sys.argv):
+            value = sys.argv[idx]
+            if value == "None":
+                RosbridgeWebSocket.params_glob = []
+            else:
+                RosbridgeWebSocket.params_glob = [element.strip().strip("'") for element in value[1:-1].split(',')]
+        else:
+            print("--params_glob argument provided without a value. (can be None or a list)")
+            sys.exit(-1)
+
+    if ("--bson_only_mode" in sys.argv) or bson_only_mode:
+        RosbridgeWebSocket.bson_only_mode = bson_only_mode
+
+    if "--websocket_ping_interval" in sys.argv:
+        idx = sys.argv.index("--websocket_ping_interval") + 1
+        if idx < len(sys.argv):
+            ping_interval = float(sys.argv[idx])
+        else:
+            print("--websocket_ping_interval argument provided without a value.")
+            sys.exit(-1)
+
+    if "--websocket_ping_timeout" in sys.argv:
+        idx = sys.argv.index("--websocket_ping_timeout") + 1
+        if idx < len(sys.argv):
+            ping_timeout = float(sys.argv[idx])
+        else:
+            print("--websocket_ping_timeout argument provided without a value.")
+            sys.exit(-1)
+
+    if "--websocket_external_port" in sys.argv:
+        idx = sys.argv.index("--websocket_external_port") + 1
+        if idx < len(sys.argv):
+            external_port = int(sys.argv[idx])
+        else:
+            print("--websocket_external_port argument provided without a value.")
+            sys.exit(-1)
+
+    # To be able to access the list of topics and services, you must be able to access the rosapi services.
+    if RosbridgeWebSocket.services_glob:
+        RosbridgeWebSocket.services_glob.append("/rosapi/*")
+
+    Subscribe.topics_glob = RosbridgeWebSocket.topics_glob
+    Advertise.topics_glob = RosbridgeWebSocket.topics_glob
+    Publish.topics_glob = RosbridgeWebSocket.topics_glob
+    AdvertiseService.services_glob = RosbridgeWebSocket.services_glob
+    UnadvertiseService.services_glob = RosbridgeWebSocket.services_glob
+    CallService.services_glob = RosbridgeWebSocket.services_glob
+
+    # Support the legacy "" address value.
+    # The socket library would interpret this as INADDR_ANY.
+    if not address:
+        address = '0.0.0.0'
+
+    ##################################################
+    # Done with parameter handling                   #
+    ##################################################
+
+    def handle_compression_offers(offers):
+        if not use_compression:
+            return
+        for offer in offers:
+            if isinstance(offer, PerMessageDeflateOffer):
+                return PerMessageDeflateOfferAccept(offer)
+
+    if certfile is not None and keyfile is not None:
+        protocol = 'wss'
+        context_factory = ssl.DefaultOpenSSLContextFactory(keyfile, certfile)
+    else:
+        protocol = 'ws'
+        context_factory = None
+
+    # For testing purposes, use an ephemeral port if port == 0.
+    if port == 0:
+        rospy.loginfo('Rosbridge WebSocket Picking an ephemeral port')
+        port = get_ephemeral_port()
+    # Write the actual port as a param for tests to read.
+    rospy.set_param('~actual_port', port)
+
+    uri = '{}://{}:{}'.format(protocol, address, port)
+    factory = WebSocketServerFactory(uri, externalPort=external_port)
+    factory.protocol = RosbridgeWebSocket
+    # https://github.com/crossbario/autobahn-python/commit/2ef13a6804054de74eb36455b58a64a3c701f889
+    if LooseVersion(autobahn.__version__) < LooseVersion("0.15.0"):
+        factory.setProtocolOptions(
+            perMessageCompressionAccept=handle_compression_offers,
+            autoPingInterval=ping_interval,
+            autoPingTimeout=ping_timeout,
+        )
+    else:
+        factory.setProtocolOptions(
+            perMessageCompressionAccept=handle_compression_offers,
+            autoPingInterval=ping_interval,
+            autoPingTimeout=ping_timeout,
+            allowNullOrigin=null_origin,
+        )
+
+    connected = False
+    while not connected and not rospy.is_shutdown():
+        try:
+            listenWS(factory, context_factory)
+            rospy.loginfo('Rosbridge WebSocket server started at {}'.format(uri))
+            connected = True
+        except CannotListenError as e:
+            rospy.logwarn("Unable to start server: " + str(e) +
+                          " Retrying in " + str(retry_startup_delay) + "s.")
+            rospy.sleep(retry_startup_delay)
+
+    rospy.on_shutdown(shutdown_hook)
+    reactor.run()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/setup.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/setup.py
new file mode 100644
index 00000000..0824b5ec
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/setup.py
@@ -0,0 +1,12 @@
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=[
+        'lgsvl_ros_bridge',
+    ],
+    package_dir={'': 'src'}
+)
+
+
+setup(**d)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/__init__.py
new file mode 100644
index 00000000..77647e92
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/__init__.py
@@ -0,0 +1,5 @@
+from __future__ import absolute_import
+from .websocket_handler import RosbridgeWebSocket
+from .tcp_handler import RosbridgeTcpSocket
+from .udp_handler import RosbridgeUdpSocket,RosbridgeUdpFactory
+from .client_mananger import ClientManager
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/autobahn_websocket.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/autobahn_websocket.py
new file mode 100755
index 00000000..2d21ae82
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/autobahn_websocket.py
@@ -0,0 +1,257 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+import uuid
+
+from rosauth.srv import Authentication
+
+import sys
+import threading
+import traceback
+from functools import wraps
+from collections import deque
+
+from autobahn.twisted.websocket import WebSocketServerProtocol
+from twisted.internet import interfaces, reactor
+from zope.interface import implementer
+
+from lgsvl_rosbridge_library.rosbridge_protocol import RosbridgeProtocol
+from lgsvl_rosbridge_library.util import json, bson
+
+
+def _log_exception():
+    """Log the most recent exception to ROS."""
+    exc = traceback.format_exception(*sys.exc_info())
+    rospy.logerr(''.join(exc))
+
+
+def log_exceptions(f):
+    """Decorator for logging exceptions to ROS."""
+    @wraps(f)
+    def wrapper(*args, **kwargs):
+        try:
+            return f(*args, **kwargs)
+        except:
+            _log_exception()
+            raise
+    return wrapper
+
+
+class IncomingQueue(threading.Thread):
+    """Decouples incoming messages from the Autobahn thread.
+
+    This mitigates cases where outgoing messages are blocked by incoming,
+    and vice versa.
+    """
+    def __init__(self, protocol):
+        threading.Thread.__init__(self)
+        self.daemon = True
+        self.queue = deque()
+        self.protocol = protocol
+
+        self.cond = threading.Condition()
+        self._finished = False
+
+    def finish(self):
+        """Clear the queue and do not accept further messages."""
+        with self.cond:
+            self._finished = True
+            while len(self.queue) > 0:
+                self.queue.popleft()
+            self.cond.notify()
+
+    def push(self, msg):
+        with self.cond:
+            self.queue.append(msg)
+            self.cond.notify()
+
+    def run(self):
+        while True:
+            with self.cond:
+                if len(self.queue) == 0 and not self._finished:
+                    self.cond.wait()
+
+                if self._finished:
+                    break
+
+                msg = self.queue.popleft()
+
+            self.protocol.incoming(msg)
+
+        self.protocol.finish()
+
+
+@implementer(interfaces.IPushProducer)
+class OutgoingValve:
+    """Allows the Autobahn transport to pause outgoing messages from rosbridge.
+
+    The purpose of this valve is to connect backpressure from the WebSocket client
+    back to the rosbridge protocol, which depends on backpressure for queueing.
+    Without this flow control, rosbridge will happily keep writing messages to
+    the WebSocket until the system runs out of memory.
+
+    This valve is closed and opened automatically by the Twisted TCP server.
+    In practice, Twisted should only close the valve when its userspace write buffer
+    is full and it should only open the valve when that buffer is empty.
+
+    When the valve is closed, the rosbridge protocol instance's outgoing writes
+    must block until the valve is opened.
+    """
+    def __init__(self, proto):
+        self._proto = proto
+        self._valve = threading.Event()
+        self._finished = False
+
+    @log_exceptions
+    def relay(self, message, compression="none"):
+        self._valve.wait()
+        if self._finished:
+            return
+        reactor.callFromThread(self._proto.outgoing, message, compression=compression)
+
+    def pauseProducing(self):
+        if not self._finished:
+            self._valve.clear()
+
+    def resumeProducing(self):
+        self._valve.set()
+
+    def stopProducing(self):
+        self._finished = True
+        self._valve.set()
+
+
+class RosbridgeWebSocket(WebSocketServerProtocol):
+    client_id_seed = 0
+    clients_connected = 0
+    authenticate = False
+
+    # The following are passed on to RosbridgeProtocol
+    # defragmentation.py:
+    fragment_timeout = 600                  # seconds
+    # protocol.py:
+    delay_between_messages = 0              # seconds
+    max_message_size = None                 # bytes
+    unregister_timeout = 10.0               # seconds
+    bson_only_mode = False
+
+    def onOpen(self):
+        cls = self.__class__
+        parameters = {
+            "fragment_timeout": cls.fragment_timeout,
+            "delay_between_messages": cls.delay_between_messages,
+            "max_message_size": cls.max_message_size,
+            "unregister_timeout": cls.unregister_timeout,
+            "bson_only_mode": cls.bson_only_mode
+        }
+        try:
+            self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters)
+            self.incoming_queue = IncomingQueue(self.protocol)
+            self.incoming_queue.start()
+            producer = OutgoingValve(self)
+            self.transport.registerProducer(producer, True)
+            producer.resumeProducing()
+            self.protocol.outgoing = producer.relay
+            self.authenticated = False
+            cls.client_id_seed += 1
+            cls.clients_connected += 1
+            self.client_id = uuid.uuid4()
+            self.peer = self.transport.getPeer().host
+            if cls.client_manager:
+                cls.client_manager.add_client(self.client_id, self.peer)
+
+        except Exception as exc:
+            rospy.logerr("Unable to accept incoming connection.  Reason: %s", str(exc))
+        rospy.loginfo("Client connected.  %d clients total.", cls.clients_connected)
+        if cls.authenticate:
+            rospy.loginfo("Awaiting proper authentication...")
+
+    def onMessage(self, message, binary):
+        cls = self.__class__
+        if not binary:
+            message = message.decode('utf-8')
+        # check if we need to authenticate
+        if cls.authenticate and not self.authenticated:
+            try:
+                if cls.bson_only_mode:
+                    msg = bson.BSON(message).decode()
+                else:
+                    msg = json.loads(message)
+
+                if msg['op'] == 'auth':
+                    # check the authorization information
+                    auth_srv = rospy.ServiceProxy('authenticate', Authentication)
+                    resp = auth_srv(msg['mac'], msg['client'], msg['dest'],
+                                                  msg['rand'], rospy.Time(msg['t']), msg['level'],
+                                                  rospy.Time(msg['end']))
+                    self.authenticated = resp.authenticated
+                    if self.authenticated:
+                        rospy.loginfo("Client %d has authenticated.", self.protocol.client_id)
+                        return
+                # if we are here, no valid authentication was given
+                rospy.logwarn("Client %d did not authenticate. Closing connection.",
+                              self.protocol.client_id)
+                self.sendClose()
+            except:
+                # proper error will be handled in the protocol class
+                self.incoming_queue.push(message)
+        else:
+            # no authentication required
+            self.incoming_queue.push(message)
+
+    def outgoing(self, message, compression="none"):
+        if type(message) == bson.BSON:
+            binary = True
+            message = bytes(message)
+        elif type(message) == bytearray:
+            binary = True
+            message = bytes(message)
+        elif compression in ["cbor", "cbor-raw"]:
+            binary = True
+        else:
+            binary = False
+            message = message.encode('utf-8')
+
+        self.sendMessage(message, binary)
+
+    def onClose(self, was_clean, code, reason):
+        if not hasattr(self, 'protocol'):
+            return  # Closed before connection was opened.
+        cls = self.__class__
+        cls.clients_connected -= 1
+
+        if cls.client_manager:
+            cls.client_manager.remove_client(self.client_id, self.peer)
+        rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected)
+
+        self.incoming_queue.finish()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/client_mananger.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/client_mananger.py
new file mode 100644
index 00000000..f79a49b2
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/client_mananger.py
@@ -0,0 +1,69 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2018, Intermodalics
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Intermodalics nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+import rospy
+import threading
+from rosbridge_msgs.msg import ConnectedClient, ConnectedClients
+from std_msgs.msg import Int32
+
+
+class ClientManager:
+    def __init__(self):
+        # Publisher for number of connected clients
+        self._client_count_pub = rospy.Publisher(
+          'client_count', Int32, queue_size=10, latch=True)
+        # Publisher for connected clients
+        self._conn_clients_pub = rospy.Publisher(
+          'connected_clients', ConnectedClients, queue_size=10, latch=True)
+        self._lock = threading.Lock()
+        self._client_count = 0
+        self._clients = {}
+        self.__publish()
+
+    def __publish(self):
+        msg = ConnectedClients()
+        msg.clients = list(self._clients.values())
+        self._conn_clients_pub.publish(msg)
+        self._client_count_pub.publish(len(msg.clients))
+
+    def add_client(self, client_id, ip_address):
+        with self._lock:
+            client = ConnectedClient()
+            client.ip_address = ip_address
+            client.connection_time = rospy.Time.now()
+            self._clients[client_id] = client
+            self.__publish()
+
+    def remove_client(self, client_id, ip_address):
+        with self._lock:
+            self._clients.pop(client_id, None)
+            self.__publish()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/tcp_handler.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/tcp_handler.py
new file mode 100755
index 00000000..bbc894aa
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/tcp_handler.py
@@ -0,0 +1,143 @@
+import rospy
+import struct
+from lgsvl_rosbridge_library.rosbridge_protocol import RosbridgeProtocol
+
+try:
+    import SocketServer
+except ImportError:
+    import socketserver as SocketServer
+
+class RosbridgeTcpSocket(SocketServer.BaseRequestHandler):
+    """
+    TCP Socket server for rosbridge.
+    An instance of this class is created for each request.
+    """
+
+    busy = False
+    queue = []
+    client_id_seed = 0
+    clients_connected = 0
+    client_count_pub = None
+
+    # list of parameters
+    incoming_buffer = 65536                 # bytes
+    socket_timeout = 10                     # seconds
+    # The following are passed on to RosbridgeProtocol
+    # defragmentation.py:
+    fragment_timeout = 600                  # seconds
+    # protocol.py:
+    delay_between_messages = 0              # seconds
+    max_message_size = None                 # bytes
+    unregister_timeout = 10.0               # seconds
+    bson_only_mode = False
+
+    def setup(self):
+        """
+        Called before the handle() method to perform any initialization
+        actions required. The default implementation does nothing.
+        """
+        cls = self.__class__
+        parameters = {
+            "fragment_timeout": cls.fragment_timeout,
+            "delay_between_messages": cls.delay_between_messages,
+            "max_message_size": cls.max_message_size,
+            "unregister_timeout": cls.unregister_timeout,
+            "bson_only_mode": cls.bson_only_mode
+        }
+
+        try:
+            self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters)
+            self.protocol.outgoing = self.send_message
+            cls.client_id_seed += 1
+            cls.clients_connected += 1
+            if cls.client_count_pub:
+                cls.client_count_pub.publish(cls.clients_connected)
+            self.protocol.log("info", "connected. " + str(cls.clients_connected) + " client total.")
+        except Exception as exc:
+            rospy.logerr("Unable to accept incoming connection.  Reason: %s", str(exc))
+
+    def recvall(self,n):
+        # http://stackoverflow.com/questions/17667903/python-socket-receive-large-amount-of-data
+        # Helper function to recv n bytes or return None if EOF is hit
+        data = bytearray()
+        while len(data) < n:
+            packet = self.request.recv(n - len(data))
+            if not packet:
+                return None
+            data.extend(packet)
+        return data
+
+    def recv_bson(self):
+        # Read 4 bytes to get the length of the BSON packet
+        BSON_LENGTH_IN_BYTES = 4
+        raw_msglen = self.recvall(BSON_LENGTH_IN_BYTES)
+        if not raw_msglen:
+            return None
+        msglen = struct.unpack('i', raw_msglen)[0]
+
+        # Retrieve the rest of the message
+        data = self.recvall(msglen - BSON_LENGTH_IN_BYTES)
+        if data == None:
+            return None
+        data = raw_msglen + data # Prefix the data with the message length that has already been received.
+                                 # The message length is part of BSONs message format
+
+        # Exit on empty message
+        if len(data) == 0:
+            return None
+
+        self.protocol.incoming(data)
+        return True
+
+    def handle(self):
+        """
+        Listen for TCP messages and do all the work to service a request.
+        """
+        cls = self.__class__
+        self.request.settimeout(cls.socket_timeout)
+        while True:
+            try:
+                if self.bson_only_mode:
+                    if self.recv_bson() == None:
+                        break
+                    continue
+
+                # non-BSON handling
+                bytes = self.request.recv(cls.incoming_buffer)
+
+                # Exit on empty message because socket was closed
+                if len(bytes) == 0:
+                    break
+                else:
+                    string_encoded = bytes.decode().strip()
+                    if len(string_encoded) > 0:
+                        self.protocol.incoming(string_encoded)
+
+            except Exception as e:
+                pass
+                self.protocol.log("debug", "socket connection timed out! (ignore warning if client is only listening..)")
+
+    def finish(self):
+        """
+        Called when TCP connection finishes.
+        Called after the handle() method to perform any clean-up actions required.
+        If setup() raises an exception, this function will not be called.
+        """
+        cls = self.__class__
+        cls.clients_connected -= 1
+        self.protocol.finish()
+        if cls.client_count_pub:
+            cls.client_count_pub.publish(cls.clients_connected)
+        self.protocol.log("info", "disconnected. " + str(cls.clients_connected) + " client total." )
+
+    def send_message(self, message=None, compression="none"):
+        """
+        Callback from rosbridge
+        """
+        if self.bson_only_mode:
+            self.request.sendall(message)
+        elif message is not None:
+            self.request.sendall(message.encode())
+        else:
+            rospy.logerr("send_message called with no message or message is None, not sending")
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/udp_handler.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/udp_handler.py
new file mode 100644
index 00000000..4837c151
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/udp_handler.py
@@ -0,0 +1,100 @@
+import rospy
+from rosauth.srv import Authentication
+from lgsvl_rosbridge_library.rosbridge_protocol import RosbridgeProtocol
+from lgsvl_rosbridge_library.util import json, bson
+
+from twisted.internet.protocol import DatagramProtocol,Factory
+
+class RosbridgeUdpFactory(DatagramProtocol):
+    def startProtocol(self):
+        self.socks = dict()
+    def datagramReceived(self, message, source_addr):
+        (host, port) = source_addr
+        endpoint = host.__str__() + port.__str__()
+        if endpoint in self.socks:
+            self.socks[endpoint].datagramReceived(message)
+        else:
+            writefunc = lambda msg: self.transport.write(msg.encode(), (host,port))
+            self.socks[endpoint] = RosbridgeUdpSocket(writefunc)
+            self.socks[endpoint].startProtocol()
+            self.socks[endpoint].datagramReceived(message)
+
+class RosbridgeUdpSocket:
+    client_id_seed = 0
+    clients_connected = 0
+    client_count_pub = None
+    authenticate = False
+
+    # The following parameters are passed on to RosbridgeProtocol
+    # defragmentation.py:
+    fragment_timeout = 600                  # seconds
+    # protocol.py:
+    delay_between_messages = 0              # seconds
+    max_message_size = None                 # bytes
+    unregister_timeout = 10.0               # seconds
+
+    def __init__(self,write):
+        self.write = write
+        self.authenticated = False
+
+    def startProtocol(self):
+        cls = self.__class__
+        parameters = {
+            "fragment_timeout": cls.fragment_timeout,
+            "delay_between_messages": cls.delay_between_messages,
+            "max_message_size": cls.max_message_size,
+            "unregister_timeout": cls.unregister_timeout
+        }
+        try:
+            self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters)
+            self.protocol.outgoing = self.send_message
+            self.authenticated = False
+            cls.client_id_seed += 1
+            cls.clients_connected += 1
+            if cls.client_count_pub:
+                cls.client_count_pub.publish(cls.clients_connected)
+        except Exception as exc:
+            rospy.logerr("Unable to accept incoming connection.  Reason: %s", str(exc))
+        rospy.loginfo("Client connected.  %d clients total.", cls.clients_connected)
+        if cls.authenticate:
+            rospy.loginfo("Awaiting proper authentication...")
+
+    def datagramReceived(self, message):
+        cls = self.__class__
+        # check if we need to authenticate
+        if cls.authenticate and not self.authenticated:
+            try:
+                msg = json.loads(message)
+                if msg['op'] == 'auth':
+                    # check the authorization information
+                    auth_srv = rospy.ServiceProxy('authenticate', Authentication)
+                    resp = auth_srv(msg['mac'], msg['client'], msg['dest'],
+                                                  msg['rand'], rospy.Time(msg['t']), msg['level'],
+                                                  rospy.Time(msg['end']))
+                    self.authenticated = resp.authenticated
+                    if self.authenticated:
+                        rospy.loginfo("Client %d has authenticated.", self.protocol.client_id)
+                        return
+                # if we are here, no valid authentication was given
+                rospy.logwarn("Client %d did not authenticate. Closing connection.",
+                              self.protocol.client_id)
+                self.close()
+            except:
+                # proper error will be handled in the protocol class
+                self.protocol.incoming(message)
+        else:
+            # no authentication required
+            self.protocol.incoming(message)
+
+    def stopProtocol(self):
+        cls = self.__class__
+        cls.clients_connected -= 1
+        self.protocol.finish()
+        if cls.client_count_pub:
+            cls.client_count_pub.publish(cls.clients_connected)
+        rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected)
+    def send_message(self, message, compression="none"):
+        binary = type(message)==bson.BSON
+        self.write(message)
+    def check_origin(self, origin):
+        return False
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/util.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/util.py
new file mode 100644
index 00000000..b546e985
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/util.py
@@ -0,0 +1,12 @@
+import socket
+
+
+def get_ephemeral_port(sock_family=socket.AF_INET, sock_type=socket.SOCK_STREAM):
+    """Return an ostensibly available ephemeral port number."""
+    # We expect that the operating system is polite enough to not hand out the
+    # same ephemeral port before we can explicitly bind it a second time.
+    s = socket.socket(sock_family, sock_type)
+    s.bind(('', 0))
+    port = s.getsockname()[1]
+    s.close()
+    return port
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/websocket_handler.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/websocket_handler.py
new file mode 100644
index 00000000..0c44ae90
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/src/lgsvl_ros_bridge/websocket_handler.py
@@ -0,0 +1,214 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+import uuid
+
+from rosauth.srv import Authentication
+
+import sys
+import threading
+import traceback
+from functools import partial, wraps
+
+from tornado import version_info as tornado_version_info
+from tornado.ioloop import IOLoop
+from tornado.iostream import StreamClosedError
+from tornado.websocket import WebSocketHandler, WebSocketClosedError
+from tornado.gen import coroutine, BadYieldError
+
+from lgsvl_rosbridge_library.rosbridge_protocol import RosbridgeProtocol
+from lgsvl_rosbridge_library.util import json, bson
+
+
+def _log_exception():
+    """Log the most recent exception to ROS."""
+    exc = traceback.format_exception(*sys.exc_info())
+    rospy.logerr(''.join(exc))
+
+
+def log_exceptions(f):
+    """Decorator for logging exceptions to ROS."""
+    @wraps(f)
+    def wrapper(*args, **kwargs):
+        try:
+            return f(*args, **kwargs)
+        except:
+            _log_exception()
+            raise
+    return wrapper
+
+
+class RosbridgeWebSocket(WebSocketHandler):
+    client_id_seed = 0
+    clients_connected = 0
+    authenticate = False
+    use_compression = False
+
+    # The following are passed on to RosbridgeProtocol
+    # defragmentation.py:
+    fragment_timeout = 600                  # seconds
+    # protocol.py:
+    delay_between_messages = 0              # seconds
+    max_message_size = 10 * 1024 * 1024     # bytes
+    unregister_timeout = 10.0               # seconds
+    bson_only_mode = False
+
+    @log_exceptions
+    def open(self):
+        rospy.logwarn_once('The Tornado Rosbridge WebSocket implementation is deprecated.'
+                           ' See lgsvl_ros_bridge.autobahn_websocket'
+                           ' and rosbridge_websocket.py')
+        cls = self.__class__
+        parameters = {
+            "fragment_timeout": cls.fragment_timeout,
+            "delay_between_messages": cls.delay_between_messages,
+            "max_message_size": cls.max_message_size,
+            "unregister_timeout": cls.unregister_timeout,
+            "bson_only_mode": cls.bson_only_mode
+        }
+        try:
+            self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters)
+            self.protocol.outgoing = self.send_message
+            self.set_nodelay(True)
+            self.authenticated = False
+            self._write_lock = threading.RLock()
+            cls.client_id_seed += 1
+            cls.clients_connected += 1
+            self.client_id = uuid.uuid4()
+            if cls.client_manager:
+                cls.client_manager.add_client(self.client_id, self.request.remote_ip)
+        except Exception as exc:
+            rospy.logerr("Unable to accept incoming connection.  Reason: %s", str(exc))
+        rospy.loginfo("Client connected.  %d clients total.", cls.clients_connected)
+        if cls.authenticate:
+            rospy.loginfo("Awaiting proper authentication...")
+
+    @log_exceptions
+    def on_message(self, message):
+        cls = self.__class__
+        # check if we need to authenticate
+        if cls.authenticate and not self.authenticated:
+            try:
+                if cls.bson_only_mode:
+                    msg = bson.BSON(message).decode()
+                else:
+                    msg = json.loads(message)
+
+                if msg['op'] == 'auth':
+                    # check the authorization information
+                    auth_srv = rospy.ServiceProxy('authenticate', Authentication)
+                    resp = auth_srv(msg['mac'], msg['client'], msg['dest'],
+                                                  msg['rand'], rospy.Time(msg['t']), msg['level'],
+                                                  rospy.Time(msg['end']))
+                    self.authenticated = resp.authenticated
+                    if self.authenticated:
+                        rospy.loginfo("Client %d has authenticated.", self.protocol.client_id)
+                        return
+                # if we are here, no valid authentication was given
+                rospy.logwarn("Client %d did not authenticate. Closing connection.",
+                              self.protocol.client_id)
+                self.close()
+            except:
+                # proper error will be handled in the protocol class
+                self.protocol.incoming(message)
+        else:
+            # no authentication required
+            self.protocol.incoming(message)
+
+    @log_exceptions
+    def on_close(self):
+        cls = self.__class__
+        cls.clients_connected -= 1
+        self.protocol.finish()
+        if cls.client_manager:
+            cls.client_manager.remove_client(self.client_id, self.request.remote_ip)
+        rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected)
+
+    def send_message(self, message, compression="none"):
+        if type(message) == bson.BSON:
+            binary = True
+        elif type(message) == bytearray:
+            binary = True
+            message = bytes(message)
+        else:
+            binary = False
+
+        with self._write_lock:
+            IOLoop.instance().add_callback(partial(self.prewrite_message, message, binary))
+
+    @coroutine
+    def prewrite_message(self, message, binary):
+        # Use a try block because the log decorator doesn't cooperate with @coroutine.
+        try:
+            with self._write_lock:
+                future = self.write_message(message, binary)
+
+                # When closing, self.write_message() return None even if it's an undocument output.
+                # Consider it as WebSocketClosedError
+                # For tornado versions <4.3.0 self.write_message() does not have a return value
+                if future is None and tornado_version_info >= (4,3,0,0):
+                    raise WebSocketClosedError
+
+                yield future
+        except WebSocketClosedError:
+            rospy.logwarn_throttle(1, 'WebSocketClosedError: Tried to write to a closed websocket')
+            raise
+        except StreamClosedError:
+            rospy.logwarn_throttle(1, 'StreamClosedError: Tried to write to a closed stream')
+            raise
+        except BadYieldError:
+            # Tornado <4.5.0 doesn't like its own yield and raises BadYieldError.
+            # This does not affect functionality, so pass silently only in this case.
+            if tornado_version_info < (4, 5, 0, 0):
+                pass
+            else:
+                _log_exception()
+                raise
+        except:
+            _log_exception()
+            raise
+
+    @log_exceptions
+    def check_origin(self, origin):
+        return True
+
+    @log_exceptions
+    def get_compression_options(self):
+        # If this method returns None (the default), compression will be disabled.
+        # If it returns a dict (even an empty one), it will be enabled.
+        cls = self.__class__
+
+        if not cls.use_compression:
+            return None
+
+        return {}
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_cbor_raw.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_cbor_raw.py
new file mode 100755
index 00000000..c3a8d98f
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_cbor_raw.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+import json
+import io
+import os
+import rostest
+import sys
+import threading
+import unittest
+
+import rospy
+from std_msgs.msg import String
+
+from twisted.internet import reactor
+from autobahn.twisted.websocket import (WebSocketClientProtocol,
+                                        WebSocketClientFactory)
+
+from lgsvl_rosbridge_library.util.cbor import loads as decode_cbor
+
+from twisted.python import log
+log.startLogging(sys.stderr)
+
+TOPIC = '/b_topic'
+STRING = 'B' * 10000
+WARMUP_DELAY = 1.0  # seconds
+TIME_LIMIT = 5.0  # seconds
+
+
+class TestWebsocketCborRaw(unittest.TestCase):
+    def test_cbor_raw(self):
+        test_client_received = []
+        class TestClientProtocol(WebSocketClientProtocol):
+            def onOpen(self):
+                self.sendMessage(json.dumps({
+                    'op': 'subscribe',
+                    'topic': TOPIC,
+                    'compression': 'cbor-raw',
+                }).encode('utf-8'))
+
+            def onMessage(self, payload, binary):
+                test_client_received.append(payload)
+
+        protocol = os.environ.get('PROTOCOL')
+        port = rospy.get_param('/rosbridge_websocket/actual_port')
+        url = protocol + '://127.0.0.1:' + str(port)
+
+        factory = WebSocketClientFactory(url)
+        factory.protocol = TestClientProtocol
+        reactor.connectTCP('127.0.0.1', port, factory)
+
+        pub = rospy.Publisher(TOPIC, String, queue_size=1)
+        def publish_timer():
+            rospy.sleep(WARMUP_DELAY)
+            pub.publish(String(STRING))
+            rospy.sleep(TIME_LIMIT)
+            reactor.stop()
+        reactor.callInThread(publish_timer)
+        reactor.run()
+
+        self.assertEqual(len(test_client_received), 1)
+        websocket_message = decode_cbor(test_client_received[0])
+        self.assertEqual(websocket_message['topic'], TOPIC)
+        buff = io.BytesIO()
+        String(STRING).serialize(buff)
+        self.assertEqual(websocket_message['msg']['bytes'], buff.getvalue())
+
+
+PKG = 'lgsvl_ros_bridge'
+NAME = 'test_websocket_cbor_raw'
+
+if __name__ == '__main__':
+    rospy.init_node(NAME)
+
+    while not rospy.is_shutdown() and not rospy.has_param('/rosbridge_websocket/actual_port'):
+        rospy.sleep(1.0)
+
+    rostest.rosrun(PKG, NAME, TestWebsocketCborRaw)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_smoke.py b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_smoke.py
new file mode 100755
index 00000000..c31f98a4
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_smoke.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python
+import json
+import os
+import rostest
+import sys
+import threading
+import unittest
+
+import rospy
+from std_msgs.msg import String
+
+from twisted.internet import reactor
+from autobahn.twisted.websocket import (WebSocketClientProtocol,
+                                        WebSocketClientFactory)
+
+from twisted.python import log
+log.startLogging(sys.stderr)
+
+# For consistency, the number of messages must not exceed the the protocol
+# Subscriber queue_size.
+NUM_MSGS = 10
+MSG_SIZE = 10
+A_TOPIC = '/a_topic'
+B_TOPIC = '/b_topic'
+A_STRING = 'A' * MSG_SIZE
+B_STRING = 'B' * MSG_SIZE
+WARMUP_DELAY = 1.0  # seconds
+TIME_LIMIT = 5.0  # seconds
+
+
+class TestClientProtocol(WebSocketClientProtocol):
+    def onOpen(self):
+        self._sendDict({
+            'op': 'subscribe',
+            'topic': B_TOPIC,
+            'type': 'std_msgs/String',
+            'queue_length': 0,  # Test the roslib default.
+        })
+        self._sendDict({
+            'op': 'advertise',
+            'topic': A_TOPIC,
+            'type': 'std_msgs/String',
+        })
+        self._sendDict({
+            'op': 'publish',
+            'topic': A_TOPIC,
+            'msg': {
+                'data': A_STRING,
+            },
+        }, NUM_MSGS)
+
+    def _sendDict(self, msg_dict, times=1):
+        msg = json.dumps(msg_dict).encode('utf-8')
+        for _ in range(times):
+            self.sendMessage(msg)
+
+    def onMessage(self, payload, binary):
+        self.__class__.received.append(payload)
+
+
+class TestWebsocketSmoke(unittest.TestCase):
+    def test_smoke(self):
+        protocol = os.environ.get('PROTOCOL')
+        port = rospy.get_param('/rosbridge_websocket/actual_port')
+        url = protocol + '://127.0.0.1:' + str(port)
+
+        factory = WebSocketClientFactory(url)
+        factory.protocol = TestClientProtocol
+        reactor.connectTCP('127.0.0.1', port, factory)
+
+        ros_received = []
+        TestClientProtocol.received = []
+        rospy.Subscriber(A_TOPIC, String, ros_received.append)
+        pub = rospy.Publisher(B_TOPIC, String, queue_size=NUM_MSGS)
+
+        def publish_timer():
+            rospy.sleep(WARMUP_DELAY)
+            msg = String(B_STRING)
+            for _ in range(NUM_MSGS):
+                pub.publish(msg)
+
+        def shutdown_timer():
+            rospy.sleep(WARMUP_DELAY + TIME_LIMIT)
+            reactor.stop()
+
+        reactor.callInThread(publish_timer)
+        reactor.callInThread(shutdown_timer)
+        reactor.run()
+
+        for received in TestClientProtocol.received:
+            msg = json.loads(received)
+            self.assertEqual('publish', msg['op'])
+            self.assertEqual(B_TOPIC, msg['topic'])
+            self.assertEqual(B_STRING, msg['msg']['data'])
+        self.assertEqual(NUM_MSGS, len(TestClientProtocol.received))
+
+        for msg in ros_received:
+            self.assertEqual(A_STRING, msg.data)
+        self.assertEqual(NUM_MSGS, len(ros_received))
+
+
+PKG = 'lgsvl_ros_bridge'
+NAME = 'test_websocket_smoke'
+
+if __name__ == '__main__':
+    rospy.init_node(NAME)
+
+    while not rospy.is_shutdown() and not rospy.has_param('/rosbridge_websocket/actual_port'):
+        rospy.sleep(1.0)
+
+    rostest.rosrun(PKG, NAME, TestWebsocketSmoke)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_smoke.test b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_smoke.test
new file mode 100644
index 00000000..9d192e93
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_ros_bridge/test/websocket/test_smoke.test
@@ -0,0 +1,8 @@
+<launch>
+  <env name="PROTOCOL" value="ws" />
+  <node name="rosbridge_websocket" pkg="lgsvl_ros_bridge" type="rosbridge_websocket.py">
+    <param name="port" value="0" />
+  </node>
+  <test test-name="test_smoke" pkg="lgsvl_ros_bridge" type="test_smoke.py" />
+  <test test-name="test_cbor_raw" pkg="lgsvl_ros_bridge" type="test_cbor_raw.py" />
+</launch>
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/CHANGELOG.rst b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/CHANGELOG.rst
new file mode 100644
index 00000000..11f45f12
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/CHANGELOG.rst
@@ -0,0 +1,650 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package lgsvl_rosbridge_library
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.11.16 (2022-10-18)
+--------------------
+* Bump minimum required cmake version. (`#814 <https://github.com/RobotWebTools/rosbridge_suite/issues/814>`_)
+* Fix send_message being called with wrong arguments. (`#812 <https://github.com/RobotWebTools/rosbridge_suite/issues/812>`_)
+* Contributors: Hans-Joachim Krauch
+
+0.11.15 (2022-10-06)
+--------------------
+* Remove unnecessary checking of topic globs. (`#793 <https://github.com/RobotWebTools/rosbridge_suite/issues/793>`_)
+  We do not have to check the topic globs on each incoming ros message. It is enough to check if the topic is allowed to be subscribed in the actual subscribe call as the message callback method (publish) is only called when the topic is subscribed to.
+* Skip unnecessary conversion for cbor/cbor-raw compression (`#792 <https://github.com/RobotWebTools/rosbridge_suite/issues/792>`_)
+  * Skip unnecessary conversion for cbor compression.
+  This change avoids some unnecessary conversions when using cbor/cbor-raw compression, leading to a significantly perfomance boost.
+  * Add caching for subscriptions with cbor compression.
+* Contributors: Hans-Joachim Krauch
+
+0.11.14 (2022-06-13)
+--------------------
+* Fix bson support for python3 (`#545 <https://github.com/RobotWebTools/rosbridge_suite/issues/545>`_)
+* Small fixes (`#681 <https://github.com/RobotWebTools/rosbridge_suite/issues/681>`_)
+* Check if key exists before accessing the dict in SubscriberManager::unsubscribe(), patches https://github.com/RobotWebTools/rosbridge_suite/issues/580 (`#638 <https://github.com/RobotWebTools/rosbridge_suite/issues/638>`_)
+* Contributors: Matthijs van der Burgh, Nick Paul, Steve Golton
+
+0.11.13 (2020-12-08)
+--------------------
+* fix python-future dependency (`#542 <https://github.com/RobotWebTools/rosbridge_suite/issues/542>`_)
+  * fix python-future dependency
+  This was not fixed with fadffa09b1b572e4dc11848c31bb6ebab4a3e95d
+  The dependency was added with `#541 <https://github.com/RobotWebTools/rosbridge_suite/issues/541>`_
+  Python doesn't have build dependencies, only exec dependencies
+  * update ros-tooling/action-ros-ci
+  See:
+  https://github.blog/changelog/2020-10-01-github-actions-deprecating-set-env-and-add-path-commands/
+  This was fixed upstream
+  - https://github.com/ros-tooling/action-ros-ci/issues/429
+  - https://github.com/ros-tooling/action-ros-ci/pull/430
+  * update ros-tooling/setup-ros
+  See:
+  https://github.blog/changelog/2020-10-01-github-actions-deprecating-set-env-and-add-path-commands/
+  This is needed for upstream fix
+  - https://github.com/ros-tooling/setup-ros/pull/303
+* Contributors: Alex Moriarty
+
+0.11.12 (2020-11-25)
+--------------------
+* Fix missing python-future python 2 dep
+* Contributors: Matt Vollrath
+
+0.11.11 (2020-11-24)
+--------------------
+* Use Python 3 str in incoming (`#541 <https://github.com/RobotWebTools/rosbridge_suite/issues/541>`_)
+  Fixes Python 2 unicode issues.
+  Supersedes `#539 <https://github.com/RobotWebTools/rosbridge_suite/issues/539>`_ to fix regression in `#514 <https://github.com/RobotWebTools/rosbridge_suite/issues/514>`_
+* Contributors: Matt Vollrath
+
+0.11.10 (2020-09-08)
+--------------------
+* possible fix for error when working with RosSharp, TypeError: can only concatenate str (not bytes) to str (`#514 <https://github.com/RobotWebTools/rosbridge_suite/issues/514>`_)
+  Co-authored-by: Dmitri <dmitri@dmitri.com>
+* Contributors: Dmitri
+
+0.11.9 (2020-05-27)
+-------------------
+* noetic tests and fixes (`#503 <https://github.com/RobotWebTools/rosbridge_suite/issues/503>`_)
+* Contributors: Matt Vollrath
+
+0.11.8 (2020-05-21)
+-------------------
+
+0.11.7 (2020-05-13)
+-------------------
+* Fix backpressure deadlock (`#496 <https://github.com/RobotWebTools/rosbridge_suite/issues/496>`_)
+  * Don't block Subscription.unregister()
+  * Don't add messages to finished queue handler
+  * Decouple incoming WS handling from server thread
+* Contributors: Matt Vollrath
+
+0.11.6 (2020-04-29)
+-------------------
+
+0.11.5 (2020-04-08)
+-------------------
+* Add script for dockerized development shell (`#479 <https://github.com/RobotWebTools/rosbridge_suite/issues/479>`_)
+  * Add script for dockerized development shell
+  * Fix queue dropping test
+* Subscriber concurrency review (`#478 <https://github.com/RobotWebTools/rosbridge_suite/issues/478>`_)
+  * Lock access to SubscriberManager public methods
+  Prevent subscribe during unsubscribe critical section.
+  * Unsubscribing an unsubscribed topic is an error
+  This branch must not be ignored.
+  * Cleanup some redundant syntax in subscribers impl
+* Fix queue blocking (`#464 <https://github.com/RobotWebTools/rosbridge_suite/issues/464>`_)
+  * Unblock QueueMessageHandler.handle_message
+  The thread was holding the lock while pushing to a potentially blocking
+  function.  Rewrite the logic and use a deque while we're at it.
+  * Add test for subscription queue behavior
+  Guarantee that the queue drops messages when blocked.
+* Python 3 updates/fixes (`#460 <https://github.com/RobotWebTools/rosbridge_suite/issues/460>`_)
+  * lgsvl_rosbridge_library, rosbridge_server: Update package format
+  Add Python3 conditional dependencies where applicable.
+  * lgsvl_rosbridge_library: Fix pngcompression for Python 3
+  * rosapi: Use catkin_install_python for scripts
+* Fixing wrong header/stamp in published ROS-messsages (`#472 <https://github.com/RobotWebTools/rosbridge_suite/issues/472>`_)
+  When publishing a message to ROS (i.e. incoming from rosbridge_server's perspective), timestamps in the Header attributes all point to the same Time object iff the message contains multiple Header attributes (typically the case if a ROS message contains other ROS messages, e.g. ...Array-types) and rosparam use_sim_time is true.
+* Contributors: Alexey Rogachevskiy, Matt Vollrath, danielmaier
+
+0.11.4 (2020-02-20)
+-------------------
+* Concurrency review (`#458 <https://github.com/RobotWebTools/rosbridge_suite/issues/458>`_)
+  * Safer locking in PublisherConsistencyListener
+  * Safer locking in ros_loader
+  * Print QueueMessageHandler exceptions to stderr
+  * Register before resuming outgoing valve
+  * Don't pause a finished socket valve
+* Add cbor-raw compression (`#452 <https://github.com/RobotWebTools/rosbridge_suite/issues/452>`_)
+  The CBOR compression is already a huge win over JSON or PNG encoding,
+  but it’s still suboptimal in some situations. This PR adds support for
+  getting messages in their raw binary (ROS-serialized) format. This has
+  benefits in the following cases:
+  - Your application already knows how to parse messages in bag files
+  (e.g. using [rosbag.js](https://github.com/cruise-automation/rosbag.js),
+  which means that now you can use consistent code paths for both bags
+  and live messages.
+  - You want to parse messages as late as possible, or in parallel, e.g.
+  only in the thread or WebWorker that cares about the message. Delaying
+  the parsing of the message means that moving or copying the message to
+  the thread is cheaper when its in binary form, since no serialization
+  between threads is necessary.
+  - You only care about part of the message, and don't need to parse the
+  rest of it.
+  - You really care about performance; no conversion between the ROS
+  binary format and CBOR is done in the rosbridge_sever.
+* Fix typos in lgsvl_rosbridge_library's description (`#450 <https://github.com/RobotWebTools/rosbridge_suite/issues/450>`_)
+* Python 3 fix for dict::values (`#446 <https://github.com/RobotWebTools/rosbridge_suite/issues/446>`_)
+  Under Python 3, values() returns a view-like object, and because that object is used outside the mutex, we were getting `RuntimeError: dictionary changed size during iteration` under some circumstances. This creates a copy of the values, restoring the Python 2 behaviour and fixing the problem.
+* Contributors: Jan Paul Posma, Matt Vollrath, Mike Purvis, miller-alex
+
+0.11.3 (2019-08-07)
+-------------------
+
+0.11.2 (2019-07-08)
+-------------------
+
+0.11.1 (2019-05-08)
+-------------------
+* fixed logwarn msg formatting in publishers (`#398 <https://github.com/RobotWebTools/rosbridge_suite/issues/398>`_)
+* Contributors: Gautham P Das
+
+0.11.0 (2019-03-29)
+-------------------
+* BSON can send Nan and Inf (`#391 <https://github.com/RobotWebTools/rosbridge_suite/issues/391>`_)
+* Contributors: akira_you
+
+0.10.2 (2019-03-04)
+-------------------
+* Fix typo (`#379 <https://github.com/RobotWebTools/rosbridge_suite/issues/379>`_)
+* Contributors: David Weis
+
+0.10.1 (2018-12-16)
+-------------------
+* Inline cbor library (`#377 <https://github.com/RobotWebTools/rosbridge_suite/issues/377>`_)
+  Prefer system version with C speedups, but include pure Python implementation.
+* Contributors: Matt Vollrath
+
+0.10.0 (2018-12-14)
+-------------------
+* CBOR encoding (`#364 <https://github.com/RobotWebTools/rosbridge_suite/issues/364>`_)
+  * Add CBOR encoding
+  * Fix value extraction performance regression
+  Extract message values once per message.
+  * Fix typed array tags
+  Was using big-endian tags and encoding little-endian.
+  Always use little-endian for now since Intel is prevalent for desktop.
+  Add some comments to this effect.
+  * Update CBOR protocol documentation
+  More information about draft typed arrays and when to use CBOR.
+  * Fix 64-bit integer CBOR packing
+  Use an actual 64-bit format.
+* use package format 2, remove unnecessary dependencies (`#348 <https://github.com/RobotWebTools/rosbridge_suite/issues/348>`_)
+* removing has_key for python3, keeping backwards compatibility (`#337 <https://github.com/RobotWebTools/rosbridge_suite/issues/337>`_)
+  * removing has_key for python3, keeping backwards compatibility
+  * py3 change for itervalues, keeping py2 compatibility
+* Contributors: Andreas Klintberg, Dirk Thomas, Matt Vollrath
+
+0.9.0 (2018-04-09)
+------------------
+* Fix typo in function call
+* Add missing argument to InvalidMessageException (`#323 <https://github.com/RobotWebTools/rosbridge_suite/issues/323>`_)
+  Add missing argument to InvalidMessageException constructor
+* Make unregister_timeout configurable (`#322 <https://github.com/RobotWebTools/rosbridge_suite/issues/322>`_)
+  Pull request `#247 <https://github.com/RobotWebTools/rosbridge_suite/issues/247>`_ introduces a 10 second delay to mitigate issue `#138 <https://github.com/RobotWebTools/rosbridge_suite/issues/138>`_.
+  This change makes this delay configurable by passing an argument either
+  on the command line or when including a launch file.
+  Usage example:
+  ```xml
+  <launch>
+  <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch">
+  <arg name="unregister_timeout" value="5.0"/>
+  </include>
+  </launch>
+  ```
+  Closes `#320 <https://github.com/RobotWebTools/rosbridge_suite/issues/320>`_
+* message_conversion: create stand-alone object inst (`#319 <https://github.com/RobotWebTools/rosbridge_suite/issues/319>`_)
+  Catching the ROSInitException allows to create object
+  instances without an initialized ROS state
+* Fixes `#313 <https://github.com/RobotWebTools/rosbridge_suite/issues/313>`_ by fixing has_binary in protocol.py (`#315 <https://github.com/RobotWebTools/rosbridge_suite/issues/315>`_)
+  * Fixes `#313 <https://github.com/RobotWebTools/rosbridge_suite/issues/313>`_ by fixing has_binary in protocol.py
+  Checks for lists that have binary content as well as dicts
+  * Minor refactoring for protocol.py
+* fix fragment bug (`#316 <https://github.com/RobotWebTools/rosbridge_suite/issues/316>`_)
+  * fix bug that lost data while sending large packets
+  * fixed travis ci failed by @T045T
+  * fixed travis ci failed by @T045T
+  * travis ci failed
+  * fix lgsvl_rosbridge_library/test/experimental/fragmentation+srv+tcp test bug
+  * sync .travis.yaml
+  * fix the service_response message bug
+  * fix the fragment paring error
+  * fix indentation of "service" line
+* add graceful_shutdown() method to advertise_service capability
+  This gives the service a bit of time to cancel any in-flight service requests (which should fix `#265 <https://github.com/RobotWebTools/rosbridge_suite/issues/265>`_).
+  This is important because we busy-wait for a rosbridge response for service calls and those threads do not get stopped otherwise.
+  Also, rospy service clients do not currently support timeouts, so any clients would be stuck too.
+  A new test case in test_service_capabilities.py verifies the fix works
+* Add rostest for service capabilities and fix bugs
+  also fixed some typos
+* Fix Travis config (`#311 <https://github.com/RobotWebTools/rosbridge_suite/issues/311>`_)
+  * fix Travis config
+  dist: kinetic is currently unsupported
+  * fix rostests
+  for some reason, rostest seems to hide the rosout node - changed tests to use other services
+* Contributors: Anwar, Johannes Rothe, Jørgen Borgesen, Nils Berg, Phil, WH-0501, elgarlepp
+
+0.8.6 (2017-12-08)
+------------------
+* Import StringIO from StringIO if python2 and from io if python3 fixes `#306 <https://github.com/RobotWebTools/rosbridge_suite/issues/306>`_ (`#307 <https://github.com/RobotWebTools/rosbridge_suite/issues/307>`_)
+* Contributors: Jihoon Lee
+
+0.8.5 (2017-11-23)
+------------------
+* Raise if inappropriate bson module is installed (Appease `#198 <https://github.com/RobotWebTools/rosbridge_suite/issues/198>`_) (`#270 <https://github.com/RobotWebTools/rosbridge_suite/issues/270>`_)
+  * Raise Exception if inappropriate bson module is installed (Related to `#198 <https://github.com/RobotWebTools/rosbridge_suite/issues/198>`_)
+* Add Python3 compatibility (`#300 <https://github.com/RobotWebTools/rosbridge_suite/issues/300>`_)
+  * First pass at Python 3 compatibility
+  * message_conversion: Only call encode on a Python2 str or bytes type
+  * protocol.py: Changes for dict in Python3. Compatible with Python 2 too.
+  * More Python 3 fixes, all tests pass
+  * Move definition of string_types to lgsvl_rosbridge_library.util
+* Contributors: Junya Hayashi, Kartik Mohta
+
+0.8.4 (2017-10-16)
+------------------
+
+0.8.3 (2017-09-11)
+------------------
+* Type conversion convention correction, correcting issue `#240 <https://github.com/RobotWebTools/rosbridge_suite/issues/240>`_
+* Contributors: Alexis Paques
+
+0.8.2 (2017-09-11)
+------------------
+
+0.8.1 (2017-08-30)
+------------------
+* remove ujson from dependency to build in trusty (`#290 <https://github.com/RobotWebTools/rosbridge_suite/issues/290>`_)
+* Contributors: Jihoon Lee
+
+0.8.0 (2017-08-30)
+------------------
+* Cleaning up travis configuration (`#283 <https://github.com/RobotWebTools/rosbridge_suite/issues/283>`_)
+  configure travis to use industial ci configuration. Now it uses xenial and kinetic
+* Merge pull request `#272 <https://github.com/RobotWebTools/rosbridge_suite/issues/272>`_ from ablakey/patch-1
+  Prevent a KeyError when bson_only_mode is unset.
+* Update protocol.py
+  Prevent a KeyError when bson_only_mode is unset.
+* Merge pull request `#257 <https://github.com/RobotWebTools/rosbridge_suite/issues/257>`_ from Sanic/bson-only-mode
+  Implemented a bson_only_mode flag for the TCP version of rosbridge
+* Merge pull request `#247 <https://github.com/RobotWebTools/rosbridge_suite/issues/247>`_ from v-lopez/develop
+  Delay unregister to mitigate `#138 <https://github.com/RobotWebTools/rosbridge_suite/issues/138>`_
+* Change class constant to module constant
+* Reduce timeout for tests
+  Tests will sleep for 10% extra of the timeout to prevent some situations
+  were the test sleep ended right before the unregister timer fired
+* Fix test advertise errors after delayed unregister changes
+* Fix missing tests due to delayed unregistration
+* Move UNREGISTER_TIMEOUT to member class so it's accessible from outside
+* minor change in variable usage
+* Implemented a bson_only_mode flag for the TCP version of rosbridge; This allows you to switch to a full-duplex transmission of BSON messages and therefore eliminates the need for a base64 encoding of binary data; Use the new mode by starting:'roslaunch rosbridge_server rosbridge_tcp.launch bson_only_mode:=True' or passing '--bson_only_mode' to the rosbridge_tcp.py script
+* Delay unregister to mitigate !138
+* Contributors: Andrew Blakey, Jihoon Lee, Nils Berg, Patrick Mania, Victor Lopez
+
+0.7.17 (2017-01-25)
+-------------------
+* adjust log level for security globs
+  Normal operation (i.e. no globs or successful verification of requests) is now silent, with illegal requests producing a warning.
+* add missing import
+* correct default values for security globs
+  also accept empty list as the default "do not check globs" value in addition to None.
+  Finally, append rosapi service glob after processing command line input so it's not overwritten
+* Added services_glob to CallServices, added globs to rosbridge_tcp and rosbridge_udp, and other miscellanous fixes.
+* As per the suggestions of @T045T, fixed several typos, improved logging, and made some style fixes.
+* Added new parameters for topic and service security.
+  Added 3 new parameters to rosapi and rosbridge_server which filter the
+  topics, services, and parameters broadcast by the server to match an
+  array of glob strings.
+* Contributors: Eric, Nils Berg
+
+0.7.16 (2016-08-15)
+-------------------
+* Fixed deprecated code in pillow
+* Contributors: vladrotea
+
+0.7.15 (2016-04-25)
+-------------------
+* changelog updated
+* Contributors: Russell Toris
+
+0.7.14 (2016-02-11)
+-------------------
+* Another fix for code
+* Replaced += with ''.join() for python code
+* Default Protocol delay_between_messages = 0
+  This prevents performance problems when multiple clients are subscribing to high frequency topics.
+  Fixes `#203 <https://github.com/RobotWebTools/rosbridge_suite/issues/203>`_
+* Contributors: Matt Vollrath, kiloreux
+
+0.7.13 (2015-08-14)
+-------------------
+* Nevermind o_O
+* Add test_depend too (just in case)
+* Add dependency on python bson
+* Get parameter at encode time
+* Add flag for using the bson encoding
+* revert comment regarding unpublisher
+* avoiding racing condition
+* Add bson encoding to the server side
+* Fix catkin_lint issues
+* don't unregister topic from rosbridge. It creates md5 sum warning.. #138
+* Contributors: David Lu, Jihoon Lee, Matt Vollrath, dwlee
+
+0.7.12 (2015-04-07)
+-------------------
+* use <test_depend> for test dependencies
+* use rospy.resolve_name for namespaced service calls
+* fix resolving namespaced service calls
+* Contributors: Ramon Wijnands
+
+0.7.11 (2015-03-23)
+-------------------
+
+0.7.10 (2015-02-25)
+-------------------
+
+0.7.9 (2015-02-24)
+------------------
+
+0.7.8 (2015-01-16)
+------------------
+
+0.7.7 (2015-01-06)
+------------------
+
+0.7.6 (2014-12-26)
+------------------
+* 0.7.5
+* update changelog
+* 0.7.4
+* changelog updated
+* 0.7.3
+* changelog updated
+* 0.7.2
+* changelog updated
+* 0.7.1
+* update changelog
+* 0.7.0
+* changelog updated
+* rewrite of advertise service
+* cleanup init function
+* matches original call_service
+* matches original call_service
+* service_request --> reuse of call_service (previously defined)
+* stop_service --> unadvertise_service
+* service_name --> service
+* service_type --> type
+* removed service_module
+* request_id --> id
+* Contributors: Jihoon Lee, Russell Toris
+
+0.7.5 (2014-12-26)
+------------------
+
+0.7.4 (2014-12-16)
+------------------
+
+0.7.3 (2014-12-15)
+------------------
+
+0.7.2 (2014-12-15)
+------------------
+* 0.7.1
+* update changelog
+* Contributors: Jihoon Lee
+
+0.7.1 (2014-12-09)
+------------------
+
+0.7.0 (2014-12-02)
+------------------
+* rewrite of advertise service
+* cleanup init function
+* matches original call_service
+* matches original call_service
+* service_request --> reuse of call_service (previously defined)
+* stop_service --> unadvertise_service
+* service_name --> service
+* service_type --> type
+* removed service_module
+* request_id --> id
+* Contributors: Russell Toris
+
+0.6.8 (2014-11-05)
+------------------
+* add a lock to calls to load_manifest - apparently, it's not thread safe
+  fixes #103 and #108
+* Contributors: Nils Berg
+
+0.6.7 (2014-10-22)
+------------------
+* updated package manifests
+* Contributors: Russell Toris
+
+0.6.6 (2014-10-21)
+------------------
+
+0.6.5 (2014-10-14)
+------------------
+* 0.6.4
+* update changelog
+* modify tests
+  less duplicated code, some other changes to (hopefully) improve reliability. Tested locally about 30 times without encountering any failures.
+* Change the behavior of MessageHandler.transition()
+  Now reflects usage in the tests, i.e. a QueueMessageHandler only needs queue_length to be defined, not throttle_rate.
+* 0.6.3
+* update change log
+* install util python module to fix #128
+* Contributors: Jihoon Lee, Nils Berg
+
+0.6.4 (2014-10-08)
+------------------
+
+0.6.3 (2014-10-07)
+------------------
+* install util python module to fix `#128 <https://github.com/RobotWebTools/rosbridge_suite/issues/128>`_
+* Contributors: Jihoon Lee
+
+0.6.2 (2014-10-06)
+------------------
+* Remove unused json imports; move json imports to utility
+  Fixes #7
+* Contributors: Graeme Yeates
+
+0.6.1 (2014-09-01)
+------------------
+* Handle float infinity and NAN s
+* Windows-related fix for PIL Image module import
+* Fixed typo in raising type errors.
+* something messed up indentation
+  not sure how that could happen, worked here.
+* map Inf and NaN to null
+  JSON does not support Inf and NaN values. Currently they are just written into the JSON and JSON.parse on the client side will fail. Correct is to map them to null which will then be parsed correctly by JSON.parse on the client side.
+  The issue with that is that the shortcut for lists of floats might be impossible (maybe someone else with more experience in python comes up with something else?). Maybe something similar is necessary in the to_inst case, but I can not really test them.
+  Real world application is to process laser scans, they contain inf and nan values for some drivers if the measurements are invalid or out of range.
+* Update .travis.yml and package.xml for lgsvl_rosbridge_library tests
+* Put back unregister for the publisher and clarify the reconnect behavior
+  of the test case. The exponential backoff of the client causes hard to
+  understand timing of the events.
+  All specs passed locally on hydro:
+  SUMMARY
+  * RESULT: SUCCESS
+  * TESTS: 103
+  * ERRORS: 0
+  * FAILURES: 0
+* Add copyright notice to the file
+* Remove extra whitespace
+* Make the test more deterministic
+* Remove circular dependency.
+* Contributors: Achim Konigs, Alex Sorokin, Alexander Sorokin, Jonathan Wade, jon-weisz
+
+0.6.0 (2014-05-23)
+------------------
+* Ensure that service name is a string
+  Closes `#104 <https://github.com/RobotWebTools/rosbridge_suite/issues/104>`_
+* Contributors: Piyush Khandelwal
+
+0.5.4 (2014-04-17)
+------------------
+* removing wrong import
+* test case for fixed size of uint8 array
+* uses regular expresion to match uint8 array and char array.
+* logerr when it fails while message_conversion
+* Contributors: Jihoon Lee
+
+0.5.3 (2014-03-28)
+------------------
+* use queue_size for publishers
+* Contributors: Jon Binney
+
+0.5.2 (2014-03-14)
+------------------
+* First attempt adding latching support for topic publishers
+* merging changes of groovy-devel into hydro-devel
+* adding missing dependency in lgsvl_rosbridge_library `#70 <https://github.com/RobotWebTools/rosbridge_suite/issues/70>`_
+* Fixed wrong unicode encoding
+* support publishing non-ascii letters
+* Added error message on result=False
+  When call_service returns False as result, values contains the error message.
+* added parameter lookup to rosbridge_tcp.py, modules where those are used, and default parameters to launch file; internal default-values still get used when launch-file does not provide them; internal defaults can be changed within rosbridge_tcp.py
+* Merge branch 'experimental_branch' into new_features
+* fix handling of partial/multiple/broken json by avoiding to pass nested json (without op-field) to rosbridge.. probably still needs more complex handling of incoming 'broken' json
+* nested service not MiRoR related anymore
+* added singleton for request-list; allows provider to send service response without specifying module and type, they get looked up when response is received via request_id
+* fix for nested service responses - use ros_loader and message_conversion for populating an according instance
+* use message_conversion in handle_servie_request
+* snapshot for branch to show to genpy devs
+* using float64 instead of std_msgs/Float64 lets scripts run fine.. ; next: fix with using std_msgs/Float64 --> need nested data field
+* nested srv uses now message_conversion.extract_values
+* adapted test scripted to ros_loader; (removed .srv from module_name
+* use rosloader for finding service_class
+* fixed calculation of fragment_count
+* cleanup: files, notes, some code
+* added message_field <message_intervall> to allow client to control delay between messages from rosbridge
+* added TODO: check if service successfully registered in ros
+* ..
+* ..
+* added description of new opcodes
+* tests, comments, description, ..
+* tested rosbridge_websocket with new capabilities; websocket test scripts not working yet..; but new caps are working when using rosbridge_websocket and tcp2ws wrapper --> so only testscripts need to be fixed for websockets.
+* updated websocket test service server and client script to use websocket
+* updated websocket test service server script to use websocket
+* added files to test new caps with websocket server
+* feierabend.. morgen weiter mit server & client JSON-decoder, see notes
+* fixed parsing of incomplete/multiple JSON in incoming buffer; so clients do not need to use an intervall when sending to rosbridge
+* only current changes; not yet done..
+* code cleanup, not yet finished..; rosbridge logging much cleaner now
+* fixed test_server_defragment - recodegit status
+* minor
+* linuxonandroid
+* fixed some parts; ..still better do some redesign for queueing of messages..
+* forced tcp_send to use queue and use delay between sends
+* blocking behavior for service requests to non-ros; test-scripts use get-ip4 helper function; ..needs a lot cleanup before next steps..
+* need to implement server side blocking of multiple requests, to keep implementation of service provider as easy and simple as possible
+* not finished
+* some changes.. still needs serveral fixes
+* unique request_ids
+* fixed deserialization of multiple fragments in incoming-data; was caused by too short delay between socket-sends (<0.2 seconds); maybe only temp. fixed
+* added fragment sorting to test-client and test-server
+* message_size debugging; TODO: sort list of received fragments! ; make sure receive_buffers are big enough for fragment_size + header..
+* minor changes
+* testing: service server fragmentsizes receive: 1  send: 1; client fragmentsize receive: 1; is working..
+* fixed an error that caused service_response to appear quoted as string once too often; should be ok now
+* fragmentation basically working; service_server can request fragmented service_calls, service_client can request fragmented responses; fragmentation can be requested by adding fragmentation_size parameter to any message sent to rosbridge
+* some code cleanup
+* set service_request_timeout back to 60 seconds; had 2s from timeout_tests..
+* fixed example: non-ros_service_server.py to use only 1 socket; commented and structured code and comments in test-scripts
+* some minor changes: comments, debug-output, ..
+* added test script for non-ros_service_client calling service from non-ros_service_server
+* added msg and srv files
+* fixed (removed) dependency to beginner_tutorials for service_server test-scripts. beginner_tutorials package not needed anymore.
+* behaviour on advertising existing service: replace service-provider, similar to ROS-groovy behaviour, see issues..
+* behaviour on advertising existing service: replace service-provider, similar to ROS-groovy behaviour, see issues..
+* removed obsolete test-scripts
+* stop service added
+* first working classes: service_server
+* should use its own branch: service_server.py;  add initial thoughts and code-base for developing ServiceServer capability
+* fixed errors in protocol.py and defragmentation.py
+* added test-scripts for defragmentation AND tcp-server
+* change json imports to try to use ujson or simplejson
+* change json imports to try to use ujson or simplejson; correct log_message to show length of content/data instead of overall length
+* fixed variable name in finish()
+* Clean up of defragmentation.py.
+* add defragmentation capability
+* merge with fuerte-devel
+* add defragmentation capability
+* commented out that problematic unregister line
+* Contributors: Brandon Alexander, Jihoon Lee, Julian Cerruti, Kaijen Hsiao, Stefan Profanter, dave, furushchev, fxm-db, ipa-fxm, root, unknown
+
+0.5.1 (2013-10-31)
+------------------
+* Implement multiple subscriptions to latched topics (fixes `#1 <https://github.com/RobotWebTools/rosbridge_suite/issues/1>`_).
+* generate more natural json for service call result
+* add result field to service response
+* Contributors: Siegfried-A. Gevatter Pujals, Takashi Ogura
+
+0.5.0 (2013-07-17)
+------------------
+* 0.5.0 preparation for hydro release
+* even more missing depends for unit tests
+* more missing test packages
+* missing depends added when running tests
+* rostest now uses devel instead of install
+* rostest added to package
+* Contributors: Jihoon Lee, Russell Toris
+
+0.4.4 (2013-04-08)
+------------------
+
+0.4.3 (2013-04-03 08:24)
+------------------------
+
+0.4.2 (2013-04-03 08:12)
+------------------------
+* eclipse projects removed
+* Contributors: Russell Toris
+
+0.4.1 (2013-03-07)
+------------------
+* adding message generation build dependency
+* Contributors: Jihoon Lee
+
+0.4.0 (2013-03-05)
+------------------
+* removing rostest
+* Commenting out rostest
+* Update lgsvl_rosbridge_library/package.xml
+  removed <test_depend>rospy</test_depend>
+* Fixes "'int' is not iterable" bug.
+* Adds test_all.test launch file.
+* Error fix from wrong package name.
+* Moves test package tests into lgsvl_rosbridge_library.
+  I learned about NOINSTALL for msg and srv generation in CMakeList.
+* Resolves submodule issues.
+* Uses only 1 .gitignore to avoid confusion.
+* Merge pull request `#15 <https://github.com/RobotWebTools/rosbridge_suite/issues/15>`_ from baalexander/remove_unregister
+  Removes buggy unregister call.
+* Removes buggy unregister call.
+  Fixes Issue `#12 <https://github.com/RobotWebTools/rosbridge_suite/issues/12>`_.
+* Adds BSD license header to code files.
+  See Issue `#13 <https://github.com/RobotWebTools/rosbridge_suite/issues/13>`_.
+* Removing ultrajson from rosbridge.
+  If JSON parsing becomes a performance bottle neck, we can readd it.
+* Catkinizing lgsvl_rosbridge_library and server.
+* PNG compression now creates a square RGB image padded with new-line characters
+* Add stack dependencies and rosdeps.
+* Collapse directory structure.
+* Moved the packages inside a folder called rosbridge
+* Initial commit of lgsvl_rosbridge_library
+* Contributors: Austin Hendrix, Brandon Alexander, David Gossow, Jihoon Lee, Jonathan Mace, Russell Toris
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/CMakeLists.txt b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/CMakeLists.txt
new file mode 100644
index 00000000..1b02cd72
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/CMakeLists.txt
@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(lgsvl_rosbridge_library)
+
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msgs)
+
+# Generate messages for testing (NOINSTALL)
+add_message_files(
+  FILES
+  Num.msg
+  TestChar.msg
+  TestDurationArray.msg
+  TestHeaderArray.msg
+  TestHeader.msg
+  TestHeaderTwo.msg
+  TestTimeArray.msg
+  TestUInt8.msg
+  TestUInt8FixedSizeArray16.msg
+  NOINSTALL
+)
+
+# Generate services for testing (NOINSTALL)
+add_service_files(
+  FILES
+  AddTwoInts.srv
+  SendBytes.srv
+  TestArrayRequest.srv
+  TestEmpty.srv
+  TestMultipleRequestFields.srv
+  TestMultipleResponseFields.srv
+  TestNestedService.srv
+  TestRequestAndResponse.srv
+  TestRequestOnly.srv
+  TestResponseOnly.srv
+  NOINSTALL
+)
+
+catkin_python_setup()
+
+# Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
+)
+
+# Test launch files
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/capabilities/test_capabilities.test)
+  add_rostest(test/internal/test_internal.test)
+endif()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/Num.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/Num.msg
new file mode 100644
index 00000000..98069819
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/Num.msg
@@ -0,0 +1 @@
+int64 num
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestChar.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestChar.msg
new file mode 100644
index 00000000..7aa8eba7
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestChar.msg
@@ -0,0 +1 @@
+char[] data
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestDurationArray.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestDurationArray.msg
new file mode 100644
index 00000000..1731628d
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestDurationArray.msg
@@ -0,0 +1 @@
+duration[] durations
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeader.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeader.msg
new file mode 100644
index 00000000..54c29d3f
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeader.msg
@@ -0,0 +1 @@
+std_msgs/Header header
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeaderArray.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeaderArray.msg
new file mode 100644
index 00000000..d2d1845d
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeaderArray.msg
@@ -0,0 +1 @@
+std_msgs/Header[] header
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeaderTwo.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeaderTwo.msg
new file mode 100644
index 00000000..1f83c0ba
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestHeaderTwo.msg
@@ -0,0 +1 @@
+Header header
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestTimeArray.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestTimeArray.msg
new file mode 100644
index 00000000..ad05ecbf
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestTimeArray.msg
@@ -0,0 +1 @@
+time[] times
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestUInt8.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestUInt8.msg
new file mode 100644
index 00000000..17350525
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestUInt8.msg
@@ -0,0 +1 @@
+uint8[] data
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestUInt8FixedSizeArray16.msg b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestUInt8FixedSizeArray16.msg
new file mode 100644
index 00000000..637d12d9
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/msg/TestUInt8FixedSizeArray16.msg
@@ -0,0 +1 @@
+uint8[16] data
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/package.xml b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/package.xml
new file mode 100644
index 00000000..19b48557
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/package.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<?xml-model
+  href="http://download.ros.org/schema/package_format3.xsd"
+  schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lgsvl_rosbridge_library</name>
+  <version>0.11.16</version>
+  <description>
+    The core rosbridge package, responsible for interpreting JSON and performing
+    the appropriate ROS action, like subscribe, publish, call service, and
+    interact with params.
+  </description>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/lgsvl_rosbridge_library</url>
+  <url type="bugtracker">https://github.com/RobotWebTools/rosbridge_suite/issues</url>
+  <url type="repository">https://github.com/RobotWebTools/rosbridge_suite</url>
+
+  <author email="jonathan.c.mace@gmail.com">Jonathan Mace</author>
+  <maintainer email="rctoris@wpi.edu">Russell Toris</maintainer>
+  <maintainer email="jihoonlee.in@gmail.com">Jihoon Lee</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
+  <buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 2">python-future</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 2">python-imaging</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 3">python3-pil</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 2">python-bson</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 3">python3-bson</build_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rosgraph</exec_depend>
+  <exec_depend>rosservice</exec_depend>
+  <exec_depend>rostopic</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-future</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-imaging</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-pil</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-bson</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-bson</exec_depend>
+
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+  <test_depend>actionlib_msgs</test_depend>
+  <test_depend>diagnostic_msgs</test_depend>
+  <test_depend>nav_msgs</test_depend>
+  <test_depend>rospy_tutorials</test_depend>
+  <test_depend>sensor_msgs</test_depend>
+  <test_depend>std_srvs</test_depend>
+  <test_depend>stereo_msgs</test_depend>
+  <test_depend>tf2_msgs</test_depend>
+  <test_depend>trajectory_msgs</test_depend>
+  <test_depend>visualization_msgs</test_depend>
+</package>
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/setup.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/setup.py
new file mode 100644
index 00000000..94e95929
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/setup.py
@@ -0,0 +1,9 @@
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['lgsvl_rosbridge_library', 'lgsvl_rosbridge_library.internal', 'lgsvl_rosbridge_library.capabilities', 'lgsvl_rosbridge_library.util'],
+    package_dir={'' : 'src'},
+)
+
+setup(**d)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/advertise.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/advertise.py
new file mode 100644
index 00000000..ad68b1e8
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/advertise.py
@@ -0,0 +1,164 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# Copyright (c) 2014, Creativa 77 SRL
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import fnmatch
+from lgsvl_rosbridge_library.capability import Capability
+from lgsvl_rosbridge_library.internal.publishers import manager
+from lgsvl_rosbridge_library.util import string_types
+
+
+class Registration():
+    """ Keeps track of how many times a client has requested to advertise
+    a publisher.
+
+    A client could advertise and unadvertise a topic multiple times, and we
+    must make sure that the underlying publisher is only created and destroyed
+    at the appropriate moments
+
+    """
+
+    def __init__(self, client_id, topic):
+        # Initialise variables
+        self.client_id = client_id
+        self.topic = topic
+        self.clients = {}
+
+    def unregister(self):
+        manager.unregister(self.client_id, self.topic)
+
+    def register_advertisement(self, msg_type, adv_id=None, latch=False, queue_size=100):
+        # Register with the publisher manager, propagating any exception
+        manager.register(self.client_id, self.topic, msg_type, latch=latch, queue_size=queue_size)
+
+        self.clients[adv_id] = True
+
+    def unregister_advertisement(self, adv_id=None):
+        if adv_id is None:
+            self.clients.clear()
+        elif adv_id in self.clients:
+            del self.clients[adv_id]
+
+    def is_empty(self):
+        return len(self.clients) == 0
+
+
+class Advertise(Capability):
+
+    advertise_msg_fields = [(True, "topic", string_types), (True, "type", string_types)]
+    unadvertise_msg_fields = [(True, "topic", string_types)]
+
+    topics_glob = None
+
+    def __init__(self, protocol):
+        # Call superclas constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("advertise", self.advertise)
+        protocol.register_operation("unadvertise", self.unadvertise)
+
+        # Initialize class variables
+        self._registrations = {}
+
+        if protocol.parameters and "unregister_timeout" in protocol.parameters:
+            manager.unregister_timeout = protocol.parameters.get("unregister_timeout")
+
+    def advertise(self, message):
+        # Pull out the ID
+        aid = message.get("id", None)
+
+        self.basic_type_check(message, self.advertise_msg_fields)
+        topic = message["topic"]
+        msg_type = message["type"]
+        latch = message.get("latch", False)
+        queue_size = message.get("queue_size", 100)
+
+        if Advertise.topics_glob is not None and Advertise.topics_glob:
+            self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic)
+            match = False
+            for glob in Advertise.topics_glob:
+                if (fnmatch.fnmatch(topic, glob)):
+                    self.protocol.log("debug", "Found match with glob " + glob + ", continuing advertisement...")
+                    match = True
+                    break
+            if not match:
+                self.protocol.log("warn", "No match found for topic, cancelling advertisement of: " + topic)
+                return
+        else:
+            self.protocol.log("debug", "No topic security glob, not checking advertisement.")
+
+        # Create the Registration if one doesn't yet exist
+        if not topic in self._registrations:
+            client_id = self.protocol.client_id
+            self._registrations[topic] = Registration(client_id, topic)
+
+        # Register, propagating any exceptions
+        self._registrations[topic].register_advertisement(msg_type, aid, latch, queue_size)
+
+    def unadvertise(self, message):
+        # Pull out the ID
+        aid = message.get("id", None)
+
+        self.basic_type_check(message, self.unadvertise_msg_fields)
+        topic = message["topic"]
+
+        if Advertise.topics_glob is not None and Advertise.topics_glob:
+            self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic)
+            match = False
+            for glob in Advertise.topics_glob:
+                if (fnmatch.fnmatch(topic, glob)):
+                    self.protocol.log("debug", "Found match with glob " + glob + ", continuing unadvertisement...")
+                    match = True
+                    break
+            if not match:
+                self.protocol.log("warn", "No match found for topic, cancelling unadvertisement of: " + topic)
+                return
+        else:
+            self.protocol.log("debug", "No topic security glob, not checking unadvertisement.")
+
+        # Now unadvertise the topic
+        if topic not in self._registrations:
+            return
+        self._registrations[topic].unregister_advertisement(aid)
+
+        # Check if the registration is now finished with
+        if self._registrations[topic].is_empty():
+            self._registrations[topic].unregister()
+            del self._registrations[topic]
+
+    def finish(self):
+        for registration in self._registrations.values():
+            registration.unregister()
+        self._registrations.clear()
+        self.protocol.unregister_operation("advertise")
+        self.protocol.unregister_operation("unadvertise")
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/advertise_service.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/advertise_service.py
new file mode 100644
index 00000000..02bb8b0b
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/advertise_service.py
@@ -0,0 +1,125 @@
+import fnmatch
+from threading import Lock
+import time
+
+from lgsvl_rosbridge_library.internal.ros_loader import get_service_class
+from lgsvl_rosbridge_library.internal import message_conversion
+from lgsvl_rosbridge_library.capability import Capability
+from lgsvl_rosbridge_library.util import string_types
+
+import rospy
+
+class AdvertisedServiceHandler():
+
+    id_counter = 1
+    responses = {}
+
+    def __init__(self, service_name, service_type, protocol):
+        self.active_requests = 0
+        self.shutdown_requested = False
+        self.lock = Lock()
+        self.service_name = service_name
+        self.service_type = service_type
+        self.protocol = protocol
+        # setup the service
+        self.service_handle = rospy.Service(service_name, get_service_class(service_type), self.handle_request)
+
+    def next_id(self):
+        id = self.id_counter
+        self.id_counter += 1
+        return id
+
+    def handle_request(self, req):
+        with self.lock:
+            self.active_requests += 1
+        # generate a unique ID
+        request_id = "service_request:" + self.service_name + ":" + str(self.next_id())
+
+        # build a request to send to the external client
+        request_message = {
+            "op": "call_service",
+            "id": request_id,
+            "service": self.service_name,
+            "args": message_conversion.extract_values(req)
+        }
+        self.protocol.send(request_message)
+
+        # wait for a response
+        while request_id not in self.responses.keys():
+            with self.lock:
+                if self.shutdown_requested:
+                    break
+            time.sleep(0)
+
+        with self.lock:
+            self.active_requests -= 1
+
+            if self.shutdown_requested:
+                self.protocol.log(
+                    "warning",
+                    "Service %s was unadvertised with a service call in progress, "
+                    "aborting service call with request ID %s" % (self.service_name, request_id))
+                return None
+
+        resp = self.responses[request_id]
+        del self.responses[request_id]
+        return resp
+
+    def graceful_shutdown(self, timeout):
+        """
+        Signal the AdvertisedServiceHandler to shutdown
+
+        Using this, rather than just rospy.Service.shutdown(), allows us
+        time to stop any active service requests, ending their busy wait
+        loops.
+        """
+        with self.lock:
+            self.shutdown_requested = True
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            time.sleep(0)
+
+class AdvertiseService(Capability):
+    services_glob = None
+
+    advertise_service_msg_fields = [(True, "service", string_types), (True, "type", string_types)]
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("advertise_service", self.advertise_service)
+
+    def advertise_service(self, message):
+        # Typecheck the args
+        self.basic_type_check(message, self.advertise_service_msg_fields)
+
+        # parse the incoming message
+        service_name = message["service"]
+
+        if AdvertiseService.services_glob is not None and AdvertiseService.services_glob:
+            self.protocol.log("debug", "Service security glob enabled, checking service: " + service_name)
+            match = False
+            for glob in AdvertiseService.services_glob:
+                if (fnmatch.fnmatch(service_name, glob)):
+                    self.protocol.log("debug", "Found match with glob " + glob + ", continuing service advertisement...")
+                    match = True
+                    break
+            if not match:
+                self.protocol.log("warn", "No match found for service, cancelling service advertisement for: " + service_name)
+                return
+        else:
+            self.protocol.log("debug", "No service security glob, not checking service advertisement.")
+
+        # check for an existing entry
+        if service_name in self.protocol.external_service_list.keys():
+            self.protocol.log("warn", "Duplicate service advertised. Overwriting %s." % service_name)
+            self.protocol.external_service_list[service_name].service_handle.shutdown("Duplicate advertiser.")
+            del self.protocol.external_service_list[service_name]
+
+        # setup and store the service information
+        service_type = message["type"]
+        service_handler = AdvertisedServiceHandler(service_name, service_type, self.protocol)
+        self.protocol.external_service_list[service_name] = service_handler
+        self.protocol.log("info", "Advertised service %s." % service_name)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/call_service.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/call_service.py
new file mode 100644
index 00000000..ab6cbccf
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/call_service.py
@@ -0,0 +1,129 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import fnmatch
+from functools import partial
+from lgsvl_rosbridge_library.capability import Capability
+from lgsvl_rosbridge_library.internal.services import ServiceCaller
+from lgsvl_rosbridge_library.util import string_types
+
+
+class CallService(Capability):
+
+    call_service_msg_fields = [(True, "service", string_types),
+                               (False, "fragment_size", (int, type(None))),
+                               (False, "compression", string_types)]
+
+    services_glob = None
+
+    def __init__(self, protocol):
+        # Call superclas constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("call_service", self.call_service)
+
+    def call_service(self, message):
+        # Pull out the ID
+        cid = message.get("id", None)
+
+        # Typecheck the args
+        self.basic_type_check(message, self.call_service_msg_fields)
+
+        # Extract the args
+        service = message["service"]
+        fragment_size = message.get("fragment_size", None)
+        compression = message.get("compression", "none")
+        args = message.get("args", [])
+
+        if CallService.services_glob is not None and CallService.services_glob:
+            self.protocol.log("debug", "Service security glob enabled, checking service: " + service)
+            match = False
+            for glob in CallService.services_glob:
+                if (fnmatch.fnmatch(service, glob)):
+                    self.protocol.log("debug", "Found match with glob " + glob + ", continuing service call...")
+                    match = True
+                    break
+            if not match:
+                self.protocol.log("warn", "No match found for service, cancelling service call for: " + service)
+                return
+        else:
+            self.protocol.log("debug", "No service security glob, not checking service call.")
+
+        # Check for deprecated service ID, eg. /rosbridge/topics#33
+        cid = extract_id(service, cid)
+
+        # Create the callbacks
+        s_cb = partial(self._success, cid, service, fragment_size, compression)
+        e_cb = partial(self._failure, cid, service)
+
+        # Kick off the service caller thread
+        ServiceCaller(trim_servicename(service), args, s_cb, e_cb).start()
+
+    def _success(self, cid, service, fragment_size, compression, message):
+        outgoing_message = {
+            "op": "service_response",
+            "service": service,
+            "values": message,
+            "result": True
+        }
+        if cid is not None:
+            outgoing_message["id"] = cid
+        # TODO: fragmentation, compression
+        self.protocol.send(outgoing_message)
+
+    def _failure(self, cid, service, exc):
+        self.protocol.log("error", "call_service %s: %s" %
+                          (type(exc).__name__, str(exc)), cid)
+        # send response with result: false
+        outgoing_message = {
+            "op": "service_response",
+            "service": service,
+            "values": str(exc),
+            "result": False
+        }
+        if cid is not None:
+            outgoing_message["id"] = cid
+        self.protocol.send(outgoing_message)
+
+
+def trim_servicename(service):
+    if '#' in service:
+        return service[:service.find('#')]
+    return service
+
+
+def extract_id(service, cid):
+    if cid is not None:
+        return cid
+    elif '#' in service:
+        return service[service.find('#') + 1:]
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/defragmentation.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/defragmentation.py
new file mode 100644
index 00000000..2fea6478
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/defragmentation.py
@@ -0,0 +1,198 @@
+from lgsvl_rosbridge_library.capability import Capability
+from datetime import datetime
+import threading
+
+class ReceivedFragments():
+    """
+    Singleton class to hold lists of received fragments in one 'global' object
+    """
+    class __impl:
+        """ Implementation of the singleton interface """
+        def spam(self):
+            """ Test method, return singleton id """
+            return id(self)
+
+    __instance = None
+    # List of defragmentation instances
+    # Format:
+    # {
+    #   <<message1_ID>> : {
+    #     "timestamp_last_append" : <<datetime-object>>,
+    #     "total" : <<total_fragments>>,
+    #     "fragment_list" : {
+    #       <<fragment1ID>>: <<fragment1_data>>,
+    #       <<fragment2ID>>: <<fragment2_data>>,
+    #       ...
+    #     }
+    # },
+    # ...
+    lists = {}
+
+    def __init__(self):
+        """ Create singleton instance """
+        if ReceivedFragments.__instance is None:
+            ReceivedFragments.__instance = ReceivedFragments.__impl()
+            self.lists = {}
+
+        self.__dict__['_ReceivedFragments__instance'] = ReceivedFragments.__instance
+
+    def __getattr__(self, attr):
+        """ Delegate access to implementation """
+        return getattr(self.__instance, attr)
+
+    def __setattr__(self, attr, value):
+        """ Delegate access to implementation """
+        return setattr(self.__instance, attr, value)
+
+class Defragment(Capability, threading.Thread):
+
+    fragment_timeout = 600
+    opcode = "fragment"
+    global received_fragments
+
+    protocol = None
+
+    def __init__(self, protocol):
+        Capability.__init__(self, protocol)
+
+        self.protocol = protocol
+
+        # populate parameters
+        if self.protocol.parameters != None:
+            self.fragment_timeout = self.protocol.parameters["fragment_timeout"]
+
+        protocol.register_operation(self.opcode, self.defragment)
+
+        self.received_fragments = ReceivedFragments().lists
+        threading.Thread.__init__(self)
+
+
+    # defragment() does:
+    #   1) take any incoming message with op-code "fragment"
+    #   2) check all existing fragment lists for time out                       # could be done by a thread but should be okay this way:
+    #   2.a) remove timed out lists (only if new fragment is not for this list) #   - checking whenever a new fragment is received should suffice
+    #   3) create a new fragment list for new message ids                       #     to have control over growth of fragment lists
+    #   3.a) check message fields
+    #   3.b) append the new fragment to 'the' list
+    #   3.c) add time stamp (last_fragment_appended) to 'this' list
+    #   4) check if the list of current fragment (message id) is complete
+    #   4.a) reconstruct the original message by concatenating the fragments
+    #   4.b) pass the reconstructed message string to protocol.incoming()       # protocol.incoming is checking message fields by itself, so no need to do this before passing the reconstructed message to protocol
+    #   4.c) remove the fragment list to free up memory                        
+    def defragment(self, message):
+        now = datetime.now()
+
+        if self.received_fragments != None:
+            for id in self.received_fragments.keys() :
+                time_diff = now - self.received_fragments[id]["timestamp_last_append"]
+                if (time_diff.total_seconds() > self.fragment_timeout and
+                    not self.received_fragments[id]["is_reconstructing"]):
+                    log_msg = ["fragment list ", str(id), " timed out.."]
+
+                    if message["id"] != id:
+                        log_msg.append(" -> removing it..")
+                        del self.received_fragments[id]
+                    else:
+                        log_msg.extend([" -> but we're just about to add fragment #"])
+                        log_msg.extend([str(message.get("num")), " of "])
+                        log_msg.extend([str(self.received_fragments[message.get("id")]["total"])])
+                        log_msg.extend([" ..keeping the list"])
+                    self.protocol.log("warning", ''.join(log_msg))
+
+        msg_opcode = message.get("op")
+        msg_id = message.get("id")
+        msg_num = message.get("num")
+        msg_total = message.get("total")
+        msg_data = message.get("data")
+
+        # Abort if any message field is missing
+        if ((msg_opcode == None) or (msg_id == None) or
+            (msg_num == None) or (msg_total == None) or
+            (msg_data == None)):
+            self.protocol.log("error", "received invalid fragment!")
+            return
+
+        log_msg = "fragment for messageID: " + str(msg_id) + " received."
+        self.protocol.log("debug", log_msg)
+
+        # Create fragment container if none exists yet
+        if msg_id not in self.received_fragments.keys():
+            self.received_fragments[msg_id] = {
+                "is_reconstructing": False,
+                "total": message["total"],
+                "timestamp_last_append": now,
+                "fragment_list": {}
+            }
+            log_msg = "opened new fragment list for messageID " + str(msg_id)
+            self.protocol.log("debug", log_msg)
+
+        #print "received fragments:", len(self.received_fragments[msg_id]["fragment_list"].keys())
+
+        # Add fragment to fragment container's list if not already in list
+        if ((msg_num not in self.received_fragments[msg_id]["fragment_list"].keys() ) and
+             msg_num <= self.received_fragments[msg_id]["total"] and
+             msg_total == self.received_fragments[msg_id]["total"]
+            ):
+            self.received_fragments[msg_id]["fragment_list"][msg_num] = msg_data
+            self.received_fragments[msg_id]["timestamp_last_append"] = now
+            log_msg = ["appended fragment #" + str(msg_num)]
+            log_msg.extend([" (total: ", str(msg_total), ") to fragment list for messageID ", str(msg_id)])
+            self.protocol.log("debug", ''.join(log_msg))
+        else:
+            log_msg = "error while trying to append fragment " + str(msg_num)
+            self.protocol.log("error", log_msg)
+            return
+
+        received_all_fragments = False
+        existing_fragments = len(self.received_fragments[msg_id]["fragment_list"])
+        announced_total = self.received_fragments[msg_id]["total"]
+
+        # Make sure total number of fragments received
+        if existing_fragments == announced_total:
+            log_msg = ["enough/all fragments for messageID " + str(msg_id) + " received"]
+            log_msg.extend([" [", str(existing_fragments), "]"])
+            log_msg = ''.join(log_msg)
+            self.protocol.log("debug", log_msg)
+            # Check each fragment matches up
+            received_all_fragments = True
+            for i in range(0, announced_total):
+                if i not in self.received_fragments[msg_id]["fragment_list"]:
+                    received_all_fragments = False
+                    log_msg = "fragment #" +str(i)
+                    log_msg += " for messageID " + str(msg_id) + " is missing! "
+                    self.protocol.log("error", log_msg)
+
+        self.received_fragments[msg_id]["is_reconstructing"] = received_all_fragments
+
+        if received_all_fragments:
+            log_msg = "reconstructing original message " + str(msg_id)
+            self.protocol.log("debug", log_msg)
+
+            # Reconstruct the message
+            reconstructed_msg = ''.join(self.received_fragments[msg_id]["fragment_list"].values())
+
+            log_msg = ["reconstructed original message:\n"]
+            log_msg.append(reconstructed_msg)
+            log_msg = ''.join(log_msg)
+            self.protocol.log("debug", log_msg)
+
+            duration = datetime.now() - now
+
+            # Pass the reconstructed message to rosbridge
+            self.protocol.incoming(reconstructed_msg)
+            log_msg = ["reconstructed message (ID:" + str(msg_id) + ") from "]
+            log_msg.extend([str(msg_total), " fragments. "])
+            # cannot access msg.data if message is a service_response or else!
+            #log_msg += "[message length: " + str(len(str(json.loads(reconstructed_msg)["msg"]["data"]))) +"]"
+            log_msg.extend(["[duration: ", str(duration.total_seconds()),  " s]"])
+            log_msg = ''.join(log_msg)
+            self.protocol.log("info", log_msg)
+
+            # Remove fragmentation container
+            del self.received_fragments[msg_id]
+            log_msg = "removed fragment list for messageID " + str(msg_id)
+            self.protocol.log("debug", log_msg)
+
+    def finish(self):
+        self.received_fragments = None
+        self.protocol.unregister_operation("fragment")
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/fragmentation.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/fragmentation.py
new file mode 100644
index 00000000..3f9b46f0
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/fragmentation.py
@@ -0,0 +1,106 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from lgsvl_rosbridge_library.capability import Capability
+import math
+
+
+class Fragmentation(Capability):
+    """ The Fragmentation capability doesn't define any incoming operation
+    handlers, but provides methods to fragment outgoing messages """
+    
+    fragmentation_seed = 0
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+    def fragment(self, message, fragment_size, mid=None):
+        """ Serializes the provided message, then splits the serialized
+        message according to fragment_size, then sends the fragments.
+        
+        If the size of the message is less than the fragment size, then
+        the original message is returned rather than a single fragment
+        
+        Since fragmentation is typically only used for very large messages,
+        this method returns a generator for fragments rather than a list
+        
+        Keyword Arguments
+        message       -- the message dict object to be fragmented
+        fragment_size -- the max size for the fragments
+        mid           -- (optional) if provided, the fragment messages
+        will be given this id.  Otherwise an id will be auto-generated.
+
+        Returns a generator of message dict objects representing the fragments
+        """
+        # All fragmented messages need an ID so they can be reconstructed
+        if mid is None:
+            mid = self.fragmentation_seed
+            self.fragmentation_seed = self.fragmentation_seed + 1
+            
+        serialized = self.protocol.serialize(message, mid)
+        
+        if serialized is None:
+            return []
+        
+        message_length = len(serialized)
+        if message_length <= fragment_size:
+            return [message]
+
+        msg_id = message.get("id", None)
+
+        expected_duration = int(math.ceil(math.ceil(message_length / float(fragment_size))) * self.protocol.delay_between_messages)
+
+        log_msg = "sending " + str(int(math.ceil(message_length / float(fragment_size)))) + " parts [fragment size: " + str(fragment_size) +"; expected duration: ~" + str(expected_duration) + "s]"
+        self.protocol.log("info", log_msg)
+
+        return self._fragment_generator(serialized, fragment_size, mid)
+    
+    def _fragment_generator(self, msg, size, mid):
+        """ Returns a generator of fragment messages """
+        total = ((len(msg)-1) / size) + 1
+        n = 0
+        for i in range(0, len(msg), size):
+            fragment = msg[i:i+size]
+            yield self._create_fragment(fragment, n, total, mid)
+            n = n + 1
+    
+    def _create_fragment(self, fragment, num, total, mid):
+        """ Given a string fragment of the original message, creates
+        the appropriate fragment message """
+        return {
+            "op": "fragment",
+            "id": mid,
+            "data": fragment,
+            "num": num,
+            "total": total
+        }
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/publish.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/publish.py
new file mode 100644
index 00000000..552e563e
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/publish.py
@@ -0,0 +1,113 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# Copyright (c) 2014, Creativa 77 SRL
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import fnmatch
+from lgsvl_rosbridge_library.capability import Capability
+from lgsvl_rosbridge_library.internal.publishers import manager
+from lgsvl_rosbridge_library.util import string_types
+import time
+import ctypes
+
+class Publish(Capability):
+
+    publish_msg_fields = [(True, "topic", string_types)]
+
+    topics_glob = None
+
+    #c timespec to python
+    class timespec(ctypes.Structure):
+        _fields_ = [("tv_sec", ctypes.c_long),  #second
+                    ("tv_nsec", ctypes.c_long)] #nano second
+
+    def __init__(self, protocol):
+        #declare C clock params
+        self.libc = ctypes.CDLL("libc.so.6")
+        self.CLOCK_REALTIME = 0
+        self.ts = self.timespec()
+        # Call superclas constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("publish", self.publish)
+
+        # Save the topics that are published on for the purposes of unregistering
+        self._published = {}
+
+        if protocol.parameters and "unregister_timeout" in protocol.parameters:
+            manager.unregister_timeout = protocol.parameters.get("unregister_timeout")
+
+    def publish(self, message):
+        # Do basic type checking
+        self.basic_type_check(message, self.publish_msg_fields)
+        topic = message["topic"]
+        latch = message.get("latch", False)
+        queue_size = message.get("queue_size", 100)
+
+        if Publish.topics_glob is not None and Publish.topics_glob:
+            self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic)
+            match = False
+            for glob in Publish.topics_glob:
+                if (fnmatch.fnmatch(topic, glob)):
+                    self.protocol.log("debug", "Found match with glob " + glob + ", continuing publish...")
+                    match = True
+                    break
+            if not match:
+                self.protocol.log("warn", "No match found for topic, cancelling publish to: " + topic)
+                return
+        else:
+            self.protocol.log("debug", "No topic security glob, not checking publish.")
+
+        # Register as a publishing client, propagating any exceptions
+        client_id = self.protocol.client_id
+        manager.register(client_id, topic, latch=latch, queue_size=queue_size)
+        self._published[topic] = True
+
+
+        # Get the message if one was provided
+        msg = message.get("msg", {})
+        if topic == '/points_raw_origin':
+            # print(msg["header"])
+            # print(time.time())
+            # print(ts.tv_sec, '.',ts.tv_nsec)
+            self.libc.clock_gettime(self.CLOCK_REALTIME, ctypes.byref(self.ts))
+            msg["header"]["stamp"]["secs"] = self.ts.tv_sec
+            msg["header"]["stamp"]["nsecs"] = self.ts.tv_nsec
+
+        # Publish the message
+        manager.publish(client_id, topic, msg, latch=latch, queue_size=queue_size)
+
+    def finish(self):
+        client_id = self.protocol.client_id
+        for topic in self._published:
+            manager.unregister(client_id, topic)
+        self._published.clear()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/service_response.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/service_response.py
new file mode 100644
index 00000000..f2ca5497
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/service_response.py
@@ -0,0 +1,37 @@
+from lgsvl_rosbridge_library.capability import Capability
+from lgsvl_rosbridge_library.internal import ros_loader, message_conversion
+from lgsvl_rosbridge_library.util import string_types
+
+
+class ServiceResponse(Capability):
+
+    service_response_msg_fields = [
+        (True, "service", string_types), (False, "id", string_types),
+        (False, "values", dict), (True, "result", bool)
+    ]
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("service_response", self.service_response)
+
+    def service_response(self, message):
+        # Typecheck the args
+        self.basic_type_check(message, self.service_response_msg_fields)
+
+        # check for the service
+        service_name = message["service"]
+        if service_name in self.protocol.external_service_list:
+            service_handler = self.protocol.external_service_list[service_name]
+            # parse the message
+            request_id = message["id"]
+            values = message["values"]
+            # create a message instance
+            resp = ros_loader.get_service_response_instance(service_handler.service_type)
+            message_conversion.populate_instance(values, resp)
+            # pass along the response
+            service_handler.responses[request_id] = resp
+        else:
+            self.protocol.log("error", "Service %s has not been advertised via rosbridge." % service_name)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/subscribe.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/subscribe.py
new file mode 100644
index 00000000..caffede8
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/subscribe.py
@@ -0,0 +1,307 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import sys
+PYTHON2 = sys.version_info < (3, 0)
+
+import fnmatch
+from threading import Lock
+from functools import partial
+from lgsvl_rosbridge_library.capability import Capability
+from lgsvl_rosbridge_library.internal.subscribers import manager
+from lgsvl_rosbridge_library.internal.subscription_modifiers import MessageHandler
+from lgsvl_rosbridge_library.internal.pngcompression import encode as encode_png
+
+try:
+    from ujson import dumps as encode_json
+except ImportError:
+    try:
+        from simplejson import dumps as encode_json
+    except ImportError:
+        from json import dumps as encode_json
+
+from lgsvl_rosbridge_library.util import string_types
+
+
+class Subscription():
+    """ Keeps track of the clients multiple calls to subscribe.
+
+    Chooses the most appropriate settings to send messages """
+
+    def __init__(self, client_id, topic, publish):
+        """ Create a subscription for the specified client on the specified
+        topic, with callback publish
+
+        Keyword arguments:
+        client_id -- the ID of the client making this subscription
+        topic     -- the name of the topic to subscribe to
+        publish   -- the callback function for incoming messages
+
+        """
+        self.client_id = client_id
+        self.topic = topic
+        self.publish = publish
+
+        self.clients = {}
+
+        self.handler = MessageHandler(None, self._publish)
+        self.handler_lock = Lock()
+        self.update_params()
+
+    def unregister(self):
+        """ Unsubscribes this subscription and cleans up resources """
+        manager.unsubscribe(self.client_id, self.topic)
+        with self.handler_lock:
+            self.handler.finish(block=False)
+        self.clients.clear()
+
+    def subscribe(self, sid=None, msg_type=None, throttle_rate=0,
+                  queue_length=0, fragment_size=None, compression="none"):
+        """ Add another client's subscription request
+
+        If there are multiple calls to subscribe, the values actually used for
+        queue_length, fragment_size, compression and throttle_rate are
+        chosen to encompass all subscriptions' requirements
+
+        Keyword arguments:
+        sid             -- the subscription id from the client
+        msg_type        -- the type of the message to subscribe to
+        throttle_rate   -- the minimum time (in ms) allowed between messages
+        being sent.  If multiple subscriptions, the lower of these is used
+        queue_length    -- the number of messages that can be buffered.  If
+        multiple subscriptions, the lower of these is used
+        fragment_size   -- None if no fragmentation, or the maximum length of
+        allowed outgoing messages
+        compression     -- "none" if no compression, or some other value if
+        compression is to be used (current valid values are 'png')
+
+         """
+
+        client_details = {
+            "throttle_rate": throttle_rate,
+            "queue_length": queue_length,
+            "fragment_size": fragment_size,
+            "compression": compression
+        }
+
+        self.clients[sid] = client_details
+
+        self.update_params()
+
+        if compression == "cbor-raw":
+            msg_type = "__AnyMsg"
+
+        # Subscribe with the manager. This will propagate any exceptions
+        manager.subscribe(self.client_id, self.topic, self.on_msg, msg_type)
+
+    def unsubscribe(self, sid=None):
+        """ Unsubscribe this particular client's subscription
+
+        Keyword arguments:
+        sid -- the individual subscription id.  If None, all are unsubscribed
+
+        """
+        if sid is None:
+            self.clients.clear()
+        elif sid in self.clients:
+            del self.clients[sid]
+
+        if not self.is_empty():
+            self.update_params()
+
+    def is_empty(self):
+        """ Return true if there are no subscriptions currently """
+        return len(self.clients) == 0
+
+    def _publish(self, message):
+        """ Internal method to propagate published messages to the registered
+        publish callback """
+        self.publish(message, self.fragment_size, self.compression)
+
+    def on_msg(self, msg):
+        """ Raw callback called by subscription manager for all incoming
+        messages.
+
+        Incoming messages are passed to the message handler which may drop,
+        buffer, or propagate the message
+
+        """
+        with self.handler_lock:
+            self.handler.handle_message(msg)
+
+    def update_params(self):
+        """ Determine the 'lowest common denominator' params to satisfy all
+        subscribed clients. """
+        if len(self.clients) == 0:
+            self.throttle_rate = 0
+            self.queue_length = 0
+            self.fragment_size = None
+            self.compression = "none"
+            return
+
+        def f(fieldname):
+            return [x[fieldname] for x in self.clients.values()]
+
+        self.throttle_rate = min(f("throttle_rate"))
+        self.queue_length = min(f("queue_length"))
+        frags = [x for x in f("fragment_size") if x != None]
+        if frags == []:
+            self.fragment_size = None
+        else:
+            self.fragment_size = min(frags)
+
+        self.compression = "none"
+        if "png" in f("compression"):
+            self.compression = "png"
+        if "cbor" in f("compression"):
+            self.compression = "cbor"
+        if "cbor-raw" in f("compression"):
+            self.compression = "cbor-raw"
+
+        with self.handler_lock:
+            self.handler = self.handler.set_throttle_rate(self.throttle_rate)
+            self.handler = self.handler.set_queue_length(self.queue_length)
+
+
+class Subscribe(Capability):
+
+    subscribe_msg_fields = [(True, "topic", string_types), (False, "type", string_types),
+                            (False, "throttle_rate", int), (False, "fragment_size", int),
+                            (False, "queue_length", int), (False, "compression", string_types)]
+    unsubscribe_msg_fields = [(True, "topic", string_types)]
+
+    topics_glob = None
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("subscribe", self.subscribe)
+        protocol.register_operation("unsubscribe", self.unsubscribe)
+
+        self._subscriptions = {}
+
+    def subscribe(self, msg):
+        # Pull out the ID
+        sid = msg.get("id", None)
+
+        # Check the args
+        self.basic_type_check(msg, self.subscribe_msg_fields)
+
+        # Make the subscription
+        topic = msg["topic"]
+
+        if Subscribe.topics_glob is not None and Subscribe.topics_glob:
+            self.protocol.log("debug", "Topic security glob enabled, checking topic: " + topic)
+            match = False
+            for glob in Subscribe.topics_glob:
+                if (fnmatch.fnmatch(topic, glob)):
+                    self.protocol.log("debug", "Found match with glob " + glob + ", continuing subscription...")
+                    match = True
+                    break
+            if not match:
+                self.protocol.log("warn", "No match found for topic, cancelling subscription to: " + topic)
+                return
+        else:
+            self.protocol.log("debug", "No topic security glob, not checking subscription.")
+
+        if not topic in self._subscriptions:
+            client_id = self.protocol.client_id
+            cb = partial(self.publish, topic)
+            self._subscriptions[topic] = Subscription(client_id, topic, cb)
+
+        # Register the subscriber
+        subscribe_args = {
+          "sid": sid,
+          "msg_type": msg.get("type", None),
+          "throttle_rate": msg.get("throttle_rate", 0),
+          "fragment_size": msg.get("fragment_size", None),
+          "queue_length": msg.get("queue_length", 0),
+          "compression": msg.get("compression", "none")
+        }
+        self._subscriptions[topic].subscribe(**subscribe_args)
+
+        self.protocol.log("info", "Subscribed to %s" % topic)
+
+    def unsubscribe(self, msg):
+        # Pull out the ID
+        sid = msg.get("id", None)
+
+        self.basic_type_check(msg, self.unsubscribe_msg_fields)
+
+        topic = msg["topic"]
+
+        if topic not in self._subscriptions:
+            return
+        self._subscriptions[topic].unsubscribe(sid)
+
+        if self._subscriptions[topic].is_empty():
+            self._subscriptions[topic].unregister()
+            del self._subscriptions[topic]
+
+        self.protocol.log("info", "Unsubscribed from %s" % topic)
+
+    def publish(self, topic, message, fragment_size=None, compression="none"):
+        """ Publish a message to the client
+
+        Keyword arguments:
+        topic   -- the topic to publish the message on
+        message -- a ROS message wrapped by OutgoingMessage
+        fragment_size -- (optional) fragment the serialized message into msgs
+        with payloads not greater than this value
+        compression   -- (optional) compress the message. valid values are
+        'png' and 'none'
+
+        """
+        # TODO: fragmentation, proper ids
+
+        outgoing_msg = {u"op": u"publish", u"topic": topic}
+        if compression=="png":
+            outgoing_msg["msg"] = message.get_json_values()
+            outgoing_msg_dumped = encode_json(outgoing_msg)
+            outgoing_msg = {"op": "png", "data": encode_png(outgoing_msg_dumped)}
+        elif compression=="cbor":
+            outgoing_msg = message.get_cbor(outgoing_msg)
+        elif compression=="cbor-raw":
+            outgoing_msg = message.get_cbor_raw(outgoing_msg)
+        else:
+            outgoing_msg["msg"] = message.get_json_values()
+
+        self.protocol.send(outgoing_msg, compression=compression)
+
+    def finish(self):
+        for subscription in self._subscriptions.values():
+            subscription.unregister()
+        self._subscriptions.clear()
+        self.protocol.unregister_operation("subscribe")
+        self.protocol.unregister_operation("unsubscribe")
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/unadvertise_service.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/unadvertise_service.py
new file mode 100644
index 00000000..8e7c7636
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capabilities/unadvertise_service.py
@@ -0,0 +1,43 @@
+import fnmatch
+from lgsvl_rosbridge_library.capability import Capability
+
+
+class UnadvertiseService(Capability):
+
+    # unadvertise_service_msg_fields = [(True, "service", (str, unicode))]
+
+    services_glob = None
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("unadvertise_service", self.unadvertise_service)
+
+    def unadvertise_service(self, message):
+        # parse the message
+        service_name = message["service"]
+
+        if UnadvertiseService.services_glob is not None and UnadvertiseService.services_glob:
+            self.protocol.log("debug", "Service security glob enabled, checking service: " + service_name)
+            match = False
+            for glob in UnadvertiseService.services_glob:
+                if (fnmatch.fnmatch(service_name, glob)):
+                    self.protocol.log("debug", "Found match with glob " + glob + ", continuing service unadvertisement...")
+                    match = True
+                    break
+            if not match:
+                self.protocol.log("warn", "No match found for service, cancelling service unadvertisement for: " + service_name)
+                return
+        else:
+            self.protocol.log("debug", "No service security glob, not checking service unadvertisement...")
+
+        # unregister service in ROS
+        if service_name in self.protocol.external_service_list.keys():
+            self.protocol.external_service_list[service_name].graceful_shutdown(timeout=1.0)
+            self.protocol.external_service_list[service_name].service_handle.shutdown("Unadvertise request.")
+            del self.protocol.external_service_list[service_name]
+            self.protocol.log("info", "Unadvertised service %s." % service_name)
+        else:
+            self.protocol.log("error", "Service %s has not been advertised via rosbridge, can't unadvertise." % service_name)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capability.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capability.py
new file mode 100644
index 00000000..6dc38133
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/capability.py
@@ -0,0 +1,105 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from lgsvl_rosbridge_library.internal.exceptions import InvalidArgumentException
+from lgsvl_rosbridge_library.internal.exceptions import MissingArgumentException
+
+
+class Capability:
+    """ Handles the operation-specific logic of a rosbridge message
+
+    May define one or more opcodes to handle, for example 'publish' or
+    'call_service'
+
+    Each connected client receives its own capability instance, which are
+    managed by the client's own protocol instance.
+
+    Protocol.send() is available to send messages back to the client.
+
+    """
+
+    def __init__(self, protocol):
+        """ Abstract class constructor.  All capabilities require a handle to
+        the containing protocol.
+
+        Keyword arguments:
+        protocol -- the protocol instance for this capability instance
+
+        """
+        self.protocol = protocol
+
+    def handle_message(self, message):
+        """ Handle an incoming message.
+
+        Called by the protocol after having already checked the message op code
+
+        Keyword arguments:
+        message -- the incoming message, deserialized into a dictionary
+
+        """
+        pass
+
+    def finish(self):
+        """ Notify this capability that the client is finished and that it's
+        time to free up resources. """
+        pass
+
+    def basic_type_check(self, msg, types_info):
+        """ Performs basic typechecking on fields in msg.
+
+        Keyword arguments:
+        msg        -- a message, deserialized into a dictoinary
+        types_info -- a list of tuples (mandatory, fieldname, fieldtype) where
+                mandatory - boolean, is the field mandatory
+                fieldname - the name of the field in the message
+                fieldtypes - the expected python type of the field or list of types
+
+        Throws:
+        MissingArgumentException -- if a field is mandatory but not present in
+        the message
+        InvalidArgumentException -- if a field is present but not of the type
+        specified by fieldtype
+
+        """
+        for mandatory, fieldname, fieldtypes in types_info:
+            if mandatory and fieldname not in msg:
+                raise MissingArgumentException("Expected a %s field but none was found." % fieldname)
+            elif fieldname in msg:
+                if not isinstance(fieldtypes, tuple):
+                    fieldtypes = (fieldtypes, )
+                valid = False
+                for typ in fieldtypes:
+                    if isinstance(msg[fieldname], typ):
+                        valid = True
+                if not valid:
+                    raise InvalidArgumentException("Expected field %s to be one of %s. Invalid value: %s" % (fieldname, fieldtypes, msg[fieldname]))
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/cbor_conversion.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/cbor_conversion.py
new file mode 100644
index 00000000..6d33214d
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/cbor_conversion.py
@@ -0,0 +1,100 @@
+import struct
+import sys
+
+PYTHON2 = sys.version_info < (3, 0)
+
+try:
+    from cbor import Tag
+except ImportError:
+    from lgsvl_rosbridge_library.util.cbor import Tag
+
+
+LIST_TYPES = [list, tuple]
+INT_TYPES = ['byte', 'char', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64']
+FLOAT_TYPES = ['float32', 'float64']
+STRING_TYPES = ['string']
+BOOL_TYPES = ['bool']
+TIME_TYPES = ['time', 'duration']
+BOOL_ARRAY_TYPES = ['bool[]']
+BYTESTREAM_TYPES = ['uint8[]', 'char[]']
+
+# Typed array tags according to <https://tools.ietf.org/html/draft-ietf-cbor-array-tags-00>
+# Always encode to little-endian variant, for now.
+TAGGED_ARRAY_FORMATS = {
+    'uint16[]': (69, '<{}H'),
+    'uint32[]': (70, '<{}I'),
+    'uint64[]': (71, '<{}Q'),
+    'byte[]': (72, '{}b'),
+    'int8[]': (72, '{}b'),
+    'int16[]': (77, '<{}h'),
+    'int32[]': (78, '<{}i'),
+    'int64[]': (79, '<{}q'),
+    'float32[]': (85, '<{}f'),
+    'float64[]': (86, '<{}d'),
+}
+
+
+def extract_cbor_values(msg):
+    """Extract a dictionary of CBOR-friendly values from a ROS message.
+
+    Primitive values will be casted to specific Python primitives.
+
+    Typed arrays will be tagged and packed into byte arrays.
+    """
+    out = {}
+    for slot, slot_type in zip(msg.__slots__, msg._slot_types):
+        val = getattr(msg, slot)
+
+        if PYTHON2:
+            slot = unicode(slot)  # noqa: F821
+
+        # string
+        if slot_type in STRING_TYPES:
+            out[slot] = unicode(val) if PYTHON2 else str(val)  # noqa: F821
+
+        # bool
+        elif slot_type in BOOL_TYPES:
+            out[slot] = bool(val)
+
+        # integers
+        elif slot_type in INT_TYPES:
+            out[slot] = int(val)
+
+        # floats
+        elif slot_type in FLOAT_TYPES:
+            out[slot] = float(val)
+
+        # time/duration
+        elif slot_type in TIME_TYPES:
+            out[slot] = {
+                'secs': int(val.secs),
+                'nsecs': int(val.nsecs),
+            }
+
+        # byte array
+        elif slot_type in BYTESTREAM_TYPES:
+            if PYTHON2:
+                out[slot] = bytes(bytearray(val))
+            else:
+                out[slot] = bytes(val)
+
+        # bool array
+        elif slot_type in BOOL_ARRAY_TYPES:
+            out[slot] = [bool(i) for i in val]
+
+        # numeric arrays
+        elif slot_type in TAGGED_ARRAY_FORMATS:
+            tag, fmt = TAGGED_ARRAY_FORMATS[slot_type]
+            fmt_to_length = fmt.format(len(val))
+            packed = struct.pack(fmt_to_length, *val)
+            out[slot] = Tag(tag=tag, value=packed)
+
+        # array of messages
+        elif type(val) in LIST_TYPES:
+            out[slot] = [extract_cbor_values(i) for i in val]
+
+        # message
+        else:
+            out[slot] = extract_cbor_values(val)
+
+    return out
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/exceptions.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/exceptions.py
new file mode 100644
index 00000000..24b54590
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/exceptions.py
@@ -0,0 +1,37 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+class InvalidArgumentException(Exception):
+    pass
+
+class MissingArgumentException(Exception):
+    pass
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/message_conversion.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/message_conversion.py
new file mode 100644
index 00000000..609c9d78
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/message_conversion.py
@@ -0,0 +1,308 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from __future__ import print_function
+import roslib
+import rospy
+
+from lgsvl_rosbridge_library.internal import ros_loader
+
+import math
+import re
+import string
+from base64 import standard_b64encode, standard_b64decode
+
+from lgsvl_rosbridge_library.util import string_types, bson
+
+import sys
+if sys.version_info >= (3, 0):
+    type_map = {
+    "bool":    ["bool"],
+    "int":     ["int8", "byte", "uint8", "char",
+                "int16", "uint16", "int32", "uint32",
+                "int64", "uint64", "float32", "float64"],
+    "float":   ["float32", "float64"],
+    "str":     ["string"]
+    }
+    primitive_types = [bool, int, float]
+    python2 = False
+else:
+    type_map = {
+    "bool":    ["bool"],
+    "int":     ["int8", "byte", "uint8", "char",
+                "int16", "uint16", "int32", "uint32",
+                "int64", "uint64", "float32", "float64"],
+    "float":   ["float32", "float64"],
+    "str":     ["string"],
+    "unicode": ["string"],
+    "long":    ["int64", "uint64"]
+    }
+    primitive_types = [bool, int, long, float]  # noqa: F821
+    python2 = True
+
+list_types = [list, tuple]
+ros_time_types = ["time", "duration"]
+ros_primitive_types = ["bool", "byte", "char", "int8", "uint8", "int16",
+                       "uint16", "int32", "uint32", "int64", "uint64",
+                       "float32", "float64", "string"]
+ros_header_types = ["Header", "std_msgs/Header", "roslib/Header"]
+ros_binary_types = ["uint8[]", "char[]"]
+list_braces = re.compile(r'\[[^\]]*\]')
+ros_binary_types_list_braces = [("uint8[]", re.compile(r'uint8\[[^\]]*\]')),
+                                ("char[]", re.compile(r'char\[[^\]]*\]'))]
+
+binary_encoder = None
+bson_only_mode = None
+
+def get_encoder():
+    global binary_encoder,bson_only_mode
+    if binary_encoder is None:
+        binary_encoder_type = rospy.get_param('~binary_encoder', 'default')
+        bson_only_mode = rospy.get_param('~bson_only_mode', False)
+
+        if binary_encoder_type == 'bson' or bson_only_mode:
+            binary_encoder = bson.Binary
+        elif binary_encoder_type == 'default' or binary_encoder_type == 'b64':
+             binary_encoder = standard_b64encode
+        else:
+            print("Unknown encoder type '%s'"%binary_encoder_type)
+            exit(0)
+    return binary_encoder
+
+class InvalidMessageException(Exception):
+    def __init__(self, inst):
+        Exception.__init__(self, "Unable to extract message values from %s instance" % type(inst).__name__)
+
+
+class NonexistentFieldException(Exception):
+    def __init__(self, basetype, fields):
+        Exception.__init__(self, "Message type %s does not have a field %s" % (basetype, '.'.join(fields)))
+
+
+class FieldTypeMismatchException(Exception):
+    def __init__(self, roottype, fields, expected_type, found_type):
+        if roottype == expected_type:
+            Exception.__init__(self, "Expected a JSON object for type %s but received a %s" % (roottype, found_type))
+        else:
+            Exception.__init__(self, "%s message requires a %s for field %s, but got a %s" % (roottype, expected_type, '.'.join(fields), found_type))
+
+
+def extract_values(inst):
+    rostype = getattr(inst, "_type", None)
+    if rostype is None:
+        raise InvalidMessageException(inst=inst)
+    return _from_inst(inst, rostype)
+
+
+def populate_instance(msg, inst):
+    """ Returns an instance of the provided class, with its fields populated
+    according to the values in msg """
+    return _to_inst(msg, inst._type, inst._type, inst)
+
+
+def _from_inst(inst, rostype):
+    global bson_only_mode
+    # Special case for uint8[], we encode the string
+    for binary_type, expression in ros_binary_types_list_braces:
+        if expression.sub(binary_type, rostype) in ros_binary_types:
+            encoded = get_encoder()(inst)
+            return encoded if python2 else encoded.decode('ascii')
+
+    # Check for time or duration
+    if rostype in ros_time_types:
+        return {"secs": inst.secs, "nsecs": inst.nsecs}
+
+    if(bson_only_mode is None):bson_only_mode = rospy.get_param('~bson_only_mode', False)
+    # Check for primitive types
+    if rostype in ros_primitive_types:
+        #JSON does not support Inf and NaN. They are mapped to None and encoded as null
+        if (not bson_only_mode) and (rostype in ["float32", "float64"]):
+            if math.isnan(inst) or math.isinf(inst):
+                return None
+        return inst
+
+    # Check if it's a list or tuple
+    if type(inst) in list_types:
+        return _from_list_inst(inst, rostype)
+
+    # Assume it's otherwise a full ros msg object
+    return _from_object_inst(inst, rostype)
+
+
+def _from_list_inst(inst, rostype):
+    # Can duck out early if the list is empty
+    if len(inst) == 0:
+        return []
+
+    # Remove the list indicators from the rostype
+    rostype = list_braces.sub("", rostype)
+
+    # Shortcut for primitives
+    if rostype in ros_primitive_types and not rostype in ["float32", "float64"]:
+        return list(inst)
+
+    # Call to _to_inst for every element of the list
+    return [_from_inst(x, rostype) for x in inst]
+
+
+def _from_object_inst(inst, rostype):
+    # Create an empty dict then populate with values from the inst
+    msg = {}
+    for field_name, field_rostype in zip(inst.__slots__, inst._slot_types):
+        field_inst = getattr(inst, field_name)
+        msg[field_name] = _from_inst(field_inst, field_rostype)
+    return msg
+
+
+def _to_inst(msg, rostype, roottype, inst=None, stack=[]):
+    # Check if it's uint8[], and if it's a string, try to b64decode
+    for binary_type, expression in ros_binary_types_list_braces:
+        if expression.sub(binary_type, rostype) in ros_binary_types:
+            return _to_binary_inst(msg)
+
+    # Check the type for time or rostime
+    if rostype in ros_time_types:
+        return _to_time_inst(msg, rostype, inst)
+
+    # Check to see whether this is a primitive type
+    if rostype in ros_primitive_types:
+        return _to_primitive_inst(msg, rostype, roottype, stack)
+
+    # Check whether we're dealing with a list type
+    if inst is not None and type(inst) in list_types:
+        return _to_list_inst(msg, rostype, roottype, inst, stack)
+
+    # Otherwise, the type has to be a full ros msg type, so msg must be a dict
+    if inst is None:
+        inst = ros_loader.get_message_instance(rostype)
+
+    return _to_object_inst(msg, rostype, roottype, inst, stack)
+
+
+def _to_binary_inst(msg):
+    if type(msg) in string_types:
+        try:
+            return standard_b64decode(msg)
+        except :
+            return msg
+    else:
+        try:
+            return bytes(bytearray(msg))
+        except:
+            return msg
+
+
+def _to_time_inst(msg, rostype, inst=None):
+    # Create an instance if we haven't been provided with one
+    if rostype == "time" and msg == "now":
+        return rospy.get_rostime()
+
+    if inst is None:
+        if rostype == "time":
+            inst = rospy.rostime.Time()
+        elif rostype == "duration":
+            inst = rospy.rostime.Duration()
+        else:
+            return None
+
+    # Copy across the fields
+    for field in ["secs", "nsecs"]:
+        try:
+            if field in msg:
+                setattr(inst, field, msg[field])
+        except TypeError:
+            continue
+
+    return inst
+
+
+def _to_primitive_inst(msg, rostype, roottype, stack):
+    # Typecheck the msg
+    msgtype = type(msg)
+    if msgtype in primitive_types and rostype in type_map[msgtype.__name__]:
+        return msg
+    elif msgtype in string_types and rostype in type_map[msgtype.__name__]:
+        return msg.encode("utf-8", "ignore") if python2 else msg
+    raise FieldTypeMismatchException(roottype, stack, rostype, msgtype)
+
+
+def _to_list_inst(msg, rostype, roottype, inst, stack):
+    # Typecheck the msg
+    if type(msg) not in list_types:
+        raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))
+
+    # Can duck out early if the list is empty
+    if len(msg) == 0:
+        return []
+
+    # Remove the list indicators from the rostype
+    rostype = list_braces.sub("", rostype)
+
+    # Call to _to_inst for every element of the list
+    return [_to_inst(x, rostype, roottype, None, stack) for x in msg]
+
+
+def _to_object_inst(msg, rostype, roottype, inst, stack):
+    # Typecheck the msg
+    if type(msg) is not dict:
+        raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))
+
+    # Substitute the correct time if we're an std_msgs/Header
+    try:
+        if rostype in ros_header_types:
+            cur_time = rospy.get_rostime()
+            # copy attributes of global Time obj to inst.stamp
+            inst.stamp.secs = cur_time.secs
+            inst.stamp.nsecs = cur_time.nsecs
+    except rospy.exceptions.ROSInitException as e:
+        rospy.logdebug("Not substituting the correct header time: %s" % e)
+
+    inst_fields = dict(zip(inst.__slots__, inst._slot_types))
+
+    for field_name in msg:
+        # Add this field to the field stack
+        field_stack = stack + [field_name]
+
+        # Raise an exception if the msg contains a bad field
+        if not field_name in inst_fields:
+            raise NonexistentFieldException(roottype, field_stack)
+
+        field_rostype = inst_fields[field_name]
+        field_inst = getattr(inst, field_name)
+
+        field_value = _to_inst(msg[field_name], field_rostype,
+                    roottype, field_inst, field_stack)
+
+        setattr(inst, field_name, field_value)
+
+    return inst
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/outgoing_message.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/outgoing_message.py
new file mode 100644
index 00000000..1ce40cf6
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/outgoing_message.py
@@ -0,0 +1,50 @@
+from lgsvl_rosbridge_library.internal.message_conversion import extract_values as extract_json_values
+from lgsvl_rosbridge_library.internal.cbor_conversion import extract_cbor_values
+from rospy import get_rostime
+try:
+    from cbor import dumps as encode_cbor
+except ImportError:
+    from lgsvl_rosbridge_library.util.cbor import dumps as encode_cbor
+
+
+class OutgoingMessage:
+    """A message wrapper for caching encoding operations."""
+    def __init__(self, message):
+        self._message = message
+        self._json_values = None
+        self._cbor_values = None
+        self._cbor_msg = None
+        self._cbor_raw_msg = None
+
+    @property
+    def message(self):
+        return self._message
+
+    def get_json_values(self):
+        if self._json_values is None:
+            self._json_values = extract_json_values(self._message)
+        return self._json_values
+
+    def get_cbor_values(self):
+        if self._cbor_values is None:
+            self._cbor_values = extract_cbor_values(self._message)
+        return self._cbor_values
+
+    def get_cbor(self, outgoing_msg):
+        if self._cbor_msg is None:
+            outgoing_msg[u"msg"] = self.get_cbor_values()
+            self._cbor_msg = encode_cbor(outgoing_msg)
+
+        return self._cbor_msg
+
+    def get_cbor_raw(self, outgoing_msg):
+        if self._cbor_raw_msg is None:
+            now = get_rostime()
+            outgoing_msg[u"msg"] = {
+                u"secs": now.secs,
+                u"nsecs": now.nsecs,
+                u"bytes": self._message._buff
+            }
+            self._cbor_raw_msg = encode_cbor(outgoing_msg)
+
+        return self._cbor_raw_msg
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/pngcompression.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/pngcompression.py
new file mode 100644
index 00000000..dc6a4346
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/pngcompression.py
@@ -0,0 +1,61 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from PIL import Image
+from base64 import standard_b64encode, standard_b64decode
+from lgsvl_rosbridge_library.util import BytesIO
+from math import floor, ceil, sqrt
+
+
+def encode(string):
+    """ PNG-compress the string in a square RBG image padded with '\n', return the b64 encoded bytes """
+    string_bytes = string.encode('utf-8')
+    length = len(string_bytes)
+    width = floor(sqrt(length/3.0))
+    height = ceil((length/3.0) / width)
+    bytes_needed = int(width * height * 3)
+    string_padded = string_bytes + (b'\n' * (bytes_needed - length))
+    i = Image.frombytes('RGB', (int(width), int(height)), string_padded)
+    buff = BytesIO()
+    i.save(buff, "png")
+    encoded = standard_b64encode(buff.getvalue()).decode()
+    return encoded
+
+def decode(string):
+    """ b64 decode the string, then PNG-decompress """
+    decoded = standard_b64decode(string)
+    buff = BytesIO(decoded)
+    i = Image.open(buff)
+    try:
+        return i.tostring()
+    except NotImplementedError:
+        return i.tobytes().decode('utf-8')
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/publishers.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/publishers.py
new file mode 100644
index 00000000..b7b7d612
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/publishers.py
@@ -0,0 +1,357 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# Copyright (c) 2014, Creativa 77 SRL
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from time import time
+from copy import copy
+from threading import Lock, Timer
+from rospy import Publisher, SubscribeListener
+from rospy import logwarn
+from rostopic import get_topic_type
+from lgsvl_rosbridge_library.internal import ros_loader, message_conversion
+from lgsvl_rosbridge_library.internal.topics import TopicNotEstablishedException, TypeConflictException
+
+
+class PublisherConsistencyListener(SubscribeListener):
+    """ This class is used to solve the problem that sometimes we create a
+    publisher and then immediately publish a message, before the subscribers
+    have set up their connections.
+
+    Call attach() to attach the listener to a publisher.  It sets up a buffer
+    of outgoing messages, then when a new connection occurs, sends the messages
+    in the buffer.
+
+    Call detach() to detach the listener from the publisher and restore the
+    original publish methods.
+
+    After some particular timeout (default to 1 second), the listener stops
+    buffering messages as it is assumed by this point all subscribers will have
+    successfully set up their connections."""
+
+    timeout = 1  # Timeout in seconds to wait for new subscribers
+    attached = False
+
+    def attach(self, publisher):
+        """ Overrides the publisher's publish method, and attaches a subscribe
+        listener to the publisher, effectively routing incoming connections
+        and outgoing publish requests through this class instance """
+        # Do the attaching
+        self.publisher = publisher
+        publisher.impl.add_subscriber_listener(self)
+        self.publish = publisher.publish
+        publisher.publish = self.publish_override
+
+        # Set state variables
+        self.lock = Lock()
+        self.established_time = time()
+        self.msg_buffer = []
+        self.attached = True
+
+    def detach(self):
+        """ Restores the publisher's original publish method and unhooks the
+        subscribe listeners, effectively finishing with this object """
+        self.publisher.publish = self.publish
+        if self in self.publisher.impl.subscriber_listeners:
+            self.publisher.impl.subscriber_listeners.remove(self)
+        self.attached = False
+
+    def peer_subscribe(self, topic_name, topic_publish, peer_publish):
+        """ Called whenever there's a new subscription.
+
+        If we're still inside the subscription setup window, then we publish
+        any buffered messages to the peer.
+
+        We also check if we're timed out, but if we are we don't detach (due
+        to threading complications), we just don't propagate buffered messages
+        """
+        if not self.timed_out():
+            with self.lock:
+                msgs = copy(self.msg_buffer)
+            for msg in msgs:
+                peer_publish(msg)
+
+    def timed_out(self):
+        """ Checks to see how much time has elapsed since the publisher was
+        created """
+        return time() - self.established_time > self.timeout
+
+    def publish_override(self, message):
+        """ The publisher's publish method is replaced with this publish method
+        which checks for timeout and if we haven't timed out, buffers outgoing
+        messages in preparation for new subscriptions """
+        if not self.timed_out():
+            with self.lock:
+                self.msg_buffer.append(message)
+        self.publish(message)
+
+
+class MultiPublisher():
+    """ Keeps track of the clients that are using a particular publisher.
+
+    Provides an API to publish messages and register clients that are using
+    this publisher """
+
+    def __init__(self, topic, msg_type=None, latched_client_id=None, queue_size=100):
+        """ Register a publisher on the specified topic.
+
+        Keyword arguments:
+        topic    -- the name of the topic to register the publisher to
+        msg_type -- (optional) the type to register the publisher as.  If not
+        provided, an attempt will be made to infer the topic type
+        latch    -- (optional) if a client requested this publisher to be latched,
+                    provide the client_id of that client here
+
+        Throws:
+        TopicNotEstablishedException -- if no msg_type was specified by the
+        caller and the topic is not yet established, so a topic type cannot
+        be inferred
+        TypeConflictException        -- if the msg_type was specified by the
+        caller and the topic is established, and the established type is
+        different to the user-specified msg_type
+
+        """
+        # First check to see if the topic is already established
+        topic_type = get_topic_type(topic)[0]
+
+        # If it's not established and no type was specified, exception
+        if msg_type is None and topic_type is None:
+            raise TopicNotEstablishedException(topic)
+
+        # Use the established topic type if none was specified
+        if msg_type is None:
+            msg_type = topic_type
+
+        # Load the message class, propagating any exceptions from bad msg types
+        msg_class = ros_loader.get_message_class(msg_type)
+
+        # Make sure the specified msg type and established msg type are same
+        if topic_type is not None and topic_type != msg_class._type:
+            raise TypeConflictException(topic, topic_type, msg_class._type)
+
+        # Create the publisher and associated member variables
+        self.clients = {}
+        self.latched_client_id = latched_client_id
+        self.topic = topic
+        self.msg_class = msg_class
+        self.publisher = Publisher(topic, msg_class, latch=(latched_client_id!=None), queue_size=queue_size)
+        self.listener = PublisherConsistencyListener()
+        self.listener.attach(self.publisher)
+
+    def unregister(self):
+        """ Unregisters the publisher and clears the clients """
+        self.publisher.unregister()
+        self.clients.clear()
+
+    def verify_type(self, msg_type):
+        """ Verify that the publisher publishes messages of the specified type.
+
+        Keyword arguments:
+        msg_type -- the type to check this publisher against
+
+        Throws:
+        Exception -- if ros_loader cannot load the specified msg type
+        TypeConflictException -- if the msg_type is different than the type of
+        this publisher
+
+        """
+        if not ros_loader.get_message_class(msg_type) is self.msg_class:
+            raise TypeConflictException(self.topic,
+                                        self.msg_class._type, msg_type)
+        return
+
+    def publish(self, msg):
+        """ Publish a message using this publisher.
+
+        Keyword arguments:
+        msg -- the dict (json) message to publish
+
+        Throws:
+        Exception -- propagates exceptions from message conversion if the
+        provided msg does not properly conform to the message type of this
+        publisher
+
+        """
+        # First, check the publisher consistency listener to see if it's done
+        if self.listener.attached and self.listener.timed_out():
+            self.listener.detach()
+
+        # Create a message instance
+        inst = self.msg_class()
+
+        # Populate the instance, propagating any exceptions that may be thrown
+        message_conversion.populate_instance(msg, inst)
+
+        # Publish the message
+        self.publisher.publish(inst)
+
+    def register_client(self, client_id):
+        """ Register the specified client as a client of this publisher.
+
+        Keyword arguments:
+        client_id -- the ID of the client using the publisher
+
+        """
+        self.clients[client_id] = True
+
+    def unregister_client(self, client_id):
+        """ Unregister the specified client from this publisher.
+
+        If the specified client_id is not a client of this publisher, nothing
+        happens.
+
+        Keyword arguments:
+        client_id -- the ID of the client to remove
+
+        """
+        if client_id in self.clients:
+            del self.clients[client_id]
+
+    def has_clients(self):
+        """ Return true if there are clients to this publisher. """
+        return len(self.clients) != 0
+
+
+class PublisherManager():
+    """ The PublisherManager keeps track of ROS publishers
+
+    It maintains a MultiPublisher instance for each registered topic
+
+    When unregistering a client, if there are no more clients for a publisher,
+    then that publisher is unregistered from the ROS Master
+    """
+
+    def __init__(self):
+        self._publishers = {}
+        self.unregister_timers = {}
+        self.unregister_timeout = 10.0
+
+    def register(self, client_id, topic, msg_type=None, latch=False, queue_size=100):
+        """ Register a publisher on the specified topic.
+
+        Publishers are shared between clients, so a single MultiPublisher
+        instance is created per topic, even if multiple clients register.
+
+        Keyword arguments:
+        client_id  -- the ID of the client making this request
+        topic      -- the name of the topic to publish on
+        msg_type   -- (optional) the type to publish
+        latch      -- (optional) whether to make this publisher latched
+        queue_size -- (optional) rospy publisher queue_size to use
+
+        Throws:
+        Exception -- exceptions are propagated from the MultiPublisher if
+        there is a problem loading the specified msg class or establishing
+        the publisher
+
+        """
+        latched_client_id = client_id if latch else None
+        if not topic in self._publishers:
+            self._publishers[topic] = MultiPublisher(topic, msg_type, latched_client_id,
+             queue_size=queue_size)
+        elif latch and self._publishers[topic].latched_client_id != client_id:
+            logwarn("Client ID %s attempted to register topic [%s] as latched " +
+                    "but this topic was previously registered.", client_id, topic)
+            logwarn("Only a single registered latched publisher is supported at the time")
+        elif not latch and self._publishers[topic].latched_client_id:
+            logwarn("New non-latched publisher registration for topic [%s] which is " +
+                    "already registered as latched. but this topic was previously " +
+                    "registered.", topic)
+            logwarn("Only a single registered latched publisher is supported at the time")
+
+        if msg_type is not None:
+            self._publishers[topic].verify_type(msg_type)
+
+        self._publishers[topic].register_client(client_id)
+
+    def unregister(self, client_id, topic):
+        """ Unregister a client from the publisher for the given topic.
+            Will wait some time before actually unregistering, it is done in
+            _unregister_impl
+
+        If there are no clients remaining for that publisher, then the
+        publisher is unregistered from the ROS Master
+
+        Keyword arguments:
+        client_id -- the ID of the client making this request
+        topic     -- the topic to unregister the publisher for
+
+        """
+        if not topic in self._publishers:
+            return
+
+        self._publishers[topic].unregister_client(client_id)
+        if topic in self.unregister_timers:
+            self.unregister_timers[topic].cancel()
+            del self.unregister_timers[topic]
+        self.unregister_timers[topic] = Timer(self.unregister_timeout, self._unregister_impl,
+                                              [topic])
+        self.unregister_timers[topic].start()
+
+    def _unregister_impl(self, topic):
+        if not self._publishers[topic].has_clients():
+            self._publishers[topic].unregister()
+            del self._publishers[topic]
+        del self.unregister_timers[topic]
+
+    def unregister_all(self, client_id):
+        """ Unregisters a client from all publishers that they are registered
+        to.
+
+        Keyword arguments:
+        client_id -- the ID of the client making this request """
+        for topic in self._publishers.keys():
+            self.unregister(client_id, topic)
+
+    def publish(self, client_id, topic, msg, latch=False, queue_size=100):
+        """ Publish a message on the given topic.
+
+        Tries to create a publisher on the topic if one does not already exist.
+
+        Keyword arguments:
+        client_id -- the ID of the client making this request
+        topic     -- the topic to publish the message on
+        msg       -- a JSON-like dict of fields and values
+        latch     -- (optional) whether to make this publisher latched
+        queue_size -- (optional) rospy publisher queue_size to use
+
+        Throws:
+        Exception -- a variety of exceptions are propagated.  They can be
+        thrown if there is a problem setting up or getting the publisher,
+        or if the provided msg does not map to the msg class of the publisher.
+
+        """
+        self.register(client_id, topic, latch=latch, queue_size=queue_size)
+
+        self._publishers[topic].publish(msg)
+
+
+manager = PublisherManager()
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/ros_loader.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/ros_loader.py
new file mode 100644
index 00000000..e0f8b2ed
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/ros_loader.py
@@ -0,0 +1,224 @@
+import time
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import roslib
+import rospy
+
+from threading import Lock
+
+""" ros_loader contains methods for dynamically loading ROS message classes at
+runtime.  It's achieved by using roslib to load the manifest files for the
+package that the respective class is contained in.
+
+Methods typically return the requested class or instance, or None if not found
+"""
+
+# Variable containing the loaded classes
+_loaded_msgs = {}
+_loaded_srvs = {}
+_msgs_lock = Lock()
+_srvs_lock = Lock()
+_manifest_lock = Lock()
+
+
+class InvalidTypeStringException(Exception):
+    def __init__(self, typestring):
+        Exception.__init__(self, "%s is not a valid type string" % typestring)
+
+
+class InvalidPackageException(Exception):
+    def __init__(self, package, original_exception):
+        Exception.__init__(self,
+           "Unable to load the manifest for package %s. Caused by: %s"
+           % (package, str(original_exception))
+       )
+
+
+class InvalidModuleException(Exception):
+    def __init__(self, modname, subname, original_exception):
+        Exception.__init__(self,
+           "Unable to import %s.%s from package %s. Caused by: %s"
+           % (modname, subname, modname, str(original_exception))
+        )
+
+
+class InvalidClassException(Exception):
+    def __init__(self, modname, subname, classname, original_exception):
+        Exception.__init__(self,
+           "Unable to import %s class %s from package %s. Caused by %s"
+           % (subname, classname, modname, str(original_exception))
+        )
+
+
+def get_message_class(typestring):
+    """ Loads the message type specified.
+
+    Returns the loaded class, or throws exceptions on failure """
+    return _get_msg_class(typestring)
+
+
+def get_service_class(typestring):
+    """ Loads the service type specified.
+
+    Returns the loaded class, or None on failure """
+    return _get_srv_class(typestring)
+
+
+def get_message_instance(typestring):
+    """ If not loaded, loads the specified type.
+    Then returns an instance of it, or None. """
+    cls = get_message_class(typestring)
+    return cls()
+
+
+def get_service_instance(typestring):
+    """ If not loaded, loads the specified type.
+    Then returns an instance of it, or None. """
+    cls = get_service_class(typestring)
+    return cls()
+
+
+def get_service_request_instance(typestring):
+    cls = get_service_class(typestring)
+    return cls._request_class()
+
+
+def get_service_response_instance(typestring):
+    cls = get_service_class(typestring)
+    return cls._response_class()
+
+
+def _get_msg_class(typestring):
+    """ If not loaded, loads the specified msg class then returns an instance
+    of it
+
+    Throws various exceptions if loading the msg class fails """
+    global _loaded_msgs, _msgs_lock
+    return _get_class(typestring, "msg", _loaded_msgs, _msgs_lock)
+
+
+def _get_srv_class(typestring):
+    """ If not loaded, loads the specified srv class then returns an instance
+    of it
+
+    Throws various exceptions if loading the srv class fails """
+    global _loaded_srvs, _srvs_lock
+    return _get_class(typestring, "srv", _loaded_srvs, _srvs_lock)
+
+
+def _get_class(typestring, subname, cache, lock):
+    """ If not loaded, loads the specified class then returns an instance
+    of it.
+
+    Loaded classes are cached in the provided cache dict
+
+    Throws various exceptions if loading the msg class fails """
+
+    # First, see if we have this type string cached
+    cls = _get_from_cache(cache, lock, typestring)
+    if cls is not None:
+        return cls
+
+    # Now normalise the typestring
+    modname, classname = _splittype(typestring)
+    norm_typestring = modname + "/" + classname
+
+    # Check to see if the normalised type string is cached
+    cls = _get_from_cache(cache, lock, norm_typestring)
+    if cls is not None:
+        return cls
+
+    # Load the class
+    cls = _load_class(modname, subname, classname)
+
+    # Cache the class for both the regular and normalised typestring
+    _add_to_cache(cache, lock, typestring, cls)
+    _add_to_cache(cache, lock, norm_typestring, cls)
+
+    return cls
+
+
+def _load_class(modname, subname, classname):
+    """ Loads the manifest and imports the module that contains the specified
+    type.
+
+    Logic is similar to that of roslib.message.get_message_class, but we want
+    more expressive exceptions.
+
+    Returns the loaded module, or None on failure """
+    global loaded_modules
+
+    try:
+        with _manifest_lock:
+            # roslib maintains a cache of loaded manifests, so no need to duplicate
+            roslib.launcher.load_manifest(modname)
+    except Exception as exc:
+        raise InvalidPackageException(modname, exc)
+
+    try:
+        pypkg = __import__('%s.%s' % (modname, subname))
+    except Exception as exc:
+        raise InvalidModuleException(modname, subname, exc)
+
+    try:
+        return getattr(getattr(pypkg, subname), classname)
+    except Exception as exc:
+        raise InvalidClassException(modname, subname, classname, exc)
+
+
+def _splittype(typestring):
+    """ Split the string the / delimiter and strip out empty strings
+
+    Performs similar logic to roslib.names.package_resource_name but is a bit
+    more forgiving about excess slashes
+    """
+    splits = [x for x in typestring.split("/") if x]
+    if len(splits) == 2:
+        return splits
+    raise InvalidTypeStringException(typestring)
+
+
+def _add_to_cache(cache, lock, key, value):
+    with lock:
+        cache[key] = value
+
+
+def _get_from_cache(cache, lock, key):
+    """ Returns the value for the specified key from the cache.
+    Locks the lock before doing anything. Returns None if key not in cache """
+    ret = None
+    with lock:
+        if key in cache:
+            ret = cache[key]
+    return ret
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/services.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/services.py
new file mode 100644
index 00000000..0a41e503
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/services.py
@@ -0,0 +1,119 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from threading import Thread
+from rospy import ServiceProxy, resolve_name
+from rosservice import get_service_type
+from lgsvl_rosbridge_library.internal.ros_loader import get_service_class
+from lgsvl_rosbridge_library.internal.ros_loader import get_service_request_instance
+from lgsvl_rosbridge_library.internal.message_conversion import populate_instance
+from lgsvl_rosbridge_library.internal.message_conversion import extract_values
+
+
+class InvalidServiceException(Exception):
+    def __init__(self, servicename):
+        Exception.__init__(self, "Service %s does not exist" % servicename)
+
+
+class ServiceCaller(Thread):
+
+    def __init__(self, service, args, success_callback, error_callback):
+        """ Create a service caller for the specified service.  Use start()
+        to start in a separate thread or run() to run in this thread.
+
+        Keyword arguments:
+        service          -- the name of the service to call
+        args             -- arguments to pass to the service.  Can be an
+        ordered list, or a dict of name-value pairs.  Anything else will be
+        treated as though no arguments were provided (which is still valid for
+        some kinds of service)
+        success_callback -- a callback to call with the JSON result of the
+        service call
+        error_callback   -- a callback to call if an error occurs.  The
+        callback will be passed the exception that caused the failure
+
+         """
+        Thread.__init__(self)
+        self.daemon = True
+        self.service = service
+        self.args = args
+        self.success = success_callback
+        self.error = error_callback
+
+    def run(self):
+        try:
+            # Call the service and pass the result to the success handler
+            self.success(call_service(self.service, self.args))
+        except Exception as e:
+            # On error, just pass the exception to the error handler
+            self.error(e)
+
+
+def args_to_service_request_instance(service, inst, args):
+    """ Populate a service request instance with the provided args
+
+    args can be a dictionary of values, or a list, or None
+
+    Propagates any exceptions that may be raised. """
+    msg = {}
+    if isinstance(args, list):
+        msg = dict(zip(inst.__slots__, args))
+    elif isinstance(args, dict):
+        msg = args
+
+    # Populate the provided instance, propagating any exceptions
+    populate_instance(msg, inst)
+
+
+def call_service(service, args=None):
+    # Given the service name, fetch the type and class of the service,
+    # and a request instance
+
+    service = resolve_name(service)
+
+    service_type = get_service_type(str(service))
+    if service_type is None:
+        raise InvalidServiceException(service)
+    service_class = get_service_class(service_type)
+    inst = get_service_request_instance(service_type)
+
+    # Populate the instance with the provided args
+    args_to_service_request_instance(service, inst, args)
+
+    # Call the service
+    proxy = ServiceProxy(service, service_class)
+    response = proxy.call(inst)
+
+    # Turn the response into JSON and pass to the callback
+    json_response = extract_values(response)
+
+    return json_response
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/subscribers.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/subscribers.py
new file mode 100644
index 00000000..3120a593
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/subscribers.py
@@ -0,0 +1,226 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# Copyright (c) 2013, PAL Robotics SL
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from threading import Lock
+from rospy import Subscriber, logerr
+from rostopic import get_topic_type
+from lgsvl_rosbridge_library.internal import ros_loader, message_conversion
+from lgsvl_rosbridge_library.internal.topics import TopicNotEstablishedException
+from lgsvl_rosbridge_library.internal.topics import TypeConflictException
+from lgsvl_rosbridge_library.internal.outgoing_message import OutgoingMessage
+from rospy.msg import AnyMsg
+
+""" Manages and interfaces with ROS Subscriber objects.  A single subscriber
+is shared between multiple clients
+"""
+
+
+class MultiSubscriber():
+    """ Handles multiple clients for a single subscriber.
+
+    Converts msgs to JSON before handing them to callbacks.  Due to subscriber
+    callbacks being called in separate threads, must lock whenever modifying
+    or accessing the subscribed clients. """
+
+    def __init__(self, topic, msg_type=None):
+        """ Register a subscriber on the specified topic.
+
+        Keyword arguments:
+        topic    -- the name of the topic to register the subscriber on
+        msg_type -- (optional) the type to register the subscriber as.  If not
+        provided, an attempt will be made to infer the topic type
+
+        Throws:
+        TopicNotEstablishedException -- if no msg_type was specified by the
+        caller and the topic is not yet established, so a topic type cannot
+        be inferred
+        TypeConflictException        -- if the msg_type was specified by the
+        caller and the topic is established, and the established type is
+        different to the user-specified msg_type
+
+        """
+        # First check to see if the topic is already established
+        topic_type = get_topic_type(topic)[0]
+
+        # If it's not established and no type was specified, exception
+        if msg_type is None and topic_type is None:
+            raise TopicNotEstablishedException(topic)
+
+        # Use the established topic type if none was specified
+        if msg_type is None:
+            msg_type = topic_type
+
+        if msg_type == "__AnyMsg":
+            msg_class = AnyMsg
+        else:
+            # Load the message class, propagating any exceptions from bad msg types
+            msg_class = ros_loader.get_message_class(msg_type)
+
+            # Make sure the specified msg type and established msg type are same
+            if topic_type is not None and topic_type != msg_class._type:
+                raise TypeConflictException(topic, topic_type, msg_class._type)
+
+        # Create the subscriber and associated member variables
+        self.subscriptions = {}
+        self.lock = Lock()
+        self.topic = topic
+        self.msg_class = msg_class
+        self.subscriber = Subscriber(topic, msg_class, self.callback)
+
+    def unregister(self):
+        self.subscriber.unregister()
+        with self.lock:
+            self.subscriptions.clear()
+
+    def verify_type(self, msg_type):
+        """ Verify that the subscriber subscribes to messages of this type.
+
+        Keyword arguments:
+        msg_type -- the type to check this subscriber against
+
+        Throws:
+        Exception -- if ros_loader cannot load the specified msg type
+        TypeConflictException -- if the msg_type is different than the type of
+        this publisher
+
+        """
+        if msg_type == "__AnyMsg":
+            return
+        if not ros_loader.get_message_class(msg_type) is self.msg_class:
+            raise TypeConflictException(self.topic,
+                                        self.msg_class._type, msg_type)
+
+    def subscribe(self, client_id, callback):
+        """ Subscribe the specified client to this subscriber.
+
+        Keyword arguments:
+        client_id -- the ID of the client subscribing
+        callback  -- this client's callback, that will be called for incoming
+        messages
+
+        """
+        with self.lock:
+            self.subscriptions[client_id] = callback
+            # If the topic is latched, add_callback will immediately invoke
+            # the given callback.
+            self.subscriber.impl.add_callback(self.callback, [callback])
+            self.subscriber.impl.remove_callback(self.callback, [callback])
+
+    def unsubscribe(self, client_id):
+        """ Unsubscribe the specified client from this subscriber
+
+        Keyword arguments:
+        client_id -- the ID of the client to unsubscribe
+
+        """
+        with self.lock:
+            del self.subscriptions[client_id]
+
+    def has_subscribers(self):
+        """ Return true if there are subscribers """
+        with self.lock:
+            return len(self.subscriptions) != 0
+
+    def callback(self, msg, callbacks=None):
+        """ Callback for incoming messages on the rospy.Subscriber
+
+        Passes the message to registered subscriber callbacks.
+
+        Keyword Arguments:
+        msg - the ROS message coming from the subscriber
+        callbacks - subscriber callbacks to invoke
+
+        """
+        outgoing = OutgoingMessage(msg)
+
+        # Get the callbacks to call
+        if not callbacks:
+            with self.lock:
+                callbacks = list(self.subscriptions.values())
+
+        # Pass the JSON to each of the callbacks
+        for callback in callbacks:
+            try:
+                callback(outgoing)
+            except Exception as exc:
+                # Do nothing if one particular callback fails except log it
+                logerr("Exception calling subscribe callback: %s", exc)
+
+
+class SubscriberManager():
+    """
+    Keeps track of client subscriptions
+    """
+
+    def __init__(self):
+        self._lock = Lock()
+        self._subscribers = {}
+
+    def subscribe(self, client_id, topic, callback, msg_type=None):
+        """ Subscribe to a topic
+
+        Keyword arguments:
+        client_id -- the ID of the client making this subscribe request
+        topic     -- the name of the topic to subscribe to
+        callback  -- the callback to call for incoming messages on the topic
+        msg_type  -- (optional) the type of the topic
+
+        """
+        with self._lock:
+            if not topic in self._subscribers:
+                self._subscribers[topic] = MultiSubscriber(topic, msg_type)
+
+            if msg_type is not None:
+                self._subscribers[topic].verify_type(msg_type)
+
+            self._subscribers[topic].subscribe(client_id, callback)
+
+    def unsubscribe(self, client_id, topic):
+        """ Unsubscribe from a topic
+
+        Keyword arguments:
+        client_id -- the ID of the client to unsubscribe
+        topic     -- the topic to unsubscribe from
+
+        """
+        with self._lock:
+            if topic in self._subscribers:
+                self._subscribers[topic].unsubscribe(client_id)
+
+                if not self._subscribers[topic].has_subscribers():
+                    self._subscribers[topic].unregister()
+                    del self._subscribers[topic]
+
+
+manager = SubscriberManager()
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/subscription_modifiers.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/subscription_modifiers.py
new file mode 100644
index 00000000..77052ceb
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/subscription_modifiers.py
@@ -0,0 +1,171 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from collections import deque
+from threading import Thread, Condition
+from time import time
+import traceback
+import sys
+
+""" Sits between incoming messages from a subscription, and the outgoing
+publish method.  Provides throttling / buffering capabilities.
+
+When the parameters change, the handler may transition to a different kind
+of handler
+"""
+
+
+class MessageHandler():
+    def __init__(self, previous_handler=None, publish=None):
+        if previous_handler:
+            self.last_publish = previous_handler.last_publish
+            self.throttle_rate = previous_handler.throttle_rate
+            self.queue_length = previous_handler.queue_length
+            self.publish = previous_handler.publish
+        else:
+            self.last_publish = 0
+            self.throttle_rate = 0
+            self.queue_length = 0
+            self.publish = publish
+
+    def set_throttle_rate(self, throttle_rate):
+        self.throttle_rate = throttle_rate / 1000.0
+        return self.transition()
+
+    def set_queue_length(self, queue_length):
+        self.queue_length = queue_length
+        return self.transition()
+
+    def time_remaining(self):
+        return max((self.last_publish + self.throttle_rate) - time(), 0)
+
+    def handle_message(self, msg):
+        self.last_publish = time()
+        self.publish(msg)
+
+    def transition(self):
+        if self.throttle_rate == 0 and self.queue_length == 0:
+            return self
+        elif self.queue_length == 0:
+            return ThrottleMessageHandler(self)
+        else:
+            return QueueMessageHandler(self)
+
+    def finish(self, block=True):
+        pass
+
+
+class ThrottleMessageHandler(MessageHandler):
+
+    def handle_message(self, msg):
+        if self.time_remaining() == 0:
+            MessageHandler.handle_message(self, msg)
+
+    def transition(self):
+        if self.throttle_rate == 0 and self.queue_length == 0:
+            return MessageHandler(self)
+        elif self.queue_length == 0:
+            return self
+        else:
+            return QueueMessageHandler(self)
+
+    def finish(self, block=True):
+        pass
+
+
+class QueueMessageHandler(MessageHandler, Thread):
+
+    def __init__(self, previous_handler):
+        Thread.__init__(self)
+        MessageHandler.__init__(self, previous_handler)
+        self.daemon = True
+        self.queue = deque(maxlen=self.queue_length)
+        self.c = Condition()
+        self.alive = True
+        self.start()
+
+    def handle_message(self, msg):
+        with self.c:
+            if not self.alive:
+                return
+
+            should_notify = len(self.queue) == 0
+            self.queue.append(msg)
+            if should_notify:
+                self.c.notify()
+
+    def transition(self):
+        if self.throttle_rate == 0 and self.queue_length == 0:
+            self.finish()
+            return MessageHandler(self)
+        elif self.queue_length == 0:
+            self.finish()
+            return ThrottleMessageHandler(self)
+        else:
+            with self.c:
+                old_queue = self.queue
+                self.queue = deque(maxlen=self.queue_length)
+                while len(old_queue) > 0:
+                    self.queue.append(old_queue.popleft())
+                self.c.notify()
+            return self
+
+    def finish(self, block=True):
+        """ If throttle was set to 0, this pushes all buffered messages """
+        # Notify the thread to finish
+        with self.c:
+            self.alive = False
+            self.c.notify()
+
+        if block:
+            self.join()
+
+    def run(self):
+        while self.alive:
+            msg = None
+            with self.c:
+                if len(self.queue) == 0:
+                    self.c.wait()
+                else:
+                    self.c.wait(self.time_remaining())
+                if self.alive and self.time_remaining() == 0 and len(self.queue) > 0:
+                    msg = self.queue.popleft()
+            if msg is not None:
+                try:
+                    MessageHandler.handle_message(self, msg)
+                except:
+                    traceback.print_exc(file=sys.stderr)
+        while self.time_remaining() == 0 and len(self.queue) > 0:
+            try:
+                MessageHandler.handle_message(self, self.queue.popleft())
+            except:
+                traceback.print_exc(file=sys.stderr)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/topics.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/topics.py
new file mode 100644
index 00000000..8f911daa
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/internal/topics.py
@@ -0,0 +1,47 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+""" Exceptions and code common to both publishers and subscribers """
+
+
+class TopicNotEstablishedException(Exception):
+    def __init__(self, topic):
+        Exception.__init__(self,
+        "Cannot infer topic type for topic %s as it is not yet advertised" %
+        (topic,))
+
+
+class TypeConflictException(Exception):
+    def __init__(self, topic, orig_type, new_type):
+        Exception.__init__(self,
+        ("Tried to register topic %s with type %s but it is already" +
+        " established with type %s") % (topic, new_type, orig_type))
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/protocol.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/protocol.py
new file mode 100644
index 00000000..74e77952
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/protocol.py
@@ -0,0 +1,405 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from __future__ import unicode_literals
+from builtins import str
+
+import rospy
+import time
+
+from lgsvl_rosbridge_library.internal.exceptions import InvalidArgumentException
+from lgsvl_rosbridge_library.internal.exceptions import MissingArgumentException
+
+from lgsvl_rosbridge_library.capabilities.fragmentation import Fragmentation
+from lgsvl_rosbridge_library.util import json, bson
+
+
+def is_number(s):
+    try:
+        float(s)
+        return True
+    except ValueError:
+        return False
+
+
+def has_binary(obj):
+    """ Returns True if obj is a binary or contains a binary attribute
+    """
+
+    if isinstance(obj, list):
+        return any(has_binary(item) for item in obj)
+
+    if isinstance(obj, dict):
+        return any(has_binary(obj[item]) for item in obj)
+
+    return isinstance(obj, bson.binary.Binary)
+
+
+class Protocol:
+    """ The interface for a single client to interact with ROS.
+
+    See rosbridge_protocol for the default protocol used by rosbridge
+
+    The lifecycle for a Protocol instance is as follows:
+    - Pass incoming messages from the client to incoming
+    - Propagate outgoing messages to the client by overriding outgoing
+    - Call finish to clean up resources when the client is finished
+
+    """
+
+    # fragment_size can be set per client (each client has its own instance of protocol)
+    # ..same for other parameters
+    fragment_size = None
+    png = None
+    # buffer used to gather partial JSON-objects (could be caused by small tcp-buffers or similar..)
+    buffer = ""
+    old_buffer = ""
+    busy = False
+    # if this is too low, ("simple")clients network stacks will get flooded (when sending fragments of a huge message..)
+    # .. depends on message_size/bandwidth/performance/client_limits/...
+    # !! this might be related to (or even be avoided by using) throttle_rate !!
+    delay_between_messages = 0
+    # global list of non-ros advertised services
+    external_service_list = {}
+    # Use only BSON for the whole communication if the server has been started with bson_only_mode:=True
+    bson_only_mode = False
+
+    parameters = None
+
+    def __init__(self, client_id):
+        """ Keyword arguments:
+        client_id -- a unique ID for this client to take.  Uniqueness is
+        important otherwise there will be conflicts between multiple clients
+        with shared resources
+
+        """
+        self.client_id = client_id
+        self.capabilities = []
+        self.operations = {}
+
+        if self.parameters:
+            self.fragment_size = self.parameters["max_message_size"]
+            self.delay_between_messages = self.parameters["delay_between_messages"]
+            self.bson_only_mode = self.parameters.get('bson_only_mode', False)
+
+        if self.bson_only_mode:
+            self.buffer = bytearray()
+            self.old_buffer = bytearray()
+
+    # added default message_string="" to allow recalling incoming until buffer is empty without giving a parameter
+    # --> allows to get rid of (..or minimize) delay between client-side sends
+    def incoming(self, message_string=""):
+        """ Process an incoming message from the client
+
+        Keyword arguments:
+        message_string -- the wire-level message sent by the client
+
+        """
+        if self.bson_only_mode:
+            self.buffer.extend(message_string)
+        else:
+            self.buffer = self.buffer + str(message_string)
+        msg = None
+
+        # take care of having multiple JSON-objects in receiving buffer
+        # ..first, try to load the whole buffer as a JSON-object
+        try:
+            msg = self.deserialize(self.buffer)
+            if self.bson_only_mode:
+                self.buffer = bytearray()
+            else:
+                self.buffer = ''
+
+        # if loading whole object fails try to load part of it (from first opening bracket "{" to next closing bracket "}"
+        # .. this causes Exceptions on "inner" closing brackets --> so I suppressed logging of deserialization errors
+        except Exception as e:
+            if self.bson_only_mode:
+                # Since BSON should be used in conjunction with a network handler
+                # that receives exactly one full BSON message.
+                # This will then be passed to self.deserialize and shouldn't cause any
+                # exceptions because of fragmented messages (broken or invalid messages might still be sent tough)
+                self.log("error", "Exception in deserialization of BSON")
+
+            else:
+                # TODO: handling of partial/multiple/broken json data in incoming buffer
+                # this way is problematic when json contains nested json-objects ( e.g. { ... { "config": [0,1,2,3] } ...  } )
+                # .. if outer json is not fully received, stepping through opening brackets will find { "config" : ... } as a valid json object
+                # .. and pass this "inner" object to rosbridge and throw away the leading part of the "outer" object..
+                # solution for now:
+                # .. check for "op"-field. i can still imagine cases where a nested message ( e.g. complete service_response fits into the data field of a fragment..)
+                # .. would cause trouble, but if a response fits as a whole into a fragment, simply do not pack it into a fragment.
+                #
+                # --> from that follows current limitiation:
+                #     fragment data must NOT (!) contain a complete json-object that has an "op-field"
+                #
+                # an alternative solution would be to only check from first opening bracket and have a time out on data in input buffer.. (to handle broken data)
+                opening_brackets = [i for i, letter in enumerate(self.buffer) if letter == '{']
+                closing_brackets = [i for i, letter in enumerate(self.buffer) if letter == '}']
+
+                for start in opening_brackets:
+                    for end in closing_brackets:
+                        try:
+                            msg = self.deserialize(self.buffer[start:end+1])
+                            if msg.get("op",None) != None:
+                                # TODO: check if throwing away leading data like this is okay.. loops look okay..
+                                self.buffer = self.buffer[end+1:len(self.buffer)]
+                                # jump out of inner loop if json-decode succeeded
+                                break
+                        except Exception as e:
+                            # debug json-decode errors with this line
+                            #print e
+                            pass
+                    # if load was successfull --> break outer loop, too.. -> no need to check if json begins at a "later" opening bracket..
+                    if msg != None:
+                        break
+
+        # if decoding of buffer failed .. simply return
+        if msg is None:
+            return
+
+        # process fields JSON-message object that "control" rosbridge
+        mid = None
+        if "id" in msg:
+            mid = msg["id"]
+        if "op" not in msg:
+            if "receiver" in msg:
+                self.log("error", "Received a rosbridge v1.0 message.  Please refer to rosbridge.org for the correct format of rosbridge v2.0 messages.  Original message was: %s" % message_string)
+            else:
+                self.log("error", "Received a message without an op.  All messages require 'op' field with value one of: %s.  Original message was: %s" % (list(self.operations.keys()), message_string), mid)
+            return
+        op = msg["op"]
+        if op not in self.operations:
+            self.log("error", "Unknown operation: %s.  Allowed operations: %s" % (op, list(self.operations.keys())), mid)
+            return
+        # this way a client can change/overwrite it's active values anytime by just including parameter field in any message sent to rosbridge
+        #  maybe need to be improved to bind parameter values to specific operation..
+        if "fragment_size" in msg.keys():
+            self.fragment_size = msg["fragment_size"]
+            #print "fragment size set to:", self.fragment_size
+        if "message_intervall" in msg.keys() and is_number(msg["message_intervall"]):
+            self.delay_between_messages = msg["message_intervall"]
+        if "png" in msg.keys():
+            self.png = msg["msg"]
+
+        # now try to pass message to according operation
+        try:
+            self.operations[op](msg)
+        except Exception as exc:
+            self.log("error", "%s: %s" % (op, str(exc)), mid)
+
+        # if anything left in buffer .. re-call self.incoming
+        # TODO: check what happens if we have "garbage" on tcp-stack --> infinite loop might be triggered! .. might get out of it when next valid JSON arrives since only data after last 'valid' closing bracket is kept
+        if len(self.buffer) > 0:
+            # try to avoid infinite loop..
+            if self.old_buffer != self.buffer:
+                self.old_buffer = self.buffer
+                self.incoming()
+
+
+
+    def outgoing(self, message):
+        """ Pass an outgoing message to the client.  This method should be
+        overridden.
+
+        Keyword arguments:
+        message -- the wire-level message to send to the client
+
+        """
+        pass
+
+    def send(self, message, cid=None, compression="none"):
+        """ Called internally in preparation for sending messages to the client
+
+        This method pre-processes the message then passes it to the overridden
+        outgoing method.
+
+        Keyword arguments:
+        message -- a dict of message values to be marshalled and sent
+        cid     -- (optional) an associated id
+
+        """
+        serialized = message if compression in ["cbor", "cbor-raw"] else self.serialize(message, cid)
+        if serialized is not None:
+            if self.png == "png":
+                # TODO: png compression on outgoing messages
+                # encode message
+                pass
+
+            fragment_list = None
+            if self.fragment_size != None and len(serialized) > self.fragment_size:
+                mid = message.get("id", None)
+
+                # TODO: think about splitting into fragments that have specified size including header-fields!
+                # --> estimate header size --> split content into fragments that have the requested overall size, rather than requested content size
+                fragment_list = Fragmentation(self).fragment(message, self.fragment_size, mid )
+
+            # fragment list not empty -> send fragments
+            if fragment_list != None:
+                for fragment in fragment_list:
+                    if self.bson_only_mode:
+                        self.outgoing(bson.BSON.encode(fragment), compression=compression)
+                    else:
+                        self.outgoing(json.dumps(fragment), compression=compression)
+                    # okay to use delay here (sender's send()-function) because rosbridge is sending next request only to service provider when last one had finished)
+                    #  --> if this was not the case this delay needed to be implemented in service-provider's (meaning message receiver's) send_message()-function in rosbridge_tcp.py)
+                    time.sleep(self.delay_between_messages)
+            # else send message as it is
+            else:
+                self.outgoing(serialized, compression=compression)
+                time.sleep(self.delay_between_messages)
+
+    def finish(self):
+        """ Indicate that the client is finished and clean up resources.
+
+        All clients should call this method after disconnecting.
+
+        """
+        for capability in self.capabilities:
+            capability.finish()
+
+    def serialize(self, msg, cid=None):
+        """ Turns a dictionary of values into the appropriate wire-level
+        representation.
+
+        Default behaviour uses JSON.  Override to use a different container.
+
+        Keyword arguments:
+        msg -- the dictionary of values to serialize
+        cid -- (optional) an ID associated with this.  Will be logged on err.
+
+        Returns a JSON string representing the dictionary
+        """
+        try:
+            if type(msg) == bytearray:
+                return msg
+            if has_binary(msg) or self.bson_only_mode:
+                return bson.BSON.encode(msg)
+            else:
+                return json.dumps(msg)
+        except:
+            if cid is not None:
+                # Only bother sending the log message if there's an id
+                self.log("error", "Unable to serialize %s message to client"
+                         % msg["op"], cid)
+            return None
+
+    def deserialize(self, msg, cid=None):
+
+        """ Turns the wire-level representation into a dictionary of values
+
+        Default behaviour assumes JSON. Override to use a different container.
+
+        Keyword arguments:
+        msg -- the wire-level message to deserialize
+        cid -- (optional) an ID associated with this.  Is logged on error
+
+        Returns a dictionary of values
+
+        """
+        try:
+            if self.bson_only_mode:
+                bson_message = bson.BSON(msg)
+                return bson_message.decode()
+            else:
+                return json.loads(msg)
+        except Exception as e:
+            # if we did try to deserialize whole buffer .. first try to let self.incoming check for multiple/partial json-decodes before logging error
+            # .. this means, if buffer is not == msg --> we tried to decode part of buffer
+
+            # TODO: implement a way to have a final Exception when nothing works out to decode (multiple/broken/partial JSON..)
+
+            # supressed logging of exception on json-decode to keep rosbridge-logs "clean", otherwise console logs would get spammed for every failed json-decode try
+#            if msg != self.buffer:
+#                error_msg = "Unable to deserialize message from client: %s"  % msg
+#                error_msg += "\nException was: " +str(e)
+#
+#                self.log("error", error_msg, cid)
+
+            # re-raise Exception to allow handling outside of deserialize function instead of returning None
+            raise
+            #return None
+
+    def register_operation(self, opcode, handler):
+        """ Register a handler for an opcode
+
+        Keyword arguments:
+        opcode  -- the opcode to register this handler for
+        handler -- a callback function to call for messages with this opcode
+
+        """
+        self.operations[opcode] = handler
+
+    def unregister_operation(self, opcode):
+        """ Unregister a handler for an opcode
+
+        Keyword arguments:
+        opcode -- the opcode to unregister the handler for
+
+        """
+        if opcode in self.operations:
+            del self.operations[opcode]
+
+    def add_capability(self, capability_class):
+        """ Add a capability to the protocol.
+
+        This method is for convenience; assumes the default capability
+        constructor
+
+        Keyword arguments:
+        capability_class -- the class of the capability to add
+
+        """
+        self.capabilities.append(capability_class(self))
+
+    def log(self, level, message, lid=None):
+        """ Log a message to the client.  By default just sends to stdout
+
+        Keyword arguments:
+        level   -- the logger level of this message
+        message -- the string message to send to the user
+        lid     -- an associated for this log message
+
+        """
+        stdout_formatted_msg = None
+        if lid is not None:
+            stdout_formatted_msg = "[Client %s] [id: %s] %s" % (self.client_id, lid, message)
+        else:
+            stdout_formatted_msg = "[Client %s] %s" % (self.client_id, message)
+
+        if level == "error" or level == "err":
+            rospy.logerr(stdout_formatted_msg)
+        elif level == "warning" or level == "warn":
+            rospy.logwarn(stdout_formatted_msg)
+        elif level == "info" or level == "information":
+            rospy.loginfo(stdout_formatted_msg)
+        else:
+            rospy.logdebug(stdout_formatted_msg)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/rosbridge_protocol.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/rosbridge_protocol.py
new file mode 100644
index 00000000..40a898ec
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/rosbridge_protocol.py
@@ -0,0 +1,62 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from __future__ import print_function
+from lgsvl_rosbridge_library.protocol import Protocol
+from lgsvl_rosbridge_library.capabilities.call_service import CallService
+from lgsvl_rosbridge_library.capabilities.advertise import Advertise
+from lgsvl_rosbridge_library.capabilities.publish import Publish
+from lgsvl_rosbridge_library.capabilities.subscribe import Subscribe
+# imports for defragmentation
+from lgsvl_rosbridge_library.capabilities.defragmentation import Defragment
+# imports for external service_server
+from lgsvl_rosbridge_library.capabilities.advertise_service import AdvertiseService
+from lgsvl_rosbridge_library.capabilities.service_response import ServiceResponse
+from lgsvl_rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
+
+
+
+class RosbridgeProtocol(Protocol):
+    """ Adds the handlers for the rosbridge opcodes """
+    rosbridge_capabilities = [CallService, Advertise, Publish, Subscribe, Defragment, AdvertiseService, ServiceResponse, UnadvertiseService]
+
+    print("registered capabilities (classes):")
+    for cap in rosbridge_capabilities:
+        print(" -", str(cap))
+
+    parameters = None
+
+    def __init__(self, client_id, parameters = None):
+        self.parameters = parameters
+        Protocol.__init__(self, client_id)
+        for capability_class in self.rosbridge_capabilities:
+            self.add_capability(capability_class)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/util/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/util/__init__.py
new file mode 100644
index 00000000..fea9250a
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/util/__init__.py
@@ -0,0 +1,27 @@
+# try to import json-lib: 1st try ujson, 2nd try simplejson, else import standard python json
+try:
+    import ujson as json
+except ImportError:
+    try:
+        import simplejson as json
+    except ImportError:
+        import json
+
+# Differing string types for Python 2 and 3
+import sys
+if sys.version_info >= (3, 0):
+    string_types = (str,)
+    from io import StringIO, BytesIO
+else:
+    string_types = (str, unicode)  # noqa: F821
+    from StringIO import StringIO, StringIO as BytesIO
+
+import bson
+try:
+    bson.BSON
+except AttributeError:
+    raise Exception(
+        "BSON installation does not support all necessary features. "
+        "Please use the MongoDB BSON implementation. "
+        "See: https://github.com/RobotWebTools/rosbridge_suite/issues/198"
+    )
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/util/cbor.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/util/cbor.py
new file mode 100644
index 00000000..a53c128d
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/src/lgsvl_rosbridge_library/util/cbor.py
@@ -0,0 +1,523 @@
+#!python
+# -*- Python -*-
+# Copyright 2014-2015 Brian Olson
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import datetime
+import re
+import struct
+import sys
+
+_IS_PY3 = sys.version_info[0] >= 3
+
+if _IS_PY3:
+    from io import BytesIO as StringIO
+else:
+    try:
+        from cStringIO import StringIO
+    except:
+        from StringIO import StringIO
+
+
+CBOR_TYPE_MASK = 0xE0  # top 3 bits
+CBOR_INFO_BITS = 0x1F  # low 5 bits
+
+
+CBOR_UINT    = 0x00
+CBOR_NEGINT  = 0x20
+CBOR_BYTES   = 0x40
+CBOR_TEXT    = 0x60
+CBOR_ARRAY   = 0x80
+CBOR_MAP     = 0xA0
+CBOR_TAG     = 0xC0
+CBOR_7       = 0xE0  # float and other types
+
+CBOR_UINT8_FOLLOWS  = 24  # 0x18
+CBOR_UINT16_FOLLOWS = 25  # 0x19
+CBOR_UINT32_FOLLOWS = 26  # 0x1a
+CBOR_UINT64_FOLLOWS = 27  # 0x1b
+CBOR_VAR_FOLLOWS    = 31  # 0x1f
+
+CBOR_BREAK  = 0xFF
+
+CBOR_FALSE  = (CBOR_7 | 20)
+CBOR_TRUE   = (CBOR_7 | 21)
+CBOR_NULL   = (CBOR_7 | 22)
+CBOR_UNDEFINED   = (CBOR_7 | 23)  # js 'undefined' value
+
+CBOR_FLOAT16 = (CBOR_7 | 25)
+CBOR_FLOAT32 = (CBOR_7 | 26)
+CBOR_FLOAT64 = (CBOR_7 | 27)
+
+CBOR_TAG_DATE_STRING = 0 # RFC3339
+CBOR_TAG_DATE_ARRAY = 1 # any number type follows, seconds since 1970-01-01T00:00:00 UTC
+CBOR_TAG_BIGNUM = 2 # big endian byte string follows
+CBOR_TAG_NEGBIGNUM = 3 # big endian byte string follows
+CBOR_TAG_DECIMAL = 4 # [ 10^x exponent, number ]
+CBOR_TAG_BIGFLOAT = 5 # [ 2^x exponent, number ]
+CBOR_TAG_BASE64URL = 21
+CBOR_TAG_BASE64 = 22
+CBOR_TAG_BASE16 = 23
+CBOR_TAG_CBOR = 24 # following byte string is embedded CBOR data
+
+CBOR_TAG_URI = 32
+CBOR_TAG_BASE64URL = 33
+CBOR_TAG_BASE64 = 34
+CBOR_TAG_REGEX = 35
+CBOR_TAG_MIME = 36 # following text is MIME message, headers, separators and all
+CBOR_TAG_CBOR_FILEHEADER = 55799 # can open a file with 0xd9d9f7
+
+_CBOR_TAG_BIGNUM_BYTES = struct.pack('B', CBOR_TAG | CBOR_TAG_BIGNUM)
+
+
+def dumps_int(val):
+    "return bytes representing int val in CBOR"
+    if val >= 0:
+        # CBOR_UINT is 0, so I'm lazy/efficient about not OR-ing it in.
+        if val <= 23:
+            return struct.pack('B', val)
+        if val <= 0x0ff:
+            return struct.pack('BB', CBOR_UINT8_FOLLOWS, val)
+        if val <= 0x0ffff:
+            return struct.pack('!BH', CBOR_UINT16_FOLLOWS, val)
+        if val <= 0x0ffffffff:
+            return struct.pack('!BI', CBOR_UINT32_FOLLOWS, val)
+        if val <= 0x0ffffffffffffffff:
+            return struct.pack('!BQ', CBOR_UINT64_FOLLOWS, val)
+        outb = _dumps_bignum_to_bytearray(val)
+        return _CBOR_TAG_BIGNUM_BYTES + _encode_type_num(CBOR_BYTES, len(outb)) + outb
+    val = -1 - val
+    return _encode_type_num(CBOR_NEGINT, val)
+
+
+if _IS_PY3:
+    def _dumps_bignum_to_bytearray(val):
+        out = []
+        while val > 0:
+            out.insert(0, val & 0x0ff)
+            val = val >> 8
+        return bytes(out)
+else:
+    def _dumps_bignum_to_bytearray(val):
+        out = []
+        while val > 0:
+            out.insert(0, chr(val & 0x0ff))
+            val = val >> 8
+        return b''.join(out)
+
+
+def dumps_float(val):
+    return struct.pack("!Bd", CBOR_FLOAT64, val)
+
+
+_CBOR_TAG_NEGBIGNUM_BYTES = struct.pack('B', CBOR_TAG | CBOR_TAG_NEGBIGNUM)
+
+
+def _encode_type_num(cbor_type, val):
+    """For some CBOR primary type [0..7] and an auxiliary unsigned number, return CBOR encoded bytes"""
+    assert val >= 0
+    if val <= 23:
+        return struct.pack('B', cbor_type | val)
+    if val <= 0x0ff:
+        return struct.pack('BB', cbor_type | CBOR_UINT8_FOLLOWS, val)
+    if val <= 0x0ffff:
+        return struct.pack('!BH', cbor_type | CBOR_UINT16_FOLLOWS, val)
+    if val <= 0x0ffffffff:
+        return struct.pack('!BI', cbor_type | CBOR_UINT32_FOLLOWS, val)
+    if (((cbor_type == CBOR_NEGINT) and (val <= 0x07fffffffffffffff)) or
+        ((cbor_type != CBOR_NEGINT) and (val <= 0x0ffffffffffffffff))):
+        return struct.pack('!BQ', cbor_type | CBOR_UINT64_FOLLOWS, val)
+    if cbor_type != CBOR_NEGINT:
+        raise Exception("value too big for CBOR unsigned number: {0!r}".format(val))
+    outb = _dumps_bignum_to_bytearray(val)
+    return _CBOR_TAG_NEGBIGNUM_BYTES + _encode_type_num(CBOR_BYTES, len(outb)) + outb
+
+
+if _IS_PY3:
+    def _is_unicode(val):
+        return isinstance(val, str)
+else:
+    def _is_unicode(val):
+        return isinstance(val, unicode)
+
+
+def dumps_string(val, is_text=None, is_bytes=None):
+    if _is_unicode(val):
+        val = val.encode('utf8')
+        is_text = True
+        is_bytes = False
+    if (is_bytes) or not (is_text == True):
+        return _encode_type_num(CBOR_BYTES, len(val)) + val
+    return _encode_type_num(CBOR_TEXT, len(val)) + val
+
+
+def dumps_array(arr, sort_keys=False):
+    head = _encode_type_num(CBOR_ARRAY, len(arr))
+    parts = [dumps(x, sort_keys=sort_keys) for x in arr]
+    return head + b''.join(parts)
+
+
+if _IS_PY3:
+    def dumps_dict(d, sort_keys=False):
+        head = _encode_type_num(CBOR_MAP, len(d))
+        parts = [head]
+        if sort_keys:
+            for k in sorted(d.keys()):
+                v = d[k]
+                parts.append(dumps(k, sort_keys=sort_keys))
+                parts.append(dumps(v, sort_keys=sort_keys))
+        else:
+            for k,v in d.items():
+                parts.append(dumps(k, sort_keys=sort_keys))
+                parts.append(dumps(v, sort_keys=sort_keys))
+        return b''.join(parts)
+else:
+    def dumps_dict(d, sort_keys=False):
+        head = _encode_type_num(CBOR_MAP, len(d))
+        parts = [head]
+        if sort_keys:
+            for k in sorted(d.iterkeys()):
+                v = d[k]
+                parts.append(dumps(k, sort_keys=sort_keys))
+                parts.append(dumps(v, sort_keys=sort_keys))
+        else:
+            for k,v in d.iteritems():
+                parts.append(dumps(k, sort_keys=sort_keys))
+                parts.append(dumps(v, sort_keys=sort_keys))
+        return b''.join(parts)
+
+
+def dumps_bool(b):
+    if b:
+        return struct.pack('B', CBOR_TRUE)
+    return struct.pack('B', CBOR_FALSE)
+
+
+def dumps_tag(t, sort_keys=False):
+    return _encode_type_num(CBOR_TAG, t.tag) + dumps(t.value, sort_keys=sort_keys)
+    
+
+if _IS_PY3:
+    def _is_stringish(x):
+        return isinstance(x, (str, bytes))
+    def _is_intish(x):
+        return isinstance(x, int)
+else:
+    def _is_stringish(x):
+        return isinstance(x, (str, basestring, bytes, unicode))
+    def _is_intish(x):
+        return isinstance(x, (int, long))
+
+
+def dumps(ob, sort_keys=False):
+    if ob is None:
+        return struct.pack('B', CBOR_NULL)
+    if isinstance(ob, bool):
+        return dumps_bool(ob)
+    if _is_stringish(ob):
+        return dumps_string(ob)
+    if isinstance(ob, (list, tuple)):
+        return dumps_array(ob, sort_keys=sort_keys)
+    # TODO: accept other enumerables and emit a variable length array
+    if isinstance(ob, dict):
+        return dumps_dict(ob, sort_keys=sort_keys)
+    if isinstance(ob, float):
+        return dumps_float(ob)
+    if _is_intish(ob):
+        return dumps_int(ob)
+    if isinstance(ob, Tag):
+        return dumps_tag(ob, sort_keys=sort_keys)
+    raise Exception("don't know how to cbor serialize object of type %s", type(ob))
+
+
+# same basic signature as json.dump, but with no options (yet)
+def dump(obj, fp, sort_keys=False):
+    """
+    obj: Python object to serialize
+    fp: file-like object capable of .write(bytes)
+    """
+    # this is kinda lame, but probably not inefficient for non-huge objects
+    # TODO: .write() to fp as we go as each inner object is serialized
+    blob = dumps(obj, sort_keys=sort_keys)
+    fp.write(blob)
+
+
+class Tag(object):
+    def __init__(self, tag=None, value=None):
+        self.tag = tag
+        self.value = value
+
+    def __repr__(self):
+        return "Tag({0!r}, {1!r})".format(self.tag, self.value)
+
+    def __eq__(self, other):
+        if not isinstance(other, Tag):
+            return False
+        return (self.tag == other.tag) and (self.value == other.value)
+
+
+def loads(data):
+    """
+    Parse CBOR bytes and return Python objects.
+    """
+    if data is None:
+        raise ValueError("got None for buffer to decode in loads")
+    fp = StringIO(data)
+    return _loads(fp)[0]
+
+
+def load(fp):
+    """
+    Parse and return object from fp, a file-like object supporting .read(n)
+    """
+    return _loads(fp)[0]
+
+
+_MAX_DEPTH = 100
+
+
+def _tag_aux(fp, tb):
+    bytes_read = 1
+    tag = tb & CBOR_TYPE_MASK
+    tag_aux = tb & CBOR_INFO_BITS
+    if tag_aux <= 23:
+        aux = tag_aux
+    elif tag_aux == CBOR_UINT8_FOLLOWS:
+        data = fp.read(1)
+        aux = struct.unpack_from("!B", data, 0)[0]
+        bytes_read += 1
+    elif tag_aux == CBOR_UINT16_FOLLOWS:
+        data = fp.read(2)
+        aux = struct.unpack_from("!H", data, 0)[0]
+        bytes_read += 2
+    elif tag_aux == CBOR_UINT32_FOLLOWS:
+        data = fp.read(4)
+        aux = struct.unpack_from("!I", data, 0)[0]
+        bytes_read += 4
+    elif tag_aux == CBOR_UINT64_FOLLOWS:
+        data = fp.read(8)
+        aux = struct.unpack_from("!Q", data, 0)[0]
+        bytes_read += 8
+    else:
+        assert tag_aux == CBOR_VAR_FOLLOWS, "bogus tag {0:02x}".format(tb)
+        aux = None
+
+    return tag, tag_aux, aux, bytes_read
+
+
+def _read_byte(fp):
+    tb = fp.read(1)
+    if len(tb) == 0:
+        # I guess not all file-like objects do this
+        raise EOFError()
+    return ord(tb)
+
+
+def _loads_var_array(fp, limit, depth, returntags, bytes_read):
+    ob = []
+    tb = _read_byte(fp)
+    while tb != CBOR_BREAK:
+        (subob, sub_len) = _loads_tb(fp, tb, limit, depth, returntags)
+        bytes_read += 1 + sub_len
+        ob.append(subob)
+        tb = _read_byte(fp)
+    return (ob, bytes_read + 1)
+
+
+def _loads_var_map(fp, limit, depth, returntags, bytes_read):
+    ob = {}
+    tb = _read_byte(fp)
+    while tb != CBOR_BREAK:
+        (subk, sub_len) = _loads_tb(fp, tb, limit, depth, returntags)
+        bytes_read += 1 + sub_len
+        (subv, sub_len) = _loads(fp, limit, depth, returntags)
+        bytes_read += sub_len
+        ob[subk] = subv
+        tb = _read_byte(fp)
+    return (ob, bytes_read + 1)
+
+
+if _IS_PY3:
+    def _loads_array(fp, limit, depth, returntags, aux, bytes_read):
+        ob = []
+        for i in range(aux):
+            subob, subpos = _loads(fp)
+            bytes_read += subpos
+            ob.append(subob)
+        return ob, bytes_read
+    def _loads_map(fp, limit, depth, returntags, aux, bytes_read):
+        ob = {}
+        for i in range(aux):
+            subk, subpos = _loads(fp)
+            bytes_read += subpos
+            subv, subpos = _loads(fp)
+            bytes_read += subpos
+            ob[subk] = subv
+        return ob, bytes_read
+else:
+    def _loads_array(fp, limit, depth, returntags, aux, bytes_read):
+        ob = []
+        for i in xrange(aux):
+            subob, subpos = _loads(fp)
+            bytes_read += subpos
+            ob.append(subob)
+        return ob, bytes_read
+    def _loads_map(fp, limit, depth, returntags, aux, bytes_read):
+        ob = {}
+        for i in xrange(aux):
+            subk, subpos = _loads(fp)
+            bytes_read += subpos
+            subv, subpos = _loads(fp)
+            bytes_read += subpos
+            ob[subk] = subv
+        return ob, bytes_read
+        
+
+def _loads(fp, limit=None, depth=0, returntags=False):
+    "return (object, bytes read)"
+    if depth > _MAX_DEPTH:
+        raise Exception("hit CBOR loads recursion depth limit")
+
+    tb = _read_byte(fp)
+
+    return _loads_tb(fp, tb, limit, depth, returntags)
+
+def _loads_tb(fp, tb, limit=None, depth=0, returntags=False):
+    # Some special cases of CBOR_7 best handled by special struct.unpack logic here
+    if tb == CBOR_FLOAT16:
+        data = fp.read(2)
+        hibyte, lowbyte = struct.unpack_from("BB", data, 0)
+        exp = (hibyte >> 2) & 0x1F
+        mant = ((hibyte & 0x03) << 8) | lowbyte
+        if exp == 0:
+            val = mant * (2.0 ** -24)
+        elif exp == 31:
+            if mant == 0:
+                val = float('Inf')
+            else:
+                val = float('NaN')
+        else:
+            val = (mant + 1024.0) * (2 ** (exp - 25))
+        if hibyte & 0x80:
+            val = -1.0 * val
+        return (val, 3)
+    elif tb == CBOR_FLOAT32:
+        data = fp.read(4)
+        pf = struct.unpack_from("!f", data, 0)
+        return (pf[0], 5)
+    elif tb == CBOR_FLOAT64:
+        data = fp.read(8)
+        pf = struct.unpack_from("!d", data, 0)
+        return (pf[0], 9)
+
+    tag, tag_aux, aux, bytes_read = _tag_aux(fp, tb)
+
+    if tag == CBOR_UINT:
+        return (aux, bytes_read)
+    elif tag == CBOR_NEGINT:
+        return (-1 - aux, bytes_read)
+    elif tag == CBOR_BYTES:
+        ob, subpos = loads_bytes(fp, aux)
+        return (ob, bytes_read + subpos)
+    elif tag == CBOR_TEXT:
+        raw, subpos = loads_bytes(fp, aux, btag=CBOR_TEXT)
+        ob = raw.decode('utf8')
+        return (ob, bytes_read + subpos)
+    elif tag == CBOR_ARRAY:
+        if aux is None:
+            return _loads_var_array(fp, limit, depth, returntags, bytes_read)
+        return _loads_array(fp, limit, depth, returntags, aux, bytes_read)
+    elif tag == CBOR_MAP:
+        if aux is None:
+            return _loads_var_map(fp, limit, depth, returntags, bytes_read)
+        return _loads_map(fp, limit, depth, returntags, aux, bytes_read)
+    elif tag == CBOR_TAG:
+        ob, subpos = _loads(fp)
+        bytes_read += subpos
+        if returntags:
+            # Don't interpret the tag, return it and the tagged object.
+            ob = Tag(aux, ob)
+        else:
+            # attempt to interpet the tag and the value into a Python object.
+            ob = tagify(ob, aux)
+        return ob, bytes_read
+    elif tag == CBOR_7:
+        if tb == CBOR_TRUE:
+            return (True, bytes_read)
+        if tb == CBOR_FALSE:
+            return (False, bytes_read)
+        if tb == CBOR_NULL:
+            return (None, bytes_read)
+        if tb == CBOR_UNDEFINED:
+            return (None, bytes_read)
+        raise ValueError("unknown cbor tag 7 byte: {:02x}".format(tb))
+
+
+def loads_bytes(fp, aux, btag=CBOR_BYTES):
+    # TODO: limit to some maximum number of chunks and some maximum total bytes
+    if aux is not None:
+        # simple case
+        ob = fp.read(aux)
+        return (ob, aux)
+    # read chunks of bytes
+    chunklist = []
+    total_bytes_read = 0
+    while True:
+        tb = fp.read(1)[0]
+        if not _IS_PY3:
+            tb = ord(tb)
+        if tb == CBOR_BREAK:
+            total_bytes_read += 1
+            break
+        tag, tag_aux, aux, bytes_read = _tag_aux(fp, tb)
+        assert tag == btag, 'variable length value contains unexpected component'
+        ob = fp.read(aux)
+        chunklist.append(ob)
+        total_bytes_read += bytes_read + aux
+    return (b''.join(chunklist), total_bytes_read)
+
+
+if _IS_PY3:
+    def _bytes_to_biguint(bs):
+        out = 0
+        for ch in bs:
+            out = out << 8
+            out = out | ch
+        return out
+else:
+    def _bytes_to_biguint(bs):
+        out = 0
+        for ch in bs:
+            out = out << 8
+            out = out | ord(ch)
+        return out
+
+
+def tagify(ob, aux):
+    # TODO: make this extensible?
+    # cbor.register_tag_handler(tagnumber, tag_handler)
+    # where tag_handler takes (tagnumber, tagged_object)
+    if aux == CBOR_TAG_DATE_STRING:
+        # TODO: parse RFC3339 date string
+        pass
+    if aux == CBOR_TAG_DATE_ARRAY:
+        return datetime.datetime.utcfromtimestamp(ob)
+    if aux == CBOR_TAG_BIGNUM:
+        return _bytes_to_biguint(ob)
+    if aux == CBOR_TAG_NEGBIGNUM:
+        return -1 - _bytes_to_biguint(ob)
+    if aux == CBOR_TAG_REGEX:
+        # Is this actually a good idea? Should we just return the tag and the raw value to the user somehow?
+        return re.compile(ob)
+    return Tag(aux, ob)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/AddTwoInts.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/AddTwoInts.srv
new file mode 100644
index 00000000..3a68808e
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/AddTwoInts.srv
@@ -0,0 +1,4 @@
+int64 a
+int64 b
+---
+int64 sum
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/SendBytes.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/SendBytes.srv
new file mode 100644
index 00000000..a767405e
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/SendBytes.srv
@@ -0,0 +1,3 @@
+int64 count
+---
+string data
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestArrayRequest.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestArrayRequest.srv
new file mode 100644
index 00000000..48b3c476
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestArrayRequest.srv
@@ -0,0 +1,2 @@
+int32[] int
+---
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestEmpty.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestEmpty.srv
new file mode 100644
index 00000000..73b314ff
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestEmpty.srv
@@ -0,0 +1 @@
+---
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestMultipleRequestFields.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestMultipleRequestFields.srv
new file mode 100644
index 00000000..c4b3da6d
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestMultipleRequestFields.srv
@@ -0,0 +1,5 @@
+int32 int
+float32 float
+string string
+bool bool
+---
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestMultipleResponseFields.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestMultipleResponseFields.srv
new file mode 100644
index 00000000..ee5e430f
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestMultipleResponseFields.srv
@@ -0,0 +1,5 @@
+---
+int32 int
+float32 float
+string string
+bool bool
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestNestedService.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestNestedService.srv
new file mode 100644
index 00000000..2a03606d
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestNestedService.srv
@@ -0,0 +1,5 @@
+#request definition
+geometry_msgs/Pose pose
+---
+#response definition
+std_msgs/Float64 data
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestRequestAndResponse.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestRequestAndResponse.srv
new file mode 100644
index 00000000..7e8ddc8f
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestRequestAndResponse.srv
@@ -0,0 +1,3 @@
+int32 data
+---
+int32 data
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestRequestOnly.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestRequestOnly.srv
new file mode 100644
index 00000000..78b05cf6
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestRequestOnly.srv
@@ -0,0 +1,2 @@
+int32 data
+---
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestResponseOnly.srv b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestResponseOnly.srv
new file mode 100644
index 00000000..38597ecf
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/srv/TestResponseOnly.srv
@@ -0,0 +1,2 @@
+---
+int32 data
\ No newline at end of file
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_advertise.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_advertise.py
new file mode 100755
index 00000000..758e6cb0
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_advertise.py
@@ -0,0 +1,145 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+from time import sleep
+
+from std_msgs.msg import *
+
+from lgsvl_rosbridge_library.protocol import Protocol
+from lgsvl_rosbridge_library.protocol import InvalidArgumentException, MissingArgumentException
+from lgsvl_rosbridge_library.capabilities.advertise import Registration, Advertise
+from lgsvl_rosbridge_library.internal.publishers import manager
+from lgsvl_rosbridge_library.internal import ros_loader
+
+from json import loads, dumps
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_advertise'
+
+
+class TestAdvertise(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+        manager.unregister_timeout = 1.0
+
+    def is_topic_published(self, topicname):
+        return topicname in dict(rospy.get_published_topics()).keys()
+
+    def test_missing_arguments(self):
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+        msg = {"op": "advertise"}
+        self.assertRaises(MissingArgumentException, adv.advertise, loads(dumps(msg)))
+
+        msg = {"op": "advertise", "topic": "/jon"}
+        self.assertRaises(MissingArgumentException, adv.advertise, loads(dumps(msg)))
+
+        msg = {"op": "advertise", "type": "std_msgs/String"}
+        self.assertRaises(MissingArgumentException, adv.advertise, loads(dumps(msg)))
+
+    def test_invalid_arguments(self):
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+
+        msg = {"op": "advertise", "topic": 3, "type": "std_msgs/String"}
+        self.assertRaises(InvalidArgumentException, adv.advertise, loads(dumps(msg)))
+
+        msg = {"op": "advertise", "topic": "/jon", "type": 3}
+        self.assertRaises(InvalidArgumentException, adv.advertise, loads(dumps(msg)))
+
+    def test_invalid_msg_typestrings(self):
+        invalid = ["", "/", "//", "///", "////", "/////", "bad", "stillbad",
+       "not/better/still", "not//better//still", "not///better///still",
+       "better/", "better//", "better///", "/better", "//better", "///better",
+       "this\isbad", "\\"]
+
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+
+        for invalid_type in invalid:
+            msg = {"op": "advertise", "topic": "/test_invalid_msg_typestrings",
+                   "type": invalid_type}
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              adv.advertise, loads(dumps(msg)))
+
+    def test_invalid_msg_package(self):
+        nonexistent = ["wangle_msgs/Jam", "whistleblower_msgs/Document",
+        "sexual_harrassment_msgs/UnwantedAdvance", "coercion_msgs/Bribe",
+        "airconditioning_msgs/Cold", "pr2thoughts_msgs/Escape"]
+
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+
+        for invalid_type in nonexistent:
+            msg = {"op": "advertise", "topic": "/test_invalid_msg_package",
+                   "type": invalid_type}
+            self.assertRaises(ros_loader.InvalidPackageException,
+                              adv.advertise, loads(dumps(msg)))
+
+    def test_invalid_msg_module(self):
+        no_msgs = ["roslib/Time", "roslib/Duration", "roslib/Header",
+        "std_srvs/ConflictedMsg", "topic_tools/MessageMessage"]
+
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+
+        for invalid_type in no_msgs:
+            msg = {"op": "advertise", "topic": "/test_invalid_msg_module",
+                   "type": invalid_type}
+            self.assertRaises(ros_loader.InvalidModuleException,
+                              adv.advertise, loads(dumps(msg)))
+
+    def test_invalid_msg_classes(self):
+        nonexistent = ["roscpp/Time", "roscpp/Duration", "roscpp/Header",
+        "rospy/Time", "rospy/Duration", "rospy/Header", "std_msgs/Spool",
+        "geometry_msgs/Tetrahedron", "sensor_msgs/TelepathyUnit"]
+
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+
+        for invalid_type in nonexistent:
+            msg = {"op": "advertise", "topic": "/test_invalid_msg_classes",
+                   "type": invalid_type}
+            self.assertRaises(ros_loader.InvalidClassException,
+                              adv.advertise, loads(dumps(msg)))
+
+    def test_valid_msg_classes(self):
+        assortedmsgs = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus",
+        "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage",
+        "nav_msgs/OccupancyGrid", "geometry_msgs/Point32", "std_msgs/String",
+        "trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue",
+        "visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells",
+        "sensor_msgs/PointCloud2"]
+
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+
+        for valid_type in assortedmsgs:
+            msg = {"op": "advertise", "topic": "/" + valid_type,
+                   "type": valid_type}
+            adv.advertise(loads(dumps(msg)))
+            adv.unadvertise(loads(dumps(msg)))
+
+    def test_do_advertise(self):
+        proto = Protocol("hello")
+        adv = Advertise(proto)
+        topic = "/test_do_advertise"
+        type = "std_msgs/String"
+
+        msg = {"op": "advertise", "topic": topic, "type": type}
+        adv.advertise(loads(dumps(msg)))
+        self.assertTrue(self.is_topic_published(topic))
+        adv.unadvertise(loads(dumps(msg)))
+        self.assertTrue(self.is_topic_published(topic))
+        sleep(manager.unregister_timeout * 1.1)
+        self.assertFalse(self.is_topic_published(topic))
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestAdvertise)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_call_service.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_call_service.py
new file mode 100755
index 00000000..c4476644
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_call_service.py
@@ -0,0 +1,107 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+import time
+
+from roscpp.srv import GetLoggers
+
+from json import loads, dumps
+from std_msgs.msg import String
+from std_srvs.srv import SetBool
+
+from lgsvl_rosbridge_library.capabilities.call_service import CallService
+from lgsvl_rosbridge_library.protocol import Protocol
+from lgsvl_rosbridge_library.protocol import InvalidArgumentException, MissingArgumentException
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_call_service'
+
+
+class TestCallService(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def test_missing_arguments(self):
+        proto = Protocol("test_missing_arguments")
+        s = CallService(proto)
+        msg = loads(dumps({"op": "call_service"}))
+        self.assertRaises(MissingArgumentException, s.call_service, msg)
+
+    def test_invalid_arguments(self):
+        proto = Protocol("test_invalid_arguments")
+        s = CallService(proto)
+
+        msg = loads(dumps({"op": "call_service", "service": 3}))
+        self.assertRaises(InvalidArgumentException, s.call_service, msg)
+
+    def test_call_service_works(self):
+        # Prepare to call the service the 'proper' way
+        p = rospy.ServiceProxy(rospy.get_name() + "/get_loggers", GetLoggers)
+        p.wait_for_service()
+        time.sleep(1.0)
+
+        proto = Protocol("test_call_service_works")
+        s = CallService(proto)
+        msg = loads(dumps({"op": "call_service", "service": rospy.get_name() + "/get_loggers"}))
+
+        received = {"msg": None, "arrived": False}
+
+        def cb(msg, cid=None):
+            received["msg"] = msg
+            received["arrived"] = True
+
+        proto.send = cb
+
+        s.call_service(msg)
+
+        timeout = 5.0
+        start = rospy.Time.now()
+        while rospy.Time.now() - start < rospy.Duration(timeout):
+            if received["arrived"]:
+                break
+            time.sleep(0.1)
+
+        # The rosbridge service call actually causes another logger to appear,
+        # so do the "regular" service call after that.
+        ret = p()
+
+        self.assertTrue(received["msg"]["result"])
+        for x, y in zip(ret.loggers, received["msg"]["values"]["loggers"]):
+            self.assertEqual(x.name, y["name"])
+            self.assertEqual(x.level, y["level"])
+
+    def test_call_service_fail(self):
+        # Dummy service that instantly fails
+        service_server = rospy.Service("set_bool_fail", SetBool,
+                                       lambda req: None)
+
+        proto = Protocol("test_call_service_fail")
+        s = CallService(proto)
+        send_msg = loads(dumps({"op": "call_service", "service": rospy.get_name() + "/set_bool_fail", "args": '[ true ]'}))
+
+        received = {"msg": None, "arrived": False}
+        def cb(msg, cid=None):
+            received["msg"] = msg
+            received["arrived"] = True
+
+        proto.send = cb
+
+        s.call_service(send_msg)
+
+        timeout = 5.0
+        start = rospy.Time.now()
+        while rospy.Time.now() - start < rospy.Duration(timeout):
+            if received["arrived"]:
+                break
+            time.sleep(0.1)
+
+        self.assertFalse(received["msg"]["result"])
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestCallService)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_capabilities.test b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_capabilities.test
new file mode 100644
index 00000000..cd039818
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_capabilities.test
@@ -0,0 +1,7 @@
+<launch>
+  <test test-name="test_advertise" pkg="lgsvl_rosbridge_library" type="test_advertise.py" />
+  <test test-name="test_publish" pkg="lgsvl_rosbridge_library" type="test_publish.py" />
+  <test test-name="test_subscribe" pkg="lgsvl_rosbridge_library" type="test_subscribe.py" />
+  <test test-name="test_call_service" pkg="lgsvl_rosbridge_library" type="test_call_service.py" />
+  <test test-name="test_service_capabilities" pkg="lgsvl_rosbridge_library" type="test_service_capabilities.py" />
+</launch>
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_publish.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_publish.py
new file mode 100755
index 00000000..a3ae3eb8
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_publish.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+from time import sleep
+
+from lgsvl_rosbridge_library.protocol import Protocol
+from lgsvl_rosbridge_library.protocol import InvalidArgumentException, MissingArgumentException
+from lgsvl_rosbridge_library.capabilities.publish import Publish
+from lgsvl_rosbridge_library.internal.publishers import manager
+from lgsvl_rosbridge_library.internal import ros_loader
+
+from std_msgs.msg import String
+
+from json import dumps, loads
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_publish'
+
+
+class TestPublish(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def test_missing_arguments(self):
+        proto = Protocol("hello")
+        pub = Publish(proto)
+        msg = {"op": "publish"}
+        self.assertRaises(MissingArgumentException, pub.publish, msg)
+
+        msg = {"op": "publish", "msg": {}}
+        self.assertRaises(MissingArgumentException, pub.publish, msg)
+
+    def test_invalid_arguments(self):
+        proto = Protocol("hello")
+        pub = Publish(proto)
+
+        msg = {"op": "publish", "topic": 3}
+        self.assertRaises(InvalidArgumentException, pub.publish, msg)
+
+    def test_publish_works(self):
+        proto = Protocol("hello")
+        pub = Publish(proto)
+        topic = "/test_publish_works"
+        msg = {"data": "test publish works"}
+
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg
+
+        rospy.Subscriber(topic, String, cb)
+
+        pub_msg = loads(dumps({"op": "publish", "topic": topic, "msg": msg}))
+        pub.publish(pub_msg)
+
+        sleep(0.5)
+        self.assertEqual(received["msg"].data, msg["data"])
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestPublish)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_service_capabilities.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_service_capabilities.py
new file mode 100755
index 00000000..8aa94581
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_service_capabilities.py
@@ -0,0 +1,188 @@
+#!/usr/bin/env python
+import rospy
+import rosunit
+import unittest
+
+from json import loads, dumps
+
+from lgsvl_rosbridge_library.capabilities.advertise_service import AdvertiseService
+from lgsvl_rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
+from lgsvl_rosbridge_library.capabilities.call_service import CallService
+from lgsvl_rosbridge_library.capabilities.service_response import ServiceResponse
+from lgsvl_rosbridge_library.protocol import Protocol
+from lgsvl_rosbridge_library.protocol import InvalidArgumentException, MissingArgumentException
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_service_capabilities'
+
+
+class TestServiceCapabilities(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+        self.proto = Protocol(self._testMethodName)
+        # change the log function so we can verify errors are logged
+        self.proto.log = self.mock_log
+        # change the send callback so we can access the rosbridge messages
+        # being sent
+        self.proto.send = self.local_send_cb
+        self.advertise = AdvertiseService(self.proto)
+        self.unadvertise = UnadvertiseService(self.proto)
+        self.response = ServiceResponse(self.proto)
+        self.received_message = None
+        self.log_entries = []
+
+    def local_send_cb(self, msg):
+        self.received_message = msg
+
+    def mock_log(self, loglevel, message, _=None):
+        self.log_entries.append((loglevel, message))
+
+    def test_advertise_missing_arguments(self):
+        advertise_msg = loads(dumps({"op": "advertise_service"}))
+        self.assertRaises(MissingArgumentException,
+                          self.advertise.advertise_service, advertise_msg)
+
+    def test_advertise_invalid_arguments(self):
+        advertise_msg = loads(dumps({"op": "advertise_service",
+                                     "type": 42,
+                                     "service": None}))
+        self.assertRaises(InvalidArgumentException,
+                          self.advertise.advertise_service, advertise_msg)
+
+    def test_response_missing_arguments(self):
+        response_msg = loads(dumps({"op": "service_response"}))
+        self.assertRaises(MissingArgumentException,
+                          self.response.service_response, response_msg)
+
+        # this message has the optional fields, with correct types, but not the
+        # required ones
+        response_msg = loads(dumps({"op": "service_response",
+                                    "id": "dummy_service",
+                                    "values": "none"}))
+        self.assertRaises(MissingArgumentException,
+                          self.response.service_response, response_msg)
+
+    def test_response_invalid_arguments(self):
+        response_msg = loads(dumps({"op": "service_response",
+                                    "service": 5,
+                                    "result": "error"}))
+        self.assertRaises(InvalidArgumentException,
+                          self.response.service_response, response_msg)
+
+    def test_advertise_service(self):
+        service_path = "/set_bool_1"
+        advertise_msg = loads(dumps({"op": "advertise_service",
+                                     "type": "std_srvs/SetBool",
+                                     "service": service_path}))
+        self.advertise.advertise_service(advertise_msg)
+
+        # This throws an exception if the timeout is exceeded (i.e. the service
+        # is not properly advertised)
+        rospy.wait_for_service(service_path, 1.0)
+
+    def test_call_advertised_service(self):
+        service_path = "/set_bool_2"
+        advertise_msg = loads(dumps({"op": "advertise_service",
+                                     "type": "std_srvs/SetBool",
+                                     "service": service_path}))
+        self.advertise.advertise_service(advertise_msg)
+
+        # Call the service via rosbridge because rospy.ServiceProxy.call() is
+        # blocking
+        call_service = CallService(self.proto)
+        call_service.call_service(loads(dumps({"op": "call_service",
+                                               "id": "foo",
+                                               "service": service_path,
+                                               "args": [True]})))
+
+        loop_iterations = 0
+        while self.received_message is None:
+            rospy.sleep(rospy.Duration(0.5))
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("did not receive service call rosbridge message "
+                          "after waiting 2 seconds")
+
+        self.assertFalse(self.received_message is None)
+        self.assertTrue("op" in self.received_message)
+        self.assertTrue(self.received_message["op"] == "call_service")
+        self.assertTrue("id" in self.received_message)
+
+        # Now send the response
+        response_msg = loads(dumps({"op": "service_response",
+                                    "service": service_path,
+                                    "id": self.received_message["id"],
+                                    "values": {"success": True,
+                                               "message": ""},
+                                    "result": True}))
+        self.received_message = None
+        self.response.service_response(response_msg)
+
+        loop_iterations = 0
+        while self.received_message is None:
+            rospy.sleep(rospy.Duration(0.5))
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("did not receive service response rosbridge message "
+                          "after waiting 2 seconds")
+
+        self.assertFalse(self.received_message is None)
+        # Rosbridge should forward the response message to the "client"
+        # (i.e. our custom send function, see setUp())
+        self.assertEqual(self.received_message["op"], "service_response")
+        self.assertTrue(self.received_message["result"])
+
+    def test_unadvertise_with_live_request(self):
+        service_path = "/set_bool_3"
+        advertise_msg = loads(dumps({"op": "advertise_service",
+                                     "type": "std_srvs/SetBool",
+                                     "service": service_path}))
+        self.advertise.advertise_service(advertise_msg)
+
+        # Call the service via rosbridge because rospy.ServiceProxy.call() is
+        # blocking
+        call_service = CallService(self.proto)
+        call_service.call_service(loads(dumps({"op": "call_service",
+                                               "id": "foo",
+                                               "service": service_path,
+                                               "args": [True]})))
+
+        loop_iterations = 0
+        while self.received_message is None:
+            rospy.sleep(rospy.Duration(0.5))
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("did not receive service call rosbridge message "
+                          "after waiting 2 seconds")
+
+        self.assertFalse(self.received_message is None)
+        self.assertTrue("op" in self.received_message)
+        self.assertTrue(self.received_message["op"] == "call_service")
+        self.assertTrue("id" in self.received_message)
+
+        # Now send the response
+        response_msg = loads(dumps({"op": "unadvertise_service",
+                                    "service": service_path}))
+        self.received_message = None
+        self.unadvertise.unadvertise_service(response_msg)
+
+        loop_iterations = 0
+        while self.received_message is None:
+            rospy.sleep(rospy.Duration(0.5))
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("did not receive service response rosbridge message "
+                          "after waiting 2 seconds")
+
+        self.assertFalse(self.received_message is None)
+        # Rosbridge should abort the existing service call with an error
+        # (i.e. "result" should be False)
+        self.assertEqual(self.received_message["op"], "service_response")
+        self.assertFalse(self.received_message["result"])
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestServiceCapabilities)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_subscribe.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_subscribe.py
new file mode 100755
index 00000000..8986c182
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/capabilities/test_subscribe.py
@@ -0,0 +1,125 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+import time
+
+from json import loads, dumps
+from std_msgs.msg import String
+
+from lgsvl_rosbridge_library.capabilities import subscribe
+from lgsvl_rosbridge_library.protocol import Protocol
+from lgsvl_rosbridge_library.protocol import InvalidArgumentException, MissingArgumentException
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_subscribe'
+
+
+class TestSubscribe(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def dummy_cb(self, msg):
+        pass
+
+    def test_update_params(self):
+        """ Adds a bunch of random clients to the subscription and sees whether
+        the correct parameters are chosen as the min """
+        client_id = "client_test_update_params"
+        topic = "/test_update_params"
+        msg_type = "std_msgs/String"
+
+        subscription = subscribe.Subscription(client_id, topic, None)
+
+        min_throttle_rate = 5
+        min_queue_length = 2
+        min_frag_size = 20
+
+        for throttle_rate in range(min_throttle_rate, min_throttle_rate + 10):
+            for queue_length in range(min_queue_length, min_queue_length + 10):
+                for frag_size in range(min_frag_size, min_frag_size + 10):
+                    sid = throttle_rate * 100 + queue_length * 10 + frag_size
+                    subscription.subscribe(sid, msg_type, throttle_rate,
+                                           queue_length, frag_size)
+
+        subscription.update_params()
+
+        try:
+            self.assertEqual(subscription.throttle_rate, min_throttle_rate)
+            self.assertEqual(subscription.queue_length, min_queue_length)
+            self.assertEqual(subscription.fragment_size, min_frag_size)
+            self.assertEqual(subscription.compression, "none")
+
+            list(subscription.clients.values())[0]["compression"] = "png"
+
+            subscription.update_params()
+
+            self.assertEqual(subscription.throttle_rate, min_throttle_rate)
+            self.assertEqual(subscription.queue_length, min_queue_length)
+            self.assertEqual(subscription.fragment_size, min_frag_size)
+            self.assertEqual(subscription.compression, "png")
+        except:
+            subscription.unregister()
+            raise
+
+        subscription.unregister()
+
+    def test_missing_arguments(self):
+        proto = Protocol("test_missing_arguments")
+        sub = subscribe.Subscribe(proto)
+        msg = {"op": "subscribe"}
+        self.assertRaises(MissingArgumentException, sub.subscribe, msg)
+
+    def test_invalid_arguments(self):
+        proto = Protocol("test_invalid_arguments")
+        sub = subscribe.Subscribe(proto)
+
+        msg = {"op": "subscribe", "topic": 3}
+        self.assertRaises(InvalidArgumentException, sub.subscribe, msg)
+
+        msg = {"op": "subscribe", "topic": "/jon", "type": 3}
+        self.assertRaises(InvalidArgumentException, sub.subscribe, msg)
+
+        msg = {"op": "subscribe", "topic": "/jon", "throttle_rate": "fast"}
+        self.assertRaises(InvalidArgumentException, sub.subscribe, msg)
+
+        msg = {"op": "subscribe", "topic": "/jon", "fragment_size": "five cubits"}
+        self.assertRaises(InvalidArgumentException, sub.subscribe, msg)
+
+        msg = {"op": "subscribe", "topic": "/jon", "queue_length": "long"}
+        self.assertRaises(InvalidArgumentException, sub.subscribe, msg)
+
+        msg = {"op": "subscribe", "topic": "/jon", "compression": 9000}
+        self.assertRaises(InvalidArgumentException, sub.subscribe, msg)
+
+    def test_subscribe_works(self):
+        proto = Protocol("test_subscribe_works")
+        sub = subscribe.Subscribe(proto)
+        topic = "/test_subscribe_works"
+        msg = String()
+        msg.data = "test test_subscribe_works works"
+        msg_type = "std_msgs/String"
+
+        received = {"msg": None}
+
+        def send(outgoing, **kwargs):
+            received["msg"] = outgoing
+
+        proto.send = send
+
+        sub.subscribe(loads(dumps({"op": "subscribe", "topic": topic, "type": msg_type})))
+
+        p = rospy.Publisher(topic, String, queue_size=5)
+        time.sleep(0.25)
+        p.publish(msg)
+
+        time.sleep(0.25)
+        self.assertEqual(received["msg"]["msg"]["data"], msg.data)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestSubscribe)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/complex_srv+tcp/test_non-ros_service_client_complex-srv.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/complex_srv+tcp/test_non-ros_service_client_complex-srv.py
new file mode 100755
index 00000000..f414dd79
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/complex_srv+tcp/test_non-ros_service_client_complex-srv.py
@@ -0,0 +1,128 @@
+#!/usr/bin/python
+from __future__ import print_function
+import socket
+from lgsvl_rosbridge_library.util import json
+
+
+
+####################### variables begin ########################################
+# these parameters should be changed to match the actual environment           #
+################################################################################
+
+client_socket_timeout = 6                       # seconds
+max_msg_length = 2000000                        # bytes
+
+rosbridge_ip = "localhost"                      # hostname or ip
+rosbridge_port = 9090                           # port as integer
+
+service_name = "nested_srv"                     # service name
+#request_byte_count = 5000
+receiving_fragment_size = 1000
+receive_message_intervall = 0.0
+
+####################### variables end ##########################################
+
+
+################################################################################
+
+def request_service():
+    service_request_object = { "op" : "call_service",                           # op-code for rosbridge
+                               "service": "/"+service_name,                     # select service
+                               "fragment_size": receiving_fragment_size,        # optional: tells rosbridge to send fragments if message size is bigger than requested
+                               "message_intervall": receive_message_intervall,
+                               "args": { "pose": {"position": {"y": 0.0, "x": 0.0, "z": 0.0}, "orientation": {"y": 0.0, "x": 0.0, "z": 0.0, "w": 0.0}}}
+                                        #"count" : request_byte_count           # count is the parameter for send_bytes as defined in srv-file (always put into args field!)
+                              }
+    service_request = json.dumps(service_request_object)
+    print("sending JSON-message to rosbridge:", service_request)
+    sock.send(service_request)
+
+################################################################################
+
+
+####################### script begin ###########################################
+# should not need to be changed (but could be improved ;) )                    #
+################################################################################
+try:
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)                    #connect to rosbridge
+    sock.settimeout(client_socket_timeout)
+    sock.connect((rosbridge_ip, rosbridge_port))
+
+    request_service()                                                           # send service_request
+
+    incoming = None
+    buffer = ""
+    done = False
+    result = None
+    reconstructed = None
+    while not done:     # should not need a loop (maximum wait can be set by client_socket_timeout), but since its for test/demonstration only .. leave it as it is for now
+        try:
+            incoming = sock.recv(max_msg_length)                                # receive service_response from rosbridge
+            if buffer == "":
+                buffer = incoming
+                if incoming == "":
+                    print("closing socket")
+                    sock.close()
+                    break
+            else:
+                buffer = buffer + incoming
+            #print "buffer-length:", len(buffer)
+            try:                                                                # try to access service_request directly (not fragmented)
+                data_object = json.loads(buffer)
+                if data_object["op"] == "service_response":
+                    reconstructed = buffer
+                    done = True
+            except Exception as e:
+                #print "direct access to JSON failed.."
+                #print e
+                pass
+            try:                                        
+                #print "defragmenting incoming messages"
+                result_string = buffer.split("}{")                              # split buffer into fragments and re-fill curly brackets
+                result = []
+                for fragment in result_string:
+                    if fragment[0] != "{":
+                        fragment = "{"+fragment
+                    if fragment[len(fragment)-1] != "}":
+                        fragment = fragment + "}"
+                    try:
+                        result.append(json.loads(fragment))                     # try to parse json from string, and append if successful
+                    except Exception as e:
+                        #print e
+                        #print result_string
+                        raise                                                   # re-raise the last exception, allows to see and continue with processing of exception
+
+                fragment_count = len(result)
+                print("fragment_count:", fragment_count)
+                announced = int(result[0]["total"])
+                if fragment_count == announced:                                 # if all fragments received --> sort and defragment
+                    # sort fragments
+                    sorted_result = [None] * fragment_count
+                    unsorted_result = []
+                    for fragment in result:
+                        unsorted_result.append(fragment)
+                        sorted_result[int(fragment["num"])] = fragment
+                    reconstructed = ''
+                    for fragment in sorted_result:
+                        reconstructed = reconstructed + fragment["data"]
+                    done = True
+            except Exception as e:
+                #print e
+                pass
+        except Exception as e:
+#            print e
+            pass
+
+
+    returned_data = json.loads(reconstructed)                                   # when service response is received --> access it (as defined in srv-file)
+    if returned_data["values"] == None:
+        print("response was None -> service was not available")
+    else:
+        print("received:")
+        print(returned_data)#["values"]#["data"].decode('base64','strict')         # decode values-field
+    
+except Exception as e:
+    print("ERROR - could not receive service_response")
+    print(e)
+
+sock.close()                                                                    # close socket
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/complex_srv+tcp/test_non-ros_service_server_complex-srv.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/complex_srv+tcp/test_non-ros_service_server_complex-srv.py
new file mode 100755
index 00000000..df15b74a
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/complex_srv+tcp/test_non-ros_service_server_complex-srv.py
@@ -0,0 +1,236 @@
+#!/usr/bin/python
+from __future__ import print_function
+import sys
+import socket
+import time
+from random import randint
+from lgsvl_rosbridge_library.util import json
+
+
+####################### variables begin ########################################
+# these parameters should be changed to match the actual environment           #
+################################################################################
+
+tcp_socket_timeout = 10                          # seconds
+max_msg_length = 20000                           # bytes
+
+rosbridge_ip = "localhost"                       # hostname or ip
+rosbridge_port = 9090                            # port as integer
+
+service_type = "lgsvl_rosbridge_library/TestNestedService"           # make sure this matches an existing service type on rosbridge-server (in specified srv_module)
+service_name = "nested_srv"                      # service name
+
+send_fragment_size = 1000
+# delay between sends to rosbridge is not needed anymore, if using my version of protocol (uses buffer to collect data from stream)
+send_fragment_delay = 0.000#1
+receive_fragment_size = 10
+receive_message_intervall = 0.0
+
+####################### variables end ##########################################
+
+
+####################### service_calculation begin ##############################
+# change this function to match whatever service should be provided            #
+################################################################################
+
+def calculate_service_response(request):
+    request_object = json.loads(request)                                        # parse string for service request
+    args = request_object["args"]                                               # get parameter field (args)
+#    count = int(args["count"] )                                                 # get parameter(s) as described in corresponding ROS srv-file
+#
+#    message = ""                                                                # calculate service response
+#    for i in range(0,count):
+#        message += str(chr(randint(32,126)))
+#        if i% 100000 == 0:
+#            print count - i, "bytes left to generate"
+
+    message = {"data": {"data": 42.0}}
+    
+    """
+    IMPORTANT!
+    use base64 encoding to avoid JSON-parsing problems!
+    --> use .decode("base64","strict") at client side
+    """
+    #message = message.encode('base64','strict')
+    service_response_data = message                                   # service response (as defined in srv-file)
+
+    response_object = { "op": "service_response",
+                        "id": request_object["id"],
+                        "data": service_response_data                           # put service response in "data"-field of response object (in this case it's twice "data", because response value is also named data (in srv-file)
+                      }
+    response_message = json.dumps(response_object)
+    return response_message
+
+####################### service_calculation end ################################
+
+
+
+####################### helper functions / and variables begin #################
+# should not need to be changed (but could be improved )                       #
+################################################################################
+
+buffer = ""
+
+def connect_tcp_socket():
+    tcp_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)                # connect to rosbridge
+    tcp_sock.settimeout(tcp_socket_timeout)
+    tcp_sock.connect((rosbridge_ip, rosbridge_port))
+    return tcp_sock
+
+def advertise_service():                                                        # advertise service
+    advertise_message_object = {"op":"advertise_service",
+                                "type": service_type,
+                                "service": service_name,
+                                "fragment_size": receive_fragment_size,
+                                "message_intervall": receive_message_intervall
+                                }
+    advertise_message = json.dumps(advertise_message_object)                    
+    tcp_socket.send(str(advertise_message))
+
+def unadvertise_service():                                                      # unadvertise service
+    unadvertise_message_object = {"op":"unadvertise_service",
+                                  "service": service_name
+                                 }
+    unadvertise_message = json.dumps(unadvertise_message_object)                   
+    tcp_socket.send(str(unadvertise_message))
+
+def wait_for_service_request():                                                 # receive data from rosbridge
+    data = None
+    global buffer
+
+    try:
+        done = False
+        global buffer
+        while not done:
+            incoming = tcp_socket.recv(max_msg_length)                          # get data from socket
+            if incoming == '':
+                print("connection closed by peer")
+                sys.exit(1)
+            buffer = buffer + incoming                                          # append data to buffer
+            try:                                                                # try to parse JSON from buffer
+                data_object = json.loads(buffer)
+                if data_object["op"] == "call_service":
+                    data = buffer
+                    done = True
+                    return data                                                 # if parsing was successful --> return data string
+            except Exception as e:
+                #print "direct_access error:"
+                #print e
+                pass
+               
+            #print "trying to defragment"
+            try:                                                                # opcode was not "call_service" -> try to defragment
+                result_string = buffer.split("}{")                              # split buffer into fragments and re-fill with curly brackets
+                result = []
+                for fragment in result_string:
+                    if fragment[0] != "{":
+                        fragment = "{"+fragment
+                    if fragment[len(fragment)-1] != "}":
+                        fragment = fragment + "}"
+                    result.append(json.loads(fragment))
+
+                try:                                                            # try to defragment when received all fragments
+                    fragment_count = len(result)
+                    announced = int(result[0]["total"])
+
+                    if fragment_count == announced:
+                        reconstructed = ""
+                        sorted_result = [None] * fragment_count                 # sort fragments..
+                        unsorted_result = []
+                        for fragment in result:
+                            unsorted_result.append(fragment)
+                            sorted_result[int(fragment["num"])] = fragment
+
+                        for fragment in sorted_result:                          # reconstruct from fragments
+                            reconstructed = reconstructed + fragment["data"]
+
+                        #print "reconstructed", reconstructed
+                        buffer = ""                                             # empty buffer
+                        done = True
+                        print("reconstructed message from", len(result), "fragments")
+                        #print reconstructed
+                        return reconstructed
+                except Exception as e:
+                    print("not possible to defragment:", buffer)
+                    print(e)
+            except Exception as e:
+                print("defrag_error:", buffer)
+                print(e)
+                pass
+    except Exception as e:
+        #print "network-error(?):", e
+        pass
+    return data
+
+def send_service_response(response):                                            # send response to rosbridge
+    tcp_socket.send(response)
+
+def list_of_fragments(full_message, fragment_size):                             # create fragment messages for a huge message
+    message_id = randint(0,64000)                                               # generate random message id
+    fragments = []                                                              # generate list of data fragments
+    cursor = 0
+    while cursor < len(full_message):
+        fragment_begin = cursor
+        if len(full_message) < cursor + fragment_size:
+            fragment_end = len(full_message)
+            cursor = len(full_message)
+        else:
+            fragment_end = cursor + fragment_size
+            cursor += fragment_size
+        fragment = full_message[fragment_begin:fragment_end]
+        fragments.append(fragment)
+
+    fragmented_messages_list = []                                               # generate list of fragmented messages (including headers)
+    if len(fragments) > 1:
+        for count, fragment in enumerate(fragments):                            # iterate through list and have index counter
+            fragmented_message_object = {"op":"fragment",                       #   create python-object for each fragment message
+                                         "id": str(message_id),
+                                         "data": str(fragment),
+                                         "num": count,
+                                         "total": len(fragments)
+                                         }
+            fragmented_message = json.dumps(fragmented_message_object)          # create JSON-object from python-object for each fragment message
+            fragmented_messages_list.append(fragmented_message)                 # append JSON-object to list of fragmented messages
+    else:                                                                       # if only 1 fragment --> do not send as fragment, but as service_response
+        fragmented_messages_list.append(str(fragment))
+    return fragmented_messages_list                                             # return list of 'ready-to-send' fragmented messages
+
+####################### helper functions end ###################################
+
+
+####################### script begin ###########################################
+# should not need to be changed (but could be improved )                       #
+################################################################################
+
+tcp_socket = connect_tcp_socket()                                               # open tcp_socket
+advertise_service()                                                             # advertise service in ROS (via rosbridge)
+print("service provider started and waiting for requests")
+
+try:                                                                            # allows to catch KeyboardInterrupt
+    while True:                                                                 # loop forever (or until ctrl-c is pressed)
+        data = None
+        try:                                                                    # allows to catch any Exception (network, json, ..)
+          data = wait_for_service_request()                                     # receive request from rosbridge
+          if data == '':                                                        # exit on empty string
+              break                                                             
+          elif data != None and len(data) > 0:                                  # received service_request (or at least some data..)
+            response = calculate_service_response(data)                         # generate service_response
+
+            print("response calculated, now splitting into fragments..")
+            fragment_list = list_of_fragments(response, send_fragment_size)     # generate fragments to send to rosbridge
+
+            print("sending", len(fragment_list), "messages as response")
+            for fragment in fragment_list:
+                #print "sending:" ,fragment
+                send_service_response(fragment)                                 # send service_response to rosbridge (or fragments; just send any list entry)
+                time.sleep(send_fragment_delay)                                 # (not needed if using patched rosbridge protocol.py)
+        except Exception as e:
+          print(e)
+          pass
+except KeyboardInterrupt:
+    try:
+        unadvertise_service()                                                   # unadvertise service
+        tcp_socket.close()                                                      # close tcp_socket
+    except Exception as e:
+        print(e)
+    print("non-ros_service_server stopped because user pressed \"Ctrl-C\"")
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/fragmentation+srv+tcp/test_non-ros_service_client_fragmented.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/fragmentation+srv+tcp/test_non-ros_service_client_fragmented.py
new file mode 100755
index 00000000..089cec73
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/fragmentation+srv+tcp/test_non-ros_service_client_fragmented.py
@@ -0,0 +1,128 @@
+#!/usr/bin/python
+from __future__ import print_function
+import socket
+from lgsvl_rosbridge_library.util import json
+
+
+
+####################### variables begin ########################################
+# these parameters should be changed to match the actual environment           #
+################################################################################
+
+client_socket_timeout = 6                       # seconds
+max_msg_length = 2000000                        # bytes
+
+rosbridge_ip = "localhost"                      # hostname or ip
+rosbridge_port = 9090                           # port as integer
+
+service_name = "send_bytes"                     # service name
+request_byte_count = 500000                   ## note: receiving more than ~100.000 bytes without setting a fragment_size was not possible during testing..
+receiving_fragment_size = 1000
+receive_message_intervall = 0.0
+
+####################### variables end ##########################################
+
+
+################################################################################
+
+def request_service():
+    service_request_object = { "op" : "call_service",                           # op-code for rosbridge
+                               "service": "/"+service_name,                     # select service
+                               "fragment_size": receiving_fragment_size,        # optional: tells rosbridge to send fragments if message size is bigger than requested
+                               "message_intervall": receive_message_intervall,
+                               "args": { "count" : request_byte_count           # count is the parameter for send_bytes as defined in srv-file (always put into args field!)
+                                        }
+                              }
+    service_request = json.dumps(service_request_object)
+    print("sending JSON-message to rosbridge:", service_request)
+    sock.send(service_request)
+
+################################################################################
+
+
+####################### script begin ###########################################
+# should not need to be changed (but could be improved ;) )                    #
+################################################################################
+try:
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)                    #connect to rosbridge
+    sock.settimeout(client_socket_timeout)
+    sock.connect((rosbridge_ip, rosbridge_port))
+
+    request_service()                                                           # send service_request
+
+    incoming = None
+    buffer = ""
+    done = False
+    result = None
+    reconstructed = None
+    while not done:     # should not need a loop (maximum wait can be set by client_socket_timeout), but since its for test/demonstration only .. leave it as it is for now
+        try:
+            incoming = sock.recv(max_msg_length)                                # receive service_response from rosbridge
+            if buffer == "":
+                buffer = incoming
+                if incoming == "":
+                    print("closing socket")
+                    sock.close()
+                    break
+            else:
+                buffer = buffer + incoming
+            #print "buffer-length:", len(buffer)
+            try:                                                                # try to access service_request directly (not fragmented)
+                data_object = json.loads(buffer)
+                if data_object["op"] == "service_response":
+                    reconstructed = buffer
+                    done = True
+            except Exception as e:
+                #print "direct access to JSON failed.."
+                #print e
+                pass
+            try:                                        
+                #print "defragmenting incoming messages"
+                result_string = buffer.split("}{")                              # split buffer into fragments and re-fill curly brackets
+                result = []
+                for fragment in result_string:
+                    if fragment[0] != "{":
+                        fragment = "{"+fragment
+                    if fragment[len(fragment)-1] != "}":
+                        fragment = fragment + "}"
+                    try:
+                        result.append(json.loads(fragment))                     # try to parse json from string, and append if successful
+                    except Exception as e:
+                        #print e
+                        #print result_string
+                        raise                                                   # re-raise the last exception, allows to see and continue with processing of exception
+
+                fragment_count = len(result)
+                print("fragment_count:", fragment_count)
+                announced = int(result[0]["total"])
+                if fragment_count == announced:                                 # if all fragments received --> sort and defragment
+                    # sort fragments
+                    sorted_result = [None] * fragment_count
+                    unsorted_result = []
+                    for fragment in result:
+                        unsorted_result.append(fragment)
+                        sorted_result[int(fragment["num"])] = fragment
+                    reconstructed = ''
+                    for fragment in sorted_result:
+                        reconstructed = reconstructed + fragment["data"]
+                    done = True
+            except Exception as e:
+                #print e
+                pass
+        except Exception as e:
+#            print e
+            pass
+
+
+    returned_data = json.loads(reconstructed)                                   # when service response is received --> access it (as defined in srv-file)
+    if returned_data["values"] == None:
+        print("response was None -> service was not available")
+    else:
+        print("received:")
+        print(returned_data["values"]["data"].decode('base64','strict'))         # decode values-field
+    
+except Exception as e:
+    print("ERROR - could not receive service_response")
+    print(e)
+
+sock.close()                                                                    # close socket
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/fragmentation+srv+tcp/test_non-ros_service_server_fragmented.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/fragmentation+srv+tcp/test_non-ros_service_server_fragmented.py
new file mode 100755
index 00000000..b5b98930
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/experimental/fragmentation+srv+tcp/test_non-ros_service_server_fragmented.py
@@ -0,0 +1,237 @@
+#!/usr/bin/python
+from __future__ import print_function
+import sys
+import socket
+import time
+from random import randint
+from lgsvl_rosbridge_library.util import json
+
+
+####################### variables begin ########################################
+# these parameters should be changed to match the actual environment           #
+################################################################################
+
+tcp_socket_timeout = 10                          # seconds
+max_msg_length = 20000                           # bytes
+
+rosbridge_ip = "localhost"                       # hostname or ip
+rosbridge_port = 9090                            # port as integer
+
+service_type = "lgsvl_rosbridge_library/SendBytes"                       # make sure this matches an existing service type on rosbridge-server (in specified srv_module)
+service_name = "send_bytes"                      # service name
+
+send_fragment_size = 1000
+# delay between sends to rosbridge is not needed anymore, if using my version of protocol (uses buffer to collect data from stream)
+send_fragment_delay = 0.000#1
+receive_fragment_size = 10
+receive_message_intervall = 0.0
+
+####################### variables end ##########################################
+
+
+####################### service_calculation begin ##############################
+# change this function to match whatever service should be provided            #
+################################################################################
+
+def calculate_service_response(request):
+    request_object = json.loads(request)                                        # parse string for service request
+    args = request_object["args"]                                               # get parameter field (args)
+    count = int(args["count"] )                                                 # get parameter(s) as described in corresponding ROS srv-file
+    
+    message = ""
+    # calculate service response
+    for i in range(0,count):
+        #message += str(chr(randint(32,126)))
+        message+= str(chr(randint(32,126)))
+        if i% 100000 == 0:
+            print(count - i, "bytes left to generate")
+
+    """
+    IMPORTANT!
+    use base64 encoding to avoid JSON-parsing problems!
+    --> use .decode("base64","strict") at client side
+    """
+    message = message.encode('base64','strict')
+    service_response_data = { "data": message}                                  # service response (as defined in srv-file)
+
+    response_object = { "op": "service_response",
+                        "id": request_object["id"],
+                        "service": service_name,
+                        "values": service_response_data                           # put service response in "data"-field of response object (in this case it's twice "data", because response value is also named data (in srv-file)
+                      }
+    response_message = json.dumps(response_object)
+    return response_message
+
+####################### service_calculation end ################################
+
+
+
+####################### helper functions / and variables begin #################
+# should not need to be changed (but could be improved )                       #
+################################################################################
+
+buffer = ""
+
+def connect_tcp_socket():
+    tcp_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)                # connect to rosbridge
+    tcp_sock.settimeout(tcp_socket_timeout)
+    tcp_sock.connect((rosbridge_ip, rosbridge_port))
+    return tcp_sock
+
+def advertise_service():                                                        # advertise service
+    advertise_message_object = {"op":"advertise_service",
+                                "type": service_type,
+                                "service": service_name,
+                                "fragment_size": receive_fragment_size,
+                                "message_intervall": receive_message_intervall
+                                }
+    advertise_message = json.dumps(advertise_message_object)                    
+    tcp_socket.send(str(advertise_message))
+
+def unadvertise_service():                                                      # unadvertise service
+    unadvertise_message_object = {"op":"unadvertise_service",
+                                  "service": service_name
+                                 }
+    unadvertise_message = json.dumps(unadvertise_message_object)                   
+    tcp_socket.send(str(unadvertise_message))
+
+def wait_for_service_request():                                                 # receive data from rosbridge
+    data = None
+    global buffer
+
+    try:
+        done = False
+        global buffer
+        while not done:
+            incoming = tcp_socket.recv(max_msg_length)                          # get data from socket
+            if incoming == '':
+                print("connection closed by peer")
+                sys.exit(1)
+            buffer = buffer + incoming                                          # append data to buffer
+            try:                                                                # try to parse JSON from buffer
+                data_object = json.loads(buffer)
+                if data_object["op"] == "call_service":
+                    data = buffer
+                    done = True
+                    return data                                                 # if parsing was successful --> return data string
+            except Exception as e:
+                #print "direct_access error:"
+                #print e
+                pass
+               
+            #print "trying to defragment"
+            try:                                                                # opcode was not "call_service" -> try to defragment
+                result_string = buffer.split("}{")                              # split buffer into fragments and re-fill with curly brackets
+                result = []
+                for fragment in result_string:
+                    if fragment[0] != "{":
+                        fragment = "{"+fragment
+                    if fragment[len(fragment)-1] != "}":
+                        fragment = fragment + "}"
+                    result.append(json.loads(fragment))
+
+                try:                                                            # try to defragment when received all fragments
+                    fragment_count = len(result)
+                    announced = int(result[0]["total"])
+
+                    if fragment_count == announced:
+                        reconstructed = ""
+                        sorted_result = [None] * fragment_count                 # sort fragments..
+                        unsorted_result = []
+                        for fragment in result:
+                            unsorted_result.append(fragment)
+                            sorted_result[int(fragment["num"])] = fragment
+
+                        for fragment in sorted_result:                          # reconstruct from fragments
+                            reconstructed = reconstructed + fragment["data"]
+
+                        #print "reconstructed", reconstructed
+                        buffer = ""                                             # empty buffer
+                        done = True
+                        print("reconstructed message from", len(result), "fragments")
+                        #print reconstructed
+                        return reconstructed
+                except Exception as e:
+                    print("not possible to defragment:", buffer)
+                    print(e)
+            except Exception as e:
+                print("defrag_error:", buffer)
+                print(e)
+                pass
+    except Exception as e:
+        #print "network-error(?):", e
+        pass
+    return data
+
+def send_service_response(response):                                            # send response to rosbridge
+    tcp_socket.send(response)
+
+def list_of_fragments(full_message, fragment_size):                             # create fragment messages for a huge message
+    message_id = randint(0,64000)                                               # generate random message id
+    fragments = []                                                              # generate list of data fragments
+    cursor = 0
+    while cursor < len(full_message):
+        fragment_begin = cursor
+        if len(full_message) < cursor + fragment_size:
+            fragment_end = len(full_message)
+            cursor = len(full_message)
+        else:
+            fragment_end = cursor + fragment_size
+            cursor += fragment_size
+        fragment = full_message[fragment_begin:fragment_end]
+        fragments.append(fragment)
+
+    fragmented_messages_list = []                                               # generate list of fragmented messages (including headers)
+    if len(fragments) > 1:
+        for count, fragment in enumerate(fragments):                            # iterate through list and have index counter
+            fragmented_message_object = {"op":"fragment",                       #   create python-object for each fragment message
+                                         "id": str(message_id),
+                                         "data": str(fragment),
+                                         "num": count,
+                                         "total": len(fragments)
+                                         }
+            fragmented_message = json.dumps(fragmented_message_object)          # create JSON-object from python-object for each fragment message
+            fragmented_messages_list.append(fragmented_message)                 # append JSON-object to list of fragmented messages
+    else:                                                                       # if only 1 fragment --> do not send as fragment, but as service_response
+        fragmented_messages_list.append(str(fragment))
+    return fragmented_messages_list                                             # return list of 'ready-to-send' fragmented messages
+
+####################### helper functions end ###################################
+
+
+####################### script begin ###########################################
+# should not need to be changed (but could be improved )                       #
+################################################################################
+
+tcp_socket = connect_tcp_socket()                                               # open tcp_socket
+advertise_service()                                                             # advertise service in ROS (via rosbridge)
+print("service provider started and waiting for requests")
+
+try:                                                                            # allows to catch KeyboardInterrupt
+    while True:                                                                 # loop forever (or until ctrl-c is pressed)
+        data = None
+        try:                                                                    # allows to catch any Exception (network, json, ..)
+          data = wait_for_service_request()                                     # receive request from rosbridge
+          if data == '':                                                        # exit on empty string
+              break                                                             
+          elif data != None and len(data) > 0:                                  # received service_request (or at least some data..)
+            response = calculate_service_response(data)                         # generate service_response
+
+            print("response calculated, now splitting into fragments..")
+            fragment_list = list_of_fragments(response, send_fragment_size)     # generate fragments to send to rosbridge
+
+            print("sending", len(fragment_list), "messages as response")
+            for fragment in fragment_list:
+                #print "sending:" ,fragment
+                send_service_response(fragment)                                 # send service_response to rosbridge (or fragments; just send any list entry)
+                time.sleep(send_fragment_delay)                                 # (not needed if using patched rosbridge protocol.py)
+        except Exception as e:
+          print(e)
+          pass
+except KeyboardInterrupt:
+    try:
+        unadvertise_service()                                                   # unadvertise service
+        tcp_socket.close()                                                      # close tcp_socket
+    except Exception as e:
+        print(e)
+    print("non-ros_service_server stopped because user pressed \"Ctrl-C\"")
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/__init__.py
new file mode 100755
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_multi_publisher.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_multi_publisher.py
new file mode 100755
index 00000000..37900162
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_multi_publisher.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+
+from time import sleep, time
+
+from lgsvl_rosbridge_library.internal.publishers import *
+from lgsvl_rosbridge_library.internal.topics import *
+from lgsvl_rosbridge_library.internal import ros_loader
+from lgsvl_rosbridge_library.internal.message_conversion import *
+from std_msgs.msg import String, Int32
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_multi_publisher'
+
+
+class TestMultiPublisher(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def is_topic_published(self, topicname):
+        return topicname in dict(rospy.get_published_topics()).keys()
+
+    def test_register_multipublisher(self):
+        """ Register a publisher on a clean topic with a good msg type """
+        topic = "/test_register_multipublisher"
+        msg_type = "std_msgs/String"
+
+        self.assertFalse(self.is_topic_published(topic))
+        multipublisher = MultiPublisher(topic, msg_type)
+        self.assertTrue(self.is_topic_published(topic))
+
+    def test_unregister_multipublisher(self):
+        """ Register and unregister a publisher on a clean topic with a good msg type """
+        topic = "/test_unregister_multipublisher"
+        msg_type = "std_msgs/String"
+
+        self.assertFalse(self.is_topic_published(topic))
+        multipublisher = MultiPublisher(topic, msg_type)
+        self.assertTrue(self.is_topic_published(topic))
+        multipublisher.unregister()
+        self.assertFalse(self.is_topic_published(topic))
+
+    def test_register_client(self):
+        """ Adds a publisher then removes it.  """
+        topic = "/test_register_client"
+        msg_type = "std_msgs/String"
+        client_id = "client1"
+
+        p = MultiPublisher(topic, msg_type)
+        self.assertFalse(p.has_clients())
+
+        p.register_client(client_id)
+        self.assertTrue(p.has_clients())
+
+        p.unregister_client(client_id)
+        self.assertFalse(p.has_clients())
+
+    def test_register_multiple_clients(self):
+        """ Adds multiple publishers then removes them.  """
+        topic = "/test_register_multiple_clients"
+        msg_type = "std_msgs/String"
+
+        p = MultiPublisher(topic, msg_type)
+        self.assertFalse(p.has_clients())
+
+        for i in range(1000):
+            p.register_client("client%d" % i)
+            self.assertTrue(p.has_clients())
+
+        for i in range(1000):
+            self.assertTrue(p.has_clients())
+            p.unregister_client("client%d" % i)
+
+        self.assertFalse(p.has_clients())
+
+    def test_verify_type(self):
+        topic = "/test_verify_type"
+        msg_type = "std_msgs/String"
+        othertypes = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus",
+        "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage",
+        "nav_msgs/OccupancyGrid", "geometry_msgs/Point32",
+        "trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue",
+        "visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells",
+        "sensor_msgs/PointCloud2"]
+
+        p = MultiPublisher(topic, msg_type)
+        p.verify_type(msg_type)
+        for othertype in othertypes:
+            self.assertRaises(TypeConflictException, p.verify_type, othertype)
+
+    def test_publish(self):
+        """ Make sure that publishing works """
+        topic = "/test_publish"
+        msg_type = "std_msgs/String"
+        msg = {"data": "why halo thar"}
+
+        received = {"msg": None}
+        def cb(msg):
+            received["msg"] = msg
+
+        rospy.Subscriber(topic, ros_loader.get_message_class(msg_type), cb)
+        p = MultiPublisher(topic, msg_type)
+        p.publish(msg)
+
+        sleep(0.5)
+
+        self.assertEqual(received["msg"].data, msg["data"])
+
+    def test_bad_publish(self):
+        """ Make sure that bad publishing fails """
+        topic = "/test_publish"
+        msg_type = "std_msgs/String"
+        msg = {"data": 3}
+
+        p = MultiPublisher(topic, msg_type)
+        self.assertRaises(FieldTypeMismatchException, p.publish, msg)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestMultiPublisher)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_multi_unregistering.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_multi_unregistering.py
new file mode 100755
index 00000000..e280b134
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_multi_unregistering.py
@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+
+import os
+import rospy
+import rosunit
+import unittest
+
+from time import sleep
+
+from lgsvl_rosbridge_library.internal.publishers import MultiPublisher
+from lgsvl_rosbridge_library.internal import ros_loader
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_multi_unregistering'
+
+
+class TestMultiUnregistering(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME, log_level=rospy.DEBUG)
+
+    def test_publish_once(self):
+        """ Make sure that publishing works """
+        topic = "/test_publish_once"
+        msg_type = "std_msgs/String"
+        msg = {"data": "why halo thar"}
+
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg
+
+        rospy.Subscriber(topic, ros_loader.get_message_class(msg_type), cb)
+        p = MultiPublisher(topic, msg_type)
+        p.publish(msg)
+
+        sleep(1)  # Time to publish and receive
+
+        self.assertEqual(received["msg"].data, msg["data"])
+
+    def test_publish_twice(self):
+        """ Make sure that publishing works """
+        topic = "/test_publish_twice"
+        msg_type = "std_msgs/String"
+        msg = {"data": "why halo thar"}
+
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg
+
+        rospy.Subscriber(topic, ros_loader.get_message_class(msg_type), cb)
+        p = MultiPublisher(topic, msg_type)
+        p.publish(msg)
+
+        sleep(1)  # Time to publish and receive
+
+        self.assertEqual(received["msg"].data, msg["data"])
+
+        p.unregister()
+        sleep(5)  # Time to unregister
+
+        received["msg"] = None  # Reset received
+        self.assertIsNone(received["msg"])
+        p = MultiPublisher(topic, msg_type)
+        p.publish(msg)
+
+        sleep(1)   # Time to publish and receive
+
+        self.assertEqual(received["msg"].data, msg["data"])
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestMultiUnregistering)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_publisher_consistency_listener.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_publisher_consistency_listener.py
new file mode 100755
index 00000000..f6927d44
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_publisher_consistency_listener.py
@@ -0,0 +1,179 @@
+#!/usr/bin/env python
+from __future__ import print_function
+import sys
+import rospy
+import rosunit
+import unittest
+
+from time import sleep, time
+
+from lgsvl_rosbridge_library.internal.publishers import *
+from lgsvl_rosbridge_library.internal import ros_loader
+from lgsvl_rosbridge_library.internal.message_conversion import *
+from std_msgs.msg import String, Int32
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_publisher_consistency_listener'
+
+
+class TestPublisherConsistencyListener(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def test_listener_timeout(self):
+        """ See whether the listener can correctly time out """
+        topic = "/test_listener_timeout"
+        type = String
+
+        publisher = rospy.Publisher(topic, type)
+
+        listener = PublisherConsistencyListener()
+        listener.attach(publisher)
+
+        self.assertFalse(listener.timed_out())
+
+        sleep(listener.timeout / 2.0)
+
+        self.assertFalse(listener.timed_out())
+
+        sleep(listener.timeout / 2.0 + 0.1)
+
+        self.assertTrue(listener.timed_out())
+
+    def test_listener_attach_detach(self):
+        """ See whether the listener actually attaches and detaches itself """
+        topic = "/test_listener_attach_detach"
+        type = String
+
+        publisher = rospy.Publisher(topic, type)
+        orig_publish = publisher.publish
+
+        listener = PublisherConsistencyListener()
+        listener_publish = listener.publish_override
+
+        self.assertNotEqual(orig_publish, listener_publish)
+        self.assertNotIn(listener, publisher.impl.subscriber_listeners)
+
+        listener.attach(publisher)
+
+        self.assertEqual(publisher.publish, listener_publish)
+        self.assertNotEqual(publisher.publish, orig_publish)
+        self.assertIn(listener, publisher.impl.subscriber_listeners)
+
+        listener.detach()
+
+        self.assertEqual(publisher.publish, orig_publish)
+        self.assertNotEqual(publisher.publish, listener_publish)
+        self.assertNotIn(listener, publisher.impl.subscriber_listeners)
+
+    def test_immediate_publish_fails_without(self):
+        """ This test makes sure the failure case that the PublisherConsistency
+        Listener is trying to solve, is indeed a failure case """
+        topic = "/test_immediate_publish_fails_without"
+        msg_class = String
+
+        msg = String()
+        string = "why halo thar"
+        msg.data = string
+
+        received = {"msg": None}
+        def callback(msg):
+            received["msg"] = msg
+
+        rospy.Subscriber(topic, msg_class, callback)
+        publisher = rospy.Publisher(topic, msg_class)
+        publisher.publish(msg)
+        sleep(0.5)
+
+        self.assertNotEqual(received["msg"], msg)
+        self.assertEqual(received["msg"], None)
+
+    def test_immediate_publish(self):
+        """ This test makes sure the PublisherConsistencyListener is working"""
+        topic = "/test_immediate_publish"
+        msg_class = String
+
+        msg = String()
+        string = "why halo thar"
+        msg.data = string
+
+        received = {"msg": None}
+        def callback(msg):
+            print("Received a msg! ", msg)
+            received["msg"] = msg
+
+        rospy.Subscriber(topic, msg_class, callback)
+
+        class temp_listener(rospy.SubscribeListener):
+            def peer_subscribe(self, topic_name, topic_publish, peer_publish):
+                print("peer subscribe in temp listener")
+
+        listener = PublisherConsistencyListener()
+        publisher = rospy.Publisher(topic, msg_class, temp_listener())
+        listener.attach(publisher)
+        publisher.publish(msg)
+        sleep(0.5)
+
+        self.assertEqual(received["msg"], msg)
+
+    def test_immediate_multi_publish_fails_without(self):
+        """ This test makes sure the failure case that the PublisherConsistency
+        Listener is trying to solve, is indeed a failure case, even for large
+        message buffers """
+        topic = "/test_immediate_multi_publish_fails_without"
+        msg_class = Int32
+
+        msgs = []
+        for i in range(100):
+            msg = Int32()
+            msg.data = i
+            msgs.append(msg)
+
+        received = {"msgs": []}
+        def callback(msg):
+            received["msgs"].append(msg)
+
+        rospy.Subscriber(topic, msg_class, callback)
+
+        publisher = rospy.Publisher(topic, msg_class)
+        for msg in msgs:
+            publisher.publish(msg)
+        sleep(0.5)
+
+        self.assertEqual(len(received["msgs"]), 0)
+        self.assertNotEqual(received["msgs"], msgs)
+
+    def test_immediate_multi_publish(self):
+        """ This test makes sure the PublisherConsistencyListener is working
+        even with a huge message buffer"""
+        topic = "/test_immediate_multi_publish"
+        msg_class = Int32
+
+        msgs = []
+        for i in range(100):
+            msg = Int32()
+            msg.data = i
+            msgs.append(msg)
+
+        received = {"msgs": []}
+        def callback(msg):
+            received["msgs"].append(msg)
+
+        rospy.Subscriber(topic, msg_class, callback)
+
+        listener = PublisherConsistencyListener()
+        publisher = rospy.Publisher(topic, msg_class)
+        listener.attach(publisher)
+        for msg in msgs:
+            publisher.publish(msg)
+        sleep(0.5)
+
+        self.assertEqual(len(received["msgs"]), len(msgs))
+        self.assertEqual(received["msgs"], msgs)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestPublisherConsistencyListener)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_publisher_manager.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_publisher_manager.py
new file mode 100755
index 00000000..18d3d416
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/publishers/test_publisher_manager.py
@@ -0,0 +1,220 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+
+from time import sleep
+
+from lgsvl_rosbridge_library.internal.publishers import *
+from lgsvl_rosbridge_library.internal.topics import *
+from lgsvl_rosbridge_library.internal.message_conversion import FieldTypeMismatchException
+from std_msgs.msg import String, Int32
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_publisher_manager'
+
+
+class TestPublisherManager(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+        manager.unregister_timeout = 1.0
+
+    def is_topic_published(self, topicname):
+        return topicname in dict(rospy.get_published_topics()).keys()
+
+    def test_register_publisher(self):
+        """ Register a publisher on a clean topic with a good msg type """
+        topic = "/test_register_publisher"
+        msg_type = "std_msgs/String"
+        client = "client_test_register_publisher"
+
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic))
+        manager.register(client, topic, msg_type)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+
+        manager.unregister(client, topic)
+        self.assertTrue(topic in manager.unregister_timers)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+        sleep(manager.unregister_timeout*1.1)
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic))
+        self.assertFalse(topic in manager.unregister_timers)
+
+    def test_register_publisher_multiclient(self):
+        topic = "/test_register_publisher_multiclient"
+        msg_type = "std_msgs/String"
+        client1 = "client_test_register_publisher_1"
+        client2 = "client_test_register_publisher_2"
+
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic))
+        manager.register(client1, topic, msg_type)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+        manager.register(client2, topic, msg_type)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+        manager.unregister(client1, topic)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+        manager.unregister(client2, topic)
+        self.assertTrue(topic in manager.unregister_timers)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+        sleep(manager.unregister_timeout*1.1)
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic))
+        self.assertFalse(topic in manager.unregister_timers)
+
+    def test_register_publisher_conflicting_types(self):
+        topic = "/test_register_publisher_conflicting_types"
+        msg_type = "std_msgs/String"
+        msg_type_bad = "std_msgs/Int32"
+        client = "client_test_register_publisher_conflicting_types"
+
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic))
+        manager.register(client, topic, msg_type)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+
+        self.assertRaises(TypeConflictException, manager.register, "client2", topic, msg_type_bad)
+
+    def test_register_multiple_publishers(self):
+        topic1 = "/test_register_multiple_publishers1"
+        topic2 = "/test_register_multiple_publishers2"
+        msg_type = "std_msgs/String"
+        client = "client_test_register_multiple_publishers"
+
+        self.assertFalse(topic1 in manager._publishers)
+        self.assertFalse(topic2 in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic1))
+        self.assertFalse(self.is_topic_published(topic2))
+        manager.register(client, topic1, msg_type)
+        self.assertTrue(topic1 in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic1))
+        self.assertFalse(topic2 in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic2))
+        manager.register(client, topic2, msg_type)
+        self.assertTrue(topic1 in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic1))
+        self.assertTrue(topic2 in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic2))
+
+        manager.unregister(client, topic1)
+        self.assertTrue(self.is_topic_published(topic1))
+        self.assertTrue(topic1 in manager.unregister_timers)
+        self.assertTrue(topic2 in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic2))
+
+        manager.unregister(client, topic2)
+        self.assertTrue(topic2 in manager.unregister_timers)
+        self.assertTrue(self.is_topic_published(topic2))
+        sleep(manager.unregister_timeout*1.1)
+        self.assertFalse(topic1 in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic1))
+        self.assertFalse(topic2 in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic2))
+        self.assertFalse(topic1 in manager.unregister_timers)
+        self.assertFalse(topic2 in manager.unregister_timers)
+
+    def test_register_no_msgtype(self):
+        topic = "/test_register_no_msgtype"
+        client = "client_test_register_no_msgtype"
+
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic))
+        self.assertRaises(TopicNotEstablishedException, manager.register, client, topic)
+
+    def test_register_infer_topictype(self):
+        topic = "/test_register_infer_topictype"
+        client = "client_test_register_infer_topictype"
+
+        self.assertFalse(self.is_topic_published(topic))
+
+        rospy.Publisher(topic, String)
+
+        self.assertTrue(self.is_topic_published(topic))
+        self.assertFalse(topic in manager._publishers)
+        manager.register(client, topic)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+
+        manager.unregister(client, topic)
+        self.assertTrue(topic in manager.unregister_timers)
+        self.assertTrue(self.is_topic_published(topic))
+
+    def test_register_multiple_notopictype(self):
+        topic = "/test_register_multiple_notopictype"
+        msg_type = "std_msgs/String"
+        client1 = "client_test_register_multiple_notopictype_1"
+        client2 = "client_test_register_multiple_notopictype_2"
+
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(topic in manager.unregister_timers)
+        self.assertFalse(self.is_topic_published(topic))
+        manager.register(client1, topic, msg_type)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+        manager.register(client2, topic)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(self.is_topic_published(topic))
+        manager.unregister(client1, topic)
+        self.assertTrue(topic in manager._publishers)
+        self.assertTrue(topic in manager.unregister_timers)
+        self.assertTrue(self.is_topic_published(topic))
+        manager.unregister(client2, topic)
+        self.assertTrue(topic in manager.unregister_timers)
+        self.assertTrue(topic in manager._publishers)
+        sleep(manager.unregister_timeout*1.1)
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(topic in manager.unregister_timers)
+        self.assertFalse(self.is_topic_published(topic))
+
+    def test_publish_not_registered(self):
+        topic = "/test_publish_not_registered"
+        msg = {"data": "test publish not registered"}
+        client = "client_test_publish_not_registered"
+
+        self.assertFalse(topic in manager._publishers)
+        self.assertFalse(self.is_topic_published(topic))
+        self.assertRaises(TopicNotEstablishedException, manager.publish, client, topic, msg)
+
+    def test_publisher_manager_publish(self):
+        """ Make sure that publishing works """
+        topic = "/test_publisher_manager_publish"
+        msg = {"data": "test publisher manager publish"}
+        client = "client_test_publisher_manager_publish"
+
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg
+
+        rospy.Subscriber(topic, String, cb)
+        manager.publish(client, topic, msg)
+        sleep(0.5)
+
+        self.assertEqual(received["msg"].data, msg["data"])
+
+    def test_publisher_manager_bad_publish(self):
+        """ Make sure that bad publishing fails """
+        topic = "/test_publisher_manager_bad_publish"
+        client = "client_test_publisher_manager_bad_publish"
+        msg_type = "std_msgs/String"
+        msg = {"data": 3}
+
+        manager.register(client, topic, msg_type)
+        self.assertRaises(FieldTypeMismatchException, manager.publish, client, topic, msg)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestPublisherManager)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/__init__.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_multi_subscriber.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_multi_subscriber.py
new file mode 100755
index 00000000..45d049fe
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_multi_subscriber.py
@@ -0,0 +1,190 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+from rosgraph import Master
+
+from time import sleep, time
+
+from lgsvl_rosbridge_library.internal.subscribers import *
+from lgsvl_rosbridge_library.internal.topics import *
+from lgsvl_rosbridge_library.internal import ros_loader
+from lgsvl_rosbridge_library.internal.message_conversion import *
+from std_msgs.msg import String, Int32
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_multi_subscriber'
+
+
+class TestMultiSubscriber(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def is_topic_published(self, topicname):
+        return topicname in dict(rospy.get_published_topics()).keys()
+
+    def is_topic_subscribed(self, topicname):
+        return topicname in dict(Master("test_multi_subscriber").getSystemState()[1])
+
+    def test_register_multisubscriber(self):
+        """ Register a subscriber on a clean topic with a good msg type """
+        topic = "/test_register_multisubscriber"
+        msg_type = "std_msgs/String"
+
+        self.assertFalse(self.is_topic_subscribed(topic))
+        MultiSubscriber(topic, msg_type)
+        self.assertTrue(self.is_topic_subscribed(topic))
+
+    def test_unregister_multisubscriber(self):
+        """ Register and unregister a subscriber on a clean topic with a good msg type """
+        topic = "/test_unregister_multisubscriber"
+        msg_type = "std_msgs/String"
+
+        self.assertFalse(self.is_topic_subscribed(topic))
+        multi = MultiSubscriber(topic, msg_type)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        multi.unregister()
+        self.assertFalse(self.is_topic_subscribed(topic))
+
+    def test_verify_type(self):
+        topic = "/test_verify_type"
+        msg_type = "std_msgs/String"
+        othertypes = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus",
+        "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage",
+        "nav_msgs/OccupancyGrid", "geometry_msgs/Point32",
+        "trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue",
+        "visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells",
+        "sensor_msgs/PointCloud2"]
+
+        s = MultiSubscriber(topic, msg_type)
+        s.verify_type(msg_type)
+        for othertype in othertypes:
+            self.assertRaises(TypeConflictException, s.verify_type, othertype)
+
+    def test_subscribe_unsubscribe(self):
+        topic = "/test_subscribe_unsubscribe"
+        msg_type = "std_msgs/String"
+        client = "client_test_subscribe_unsubscribe"
+
+        self.assertFalse(self.is_topic_subscribed(topic))
+        multi = MultiSubscriber(topic, msg_type)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        self.assertFalse(multi.has_subscribers())
+
+        multi.subscribe(client, None)
+        self.assertTrue(multi.has_subscribers())
+
+        multi.unsubscribe(client)
+        self.assertFalse(multi.has_subscribers())
+
+        multi.unregister()
+        self.assertFalse(self.is_topic_subscribed(topic))
+
+    def test_subscribe_receive_json(self):
+        topic = "/test_subscribe_receive_json"
+        msg_type = "std_msgs/String"
+        client = "client_test_subscribe_receive_json"
+
+        msg = String()
+        msg.data = "dsajfadsufasdjf"
+
+        pub = rospy.Publisher(topic, String)
+        multi = MultiSubscriber(topic, msg_type)
+
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg.get_json_values()
+
+        multi.subscribe(client, cb)
+        sleep(0.5)
+        pub.publish(msg)
+        sleep(0.5)
+        self.assertEqual(msg.data, received["msg"]["data"])
+
+    def test_subscribe_receive_json_multiple(self):
+        topic = "/test_subscribe_receive_json_multiple"
+        msg_type = "std_msgs/Int32"
+        client = "client_test_subscribe_receive_json_multiple"
+
+        numbers = list(range(100))
+
+        pub = rospy.Publisher(topic, Int32)
+        multi = MultiSubscriber(topic, msg_type)
+
+        received = {"msgs": []}
+
+        def cb(msg):
+            received["msgs"].append(msg.get_json_values()["data"])
+
+        multi.subscribe(client, cb)
+        sleep(0.5)
+        for x in numbers:
+            msg = Int32()
+            msg.data = x
+            pub.publish(msg)
+        sleep(0.5)
+        self.assertEqual(numbers, received["msgs"])
+
+    def test_unsubscribe_does_not_receive_further_msgs(self):
+        topic = "/test_unsubscribe_does_not_receive_further_msgs"
+        msg_type = "std_msgs/String"
+        client = "client_test_unsubscribe_does_not_receive_further_msgs"
+
+        msg = String()
+        msg.data = "dsajfadsufasdjf"
+
+        pub = rospy.Publisher(topic, String)
+        multi = MultiSubscriber(topic, msg_type)
+
+        received = {"count": 0}
+
+        def cb(msg):
+            received["count"] = received["count"] + 1
+
+        multi.subscribe(client, cb)
+        sleep(0.5)
+        pub.publish(msg)
+        sleep(0.5)
+        self.assertEqual(received["count"], 1)
+        multi.unsubscribe(client)
+        sleep(0.5)
+        pub.publish(msg)
+        sleep(0.5)
+        self.assertEqual(received["count"], 1)
+
+    def test_multiple_subscribers(self):
+        topic = "/test_subscribe_receive_json"
+        msg_type = "std_msgs/String"
+        client1 = "client_test_subscribe_receive_json_1"
+        client2 = "client_test_subscribe_receive_json_2"
+
+        msg = String()
+        msg.data = "dsajfadsufasdjf"
+
+        pub = rospy.Publisher(topic, String)
+        multi = MultiSubscriber(topic, msg_type)
+
+        received = {"msg1": None, "msg2": None}
+
+        def cb1(msg):
+            received["msg1"] = msg.get_json_values()
+
+        def cb2(msg):
+            received["msg2"] = msg.get_json_values()
+
+        multi.subscribe(client1, cb1)
+        multi.subscribe(client2, cb2)
+        sleep(0.5)
+        pub.publish(msg)
+        sleep(0.5)
+        self.assertEqual(msg.data, received["msg1"]["data"])
+        self.assertEqual(msg.data, received["msg2"]["data"])
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestMultiSubscriber)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_subscriber_manager.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_subscriber_manager.py
new file mode 100755
index 00000000..04539cc3
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_subscriber_manager.py
@@ -0,0 +1,193 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+from rosgraph import Master
+
+from time import sleep
+
+from lgsvl_rosbridge_library.internal.subscribers import *
+from lgsvl_rosbridge_library.internal.topics import *
+from lgsvl_rosbridge_library.internal.message_conversion import FieldTypeMismatchException
+from std_msgs.msg import String, Int32
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_subscriber_manager'
+
+
+class TestSubscriberManager(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def is_topic_published(self, topicname):
+        return topicname in dict(rospy.get_published_topics()).keys()
+
+    def is_topic_subscribed(self, topicname):
+        return topicname in dict(Master("test_subscriber_manager").getSystemState()[1])
+
+    def test_subscribe(self):
+        """ Register a publisher on a clean topic with a good msg type """
+        topic = "/test_subscribe"
+        msg_type = "std_msgs/String"
+        client = "client_test_subscribe"
+
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+        manager.subscribe(client, topic, None, msg_type)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+
+        manager.unsubscribe(client, topic)
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+
+    def test_register_subscriber_multiclient(self):
+        topic = "/test_register_subscriber_multiclient"
+        msg_type = "std_msgs/String"
+        client1 = "client_test_register_subscriber_multiclient_1"
+        client2 = "client_test_register_subscriber_multiclient_2"
+
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+        manager.subscribe(client1, topic, None, msg_type)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        manager.subscribe(client2, topic, None, msg_type)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        manager.unsubscribe(client1, topic)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        manager.unsubscribe(client2, topic)
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+
+    def test_register_publisher_conflicting_types(self):
+        topic = "/test_register_publisher_conflicting_types"
+        msg_type = "std_msgs/String"
+        msg_type_bad = "std_msgs/Int32"
+        client = "client_test_register_publisher_conflicting_types"
+
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+        manager.subscribe(client, topic, None, msg_type)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+
+        self.assertRaises(TypeConflictException, manager.subscribe, "client2", topic, None, msg_type_bad)
+
+    def test_register_multiple_publishers(self):
+        topic1 = "/test_register_multiple_publishers1"
+        topic2 = "/test_register_multiple_publishers2"
+        msg_type = "std_msgs/String"
+        client = "client_test_register_multiple_publishers"
+
+        self.assertFalse(topic1 in manager._subscribers)
+        self.assertFalse(topic2 in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic1))
+        self.assertFalse(self.is_topic_subscribed(topic2))
+        manager.subscribe(client, topic1, None, msg_type)
+        self.assertTrue(topic1 in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic1))
+        self.assertFalse(topic2 in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic2))
+        manager.subscribe(client, topic2, None, msg_type)
+        self.assertTrue(topic1 in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic1))
+        self.assertTrue(topic2 in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic2))
+
+        manager.unsubscribe(client, topic1)
+        self.assertFalse(topic1 in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic1))
+        self.assertTrue(topic2 in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic2))
+
+        manager.unsubscribe(client, topic2)
+        self.assertFalse(topic1 in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic1))
+        self.assertFalse(topic2 in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic2))
+
+    def test_register_no_msgtype(self):
+        topic = "/test_register_no_msgtype"
+        client = "client_test_register_no_msgtype"
+
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+        self.assertRaises(TopicNotEstablishedException, manager.subscribe, client, topic, None)
+
+    def test_register_infer_topictype(self):
+        topic = "/test_register_infer_topictype"
+        client = "client_test_register_infer_topictype"
+
+        self.assertFalse(self.is_topic_subscribed(topic))
+
+        rospy.Subscriber(topic, String, None)
+
+        self.assertTrue(self.is_topic_subscribed(topic))
+        self.assertFalse(topic in manager._subscribers)
+        manager.subscribe(client, topic, None)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+
+        manager.unsubscribe(client, topic)
+        self.assertFalse(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+
+    def test_register_multiple_notopictype(self):
+        topic = "/test_register_multiple_notopictype"
+        msg_type = "std_msgs/String"
+        client1 = "client_test_register_multiple_notopictype_1"
+        client2 = "client_test_register_multiple_notopictype_2"
+
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+        manager.subscribe(client1, topic, None, msg_type)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        manager.subscribe(client2, topic, None)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        manager.unsubscribe(client1, topic)
+        self.assertTrue(topic in manager._subscribers)
+        self.assertTrue(self.is_topic_subscribed(topic))
+        manager.unsubscribe(client2, topic)
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+
+    def test_subscribe_not_registered(self):
+        topic = "/test_subscribe_not_registered"
+        client = "client_test_subscribe_not_registered"
+
+        self.assertFalse(topic in manager._subscribers)
+        self.assertFalse(self.is_topic_subscribed(topic))
+        self.assertRaises(TopicNotEstablishedException, manager.subscribe, client, topic, None)
+
+    def test_publisher_manager_publish(self):
+        topic = "/test_publisher_manager_publish"
+        msg_type = "std_msgs/String"
+        client = "client_test_publisher_manager_publish"
+
+        msg = String()
+        msg.data = "dsajfadsufasdjf"
+
+        pub = rospy.Publisher(topic, String)
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg.get_json_values()
+
+        manager.subscribe(client, topic, cb, msg_type)
+        sleep(0.5)
+        pub.publish(msg)
+        sleep(0.5)
+        self.assertEqual(msg.data, received["msg"]["data"])
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestSubscriberManager)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_subscription_modifiers.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_subscription_modifiers.py
new file mode 100755
index 00000000..c7e85e9c
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/subscribers/test_subscription_modifiers.py
@@ -0,0 +1,381 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+import time
+
+from lgsvl_rosbridge_library.internal import subscription_modifiers as subscribe
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_message_handlers'
+
+
+class TestMessageHandlers(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def dummy_cb(self, msg):
+        pass
+
+    def test_default_message_handler(self):
+        handler = subscribe.MessageHandler(None, self.dummy_cb)
+        self.help_test_default(handler)
+
+    def test_throttle_message_handler(self):
+        handler = subscribe.ThrottleMessageHandler(subscribe.MessageHandler(None, self.dummy_cb))
+        self.help_test_throttle(handler, 50)
+
+    def test_queue_message_handler_passes_msgs(self):
+        handler = subscribe.QueueMessageHandler(subscribe.MessageHandler(None, self.dummy_cb))
+        self.help_test_queue(handler, 1000)
+        handler.finish()
+
+    def test_queue_message_handler_stops(self):
+        received = {"msgs": []}
+
+        def cb(msg):
+            received["msgs"].append(msg)
+
+        handler = subscribe.QueueMessageHandler(subscribe.MessageHandler(None, cb))
+
+        self.assertTrue(handler.is_alive())
+
+        handler.finish()
+
+        self.assertFalse(handler.is_alive())
+
+    def test_queue_message_handler_queue(self):
+        received = {"msgs": []}
+
+        def cb(msg):
+            received["msgs"].append(msg)
+
+        msgs = range(1000)
+
+        handler = subscribe.MessageHandler(None, cb)
+
+        handler = handler.set_throttle_rate(10000)
+        handler = handler.set_queue_length(10)
+        self.assertIsInstance(handler, subscribe.QueueMessageHandler)
+
+        # 'hello' is handled immediately
+        handler.handle_message("hello")
+        time.sleep(0.02)
+        # queue is now empty, but throttling is in effect
+        # no messages will be handled in the next 10 seconds
+
+        # these will fill up the queue, with newer values displacing old ones
+        # nothing gets sent because the throttle rate
+        for x in msgs:
+            handler.handle_message(x)
+
+        handler = handler.set_throttle_rate(0)
+
+        time.sleep(0.1)
+
+        try:
+            self.assertEqual(["hello"] + list(range(990, 1000)), received["msgs"])
+        except:
+            handler.finish()
+            raise
+
+        handler.finish()
+
+    def test_queue_message_handler_dropping(self):
+        received = {"msgs": []}
+
+        def cb(msg):
+            received["msgs"].append(msg)
+            time.sleep(1)
+
+        queue_length = 5
+        msgs = list(range(queue_length * 5))
+
+        handler = subscribe.MessageHandler(None, cb)
+
+        handler = handler.set_queue_length(queue_length)
+        self.assertIsInstance(handler, subscribe.QueueMessageHandler)
+
+        # yield the thread to let QueueMessageHandler reach wait().
+        time.sleep(0.001)
+
+        # send all messages at once.
+        # only the first and the last queue_length should get through,
+        # because the callbacks are blocked.
+        for x in msgs:
+            handler.handle_message(x)
+            # yield the thread so the first callback can append,
+            # otherwise the first handled value is non-deterministic.
+            time.sleep(0.001)
+
+        # wait long enough for all the callbacks, and then some.
+        time.sleep(queue_length + 3)
+
+        try:
+            self.assertEqual([msgs[0]] + msgs[-queue_length:], received["msgs"])
+        except:
+            handler.finish()
+            raise
+
+        handler.finish()
+
+    def test_queue_message_handler_rate(self):
+        handler = subscribe.MessageHandler(None, self.dummy_cb)
+        self.help_test_queue_rate(handler, 50, 10)
+        handler.finish()
+
+    # Helper methods for each of the three Handler types, plus one for Queue+Rate.
+    # Used in standalone testing as well as the test_transition_functionality test
+    def help_test_default(self, handler):
+        handler = handler.set_queue_length(0)
+        handler = handler.set_throttle_rate(0)
+        self.assertIsInstance(handler, subscribe.MessageHandler)
+
+        msg = "test_default_message_handler"
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg
+        handler.publish = cb
+
+        self.assertTrue(handler.time_remaining() == 0)
+        t1 = time.time()
+        handler.handle_message(msg)
+        t2 = time.time()
+
+        self.assertEqual(received["msg"], msg)
+        self.assertLessEqual(t1, handler.last_publish)
+        self.assertLessEqual(handler.last_publish, t2)
+        self.assertEqual(handler.time_remaining(), 0)
+
+        received = {"msgs": []}
+        def cb(msg):
+            received["msgs"].append(msg)
+        handler.publish = cb
+        xs = list(range(10000))
+        for x in xs:
+            handler.handle_message(x)
+
+        self.assertEqual(received["msgs"], xs)
+        return handler
+
+    def help_test_throttle(self, handler, throttle_rate):
+        handler = handler.set_queue_length(0)
+        handler = handler.set_throttle_rate(throttle_rate)
+        self.assertIsInstance(handler, subscribe.ThrottleMessageHandler)
+
+        msg = "test_throttle_message_handler"
+
+        # First, try with a single message
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg
+
+        handler.publish = cb
+
+        # ensure the handler doesn't swallow this message
+        time.sleep(2.0 * handler.throttle_rate)
+        handler.handle_message(msg)
+        self.assertEqual(received["msg"], msg)
+
+        # sleep to make sure the handler sends right away for the second part
+        time.sleep(2.0 * handler.throttle_rate)
+
+        received = {"msgs": []}
+        def cb(msg):
+            received["msgs"].append(msg)
+
+        handler.publish = cb
+        x = 0
+        time_padding = handler.throttle_rate / 4.0
+        for i in range(1, 10):
+            # We guarantee that in the while loop below only the first message is handled
+            # All subsequent messages (within throttling window - time_padding ) are dropped
+            # Time padding is a test-only hack around race condition when time.time() - fin is within
+            # the throttling window, but handler.handle_message(x) gets a later timestamp that is outside.
+            time.sleep(2.0 * time_padding)
+            fin = time.time() + throttle_rate / 1000.0 - time_padding
+            while time.time() < fin:
+                handler.handle_message(x)
+                x = x + 1
+            self.assertEqual(len(received["msgs"]), i)
+        return handler
+
+    def help_test_queue(self, handler, queue_length):
+        handler = handler.set_queue_length(queue_length)
+        self.assertIsInstance(handler, subscribe.QueueMessageHandler)
+
+        received = {"msgs": []}
+
+        def cb(msg):
+            received["msgs"].append(msg)
+
+        handler.publish = cb
+
+        msgs = list(range(queue_length))
+        for x in msgs:
+            handler.handle_message(x)
+
+        time.sleep(0.1)
+
+        self.assertEqual(msgs, received["msgs"])
+        return handler
+
+    def help_test_queue_rate(self, handler, throttle_rate, queue_length):
+        handler = handler.set_throttle_rate(throttle_rate)
+        handler = handler.set_queue_length(queue_length)
+        self.assertIsInstance(handler, subscribe.QueueMessageHandler)
+
+        received = {"msg": None}
+
+        def cb(msg):
+            received["msg"] = msg
+
+        handler.publish = cb
+
+        throttle_rate_sec = throttle_rate / 1000.0
+
+        # ensure previous tests' last sent time is long enough ago
+        time.sleep(throttle_rate_sec)
+        for x in range(queue_length):
+            handler.handle_message(x)
+
+        time.sleep(throttle_rate_sec / 2.0)
+
+        try:
+            for x in range(10):
+                self.assertEqual(x, received["msg"])
+                time.sleep(throttle_rate_sec)
+        except:
+            handler.finish()
+            raise
+
+        return handler
+
+# Test that each transition works and is stable
+    def test_transitions(self):
+        # MessageHandler.transition is stable
+        handler = subscribe.MessageHandler(None, self.dummy_cb)
+        next_handler = handler.transition()
+        self.assertEqual(handler, next_handler)
+
+        # Going from MessageHandler to ThrottleMessageHandler...
+        handler = subscribe.MessageHandler(None, self.dummy_cb)
+        next_handler = handler.set_throttle_rate(100)
+        self.assertIsInstance(next_handler, subscribe.ThrottleMessageHandler)
+        handler = next_handler
+        # Testing transition returns another ThrottleMessageHandler
+        next_handler = handler.transition()
+        self.assertEqual(handler, next_handler)
+        # And finally going back to MessageHandler
+        next_handler = handler.set_throttle_rate(0)
+        self.assertIsInstance(next_handler, subscribe.MessageHandler)
+
+        # Same for QueueMessageHandler
+        handler = subscribe.MessageHandler(None, self.dummy_cb)
+        next_handler = handler.set_queue_length(100)
+        self.assertIsInstance(next_handler, subscribe.QueueMessageHandler)
+        handler = next_handler
+        next_handler = handler.transition()
+        self.assertEqual(handler, next_handler)
+        next_handler = handler.set_queue_length(0)
+        self.assertIsInstance(next_handler, subscribe.MessageHandler)
+
+        # Checking a QueueMessageHandler with rate limit can be generated both ways
+        handler = subscribe.MessageHandler(None, self.dummy_cb)
+        next_handler = handler.set_queue_length(100).set_throttle_rate(100)
+        self.assertIsInstance(next_handler, subscribe.QueueMessageHandler)
+        next_handler.finish()
+        next_handler = handler.set_throttle_rate(100).set_queue_length(100)
+        self.assertIsInstance(next_handler, subscribe.QueueMessageHandler)
+        next_handler.finish()
+        handler = next_handler
+        next_handler = handler.transition()
+        self.assertEqual(handler, next_handler)
+        # Check both steps on the way back to plain MessageHandler
+        next_handler = handler.set_throttle_rate(0)
+        self.assertIsInstance(next_handler, subscribe.QueueMessageHandler)
+        next_handler = handler.set_queue_length(0)
+        self.assertIsInstance(next_handler, subscribe.MessageHandler)
+
+    def test_transition_functionality(self):
+        # Test individually
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_queue(handler, 10)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_throttle(handler, 50)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_default(handler)
+        handler.finish()
+
+        # Test combinations
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_queue(handler, 10)
+        handler = self.help_test_throttle(handler, 50)
+        handler = self.help_test_default(handler)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_queue(handler, 10)
+        handler = self.help_test_default(handler)
+        handler = self.help_test_throttle(handler, 50)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_throttle(handler, 50)
+        handler = self.help_test_queue_rate(handler, 50, 10)
+        handler = self.help_test_default(handler)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_throttle(handler, 50)
+        handler = self.help_test_default(handler)
+        handler = self.help_test_queue_rate(handler, 50, 10)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_default(handler)
+        handler = self.help_test_throttle(handler, 50)
+        handler = self.help_test_queue_rate(handler, 50, 10)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_default(handler)
+        handler = self.help_test_queue(handler, 10)
+        handler = self.help_test_throttle(handler, 50)
+        handler.finish()
+
+        # Test duplicates
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_queue_rate(handler, 50, 10)
+        handler = self.help_test_queue_rate(handler, 100, 10)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_throttle(handler, 50)
+        handler = self.help_test_throttle(handler, 100)
+        handler.finish()
+
+        handler = subscribe.MessageHandler(None, None)
+        handler = self.help_test_default(handler)
+        handler = self.help_test_default(handler)
+        handler.finish()
+
+
+#        handler = self.help_test_throttle(handler, 50)
+#        handler = self.help_test_default(handler)
+#        handler = self.help_test_throttle(handler, 50)
+#        handler = self.help_test_default(handler)
+#        handler = self.help_test_throttle(handler, 50)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestMessageHandlers)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_cbor_conversion.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_cbor_conversion.py
new file mode 100755
index 00000000..ece76567
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_cbor_conversion.py
@@ -0,0 +1,162 @@
+#!/usr/bin/env python
+import rosunit
+import sys
+import unittest
+
+PYTHON2 = sys.version_info < (3, 0)
+
+import struct
+from lgsvl_rosbridge_library.internal.cbor_conversion import extract_cbor_values, TAGGED_ARRAY_FORMATS
+
+try:
+    from cbor import Tag
+except ImportError:
+    from lgsvl_rosbridge_library.util.cbor import Tag
+
+from std_msgs.msg import (
+    Bool, String,
+    Byte, Char,
+    Int8, Int16, Int32, Int64,
+    UInt8, UInt16, UInt32, UInt64,
+    Float32, Float64,
+    ByteMultiArray,
+    Int8MultiArray, Int16MultiArray, Int32MultiArray, Int64MultiArray,
+    UInt8MultiArray, UInt16MultiArray, UInt32MultiArray, UInt64MultiArray,
+    Float32MultiArray, Float64MultiArray,
+    Time, Duration,
+    Empty,
+    MultiArrayLayout, MultiArrayDimension,
+)
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_cbor_conversion'
+
+
+class TestCBORConversion(unittest.TestCase):
+    def test_string(self):
+        msg = String(data="foo")
+        extracted = extract_cbor_values(msg)
+
+        self.assertEqual(extracted['data'], msg.data)
+        if PYTHON2:
+            self.assertEqual(type(extracted['data']), unicode)  # noqa: F821
+        else:
+            self.assertEqual(type(extracted['data']), str)
+
+    def test_bool(self):
+        for val in [True, False]:
+            msg = Bool(data=val)
+            extracted = extract_cbor_values(msg)
+
+            self.assertEqual(extracted['data'], msg.data, 'val={}'.format(val))
+            self.assertEqual(type(extracted['data']), bool, 'val={}'.format(val))
+
+    def test_numbers(self):
+        for msg_type in [Int8, Int16, Int32, Int64]:
+            msg = msg_type(data=-5)
+            extracted = extract_cbor_values(msg)
+
+            self.assertEqual(extracted['data'], msg.data, 'type={}'.format(msg_type))
+            self.assertEqual(type(extracted['data']), int, 'type={}'.format(msg_type))
+
+        for msg_type in [UInt8, UInt16, UInt32, UInt64]:
+            msg = msg_type(data=5)
+            extracted = extract_cbor_values(msg)
+
+            self.assertEqual(extracted['data'], msg.data, 'type={}'.format(msg_type))
+            self.assertEqual(type(extracted['data']), int, 'type={}'.format(msg_type))
+
+        for msg_type in [Float32, Float64]:
+            msg = msg_type(data=2.3)
+            extracted = extract_cbor_values(msg)
+
+            self.assertEqual(extracted['data'], msg.data, 'type={}'.format(msg_type))
+            self.assertEqual(type(extracted['data']), float, 'type={}'.format(msg_type))
+
+    def test_time(self):
+        for msg_type in [Time, Duration]:
+            msg = msg_type()
+            extracted = extract_cbor_values(msg)
+
+            self.assertEqual(extracted['data']['secs'], msg.data.secs, 'type={}'.format(msg_type))
+            self.assertEqual(extracted['data']['nsecs'], msg.data.nsecs, 'type={}'.format(msg_type))
+            self.assertEqual(type(extracted['data']['secs']), int, 'type={}'.format(msg_type))
+            self.assertEqual(type(extracted['data']['nsecs']), int, 'type={}'.format(msg_type))
+
+    def test_byte_array(self):
+        msg = UInt8MultiArray(data=[0, 1, 2])
+        extracted = extract_cbor_values(msg)
+
+        data = extracted['data']
+        self.assertEqual(type(data), bytes)
+        for i, val in enumerate(msg.data):
+            if PYTHON2:
+                self.assertEqual(ord(data[i]), val)
+            else:
+                self.assertEqual(data[i], val)
+
+    def test_numeric_array(self):
+        for msg_type in [Int8MultiArray, Int16MultiArray, Int32MultiArray, Int64MultiArray,
+                         UInt16MultiArray, UInt32MultiArray, UInt64MultiArray,
+                         Float32MultiArray, Float64MultiArray]:
+            msg = msg_type(data=[0, 1, 2])
+            extracted = extract_cbor_values(msg)
+
+            tag = extracted['data']
+            self.assertEqual(type(tag), Tag, 'type={}'.format(msg_type))
+            self.assertEqual(type(tag.value), bytes, 'type={}'.format(msg_type))
+
+            # This is as consistent as the message defs..
+            array_type = msg._slot_types[1]
+
+            expected_tag = TAGGED_ARRAY_FORMATS[array_type][0]
+            self.assertEqual(tag.tag, expected_tag, 'type={}'.format(msg_type))
+
+            fmt = TAGGED_ARRAY_FORMATS[array_type][1]
+            fmt_to_length = fmt.format(len(msg.data))
+            unpacked = list(struct.unpack(fmt_to_length, tag.value))
+
+            self.assertEqual(unpacked, msg.data, 'type={}'.format(msg_type))
+
+    def test_nested_messages(self):
+        msg = UInt8MultiArray(layout=MultiArrayLayout(
+            data_offset=5,
+            dim=[
+                MultiArrayDimension(
+                    label="foo",
+                    size=4,
+                    stride=4,
+                ),
+                MultiArrayDimension(
+                    label="bar",
+                    size=8,
+                    stride=8,
+                ),
+            ]
+        ))
+        extracted = extract_cbor_values(msg)
+
+        ex_layout = extracted['layout']
+        self.assertEqual(type(ex_layout), dict)
+        self.assertEqual(ex_layout['data_offset'], msg.layout.data_offset)
+        self.assertEqual(len(ex_layout['dim']), len(msg.layout.dim))
+        for i, val in enumerate(msg.layout.dim):
+            self.assertEqual(ex_layout['dim'][i]['label'], val.label)
+            self.assertEqual(ex_layout['dim'][i]['size'], val.size)
+            self.assertEqual(ex_layout['dim'][i]['stride'], val.stride)
+
+    def test_unicode_keys(self):
+        msg = String(data="foo")
+        extracted = extract_cbor_values(msg)
+
+        keys = extracted.keys()
+        for key in keys:
+            if PYTHON2:
+                self.assertEqual(type(key), unicode)  # noqa: F821
+            else:
+                self.assertEqual(type(key), str)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestCBORConversion)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_compression.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_compression.py
new file mode 100755
index 00000000..88921866
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_compression.py
@@ -0,0 +1,36 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+
+from lgsvl_rosbridge_library.internal import pngcompression
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_compression'
+
+
+class TestCompression(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def test_compress(self):
+        bytes = list(range(128)) * 10000
+        string = str(bytearray(bytes))
+        encoded = pngcompression.encode(string)
+        self.assertNotEqual(string, encoded)
+
+    def test_compress_decompress(self):
+        bytes = list(range(128)) * 10000
+        string = str(bytearray(bytes))
+        encoded = pngcompression.encode(string)
+        self.assertNotEqual(string, encoded)
+        decoded = pngcompression.decode(encoded)
+        self.assertEqual(string, decoded)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestCompression)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_internal.test b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_internal.test
new file mode 100644
index 00000000..e8675d42
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_internal.test
@@ -0,0 +1,14 @@
+<launch>
+  <test test-name="test_ros_loader" pkg="lgsvl_rosbridge_library" type="test_ros_loader.py" />
+  <test test-name="test_message_conversion" pkg="lgsvl_rosbridge_library" type="test_message_conversion.py" />
+  <test test-name="test_services" pkg="lgsvl_rosbridge_library" type="test_services.py" />
+  <test test-name="test_publisher_consistency_listener" pkg="lgsvl_rosbridge_library" type="test_publisher_consistency_listener.py" />
+  <test test-name="test_multi_publisher" pkg="lgsvl_rosbridge_library" type="test_multi_publisher.py" />
+  <test test-name="test_publisher_manager" pkg="lgsvl_rosbridge_library" type="test_publisher_manager.py" />
+  <test test-name="test_multi_unregistering" pkg="lgsvl_rosbridge_library" type="test_multi_unregistering.py" />
+  <test test-name="test_multi_subscriber" pkg="lgsvl_rosbridge_library" type="test_multi_subscriber.py" />
+  <test test-name="test_subscriber_manager" pkg="lgsvl_rosbridge_library" type="test_subscriber_manager.py" />
+  <test test-name="test_subscription_modifiers" pkg="lgsvl_rosbridge_library" type="test_subscription_modifiers.py" time-limit="120" />
+  <test test-name="test_cbor_conversion" pkg="lgsvl_rosbridge_library" type="test_cbor_conversion.py" />
+  <test test-name="test_outgoing_message" pkg="lgsvl_rosbridge_library" type="test_outgoing_message.py" />
+</launch>
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_message_conversion.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_message_conversion.py
new file mode 100755
index 00000000..e719ef7c
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_message_conversion.py
@@ -0,0 +1,286 @@
+#!/usr/bin/env python
+from __future__ import print_function
+import sys
+import rospy
+import rosunit
+import unittest
+from json import loads, dumps
+
+try:
+    from cStringIO import StringIO  # Python 2.x
+except ImportError:
+    from io import BytesIO as StringIO  # Python 3.x
+
+from lgsvl_rosbridge_library.internal import message_conversion as c
+from lgsvl_rosbridge_library.internal import ros_loader
+from base64 import standard_b64encode, standard_b64decode
+
+if sys.version_info >= (3, 0):
+    string_types = (str,)
+else:
+    string_types = (str, unicode)  # noqa: F821
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_message_conversion'
+
+
+class TestMessageConversion(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def validate_instance(self, inst1):
+        """ Serializes and deserializes the inst to typecheck and ensure that
+        instances are correct """
+        # _check_types() skipped because: https://github.com/ros/genpy/issues/122
+        #inst1._check_types()
+        buff = StringIO()
+        inst1.serialize(buff)
+        inst2 = type(inst1)()
+        inst2.deserialize(buff.getvalue())
+        self.assertEqual(inst1, inst2)
+        #inst2._check_types()
+
+    def msgs_equal(self, msg1, msg2):
+        if type(msg1) in string_types and type(msg2) in string_types:
+            pass
+        else:
+            self.assertEqual(type(msg1), type(msg2))
+        if type(msg1) in c.list_types:
+            for x, y in zip(msg1, msg2):
+                self.msgs_equal(x, y)
+        elif type(msg1) in c.primitive_types or type(msg1) in c.string_types:
+            self.assertEqual(msg1, msg2)
+        else:
+            for x in msg1:
+                self.assertTrue(x in msg2)
+            for x in msg2:
+                self.assertTrue(x in msg1)
+            for x in msg1:
+                self.msgs_equal(msg1[x], msg2[x])
+
+    def do_primitive_test(self, data_value, msgtype):
+        for msg in [{"data": data_value}, loads(dumps({"data": data_value}))]:
+            inst = ros_loader.get_message_instance(msgtype)
+            c.populate_instance(msg, inst)
+            self.assertEqual(inst.data, data_value)
+            self.validate_instance(inst)
+            extracted = c.extract_values(inst)
+            for msg2 in [extracted, loads(dumps(extracted))]:
+                self.msgs_equal(msg, msg2)
+                self.assertEqual(msg["data"], msg2["data"])
+                self.assertEqual(msg2["data"], inst.data)
+
+    def do_test(self, orig_msg, msgtype):
+        for msg in [orig_msg, loads(dumps(orig_msg))]:
+            inst = ros_loader.get_message_instance(msgtype)
+            c.populate_instance(msg, inst)
+            self.validate_instance(inst)
+            extracted = c.extract_values(inst)
+            for msg2 in [extracted, loads(dumps(extracted))]:
+                self.msgs_equal(msg, msg2)
+
+    def test_int_primitives(self):
+        # Test raw primitives
+        for msg in range(-100, 100):
+            for rostype in ["int8", "int16", "int32", "int64"]:
+                self.assertEqual(c._to_primitive_inst(msg, rostype, rostype, []), msg)
+                self.assertEqual(c._to_inst(msg, rostype, rostype), msg)
+        # Test raw primitives
+        for msg in range(0, 200):
+            for rostype in ["uint8", "uint16", "uint32", "uint64"]:
+                self.assertEqual(c._to_primitive_inst(msg, rostype, rostype, []), msg)
+                self.assertEqual(c._to_inst(msg, rostype, rostype), msg)
+
+    def test_bool_primitives(self):
+        self.assertTrue(c._to_primitive_inst(True, "bool", "bool", []))
+        self.assertTrue(c._to_inst(True, "bool", "bool"))
+        self.assertFalse(c._to_primitive_inst(False, "bool", "bool", []))
+        self.assertFalse(c._to_inst(False, "bool", "bool"))
+
+    def test_float_primitives(self):
+        for msg in [0.12341234 + i for i in range(-100, 100)]:
+            for rostype in ["float32", "float64"]:
+                self.assertEqual(c._to_primitive_inst(msg, rostype, rostype, []), msg)
+                self.assertEqual(c._to_inst(msg, rostype, rostype), msg)
+                c._to_inst(msg, rostype, rostype)
+
+    def test_float_special_cases(self):
+        for msg in [1e9999999, -1e9999999, float('nan')]:
+            for rostype in ["float32", "float64"]:
+                self.assertEqual(c._from_inst(msg, rostype), None)
+                self.assertEqual(dumps({"data":c._from_inst(msg, rostype)}), "{\"data\": null}")
+
+    def test_signed_int_base_msgs(self):
+        int8s = range(-127, 128)
+        for int8 in int8s:
+            self.do_primitive_test(int8, "std_msgs/Byte")
+            self.do_primitive_test(int8, "std_msgs/Int8")
+            self.do_primitive_test(int8, "std_msgs/Int16")
+            self.do_primitive_test(int8, "std_msgs/Int32")
+            self.do_primitive_test(int8, "std_msgs/Int64")
+
+        int16s = [-32767, 32767]
+        for int16 in int16s:
+            self.do_primitive_test(int16, "std_msgs/Int16")
+            self.do_primitive_test(int16, "std_msgs/Int32")
+            self.do_primitive_test(int16, "std_msgs/Int64")
+            self.assertRaises(Exception, self.do_primitive_test, int16, "std_msgs/Byte")
+            self.assertRaises(Exception, self.do_primitive_test, int16, "std_msgs/Int8")
+
+        int32s = [-2147483647, 2147483647]
+        for int32 in int32s:
+            self.do_primitive_test(int32, "std_msgs/Int32")
+            self.do_primitive_test(int32, "std_msgs/Int64")
+            self.assertRaises(Exception, self.do_primitive_test, int32, "std_msgs/Byte")
+            self.assertRaises(Exception, self.do_primitive_test, int32, "std_msgs/Int8")
+            self.assertRaises(Exception, self.do_primitive_test, int32, "std_msgs/Int16")
+
+        int64s = [-9223372036854775807, 9223372036854775807]
+        for int64 in int64s:
+            self.do_primitive_test(int64, "std_msgs/Int64")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/Byte")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/Int8")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/Int16")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/Int32")
+
+    def test_unsigned_int_base_msgs(self):
+        int8s = range(0, 256)
+        for int8 in int8s:
+            self.do_primitive_test(int8, "std_msgs/Char")
+            self.do_primitive_test(int8, "std_msgs/UInt8")
+            self.do_primitive_test(int8, "std_msgs/UInt16")
+            self.do_primitive_test(int8, "std_msgs/UInt32")
+            self.do_primitive_test(int8, "std_msgs/UInt64")
+
+        int16s = [32767, 32768, 65535]
+        for int16 in int16s:
+            self.do_primitive_test(int16, "std_msgs/UInt16")
+            self.do_primitive_test(int16, "std_msgs/UInt32")
+            self.do_primitive_test(int16, "std_msgs/UInt64")
+            self.assertRaises(Exception, self.do_primitive_test, int16, "std_msgs/Char")
+            self.assertRaises(Exception, self.do_primitive_test, int16, "std_msgs/UInt8")
+
+        int32s = [2147483647, 2147483648, 4294967295]
+        for int32 in int32s:
+            self.do_primitive_test(int32, "std_msgs/UInt32")
+            self.do_primitive_test(int32, "std_msgs/UInt64")
+            self.assertRaises(Exception, self.do_primitive_test, int32, "std_msgs/Char")
+            self.assertRaises(Exception, self.do_primitive_test, int32, "std_msgs/UInt8")
+            self.assertRaises(Exception, self.do_primitive_test, int32, "std_msgs/UInt16")
+
+        int64s = [4294967296, 9223372036854775807, 9223372036854775808,
+                   18446744073709551615]
+        for int64 in int64s:
+            self.do_primitive_test(int64, "std_msgs/UInt64")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/Char")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/UInt8")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/UInt16")
+            self.assertRaises(Exception, self.do_primitive_test, int64, "std_msgs/UInt32")
+
+    def test_bool_base_msg(self):
+        self.do_primitive_test(True, "std_msgs/Bool")
+        self.do_primitive_test(False, "std_msgs/Bool")
+
+    def test_string_base_msg(self):
+        for x in c.ros_primitive_types:
+            self.do_primitive_test(x, "std_msgs/String")
+
+    def test_time_msg(self):
+        msg = {"data": {"secs": 3, "nsecs": 5}}
+        self.do_test(msg, "std_msgs/Time")
+
+        msg = {"times": [{"secs": 3, "nsecs": 5}, {"secs": 2, "nsecs": 7}]}
+        self.do_test(msg, "lgsvl_rosbridge_library/TestTimeArray")
+
+    def test_time_msg_now(self):
+        msg = {"data": "now"}
+        msgtype = "std_msgs/Time"
+
+        inst = ros_loader.get_message_instance(msgtype)
+        c.populate_instance(msg, inst)
+        currenttime = rospy.get_rostime()
+        self.validate_instance(inst)
+        extracted = c.extract_values(inst)
+        print(extracted)
+        self.assertIn("data", extracted)
+        self.assertIn("secs", extracted["data"])
+        self.assertIn("nsecs", extracted["data"])
+        self.assertNotEqual(extracted["data"]["secs"], 0)
+        self.assertLessEqual(extracted["data"]["secs"], currenttime.secs)
+        self.assertGreaterEqual(currenttime.secs, extracted["data"]["secs"])
+
+    def test_duration_msg(self):
+        msg = {"data": {"secs": 3, "nsecs": 5}}
+        self.do_test(msg, "std_msgs/Duration")
+
+        msg = {"durations": [{"secs": 3, "nsecs": 5}, {"secs": 2, "nsecs": 7}]}
+        self.do_test(msg, "lgsvl_rosbridge_library/TestDurationArray")
+
+    def test_header_msg(self):
+        msg = {"seq": 5, "stamp": {"secs": 12347, "nsecs": 322304}, "frame_id": "2394dnfnlcx;v[p234j]"}
+        self.do_test(msg, "std_msgs/Header")
+
+        msg = {"header": msg}
+        self.do_test(msg, "lgsvl_rosbridge_library/TestHeader")
+        self.do_test(msg, "lgsvl_rosbridge_library/TestHeaderTwo")
+
+        msg = {"header": [msg["header"], msg["header"], msg["header"]]}
+        msg["header"][1]["seq"] = 6
+        msg["header"][2]["seq"] = 7
+        self.do_test(msg, "lgsvl_rosbridge_library/TestHeaderArray")
+
+    def test_assorted_msgs(self):
+        assortedmsgs = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus",
+        "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage",
+        "nav_msgs/OccupancyGrid", "geometry_msgs/Point32", "std_msgs/String",
+        "trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue",
+        "visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells",
+        "sensor_msgs/PointCloud2"]
+        for rostype in assortedmsgs:
+            inst = ros_loader.get_message_instance(rostype)
+            msg = c.extract_values(inst)
+            self.do_test(msg, rostype)
+            l = loads(dumps(msg))
+            inst2 = ros_loader.get_message_instance(rostype)
+            c.populate_instance(msg, inst2)
+            self.assertEqual(inst, inst2)
+
+    def test_int8array(self):
+        def test_int8_msg(rostype, data):
+            msg = {"data": data}
+            inst = ros_loader.get_message_instance(rostype)
+            c.populate_instance(msg, inst)
+            self.validate_instance(inst)
+            return inst.data
+
+        for msgtype in ["TestChar", "TestUInt8"]:
+            rostype = "lgsvl_rosbridge_library/" + msgtype
+
+            int8s = list(range(0, 256))
+            ret = test_int8_msg(rostype, int8s)
+            self.assertEqual(ret, bytes(bytearray(int8s)))
+
+            str_int8s = bytes(bytearray(int8s))
+
+            b64str_int8s = standard_b64encode(str_int8s).decode('ascii')
+            ret = test_int8_msg(rostype, b64str_int8s)
+            self.assertEqual(ret, str_int8s)
+
+        for msgtype in ["TestUInt8FixedSizeArray16"]:
+            rostype = "lgsvl_rosbridge_library/" + msgtype
+
+            int8s = list(range(0, 16))
+            ret = test_int8_msg(rostype, int8s)
+            self.assertEqual(ret, bytes(bytearray(int8s)))
+
+            str_int8s = bytes(bytearray(int8s))
+
+            b64str_int8s = standard_b64encode(str_int8s).decode('ascii')
+            ret = test_int8_msg(rostype, b64str_int8s)
+            self.assertEqual(ret, str_int8s)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestMessageConversion)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_outgoing_message.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_outgoing_message.py
new file mode 100755
index 00000000..a0ef510d
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_outgoing_message.py
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+import rosunit
+import sys
+import unittest
+
+from lgsvl_rosbridge_library.internal.outgoing_message import OutgoingMessage
+from std_msgs.msg import String
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_outgoing_message'
+
+
+class TestOutgoingMessage(unittest.TestCase):
+    def test_json_values(self):
+        msg = String(data="foo")
+        outgoing = OutgoingMessage(msg)
+
+        result = outgoing.get_json_values()
+        self.assertEqual(result['data'], msg.data)
+
+        again = outgoing.get_json_values()
+        self.assertTrue(result is again)
+
+    def test_cbor_values(self):
+        msg = String(data="foo")
+        outgoing = OutgoingMessage(msg)
+
+        result = outgoing.get_cbor_values()
+        self.assertEqual(result['data'], msg.data)
+
+        again = outgoing.get_cbor_values()
+        self.assertTrue(result is again)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestOutgoingMessage)
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_ros_loader.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_ros_loader.py
new file mode 100755
index 00000000..9d63fa9f
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_ros_loader.py
@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+import sys
+import rospy
+import rosunit
+import unittest
+
+from lgsvl_rosbridge_library.internal import ros_loader
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_ros_loader'
+
+
+class TestROSLoader(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def test_bad_msgnames(self):
+        bad = ["", "/", "//", "///", "////", "/////", "bad", "stillbad",
+       "not/better/still", "not//better//still", "not///better///still",
+       "better/", "better//", "better///", "/better", "//better", "///better",
+       "this\isbad", "\\"]
+        for x in bad:
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_message_class, x)
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_message_instance, x)
+
+    def test_irregular_msgnames(self):
+        irregular = ["std_msgs//String", "//std_msgs/String",
+         "/std_msgs//String", "/std_msgs/String", "//std_msgs//String",
+         "/std_msgs/String/", "//std_msgs//String//", "std_msgs/String/",
+         "std_msgs//String//"]
+        for x in irregular:
+            self.assertNotEqual(ros_loader.get_message_class(x), None)
+            self.assertNotEqual(ros_loader.get_message_instance(x), None)
+
+    def test_std_msgnames(self):
+        stdmsgs = ["std_msgs/Bool", "std_msgs/Byte", "std_msgs/ByteMultiArray",
+        "std_msgs/ColorRGBA", "std_msgs/Duration", "std_msgs/Empty",
+        "std_msgs/Float32", "std_msgs/Float32MultiArray", "std_msgs/Float64",
+        "std_msgs/Header", "std_msgs/Int16", "std_msgs/Int16MultiArray",
+        "std_msgs/Int32", "std_msgs/Int32MultiArray", "std_msgs/Int64",
+        "std_msgs/Int64MultiArray", "std_msgs/Int8", "std_msgs/Int8MultiArray",
+        "std_msgs/MultiArrayDimension", "std_msgs/MultiArrayLayout",
+        "std_msgs/String", "std_msgs/Time", "std_msgs/UInt16",
+        "std_msgs/UInt16MultiArray", "std_msgs/UInt32MultiArray",
+        "std_msgs/UInt64MultiArray", "std_msgs/UInt32", "std_msgs/UInt64",
+        "std_msgs/UInt8", "std_msgs/UInt8MultiArray"]
+        for x in stdmsgs:
+            self.assertNotEqual(ros_loader.get_message_class(x), None)
+            inst = ros_loader.get_message_instance(x)
+            self.assertNotEqual(inst, None)
+            self.assertEqual(x, inst._type)
+
+    def test_msg_cache(self):
+        stdmsgs = ["std_msgs/Bool", "std_msgs/Byte", "std_msgs/ByteMultiArray",
+        "std_msgs/ColorRGBA", "std_msgs/Duration", "std_msgs/Empty",
+        "std_msgs/Float32", "std_msgs/Float32MultiArray", "std_msgs/Float64",
+        "std_msgs/Header", "std_msgs/Int16", "std_msgs/Int16MultiArray",
+        "std_msgs/Int32", "std_msgs/Int32MultiArray", "std_msgs/Int64",
+        "std_msgs/Int64MultiArray", "std_msgs/Int8", "std_msgs/Int8MultiArray",
+        "std_msgs/MultiArrayDimension", "std_msgs/MultiArrayLayout",
+        "std_msgs/String", "std_msgs/Time", "std_msgs/UInt16",
+        "std_msgs/UInt16MultiArray", "std_msgs/UInt32MultiArray",
+        "std_msgs/UInt64MultiArray", "std_msgs/UInt32", "std_msgs/UInt64",
+        "std_msgs/UInt8", "std_msgs/UInt8MultiArray"]
+        for x in stdmsgs:
+            self.assertNotEqual(ros_loader.get_message_class(x), None)
+            inst = ros_loader.get_message_instance(x)
+            self.assertNotEqual(inst, None)
+            self.assertEqual(x, inst._type)
+            self.assertTrue(x in ros_loader._loaded_msgs)
+
+    def test_assorted_msgnames(self):
+        assortedmsgs = ["geometry_msgs/Pose", "actionlib_msgs/GoalStatus",
+        "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage",
+        "nav_msgs/OccupancyGrid", "geometry_msgs/Point32", "std_msgs/String",
+        "trajectory_msgs/JointTrajectoryPoint", "diagnostic_msgs/KeyValue",
+        "visualization_msgs/InteractiveMarkerUpdate", "nav_msgs/GridCells",
+        "sensor_msgs/PointCloud2"]
+        for x in assortedmsgs:
+            self.assertNotEqual(ros_loader.get_message_class(x), None)
+            inst = ros_loader.get_message_instance(x)
+            self.assertNotEqual(inst, None)
+            self.assertEqual(x, inst._type)
+
+    def test_invalid_msgnames_primitives(self):
+        invalid = ["bool", "int8", "uint8", "int16", "uint16", "int32",
+        "uint32", "int64", "uint64", "float32", "float64", "string", "time",
+        "duration"]
+        for x in invalid:
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_message_class, x)
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_message_instance, x)
+
+    def test_nonexistent_packagenames(self):
+        nonexistent = ["wangle_msgs/Jam", "whistleblower_msgs/Document",
+        "sexual_harrassment_msgs/UnwantedAdvance", "coercion_msgs/Bribe",
+        "airconditioning_msgs/Cold", "pr2thoughts_msgs/Escape"]
+        for x in nonexistent:
+            self.assertRaises(ros_loader.InvalidPackageException,
+                              ros_loader.get_message_class, x)
+            self.assertRaises(ros_loader.InvalidPackageException,
+                              ros_loader.get_message_instance, x)
+
+    def test_packages_without_msgs(self):
+        no_msgs = ["roslib/Time", "roslib/Duration", "roslib/Header",
+        "std_srvs/ConflictedMsg", "topic_tools/MessageMessage"]
+        for x in no_msgs:
+            self.assertRaises(ros_loader.InvalidModuleException,
+                              ros_loader.get_message_class, x)
+            self.assertRaises(ros_loader.InvalidModuleException,
+                              ros_loader.get_message_instance, x)
+
+    def test_nonexistent_msg_classnames(self):
+        nonexistent = ["roscpp/Time", "roscpp/Duration", "roscpp/Header",
+        "rospy/Time", "rospy/Duration", "rospy/Header", "std_msgs/Spool",
+        "geometry_msgs/Tetrahedron", "sensor_msgs/TelepathyUnit"]
+        for x in nonexistent:
+            self.assertRaises(ros_loader.InvalidClassException,
+                              ros_loader.get_message_class, x)
+            self.assertRaises(ros_loader.InvalidClassException,
+                              ros_loader.get_message_instance, x)
+
+    def test_bad_servicenames(self):
+        bad = ["", "/", "//", "///", "////", "/////", "bad", "stillbad",
+       "not/better/still", "not//better//still", "not///better///still",
+       "better/", "better//", "better///", "/better", "//better", "///better",
+       "this\isbad", "\\"]
+        for x in bad:
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_service_class, x)
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_service_instance, x)
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_service_request_instance, x)
+            self.assertRaises(ros_loader.InvalidTypeStringException,
+                              ros_loader.get_service_response_instance, x)
+
+    def test_irregular_servicenames(self):
+        irregular = ["roscpp//GetLoggers", "/roscpp/GetLoggers/",
+        "/roscpp/GetLoggers", "//roscpp/GetLoggers", "/roscpp//GetLoggers",
+        "roscpp/GetLoggers//", "/roscpp/GetLoggers//", "roscpp/GetLoggers/",
+        "roscpp//GetLoggers//"]
+        for x in irregular:
+            self.assertNotEqual(ros_loader.get_service_class(x), None)
+            self.assertNotEqual(ros_loader.get_service_instance(x), None)
+            self.assertNotEqual(ros_loader.get_service_request_instance(x), None)
+            self.assertNotEqual(ros_loader.get_service_response_instance(x), None)
+
+    def test_common_servicenames(self):
+        common = ["roscpp/GetLoggers", "roscpp/SetLoggerLevel",
+        "std_srvs/Empty", "nav_msgs/GetMap", "nav_msgs/GetPlan",
+        "sensor_msgs/SetCameraInfo", "topic_tools/MuxAdd",
+        "topic_tools/MuxSelect", "tf2_msgs/FrameGraph",
+        "rospy_tutorials/BadTwoInts", "rospy_tutorials/AddTwoInts"]
+        for x in common:
+            self.assertNotEqual(ros_loader.get_service_class(x), None)
+            self.assertNotEqual(ros_loader.get_service_instance(x), None)
+            self.assertNotEqual(ros_loader.get_service_request_instance(x), None)
+            self.assertNotEqual(ros_loader.get_service_response_instance(x), None)
+            self.assertEqual(x, ros_loader.get_service_instance(x)._type)
+
+    def test_srv_cache(self):
+        common = ["roscpp/GetLoggers", "roscpp/SetLoggerLevel",
+        "std_srvs/Empty", "nav_msgs/GetMap", "nav_msgs/GetPlan",
+        "sensor_msgs/SetCameraInfo", "topic_tools/MuxAdd",
+        "topic_tools/MuxSelect", "tf2_msgs/FrameGraph",
+        "rospy_tutorials/BadTwoInts", "rospy_tutorials/AddTwoInts"]
+        for x in common:
+            self.assertNotEqual(ros_loader.get_service_class(x), None)
+            self.assertNotEqual(ros_loader.get_service_instance(x), None)
+            self.assertNotEqual(ros_loader.get_service_request_instance(x), None)
+            self.assertNotEqual(ros_loader.get_service_response_instance(x), None)
+            self.assertTrue(x in ros_loader._loaded_srvs)
+
+    def test_packages_without_srvs(self):
+        no_msgs = ["roslib/A", "roslib/B", "roslib/C",
+        "std_msgs/CuriousSrv"]
+        for x in no_msgs:
+            self.assertRaises(ros_loader.InvalidModuleException,
+                              ros_loader.get_service_class, x)
+            self.assertRaises(ros_loader.InvalidModuleException,
+                              ros_loader.get_service_instance, x)
+            self.assertRaises(ros_loader.InvalidModuleException,
+                              ros_loader.get_service_request_instance, x)
+            self.assertRaises(ros_loader.InvalidModuleException,
+                              ros_loader.get_service_response_instance, x)
+
+    def test_nonexistent_service_packagenames(self):
+        nonexistent = ["butler_srvs/FetchDrink", "money_srvs/MoreMoney",
+        "snoopdogg_srvs/SipOnGinAndJuice", "revenge_srvs/BackStab"]
+        for x in nonexistent:
+            self.assertRaises(ros_loader.InvalidPackageException,
+                              ros_loader.get_service_class, x)
+            self.assertRaises(ros_loader.InvalidPackageException,
+                              ros_loader.get_service_instance, x)
+            self.assertRaises(ros_loader.InvalidPackageException,
+                              ros_loader.get_service_request_instance, x)
+            self.assertRaises(ros_loader.InvalidPackageException,
+                              ros_loader.get_service_response_instance, x)
+
+    def test_nonexistent_service_classnames(self):
+        nonexistent = ["std_srvs/KillAllHumans", "std_srvs/Full",
+        "rospy_tutorials/SubtractTwoInts", "nav_msgs/LoseMap",
+        "topic_tools/TellMeWhatThisTopicIsActuallyAbout"]
+        for x in nonexistent:
+            self.assertRaises(ros_loader.InvalidClassException,
+                              ros_loader.get_service_class, x)
+            self.assertRaises(ros_loader.InvalidClassException,
+                              ros_loader.get_service_instance, x)
+            self.assertRaises(ros_loader.InvalidClassException,
+                              ros_loader.get_service_request_instance, x)
+            self.assertRaises(ros_loader.InvalidClassException,
+                              ros_loader.get_service_response_instance, x)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestROSLoader)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_services.py b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_services.py
new file mode 100755
index 00000000..45298f73
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/internal/test_services.py
@@ -0,0 +1,213 @@
+#!/usr/bin/env python
+from __future__ import print_function
+import sys
+import rospy
+import rosunit
+import unittest
+import time
+import random
+
+from lgsvl_rosbridge_library.internal import services, ros_loader
+from lgsvl_rosbridge_library.internal import message_conversion as c
+from lgsvl_rosbridge_library.internal.message_conversion import FieldTypeMismatchException
+
+from roscpp.srv import GetLoggers
+
+if sys.version_info >= (3, 0):
+    string_types = (str,)
+else:
+    string_types = (str, unicode)  # noqa: F821
+
+
+PKG = 'lgsvl_rosbridge_library'
+NAME = 'test_services'
+
+
+def populate_random_args(d):
+    # Given a dictionary d, replaces primitives with random values
+    if isinstance(d, dict):
+        for x in d:
+            d[x] = populate_random_args(d[x])
+        return d
+    elif isinstance(d, str):
+        return str(random.random())
+    elif sys.version_info < (3,0) and isinstance(d, unicode):  # noqa: F821
+        return unicode(random.random())  # noqa: F821
+    elif isinstance(d, bool):
+        return True
+    elif isinstance(d, int):
+        return random.randint(100, 200)
+    elif isinstance(d, float):
+        return 3.5
+    else:
+        return d
+
+
+class ServiceTester:
+
+    def __init__(self, name, srv_type):
+        self.name = name
+        self.srvClass = ros_loader.get_service_class(srv_type)
+        self.service = rospy.Service(name, self.srvClass, self.callback)
+
+    def start(self):
+        req = self.srvClass._request_class()
+        gen = c.extract_values(req)
+        gen = populate_random_args(gen)
+        self.input = gen
+        services.ServiceCaller(self.name, gen, self.success, self.error).start()
+
+    def callback(self, req):
+        self.req = req
+        time.sleep(0.5)
+        rsp = self.srvClass._response_class()
+        gen = c.extract_values(rsp)
+        gen = populate_random_args(gen)
+        try:
+            rsp = c.populate_instance(gen, rsp)
+        except:
+            print("populating instance")
+            print(rsp)
+            print("populating with")
+            print(gen)
+            raise
+        self.output = gen
+        return rsp
+
+    def success(self, rsp):
+        self.rsp = rsp
+
+    def error(self, exc):
+        self.exc = exc
+
+    def validate(self, equality_function):
+        if hasattr(self, "exc"):
+            print(self.exc)
+            print(self.exc.message)
+            raise self.exc
+        equality_function(self.input, c.extract_values(self.req))
+        equality_function(self.output, self.rsp)
+
+
+class TestServices(unittest.TestCase):
+
+    def setUp(self):
+        rospy.init_node(NAME)
+
+    def msgs_equal(self, msg1, msg2):
+        if type(msg1) in string_types and type(msg2) in string_types:
+            pass
+        else:
+            self.assertEqual(type(msg1), type(msg2))
+        if type(msg1) in c.list_types:
+            for x, y in zip(msg1, msg2):
+                self.msgs_equal(x, y)
+        elif type(msg1) in c.primitive_types or type(msg1) in c.string_types:
+            self.assertEqual(msg1, msg2)
+        else:
+            for x in msg1:
+                self.assertTrue(x in msg2)
+            for x in msg2:
+                self.assertTrue(x in msg1)
+            for x in msg1:
+                self.msgs_equal(msg1[x], msg2[x])
+
+    def test_populate_request_args(self):
+        # Test empty messages
+        for srv_type in ["TestEmpty", "TestResponseOnly"]:
+            cls = ros_loader.get_service_class("lgsvl_rosbridge_library/" + srv_type)
+            for args in [[], {}, None]:
+                # Should throw no exceptions
+                services.args_to_service_request_instance("", cls._request_class(), args)
+
+        # Test msgs with data message
+        for srv_type in ["TestRequestOnly", "TestRequestAndResponse"]:
+            cls = ros_loader.get_service_class("lgsvl_rosbridge_library/" + srv_type)
+            for args in [[3], {"data": 3}]:
+                # Should throw no exceptions
+                services.args_to_service_request_instance("", cls._request_class(), args)
+            self.assertRaises(FieldTypeMismatchException, services.args_to_service_request_instance, "", cls._request_class(), ["hello"])
+
+        # Test message with multiple fields
+        cls = ros_loader.get_service_class("lgsvl_rosbridge_library/TestMultipleRequestFields")
+        for args in [[3, 3.5, "hello", False], {"int": 3, "float": 3.5, "string": "hello", "bool": False}]:
+            # Should throw no exceptions
+            services.args_to_service_request_instance("", cls._request_class(), args)
+
+    def test_service_call(self):
+        """ Test a simple getloggers service call """
+        # First, call the service the 'proper' way
+        p = rospy.ServiceProxy(rospy.get_name() + "/get_loggers", GetLoggers)
+        p.wait_for_service(0.5)
+        ret = p()
+
+        # Now, call using the services
+        json_ret = services.call_service(rospy.get_name() + "/get_loggers")
+        for x, y in zip(ret.loggers, json_ret["loggers"]):
+            self.assertEqual(x.name, y["name"])
+            self.assertEqual(x.level, y["level"])
+
+    def test_service_caller(self):
+        """ Same as test_service_call but via the thread caller """
+        # First, call the service the 'proper' way
+        p = rospy.ServiceProxy(rospy.get_name() + "/get_loggers", GetLoggers)
+        p.wait_for_service(0.5)
+        ret = p()
+
+        rcvd = {"json": None}
+
+        def success(json):
+            rcvd["json"] = json
+
+        def error():
+            raise Exception()
+
+        # Now, call using the services
+        services.ServiceCaller(rospy.get_name() + "/get_loggers", None, success, error).start()
+
+        time.sleep(0.5)
+
+        for x, y in zip(ret.loggers, rcvd["json"]["loggers"]):
+            self.assertEqual(x.name, y["name"])
+            self.assertEqual(x.level, y["level"])
+
+    def test_service_tester(self):
+        t = ServiceTester("/test_service_tester", "lgsvl_rosbridge_library/TestRequestAndResponse")
+        t.start()
+        time.sleep(1.0)
+        t.validate(self.msgs_equal)
+
+    def test_service_tester_alltypes(self):
+        ts = []
+        for srv in ["TestResponseOnly", "TestEmpty", "TestRequestAndResponse", "TestRequestOnly",
+            "TestMultipleResponseFields", "TestMultipleRequestFields", "TestArrayRequest"]:
+            t = ServiceTester("/test_service_tester_alltypes_" + srv, "lgsvl_rosbridge_library/" + srv)
+            t.start()
+            ts.append(t)
+
+        time.sleep(1.0)
+
+        for t in ts:
+            t.validate(self.msgs_equal)
+
+    def test_random_service_types(self):
+        common = ["roscpp/GetLoggers", "roscpp/SetLoggerLevel",
+        "std_srvs/Empty", "nav_msgs/GetMap", "nav_msgs/GetPlan",
+        "sensor_msgs/SetCameraInfo", "topic_tools/MuxAdd",
+        "topic_tools/MuxSelect", "tf2_msgs/FrameGraph",
+        "rospy_tutorials/BadTwoInts", "rospy_tutorials/AddTwoInts"]
+        ts = []
+        for srv in common:
+            t = ServiceTester("/test_random_service_types/" + srv, srv)
+            t.start()
+            ts.append(t)
+
+        time.sleep(1.0)
+
+        for t in ts:
+            t.validate(self.msgs_equal)
+
+
+if __name__ == '__main__':
+    rosunit.unitrun(PKG, NAME, TestServices)
+
diff --git a/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/test_all.test b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/test_all.test
new file mode 100644
index 00000000..9efe58b3
--- /dev/null
+++ b/rubis_ws/src/ros-bridge/lgsvl_rosbridge_library/test/test_all.test
@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(find lgsvl_rosbridge_library)/test/capabilities/test_capabilities.test" />
+  <include file="$(find lgsvl_rosbridge_library)/test/internal/test_internal.test" />
+</launch>
+
diff --git a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch
index 9df6d76c..9a3bd358 100644
--- a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch
+++ b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch
@@ -9,7 +9,7 @@
         args="pub -l /carla/$(arg role_name)/enable_autopilot std_msgs/Bool '{ data: true}' " />
 
   <!-- pcl map capturing -->
-  <node pkg="pcl_recorder" type="pcl_recorder_node" name="pcl_recorder_node" output="screen">
+  <node pkg="pcl_recorder" type="pcl_recorder_node" name="pcl_recorder_node">
     <param name="role_name" value="$(arg role_name)" />
   </node>
 
diff --git a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py
index d2510032..919416b3 100644
--- a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py
+++ b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py
@@ -10,7 +10,7 @@ def launch_enable_autopilot_publisher(context, *args, **kwargs):
     topic_name = "/carla/" + launch.substitutions.LaunchConfiguration('role_name').perform(context) + "/enable_autopilot"
     return [
         launch.actions.ExecuteProcess(
-            output="screen",
+           ,
             cmd=["ros2", "topic", "pub", topic_name,
                  "std_msgs/msg/Bool", "{'data': True}", "--qos-durability", "transient_local"],
             name='topic_pub_enable_autopilot')]
diff --git a/rubis_ws/src/rubis_autorunner/CMakeLists.txt b/rubis_ws/src/rubis_autorunner/CMakeLists.txt
index 2b0febb6..0c1be735 100644
--- a/rubis_ws/src/rubis_autorunner/CMakeLists.txt
+++ b/rubis_ws/src/rubis_autorunner/CMakeLists.txt
@@ -10,6 +10,9 @@ find_package(catkin REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   autoware_msgs
+  visualization_msgs
+  nmea_msgs
+  rubis_msgs
 )
 
 catkin_package(
@@ -30,41 +33,18 @@ add_library(ros_autorunner_lib
   src/ros_autorunner_lib/ros_autorunner.cpp
 )
 
-add_executable(lgsvl_triple_lidar_autorunner
-  include/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.h
-  src/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner_node.cpp
-  src/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.cpp
-)
-
-add_executable(desktop_lane_keeping
-  include/desktop_lane_keeping/desktop_lane_keeping.h
-  src/desktop_lane_keeping/desktop_lane_keeping_node.cpp
-  src/desktop_lane_keeping/desktop_lane_keeping.cpp
-)
-
-add_executable(minicar_lane_keeping
-  include/minicar_lane_keeping/minicar_lane_keeping.h
-  src/minicar_lane_keeping/minicar_lane_keeping_node.cpp
-  src/minicar_lane_keeping/minicar_lane_keeping.cpp
-)
-  
-add_executable(cubetown_autorunner
+add_executable(cubetown_lkas_autorunner
   include/cubetown_autorunner/cubetown_autorunner.h
-  src/cubetown_autorunner/cubetown_autorunner_node.cpp
-  src/cubetown_autorunner/cubetown_autorunner.cpp
+  src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp
+  src/cubetown_autorunner/cubetown_lkas_autorunner.cpp
 )
 
-add_executable(cubetown_autorunner_gpu
+add_executable(cubetown_full_autorunner
   include/cubetown_autorunner/cubetown_autorunner.h
-  src/cubetown_autorunner/cubetown_autorunner_gpu_node.cpp
-  src/cubetown_autorunner/cubetown_autorunner_gpu.cpp
+  src/cubetown_autorunner/cubetown_full_autorunner_node.cpp
+  src/cubetown_autorunner/cubetown_full_autorunner.cpp
 )
 
-add_executable(tutorial_autorunner
-  include/tutorial_autorunner/tutorial_autorunner.h
-  src/tutorial_autorunner/tutorial_autorunner_node.cpp
-  src/tutorial_autorunner/tutorial_autorunner.cpp
-)
 
 add_executable(rubis_testbed_autorunner
   include/rubis_testbed_autorunner/rubis_testbed_autorunner.h
@@ -72,44 +52,101 @@ add_executable(rubis_testbed_autorunner
   src/rubis_testbed_autorunner/rubis_testbed_autorunner.cpp
 )
 
-add_dependencies(lgsvl_triple_lidar_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(lgsvl_triple_lidar_autorunner
+add_executable(carla_lkas_autorunner
+  include/carla_autorunner/carla_autorunner.h
+  src/carla_autorunner/carla_lkas_autorunner_node.cpp
+  src/carla_autorunner/carla_lkas_autorunner.cpp
+)
+
+add_executable(carla_full_autorunner
+  include/carla_autorunner/carla_autorunner.h
+  src/carla_autorunner/carla_full_autorunner_node.cpp
+  src/carla_autorunner/carla_full_autorunner.cpp
+)
+
+add_executable(exynos_carla_lkas_autorunner
+  include/carla_autorunner/carla_autorunner.h
+  src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp
+  src/carla_autorunner/exynos_carla_lkas_autorunner.cpp
+)
+
+add_executable(exynos_carla_full_autorunner
+  include/carla_autorunner/carla_autorunner.h
+  src/carla_autorunner/exynos_carla_full_autorunner_node.cpp
+  src/carla_autorunner/exynos_carla_full_autorunner.cpp
+)
+
+
+add_dependencies(cubetown_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(cubetown_lkas_autorunner
   ${catkin_LIBRARIES}
   ros_autorunner_lib
 )
 
-add_dependencies(desktop_lane_keeping ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(desktop_lane_keeping
+add_dependencies(cubetown_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(cubetown_full_autorunner
   ${catkin_LIBRARIES}
   ros_autorunner_lib
 )
 
-add_dependencies(minicar_lane_keeping ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(minicar_lane_keeping
+add_dependencies(rubis_testbed_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(rubis_testbed_autorunner
   ${catkin_LIBRARIES}
   ros_autorunner_lib
 )
 
-add_dependencies(cubetown_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(cubetown_autorunner
+add_dependencies(carla_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(carla_lkas_autorunner
   ${catkin_LIBRARIES}
   ros_autorunner_lib
 )
 
-add_dependencies(cubetown_autorunner_gpu ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(cubetown_autorunner_gpu
+add_dependencies(carla_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(carla_full_autorunner
   ${catkin_LIBRARIES}
   ros_autorunner_lib
 )
 
-add_dependencies(tutorial_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(tutorial_autorunner
+add_dependencies(exynos_carla_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(exynos_carla_lkas_autorunner
   ${catkin_LIBRARIES}
   ros_autorunner_lib
 )
 
-add_dependencies(rubis_testbed_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(rubis_testbed_autorunner
+add_dependencies(exynos_carla_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(exynos_carla_full_autorunner
   ${catkin_LIBRARIES}
   ros_autorunner_lib
-)
\ No newline at end of file
+)
+
+install(
+  TARGETS
+    ros_autorunner_lib
+    carla_lkas_autorunner
+    carla_full_autorunner
+    exynos_carla_lkas_autorunner
+    exynos_carla_full_autorunner
+    cubetown_lkas_autorunner
+    cubetown_full_autorunner
+    rubis_testbed_autorunner
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY cfg/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+)
+
+install(DIRECTORY scripts/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts
+)
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml
new file mode 100644
index 00000000..feb87474
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml
@@ -0,0 +1,183 @@
+# Unit of rate: hz
+# Unit of scheduling params: ns
+
+# 1s  : 1_000_000_000
+# 1ms : 1_000_000
+# 1us : 1_000
+# 1ns : 1
+
+#rate = hz
+# other time unut = ns
+
+# Lane Keeping
+lidar_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_000_000
+    deadline: 2_000_000
+    period: 2_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"  
+
+voxel_grid_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"  
+
+ndt_matching:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  
+  use_kalman_filter: False
+  tf_x: 1.0510799 
+  tf_y: 0
+  tf_z: 1.96
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+pure_pursuit:
+  rate: 30 #30
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"  
+
+  dynamic_params_flag: False
+  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
+
+twist_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
+
+# Detection
+ray_ground_filter_center:
+  rate: 10 
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
+
+lidar_euclidean_cluster_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
+
+  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
+  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
+
+# Planning
+op_global_planner:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
+
+  multilap_flag: 1
+
+op_common_params:
+  rollOutDensity: 4
+  rollOutsNumber: 2
+  maxVelocity: 10.0
+  maxAcceleration: 10.0
+  maxDeceleration: -10.0
+
+op_trajectory_generator:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
+
+op_trajectory_evaluator:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
+
+  weightPriority: 1
+  weightTransition: 0.5
+  weightLong: 1
+  weightLat: 1
+  ImageWidth: 1920
+  ImageHeight: 1080
+  SprintDecisionTime: 9999999.0
+
+op_behavior_selector:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
+
+  distanceToPedestrianThreshold: 15.0
+  sprintSpeed: 10.0
+  obstacleWaitingTimeinIntersection: 2.0
+  turnThreshold: 30.0
+
+op_motion_predictor:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
+
+# Independent
+twist_gate:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+
+  zero_flag: 0 ## Publish target velocity as 0
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml
new file mode 100644
index 00000000..83c242dd
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml
@@ -0,0 +1,14 @@
+total_step_num: 5
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh"
+# step_# : 
+#     [0] pacakge name
+#     [1] target name( node name or launch script name )
+#     [2] state that create constraint topic or not ( "true" or "false" )
+#     [3] state that check constraint topic or not ( "true" or "false" )
+#     [4] target type( RUN or LAUNCH )
+
+step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"]
+step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"]
+step_3: ["rubis_autorunner", "_carla_autorunner_3_detection.launch", "true", "true", "LAUNCH"]
+step_4: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"]
+step_5: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"]
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml
new file mode 100644
index 00000000..6fed9fc7
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml
@@ -0,0 +1,13 @@
+total_step_num: 4
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh"
+# step_# : 
+#     [0] pacakge name
+#     [1] target name( node name or launch script name )
+#     [2] state that create constraint topic or not ( "true" or "false" )
+#     [3] state that check constraint topic or not ( "true" or "false" )
+#     [4] target type( RUN or LAUNCH )
+
+step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"]
+step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"]
+step_3: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"]
+step_4: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"]
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_spawn_points.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_spawn_points.yaml
new file mode 100644
index 00000000..9d2280d3
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_spawn_points.yaml
@@ -0,0 +1,75 @@
+# Unit of rate: hz
+# Unit of scheduling params: ns
+
+# 1s  : 1_000_000_000
+# 1ms : 1_000_000
+# 1us : 1_000
+# 1ns : 1
+
+#rate = hz
+# other time unut = ns
+
+# Lane Keeping
+ego_vehicle:
+  spawn_points:
+    x: 314.065
+    y: 129.676
+    z: 0.3
+    roll: 0
+    pitch: 0
+    yaw: 90
+  # spawn_points:
+  #   x: 200.9
+  #   y: 250.5
+  #   z: 0.3
+  #   roll: 0
+  #   pitch: 0
+  #   yaw: 120
+
+obstacle:
+  enable: False
+  spawn_points:
+    x: 314.8
+    y: 192.664
+    z: 0.3
+    roll: 0
+    pitch: 0
+    yaw: 270
+
+  # <!-- <arg name="spawn_point_walker" default="212.0, 297.0, 0.3, 0, 0, 180"/> -->
+  # <!-- <arg name="spawn_point_walker" default="-10, 214, 5, 0, 0, -90"/>
+  # <arg name="walker_point1" default="206.0, 297.0, 0.3, 0, 0, 180"/>
+  # <arg name="walker_point2" default="204.0, 297.0, 0.3, 0, 0, 180"/>
+  # <arg name="walker_point3" default="198.0, 297.0, 0.3, 0, 0, 180"/>
+
+walker:
+  enable: False
+  spawn_points:
+    x: 196.0
+    y: 260.0
+    z: 1.0
+    roll: 0
+    pitch: 0
+    yaw: 0
+  walker_point1:
+    x: 200.0
+    y: 260.0
+    z: 0.3
+    roll: 0
+    pitch: 0
+    yaw: 0
+  walker_point2:
+    x: 202.0
+    y: 260.0
+    z: 0.3
+    roll: 0
+    pitch: 0
+    yaw: 0
+  walker_point3:
+    x: 220.0
+    y: 260.0
+    z: 0.3
+    roll: 0
+    pitch: 0
+    yaw: 0
+  walker_target_speed: 2
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_autorunner_params.yaml
new file mode 100644
index 00000000..ae0a1981
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_autorunner_params.yaml
@@ -0,0 +1,180 @@
+# Unit of rate: hz
+# Unit of scheduling params: ns
+
+# 1s  : 1_000_000_000
+# 1ms : 1_000_000
+# 1us : 1_000
+# 1ns : 1
+
+#rate = hz
+# other time unut = ns
+
+# Lane Keeping
+lidar_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_000_000
+    deadline: 2_000_000
+    period: 2_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"  
+
+voxel_grid_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"  
+
+ndt_matching:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  
+  use_kalman_filter: False
+  tf_x: 1.0510799 
+  tf_y: 0
+  tf_z: 1.96
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+pure_pursuit:
+  rate: 30 #30
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"  
+
+  dynamic_params_flag: False
+  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
+
+twist_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
+
+# Detection
+ray_ground_filter_center:
+  rate: 10 
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
+
+lidar_euclidean_cluster_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
+
+# Planning
+op_global_planner:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
+
+  multilap_flag: 1
+
+op_common_params:
+  rollOutDensity: 4
+  rollOutsNumber: 2
+  maxVelocity: 8.0
+  maxAcceleration: 8.0
+  maxDeceleration: -8.0
+
+op_trajectory_generator:
+  rate: 20 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
+
+op_trajectory_evaluator:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
+
+  weightPriority: 1
+  weightTransition: 0.5
+  weightLong: 1
+  weightLat: 1
+  ImageWidth: 1920
+  ImageHeight: 1080
+  SprintDecisionTime: 9999999.0
+
+op_behavior_selector:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
+
+  distanceToPedestrianThreshold: 15.0
+  sprintSpeed: 10.0
+  obstacleWaitingTimeinIntersection: 2.0
+  turnThreshold: 30.0
+
+op_motion_predictor:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
+
+# Independent
+twist_gate:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+
+  zero_flag: 0 ## Publish target velocity as 0
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_gpu.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_full_autorunner.yaml
similarity index 60%
rename from rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_gpu.yaml
rename to rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_full_autorunner.yaml
index 1cdfcee7..f8c64742 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_gpu.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_full_autorunner.yaml
@@ -1,5 +1,5 @@
 total_step_num: 5
-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh"
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh"
 # step_# : 
 #     [0] pacakge name
 #     [1] target name( node name or launch script name )
@@ -7,8 +7,8 @@ terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorun
 #     [3] state that check constraint topic or not ( "true" or "false" )
 #     [4] target type( RUN or LAUNCH )
 
-step_1: ["rubis_autorunner", "_cubetown_autorunner_gpu_1_sensing.launch", "true", "false", "LAUNCH"]
-step_2: ["rubis_autorunner", "_cubetown_autorunner_gpu_2_localization.launch", "true", "true", "LAUNCH"]
-step_3: ["rubis_autorunner", "_cubetown_autorunner_gpu_3_detection.launch", "true", "true", "LAUNCH"]
-step_4: ["rubis_autorunner", "_cubetown_autorunner_gpu_4_planning.launch", "true", "true", "LAUNCH"]
-step_5: ["rubis_autorunner", "_cubetown_autorunner_gpu_5_control.launch", "false", "true", "LAUNCH"]
+step_1: ["rubis_autorunner", "_exynos_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"]
+step_2: ["rubis_autorunner", "_exynos_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"]
+step_3: ["rubis_autorunner", "_exynos_carla_autorunner_3_detection.launch", "true", "true", "LAUNCH"]
+step_4: ["rubis_autorunner", "_exynos_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"]
+step_5: ["rubis_autorunner", "_exynos_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"]
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_lkas_autorunner.yaml
new file mode 100644
index 00000000..e78908d3
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/exynos_carla_lkas_autorunner.yaml
@@ -0,0 +1,13 @@
+total_step_num: 4
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh"
+# step_# : 
+#     [0] pacakge name
+#     [1] target name( node name or launch script name )
+#     [2] state that create constraint topic or not ( "true" or "false" )
+#     [3] state that check constraint topic or not ( "true" or "false" )
+#     [4] target type( RUN or LAUNCH )
+
+step_1: ["rubis_autorunner", "_exynos_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"]
+step_2: ["rubis_autorunner", "_exynos_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"]
+step_3: ["rubis_autorunner", "_exynos_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"]
+step_4: ["rubis_autorunner", "_exynos_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"]
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla/lane_info.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/lane_info.yaml
similarity index 100%
rename from rubis_ws/src/rubis_autorunner/cfg/carla/lane_info.yaml
rename to rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/lane_info.yaml
diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml
similarity index 80%
rename from rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml
rename to rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml
index 1680abe6..a7f3c4f3 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml
@@ -12,7 +12,7 @@
 # Sensing
 ray_ground_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -22,7 +22,7 @@ ray_ground_filter:
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -33,17 +33,12 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 100
   task_minimum_inter_release_time: 100000000
   task_execution_time: 70000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 logger_brake_dist:
   rate: 10
@@ -55,37 +50,27 @@ calibration_publisher:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 100
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -94,7 +79,7 @@ imm_ukf_pda_track:
 
 range_vision_fusion:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -104,7 +89,7 @@ range_vision_fusion:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100000000
@@ -122,7 +107,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -131,7 +116,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100000000
@@ -154,7 +139,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -167,7 +152,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 40000000
@@ -177,7 +162,7 @@ op_motion_predictor:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 30 #30
   task_minimum_inter_release_time: 33333333
@@ -188,7 +173,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -197,7 +182,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_gpu_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_gpu_params.yaml
deleted file mode 100644
index 422cb9e8..00000000
--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_gpu_params.yaml
+++ /dev/null
@@ -1,224 +0,0 @@
-# Unit of rate: hz
-# Unit of task information: nano seconds
-
-# 1s  : 1000000000
-# 1ms : 1000000
-# 1us : 1000
-# 1ns : 1
-
-#rate = hz
-# other time unut = ns
-
-# Localization
-voxel_grid_filter:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 1_600_000
-  task_relative_deadline: 100_000_000
-
-ndt_matching:
-  tf_x: 1.0510799 
-  tf_y: 0
-  tf_z: 1.96
-  tf_roll: 0
-  tf_pitch: 0
-  tf_yaw: 0
-  localizer: "velodyne"
-
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 80_000_000
-  task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
-
-# Detection
-ray_ground_filter:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
-  rate: 10 
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 100_000_000
-  task_relative_deadline: 100_000_000
-
-lidar_euclidean_cluster_detect:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
-  rate: 10
-  task_minimum_inter_release_time: 200_000_000
-  task_execution_time: 50_000_000
-  task_relative_deadline: 200_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv"
-
-vision_darknet_detect:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 75_000_000
-  task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
-  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg"
-  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights"
-
-imm_ukf_pda_track:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
-  rate: 10
-  task_minimum_inter_release_time: 200_000_000
-  task_execution_time: 100_000_000
-  task_relative_deadline: 200_000_000
-
-# Planning
-op_global_planner:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
-  rate: 25 #25
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 45_000_000
-  task_relative_deadline: 100_000_000
-  multilap_flag: 1
-
-op_common_params:
-  rollOutDensity: 4
-  rollOutsNumber: 0
-  maxVelocity: 10.0
-  maxAcceleration: 10.0
-  maxDeceleration: -10.0
-
-op_trajectory_generator:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 2_900_000
-  task_relative_deadline: 100_000_000
-
-op_trajectory_evaluator:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 3_600_000
-  task_relative_deadline: 100_000_000
-  weightPriority: 0
-  weightTransition: 5
-  weightLong: 4
-  weightLat: 4
-  ImageWidth: 1920
-  ImageHeight: 1080
-  SprintDecisionTime: 9999999.0
-
-op_behavior_selector:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 20_800_000
-  task_relative_deadline: 100_000_000
-  distanceToPedestrianThreshold: 15.0
-  sprintSpeed: 10.0
-  obstacleWaitingTimeinIntersection: 2.0
-  turnThreshold: 30.0
-
-op_motion_predictor:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
-  rate: 25 #25
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 3_800_000
-  task_relative_deadline: 100_000_000
-
-# Control
-pure_pursuit:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
-  rate: 30 #30
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 3_000_000
-  task_relative_deadline: 100_000_000
-  dynamic_params_flag: False
-  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
-
-twist_filter:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 2_000_000
-  task_relative_deadline: 100_000_000
-
-twist_gate:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 2_000_000
-  task_relative_deadline: 100_000_000
-  zero_flag: 0 ## Publish target velocity as 0
-
-# Others
-lidar_republisher:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 70_000_000
-  task_relative_deadline: 100_000_000
-
-vel_relay:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 300_000
-  task_relative_deadline: 100_000_000
-
-pose_relay:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 300_000
-  task_relative_deadline: 100_000_000
-
-republish:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 35_000_000
-  task_relative_deadline: 100_000_000
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml
index 8bbe7035..66542f69 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml
@@ -1,260 +1,178 @@
-# Unit of rate: hz
-# Unit of task information: nano seconds
-
-# 1s  : 1000000000
-# 1ms : 1000000
-# 1us : 1000
-# 1ns : 1
-
-#rate = hz
-# other time unut = ns
-
-# Localization
-voxel_grid_filter:
-  instance_mode: 1
-
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 1_600_000
-  task_relative_deadline: 100_000_000
-
+lane_detector_left:
+  task_response_time_filename: ~/Documents/profiling/response_time/lane_detector_left.csv
+  task_scheduling_configs:
+    deadline: 2000000
+    exec_time: 1000000
+    period: 2000000
+    policy: NONE
+    priority: 20
+lane_detector_rear:
+  task_response_time_filename: ~/Documents/profiling/response_time/lane_detector_rear.csv
+  task_scheduling_configs:
+    deadline: 2000000
+    exec_time: 1000000
+    period: 2000000
+    policy: NONE
+    priority: 20
+lane_detector_right:
+  task_response_time_filename: ~/Documents/profiling/response_time/lane_detector_right.csv
+  task_scheduling_configs:
+    deadline: 2000000
+    exec_time: 1000000
+    period: 2000000
+    policy: NONE
+    priority: 20
+lane_detector_top:
+  task_response_time_filename: ~/Documents/profiling/response_time/lane_detector_top.csv
+  task_scheduling_configs:
+    deadline: 2000000
+    exec_time: 1000000
+    period: 2000000
+    policy: NONE
+    priority: 20
+lidar_euclidean_cluster_detect:
+  network_definition_file: ~/autoware.ai/autoware_files/vision/yolov3-320.cfg
+  pretrained_model_file: ~/autoware.ai/autoware_files/vision/yolov3-320.weights
+  task_response_time_filename: ~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
+lidar_republisher:
+  task_response_time_filename: ~/Documents/profiling/response_time/lidar_republisher.csv
+  task_scheduling_configs:
+    deadline: 2000000
+    exec_time: 1000000
+    period: 2000000
+    policy: NONE
+    priority: 20
 ndt_matching:
-  instance_mode: 1
-  use_kalman_filter: True
-
-  tf_x: 1.0510799 
-  tf_y: 0
-  tf_z: 1.96
-  tf_roll: 0
+  localizer: velodyne
+  task_response_time_filename: ~/Documents/profiling/response_time/ndt_matching.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
   tf_pitch: 0
-  tf_yaw: 0
-  localizer: "velodyne"
-
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 80_000_000
-  task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
-
-modular_ndt_matching_CENTER:
-  instance_mode: 1
-  use_kalman_filter: True
-
-  tf_x: 1.0510799 
-  tf_y: 0
-  tf_z: 1.96
   tf_roll: 0
-  tf_pitch: 0
+  tf_x: 1.0510799
+  tf_y: 0
   tf_yaw: 0
-  localizer: "velodyne"
-
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 80_000_000
-  task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
-
-# Detection
-ray_ground_filter_center:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
-  rate: 10 
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 100_000_000
-  task_relative_deadline: 100_000_000
-
-
-lidar_euclidean_cluster_detect:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
-  rate: 10
-  task_minimum_inter_release_time: 200_000_000
-  task_execution_time: 50_000_000
-  task_relative_deadline: 200_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv"
-
-vision_darknet_detect:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 75_000_000
-  task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
-  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
-  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
-
-imm_ukf_pda_track:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
-  rate: 10
-  task_minimum_inter_release_time: 200_000_000
-  task_execution_time: 100_000_000
-  task_relative_deadline: 200_000_000
-
-# Planning
-op_global_planner:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
-  rate: 25 #25
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 45_000_000
-  task_relative_deadline: 100_000_000
-  multilap_flag: 1
-
-op_common_params:
-  rollOutDensity: 4
-  rollOutsNumber: 2
-  maxVelocity: 10.0
-  maxAcceleration: 10.0
-  maxDeceleration: -10.0
-
-op_trajectory_generator:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 2_900_000
-  task_relative_deadline: 100_000_000
-
-op_trajectory_evaluator:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 3_600_000
-  task_relative_deadline: 100_000_000
-  weightPriority: 0
-  weightTransition: 5
-  weightLong: 4
-  weightLat: 4
-  ImageWidth: 1920
-  ImageHeight: 1080
-  SprintDecisionTime: 9999999.0
-
+  tf_z: 1.96
+  use_kalman_filter: false
 op_behavior_selector:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 20_800_000
-  task_relative_deadline: 100_000_000
   distanceToPedestrianThreshold: 15.0
-  sprintSpeed: 10.0
   obstacleWaitingTimeinIntersection: 2.0
+  sprintSpeed: 10.0
+  task_response_time_filename: ~/Documents/profiling/response_time/op_behavior_selector.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
   turnThreshold: 30.0
-
+op_common_params:
+  blockIdx: -1
+  maxAcceleration: 7.5
+  maxDeceleration: -7.5
+  maxVelocity: 10.0
+  rollOutDensity: 4
+  rollOutsNumber: 0
+op_global_planner:
+  multilap_flag: 1
+  rate: 25
+  task_response_time_filename: ~/Documents/profiling/response_time/op_global_planner.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
 op_motion_predictor:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
-  rate: 25 #25
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 3_800_000
-  task_relative_deadline: 100_000_000
-
-# Control
+  task_response_time_filename: ~/Documents/profiling/response_time/op_motion_predictor.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
+op_trajectory_evaluator:
+  ImageHeight: 1080
+  ImageWidth: 1920
+  SprintDecisionTime: 9999999.0
+  blockThreshold: 1.5
+  task_response_time_filename: ~/Documents/profiling/response_time/op_trajectory_evaluator.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
+  weightLat: 10
+  weightLong: 10
+  weightPriority: 1
+  weightTransition: 0.5
+op_trajectory_generator:
+  task_response_time_filename: ~/Documents/profiling/response_time/op_trajectory_generator.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
 pure_pursuit:
-  instance_mode: 1
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
-  rate: 30 #30
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 3_000_000
-  task_relative_deadline: 100_000_000
-  dynamic_params_flag: False
-  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
-
+  dynamic_params_flag: false
+  dynamic_params_path: ~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml
+  task_response_time_filename: ~/Documents/profiling/response_time/pure_pursuit.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
+ray_ground_filter_center:
+  task_response_time_filename: ~/Documents/profiling/response_time/ray_ground_filter.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
+svl_sensing:
+  task_response_time_filename: ~/Documents/profiling/response_time/svl_sensing.csv
+  task_scheduling_configs:
+    deadline: 2000000
+    exec_time: 1000000
+    period: 2000000
+    policy: NONE
+    priority: 20
 twist_filter:
-  instance_mode: 1
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 2_000_000
-  task_relative_deadline: 100_000_000
-
+  task_response_time_filename: ~/Documents/profiling/response_time/twist_filter.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
 twist_gate:
-  instance_mode: 1
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 2_000_000
-  task_relative_deadline: 100_000_000
-  zero_flag: 0 ## Publish target velocity as 0
-
-# Others
-lidar_republisher:
-  instance_mode: 1
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 70_000_000
-  task_relative_deadline: 100_000_000
-
-vel_relay:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 300_000
-  task_relative_deadline: 100_000_000
-
-pose_relay:
-  instance_mode: 1
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 300_000
-  task_relative_deadline: 100_000_000
-
-republish:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100_000_000
-  task_execution_time: 35_000_000
-  task_relative_deadline: 100_000_000
\ No newline at end of file
+  task_response_time_filename: ~/Documents/profiling/response_time/twist_gate.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
+  zero_flag: 0
+voxel_grid_filter:
+  task_response_time_filename: ~/Documents/profiling/response_time/voxel_grid_filter.csv
+  task_scheduling_configs:
+    deadline: 0
+    exec_time: 0
+    period: 0
+    policy: NONE
+    priority: 20
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup
deleted file mode 100644
index f8e2f9cf..00000000
--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup
+++ /dev/null
@@ -1,238 +0,0 @@
-# Unit of rate: hz
-# Unit of task information: nano seconds
-
-# 1s  : 1000000000
-# 1ms : 1000000
-# 1us : 1000
-# 1ns : 1
-
-#rate = hz
-# other time unut = ns
-
-# Localization
-voxel_grid_filter:
-  task_scheduling_flag: 1
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
-  rate: 10 
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-
-ndt_matching:
-  tf_x: 1.0510799 
-  tf_y: 0
-  tf_z: 1.96
-  tf_roll: 0
-  tf_pitch: 0
-  tf_yaw: 0
-  localizer: "velodyne"
-
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 70000000
-  task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
-
-# Detection
-compare_map_filter:
-  distance_threshold: 0.4
-  min_clipping_height: -1.7
-  max_clipping_height: 0.5
-
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 70000000
-  task_relative_deadline: 100000000
-
-
-lidar_euclidean_cluster_detect:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
-  rate: 10 
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv"
-
-vision_darknet_detect:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
-  rate: 10 
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
-  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg"
-  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights"
-
-imm_ukf_pda_track:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
-  rate: 10 
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-
-range_vision_fusion:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv"
-  rate: 10 
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-
-# Planning
-op_global_planner:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
-  rate: 25 #25
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-  multilap_flag: 1
-
-op_common_params:
-  rollOutDensity: 4
-  rollOutsNumber: 0
-  maxVelocity: 10.0
-  maxAcceleration: 10.0
-  maxDeceleration: -10.0
-
-op_trajectory_generator:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 10000000
-  task_execution_time: 10000000
-  task_relative_deadline: 10000000
-
-op_trajectory_evaluator:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-  weightPriority: 0
-  weightTransition: 5
-  weightLong: 4
-  weightLat: 4
-  ImageWidth: 1920
-  ImageHeight: 1080
-  SprintDecisionTime: 9999999.0
-
-op_behavior_selector:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
-  rate: 100 #100
-  task_minimum_inter_release_time: 10000000
-  task_execution_time: 10000000
-  task_relative_deadline: 10000000
-  distanceToPedestrianThreshold: 15.0
-  sprintSpeed: 10.0
-  obstacleWaitingTimeinIntersection: 2.0
-  turnThreshold: 30.0
-
-op_motion_predictor:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
-  rate: 25 #25
-  task_minimum_inter_release_time: 40000000
-  task_execution_time: 40000000
-  task_relative_deadline: 40000000
-
-# Control
-pure_pursuit:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
-  rate: 30 #30
-  task_minimum_inter_release_time: 33333333
-  task_execution_time: 33333333
-  task_relative_deadline: 33333333
-  dynamic_params_flag: False
-  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
-
-twist_filter:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-
-twist_gate:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-  zero_flag: 0 ## Publish target velocity as 0
-
-# Others
-lidar_republisher:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-
-vel_relay:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-
-vel_relay:
-  task_scheduling_flag: 0
-  task_profiling_flag: 0
-  task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
-
-republish:
-  task_scheduling_flag: 0
-  task_profiling_flag: 1
-  task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
-  rate: 10
-  task_minimum_inter_release_time: 100000000
-  task_execution_time: 100000000
-  task_relative_deadline: 100000000
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params_none_priority.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params_none_priority.yaml
new file mode 100644
index 00000000..32f96652
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params_none_priority.yaml
@@ -0,0 +1,183 @@
+# Unit of rate: hz
+# Unit of scheduling params: ns
+
+# 1s  : 1_000_000_000
+# 1ms : 1_000_000
+# 1us : 1_000
+# 1ns : 1
+
+#rate = hz
+# other time unut = ns
+
+# Lane Keeping
+lidar_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_000_000
+    deadline: 2_000_000
+    period: 2_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"  
+
+voxel_grid_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"  
+
+ndt_matching:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  
+  use_kalman_filter: False
+  tf_x: 1.0510799 
+  tf_y: 0
+  tf_z: 1.96
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+pure_pursuit:
+  rate: 30 #30
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"  
+
+  dynamic_params_flag: False
+  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
+
+twist_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
+
+# Detection
+ray_ground_filter_center:
+  rate: 10 
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
+
+lidar_euclidean_cluster_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
+
+  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
+  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
+
+# Planning
+op_global_planner:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
+
+  multilap_flag: 1
+
+op_common_params:
+  rollOutDensity: 4
+  rollOutsNumber: 2
+  maxVelocity: 7.5
+  maxAcceleration: 7.5
+  maxDeceleration: -7.5
+
+op_trajectory_generator:
+  rate: 10 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
+
+op_trajectory_evaluator:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
+
+  weightPriority: 1
+  weightTransition: 0.5
+  weightLong: 10
+  weightLat: 10
+  ImageWidth: 1920
+  ImageHeight: 1080
+  SprintDecisionTime: 9999999.0
+
+op_behavior_selector:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
+
+  distanceToPedestrianThreshold: 15.0
+  sprintSpeed: 10.0
+  obstacleWaitingTimeinIntersection: 2.0
+  turnThreshold: 30.0
+
+op_motion_predictor:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
+
+# Independent
+twist_gate:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+
+  zero_flag: 0 ## Publish target velocity as 0
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params_priority.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params_priority.yaml
new file mode 100644
index 00000000..bc8b26a9
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params_priority.yaml
@@ -0,0 +1,183 @@
+# Unit of rate: hz
+# Unit of scheduling params: ns
+
+# 1s  : 1_000_000_000
+# 1ms : 1_000_000
+# 1us : 1_000
+# 1ns : 1
+
+#rate = hz
+# other time unut = ns
+
+# Lane Keeping
+lidar_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 94
+    exec_time: 1_000_000
+    deadline: 2_000_000
+    period: 2_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"  
+
+voxel_grid_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 95
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"  
+
+ndt_matching:
+  rate: 10
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 96
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  
+  use_kalman_filter: False
+  tf_x: 1.0510799 
+  tf_y: 0
+  tf_z: 1.96
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+pure_pursuit:
+  rate: 30 #30
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 97
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"  
+
+  dynamic_params_flag: False
+  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
+
+twist_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 98
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
+
+# Detection
+ray_ground_filter_center:
+  rate: 10 
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 95
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
+
+lidar_euclidean_cluster_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 96
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
+
+  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
+  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
+
+# Planning
+op_global_planner:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 94
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
+
+  multilap_flag: 1
+
+op_common_params:
+  rollOutDensity: 4
+  rollOutsNumber: 2
+  maxVelocity: 7.5
+  maxAcceleration: 7.5
+  maxDeceleration: -7.5
+
+op_trajectory_generator:
+  rate: 10 #100
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 94
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
+
+op_trajectory_evaluator:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 95
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
+
+  weightPriority: 1
+  weightTransition: 0.5
+  weightLong: 10
+  weightLat: 10
+  ImageWidth: 1920
+  ImageHeight: 1080
+  SprintDecisionTime: 9999999.0
+
+op_behavior_selector:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 96
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
+
+  distanceToPedestrianThreshold: 15.0
+  sprintSpeed: 10.0
+  obstacleWaitingTimeinIntersection: 2.0
+  turnThreshold: 30.0
+
+op_motion_predictor:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 97
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
+
+# Independent
+twist_gate:
+  rate: 10
+  task_scheduling_configs:
+    policy: "SCHED_FIFO"
+    priority: 99
+    exec_time: 0 # ns
+    deadline: 0 # ns
+    period: 0 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+
+  zero_flag: 0 ## Publish target velocity as 0
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml
similarity index 100%
rename from rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml
rename to rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml
diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml
new file mode 100644
index 00000000..c70a8e39
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml
@@ -0,0 +1,13 @@
+total_step_num: 4
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh"
+# step_# : 
+#     [0] pacakge name
+#     [1] target name( node name or launch script name )
+#     [2] state that create constraint topic or not ( "true" or "false" )
+#     [3] state that check constraint topic or not ( "true" or "false" )
+#     [4] target type( RUN or LAUNCH )
+
+step_1: ["rubis_autorunner", "_cubetown_autorunner_1_sensing.launch", "true", "false", "LAUNCH"]
+step_2: ["rubis_autorunner", "_cubetown_autorunner_2_localization.launch", "true", "true", "LAUNCH"]
+step_3: ["rubis_autorunner", "_cubetown_autorunner_4_planning.launch", "true", "true", "LAUNCH"]
+step_4: ["rubis_autorunner", "_cubetown_autorunner_5_control.launch", "false", "true", "LAUNCH"]
diff --git a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml
index 3548041c..f142af8a 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml
@@ -1,5 +1,5 @@
 total_step_num: 4
-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh"
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate_desktop.sh"
 # step_# : 
 #     [0] pacakge name
 #     [1] target name( node name or launch script name )
diff --git a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml
index 29cc8960..0817c790 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml
@@ -12,7 +12,7 @@
 # Sensing
 ray_ground_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -22,7 +22,7 @@ ray_ground_filter:
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -33,17 +33,12 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
   task_execution_time: 70000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Detection
 calibration_publisher:
@@ -51,37 +46,27 @@ calibration_publisher:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
   task_execution_time: 100000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -90,7 +75,7 @@ imm_ukf_pda_track:
 
 range_vision_fusion:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -100,7 +85,7 @@ range_vision_fusion:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100000000
@@ -118,7 +103,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -127,7 +112,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100000000
@@ -143,7 +128,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -156,7 +141,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 40000000
@@ -166,7 +151,7 @@ op_motion_predictor:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 30 #30
   task_minimum_inter_release_time: 33333333
@@ -177,7 +162,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -186,7 +171,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml
index 446ed3b2..3cb361bc 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml
@@ -19,7 +19,7 @@ voxel_grid_filter:
   instance_mode: 1
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -30,20 +30,14 @@ modular_ndt_matching_FR:
   instance_mode: 1
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 80_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 ndt_matching:
-  instance_mode: 1
   use_kalman_filter: False
 
   tf_x: 3.3
@@ -55,24 +49,17 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 80_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
-
-
 
 # Detection
 ray_ground_filter_center:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100_000_000
@@ -82,37 +69,27 @@ ray_ground_filter_center:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10
   task_minimum_inter_release_time: 200_000_000
   task_execution_time: 50_000_000
   task_relative_deadline: 200_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 75_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10
   task_minimum_inter_release_time: 200_000_000
@@ -122,7 +99,7 @@ imm_ukf_pda_track:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 10 #25
   task_minimum_inter_release_time: 100_000_000
@@ -139,7 +116,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 10 #100
   task_minimum_inter_release_time: 100_000_000
@@ -148,7 +125,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 10 #100
   task_minimum_inter_release_time: 100_000_000
@@ -166,7 +143,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 10 #100
   task_minimum_inter_release_time: 100_000_000
@@ -179,7 +156,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 10 #25
   task_minimum_inter_release_time: 100_000_000
@@ -188,9 +165,8 @@ op_motion_predictor:
 
 # Control
 pure_pursuit:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 10 #30
   task_minimum_inter_release_time: 100_000_000
@@ -200,9 +176,8 @@ pure_pursuit:
   dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
 
 twist_filter:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -210,9 +185,8 @@ twist_filter:
   task_relative_deadline: 100_000_000
 
 twist_gate:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -222,18 +196,27 @@ twist_gate:
 
 # Others
 lidar_republisher:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 70_000_000
   task_relative_deadline: 100_000_000
 
+lidar_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_000_000
+    deadline: 2_000_000
+    period: 2_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"  
+
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -241,9 +224,8 @@ vel_relay:
   task_relative_deadline: 100_000_000
 
 pose_relay:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -252,7 +234,7 @@ pose_relay:
 
 republish:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_event_driven.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_event_driven.yaml
new file mode 100644
index 00000000..5dc50602
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_event_driven.yaml
@@ -0,0 +1,262 @@
+# Unit of rate: hz
+# Unit of scheduling params: ns
+
+# 1s  : 1_000_000_000
+# 1ms : 1_000_000
+# 1us : 1_000
+# 1ns : 1
+
+#rate = hz
+# other time unut = ns
+
+# Vision
+camera_image:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_600_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/camera_image.csv" 
+  camera_id: 0
+
+# Lane Keeping
+lidar_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 70_000_000
+    deadline: 100_000_000
+    period: 100_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"  
+
+camera_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 70_000_000
+    deadline: 100_000_000
+    period: 100_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/camera_republisher.csv"  
+
+vel_relay:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 300_000
+    deadline: 100_000_000
+    period: 100_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
+
+pose_relay:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 300_000
+    deadline: 100_000_000
+    period: 100_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
+
+republish:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 35_000_000
+    deadline: 100_000_000
+    period: 100_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
+
+voxel_grid_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_600_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"  
+
+modular_ndt_matching_FR:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 80_000_000 # ns
+    deadline: 80_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  use_kalman_filter: False
+  tf_x: 3.3
+  tf_y: -0.8
+  tf_z: 0.09
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+ndt_matching:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 80_000_000 # ns
+    deadline: 80_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  use_kalman_filter: False
+  tf_x: 3.3
+  tf_y: -0.8
+  tf_z: 0.09
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+# Detection
+ray_ground_filter_center:
+  rate: 10 
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
+
+lidar_euclidean_cluster_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 50_000_000 # ns
+    deadline: 200_000_000 # ns
+    period: 200_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
+
+vision_darknet_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
+  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
+  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3.weights"
+
+# Planning
+op_global_planner:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 45_000_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
+
+  multilap_flag: 1
+
+op_common_params:
+  rollOutDensity: 3.5
+  rollOutsNumber: 2
+  maxVelocity: 8.0
+  maxAcceleration: 10.0
+  maxDeceleration: -10.0
+
+op_trajectory_generator:
+  rate: 20 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 2_900_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
+
+op_trajectory_evaluator:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 3_600_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
+
+  weightPriority: 0
+  weightTransition: 5
+  weightLong: 4
+  weightLat: 4
+  ImageWidth: 1280
+  ImageHeight: 720
+  SprintDecisionTime: 9999999.0
+  PedestrianStopImgHeightThreshold: 80
+  PedestrianImageDetectionRange: 0.3
+
+op_behavior_selector:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 20_800_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
+  distanceToPedestrianThreshold: 15.0
+  sprintSpeed: 10.0
+  obstacleWaitingTimeinIntersection: 2.0
+  turnThreshold: 30.0
+
+op_motion_predictor:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 3_800_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
+
+# Control
+twist_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 2_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
+
+pure_pursuit:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 3_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+  dynamic_params_flag: False
+  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
+
+# Independent
+twist_gate:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 2_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+
+  zero_flag: 0 ## Publish target velocity as 0
diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_new.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_new.yaml
new file mode 100644
index 00000000..6a42c296
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_new.yaml
@@ -0,0 +1,268 @@
+# Unit of rate: hz
+# Unit of scheduling params: ns
+
+# 1s  : 1_000_000_000
+# 1ms : 1_000_000
+# 1us : 1_000
+# 1ns : 1
+
+#rate = hz
+# other time unut = ns
+
+# Vision
+camera_image:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_600_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/camera_image.csv" 
+  camera_id: 0
+
+# Lane Keeping
+lidar_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 70_000_000
+    deadline: 100_000_000
+    period: 100_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"  
+
+camera_republisher:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 70_000_000
+    deadline: 100_000_000
+    period: 100_000_000
+  task_response_time_filename: "~/Documents/profiling/response_time/camera_republisher.csv"  
+
+# vel_relay:
+#   rate: 10
+#   task_scheduling_configs:
+#     policy: "NONE"
+#     priority: 99
+#     exec_time: 300_000
+#     deadline: 100_000_000
+#     period: 100_000_000
+#   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
+
+# pose_relay:
+#   rate: 10
+#   task_scheduling_configs:
+#     policy: "NONE"
+#     priority: 99
+#     exec_time: 300_000
+#     deadline: 100_000_000
+#     period: 100_000_000
+#   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
+
+# republish:
+#   rate: 10
+#   task_scheduling_configs:
+#     policy: "NONE"
+#     priority: 99
+#     exec_time: 35_000_000
+#     deadline: 100_000_000
+#     period: 100_000_000
+#   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
+
+voxel_grid_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 99
+    exec_time: 1_600_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"  
+
+modular_ndt_matching_FR:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 80_000_000 # ns
+    deadline: 80_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  use_kalman_filter: False
+  tf_x: 3.3
+  tf_y: -0.8
+  tf_z: 0.09
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+ndt_matching:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 80_000_000 # ns
+    deadline: 80_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
+  use_kalman_filter: False
+  tf_x: 3.3
+  tf_y: -0.8
+  tf_z: 0.09
+  tf_roll: 0
+  tf_pitch: 0
+  tf_yaw: 0
+  localizer: "velodyne"
+
+# Detection
+ray_ground_filter_center:
+  rate: 10 
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
+
+lidar_euclidean_cluster_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 50_000_000 # ns
+    deadline: 200_000_000 # ns
+    period: 200_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
+
+vision_darknet_detect:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
+  network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
+  pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3.weights"
+
+# Planning
+op_global_planner:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 45_000_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
+
+  multilap_flag: 1
+
+op_common_params:
+  rollOutDensity: 3.5
+  rollOutsNumber: 2
+  maxVelocity: 7.0
+  maxAcceleration: 10.0
+  maxDeceleration: -10.0
+
+op_trajectory_generator:
+  rate: 20 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 2_900_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
+
+op_trajectory_evaluator:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 3_600_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
+
+  # weightPriority: 0
+  # weightTransition: 5
+  # weightLong: 4
+  # weightLat: 4
+  weightPriority: 1
+  weightTransition: 1
+  weightLong: 1
+  weightLat: 1
+  
+  ImageWidth: 1280
+  ImageHeight: 720
+  SprintDecisionTime: 9999999.0
+  PedestrianStopImgHeightThreshold: 80
+  # 40 increase needed
+  PedestrianImageDetectionRange: 0.4
+
+op_behavior_selector:
+  rate: 100 #100
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 20_800_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
+  distanceToPedestrianThreshold: 15.0
+  sprintSpeed: 10.0
+  obstacleWaitingTimeinIntersection: 2.0
+  turnThreshold: 30.0
+
+op_motion_predictor:
+  rate: 25 #25
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 3_800_000 # ns
+    deadline: 100_000_000 # ns
+    period: 100_000_000 # ns  
+  task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
+
+# Control
+twist_filter:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 2_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
+
+pure_pursuit:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 3_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+  dynamic_params_flag: False
+  dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
+
+# Independent
+twist_gate:
+  rate: 10
+  task_scheduling_configs:
+    policy: "NONE"
+    priority: 20
+    exec_time: 100_000_000 # ns
+    deadline: 2_000_000 # ns
+    period: 100_000_000 # ns
+  task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
+
+  zero_flag: 0 ## Publish target velocity as 0
diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml
index cb127f78..f979d738 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml
@@ -18,7 +18,7 @@ voxel_grid_filter:
   instance_mode: 1
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
@@ -29,20 +29,14 @@ modular_ndt_matching_FR:
   instance_mode: 1
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 80_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 ndt_matching:
-  instance_mode: 1
   use_kalman_filter: False
 
   tf_x: 3.3
@@ -54,24 +48,17 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 80_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
-
-
 
 # Detection
 ray_ground_filter_center:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
@@ -81,37 +68,27 @@ ray_ground_filter_center:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 40
   task_minimum_inter_release_time: 200_000_000
   task_execution_time: 50_000_000
   task_relative_deadline: 200_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 75_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 40
   task_minimum_inter_release_time: 200_000_000
@@ -121,7 +98,7 @@ imm_ukf_pda_track:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 40 #25
   task_minimum_inter_release_time: 100_000_000
@@ -138,7 +115,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 40 #100
   task_minimum_inter_release_time: 100_000_000
@@ -147,7 +124,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 40 #100
   task_minimum_inter_release_time: 100_000_000
@@ -165,7 +142,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 40 #100
   task_minimum_inter_release_time: 100_000_000
@@ -178,7 +155,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 40 #25
   task_minimum_inter_release_time: 100_000_000
@@ -187,9 +164,8 @@ op_motion_predictor:
 
 # Control
 pure_pursuit:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 40 #30
   task_minimum_inter_release_time: 100_000_000
@@ -199,9 +175,8 @@ pure_pursuit:
   dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
 
 twist_filter:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
@@ -209,9 +184,8 @@ twist_filter:
   task_relative_deadline: 100_000_000
 
 twist_gate:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
@@ -221,9 +195,8 @@ twist_gate:
 
 # Others
 lidar_republisher:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
@@ -232,7 +205,7 @@ lidar_republisher:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
@@ -240,9 +213,8 @@ vel_relay:
   task_relative_deadline: 100_000_000
 
 pose_relay:
-  instance_mode: 1
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
@@ -251,7 +223,7 @@ pose_relay:
 
 republish:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
   rate: 40
   task_minimum_inter_release_time: 100_000_000
diff --git a/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml
index 717968c5..e0e4d6ab 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml
@@ -1,5 +1,5 @@
 total_step_num: 9
-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh"
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate_desktop.sh"
 # step_# : 
 #     [0] pacakge name
 #     [1] target name( node name or launch script name )
diff --git a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml
index 1f7f7e4e..b1249827 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml
@@ -1,5 +1,5 @@
 total_step_num: 4
-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/minicar_lane_keeping/terminate.sh"
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/minicar_lane_keeping/terminate_desktop.sh"
 # step_# : 
 #     [0] pacakge name
 #     [1] target name( node name or launch script name )
diff --git a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml
index 17751521..b2066de6 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml
@@ -15,7 +15,7 @@ camera_image:
   frequency: 10
   
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   # rate: 10 # Frequency replaces rate
   task_minimum_inter_release_time: 100000000
@@ -24,7 +24,7 @@ camera_image:
 
 ray_ground_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -34,7 +34,7 @@ ray_ground_filter:
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100000000
@@ -45,22 +45,17 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
   task_execution_time: 70000000
   task_relative_deadline: 100000000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100000000
@@ -78,7 +73,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -87,7 +82,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100000000
@@ -103,7 +98,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 10000000
@@ -117,7 +112,7 @@ op_behavior_selector:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 30 #30
   task_minimum_inter_release_time: 33333333
@@ -128,7 +123,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -137,7 +132,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -148,7 +143,7 @@ twist_gate:
 # Others
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
@@ -157,7 +152,7 @@ vel_relay:
 
 pose_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100000000
diff --git a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml
index 7c9a8b8d..b4235314 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml
@@ -1,5 +1,5 @@
 total_step_num: 5
-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/terminate.sh"
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/terminate_desktop.sh"
 # step_# : 
 #     [0] pacakge name
 #     [1] target name( node name or launch script name )
diff --git a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml
index 59cfa745..ccd8802d 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml
@@ -14,7 +14,7 @@ voxel_grid_filter:
   instance_mode: 0
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -33,22 +33,17 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 80_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Detection
 ray_ground_filter_center:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv"
   rate: 10 
   task_minimum_inter_release_time: 100_000_000
@@ -58,37 +53,27 @@ ray_ground_filter_center:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10
   task_minimum_inter_release_time: 200_000_000
   task_execution_time: 50_000_000
   task_relative_deadline: 200_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 75_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 0
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10
   task_minimum_inter_release_time: 200_000_000
@@ -98,7 +83,7 @@ imm_ukf_pda_track:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100_000_000
@@ -115,7 +100,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100_000_000
@@ -124,7 +109,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100_000_000
@@ -140,7 +125,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100_000_000
@@ -153,7 +138,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100_000_000
@@ -162,9 +147,8 @@ op_motion_predictor:
 
 # Control
 pure_pursuit:
-  instance_mode: 0
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 30 #30
   task_minimum_inter_release_time: 100_000_000
@@ -174,9 +158,8 @@ pure_pursuit:
   dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml"
 
 twist_filter:
-  instance_mode: 0
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -184,9 +167,8 @@ twist_filter:
   task_relative_deadline: 100_000_000
 
 twist_gate:
-  instance_mode: 0
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -196,9 +178,8 @@ twist_gate:
 
 # Others
 lidar_republisher:
-  instance_mode: 0
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -207,7 +188,7 @@ lidar_republisher:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -215,9 +196,8 @@ vel_relay:
   task_relative_deadline: 100_000_000
 
 pose_relay:
-  instance_mode: 0
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -226,7 +206,7 @@ pose_relay:
 
 republish:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
diff --git a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml
index 7bbaa3d4..89161057 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml
@@ -1,5 +1,5 @@
 total_step_num: 5
-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/tutorial/terminate.sh"
+terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/tutorial/terminate_desktop.sh"
 # step_# : 
 #     [0] pacakge name
 #     [1] target name( node name or launch script name )
diff --git a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml
index 5ff124da..39f6cf02 100644
--- a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml
+++ b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml
@@ -12,7 +12,7 @@
 # Localization
 voxel_grid_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 0
+  
   task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -29,17 +29,12 @@ ndt_matching:
   localizer: "velodyne"
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 80_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 1
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"
 
 # Detection
 compare_map_filter:
@@ -48,7 +43,7 @@ compare_map_filter:
   max_clipping_height: 0.5
 
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 200_000_000
@@ -58,37 +53,27 @@ compare_map_filter:
 
 lidar_euclidean_cluster_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"
   rate: 10
   task_minimum_inter_release_time: 200_000_000
   task_execution_time: 50_000_000
   task_relative_deadline: 200_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 1
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv"
 
 vision_darknet_detect:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
   task_execution_time: 75_000_000
   task_relative_deadline: 80_000_000
-  gpu_scheduling_flag: 0
-  gpu_profiling_flag: 1
-  gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv"
-  gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv"
-  gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"
   network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg"
   pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights"
 
 imm_ukf_pda_track:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv"
   rate: 10
   task_minimum_inter_release_time: 200_000_000
@@ -98,7 +83,7 @@ imm_ukf_pda_track:
 # Planning
 op_global_planner:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100_000_000
@@ -115,7 +100,7 @@ op_common_params:
 
 op_trajectory_generator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100_000_000
@@ -124,7 +109,7 @@ op_trajectory_generator:
 
 op_trajectory_evaluator:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100_000_000
@@ -140,7 +125,7 @@ op_trajectory_evaluator:
 
 op_behavior_selector:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv"
   rate: 100 #100
   task_minimum_inter_release_time: 100_000_000
@@ -153,7 +138,7 @@ op_behavior_selector:
 
 op_motion_predictor:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv"
   rate: 25 #25
   task_minimum_inter_release_time: 100_000_000
@@ -163,7 +148,7 @@ op_motion_predictor:
 # Control
 pure_pursuit:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv"
   rate: 30 #30
   task_minimum_inter_release_time: 100_000_000
@@ -174,7 +159,7 @@ pure_pursuit:
 
 twist_filter:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -183,7 +168,7 @@ twist_filter:
 
 twist_gate:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -194,7 +179,7 @@ twist_gate:
 # Others
 lidar_republisher:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -203,7 +188,7 @@ lidar_republisher:
 
 vel_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -212,7 +197,7 @@ vel_relay:
 
 pose_relay:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
@@ -221,7 +206,7 @@ pose_relay:
 
 republish:
   task_scheduling_flag: 0
-  task_profiling_flag: 1
+  
   task_response_time_filename: "~/Documents/profiling/response_time/republish.csv"
   rate: 10
   task_minimum_inter_release_time: 100_000_000
diff --git a/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h b/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h
new file mode 100644
index 00000000..d7b9aaaf
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h
@@ -0,0 +1,33 @@
+#include <ros/ros.h>
+#include <ros_autorunner_lib/ros_autorunner.h>
+
+// Include subscribe message type
+#include <sensor_msgs/PointCloud2.h>
+#include <autoware_msgs/NDTStat.h>
+#include <autoware_msgs/DetectedObjectArray.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#define SLEEP_PERIOD 1
+
+class CarlaAutorunner : public AutorunnerBase{
+private:    
+    ros::NodeHandle     nh_;
+    ROSAutorunner       ros_autorunner_;
+private:
+    virtual void register_subscribers();
+private:
+    void points_raw_cb(const sensor_msgs::PointCloud2& msg);
+    void ndt_pose_cb(const geometry_msgs::PoseStamped& msg);
+    void detection_cb(const autoware_msgs::DetectedObjectArray& msg);
+    void behavior_state_cb(const visualization_msgs::MarkerArray& msg);
+
+public:
+    Sub_v               sub_v_;
+    ros::Publisher      initial_pose_pub_;
+    ros::Publisher      goal_pub_;
+public:
+    CarlaAutorunner() {}
+    CarlaAutorunner(ros::NodeHandle nh) : nh_(nh){}
+    virtual void Run();
+};
diff --git a/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h b/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h
index f1c118d0..8f21bd5e 100644
--- a/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h
+++ b/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h
@@ -6,23 +6,27 @@
 #include <autoware_msgs/NDTStat.h>
 #include <autoware_msgs/DetectedObjectArray.h>
 #include <visualization_msgs/MarkerArray.h>
-#define TOTAL_STEP_NUM 5 // Need to change when total step number is changed
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <rubis_msgs/PointCloud2.h>
+#include <rubis_msgs/PoseTwistStamped.h>
 #define SLEEP_PERIOD 1
 
 class CubetownAutorunner : public AutorunnerBase{
 private:    
     ros::NodeHandle     nh_;
-    ROSAutorunner        ros_autorunner_;
+    ROSAutorunner       ros_autorunner_;
 private:
     virtual void register_subscribers();
 private:
-    void points_raw_cb(const sensor_msgs::PointCloud2& msg);
-    void ndt_stat_cb(const autoware_msgs::NDTStat& msg);
+    void points_raw_cb(const rubis_msgs::PointCloud2& msg);
+    void pose_twist_cb(const rubis_msgs::PoseTwistStamped& msg);
     void detection_cb(const autoware_msgs::DetectedObjectArray& msg);
     void behavior_state_cb(const visualization_msgs::MarkerArray& msg);
 
 public:
     Sub_v               sub_v_;
+    ros::Publisher      initial_pose_pub_;
 public:
     CubetownAutorunner() {}
     CubetownAutorunner(ros::NodeHandle nh) : nh_(nh){}
diff --git a/rubis_ws/src/rubis_autorunner/include/ros_autorunner_lib/ros_autorunner.h b/rubis_ws/src/rubis_autorunner/include/ros_autorunner_lib/ros_autorunner.h
index dd197a05..d5f498cc 100755
--- a/rubis_ws/src/rubis_autorunner/include/ros_autorunner_lib/ros_autorunner.h
+++ b/rubis_ws/src/rubis_autorunner/include/ros_autorunner_lib/ros_autorunner.h
@@ -37,7 +37,7 @@ class ROSAutorunner{
 public:
     ROSAutorunner(){}
     void init(ros::NodeHandle nh, Sub_v sub_v);
-    void Run();         // Execute each steps
+    bool Run();         // Execute each steps
 };
 
 class AutorunnerBase{
diff --git a/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch
new file mode 100644
index 00000000..b5aa81af
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml" />
+    <node pkg="rubis_autorunner" type="carla_full_autorunner" name="carla_full_autorunner" output="screen"/>
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch
new file mode 100644
index 00000000..41f53b5f
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml" />
+    <node pkg="rubis_autorunner" type="carla_lkas_autorunner" name="carla_lkas_autorunner" output="screen" />
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/cubetown_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/cubetown_autorunner.launch
deleted file mode 100644
index e3d788b3..00000000
--- a/rubis_ws/src/rubis_autorunner/launch/cubetown_autorunner.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
-    <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml" />
-    <node pkg="rubis_autorunner" type="cubetown_autorunner" name="cubetown_autorunner" output="screen" />
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/cubetown_autorunner_gpu.launch b/rubis_ws/src/rubis_autorunner/launch/cubetown_autorunner_gpu.launch
deleted file mode 100644
index 7bc88555..00000000
--- a/rubis_ws/src/rubis_autorunner/launch/cubetown_autorunner_gpu.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
-    <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_gpu.yaml" />
-    <node pkg="rubis_autorunner" type="cubetown_autorunner_gpu" name="cubetown_autorunner_gpu" output="screen" />
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/cubetown_full_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/cubetown_full_autorunner.launch
new file mode 100644
index 00000000..7275d758
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/launch/cubetown_full_autorunner.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml" />
+    <node pkg="rubis_autorunner" type="cubetown_full_autorunner" name="cubetown_full_autorunner" output="screen" />
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/cubetown_lkas_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/cubetown_lkas_autorunner.launch
new file mode 100644
index 00000000..7d60e845
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/launch/cubetown_lkas_autorunner.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml" />
+    <node pkg="rubis_autorunner" type="cubetown_lkas_autorunner" name="cubetown_lkas_autorunner" output="screen" />
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/desktop_lane_keeping.launch b/rubis_ws/src/rubis_autorunner/launch/desktop_lane_keeping.launch
index 62dcaa4c..3bc360b6 100644
--- a/rubis_ws/src/rubis_autorunner/launch/desktop_lane_keeping.launch
+++ b/rubis_ws/src/rubis_autorunner/launch/desktop_lane_keeping.launch
@@ -1,4 +1,4 @@
 <launch>
     <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml" />
-    <node pkg="rubis_autorunner" type="desktop_lane_keeping" name="desktop_lane_keeping" output="screen" />
+    <node pkg="rubis_autorunner" type="desktop_lane_keeping" name="desktop_lane_keeping" />
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/launch/exynos_carla_full_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/exynos_carla_full_autorunner.launch
new file mode 100644
index 00000000..0f4027d8
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/launch/exynos_carla_full_autorunner.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam command="load" file="$(find rubis_autorunner)/cfg/carla_autorunner/exynos_carla_full_autorunner.yaml" />
+    <node pkg="rubis_autorunner" type="exynos_carla_full_autorunner" name="exynos_carla_full_autorunner" output="screen"/>
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/exynos_carla_lkas_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/exynos_carla_lkas_autorunner.launch
new file mode 100644
index 00000000..bbe95892
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/launch/exynos_carla_lkas_autorunner.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam command="load" file="$(find rubis_autorunner)cfg/carla_autorunner/exynos_carla_lkas_autorunner.yaml" />
+    <node pkg="rubis_autorunner" type="exynos_carla_lkas_autorunner" name="exynos_carla_lkas_autorunner" output="screen" />
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/lgsvl_triple_lidar_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/lgsvl_triple_lidar_autorunner.launch
index 6760fdc6..ee84098b 100644
--- a/rubis_ws/src/rubis_autorunner/launch/lgsvl_triple_lidar_autorunner.launch
+++ b/rubis_ws/src/rubis_autorunner/launch/lgsvl_triple_lidar_autorunner.launch
@@ -1,4 +1,4 @@
 <launch>
     <rosparam command="load" file="$(env HOME)/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml" />
-    <node pkg="rubis_autorunner" type="lgsvl_triple_lidar_autorunner" name="lgsvl_triple_lidar_autorunner" output="screen" />
+    <node pkg="rubis_autorunner" type="lgsvl_triple_lidar_autorunner" name="lgsvl_triple_lidar_autorunner" />
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/launch/minicar_lane_keeping.launch b/rubis_ws/src/rubis_autorunner/launch/minicar_lane_keeping.launch
index 9c66651f..7f17eacb 100644
--- a/rubis_ws/src/rubis_autorunner/launch/minicar_lane_keeping.launch
+++ b/rubis_ws/src/rubis_autorunner/launch/minicar_lane_keeping.launch
@@ -1,4 +1,4 @@
 <launch>
     <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml" />
-    <node pkg="rubis_autorunner" type="minicar_lane_keeping" name="minicar_lane_keeping" output="screen" />
+    <node pkg="rubis_autorunner" type="minicar_lane_keeping" name="minicar_lane_keeping" />
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/launch/rubis_testbed_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/rubis_testbed_autorunner.launch
index 7c07d513..d3d7e249 100644
--- a/rubis_ws/src/rubis_autorunner/launch/rubis_testbed_autorunner.launch
+++ b/rubis_ws/src/rubis_autorunner/launch/rubis_testbed_autorunner.launch
@@ -1,4 +1,4 @@
 <launch>
     <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml" />
-    <node pkg="rubis_autorunner" type="rubis_testbed_autorunner" name="rubis_testbed_autorunner" output="screen" />
+    <node pkg="rubis_autorunner" type="rubis_testbed_autorunner" name="rubis_testbed_autorunner" />
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/launch/tutorial_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/tutorial_autorunner.launch
index 723a2c3e..a12ef31a 100644
--- a/rubis_ws/src/rubis_autorunner/launch/tutorial_autorunner.launch
+++ b/rubis_ws/src/rubis_autorunner/launch/tutorial_autorunner.launch
@@ -1,4 +1,4 @@
 <launch>
     <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml" />
-    <node pkg="rubis_autorunner" type="tutorial_autorunner" name="tutorial_autorunner" output="screen" />
+    <node pkg="rubis_autorunner" type="tutorial_autorunner" name="tutorial_autorunner" />
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/package.xml b/rubis_ws/src/rubis_autorunner/package.xml
index 4df4bbdc..7266b255 100644
--- a/rubis_ws/src/rubis_autorunner/package.xml
+++ b/rubis_ws/src/rubis_autorunner/package.xml
@@ -6,7 +6,7 @@
 
   <maintainer email="jwhan@todo.todo">jwhan</maintainer>
 
-  <license>TODO</license>
+  <license>Apache 2</license>
 
 
   <buildtool_depend>catkin</buildtool_depend>
@@ -18,7 +18,10 @@
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>autoware_msgs</exec_depend>
   <exec_depend>roscpp</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>  
+  <depend>visualization_msgs</depend>
+  <depend>nmea_msgs</depend>
+  <depend>rubis_msgs</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch
new file mode 100644
index 00000000..b9ba7ca6
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch
@@ -0,0 +1,46 @@
+<launch>
+  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml" />
+
+  <arg name="lidar_input_topic" default="/points_raw_origin"/>
+  <arg name="lidar_output_topic" default="/points_raw"/>
+
+  <!-- Map TF Publisher -->	
+	<!-- <include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" /> -->
+  
+  <!-- Vector Map Loader -->
+  <!-- Default -->
+	<!-- <node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/dtlane.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/lane.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/node.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/point.csv"/> -->
+
+  <!-- carla town04 straight -->
+  <!-- <node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv"/> -->
+  
+  <!-- carla town04 circle(not connected) -->
+  <node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv"/>
+
+  <!-- Point Map Loader -->
+	<include file="$(find map_file)/launch/points_map_loader.launch">
+    <arg name="scene_num" value="noupdate" />
+    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/Town04_Carla.pcd"/>
+  </include>
+
+  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
+  <!-- <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/image_raw_origin out:=/image_raw"  /> -->
+
+  <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
+    <arg name="input_topic" value="$(arg lidar_input_topic)" />
+    <arg name="output_topic" value="$(arg lidar_output_topic)" />
+  </include>
+
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch
similarity index 54%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch
index af5aafc3..2df196f3 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch
@@ -1,11 +1,19 @@
 <launch>
-  <!--TODO: Set LidAR Inital Pose-->
-  <arg name="init_x" value="57.1" />
-  <arg name="init_y" value="1.38" />
-  <arg name="init_z" value="0.48" />
+  <!-- carla town04 default-->
+  <!-- <arg name="init_x" value="258.4" />
+  <arg name="init_y" value="206.5" />
+  <arg name="init_z" value="0.0" />
   <arg name="init_roll" value="0.0" />
   <arg name="init_pitch" value="0.0" />
-  <arg name="init_yaw" value="1.57" />
+  <arg name="init_yaw" value="1.54" /> -->
+
+  <!-- carla town04 circle-->
+  <arg name="init_x" value="314.065" />
+  <arg name="init_y" value="129.676" />
+  <arg name="init_z" value="0.0" />
+  <arg name="init_roll" value="0.0" />
+  <arg name="init_pitch" value="0.0" />
+  <arg name="init_yaw" value="1.54" />
 
   <!-- vel/pose connect -->
   <arg name="topic_pose_stamped" default="/ndt_pose" />
@@ -13,20 +21,18 @@
   <!-- <arg name="topic_twist_stamped" default="/odom_twist" /> -->
 
   <!-- points downsampler -->
-  <include file="$(find points_downsampler)/launch/voxel_grid_filter_params.launch" />
+  <include file="$(find points_downsampler)/launch/voxel_grid_filter_params.launch">
+    <arg name="measurement_range" value="100" />
+    <arg name="leaf_size" value="0.5" />
+  </include>
 
   <!-- ndt matching kalman filter parameters -->
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/kalman_filter.yaml" />
+  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/kalman_filter.yaml" />
 
   <!-- ndt_matching --><!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
   <include file="$(find lidar_localizer)/launch/ndt_matching_params.launch">
     <arg name="method_type" value="0" /> 
-    <arg name="get_height" value="true" />
-    <arg name="init_match_threshold" value="8.0" /> 
-    <arg name="failure_score_diff_threshold" value="10.0" /> 
-    <arg name="recovery_score_diff_threshold" value="1.0" /> 
-    <arg name="failure_pose_diff_threshold" value="4.0" /> 
-    <arg name="recovery_pose_diff_threshold" value="1.0" /> 
+    <arg name="get_height" value="true" /> 
   </include>
 
   <!-- ndt config -->
@@ -41,17 +47,11 @@
     pitch: $(arg init_pitch),
     yaw: $(arg init_yaw),
     use_predict_pose: 1,
-    error_threshold: 1.2,
-    resolution: 4.0,
+    error_threshold: 0.05,
+    resolution: 1.0,
     step_size: 0.5,
-    trans_epsilon: 0.05,
-    max_iterations: 10}
-  '"/> 
-  
-  <!-- vel_pose_mux -->
-  <include file="$(find rubis_pkg)/launch/vel_pose_connect_params.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
+    trans_epsilon: 0.01,
+    max_iterations: 2}
+  '"/>
 
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch
similarity index 60%
rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch
index 59475396..df533eb6 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch
@@ -1,30 +1,27 @@
 <launch>
-  <!-- cubetown -->
-  <arg name="init_x" value="162.6" />
-  <arg name="init_y" value="157.6" />
-  <arg name="init_z" value="0.48" />
+  <!-- Town04 -->
+  <arg name="init_x" value="258.4" />
+  <arg name="init_y" value="206.5" />
+  <arg name="init_z" value="0.0" />
   <arg name="init_roll" value="0.0" />
   <arg name="init_pitch" value="0.0" />
-  <arg name="init_yaw" value="0.0" />
-
-  <!-- vel/pose connect -->
-  <arg name="topic_pose_stamped" default="/gicp_pose" />
-  <arg name="topic_twist_stamped" default="/gicp_vel" /> 
-  <!-- <arg name="topic_twist_stamped" default="/odom_twist" /> -->
+  <arg name="init_yaw" value="1.54" />
 
   <!-- points downsampler -->
-  <include file="$(find points_downsampler)/launch/voxel_grid_filter_params.launch" />
+  <include file="$(find points_downsampler)/launch/points_downsample.launch" />
 
+  <!-- gicp_localizer -->
   <arg name="enable_gnss_backup" default="false" /> 
-
   <arg name="base_frame" default="base_link" doc="Vehicle reference frame" />
+  <!-- <arg name="sensor_frame" default="velodyne" doc="Vehicle reference frame" /> -->
+  <arg name="sensor_frame" default="ego_vehicle/lidar" doc="Vehicle reference frame" />
   <arg name="resolution" default="2.0" doc="The ND voxel grid resolution" />
   <arg name="neighborSearchMethod" default="DIRECT7"/>
 
   <arg name="numthreads" default="4" doc="vgicp threads num" />
-  <arg name="leafsize" default="0.1" doc="point map downsample leafsize" />
+  <arg name="leafsize" default="0.01" doc="point map downsample leafsize" />
 
-  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node" output="screen">
+  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node">
     <param name="init_x" value="$(arg init_x)" />
     <param name="init_y" value="$(arg init_y)" />
     <param name="init_z" value="$(arg init_z)" />
@@ -42,10 +39,4 @@
     <param name="leafsize" value="$(arg leafsize)" />
   </node>
 
-  <!-- vel_pose_mux -->
-  <include file="$(find rubis_pkg)/launch/vel_pose_connect_params.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
-
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch
similarity index 74%
rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch
index 66594cf2..8bb60870 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch
@@ -43,7 +43,7 @@
     <arg name="clustering_distance" value="0.75" />
     <arg name="use_vector_map" value="false" />
     <arg name="wayarea_gridmap_layer" value="wayarea" />
-    <arg name="output_frame" value="velodyne" />
+    <arg name="output_frame" value="ego_vehicle/lidar" />
     <arg name="remove_points_upto" value="0.0" />
     <arg name="use_gpu" value="true" />    
     <arg name="use_multiple_thres" value="false"/>
@@ -53,34 +53,12 @@
   </include>
   
   <!-- vision_darknet_detect -->
-  <include file="$(find vision_darknet_detect)/launch/vision_darknet_detect_parameter.launch">
+  <!-- <include file="$(find vision_darknet_detect)/launch/vision_darknet_detect_parameter.launch">
     <arg name="gpu_device_id" value="0"/>
     <arg name="score_threshold" value="0.5"/>
     <arg name="nms_threshold" value="0.45"/>
     <arg name="names_file" value="$(find vision_darknet_detect)/darknet/cfg/coco.names"/>
     <arg name="camera_id" value="/"/>
     <arg name="image_src" value="/image_raw"/>    
-  </include>
-
-  <!-- imm_ukf_pda_track -->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
-    <arg name="namespace" default="/detection/object_tracker"/>
-    <arg name="tracker_input_topic" default="/detection/lidar_detector/objects_center" />
-    <arg name="tracker_output_topic" default="/detection/object_tracker/objects_center" />
-    <arg name="tracking_frame" default="/world" />
-    <arg name="gating_threshold" default="9.22" />
-    <arg name="gate_probability" default="0.99" />
-    <arg name="detection_probability" default="0.9" />
-    <arg name="life_time_threshold" default="8" />
-    <arg name="static_velocity_threshold" default="0.5" />
-    <arg name="static_num_history_threshold" default="3" />
-    <arg name="prevent_explosion_threshold" default="1000" />
-    <arg name="merge_distance_threshold" default="0.5"/>
-    <arg name="use_sukf" default="false" />
-    <arg name="use_map_info" default="false" />
-    <arg name="lane_direction_chi_threshold" default="2.71" />
-    <arg name="nearest_lane_distance_threshold" default="1.0" />
-    <arg name="vectormap_frame" default="/map" />
-  </include>
-
+  </include> -->
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch
similarity index 83%
rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch
index cf4d32cc..4f5d25e6 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch
@@ -9,15 +9,25 @@
     <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
     <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
     <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
-    <!-- Borregas Avenue -->
+    <!-- Town04 -->
+    <!-- <arg name="use_static_goal"       value="true" />
+    <arg name="goal_pose_x"           value="284.118" />
+    <arg name="goal_pose_y"           value="172.178" />
+    <arg name="goal_pose_z"           value="0.0" />
+    <arg name="goal_ori_x"            value="0.0" />
+    <arg name="goal_ori_y"            value="0.0" />
+    <arg name="goal_ori_z"            value="0.99" />
+    <arg name="goal_ori_w"            value="0.02" /> -->
+
+    <!-- Town04 circle -->
     <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="57.0422019958" />
-    <arg name="goal_pose_y"           value="-5.43001270294" />
+    <arg name="goal_pose_x"           value="302.250" />
+    <arg name="goal_pose_y"           value="118.089" />
     <arg name="goal_pose_z"           value="0.0" />
     <arg name="goal_ori_x"            value="0.0" />
     <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.719615198264" />
-    <arg name="goal_ori_w"            value="0.694373074383" />
+    <arg name="goal_ori_z"            value="-0.015" />
+    <arg name="goal_ori_w"            value="1.000" />
   </include>
 
   <!-- op_common_params  -->
@@ -53,7 +63,9 @@
     <arg name="minVelocity"                     default="0.1" />  
     <arg name="speedProfileFactor"              default="1.2"  />
     <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />  
+    <arg name="smoothingSmoothWeight"           default="0.4"  />
+    <arg name="enableSlowDownOnCurve"           default="true" />
+    <arg name="curveVelocityRatio"           default="0.4" />
   </include>
 
   <!-- op_trajectory_generator -->
@@ -67,8 +79,8 @@
   <!-- op_motion_predictor -->
   <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
     <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/object_tracker/objects_center]" />
-    <arg name="object_tf_list"                  default="[camera,velodyne]" />
+    <arg name="input_object_list"               default="[/detection/lidar_detector/rubis_objects_center]" />
+    <arg name="object_tf_list"                  default="[ego_vehicle/lidar]" />
     <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
     <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
     <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
@@ -82,16 +94,17 @@
 
   <!-- op_trajectory_evaluator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_evaluator_parameter.launch">
+    <arg name="enableDebug"      default="0" />
+
     <arg name="enablePrediction"                default="false" />                
     <arg name="horizontalSafetyDistance"        default="1.2" />
     <arg name="verticalSafetyDistance"          default="0.8" />
-    <arg name="LateralSkipDistance"             default="5.0" />
-    
-    <arg name="PedestrianRightThreshold" default="4.0" />
-    <arg name="PedestrianLeftThreshold" default="4.0" />
-    <arg name="PedestrianImageDetectionRange" default="0.75" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-    <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is not located in this range, our car set speed as max velocity --> 
-    <arg name="VehicleImageWidthThreshold" default="0.018" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
+    <arg name="LateralSkipDistance"             default="5.0" />  
+
+    <!-- Trajectory blocking threshold  -->
+    <arg name="lateralBlockingThreshold"           default="0.0" />
+    <arg name="frontLongitudinalBlockingThreshold"      default="30" />
+    <arg name="rearLongitudinalBlockingThreshold"      default="-5" />
 
     <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
   </include>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch
similarity index 86%
rename from rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch
index 3653c630..59144340 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch
@@ -3,10 +3,10 @@
   <rosparam command="load" file="$(env USER_HOME)/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" />
   
   <!-- pure_pursuit  -->
-  <include file="$(find pure_pursuit)/launch/pure_pursuit.launch">    
+  <include file="$(find pure_pursuit)/launch/pure_pursuit_params.launch">   
     <!-- Core Parameter -->
-    <arg name="lookahead_ratio"               value="1.2"/>
-    <arg name="minimum_lookahead_distance"    value="3.0"/>
+    <arg name="lookahead_ratio"               value="1.5"/>
+    <arg name="minimum_lookahead_distance"    value="10.0"/> 
     
     <arg name="const_velocity"                value="5.0"/>     
     <arg name="const_lookahead_distance"      value="4.0"/>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_1_sensing.launch
new file mode 100644
index 00000000..49b200e5
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_1_sensing.launch
@@ -0,0 +1,43 @@
+<launch>
+  <rosparam command="load" file="$(find rubis_autorunner)/cfg/carla_autorunner/carla_autorunner_params.yaml" />
+
+  <arg name="lidar_input_topic" default="/points_raw_origin"/>
+  <arg name="lidar_output_topic" default="/points_raw"/>
+
+  <!-- Map TF Publisher -->	
+	<!-- <include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" /> -->
+  
+  <!-- Vector Map Loader -->
+  <!-- Default -->
+	<!-- <node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/dtlane.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/lane.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/node.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220824_carla_town04/point.csv"/> -->
+
+  <!-- carla town04 straight -->
+  <!-- <node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv"/> -->
+  
+  <!-- carla town04 circle(not connected) -->
+  <node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv 
+    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv"/>
+
+  <!-- Point Map Loader -->
+	<include file="$(find map_file)/launch/points_map_loader.launch">
+    <arg name="scene_num" value="noupdate" />
+    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/Town04_Carla.pcd"/>
+  </include>
+
+  <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
+    <arg name="input_topic" value="$(arg lidar_input_topic)" />
+    <arg name="output_topic" value="$(arg lidar_output_topic)" />
+  </include>
+
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_2_localization.launch
new file mode 100644
index 00000000..e1d2ee10
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_2_localization.launch
@@ -0,0 +1,68 @@
+<launch>
+  <!-- carla town04 default-->
+  <!-- <arg name="init_x" value="258.4" />
+  <arg name="init_y" value="206.5" />
+  <arg name="init_z" value="0.0" />
+  <arg name="init_roll" value="0.0" />
+  <arg name="init_pitch" value="0.0" />
+  <arg name="init_yaw" value="1.54" /> -->
+
+  <!-- carla town04 circle-->
+  <arg name="init_x" value="314.065" />
+  <arg name="init_y" value="129.676" />
+  <arg name="init_z" value="0.0" />
+  <arg name="init_roll" value="0.0" />
+  <arg name="init_pitch" value="0.0" />
+  <arg name="init_yaw" value="1.54" />
+
+  <!-- vel/pose connect -->
+  <arg name="topic_pose_stamped" default="/ndt_pose" />
+  <arg name="topic_twist_stamped" default="/estimate_twist" /> 
+  <!-- <arg name="topic_twist_stamped" default="/odom_twist" /> -->
+
+  <!-- points downsampler -->
+  <include file="$(find points_downsampler)/launch/voxel_grid_filter_params.launch">
+    <arg name="leaf_size" value="0.5" />
+    <arg name="measurement_range" value="100" />
+  </include>
+
+  <!-- <arg name="leaf_size" default="1.0" />
+  <arg name="measurement_range" default="200" />
+
+  <node pkg="points_downsampler" name="voxel_grid_filter" type="voxel_grid_filter">
+    <param name="input_topic_name" value="points_raw" />
+    <param name="output_topic_name" value="filtered_points" />
+    <param name="leaf_size" value="$(arg leaf_size)" />
+    <param name="measurement_range" value="$(arg measurement_range)" />
+  </node> -->
+
+
+  <!-- ndt matching kalman filter parameters -->
+  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/kalman_filter.yaml" />
+
+  <!-- ndt_matching --><!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
+  <include file="$(find lidar_localizer)/launch/ndt_matching_params.launch">
+    <arg name="method_type" value="0" /> 
+    <arg name="get_height" value="true" /> 
+  </include>
+
+  <!-- ndt config -->
+  <node pkg="rostopic" type="rostopic" name="config_ndt"
+  args="pub /config/ndt autoware_config_msgs/ConfigNDT
+  '{header: {seq: 8, stamp: {secs: 0, nsecs: 0}, frame_id: ''},
+    init_pos_gnss: 0,
+    x: $(arg init_x),
+    y: $(arg init_y),
+    z: $(arg init_z),
+    roll: $(arg init_roll), 
+    pitch: $(arg init_pitch),
+    yaw: $(arg init_yaw),
+    use_predict_pose: 1,
+    error_threshold: 0.05,
+    resolution: 1.0,
+    step_size: 0.5,
+    trans_epsilon: 0.01,
+    max_iterations: 2}
+  '"/>
+
+</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_3_detection.launch
similarity index 59%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_3_detection.launch
index 66594cf2..ef383011 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_3_detection.launch
@@ -10,15 +10,15 @@
   <include file="$(find points_preprocessor)/launch/ray_ground_filter_params.launch">
     <arg name="input_point_topic" value="/points_raw" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
     <arg name="base_frame" value="base_link" />  <!-- Coordinate system to perform transform (default base_link) -->
-    <arg name="max_clipping_height" value="5" />  <!-- Remove Points above this height value (default 2.0 meters) -->
+    <arg name="max_clipping_height" value="2.0" />  <!-- Remove Points above this height value (default 2.0 meters) -->
     <arg name="min_clipping_height" value="-0.5" />  <!-- Remove Points below this height value (default -0.5 meters) -->
-    <arg name="min_point_distance" value="3.5" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
+    <arg name="min_point_distance" value="1.85" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
     <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
     <arg name="concentric_divider_distance" value="0.0" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
     <arg name="local_max_slope" value="8" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
     <arg name="general_max_slope" value="1" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
     <arg name="min_height_threshold" value="0.05" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
-    <arg name="reclass_distance_threshold" value="0.02" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
+    <arg name="reclass_distance_threshold" value="0.2" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
     <arg name="no_ground_point_topic" value="/points_no_ground" />
     <arg name="ground_point_topic" value="/points_ground" />
   </include>
@@ -43,7 +43,7 @@
     <arg name="clustering_distance" value="0.75" />
     <arg name="use_vector_map" value="false" />
     <arg name="wayarea_gridmap_layer" value="wayarea" />
-    <arg name="output_frame" value="velodyne" />
+    <arg name="output_frame" value="ego_vehicle/lidar" />
     <arg name="remove_points_upto" value="0.0" />
     <arg name="use_gpu" value="true" />    
     <arg name="use_multiple_thres" value="false"/>
@@ -51,36 +51,4 @@
     <arg name="clustering_distances"
          value="[0.5,1.1,1.6,2.1,2.6]"/>    
   </include>
-  
-  <!-- vision_darknet_detect -->
-  <include file="$(find vision_darknet_detect)/launch/vision_darknet_detect_parameter.launch">
-    <arg name="gpu_device_id" value="0"/>
-    <arg name="score_threshold" value="0.5"/>
-    <arg name="nms_threshold" value="0.45"/>
-    <arg name="names_file" value="$(find vision_darknet_detect)/darknet/cfg/coco.names"/>
-    <arg name="camera_id" value="/"/>
-    <arg name="image_src" value="/image_raw"/>    
-  </include>
-
-  <!-- imm_ukf_pda_track -->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
-    <arg name="namespace" default="/detection/object_tracker"/>
-    <arg name="tracker_input_topic" default="/detection/lidar_detector/objects_center" />
-    <arg name="tracker_output_topic" default="/detection/object_tracker/objects_center" />
-    <arg name="tracking_frame" default="/world" />
-    <arg name="gating_threshold" default="9.22" />
-    <arg name="gate_probability" default="0.99" />
-    <arg name="detection_probability" default="0.9" />
-    <arg name="life_time_threshold" default="8" />
-    <arg name="static_velocity_threshold" default="0.5" />
-    <arg name="static_num_history_threshold" default="3" />
-    <arg name="prevent_explosion_threshold" default="1000" />
-    <arg name="merge_distance_threshold" default="0.5"/>
-    <arg name="use_sukf" default="false" />
-    <arg name="use_map_info" default="false" />
-    <arg name="lane_direction_chi_threshold" default="2.71" />
-    <arg name="nearest_lane_distance_threshold" default="1.0" />
-    <arg name="vectormap_frame" default="/map" />
-  </include>
-
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_4_planning.launch
similarity index 81%
rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_4_planning.launch
index cf4d32cc..315623ae 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_4_planning.launch
@@ -9,15 +9,25 @@
     <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
     <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
     <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
-    <!-- Borregas Avenue -->
-    <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="57.0422019958" />
-    <arg name="goal_pose_y"           value="-5.43001270294" />
+    <!-- Town04 -->
+    <!-- <arg name="use_static_goal"       value="true" />
+    <arg name="goal_pose_x"           value="284.118" />
+    <arg name="goal_pose_y"           value="172.178" />
     <arg name="goal_pose_z"           value="0.0" />
     <arg name="goal_ori_x"            value="0.0" />
     <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.719615198264" />
-    <arg name="goal_ori_w"            value="0.694373074383" />
+    <arg name="goal_ori_z"            value="0.99" />
+    <arg name="goal_ori_w"            value="0.02" /> -->
+
+    <!-- Town04 circle -->
+    <arg name="use_static_goal"       value="false" />
+    <arg name="goal_pose_x"           value="302.250" />
+    <arg name="goal_pose_y"           value="118.089" />
+    <arg name="goal_pose_z"           value="0.0" />
+    <arg name="goal_ori_x"            value="0.0" />
+    <arg name="goal_ori_y"            value="0.0" />
+    <arg name="goal_ori_z"            value="-0.015" />
+    <arg name="goal_ori_w"            value="1.000" />
   </include>
 
   <!-- op_common_params  -->
@@ -53,13 +63,15 @@
     <arg name="minVelocity"                     default="0.1" />  
     <arg name="speedProfileFactor"              default="1.2"  />
     <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />  
+    <arg name="smoothingSmoothWeight"           default="0.4"  />
+    <arg name="enableSlowDownOnCurve"           default="true" />
+    <arg name="curveVelocityRatio"           default="0.4" />
   </include>
 
   <!-- op_trajectory_generator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
     <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
+    <arg name="samplingOutMargin"               default="20" />        <!-- Roll In Margin -->
     <arg name="samplingSpeedFactor"             default="0.25" />    
     <arg name="enableHeadingSmoothing"          default="false" />
   </include>
@@ -67,8 +79,8 @@
   <!-- op_motion_predictor -->
   <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
     <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/object_tracker/objects_center]" />
-    <arg name="object_tf_list"                  default="[camera,velodyne]" />
+    <arg name="input_object_list"               default="[/detection/lidar_detector/rubis_objects_center]" />
+    <arg name="object_tf_list"                  default="[ego_vehicle/lidar]" />
     <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
     <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
     <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
@@ -82,16 +94,17 @@
 
   <!-- op_trajectory_evaluator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_evaluator_parameter.launch">
+    <arg name="enableDebug"      default="0" />
+
     <arg name="enablePrediction"                default="false" />                
     <arg name="horizontalSafetyDistance"        default="1.2" />
     <arg name="verticalSafetyDistance"          default="0.8" />
-    <arg name="LateralSkipDistance"             default="5.0" />
-    
-    <arg name="PedestrianRightThreshold" default="4.0" />
-    <arg name="PedestrianLeftThreshold" default="4.0" />
-    <arg name="PedestrianImageDetectionRange" default="0.75" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-    <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is not located in this range, our car set speed as max velocity --> 
-    <arg name="VehicleImageWidthThreshold" default="0.018" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
+    <arg name="LateralSkipDistance"             default="5.0" />  
+
+    <!-- Trajectory blocking threshold  -->
+    <arg name="lateralBlockingThreshold"           default="0.5" />
+    <arg name="frontLongitudinalBlockingThreshold"      default="30" />
+    <arg name="rearLongitudinalBlockingThreshold"      default="-5" />
 
     <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
   </include>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_5_control.launch
similarity index 84%
rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch
rename to rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_5_control.launch
index 1fa79612..59144340 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_5_control.launch
@@ -3,10 +3,10 @@
   <rosparam command="load" file="$(env USER_HOME)/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" />
   
   <!-- pure_pursuit  -->
-  <include file="$(find pure_pursuit)/launch/pure_pursuit.launch">    
+  <include file="$(find pure_pursuit)/launch/pure_pursuit_params.launch">   
     <!-- Core Parameter -->
     <arg name="lookahead_ratio"               value="1.5"/>
-    <arg name="minimum_lookahead_distance"    value="12.0"/>
+    <arg name="minimum_lookahead_distance"    value="10.0"/> 
     
     <arg name="const_velocity"                value="5.0"/>     
     <arg name="const_lookahead_distance"      value="4.0"/>
@@ -16,7 +16,7 @@
   </include>
 
   <!-- twist_filter -->
-  <include file="$(find twist_filter)/launch/twist_filter.launch">    
+  <include file="$(find twist_filter)/launch/twist_filter_params.launch">    
     <!-- For twist_filter -->
     <arg name="wheel_base" default="2.7" />
     <arg name="lateral_accel_limit" default="5.0" />
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch
index 2f156bae..b25f38f2 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch
@@ -1,56 +1,64 @@
 <launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml" />
+  <rosparam command="load"
+    file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml" />
+  <arg name="lidar_input_topic" default="/points_raw_origin" />
+  <arg name="lidar_output_topic" default="/points_raw" />
+  <arg name="node_name" default="lidar_republisher" />
+  <arg name="lidar_output_topic_redundant" default="/points_raw_redundant" />
 
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
+  <!-- Map TF Publisher -->
+  <include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
 
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
   <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+  <node pkg="map_file" type="vector_map_loader" name="vector_map_loader"
+    args="
 	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/dtlane.csv 
 	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/idx.csv 
 	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/lane.csv 
 	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/point.csv"/>
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/point.csv" />
 
   <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
+  <include file="$(find map_file)/launch/points_map_loader.launch">
     <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/CubeTown.pcd"/>
+    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/CubeTown.pcd" />
   </include>
 
-  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
+  <node name="svl_camera_top" type="republish" pkg="image_transport" output="screen"
+    args="compressed in:=/svl_image_top raw out:=/republished_image_top" />
+  <!-- <node name="svl_camera_right" type="republish" pkg="image_transport" output="screen"
+  args="compressed in:=/svl_image_right raw out:=/republished_image_right"  />
+  <node name="svl_camera_left" type="republish" pkg="image_transport" output="screen"
+  args="compressed in:=/svl_image_left raw out:=/republished_image_left"  />
+  <node name="svl_camera_rear" type="republish" pkg="image_transport" output="screen"
+  args="compressed in:=/svl_image_rear raw out:=/republished_image_rear"  /> -->
 
-  <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
-    <arg name="input_topic" value="$(arg lidar_input_topic)" />
-    <arg name="output_topic" value="$(arg lidar_output_topic)" />
+  <include file="$(find svl_pkg)/launch/svl_sensing.launch">
+    <arg name="lidar_topic" value="/points_raw_origin" />
+    <arg name="odom_topic" value="/odom" />
+    <arg name="image_topic_1" value="/republished_image_top" />
+    <!-- <arg name="image_topic_2" value="/republished_image_right" /> -->
+    <!-- <arg name="image_topic_3" value="/republished_image_left" /> -->
+    <!-- <arg name="image_topic_4" value="/republished_image_rear" /> -->
   </include>
 
-  <!-- Normal LiDAR -->
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="1.2 0 2.0 0 0 0 base_link velodyne 10"/>
-  
-  <!-- LiDAR Offset -->
-  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -1.0 0.3 -0.785 0 0 base_link velodyne 10"/> -->
+  <!-- <include file="$(find vision_workloads)/launch/lane_detector.launch">
+    <arg name="debug" value="false" />
+    <arg name="node_name" value="lane_detector_rear"/>
+    <arg name="input_topic" value="/image_raw4"/>
+    <arg name="filter_image" value="false" />
+    <arg name="gray_scale" value="true" />
+    <arg name="gaussian_blur" value="false" />
+    <arg name="canny" value="true" />
+    <arg name="roi" value="true" />
+  </include> -->
 
-  <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gps base_link 10"/> -->
-  <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" />
 
-  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss" args="0.0 0.0 0.0 0.0 0.0 0.0 velodyne gnss 10" /> -->
-
-  <!-- gnss localizer -->
-  <rosparam command="load" file="$(env USER_HOME)/autoware.ai/autoware_files/transformation/2022-10-05_pose_info_w_theta+noise_2m.yaml" />
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
-    <param name="plane" value="0"/>
-    <param name="enable_noise" value="true"/>
-    <param name="max_noise" value="2.0"/>
-    <param name="enable_offset" value="true"/>    
-    <param name="offset_bx" value="10032.7"/>
-    <param name="offset_by" value="-21925.6"/>
-    <param name="offset_theta" value="37.2"/>
-  </node>
-
-</launch>
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne"
+    args="1.2 0 2.0 0 0 0 base_link velodyne 10" />
+  <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0
+  gps base_link 10"/> -->
+  <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera"
+    args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" />
 
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch
index 44835cc4..2f87ac46 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch
@@ -7,11 +7,6 @@
   <arg name="init_pitch" value="0.0" />
   <arg name="init_yaw" value="1.57" />
 
-  <!-- vel/pose connect -->
-  <arg name="topic_pose_stamped" default="/ndt_pose" />
-  <arg name="topic_twist_stamped" default="/estimate_twist" /> 
-  <!-- <arg name="topic_twist_stamped" default="/odom_twist" /> -->
-
   <!-- points downsampler -->
   <include file="$(find points_downsampler)/launch/voxel_grid_filter_params.launch" />
 
@@ -20,8 +15,9 @@
 
   <!-- ndt_matching --><!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
   <include file="$(find lidar_localizer)/launch/ndt_matching_params.launch">
-    <arg name="method_type" value="0" /> 
+    <arg name="method_type" value="1" /> 
     <arg name="get_height" value="true" /> 
+    <arg name="use_svl_gnss" value="true"/>
   </include>
 
   <!-- ndt config -->
@@ -36,17 +32,11 @@
     pitch: $(arg init_pitch),
     yaw: $(arg init_yaw),
     use_predict_pose: 1,
-    error_threshold: 0.01,
-    resolution: 3.0,
-    step_size: 0.3,
+    error_threshold: 0.05,
+    resolution: 1.0,
+    step_size: 0.5,
     trans_epsilon: 0.01,
-    max_iterations: 10}
+    max_iterations: 2}
   '"/>
 
-  <!-- vel_pose_mux -->
-  <include file="$(find rubis_pkg)/launch/vel_pose_connect_params.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
-
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch
index 66594cf2..49b5c1fe 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch
@@ -1,30 +1,34 @@
 <launch>
-  <!-- Ground Filter -->
-  <!-- <include file="$(find points_preprocessor)/launch/compare_map_filter_params.launch">
-     <arg name="input_point_topic" value="/points_raw" />
-    <arg name="input_map_topic" value="/points_map" />
-    <arg name="output_match_topic" value="/points_ground_center" />
-    <arg name="output_unmatch_topic" value="/points_no_ground_center" />
-  </include> -->
-
   <include file="$(find points_preprocessor)/launch/ray_ground_filter_params.launch">
-    <arg name="input_point_topic" value="/points_raw" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
-    <arg name="base_frame" value="base_link" />  <!-- Coordinate system to perform transform (default base_link) -->
-    <arg name="max_clipping_height" value="5" />  <!-- Remove Points above this height value (default 2.0 meters) -->
-    <arg name="min_clipping_height" value="-0.5" />  <!-- Remove Points below this height value (default -0.5 meters) -->
-    <arg name="min_point_distance" value="3.5" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
-    <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
-    <arg name="concentric_divider_distance" value="0.0" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
-    <arg name="local_max_slope" value="8" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
-    <arg name="general_max_slope" value="1" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
-    <arg name="min_height_threshold" value="0.05" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
-    <arg name="reclass_distance_threshold" value="0.02" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
+    <arg name="input_point_topic" value="/points_raw" />  <!-- input_point_topic, ground filtering
+    will be performed over the pointcloud in this topic. -->
+    <arg name="base_frame" value="base_link" />  <!-- Coordinate system to perform transform (default
+    base_link) -->
+    <arg name="max_clipping_height" value="5" />  <!-- Remove Points above this height value (default
+    2.0 meters) -->
+    <arg name="min_clipping_height" value="-0.5" />  <!-- Remove Points below this height value
+    (default -0.5 meters) -->
+    <arg name="min_point_distance" value="3.5" />  <!-- Removes Points closer than this distance from
+    the sensor origin (default 1.85 meters) -->
+    <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY
+    Plane (default 0.08 degrees)-->
+    <arg name="concentric_divider_distance" value="0.0" />  <!-- Distance of each concentric division
+    on the XY Plane (default 0.0 meters) -->
+    <arg name="local_max_slope" value="8" />  <!-- Max Slope of the ground between Points (default 8
+    degrees) -->
+    <arg name="general_max_slope" value="1" />  <!-- Max Slope of the ground in the entire PointCloud,
+    used when reclassification occurs (default 5 degrees)-->
+    <arg name="min_height_threshold" value="0.05" />  <!-- Minimum height threshold between points
+    (default 0.05 meters)-->
+    <arg name="reclass_distance_threshold" value="0.02" />  <!-- Distance between points at which re
+    classification will occur (default 0.2 meters)-->
     <arg name="no_ground_point_topic" value="/points_no_ground" />
     <arg name="ground_point_topic" value="/points_ground" />
   </include>
-  
+
   <!-- lidar_euclidean_cluster_detect-->
-  <include file="$(find lidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch">
+  <include
+    file="$(find lidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch">
     <arg name="points_node" value="/points_no_ground_center" />
     <arg name="remove_ground" value="true" />
     <arg name="downsample_cloud" value="false" />
@@ -45,42 +49,22 @@
     <arg name="wayarea_gridmap_layer" value="wayarea" />
     <arg name="output_frame" value="velodyne" />
     <arg name="remove_points_upto" value="0.0" />
-    <arg name="use_gpu" value="true" />    
-    <arg name="use_multiple_thres" value="false"/>
-    <arg name="clustering_ranges" value="[15,30,45,60]"/>
+    <arg name="use_gpu" value="false" />
+    <arg name="use_multiple_thres" value="false" />
+    <arg name="clustering_ranges" value="[15,30,45,60]" />
     <arg name="clustering_distances"
-         value="[0.5,1.1,1.6,2.1,2.6]"/>    
-  </include>
-  
-  <!-- vision_darknet_detect -->
-  <include file="$(find vision_darknet_detect)/launch/vision_darknet_detect_parameter.launch">
-    <arg name="gpu_device_id" value="0"/>
-    <arg name="score_threshold" value="0.5"/>
-    <arg name="nms_threshold" value="0.45"/>
-    <arg name="names_file" value="$(find vision_darknet_detect)/darknet/cfg/coco.names"/>
-    <arg name="camera_id" value="/"/>
-    <arg name="image_src" value="/image_raw"/>    
+      value="[0.5,1.1,1.6,2.1,2.6]" />
   </include>
 
-  <!-- imm_ukf_pda_track -->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
-    <arg name="namespace" default="/detection/object_tracker"/>
-    <arg name="tracker_input_topic" default="/detection/lidar_detector/objects_center" />
-    <arg name="tracker_output_topic" default="/detection/object_tracker/objects_center" />
-    <arg name="tracking_frame" default="/world" />
-    <arg name="gating_threshold" default="9.22" />
-    <arg name="gate_probability" default="0.99" />
-    <arg name="detection_probability" default="0.9" />
-    <arg name="life_time_threshold" default="8" />
-    <arg name="static_velocity_threshold" default="0.5" />
-    <arg name="static_num_history_threshold" default="3" />
-    <arg name="prevent_explosion_threshold" default="1000" />
-    <arg name="merge_distance_threshold" default="0.5"/>
-    <arg name="use_sukf" default="false" />
-    <arg name="use_map_info" default="false" />
-    <arg name="lane_direction_chi_threshold" default="2.71" />
-    <arg name="nearest_lane_distance_threshold" default="1.0" />
-    <arg name="vectormap_frame" default="/map" />
+  <include file="$(find vision_workloads)/launch/lane_detector.launch">
+    <arg name="debug" value="true" />
+    <arg name="node_name" value="lane_detector_top" />
+    <arg name="input_topic" value="/image_raw1" />
+    <arg name="filter_image" value="false" />
+    <arg name="gray_scale" value="true" />
+    <arg name="gaussian_blur" value="false" />
+    <arg name="canny" value="true" />
+    <arg name="roi" value="true" />
   </include>
 
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch
index ee35d158..289164bc 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch
@@ -1,99 +1,97 @@
 <launch>
   <!-- op_global_planning -->
   <include file="$(find op_global_planner)/launch/op_global_planner.launch">
-    <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
-    <arg name="enableSmoothing"       value="false" /> <!-- 1 or 0 -->
-    <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
-    <arg name="enableRvizInput"       value="true" /> <!-- 1 or 0 -->
-    <arg name="enableReplan"          value="false" /> <!-- 1 or 0 -->  
-    <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
-    <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
-    <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
+    <arg name="pathDensity" value="1" /> <!-- distance between each two waypoints-->
+    <arg name="enableSmoothing" value="false" /> <!-- 1 or 0 -->
+    <arg name="enableLaneChange" value="false" /> <!-- 1 or 0 -->
+    <arg name="enableRvizInput" value="true" /> <!-- 1 or 0 -->
+    <arg name="enableReplan" value="false" /> <!-- 1 or 0 -->
+    <arg name="velocitySource" value="1" /> <!-- read velocities from (0- Odometry, 1- autoware
+    current_velocities, 2- car_info) "" -->
+    <arg name="mapSource" value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
+    <arg name="mapFileName" value="" /> <!-- incase of kml map source -->
+    <arg name="use_svl_sensor" value="true" />
     <!-- Borregas Avenue -->
-    <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="57.0422019958" />
-    <arg name="goal_pose_y"           value="-5.43001270294" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.719615198264" />
-    <arg name="goal_ori_w"            value="0.694373074383" />
+    <arg name="use_static_goal" value="true" />
+    <arg name="goal_pose_x" value="57.0422019958" />
+    <arg name="goal_pose_y" value="-5.43001270294" />
+    <arg name="goal_pose_z" value="0.0" />
+    <arg name="goal_ori_x" value="0.0" />
+    <arg name="goal_ori_y" value="0.0" />
+    <arg name="goal_ori_z" value="0.719615198264" />
+    <arg name="goal_ori_w" value="0.694373074383" />
   </include>
 
   <!-- op_common_params  -->
   <include file="$(find op_local_planner)/launch/op_common_params_parameter.launch">
     <!-- Included in app window -->
-    <arg name="horizonDistance"                 default="120"  />     <!-- Horizon -->
-    <arg name="maxLocalPlanDistance"            default="80" />       <!-- Plan Distance-->
-    <arg name="pathDensity"                     default="0.5" />      <!-- Path Density-->
-    <arg name="enableFollowing"                 default="true" />     <!-- Enable Following -->
-    <arg name="enableSwerving"                  default="true"  />    <!-- Enable Avoidance -->
-    <arg name="minFollowingDistance"            default="30.0"  />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->  
-    <arg name="minDistanceToAvoid"              default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
-    <arg name="maxDistanceToAvoid"              default="0.1"  />     <!-- Avoidnace Limit--> <!-- should be smaller than minDistanceToAvoid -->
-    <arg name="enableStopSignBehavior"          default="false" />    <!-- Enable Stop Sign Stop-->
-    <arg name="enableTrafficLightBehavior"      default="true" />     <!-- Enable Traffic Light -->
-    <arg name="enableLaneChange"                default="false" />    <!-- Enable Lane Change -->
-    <arg name="horizontalSafetyDistance"        default="1"  />       <!-- Lateral Safety --> 
-    <arg name="verticalSafetyDistance"          default="2"  />       <!-- Longitudinal Safet y-->
-    <arg name="velocitySource"                  default="1" />        <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->   
+    <arg name="horizonDistance" default="120" />     <!-- Horizon -->
+    <arg name="maxLocalPlanDistance" default="80" />       <!-- Plan Distance-->
+    <arg name="pathDensity" default="0.5" />      <!-- Path Density-->
+    <arg name="enableFollowing" default="true" />     <!-- Enable Following -->
+    <arg name="enableSwerving" default="true" />    <!-- Enable Avoidance -->
+    <arg name="minFollowingDistance" default="30.0" />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->
+    <arg name="minDistanceToAvoid" default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance
+    and larger than maxDistanceToAvoid -->
+    <arg name="maxDistanceToAvoid" default="0.1" />     <!-- Avoidnace Limit--> <!-- should be smaller than minDistanceToAvoid -->
+    <arg name="enableStopSignBehavior" default="false" />    <!-- Enable Stop Sign Stop-->
+    <arg name="enableTrafficLightBehavior" default="true" />     <!-- Enable Traffic Light -->
+    <arg name="enableLaneChange" default="false" />    <!-- Enable Lane Change -->
+    <arg name="horizontalSafetyDistance" default="1" />       <!-- Lateral Safety -->
+    <arg name="verticalSafetyDistance" default="2" />       <!-- Longitudinal Safet y-->
+    <arg name="velocitySource" default="1" />        <!-- read velocities from (0- Odometry, 1- autoware
+    current_velocities, 2- car_info) "" -->
     <!-- Vehicle Specification -->
-    <arg name="width"                           default="1.85"  />
-    <arg name="length"                          default="4.2"  />
-    <arg name="wheelBaseLength"                 default="2.7"  />
-    <arg name="turningRadius"                   default="5.2"  />
-    <arg name="maxSteerAngle"                   default="0.45" />
-    <arg name="steeringDelay"                   default="1.2" />
+    <arg name="width" default="1.85" />
+    <arg name="length" default="4.2" />
+    <arg name="wheelBaseLength" default="2.7" />
+    <arg name="turningRadius" default="5.2" />
+    <arg name="maxSteerAngle" default="0.45" />
+    <arg name="steeringDelay" default="1.2" />
     <!-- Not included in app window -->
-    <arg name="minPursuiteDistance"             default="3.0"  />
-    <arg name="additionalBrakingDistance"       default="5.0"  />
-    <arg name="giveUpDistance"                  default="-4.0"  />
-    <arg name="mapSource"                       default="0" />        <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
-    <arg name="mapFileName"                     default="" />
-    <arg name="minVelocity"                     default="0.1" />  
-    <arg name="speedProfileFactor"              default="1.2"  />
-    <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />  
+    <arg name="minPursuiteDistance" default="3.0" />
+    <arg name="additionalBrakingDistance" default="5.0" />
+    <arg name="giveUpDistance" default="-4.0" />
+    <arg name="mapSource" default="0" />        <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
+    <arg name="mapFileName" default="" />
+    <arg name="minVelocity" default="0.1" />
+    <arg name="speedProfileFactor" default="1.2" />
+    <arg name="smoothingDataWeight" default="0.45" />
+    <arg name="smoothingSmoothWeight" default="0.4" />
+    <arg name="enableSlowDownOnCurve" default="false" />
+    <arg name="curveVelocityRatio" default="1.0" />
   </include>
 
   <!-- op_trajectory_generator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
-    <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
-    <arg name="samplingSpeedFactor"             default="0.25" />    
-    <arg name="enableHeadingSmoothing"          default="false" />
-  </include>
-
-  <!-- op_motion_predictor -->
-  <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
-    <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/object_tracker/objects_center]" />
-    <arg name="object_tf_list"                  default="[camera,velodyne]" />
-    <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
-    <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
-    <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
-    <arg name="prediction_distance"             default="25.0"/>      <!-- Prediction distance-->
-    <arg name="enableStepByStepSignal"          default="false" />    <!-- Enable Step by Step Test-->
-    <arg name="enableParticleFilterPrediction"  default="false" />    <!-- Enable Particle Filter prediction -->
-    <!-- Not included in app window -->   
-    <arg name="distanceBetweenCurbs"            default="1.5" />
-    <arg name="visualizationTime"               default="0.25" />
+    <arg name="samplingTipMargin" default="4" />       <!-- Tip Margin -->
+    <arg name="samplingOutMargin" default="8" />        <!-- Roll In Margin -->
+    <arg name="samplingSpeedFactor" default="0.25" />
+    <arg name="enableHeadingSmoothing" default="false" />
   </include>
 
   <!-- op_trajectory_evaluator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_evaluator_parameter.launch">
-    <arg name="enablePrediction"                default="false" />                
-    <arg name="horizontalSafetyDistance"        default="1.2" />
-    <arg name="verticalSafetyDistance"          default="0.8" />
-    <arg name="LateralSkipDistance"             default="5.0" />  
+    <arg name="enableDebug" default="0" />
+    <arg name="laneTopic" default="lane_detector_top_lane" />
+
+    <arg name="enablePrediction" default="false" />
+    <arg name="horizontalSafetyDistance" default="1.2" />
+    <arg name="verticalSafetyDistance" default="0.8" />
+    <arg name="LateralSkipDistance" default="5.0" />
+
+    <!-- Trajectory blocking threshold  -->
+    <arg name="lateralBlockingThreshold" default="1.5" />
+    <arg name="frontLongitudinalBlockingThreshold" default="30" />
+    <arg name="rearLongitudinalBlockingThreshold" default="-5" />
 
-    <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
+    <arg name="intersectionFile" default="crossing_test/crossing_intersection.yaml" />
   </include>
 
   <!-- op_behavior_selector -->
   <include file="$(find op_local_planner)/launch/op_behavior_selector_parameter.launch">
-    <arg name="evidence_tust_number"            default="25"/>
-    <arg name="trafficLightFile"                default="crossing_test/crossing_traffic_light.yaml" />
-    <arg name="stopLineFile"                    default="crossing_test/crossing_stop_line.yaml" />
+    <arg name="evidence_tust_number" default="25" />
+    <arg name="trafficLightFile" default="crossing_test/crossing_traffic_light.yaml" />
+    <arg name="stopLineFile" default="crossing_test/crossing_stop_line.yaml" />
   </include>
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_5_control.launch
index d7eb35a6..dbcacb76 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_5_control.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_5_control.launch
@@ -6,13 +6,14 @@
   <include file="$(find pure_pursuit)/launch/pure_pursuit_params.launch">   
     <!-- Core Parameter -->
     <arg name="lookahead_ratio"               value="1.5"/>
-    <arg name="minimum_lookahead_distance"    value="12.0"/>
+    <arg name="minimum_lookahead_distance"    value="6.0"/>
     
     <arg name="const_velocity"                value="5.0"/>     
     <arg name="const_lookahead_distance"      value="4.0"/>
     <arg name="is_linear_interpolation"       value="True"/>
     <arg name="publishes_for_steering_robot"  value="True"/>
     <arg name="add_virtual_end_waypoints"     value="False"/>
+    <arg name="wait_until_reaching_max_velocity" value="True" /> 
   </include>
 
   <!-- twist_filter -->
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch
index 68b2046e..6fbbc3a2 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch
@@ -15,7 +15,6 @@
   <arg name="output_log" default="false" />
   <arg name="leaf_size" default="1.0" />
   <arg name="measurement_range" default="200" />
-  <arg name="instance_mode" default="0"/>
 
   <arg name="method_type" default="0"/>
   <arg name="max_iter" default="10"/>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch
index d7dd14fc..b399ee97 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch
@@ -19,26 +19,21 @@
   </include>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
-
-  <arg name="instance_mode" default="0"/>
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
 
   <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher_FL">
     <param name="/input_topic" value="points_raw_FL" />
     <param name="/output_topic" value="points_raw_FL_repub" />
-    <param name="/instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher_FR">
     <param name="/input_topic" value="points_raw_FR" />
     <param name="/output_topic" value="points_raw_FR_repub" />
-    <param name="/instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher_B">
     <param name="/input_topic" value="points_raw_B" />
     <param name="/output_topic" value="points_raw_B_repub" />
-    <param name="/instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <node pkg="tf" type="static_transform_publisher" name="vel_to_velodyne_FR" args="0 0 0 0 0 0 velodyne_FR velodyne 10"/>
@@ -53,7 +48,7 @@
 
   <!-- gnss localizer -->
   <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node>
 
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch
index e7d3fd19..05ed4386 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch
@@ -15,7 +15,6 @@
   <arg name="output_log" default="false" />
   <arg name="leaf_size" default="0.1" />
   <arg name="measurement_range" default="200" />
-  <arg name="instance_mode" default="0"/>
 
   <arg name="method_type" default="0"/>
   <arg name="max_iter" default="10"/>
@@ -33,7 +32,6 @@
     <param name="leaf_size" value="$(arg leaf_size)" />
     <param name="measurement_range" value="$(arg measurement_range)" />
     <param name="output_log" value="$(arg output_log)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <node pkg="lidar_localizer" type="modular_ndt_matching" name="modular_ndt_matching_FL">
@@ -78,10 +76,9 @@
     <param name="leaf_size" value="$(arg leaf_size)" />
     <param name="measurement_range" value="$(arg measurement_range)" />
     <param name="output_log" value="$(arg output_log)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 
-  <node pkg="lidar_localizer" type="modular_ndt_matching" name="modular_ndt_matching_FR" output="screen">
+  <node pkg="lidar_localizer" type="modular_ndt_matching" name="modular_ndt_matching_FR">
     <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
     <param name="method_type" value="$(arg method_type)" />
 
@@ -123,7 +120,6 @@
     <param name="leaf_size" value="$(arg leaf_size)" />
     <param name="measurement_range" value="$(arg measurement_range)" />
     <param name="output_log" value="$(arg output_log)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <node pkg="lidar_localizer" type="modular_ndt_matching" name="modular_ndt_matching_B">
@@ -161,7 +157,7 @@
     <param name="ndt_time_topic" value="ndt_time_B" />
   </node>
 
-  <node pkg="lidar_localizer" type="multi_ndt_combiner" name="multi_ndt_combiner" output="screen">
+  <node pkg="lidar_localizer" type="multi_ndt_combiner" name="multi_ndt_combiner">
     <rosparam param="pose_topics">["ndt_pose_FL", "ndt_pose_FR", "ndt_pose_B"]</rosparam>
     <rosparam param="stat_topics">["ndt_stat_FL", "ndt_stat_FR", "ndt_stat_B"]</rosparam>
     <param name="baselink_frame" value="base_link" />
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch
index a9efdf71..106b4686 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch
@@ -36,22 +36,18 @@
   <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
     <param name="/input_topic" value="$(arg lidar_input_topic)" />
     <param name="/output_topic" value="$(arg lidar_output_topic)" />
-    <param name="/instance_mode" value="1" />
   </node>
 
   <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="1.2 0 2.0 0 0 0 base_link velodyne 10"/>
   <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gps base_link 10"/> -->
   <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" />
 
-  <include file="$(find points_downsampler)/launch/voxel_grid_filter.launch">
-    <arg name="instance_mode" value="1" />
-  </include>
+  <include file="$(find points_downsampler)/launch/voxel_grid_filter.launch"/>
 
   <!-- ndt_matching --><!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
   <include file="$(find lidar_localizer)/launch/ndt_matching.launch">
     <arg name="method_type" value="0" /> 
     <arg name="get_height" value="true" /> 
-    <arg name="instance_mode" value="0" />
   </include>
 
   <!-- ndt config -->
@@ -76,7 +72,6 @@
 <include file="$(find rubis_pkg)/launch/vel_pose_connect.launch">
   <arg name="topic_pose_stamped" value="/ndt_pose" />
   <arg name="topic_twist_stamped" value="/estimate_twist" />
-  <arg name="instance_mode" value="1" />
 </include>
 
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch
deleted file mode 100644
index 59c8f802..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch
+++ /dev/null
@@ -1,43 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/dtlane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/idx.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/CubeTown.pcd"/>
-  </include>
-
-  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
-
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
-    <param name="/input_topic" value="$(arg lidar_input_topic)" />
-    <param name="/output_topic" value="$(arg lidar_output_topic)" />
-  </node>
-
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="1.0510799 0 1.96 0 0 0 base_link velodyne 10"/>
-  <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gps base_link 10"/> -->
-  <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" />
-
-  <!-- gnss localizer -->
-  <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
-    <param name="plane" value="$(arg plane)"/>
-  </node> -->
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch
deleted file mode 100644
index a14b621b..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch
+++ /dev/null
@@ -1,48 +0,0 @@
-<launch>
-  <!-- cubetown -->
-  <arg name="init_x" value="57.1" />
-  <arg name="init_y" value="1.38" />
-  <arg name="init_z" value="0.48" />
-  <arg name="init_roll" value="0.0" />
-  <arg name="init_pitch" value="0.0" />
-  <arg name="init_yaw" value="1.57" />
-
-  <!-- vel/pose connect -->
-  <arg name="topic_pose_stamped" default="/ndt_pose" />
-  <arg name="topic_twist_stamped" default="/estimate_twist" />
-
-  <!-- points downsampler -->
-  <include file="$(find points_downsampler)/launch/points_downsample.launch" />
-
-  <!-- ndt_matching --><!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
-  <include file="$(find lidar_localizer)/launch/ndt_matching.launch">
-    <arg name="method_type" value="0" /> 
-    <arg name="get_height" value="true" /> 
-  </include>
-
-  <!-- ndt config -->
-  <node pkg="rostopic" type="rostopic" name="config_ndt"
-  args="pub /config/ndt autoware_config_msgs/ConfigNDT
-  '{header: {seq: 8, stamp: {secs: 0, nsecs: 0}, frame_id: ''},
-    init_pos_gnss: 0,
-    x: $(arg init_x),
-    y: $(arg init_y),
-    z: $(arg init_z),
-    roll: $(arg init_roll), 
-    pitch: $(arg init_pitch),
-    yaw: $(arg init_yaw),
-    use_predict_pose: 1,
-    error_threshold: 0.01,
-    resolution: 3.0,
-    step_size: 0.5,
-    trans_epsilon: 0.01,
-    max_iterations: 1}
-  '"/>
-
-  <!-- vel_pose_mux -->
-  <include file="$(find autoware_connector)/launch/vel_pose_connect.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
-
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch
deleted file mode 100644
index cd906d09..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch
+++ /dev/null
@@ -1,86 +0,0 @@
-<launch>
-  <!-- Ground Filter -->
-  <!-- <include file="$(find points_preprocessor)/launch/compare_map_filter_params.launch">
-     <arg name="input_point_topic" value="/points_raw" />
-    <arg name="input_map_topic" value="/points_map" />
-    <arg name="output_match_topic" value="/points_ground_center" />
-    <arg name="output_unmatch_topic" value="/points_no_ground_center" />
-  </include> -->
-
-  <include file="$(find points_preprocessor)/launch/ray_ground_filter.launch">
-    <arg name="input_point_topic" value="/points_raw" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
-    <arg name="base_frame" value="base_link" />  <!-- Coordinate system to perform transform (default base_link) -->
-    <arg name="max_clipping_height" value="1.5" />  <!-- Remove Points above this height value (default 2.0 meters) -->
-    <arg name="min_clipping_height" value="0.0" />  <!-- Remove Points below this height value (default -0.5 meters) -->
-    <arg name="min_point_distance" value="2.9" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
-    <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
-    <arg name="concentric_divider_distance" value="0.0" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
-    <arg name="local_max_slope" value="3" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
-    <arg name="general_max_slope" value="9" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
-    <arg name="min_height_threshold" value="0.4" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
-    <arg name="reclass_distance_threshold" value="0.01" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
-    <arg name="no_ground_point_topic" value="/points_no_ground" />
-    <arg name="ground_point_topic" value="/points_ground" />
-  </include>
-  
-  <!-- lidar_euclidean_cluster_detect-->
-  <include file="$(find lidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch">
-    <arg name="points_node" value="/points_no_ground_center" />
-    <arg name="remove_ground" value="true" />
-    <arg name="downsample_cloud" value="false" />
-    <arg name="leaf_size" value="0.68" />
-    <arg name="cluster_size_min" value="1" />
-    <arg name="cluster_size_max" value="100000" />
-    <arg name="sync" value="false" />
-    <arg name="use_diffnormals" value="false" />
-    <arg name="pose_estimation" value="false" />
-    <arg name="clip_min_height" value="-10.0" />
-    <arg name="clip_max_height" value="10" />
-    <arg name="keep_lanes" value="false" />
-    <arg name="keep_lane_left_distance" value="5" />
-    <arg name="keep_lane_right_distance" value="5" />
-    <arg name="cluster_merge_threshold" value="1.5" />
-    <arg name="clustering_distance" value="0.75" />
-    <arg name="use_vector_map" value="false" />
-    <arg name="wayarea_gridmap_layer" value="wayarea" />
-    <arg name="output_frame" value="velodyne" />
-    <arg name="remove_points_upto" value="0.0" />
-    <arg name="use_gpu" value="true" />    
-    <arg name="use_multiple_thres" value="false"/>
-    <arg name="clustering_ranges" value="[15,30,45,60]"/>
-    <arg name="clustering_distances"
-         value="[0.5,1.1,1.6,2.1,2.6]"/>    
-  </include>
-  
-  <!-- vision_darknet_detect -->
-  <include file="$(find vision_darknet_detect)/launch/vision_darknet_detect_parameter.launch">
-    <arg name="gpu_device_id" value="0"/>
-    <arg name="score_threshold" value="0.5"/>
-    <arg name="nms_threshold" value="0.45"/>
-    <arg name="names_file" value="$(find vision_darknet_detect)/darknet/cfg/coco.names"/>
-    <arg name="camera_id" value="/"/>
-    <arg name="image_src" value="/image_raw"/>    
-  </include>
-
-  <!-- imm_ukf_pda_track -->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
-    <arg name="namespace" default="/detection/object_tracker"/>
-    <arg name="tracker_input_topic" default="/detection/lidar_detector/objects_center" />
-    <arg name="tracker_output_topic" default="/detection/object_tracker/objects_center" />
-    <arg name="tracking_frame" default="/world" />
-    <arg name="gating_threshold" default="9.22" />
-    <arg name="gate_probability" default="0.99" />
-    <arg name="detection_probability" default="0.9" />
-    <arg name="life_time_threshold" default="8" />
-    <arg name="static_velocity_threshold" default="0.5" />
-    <arg name="static_num_history_threshold" default="3" />
-    <arg name="prevent_explosion_threshold" default="1000" />
-    <arg name="merge_distance_threshold" default="0.5"/>
-    <arg name="use_sukf" default="false" />
-    <arg name="use_map_info" default="false" />
-    <arg name="lane_direction_chi_threshold" default="2.71" />
-    <arg name="nearest_lane_distance_threshold" default="1.0" />
-    <arg name="vectormap_frame" default="/map" />
-  </include>
-
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch
deleted file mode 100644
index ee554f3a..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch
+++ /dev/null
@@ -1,44 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/dtlane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/idx.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/FMTC_merged.pcd"/>
-  </include>
-
-  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
-
-  <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
-    <arg name="input_topic" value="$(arg lidar_input_topic)" />
-    <arg name="output_topic" value="$(arg lidar_output_topic)" />
-  </include>
-
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.68 0 0 base_link velodyne 10"/>
-  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="1.2 0 2.0 0 0 0 base_link velodyne 10"/> -->
-  <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gnss base_link 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" />
-
-  <!-- gnss localizer -->
-  <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
-    <param name="plane" value="$(arg plane)"/>
-  </node>
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch
deleted file mode 100644
index f66cef13..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch
+++ /dev/null
@@ -1,29 +0,0 @@
-<launch>
-  <!-- pure_pursuit  -->
-  <include file="$(find pure_pursuit)/launch/pure_pursuit_params.launch">   
-    <!-- Core Parameter -->
-    <arg name="lookahead_ratio"               value="1.5"/>
-    <arg name="minimum_lookahead_distance"    value="15.0"/>
-    
-    <arg name="const_velocity"                value="5.0"/>     
-    <arg name="const_lookahead_distance"      value="4.0"/>
-    <arg name="is_linear_interpolation"       value="True"/>
-    <arg name="publishes_for_steering_robot"  value="True"/>
-    <arg name="add_virtual_end_waypoints"     value="False"/>
-  </include>
-
-  <!-- twist_filter -->
-  <include file="$(find twist_filter)/launch/twist_filter_params.launch">    
-    <!-- For twist_filter -->
-    <arg name="wheel_base" default="2.7" />
-    <arg name="lateral_accel_limit" default="5.0" />
-    <arg name="lateral_jerk_limit" default="5.0" />
-    <arg name="lowpass_gain_linear_x" default="0.0" />
-    <arg name="lowpass_gain_angular_z" default="0.0" />
-    <arg name="lowpass_gain_steering_angle" default="0.0" />
-    <arg name="max_stop_count" default="40" /> <!-- 10 = 1 second -->  
-    <!-- For twist_gate -->
-    <arg name="loop_rate" default="30.0" />
-    <arg name="use_decision_maker" default="false" />
-  </include>
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/terminate.sh b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/terminate.sh
deleted file mode 100644
index 242e6f2c..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/terminate.sh
+++ /dev/null
@@ -1 +0,0 @@
-rosnode kill /cubetown
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch
deleted file mode 100644
index b410f94d..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch
+++ /dev/null
@@ -1,65 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-  <arg name="image_input_topic" default="/image_raw_origin"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/dtlane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/idx.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/CubeTown.pcd"/>
-  </include>
-
-  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw_origin"  />
-
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
-    <param name="/input_topic" value="$(arg lidar_input_topic)" />
-    <param name="/output_topic" value="$(arg lidar_output_topic)" />
-  </node>
-
-  <node pkg="rubis_pkg" type="camera_republisher" name="camera_republisher" output="screen">
-    <param name="/input_topic" value="$(arg image_input_topic)" />
-  </node>
-
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="1.2 0 2.0 0 0 0 base_link velodyne 10"/>
-  <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gps base_link 10"/> -->
-  <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" />
-
-  <!-- gnss localizer -->
-  <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
-    <param name="plane" value="$(arg plane)"/>
-  </node> -->
-
-  <!-- ray_ground_filter -->
-  <include file="$(find points_preprocessor)/launch/ray_ground_filter.launch">
-    <arg name="input_point_topic" value="/points_raw" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
-    <arg name="base_frame" value="base_link" />  <!-- Coordinate system to perform transform (default base_link) -->
-    <arg name="max_clipping_height" value="1.5" />  <!-- Remove Points above this height value (default 2.0 meters) -->
-    <arg name="min_clipping_height" value="0.0" />  <!-- Remove Points below this height value (default -0.5 meters) -->
-    <arg name="min_point_distance" value="2.9" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
-    <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
-    <arg name="concentric_divider_distance" value="0.0" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
-    <arg name="local_max_slope" value="3" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
-    <arg name="general_max_slope" value="9" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
-    <arg name="min_height_threshold" value="0.4" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
-    <arg name="reclass_distance_threshold" value="0.01" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
-    <arg name="no_ground_point_topic" value="/points_no_ground" />
-    <arg name="ground_point_topic" value="/points_ground" />
-  </include>
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch
deleted file mode 100644
index ab9971b3..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch
+++ /dev/null
@@ -1,60 +0,0 @@
-<launch>
-  <!-- testbed -->
-  <!-- <arg name="init_x" value="-8.72" />
-  <arg name="init_y" value="51.74" />
-  <arg name="init_z" value="-0.71" />
-  <arg name="init_roll" value="0.0" />
-  <arg name="init_pitch" value="0.0" />
-  <arg name="init_yaw" value="-1.8" /> -->
-  <!-- cubetown -->
-  <arg name="init_x" value="57.1" />
-  <arg name="init_y" value="1.38" />
-  <arg name="init_z" value="0.48" />
-  <arg name="init_roll" value="0.0" />
-  <arg name="init_pitch" value="0.0" />
-  <arg name="init_yaw" value="1.57" />
-
-  <!-- vel/pose connect -->
-  <arg name="topic_pose_stamped" default="/ndt_pose" />
-  <arg name="topic_twist_stamped" default="/estimate_twist" />
-
-  <!-- points downsampler -->
-  <include file="$(find points_downsampler)/launch/points_downsample.launch" />
-
-  <!-- nmea2tfpose -->
-  <!-- <include file="$(find gnss_localizer)/launch/nmea2tfpose.launch">
-    <arg name="plane" value="0" />
-  </include> -->
-
-  <!-- ndt_matching --><!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
-  <include file="$(find lidar_localizer)/launch/ndt_matching.launch">
-    <arg name="method_type" value="0" /> 
-    <arg name="get_height" value="true" /> 
-  </include>
-
-  <!-- ndt config -->
-  <node pkg="rostopic" type="rostopic" name="config_ndt"
-  args="pub /config/ndt autoware_config_msgs/ConfigNDT
-  '{header: {seq: 8, stamp: {secs: 0, nsecs: 0}, frame_id: ''},
-    init_pos_gnss: 0,
-    x: $(arg init_x),
-    y: $(arg init_y),
-    z: $(arg init_z),
-    roll: $(arg init_roll),
-    pitch: $(arg init_pitch),
-    yaw: $(arg init_yaw),
-    use_predict_pose: 1,
-    error_threshold: 0.01,
-    resolution: 3.0,
-    step_size: 0.5,
-    trans_epsilon: 0.01,
-    max_iterations: 5}
-  '"/>
-
-  <!-- vel_pose_mux -->
-  <include file="$(find autoware_connector)/launch/vel_pose_connect.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
-
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch
deleted file mode 100644
index 6f2849ef..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch
+++ /dev/null
@@ -1,116 +0,0 @@
-<launch>
-  <!-- op_global_planning -->
-  <include file="$(find op_global_planner)/launch/op_global_planner.launch">
-    <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
-    <arg name="enableSmoothing"       value="false" /> <!-- 1 or 0 -->
-    <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
-    <arg name="enableRvizInput"       value="true" /> <!-- 1 or 0 -->
-    <arg name="enableReplan"          value="false" /> <!-- 1 or 0 -->  
-    <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
-    <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
-    <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
-    <!-- Borregas Avenue -->
-    <!-- <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="57.0422019958" />
-    <arg name="goal_pose_y"           value="-5.43001270294" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.719615198264" />
-    <arg name="goal_ori_w"            value="0.694373074383" /> -->
-    <!-- CubeTown -->
-    <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="55.9" />
-    <arg name="goal_pose_y"           value="-18.8" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.70" />
-    <arg name="goal_ori_w"            value="0.70" />
-  </include>
-
-  <!-- op_common_params  -->
-  <include file="$(find op_local_planner)/launch/op_common_params_parameter.launch">
-    <!-- Included in app window -->
-    <arg name="horizonDistance"                 default="120"  />     <!-- Horizon -->
-    <arg name="maxLocalPlanDistance"            default="80" />       <!-- Plan Distance-->
-    <arg name="pathDensity"                     default="0.5" />      <!-- Path Density-->
-    <arg name="enableFollowing"                 default="true" />     <!-- Enable Following -->
-    <arg name="enableSwerving"                  default="true"  />    <!-- Enable Avoidance -->
-    <arg name="minFollowingDistance"            default="30.0"  />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->  
-    <arg name="minDistanceToAvoid"              default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
-    <arg name="maxDistanceToAvoid"              default="0.1"  />     <!-- Avoidnace Limit--> <!-- should be smaller than minDistanceToAvoid -->
-    <arg name="enableStopSignBehavior"          default="false" />    <!-- Enable Stop Sign Stop-->
-    <arg name="enableTrafficLightBehavior"      default="true" />     <!-- Enable Traffic Light -->
-    <arg name="enableLaneChange"                default="false" />    <!-- Enable Lane Change -->
-    <arg name="horizontalSafetyDistance"        default="1"  />       <!-- Lateral Safety --> 
-    <arg name="verticalSafetyDistance"          default="2"  />       <!-- Longitudinal Safet y-->
-    <arg name="velocitySource"                  default="1" />        <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->   
-    <!-- Vehicle Specification -->
-    <arg name="width"                           default="1.85"  />
-    <arg name="length"                          default="4.2"  />
-    <arg name="wheelBaseLength"                 default="2.7"  />
-    <arg name="turningRadius"                   default="5.2"  />
-    <arg name="maxSteerAngle"                   default="0.45" />
-    <arg name="steeringDelay"                   default="1.2" />
-    <!-- Not included in app window -->
-    <arg name="minPursuiteDistance"             default="3.0"  />
-    <arg name="additionalBrakingDistance"       default="5.0"  />
-    <arg name="giveUpDistance"                  default="-4.0"  />
-    <arg name="mapSource"                       default="0" />        <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
-    <arg name="mapFileName"                     default="" />
-    <arg name="minVelocity"                     default="0.1" />  
-    <arg name="speedProfileFactor"              default="1.2"  />
-    <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />
-    <arg name="enableSlowDownOnCurve"           default="true" />
-  </include>
-
-  <!-- op_trajectory_generator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
-    <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
-    <arg name="samplingSpeedFactor"             default="0.25" />    
-    <arg name="enableHeadingSmoothing"          default="false" />
-  </include>
-
-  <!-- op_motion_predictor -->
-  <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
-    <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" />
-    <arg name="object_tf_list"                  default="[velodyne]" />
-    <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
-    <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
-    <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
-    <arg name="prediction_distance"             default="25.0"/>      <!-- Prediction distance-->
-    <arg name="enableStepByStepSignal"          default="false" />    <!-- Enable Step by Step Test-->
-    <arg name="enableParticleFilterPrediction"  default="false" />    <!-- Enable Particle Filter prediction -->
-    <!-- Not included in app window -->   
-    <arg name="distanceBetweenCurbs"            default="1.5" />
-    <arg name="visualizationTime"               default="0.25" />
-  </include>
-
-  <!-- op_trajectory_evaluator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_evaluator_parameter.launch">
-    <arg name="enablePrediction"                default="false" />                
-    <arg name="horizontalSafetyDistance"        default="1.2" />
-    <arg name="verticalSafetyDistance"          default="0.8" />
-    <arg name="LateralSkipDistance"             default="5.0" />
-    
-    <arg name="PedestrianRightThreshold" default="4.0" />
-    <arg name="PedestrianLeftThreshold" default="4.0" />
-    <arg name="PedestrianImageDetectionRange" default="0.75" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-    <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is not located in this range, our car set speed as max velocity --> 
-    <arg name="VehicleImageWidthThreshold" default="0.018" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
-
-    <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
-  </include>
-
-  <!-- op_behavior_selector -->
-  <include file="$(find op_local_planner)/launch/op_behavior_selector_parameter.launch">
-    <arg name="evidence_tust_number"            default="25"/>
-    <arg name="trafficLightFile"                default="crossing_test/crossing_traffic_light.yaml" />
-    <arg name="stopLineFile"                    default="crossing_test/crossing_stop_line.yaml" />
-    <!-- <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" /> -->
-  </include>
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh b/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh
deleted file mode 100644
index 230ba421..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh
+++ /dev/null
@@ -1 +0,0 @@
-rosnode kill /desktop_lane_keeping
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/test.launch b/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/test.launch
deleted file mode 100644
index 1f08cd4a..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/test.launch
+++ /dev/null
@@ -1,26 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-  <arg name="image_input_topic" default="/image_raw_origin"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/dtlane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/idx.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/CubeTown.pcd"/>
-  </include>
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_1_sensing.launch
similarity index 50%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_1_sensing.launch
index 8d85ce76..8b070783 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_1_sensing.launch
@@ -1,15 +1,18 @@
 <launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml" />
+  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_event_driven.yaml" />
 
   <arg name="lidar_input_topic" default="/points_raw_origin"/>
   <arg name="lidar_output_topic" default="/points_raw"/>
 
+  <arg name="image_input_topic" default="/image_raw_origin"/>
+  <arg name="image_output_topic" default="/image_raw"/>
+
   <!-- Map TF Publisher -->	
 	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
   
   <!-- Vector Map Loader -->
 	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/dtlane.csv
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/dtlane.csv 
 	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/lane.csv 
 	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/node.csv 
 	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/point.csv"/>
@@ -17,41 +20,56 @@
   <!-- Point Map Loader -->
 	<include file="$(find map_file)/launch/points_map_loader.launch">
     <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/220111_138ground_leaf1.pcd"/>
-  </include>
+    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/220111_138ground_leaf0_1.pcd"/>
+  </include>  
 
-  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <!-- <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  /> -->
+  <!-- Vision Sensor -->
+  <include file="$(find camera_image)/launch/camera_image.launch"/>
 
 <!-- Lidar Launch File -->
-  <include file="$(find runtime_manager)/launch_files/velodyne_vlp16_hires.launch" />
+  <include file="$(find runtime_manager)/launch_files/rubis_velodyne_vlp16_hires_FR.launch" />
   
-  <!-- <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
-    <arg name="input_topic" value="$(arg lidar_input_topic)" />
-    <arg name="output_topic" value="$(arg lidar_output_topic)" />
-  </include> -->
-
 <!-- TF -->
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.53 0 0 base_link velodyne 10"/>
-  <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gps base_link 10"/> -->
+  <!-- right LiDAR -->
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.68 0 0 base_link velodyne 10"/>
+
+  <!-- left LiDAR -->
+  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 0.8 0.09 0.68 0 0 base_link velodyne 10"/> -->
+
+  <!--Back LiDAR -->
+  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="-0.78 -0.3 0.36 3.14 0 0 base_link velodyne 10"/> -->
+  
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss" args="-0.165 0.285 0.00 0.0 0 0 base_link gnss 10"/>
   <!-- <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" /> -->
 
-  <!-- gnss localizer -->
-  <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
-    <param name="plane" value="$(arg plane)"/>
-  </node> -->
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 0 0 0 base_link camera 10"/>
 
   <!-- CAN Interface -->
   <include file="$(find can_translate)/launch/can_translate.launch" />
 
   <!-- INS-D Sensor -->
   <!-- <include file="$(find inertiallabs_ins)/launch/ins.launch" />   -->
-
+  
   <!-- INS twist generator -->
   <include file="$(find ins_twist_generator)/launch/ins_twist_generator.launch">
-    <arg name="yaw_offset" value="-84.5828" />
+    <arg name="yaw_offset" value="-64.569" />
+  </include>
+
+  <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
+    <arg name="input_topic" value="$(arg lidar_input_topic)" />
+    <arg name="output_topic" value="$(arg lidar_output_topic)" />
+    
   </include>
 
+  <include file="$(find rubis_pkg)/launch/camera_republisher.launch">
+    <arg name="input_topic" value="$(arg image_input_topic)" />
+    <arg name="output_topic" value="$(arg image_output_topic)" />
+  </include>
+
+  <!-- <include file="$(find ins_twist_generator)/launch/dynamic_ins_twist_generator.launch">
+    <arg name="yaw_offset" value="-72.7014" />
+    <arg name="yaw_diff_threshold" value="2.0" />
+  </include> -->
+
 </launch>
 
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_2_localization.launch
similarity index 58%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_2_localization.launch
index 49a153d4..3256b24d 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch	
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_2_localization.launch
@@ -1,15 +1,15 @@
 <launch>
   <!--TODO: Set LidAR Inital Pose-->
-  <arg name="init_x" value="57.1" />
-  <arg name="init_y" value="1.38" />
-  <arg name="init_z" value="0.48" />
+  <arg name="init_x" value="5.8" />
+  <arg name="init_y" value="2.9" />
+  <arg name="init_z" value="1.7" />
   <arg name="init_roll" value="0.0" />
   <arg name="init_pitch" value="0.0" />
   <arg name="init_yaw" value="1.57" />
 
   <!-- vel/pose connect -->
-  <arg name="topic_pose_stamped" default="/ndt_pose" />
-  <arg name="topic_twist_stamped" default="/estimate_twist" /> 
+  <!-- <arg name="topic_pose_stamped" default="/ndt_pose" />
+  <arg name="topic_twist_stamped" default="/estimate_twist" />  -->
   <!-- <arg name="topic_twist_stamped" default="/odom_twist" /> -->
 
   <!-- points downsampler -->
@@ -22,11 +22,11 @@
   <include file="$(find lidar_localizer)/launch/ndt_matching_params.launch">
     <arg name="method_type" value="0" /> 
     <arg name="get_height" value="true" /> 
-    <arg name="init_match_threshold" value="0.5" /> 
-    <arg name="failure_score_diff_threshold" value="10.0" /> 
-    <arg name="recovery_score_diff_threshold" value="1.0" /> 
-    <arg name="failure_pose_diff_threshold" value="4.0" /> 
-    <arg name="recovery_pose_diff_threshold" value="1.0" /> 
+    <arg name="init_match_threshold" value="5.0" /> 
+    <arg name="failure_score_diff_threshold" default="1000.0" />   
+    <arg name="recovery_score_diff_threshold" default="1000.0" />  
+    <arg name="failure_pose_diff_threshold" default="1.0" /> 
+    <arg name="recovery_pose_diff_threshold" default="0.5" />
   </include>
 
   <!-- ndt config -->
@@ -41,17 +41,11 @@
     pitch: $(arg init_pitch),
     yaw: $(arg init_yaw),
     use_predict_pose: 1,
-    error_threshold: 1.2,
-    resolution: 4.0,
-    step_size: 0.5,
-    trans_epsilon: 0.05,
+    error_threshold: 1.0,
+    resolution: 1.0,
+    step_size: 0.3,
+    trans_epsilon: 0.1,
     max_iterations: 10}
   '"/> 
-  
-  <!-- vel_pose_mux -->
-  <include file="$(find rubis_pkg)/launch/vel_pose_connect_params.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
 
 </launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_3_detection.launch
similarity index 98%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_3_detection.launch
index 66594cf2..9af37442 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch	
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_3_detection.launch
@@ -63,7 +63,7 @@
   </include>
 
   <!-- imm_ukf_pda_track -->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
+  <!-- <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
     <arg name="namespace" default="/detection/object_tracker"/>
     <arg name="tracker_input_topic" default="/detection/lidar_detector/objects_center" />
     <arg name="tracker_output_topic" default="/detection/object_tracker/objects_center" />
@@ -81,6 +81,6 @@
     <arg name="lane_direction_chi_threshold" default="2.71" />
     <arg name="nearest_lane_distance_threshold" default="1.0" />
     <arg name="vectormap_frame" default="/map" />
-  </include>
+  </include> -->
 
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_4_planning.launch
similarity index 84%
rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_4_planning.launch
index f25e94df..782641ab 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_4_planning.launch
@@ -1,6 +1,6 @@
 <launch>
   <!-- op_global_planning -->
-  <arg name="LaneInfoFile"          default="cubetown_autorunner/default_lane_info.yaml" />
+  <arg name="LaneInfoFile"          default="ionic_autorunner/lane_info.yaml" />
   <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
   <arg name="enableSmoothing"       value="false" /> <!-- 1 or 0 -->
   <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
@@ -11,16 +11,16 @@
   <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
   <arg name="enableDynamicMapUpdate"       default="false" />  
   <!-- Borregas Avenue -->
-  <arg name="use_static_goal"       value="true" />
-  <arg name="goal_pose_x"           value="57.0422019958" />
-  <arg name="goal_pose_y"           value="-5.43001270294" />
+  <arg name="use_static_goal"       value="false" />
+  <arg name="goal_pose_x"           value="15.0" />
+  <arg name="goal_pose_y"           value="40.5" />
   <arg name="goal_pose_z"           value="0.0" />
   <arg name="goal_ori_x"            value="0.0" />
   <arg name="goal_ori_y"            value="0.0" />
-  <arg name="goal_ori_z"            value="0.719615198264" />
-  <arg name="goal_ori_w"            value="0.694373074383" />
+  <arg name="goal_ori_z"            value="0.885" />
+  <arg name="goal_ori_w"            value="-0.464" />
 
-  <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner" output="screen">    
+  <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner">    
     <rosparam command="load" file="$(find rubis_autorunner)/cfg/$(arg LaneInfoFile)"/>
     <param name="pathDensity"         value="$(arg pathDensity)" />
     <param name="enableSmoothing"       value="$(arg enableSmoothing)" />
@@ -47,7 +47,7 @@
     <arg name="horizonDistance"                 default="120"  />     <!-- Horizon -->
     <arg name="maxLocalPlanDistance"            default="80" />       <!-- Plan Distance-->
     <arg name="pathDensity"                     default="0.5" />      <!-- Path Density-->
-    <arg name="enableFollowing"                 default="true" />     <!-- Enable Following -->
+    <arg name="enableFollowing"                 default="false" />     <!-- Enable Following -->
     <arg name="enableSwerving"                  default="true"  />    <!-- Enable Avoidance -->
     <arg name="minFollowingDistance"            default="30.0"  />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->  
     <arg name="minDistanceToAvoid"              default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
@@ -59,28 +59,29 @@
     <arg name="verticalSafetyDistance"          default="2"  />       <!-- Longitudinal Safet y-->
     <arg name="velocitySource"                  default="1" />        <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->   
     <!-- Vehicle Specification -->
-    <arg name="width"                           default="1.85"  />
-    <arg name="length"                          default="4.2"  />
+    <arg name="width"                           default="1.82"  />
+    <arg name="length"                          default="4.47"  />
     <arg name="wheelBaseLength"                 default="2.7"  />
-    <arg name="turningRadius"                   default="5.2"  />
-    <arg name="maxSteerAngle"                   default="0.45" />
+    <arg name="turningRadius"                   default="3.52826"  />
+    <arg name="maxSteerAngle"                   default="0.65319" />
     <arg name="steeringDelay"                   default="1.2" />
     <!-- Not included in app window -->
     <arg name="minPursuiteDistance"             default="3.0"  />
-    <arg name="additionalBrakingDistance"       default="5.0"  />
+    <arg name="additionalBrakingDistance"       default="30.0"  />
     <arg name="giveUpDistance"                  default="-4.0"  />
     <arg name="mapSource"                       default="0" />        <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
     <arg name="mapFileName"                     default="" />
     <arg name="minVelocity"                     default="0.1" />  
     <arg name="speedProfileFactor"              default="1.2"  />
     <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />  
+    <arg name="smoothingSmoothWeight"           default="0.4"  />
+    <arg name="enableSlowDownOnCurve"           default="true" />
   </include>
 
   <!-- op_trajectory_generator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
     <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
+    <arg name="samplingOutMargin"               default="20" />        <!-- Roll In Margin -->
     <arg name="samplingSpeedFactor"             default="0.25" />    
     <arg name="enableHeadingSmoothing"          default="false" />
   </include>
@@ -88,8 +89,10 @@
   <!-- op_motion_predictor -->
   <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
     <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/object_tracker/objects_center]" />
-    <arg name="object_tf_list"                  default="[camera,velodyne]" />
+    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/lidar_detector/rubis_objects_center]" />
+    <arg name="object_tf_list"                  default="[velodyne,camera]" /> -->
+    <!-- <arg name="input_object_list"               default="[/detection/object_tracker/objects_center]" />
+    <arg name="object_tf_list"                  default="[velodyne]" /> -->
     <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
     <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
     <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
@@ -107,6 +110,7 @@
     <arg name="horizontalSafetyDistance"        default="1.2" />
     <arg name="verticalSafetyDistance"          default="0.8" />
     <arg name="LateralSkipDistance"             default="5.0" />
+    
 
     <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
   </include>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_5_control.launch
similarity index 100%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_138/_ionic_autorunner_5_control.launch
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch
deleted file mode 100644
index a62287b4..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch	
+++ /dev/null
@@ -1,58 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/double_curve/dtlane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/double_curve/idx.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/double_curve/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/double_curve/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/double_curve/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/220111_138ground_leaf1.pcd"/>
-  </include>
-
-  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <!-- <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  /> -->
-
-<!-- Lidar Launch File -->
-  <include file="$(find runtime_manager)/launch_files/velodyne_vlp16_hires.launch" />
-  
-  <!-- <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
-    <arg name="input_topic" value="$(arg lidar_input_topic)" />
-    <arg name="output_topic" value="$(arg lidar_output_topic)" />
-  </include> -->
-
-<!-- TF -->
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.53 0 0 base_link velodyne 10"/>
-  <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gps base_link 10"/> -->
-  <!-- <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" /> -->
-
-  <!-- gnss localizer -->
-  <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
-    <param name="plane" value="$(arg plane)"/>
-  </node> -->
-
-  <!-- CAN Interface -->
-  <include file="$(find can_translate)/launch/can_translate.launch" />
-
-  <!-- INS-D Sensor -->
-  <!-- <include file="$(find inertiallabs_ins)/launch/ins.launch" />   -->
-
-  <!-- INS twist generator -->
-  <include file="$(find ins_twist_generator)/launch/ins_twist_generator.launch">
-    <arg name="yaw_offset" value="-115.139" />
-  </include>
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch
deleted file mode 100644
index 92401e07..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch	
+++ /dev/null
@@ -1,105 +0,0 @@
-<launch>
-  <!-- op_global_planning -->
-  <include file="$(find op_global_planner)/launch/op_global_planner.launch">
-    <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
-    <arg name="enableSmoothing"       value="false" /> <!-- 1 or 0 -->
-    <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
-    <arg name="enableRvizInput"       value="true" /> <!-- 1 or 0 -->
-    <arg name="enableReplan"          value="false" /> <!-- 1 or 0 -->  
-    <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
-    <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
-    <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
-    <!-- 138ground single curve -->
-    <arg name="use_static_goal"       value="false" />
-    <arg name="goal_pose_x"           value="57.0422019958" />
-    <arg name="goal_pose_y"           value="-5.43001270294" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.719615198264" />
-    <arg name="goal_ori_w"            value="0.694373074383" />
-  </include>
-
-  <!-- op_common_params  -->
-  <include file="$(find op_local_planner)/launch/op_common_params_parameter.launch">
-    <!-- Included in app window -->
-    <arg name="horizonDistance"                 default="120"  />     <!-- Horizon -->
-    <arg name="maxLocalPlanDistance"            default="80" />       <!-- Plan Distance-->
-    <arg name="pathDensity"                     default="0.5" />      <!-- Path Density-->
-    <arg name="enableFollowing"                 default="true" />     <!-- Enable Following -->
-    <arg name="enableSwerving"                  default="true"  />    <!-- Enable Avoidance -->
-    <arg name="minFollowingDistance"            default="30.0"  />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->  
-    <arg name="minDistanceToAvoid"              default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
-    <arg name="maxDistanceToAvoid"              default="0.1"  />     <!-- Avoidnace Limit--> <!-- should be smaller than minDistanceToAvoid -->
-    <arg name="enableStopSignBehavior"          default="false" />    <!-- Enable Stop Sign Stop-->
-    <arg name="enableTrafficLightBehavior"      default="true" />     <!-- Enable Traffic Light -->
-    <arg name="enableLaneChange"                default="false" />    <!-- Enable Lane Change -->
-    <arg name="horizontalSafetyDistance"        default="1"  />       <!-- Lateral Safety --> 
-    <arg name="verticalSafetyDistance"          default="2"  />       <!-- Longitudinal Safet y-->
-    <arg name="velocitySource"                  default="1" />        <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->   
-    <!-- Vehicle Specification -->
-    <arg name="width"                           default="1.82"  />
-    <arg name="length"                          default="4.47"  />
-    <arg name="wheelBaseLength"                 default="2.7"  />
-    <arg name="turningRadius"                   default="3.52826"  />
-    <arg name="maxSteerAngle"                   default="0.65319" />
-    <arg name="steeringDelay"                   default="1.2" />
-    <!-- Not included in app window -->
-    <arg name="minPursuiteDistance"             default="3.0"  />
-    <arg name="additionalBrakingDistance"       default="5.0"  />
-    <arg name="giveUpDistance"                  default="-4.0"  />
-    <arg name="mapSource"                       default="0" />        <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
-    <arg name="mapFileName"                     default="" />
-    <arg name="minVelocity"                     default="0.1" />  
-    <arg name="speedProfileFactor"              default="1.2"  />
-    <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />  
-  </include>
-
-  <!-- op_trajectory_generator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
-    <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
-    <arg name="samplingSpeedFactor"             default="0.25" />    
-    <arg name="enableHeadingSmoothing"          default="false" />
-  </include>
-
-  <!-- op_motion_predictor -->
-  <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
-    <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/object_tracker/objects_center]" />
-    <arg name="object_tf_list"                  default="[camera,velodyne]" />
-    <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
-    <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
-    <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
-    <arg name="prediction_distance"             default="25.0"/>      <!-- Prediction distance-->
-    <arg name="enableStepByStepSignal"          default="false" />    <!-- Enable Step by Step Test-->
-    <arg name="enableParticleFilterPrediction"  default="false" />    <!-- Enable Particle Filter prediction -->
-    <!-- Not included in app window -->   
-    <arg name="distanceBetweenCurbs"            default="1.5" />
-    <arg name="visualizationTime"               default="0.25" />
-  </include>
-
-  <!-- op_trajectory_evaluator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_evaluator_parameter.launch">
-    <arg name="enablePrediction"                default="false" />                
-    <arg name="horizontalSafetyDistance"        default="1.2" />
-    <arg name="verticalSafetyDistance"          default="0.8" />
-    <arg name="LateralSkipDistance"             default="5.0" />
-    
-    <arg name="PedestrianRightThreshold" default="4.0" />
-    <arg name="PedestrianLeftThreshold" default="4.0" />
-    <arg name="PedestrianImageDetectionRange" default="0.75" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-    <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is not located in this range, our car set speed as max velocity --> 
-    <arg name="VehicleImageWidthThreshold" default="0.018" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
-
-    <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
-  </include>
-
-  <!-- op_behavior_selector -->
-  <include file="$(find op_local_planner)/launch/op_behavior_selector_parameter.launch">
-    <arg name="evidence_tust_number"            default="25"/>
-    <arg name="trafficLightFile"                default="crossing_test/crossing_traffic_light.yaml" />
-    <arg name="stopLineFile"                    default="crossing_test/crossing_stop_line.yaml" />
-  </include>
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch
deleted file mode 100644
index 41114441..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch	
+++ /dev/null
@@ -1,38 +0,0 @@
-<launch>
-  <!--  Dynamic Pure Pursuit Param -->
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_pure_pursuit_params.yaml" />
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/138ground_double_curve_vel.yaml" />
-  
-  <!-- TODO: param tunning -->
-  <!-- pure_pursuit  -->
-  <include file="$(find pure_pursuit)/launch/pure_pursuit_params.launch">   
-    <!-- Core Parameter -->
-    <arg name="lookahead_ratio"               value="1.5"/>
-    <arg name="minimum_lookahead_distance"    value="12.0"/>
-    
-    <arg name="const_velocity"                value="5.0"/>     
-    <arg name="const_lookahead_distance"      value="4.0"/>
-    <arg name="is_linear_interpolation"       value="True"/>
-    <arg name="publishes_for_steering_robot"  value="True"/>
-    <arg name="add_virtual_end_waypoints"     value="False"/>
-  </include>
-
-  <!-- twist_filter -->
-  <include file="$(find twist_filter)/launch/twist_filter_params.launch">    
-    <!-- For twist_filter -->
-    <arg name="wheel_base" default="2.7" />
-    <arg name="lateral_accel_limit" default="5.0" />
-    <arg name="lateral_jerk_limit" default="5.0" />
-    <arg name="lowpass_gain_linear_x" default="0.0" />
-    <arg name="lowpass_gain_angular_z" default="0.0" />
-    <arg name="lowpass_gain_steering_angle" default="0.0" />
-    <arg name="max_stop_count" default="40" /> <!-- 10 = 1 second -->  
-    <!-- For twist_gate -->
-    <arg name="loop_rate" default="30.0" />
-    <arg name="use_decision_maker" default="false" />
-  </include>
-
-  <!-- controller -->
-  <include file="$(find controller)/launch/pid_controller.launch" />
-
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch
deleted file mode 100644
index be1dd9e2..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch
+++ /dev/null
@@ -1,46 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220503_138gound_gps/dtlane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220503_138gound_gps/idx.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220503_138gound_gps/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220503_138gound_gps/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220503_138gound_gps/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<!-- <include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/FMTC_merged.pcd"/>
-  </include>   -->
-
-<!-- Lidar Launch File -->
-  <!-- <include file="$(find runtime_manager)/launch_files/velodyne_vlp16_hires.launch" /> -->
-  
-
-<!-- TF -->
-  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.53 0 0 base_link velodyne 10"/> -->
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.40 0 0 base_link velodyne 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss" args="0.08 0.39 0 0 0 0 base_link gnss 10" />
-
-  <!-- CAN Interface -->
-  <include file="$(find can_translate)/launch/can_translate.launch" />
-
-  <!-- INS twist generator -->
-  <include file="$(find ins_twist_generator)/launch/ins_twist_generator.launch">
-    <arg name="yaw_offset" value="0.0" />
-  </include>
-
-  <!-- GNSS Module-->
-  <include file="$(find gnss_module)/launch/gnss_module.launch">
-  </include>
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch
deleted file mode 100644
index 7d14bf71..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-  <!--TODO: Set LidAR Inital Pose-->
-  <arg name="init_x" value="57.1" />
-  <arg name="init_y" value="1.38" />
-  <arg name="init_z" value="0.48" />
-  <arg name="init_roll" value="0.0" />
-  <arg name="init_pitch" value="0.0" />
-  <arg name="init_yaw" value="1.57" />
-
-  <!-- vel/pose connect -->
-  <arg name="topic_pose_stamped" default="/gnss_pose" />
-  <arg name="topic_twist_stamped" default="/ins_twist" /> 
-  
-  <!-- vel_pose_mux -->
-  <include file="$(find rubis_pkg)/launch/vel_pose_connect_params.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
-
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch
deleted file mode 100644
index 899f293d..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch
+++ /dev/null
@@ -1,106 +0,0 @@
-<launch>
-  <!-- op_global_planning -->
-  <include file="$(find op_global_planner)/launch/op_global_planner.launch">
-    <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
-    <arg name="enableSmoothing"       value="false" /> <!-- 1 or 0 -->
-    <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
-    <arg name="enableRvizInput"       value="true" /> <!-- 1 or 0 -->
-    <arg name="enableReplan"          value="false" /> <!-- 1 or 0 -->  
-    <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
-    <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
-    <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
-    <!-- 138ground single curve -->
-    <arg name="use_static_goal"       value="false" />
-    <arg name="goal_pose_x"           value="57.0422019958" />
-    <arg name="goal_pose_y"           value="-5.43001270294" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.719615198264" />
-    <arg name="goal_ori_w"            value="0.694373074383" />
-  </include>
-
-  <!-- op_common_params  -->
-  <include file="$(find op_local_planner)/launch/op_common_params_parameter.launch">
-    <!-- Included in app window -->
-    <arg name="horizonDistance"                 default="120"  />     <!-- Horizon -->
-    <arg name="maxLocalPlanDistance"            default="80" />       <!-- Plan Distance-->
-    <arg name="pathDensity"                     default="0.5" />      <!-- Path Density-->
-    <arg name="enableFollowing"                 default="true" />     <!-- Enable Following -->
-    <arg name="enableSwerving"                  default="true"  />    <!-- Enable Avoidance -->
-    <arg name="minFollowingDistance"            default="30.0"  />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->  
-    <arg name="minDistanceToAvoid"              default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
-    <arg name="maxDistanceToAvoid"              default="0.1"  />     <!-- Avoidnace Limit--> <!-- should be smaller than minDistanceToAvoid -->
-    <arg name="enableStopSignBehavior"          default="false" />    <!-- Enable Stop Sign Stop-->
-    <arg name="enableTrafficLightBehavior"      default="true" />     <!-- Enable Traffic Light -->
-    <arg name="enableLaneChange"                default="false" />    <!-- Enable Lane Change -->
-    <arg name="horizontalSafetyDistance"        default="1"  />       <!-- Lateral Safety --> 
-    <arg name="verticalSafetyDistance"          default="2"  />       <!-- Longitudinal Safet y-->
-    <arg name="velocitySource"                  default="1" />        <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->   
-    <!-- Vehicle Specification -->
-    <arg name="width"                           default="1.82"  />
-    <arg name="length"                          default="4.47"  />
-    <arg name="wheelBaseLength"                 default="2.7"  />
-    <arg name="turningRadius"                   default="3.52826"  />
-    <arg name="maxSteerAngle"                   default="0.65319" />
-    <arg name="steeringDelay"                   default="1.2" />
-    <!-- Not included in app window -->
-    <arg name="minPursuiteDistance"             default="3.0"  />
-    <arg name="additionalBrakingDistance"       default="5.0"  />
-    <arg name="giveUpDistance"                  default="-4.0"  />
-    <arg name="mapSource"                       default="0" />        <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
-    <arg name="mapFileName"                     default="" />
-    <arg name="minVelocity"                     default="0.1" />  
-    <arg name="speedProfileFactor"              default="1.2"  />
-    <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />
-    <arg name="enableSlowDownOnCurve"           default="true" />
-  </include>
-
-  <!-- op_trajectory_generator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
-    <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
-    <arg name="samplingSpeedFactor"             default="0.25" />    
-    <arg name="enableHeadingSmoothing"          default="false" />
-  </include>
-
-  <!-- op_motion_predictor -->
-  <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
-    <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/object_tracker/objects_center]" />
-    <arg name="object_tf_list"                  default="[camera,velodyne]" />
-    <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
-    <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
-    <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
-    <arg name="prediction_distance"             default="25.0"/>      <!-- Prediction distance-->
-    <arg name="enableStepByStepSignal"          default="false" />    <!-- Enable Step by Step Test-->
-    <arg name="enableParticleFilterPrediction"  default="false" />    <!-- Enable Particle Filter prediction -->
-    <!-- Not included in app window -->   
-    <arg name="distanceBetweenCurbs"            default="1.5" />
-    <arg name="visualizationTime"               default="0.25" />
-  </include>
-
-  <!-- op_trajectory_evaluator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_evaluator_parameter.launch">
-    <arg name="enablePrediction"                default="false" />                
-    <arg name="horizontalSafetyDistance"        default="1.2" />
-    <arg name="verticalSafetyDistance"          default="0.8" />
-    <arg name="LateralSkipDistance"             default="5.0" />
-    
-    <arg name="PedestrianRightThreshold" default="4.0" />
-    <arg name="PedestrianLeftThreshold" default="4.0" />
-    <arg name="PedestrianImageDetectionRange" default="0.75" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-    <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is not located in this range, our car set speed as max velocity --> 
-    <arg name="VehicleImageWidthThreshold" default="0.018" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
-
-    <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
-  </include>
-
-  <!-- op_behavior_selector -->
-  <include file="$(find op_local_planner)/launch/op_behavior_selector_parameter.launch">
-    <arg name="evidence_tust_number"            default="25"/>
-    <arg name="trafficLightFile"                default="crossing_test/crossing_traffic_light.yaml" />
-    <arg name="stopLineFile"                    default="crossing_test/crossing_stop_line.yaml" />
-  </include>
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch
deleted file mode 100644
index c788f638..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch
+++ /dev/null
@@ -1,35 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/dtlane.csv
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/220118_138ground/long_circle/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/220111_138ground_leaf1.pcd"/>
-  </include>
-
-
-<!-- Lidar Launch File -->
-  <include file="$(find runtime_manager)/launch_files/velodyne_vlp16_hires.launch" />
-  
-
-<!-- TF -->
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.4 0 0 base_link velodyne 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss" args="0.0 0.0 0.00 0.0 0 0 base_link gnss 10"/>
-  <!-- <node pkg="tf" type="static_transform_publisher" name="gps_to_base_link" args="0 0 0 0 0 0 gps base_link 10"/> -->
-  <!-- <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" /> -->
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch
index 37db9886..340132b4 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch
@@ -22,7 +22,7 @@
   </include>  
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <!-- <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  /> -->
+  <!-- <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  /> -->
 
 <!-- Lidar Launch File -->
   <include file="$(find runtime_manager)/launch_files/velodyne_vlp16_hires.launch" />
@@ -42,7 +42,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_1_sensing_spin40.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_1_sensing_spin40.launch
index 96abfd87..af7b3889 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_1_sensing_spin40.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_1_sensing_spin40.launch
@@ -1,9 +1,11 @@
 <launch>
   <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml" />
 
-  <arg name="lidar_input_topic" default="/points_raw"/>
+  <arg name="lidar_input_topic" default="/points_raw_origin"/>
   <arg name="lidar_output_topic" default="/points_raw"/>
 
+  <arg name="image_input_topic" default="/image_raw_origin"/>
+  <arg name="image_output_topic" default="/image_raw"/>
   <!-- Map TF Publisher -->	
 	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
   
@@ -58,6 +60,11 @@
     <arg name="output_topic" value="$(arg lidar_output_topic)" />
   </include>
 
+  <include file="$(find rubis_pkg)/launch/camera_republisher.launch">
+    <arg name="input_topic" value="$(arg image_input_topic)" />
+    <!-- <arg name="output_topic" value="$(arg image_output_topic)" /> -->
+  </include>
+
   <!-- <include file="$(find ins_twist_generator)/launch/dynamic_ins_twist_generator.launch">
     <arg name="yaw_offset" value="-72.7014" />
     <arg name="yaw_diff_threshold" value="2.0" />
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch
index 76fdc686..44a69ccc 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch
@@ -15,7 +15,6 @@
   <arg name="output_log" default="false" />
   <arg name="leaf_size" default="0.5" />
   <arg name="measurement_range" default="200" />
-  <arg name="instance_mode" default="0"/>
 
   <node pkg="points_downsampler" name="voxel_grid_filter" type="voxel_grid_filter">
     <param name="input_topic_name" value="points_raw" />
@@ -23,7 +22,6 @@
     <param name="leaf_size" value="$(arg leaf_size)" />
     <param name="measurement_range" value="$(arg measurement_range)" />
     <param name="output_log" value="$(arg output_log)" />
-    <param name="instance_mode" value="$(arg instance_mode)" />
   </node>
 
   <arg name="enable_gnss_backup" default="true" /> 
@@ -35,7 +33,7 @@
   <arg name="numthreads" default="4" doc="vgicp threads num" />
   <arg name="leafsize" default="1.0" doc="point map downsample leafsize" />
 
-  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node" output="screen">
+  <node pkg="gicp_localizer" type="gicp_localizer_node" name="gicp_localizer_node">
     <param name="init_x" value="$(arg init_x)" />
     <param name="init_y" value="$(arg init_y)" />
     <param name="init_z" value="$(arg init_z)" />
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch
index 2a317ad2..c71066ba 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch
@@ -14,7 +14,6 @@
   <arg name="output_log" default="false" />
   <arg name="leaf_size" default="0.5" />
   <arg name="measurement_range" default="200" />
-  <!-- <arg name="instance_mode" default="0"/> -->
 
   <arg name="method_type" default="0"/>
   <arg name="max_iter" default="10"/>
@@ -32,7 +31,6 @@
     <param name="leaf_size" value="$(arg leaf_size)" />
     <param name="measurement_range" value="$(arg measurement_range)" />
     <param name="output_log" value="$(arg output_log)" />
-    <!-- <param name="instance_mode" value="$(arg instance_mode)" /> -->
   </node>
 
   <node pkg="lidar_localizer" type="modular_ndt_matching" name="modular_ndt_matching_FR">
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch
index be3db144..f3b7d877 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch
@@ -20,7 +20,7 @@
   <arg name="goal_ori_z"            value="0.885" />
   <arg name="goal_ori_w"            value="-0.464" />
 
-  <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner" output="screen">    
+  <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner">    
     <rosparam command="load" file="$(find rubis_autorunner)/cfg/$(arg LaneInfoFile)"/>
     <param name="pathDensity"         value="$(arg pathDensity)" />
     <param name="enableSmoothing"       value="$(arg enableSmoothing)" />
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_1_sensing.launch
new file mode 100644
index 00000000..c60a361a
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_1_sensing.launch
@@ -0,0 +1,76 @@
+<launch>
+  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_new.yaml" />
+
+  <arg name="lidar_input_topic" default="/points_raw_origin"/>
+  <arg name="lidar_output_topic" default="/points_raw"/>
+
+  <arg name="image_input_topic" default="/image_raw_origin"/>
+  <arg name="image_output_topic" default="/image_raw"/>
+
+  <!-- Map TF Publisher -->	
+	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
+  
+  <!-- Vector Map Loader -->
+	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/221104_FMTC_red_course_circle_v1/dtlane.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/221104_FMTC_red_course_circle_v1/idx.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/221104_FMTC_red_course_circle_v1/lane.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/221104_FMTC_red_course_circle_v1/node.csv 
+	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/221104_FMTC_red_course_circle_v1/point.csv"/>
+
+  <!-- Point Map Loader -->
+	<include file="$(find map_file)/launch/points_map_loader.launch">
+    <arg name="scene_num" value="noupdate" />
+    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/FMTC_MobileTech_005.pcd"/>
+    <!-- <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/FMTC_MobileTech.pcd"/> -->
+  </include>  
+
+  <!-- Vision Sensor -->
+  <include file="$(find camera_image)/launch/camera_image.launch"/>
+
+<!-- Lidar Launch File -->
+  <include file="$(find runtime_manager)/launch_files/rubis_velodyne_vlp16_hires_FR.launch" />
+  
+<!-- TF -->
+  <!-- right LiDAR -->
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 -0.8 0.09 -0.68 0 0 base_link velodyne 10"/>
+
+  <!-- left LiDAR -->
+  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="3.3 0.8 0.09 0.68 0 0 base_link velodyne 10"/> -->
+
+  <!--Back LiDAR -->
+  <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="-0.78 -0.3 0.36 3.14 0 0 base_link velodyne 10"/> -->
+  
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss" args="-0.165 0.285 0.00 0.0 0 0 base_link gnss 10"/>
+  <!-- <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" /> -->
+
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 0 0 0 base_link camera 10"/>
+
+  <!-- CAN Interface -->
+  <include file="$(find can_translate)/launch/can_translate.launch" />
+
+  <!-- INS-D Sensor -->
+  <!-- <include file="$(find inertiallabs_ins)/launch/ins.launch" />   -->
+  
+  <!-- INS twist generator -->
+  <include file="$(find ins_twist_generator)/launch/ins_twist_generator.launch">
+    <arg name="yaw_offset" value="-64.569" />
+  </include>
+
+  <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
+    <arg name="input_topic" value="$(arg lidar_input_topic)" />
+    <arg name="output_topic" value="$(arg lidar_output_topic)" />
+  </include>
+  
+  <include file="$(find rubis_pkg)/launch/camera_republisher.launch">
+    <arg name="input_topic" value="$(arg image_input_topic)" />
+    <arg name="output_topic" value="$(arg image_output_topic)" />
+  </include>
+
+  <!-- <include file="$(find ins_twist_generator)/launch/dynamic_ins_twist_generator.launch">
+    <arg name="yaw_offset" value="-72.7014" />
+    <arg name="yaw_diff_threshold" value="2.0" />
+  </include> -->
+
+</launch>
+
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_2_localization.launch
new file mode 100644
index 00000000..c44c0f60
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_2_localization.launch
@@ -0,0 +1,79 @@
+<launch>
+  <!--TODO: Set LidAR Inital Pose-->
+  <!-- <arg name="init_x" value="57.1" />
+  <arg name="init_y" value="1.38" />
+  <arg name="init_z" value="0.48" />
+  <arg name="init_roll" value="0.0" />
+  <arg name="init_pitch" value="0.0" />
+  <arg name="init_yaw" value="1.57" /> -->
+
+  <arg name="init_x" value="-49.91" />
+  <arg name="init_y" value="41.82" />
+  <arg name="init_z" value="8.65" />
+  <arg name="init_roll" value="0.0" />
+  <arg name="init_pitch" value="0.0" />
+  <arg name="init_yaw" value="2.45" />
+
+  <!-- 0.94 -->
+
+  <!-- vel/pose connect -->
+  <!-- <arg name="topic_pose_stamped" default="/ndt_pose" />
+  <arg name="topic_twist_stamped" default="/estimate_twist" />  -->
+  <!-- <arg name="topic_twist_stamped" default="/odom_twist" /> -->
+
+  <!-- points downsampler -->
+  <include file="$(find points_downsampler)/launch/voxel_grid_filter_params.launch">
+  </include>
+
+  <!-- ndt matching kalman filter parameters -->
+  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/kalman_filter.yaml" />
+
+  <!-- ndt_matching --><!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
+  <include file="$(find lidar_localizer)/launch/ndt_matching_params.launch">
+    <arg name="method_type" value="0" /> 
+    <arg name="get_height" value="true" /> 
+    <arg name="init_match_threshold" value="5.0" /> 
+    <arg name="failure_score_diff_threshold" default="1000.0" />   
+    <arg name="recovery_score_diff_threshold" default="1000.0" />  
+    <arg name="failure_pose_diff_threshold" default="1.0" /> 
+    <arg name="recovery_pose_diff_threshold" default="0.5" />
+  </include>
+
+  <!-- ndt config -->
+  <!-- <node pkg="rostopic" type="rostopic" name="config_ndt"
+  args="pub /config/ndt autoware_config_msgs/ConfigNDT
+  '{header: {seq: 8, stamp: {secs: 0, nsecs: 0}, frame_id: ''},
+    init_pos_gnss: 0,
+    x: $(arg init_x),
+    y: $(arg init_y),
+    z: $(arg init_z),
+    roll: $(arg init_roll), 
+    pitch: $(arg init_pitch),
+    yaw: $(arg init_yaw),
+    use_predict_pose: 1,
+    error_threshold: 1.0,
+    resolution: 1.0,
+    step_size: 0.3,
+    trans_epsilon: 0.1,
+    max_iterations: 10}
+  '"/>  -->
+
+  <node pkg="rostopic" type="rostopic" name="config_ndt"
+    args="pub /config/ndt autoware_config_msgs/ConfigNDT
+    '{header: {seq: 8, stamp: {secs: 0, nsecs: 0}, frame_id: ''},
+      init_pos_gnss: 0,
+      x: $(arg init_x),
+      y: $(arg init_y),
+      z: $(arg init_z),
+      roll: $(arg init_roll), 
+      pitch: $(arg init_pitch),
+      yaw: $(arg init_yaw),
+      use_predict_pose: 1,
+      error_threshold: 1.0,
+      resolution: 1.0,
+      step_size: 0.3,
+      trans_epsilon: 0.1,
+      max_iterations: 10}
+    '"/> 
+
+</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_3_detection.launch
similarity index 98%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_3_detection.launch
index 66594cf2..9af37442 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_3_detection.launch
@@ -63,7 +63,7 @@
   </include>
 
   <!-- imm_ukf_pda_track -->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
+  <!-- <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
     <arg name="namespace" default="/detection/object_tracker"/>
     <arg name="tracker_input_topic" default="/detection/lidar_detector/objects_center" />
     <arg name="tracker_output_topic" default="/detection/object_tracker/objects_center" />
@@ -81,6 +81,6 @@
     <arg name="lane_direction_chi_threshold" default="2.71" />
     <arg name="nearest_lane_distance_threshold" default="1.0" />
     <arg name="vectormap_frame" default="/map" />
-  </include>
+  </include> -->
 
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_4_planning.launch
similarity index 59%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_4_planning.launch
index c15f91ce..1d61d32f 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_4_planning.launch
@@ -1,24 +1,45 @@
 <launch>
   <!-- op_global_planning -->
-  <include file="$(find op_global_planner)/launch/op_global_planner.launch">
-    <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
-    <arg name="enableSmoothing"       value="false" /> <!-- 1 or 0 -->
-    <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
-    <arg name="enableRvizInput"       value="true" /> <!-- 1 or 0 -->
-    <arg name="enableReplan"          value="false" /> <!-- 1 or 0 -->  
-    <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
-    <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
-    <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
-    <!-- 138ground single curve -->
-    <arg name="use_static_goal"       value="false" />
-    <arg name="goal_pose_x"           value="57.0422019958" />
-    <arg name="goal_pose_y"           value="-5.43001270294" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.719615198264" />
-    <arg name="goal_ori_w"            value="0.694373074383" />
-  </include>
+  <arg name="LaneInfoFile"          default="ionic_autorunner/lane_info.yaml" />
+  <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
+  <arg name="enableSmoothing"       value="false" /> <!-- 1 or 0 -->
+  <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
+  <arg name="enableRvizInput"       value="true" /> <!-- 1 or 0 -->
+  <arg name="enableReplan"          value="false" /> <!-- 1 or 0 -->  
+  <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
+  <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
+  <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
+  <arg name="enableDynamicMapUpdate"       default="false" />  
+  <!-- Borregas Avenue -->
+  <arg name="use_static_goal"       value="true" />
+  <arg name="goal_pose_x"           value="16.35" />
+  <arg name="goal_pose_y"           value="41.93" />
+  <arg name="goal_pose_z"           value="0.0" />
+  <arg name="goal_ori_x"            value="0.0" />
+  <arg name="goal_ori_y"            value="0.0" />
+  <arg name="goal_ori_z"            value="0.885" />
+  <arg name="goal_ori_w"            value="-0.464" />
+
+  <node pkg="op_global_planner" type="op_global_planner" name="op_global_planner">    
+    <rosparam command="load" file="$(find rubis_autorunner)/cfg/$(arg LaneInfoFile)"/>
+    <param name="pathDensity"         value="$(arg pathDensity)" />
+    <param name="enableSmoothing"       value="$(arg enableSmoothing)" />
+    <param name="enableLaneChange"       value="$(arg enableLaneChange)" />
+    <param name="enableRvizInput"       value="$(arg enableRvizInput)" />
+    <param name="enableReplan"         value="$(arg enableReplan)" />        
+    <param name="velocitySource"       value="$(arg velocitySource)" />
+    <param name="mapSource"         value="$(arg mapSource)" />
+    <param name="mapFileName"         value="$(arg mapFileName)" />    
+    <param name="enableDynamicMapUpdate"   value="$(arg enableDynamicMapUpdate)" />    
+    <param name="use_static_goal"       value="$(arg use_static_goal)"/>
+    <param name="goal_pose_x"           value="$(arg goal_pose_x)" />
+    <param name="goal_pose_y"           value="$(arg goal_pose_y)" />
+    <param name="goal_pose_z"           value="$(arg goal_pose_z)" />
+    <param name="goal_ori_x"            value="$(arg goal_ori_x)" />
+    <param name="goal_ori_y"            value="$(arg goal_ori_y)" />
+    <param name="goal_ori_z"            value="$(arg goal_ori_z)" />
+    <param name="goal_ori_w"            value="$(arg goal_ori_w)" />      
+  </node> 
 
   <!-- op_common_params  -->
   <include file="$(find op_local_planner)/launch/op_common_params_parameter.launch">
@@ -26,7 +47,7 @@
     <arg name="horizonDistance"                 default="120"  />     <!-- Horizon -->
     <arg name="maxLocalPlanDistance"            default="80" />       <!-- Plan Distance-->
     <arg name="pathDensity"                     default="0.5" />      <!-- Path Density-->
-    <arg name="enableFollowing"                 default="true" />     <!-- Enable Following -->
+    <arg name="enableFollowing"                 default="false" />     <!-- Enable Following -->
     <arg name="enableSwerving"                  default="true"  />    <!-- Enable Avoidance -->
     <arg name="minFollowingDistance"            default="30.0"  />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->  
     <arg name="minDistanceToAvoid"              default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
@@ -60,7 +81,7 @@
   <!-- op_trajectory_generator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
     <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
+    <arg name="samplingOutMargin"               default="20" />        <!-- Roll In Margin -->
     <arg name="samplingSpeedFactor"             default="0.25" />    
     <arg name="enableHeadingSmoothing"          default="false" />
   </include>
@@ -68,8 +89,10 @@
   <!-- op_motion_predictor -->
   <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
     <!-- <arg name="input_object_list"               default="[/detection/fusion_tools/objects_center]" /> -->
-    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/object_tracker/objects_center]" />
-    <arg name="object_tf_list"                  default="[camera,velodyne]" />
+    <arg name="input_object_list"               default="[/detection/image_detector/objects,/detection/lidar_detector/rubis_objects_center]" />
+    <arg name="object_tf_list"                  default="[velodyne,camera]" /> -->
+    <!-- <arg name="input_object_list"               default="[/detection/object_tracker/objects_center]" />
+    <arg name="object_tf_list"                  default="[velodyne]" /> -->
     <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
     <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
     <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
@@ -83,16 +106,17 @@
 
   <!-- op_trajectory_evaluator -->
   <include file="$(find op_local_planner)/launch/op_trajectory_evaluator_parameter.launch">
+    <arg name="enableDebug"                default="0" />                
     <arg name="enablePrediction"                default="false" />                
     <arg name="horizontalSafetyDistance"        default="1.2" />
+    <!-- <arg name="horizontalSafetyDistance"        default="0.2" /> -->
     <arg name="verticalSafetyDistance"          default="0.8" />
     <arg name="LateralSkipDistance"             default="5.0" />
     
-    <arg name="PedestrianRightThreshold" default="4.0" />
-    <arg name="PedestrianLeftThreshold" default="4.0" />
-    <arg name="PedestrianImageDetectionRange" default="0.75" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-    <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is not located in this range, our car set speed as max velocity --> 
-    <arg name="VehicleImageWidthThreshold" default="0.018" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
+    <!-- Trajectory blocking threshold  -->
+    <arg name="lateralBlockingThreshold"           default="0.2" />
+    <arg name="frontLongitudinalBlockingThreshold"      default="40" />
+    <arg name="rearLongitudinalBlockingThreshold"      default="0" />
 
     <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
   </include>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_5_control.launch
similarity index 95%
rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch
rename to rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_5_control.launch
index 1b3fd1e0..7585a0a9 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course_new/_ionic_autorunner_5_control.launch
@@ -1,8 +1,8 @@
 <launch>
   <!--  Dynamic Pure Pursuit Param -->
   <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_pure_pursuit_params.yaml" />
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/138ground_single_curve_vel.yaml" />
-  
+  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_FMTC_red_course_vel.yaml" />
+
   <!-- TODO: param tunning -->
   <!-- pure_pursuit  -->
   <include file="$(find pure_pursuit)/launch/pure_pursuit_params.launch">   
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch
deleted file mode 100755
index 38c45a8a..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch
+++ /dev/null
@@ -1,50 +0,0 @@
-<launch>
-	<!-- ROS-Bridge node for Simulator connection -->
-	<node name="websocket_bridge" pkg="rosbridge_server" type="rosbridge_websocket" output="screen" clear_params="true" required="true" />
-
-	<!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env HOME)/autoware.ai/autoware_files/lgsvl/vector_map/intersection_left_turn/dtlane.csv 
-	    $(env HOME)/autoware.ai/autoware_files/lgsvl/vector_map/intersection_left_turn/idx.csv 
-	    $(env HOME)/autoware.ai/autoware_files/lgsvl/vector_map/intersection_left_turn/lane.csv 
-	    $(env HOME)/autoware.ai/autoware_files/lgsvl/vector_map/intersection_left_turn/node.csv 
-	    $(env HOME)/autoware.ai/autoware_files/lgsvl/vector_map/intersection_left_turn/point.csv"/>
-	
-	<!-- Point Map Loader -->
-	<!-- <node pkg="map_file"type="points_map_loader" name="points_map_loader"
-		args="noupdate /home/nvidia/autoware_files/map/points_map/191028_rubis_track_origin.pcd"/> -->
-
-	<!-- Map TF Publisher -->	
-	<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0001 0 0 0 0 0 /world /map 10" />
-	<node pkg="tf" type="static_transform_publisher" name="map_to_mobility" args="0 0 0 0 0 0 /map /mobility 10" />
-
-	<!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-	<node name="image_republisher" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw_origin"  />
-
-	<!-- LiDAR Republisher -->
-	<include file="$(find rubis_pkg)/launch/lidar_republisher.launch">
-		<arg name="node_name" value="lidar_republisher_left" />
-		<arg name="input_topic" value="/points_raw_l_all_direction" />
-		<arg name="output_topic" value="/points_raw_l_republished" />
-	</include>
-
-	<include file="$(find rubis_pkg)/launch/lidar_republisher.launch">
-		<arg name="node_name" value="lidar_republisher_right" />
-		<arg name="input_topic" value="/points_raw_r_all_direction" />
-		<arg name="output_topic" value="/points_raw_r_republished" />
-	</include>
-
-	<include file="$(find rubis_pkg)/launch/lidar_republisher.launch">
-		<arg name="node_name" value="lidar_republisher_back" />
-		<arg name="input_topic" value="/points_raw_b_all_direction" />
-		<arg name="output_topic" value="/points_raw_b_republished" />
-	</include>
-
-	<include file="$(find rubis_pkg)/launch/camera_republisher.launch"/>
-	<include file="$(find rubis_pkg)/launch/gnss_republisher.launch"/>
-
-	<!-- Traffic Signal -->
-	<include file="$(find rubis_pkg)/launch/lgsvl_traffic_signal_test.launch">
-		<arg name="stopline_data" value="$(env HOME)/rubis_ws/src/rubis_pkg/cfg/testbed_stop_line.yaml"/>
-	</include>
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch
deleted file mode 100755
index c9bb8c2b..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch
+++ /dev/null
@@ -1,96 +0,0 @@
-<launch>
-	<!-- static tf publisher between base_link and velodyne -->
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss" args="0 0 0 0 0 0 gps base_link 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="0 0 0 0 0 0 base_link velodyne 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="velodyne_to_velodyne_left" args="2.3 0.7 0.7 0 0 0 velodyne velodyne_left 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="velodyne_to_velodyne_right" args="2.3 -0.7 0.7 0 0 0 velodyne velodyne_right 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="velodyne_to_velodyne_back" args="-2.5 0.0 0.7 3.14 0 0 velodyne velodyne_back 10"/>
-
-  <include file="$(find rubis_pkg)/launch/pcd_clipper.launch">
-    <arg name="node_name" value="pcd_clipper_left"/>
-    <arg name="enableLocalize" value="false"/>
-    <arg name="input_topic" value="/points_raw_l_republished"/>
-    <arg name="clipped_point_output_topic" value="/points_raw_left"/>
-    <arg name="input_frame_id" value="velodyne_left" />
-    <arg name="clipped_center_angle" value="315"/>
-    <arg name="clipped_viewing_angle" value="210"/>
-  </include>
-
-  <include file="$(find rubis_pkg)/launch/pcd_clipper.launch">
-    <arg name="node_name" value="pcd_clipper_right"/>
-    <arg name="enableLocalize" value="true"/>
-    <arg name="input_topic" value="/points_raw_r_republished"/>        
-    <arg name="input_frame_id" value="velodyne_right" />
-    <arg name="clipped_center_angle" value="90"/>
-    <arg name="clipped_viewing_angle" value="120"/>    
-    <arg name="clipped_point_output_topic" value="/points_raw_right"/>
-    <arg name="localization_point_output_topic" value="/points_raw"/>
-    <arg name="localization_center_angle" value="45"/>
-    <arg name="localization_viewing_angle" value="210"/>
-  </include>
-
-    <include file="$(find rubis_pkg)/launch/pcd_clipper.launch">
-    <arg name="node_name" value="pcd_clipper_back"/>
-    <arg name="enableLocalize" value="true"/>
-    <arg name="input_topic" value="/points_raw_b_republished"/>        
-    <arg name="input_frame_id" value="velodyne_back" />
-    <arg name="clipped_center_angle" value="0"/>
-    <arg name="clipped_viewing_angle" value="180"/>    
-    <arg name="clipped_point_output_topic" value="/points_raw_back"/>
-    <arg name="localization_center_angle" value="45"/>
-    <arg name="localization_viewing_angle" value="210"/>
-  </include>
-
-  <!-- ray_ground_filter LEFT-->
-  <include file="$(find points_preprocessor)/launch/ray_ground_filter.launch">
-    <arg name="label" value="left" />
-    <arg name="input_point_topic" value="/points_raw_left" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
-    <arg name="base_frame" value="velodyne_left" />  <!-- Coordinate system to perform transform (default base_link) -->
-    <arg name="max_clipping_height" value="5.0" />  <!-- Remove Points above this height value (default 2.0 meters) -->
-    <arg name="min_clipping_height" value="-0.5" />  <!-- Remove Points below this height value (default -0.5 meters) -->
-    <arg name="min_point_distance" value="2.9" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
-    <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
-    <arg name="concentric_divider_distance" value="0.71" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
-    <arg name="local_max_slope" value="3" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
-    <arg name="general_max_slope" value="9" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
-    <arg name="min_height_threshold" value="0.4" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
-    <arg name="reclass_distance_threshold" value="0.01" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
-  </include>
-
-    <!-- ray_ground_filter RIGHT-->
-  <include file="$(find points_preprocessor)/launch/ray_ground_filter.launch">
-    <arg name="label" value="right" />
-    <arg name="input_point_topic" value="/points_raw_right" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
-    <arg name="base_frame" value="velodyne_right" />  <!-- Coordinate system to perform transform (default base_link) -->
-    <arg name="max_clipping_height" value="5.0" />  <!-- Remove Points above this height value (default 2.0 meters) -->
-    <arg name="min_clipping_height" value="-0.5" />  <!-- Remove Points below this height value (default -0.5 meters) -->
-    <arg name="min_point_distance" value="2.9" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
-    <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
-    <arg name="concentric_divider_distance" value="0.71" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
-    <arg name="local_max_slope" value="3" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
-    <arg name="general_max_slope" value="9" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
-    <arg name="min_height_threshold" value="0.4" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
-    <arg name="reclass_distance_threshold" value="0.01" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
-  </include>
-
-  <!-- ray_ground_filter BACK-->
-  <include file="$(find points_preprocessor)/launch/ray_ground_filter.launch">
-    <arg name="label" value="back" />
-    <arg name="input_point_topic" value="/points_raw_back" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
-    <arg name="base_frame" value="velodyne_back" />  <!-- Coordinate system to perform transform (default base_link) -->
-    <arg name="max_clipping_height" value="5.0" />  <!-- Remove Points above this height value (default 2.0 meters) -->
-    <arg name="min_clipping_height" value="-0.5" />  <!-- Remove Points below this height value (default -0.5 meters) -->
-    <arg name="min_point_distance" value="2.9" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
-    <arg name="radial_divider_angle" value="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
-    <arg name="concentric_divider_distance" value="0.71" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
-    <arg name="local_max_slope" value="3" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
-    <arg name="general_max_slope" value="9" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
-    <arg name="min_height_threshold" value="0.4" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
-    <arg name="reclass_distance_threshold" value="0.01" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
-  </include>
-
-
-  <!-- points downsampler -->
-  <include file="$(find points_downsampler)/launch/points_downsample.launch" />
-			
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch
deleted file mode 100755
index 06c2022c..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch
+++ /dev/null
@@ -1,16 +0,0 @@
-<launch>
-  <!-- nmea2tfpose -->
-  <include file="$(find rubis_pkg)/launch/gnss_localizer.launch">
-    <arg name="plane" value="0" />
-  </include>
-
-  <!-- vel/pose connect -->
-  <arg name="topic_twist_stamped" default="/gnss_vel" />
-  <arg name="topic_pose_stamped" default="/gnss_pose" />
-
-  <!-- vel_pose_mux -->
-  <include file="$(find autoware_connector)/launch/vel_pose_connect.launch">
-    <arg name="topic_pose_stamped" value="$(arg topic_pose_stamped)" />
-    <arg name="topic_twist_stamped" value="$(arg topic_twist_stamped)" />
-  </include>
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch
deleted file mode 100755
index b1c32eae..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<launch>
-  <!-- vision_darknet_detect -->
-  <include file="$(find vision_darknet_detect)/launch/vision_yolo3_detect.launch">
-    <arg name="gpu_device_id" value="0"/>
-    <arg name="score_threshold" value="0.5"/>
-    <arg name="nms_threshold" value="0.45"/>
-    <arg name="network_definition_file" value="$(env HOME)/autoware.ai/autoware_files/vision/yolov3-320.cfg"/>
-    <arg name="pretrained_model_file" value="$(env HOME)/autoware.ai/autoware_files/vision/yolov3-320.weights"/>
-    <arg name="names_file" value="$(find vision_darknet_detect)/darknet/cfg/coco.names"/>
-    <arg name="camera_id" value="/"/>
-    <arg name="image_src" value="/image_raw"/>
-  </include>
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch
deleted file mode 100755
index 105bff0b..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch
+++ /dev/null
@@ -1,225 +0,0 @@
-<launch>
-  <!-- setting of this launch file -->
-  <arg name="car_detection" default="true" />
-  <arg name="pedestrian_detection" default="false" />
-  <arg name="use_gpu" default="false" />
-  <arg name="is_register_lidar2camera_tf" default="true" />
-  <arg name="is_publish_projection_matrix" default="true" />
-  <arg name="is_publish_camera_info" default="true" />
-  <arg name="left_calib" default="$(env HOME)/autoware.ai/autoware_files/lgsvl/data/calibration/double_lidar/left_calibration.yaml"/>
-  <arg name="right_calib" default="$(env HOME)/autoware.ai/autoware_files/lgsvl/data/calibration/double_lidar/right_calibration.yaml"/>
-
-  <!-- calibration_publisher LEFT -->
-  <node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher_left" ns="/" output="screen">
-    <param name="register_lidar2camera_tf" type="bool" value="true"/>
-    <param name="publish_extrinsic_mat" type="bool" value="true"/>
-    <param name="publish_camera_info" type="bool" value="true"/>
-    <param name="calibration_file" type="str" value="$(arg left_calib)"/>
-    <param name="target_frame" type="str" value="/velodyne"/>
-    <param name="camera_frame" type="str" value="/camera"/>
-    <param name="image_topic_src" type="str" value="/image_raw"/>
-    <param name="camera_info_topic" type="str" value="/camera_info_left"/>
-    <param name="projection_matrix_topic" type="str" value="/projection_matrix_left"/>
-  </node>
-
-  <!-- lidar_euclidean_cluster_detect LEFT-->
-  <include file="$(find lidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch">
-    <arg name="points_node" value="/points_no_ground_left" /><!--CHANGE THIS TO READ WHETHER FROM VSCAN OR POINTS_RAW -->
-    <arg name="label" value="left" /><!-- Label of the topics -->
-    <arg name="output_frame" value="velodyne_left" />
-    <arg name="remove_ground" value="true" />
-    <arg name="downsample_cloud" value="false" /> <!-- Apply VoxelGrid Filter with the value given by "leaf_size"-->
-    <arg name="leaf_size" value="0.7" /><!-- Voxel Grid Filter leaf size-->
-    <arg name="cluster_size_min" value="1" /><!-- Minimum number of points to consider a cluster as valid-->
-    <arg name="cluster_size_max" value="100000" /><!-- Maximum number of points to allow inside a cluster-->
-    <arg name="sync" value="false" />
-    <arg name="use_diffnormals" value="false" />
-    <arg name="pose_estimation" value="false" />
-    <arg name="clip_min_height" value="-3.0" />
-    <arg name="clip_max_height" value="4" />
-    <arg name="keep_lanes" value="false" />
-    <arg name="keep_lane_left_distance" value="5" />
-    <arg name="keep_lane_right_distance" value="5" />
-    <arg name="cluster_merge_threshold" value="1.5" />
-    <arg name="clustering_distance" value="0.01" />
-    <arg name="use_vector_map" value="false" />
-    <arg name="wayarea_gridmap_layer" value="wayarea" />
-    <arg name="remove_points_upto" value="0.0" />
-    <arg name="use_gpu" value="$(arg use_gpu)" />
-    <arg name="use_multiple_thres" value="false"/>
-    <arg name="clustering_ranges" value="[15,30,45,60]"/><!-- Distances to segment pointcloud -->
-    <arg name="clustering_distances"
-         value="[0.5,1.1,1.6,2.1,2.6]"/><!-- Euclidean Clustering threshold distance for each segment -->
-  </include>
-
-  <!-- imm_ukf_pda_track LEFT-->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
-    <arg name="label" value="left" />
-    <arg name="namespace" value="/detection/object_tracker"/>
-    <arg name="tracker_input_topic" value="/detection/lidar_detector/objects_left" />
-    <arg name="tracker_output_topic" value="/detection/object_tracker/objects_left" />
-    <arg name="tracking_frame" value="/world" />
-    <arg name="gating_threshold" value="9.22" />
-    <arg name="gate_probability" value="0.99" />
-    <arg name="detection_probability" value="0.9" />
-    <arg name="life_time_threshold" value="8" />
-    <arg name="static_velocity_threshold" value="0.5" />
-    <arg name="static_num_history_threshold" value="3" />
-    <arg name="prevent_explosion_threshold" value="1000" />
-    <arg name="merge_distance_threshold" value="0.5"/>
-    <arg name="use_sukf" value="false" />
-    <arg name="use_map_info" value="false" />
-    <arg name="lane_direction_chi_threshold" value="2.71" />
-    <arg name="nearest_lane_distance_threshold" value="1.0" />
-    <arg name="vectormap_frame" value="/map" />
-  </include>
-
-  <!-- range fusion LEFT-->
-  <include file="$(find range_vision_fusion)/launch/range_vision_fusion.launch">
-    <arg name="label" default="left" />
-    <arg name="detected_objects_range" default="/detection/object_tracker/objects_left"/>
-    <arg name="detected_objects_vision" default="/detection/image_detector/objects"/>
-    <arg name="camera_info_src" default="/camera_info_left"/>
-    <arg name="min_car_dimensions" default="[3,2,2]"/>
-    <arg name="min_person_dimensions" default="[1,2,1]"/>
-    <arg name="min_truck_dimensions" default="[4,2,2]"/>
-    <arg name="sync_topics" default="true"/>
-    <arg name="overlap_threshold" default="0.000001"/>
-    <arg name="vision_area_ratio_threshold" default="0.000001"/>
-    <arg name="use_vector_map" default="false"/>
-    <arg name="namespace" default="/detection/fusion_tools"/>
-  </include>
-
-  <!-- calibration_publisher RIGHT -->
-  <node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher_right" ns="/" output="screen">
-    <param name="register_lidar2camera_tf" type="bool" value="true"/>
-    <param name="publish_extrinsic_mat" type="bool" value="true"/>
-    <param name="publish_camera_info" type="bool" value="true"/>
-    <param name="calibration_file" type="str" value="$(arg right_calib)"/>
-    <param name="target_frame" type="str" value="/velodyne"/>
-    <param name="camera_frame" type="str" value="/camera"/>
-    <param name="image_topic_src" type="str" value="/image_raw"/>
-    <param name="camera_info_topic" type="str" value="/camera_info_right"/>
-    <param name="projection_matrix_topic" type="str" value="/projection_matrix_right"/>
-  </node>
-
-  <!-- lidar_euclidean_cluster_detect RIGHT-->
-  <include file="$(find lidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch">
-    <arg name="points_node" value="/points_no_ground_right" /><!--CHANGE THIS TO READ WHETHER FROM VSCAN OR POINTS_RAW -->
-    <arg name="label" value="right" /><!-- Label of the topics -->
-    <arg name="output_frame" value="velodyne_right" />
-    <arg name="remove_ground" value="true" />
-    <arg name="downsample_cloud" value="false" /> <!-- Apply VoxelGrid Filter with the value given by "leaf_size"-->
-    <arg name="leaf_size" value="0.7" /><!-- Voxel Grid Filter leaf size-->
-    <arg name="cluster_size_min" value="1" /><!-- Minimum number of points to consider a cluster as valid-->
-    <arg name="cluster_size_max" value="100000" /><!-- Maximum number of points to allow inside a cluster-->
-    <arg name="sync" value="false" />
-    <arg name="use_diffnormals" value="false" />
-    <arg name="pose_estimation" value="false" />
-    <arg name="clip_min_height" value="-3.0" />
-    <arg name="clip_max_height" value="4" />
-    <arg name="keep_lanes" value="false" />
-    <arg name="keep_lane_left_distance" value="5" />
-    <arg name="keep_lane_right_distance" value="5" />
-    <arg name="cluster_merge_threshold" value="1.5" />
-    <arg name="clustering_distance" value="0.01" />
-    <arg name="use_vector_map" value="false" />
-    <arg name="wayarea_gridmap_layer" value="wayarea" />
-    <arg name="remove_points_upto" value="0.0" />
-    <arg name="use_gpu" value="$(arg use_gpu)" />
-    <arg name="use_multiple_thres" value="false"/>
-    <arg name="clustering_ranges" value="[15,30,45,60]"/><!-- Distances to segment pointcloud -->
-    <arg name="clustering_distances"
-         value="[0.5,1.1,1.6,2.1,2.6]"/><!-- Euclidean Clustering threshold distance for each segment -->
-  </include>
-
-  <!-- imm_ukf_pda_track RIGHT-->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
-    <arg name="label" value="right" />
-    <arg name="namespace" value="/detection/object_tracker"/>
-    <arg name="tracker_input_topic" value="/detection/lidar_detector/objects_right" />
-    <arg name="tracker_output_topic" value="/detection/object_tracker/objects_right" />
-    <arg name="tracking_frame" value="/world" />
-    <arg name="gating_threshold" value="9.22" />
-    <arg name="gate_probability" value="0.99" />
-    <arg name="detection_probability" value="0.9" />
-    <arg name="life_time_threshold" value="8" />
-    <arg name="static_velocity_threshold" value="0.5" />
-    <arg name="static_num_history_threshold" value="3" />
-    <arg name="prevent_explosion_threshold" value="1000" />
-    <arg name="merge_distance_threshold" value="0.5"/>
-    <arg name="use_sukf" value="false" />
-    <arg name="use_map_info" value="false" />
-    <arg name="lane_direction_chi_threshold" value="2.71" />
-    <arg name="nearest_lane_distance_threshold" value="1.0" />
-    <arg name="vectormap_frame" value="/map" />
-  </include>
-
-  <!-- range fusion RIGHT-->
-  <include file="$(find range_vision_fusion)/launch/range_vision_fusion.launch">
-    <arg name="label" default="right" />
-    <arg name="detected_objects_range" default="/detection/object_tracker/objects_right"/>
-    <arg name="detected_objects_vision" default="/detection/image_detector/objects"/>
-    <arg name="camera_info_src" default="/camera_info_right"/>
-    <arg name="min_car_dimensions" default="[3,2,2]"/>
-    <arg name="min_person_dimensions" default="[1,2,1]"/>
-    <arg name="min_truck_dimensions" default="[4,2,2]"/>
-    <arg name="sync_topics" default="true"/>
-    <arg name="overlap_threshold" default="0.2"/>
-    <arg name="vision_area_ratio_threshold" default="0.0025"/>
-    <arg name="use_vector_map" default="false"/>
-    <arg name="namespace" default="/detection/fusion_tools"/>
-  </include>
-
-  <!-- lidar_euclidean_cluster_detect BACK-->
-  <include file="$(find lidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch">
-    <arg name="points_node" value="/points_no_ground_back" /><!--CHANGE THIS TO READ WHETHER FROM VSCAN OR POINTS_RAW -->
-    <arg name="label" value="back" /><!-- Label of the topics -->
-    <arg name="output_frame" value="velodyne_back" />
-    <arg name="remove_ground" value="true" />
-    <arg name="downsample_cloud" value="false" /> <!-- Apply VoxelGrid Filter with the value given by "leaf_size"-->
-    <arg name="leaf_size" value="0.7" /><!-- Voxel Grid Filter leaf size-->
-    <arg name="cluster_size_min" value="1" /><!-- Minimum number of points to consider a cluster as valid-->
-    <arg name="cluster_size_max" value="100000" /><!-- Maximum number of points to allow inside a cluster-->
-    <arg name="sync" value="false" />
-    <arg name="use_diffnormals" value="false" />
-    <arg name="pose_estimation" value="false" />
-    <arg name="clip_min_height" value="-3.0" />
-    <arg name="clip_max_height" value="4" />
-    <arg name="keep_lanes" value="false" />
-    <arg name="keep_lane_left_distance" value="5" />
-    <arg name="keep_lane_right_distance" value="5" />
-    <arg name="cluster_merge_threshold" value="1.5" />
-    <arg name="clustering_distance" value="0.01" />
-    <arg name="use_vector_map" value="false" />
-    <arg name="wayarea_gridmap_layer" value="wayarea" />
-    <arg name="remove_points_upto" value="0.0" />
-    <arg name="use_gpu" value="$(arg use_gpu)" />
-    <arg name="use_multiple_thres" value="false"/>
-    <arg name="clustering_ranges" value="[15,30,45,60]"/><!-- Distances to segment pointcloud -->
-    <arg name="clustering_distances"
-         value="[0.5,1.1,1.6,2.1,2.6]"/><!-- Euclidean Clustering threshold distance for each segment -->
-  </include>
-
-  <!-- imm_ukf_pda_track BACK-->
-  <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track.launch">
-    <arg name="label" value="back" />
-    <arg name="namespace" value="/detection/object_tracker"/>
-    <arg name="tracker_input_topic" value="/detection/lidar_detector/objects_back" />
-    <arg name="tracker_output_topic" value="/detection/object_tracker/objects_back" />
-    <arg name="tracking_frame" value="/world" />
-    <arg name="gating_threshold" value="9.22" />
-    <arg name="gate_probability" value="0.99" />
-    <arg name="detection_probability" value="0.9" />
-    <arg name="life_time_threshold" value="8" />
-    <arg name="static_velocity_threshold" value="0.5" />
-    <arg name="static_num_history_threshold" value="3" />
-    <arg name="prevent_explosion_threshold" value="1000" />
-    <arg name="merge_distance_threshold" value="0.5"/>
-    <arg name="use_sukf" value="false" />
-    <arg name="use_map_info" value="false" />
-    <arg name="lane_direction_chi_threshold" value="2.71" />
-    <arg name="nearest_lane_distance_threshold" value="1.0" />
-    <arg name="vectormap_frame" value="/map" />
-  </include>
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch
deleted file mode 100755
index 1bd2b8cf..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch
+++ /dev/null
@@ -1,40 +0,0 @@
-<launch>
-  <!-- op_global_planning -->
-  <include file="$(find op_global_planner)/launch/op_global_planner.launch">
-    <arg name="pathDensity"           value="1" /> <!-- distance between each two waypoints-->
-    <arg name="enableSmoothing"       value="true" /> <!-- 1 or 0 -->
-    <arg name="enableLaneChange"      value="false" /> <!-- 1 or 0 -->
-    <arg name="enableRvizInput"       value="true" /> <!-- 1 or 0 -->
-    <arg name="enableReplan"          value="false" /> <!-- 1 or 0 -->  
-    <arg name="velocitySource"        value="1" /> <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->
-    <arg name="mapSource"             value="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
-    <arg name="mapFileName"           value="" /> <!-- incase of kml map source -->
-    <!-- Borregas Avenue -->
-    <!-- <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="-59.64" />
-    <arg name="goal_pose_y"           value="-153.71" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="-0.81" />
-    <arg name="goal_ori_w"            value="0.58" /> -->
-    <!-- TestBed Avenue -->
-    <!-- <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="651.96" />
-    <arg name="goal_pose_y"           value="-610.03" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="0.74" />
-    <arg name="goal_ori_w"            value="0.67" /> -->
-    <!-- TestBed LeftTurn Scenario -->
-    <arg name="use_static_goal"       value="true" />
-    <arg name="goal_pose_x"           value="560.744" />
-    <arg name="goal_pose_y"           value="-1122.939" />
-    <arg name="goal_pose_z"           value="0.0" />
-    <arg name="goal_ori_x"            value="0.0" />
-    <arg name="goal_ori_y"            value="0.0" />
-    <arg name="goal_ori_z"            value="-0.99" />
-    <arg name="goal_ori_w"            value="0.024" />
-  </include>
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch
deleted file mode 100755
index 5d64aed7..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch
+++ /dev/null
@@ -1,89 +0,0 @@
-<launch>
-    <!-- op_common_params  -->
-  <include file="$(find op_local_planner)/launch/op_common_params.launch">
-    <!-- Included in app window -->
-    <arg name="horizonDistance"                 default="120"  />     <!-- Horizon -->
-    <arg name="maxLocalPlanDistance"            default="80" />       <!-- Plan Distance-->
-    <arg name="pathDensity"                     default="0.5" />      <!-- Path Density-->
-    <arg name="rollOutDensity"                  default="5" />      <!-- Horizontal Density -->
-    <arg name="rollOutsNumber"                  default="1"    />     <!-- Rollouts Number -->
-    <arg name="maxVelocity"                     default="10.0" />     <!-- Max Velocity -->
-    <arg name="maxAcceleration"                 default="5.0" />      <!-- Acceleration -->
-    <arg name="maxDeceleration"                 default="-15.0" />    <!-- Deceleration -->
-    <arg name="enableFollowing"                 default="true" />     <!-- Enable Following -->
-    <arg name="enableSwerving"                  default="true"  />    <!-- Enable Avoidance -->
-    <arg name="minFollowingDistance"            default="30.0"  />    <!-- Follow Distance --> <!-- should be bigger than Distance to follow -->  
-    <arg name="minDistanceToAvoid"              default="20.0" />     <!-- Avoiding Distance --> <!-- should be smaller than minFollowingDistance and larger than maxDistanceToAvoid -->
-    <arg name="maxDistanceToAvoid"              default="0.1"  />     <!-- Avoidnace Limit--> <!-- should be smaller than minDistanceToAvoid -->
-    <arg name="enableStopSignBehavior"          default="false" />    <!-- Enable Stop Sign Stop-->
-    <arg name="enableTrafficLightBehavior"      default="true" />     <!-- Enable Traffic Light -->
-    <arg name="enableLaneChange"                default="false" />    <!-- Enable Lane Change -->
-    <arg name="horizontalSafetyDistance"        default="1"  />       <!-- Lateral Safety --> 
-    <arg name="verticalSafetyDistance"          default="2"  />       <!-- Longitudinal Safet y-->
-    <arg name="velocitySource"                  default="1" />        <!-- read velocities from (0- Odometry, 1- autoware current_velocities, 2- car_info) "" -->   
-    <!-- Vehicle Specification -->
-    <arg name="width"                           default="1.85"  />
-    <arg name="length"                          default="4.2"  />
-    <arg name="wheelBaseLength"                 default="2.7"  />
-    <arg name="turningRadius"                   default="5.2"  />
-    <arg name="maxSteerAngle"                   default="0.45" />
-    <arg name="steeringDelay"                   default="1.2" />
-    <!-- Not included in app window -->
-    <arg name="minPursuiteDistance"             default="3.0"  />
-    <arg name="additionalBrakingDistance"       default="5.0"  />
-    <arg name="giveUpDistance"                  default="-4.0"  />
-    <arg name="mapSource"                       default="0" />        <!-- Autoware=0, Vector Map Folder=1, kml=2 -->
-    <arg name="mapFileName"                     default="" />
-    <arg name="minVelocity"                     default="0.1" />  
-    <arg name="speedProfileFactor"              default="1.2"  />
-    <arg name="smoothingDataWeight"             default="0.45"  />
-    <arg name="smoothingSmoothWeight"           default="0.4"  />  
-  </include>
-
-  <!-- op_trajectory_generator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_generator.launch">
-    <arg name="samplingTipMargin"               default="4"  />       <!-- Tip Margin -->
-    <arg name="samplingOutMargin"               default="8" />        <!-- Roll In Margin -->
-    <arg name="samplingSpeedFactor"             default="0.25" />    
-    <arg name="enableHeadingSmoothing"          default="false" />
-  </include>
-
-  <!-- op_motion_predictor -->
-  <include file="$(find op_local_planner)/launch/op_motion_predictor.launch">
-    <arg name="input_object_list"               default="[/detection/fusion_tools/objects_left,/detection/fusion_tools/objects_right,/detection/object_tracker/objects_back]" />
-    <!-- <arg name="input_object_list"               default="[/detection/object_tracker/objects_left,/detection/object_tracker/objects_right,/detection/object_tracker/objects_back]" /> -->
-    <arg name="object_tf_list"                  default="[velodyne_left,velodyne_right,velodyne_back]" />
-    <arg name="enableCurbObstacles"             default="false" />    <!-- Detect curbs from map-->
-    <arg name="enableGenrateBranches"           default="false"/>     <!-- Enable Branching -->
-    <arg name="max_distance_to_lane"            default="2.0"/>       <!-- Distance to closest lane-->
-    <arg name="prediction_distance"             default="25.0"/>      <!-- Prediction distance-->
-    <arg name="enableStepByStepSignal"          default="false" />    <!-- Enable Step by Step Test-->
-    <arg name="enableParticleFilterPrediction"  default="false" />    <!-- Enable Particle Filter prediction -->
-    <!-- Not included in app window -->   
-    <arg name="distanceBetweenCurbs"            default="1.5" />
-    <arg name="visualizationTime"               default="0.25" />
-  </include>
-
-  <!-- op_trajectory_evaluator -->
-  <include file="$(find op_local_planner)/launch/op_trajectory_evaluator.launch">
-    <arg name="enablePrediction"                default="false" />                
-    <arg name="horizontalSafetyDistance"        default="1.2" />
-    <arg name="verticalSafetyDistance"          default="0.8" />
-    <arg name="weightPriority"                  default="0" />
-    <arg name="weightTransition"                default="3" />
-    <arg name="weightLong"                      default="4" />
-    <arg name="weightLat"                       default="10" />
-    <arg name="LateralSkipDistance"             default="5.0" />
-    
-    <arg name="ImageWidth" default="1920" />
-    <arg name="ImageHeight" default="1080" />
-    <arg name="PedestrianRightThreshold" default="4.0" />
-    <arg name="PedestrianLeftThreshold" default="4.0" />
-    <arg name="PedestrianImageDetectionRange" default="0.75" /> <!-- (Ratio If pedestrian's center point is located in this range, our car will be stopped -->               
-    <arg name="VehicleImageDetectionRange" default="0.3" /> <!-- (Ratio) If vehicle's center point is located in this range, our car set speed as max velocity --> 
-    <arg name="VehicleImageWidthThreshold" default="0.018" /> <!-- (Ratio) If vehilce's width ratio is lower than this, it will be not detected -->
-    <arg name="SprintDecisionTime" default="5.0" /> <!-- Seconds -->
-
-    <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" />
-  </include>
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch
deleted file mode 100755
index 6d826bb0..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<launch>
-    <!-- op_behavior_selector -->
-  <include file="$(find op_local_planner)/launch/op_behavior_selector.launch">
-    <arg name="evidence_tust_number"            default="25"/>
-    <arg name="distanceToPedestrianThreshold"   default="25.0" />
-    <arg name="sprintSpeed"                    default="13.5" />
-    <arg name="obstacleWaitingTimeinIntersection"  default="2.0" />
-    <arg name="turnThreshold"                   default="30.0" />
-    <arg name="trafficLightFile"                default="crossing_test/crossing_traffic_light.yaml" />
-    <arg name="stopLineFile"                    default="crossing_test/crossing_stop_line.yaml" />
-    <!-- <arg name="intersectionFile"                default="crossing_test/crossing_intersection.yaml" /> -->
-  </include>
-</launch>
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch
deleted file mode 100755
index e11fc7dd..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch
+++ /dev/null
@@ -1,30 +0,0 @@
-<launch>
-  <!-- pure_pursuit  -->
-  <include file="$(find pure_pursuit)/launch/pure_pursuit.launch">    
-    <!-- Core Parameter -->
-    <arg name="lookahead_ratio"               value="1.0"/>
-    <arg name="minimum_lookahead_distance"    value="12.0"/>
-    
-    <arg name="const_velocity"                value="5.0"/>     
-    <arg name="const_lookahead_distance"      value="4.0"/>
-    <arg name="is_linear_interpolation"       value="True"/>
-    <arg name="publishes_for_steering_robot"  value="True"/>
-    <arg name="add_virtual_end_waypoints"     value="False"/>
-    <arg name="dynamic_params_path"           value="$(env HOME)/autoware.ai/autoware_files/lgsvl/parameter/lgsvl_triple_pure_pursuit.yaml" />
-  </include>
-
-  <!-- twist_filter -->
-  <include file="$(find twist_filter)/launch/twist_filter.launch">    
-    <!-- For twist_filter -->
-    <arg name="wheel_base" default="2.7" />
-    <arg name="lateral_accel_limit" default="5.0" />
-    <arg name="lateral_jerk_limit" default="5.0" />
-    <arg name="lowpass_gain_linear_x" default="0.0" />
-    <arg name="lowpass_gain_angular_z" default="0.0" />
-    <arg name="lowpass_gain_steering_angle" default="0.0" />
-    <arg name="max_stop_count" default="40" /> <!-- 10 = 1 second -->  
-    <!-- For twist_gate -->
-    <arg name="loop_rate" default="30.0" />
-    <arg name="use_decision_maker" default="false" />
-  </include>
-</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh b/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh
deleted file mode 100755
index b2c331c0..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh
+++ /dev/null
@@ -1,56 +0,0 @@
-rosnode kill /lgsvl_triple_lidar_autorunner
-# rosnode kill /websocket_bridge
-# rosnode kill /lgsvl_traffic_signal_test
-# rosnode kill /image_republisher
-# rosnode kill /lidar_republisher_left
-# rosnode kill /lidar_republisher_right
-# rosnode kill /lidar_republisher_back
-# rosnode kill /camera_republisher
-# rosnode kill /gnss_republisher
-# rosnode kill /base_link_to_gnss
-# rosnode kill /base_link_to_velodyne
-# rosnode kill /vector_map_loader
-# rosnode kill /world_to_map
-# rosnode kill /map_to_mobility
-# rosnode kill /velodyne_to_velodyne_left
-# rosnode kill /velodyne_to_velodyne_right
-# rosnode kill /velodyne_to_velodyne_back
-# rosnode kill /pcd_clipper_left
-# rosnode kill /pcd_clipper_right
-# rosnode kill /pcd_clipper_back
-# rosnode kill /ray_ground_filter_left
-# rosnode kill /ray_ground_filter_right
-# rosnode kill /ray_ground_filter_back
-# rosnode kill /voxel_grid_filter
-# rosnode kill /gnss_localizer
-# rosnode kill /pose_relay
-# rosnode kill /vel_relay
-# rosnode kill /vision_darknet_detect
-# rosnode kill /yolo3_rects
-# rosnode kill /calibration_publisher_left
-# rosnode kill /calibration_publisher_right
-# rosnode kill /lidar_euclidean_cluster_detect_left
-# rosnode kill /lidar_euclidean_cluster_detect_right
-# rosnode kill /lidar_euclidean_cluster_detect_back
-# rosnode kill /imm_ukf_pda_left
-# rosnode kill /imm_ukf_pda_right
-# rosnode kill /imm_ukf_pda_back
-# rosnode kill /range_vision_fusion_left
-# rosnode kill /range_vision_fusion_right
-# rosnode kill /detection/fusion_tools/range_fusion_visualization_left
-# rosnode kill /detection/fusion_tools/range_fusion_visualization_right
-# rosnode kill /detection/lidar_detector/cluster_detect_visualization_back
-# rosnode kill /detection/lidar_detector/cluster_detect_visualization_left
-# rosnode kill /detection/lidar_detector/cluster_detect_visualization_right
-# rosnode kill /detection/object_tracker/ukf_track_visualization_back
-# rosnode kill /detection/object_tracker/ukf_track_visualization_left
-# rosnode kill /detection/object_tracker/ukf_track_visualization_right
-# rosnode kill op_global_planner
-# rosnode kill op_common_params
-# rosnode kill op_trajectory_generator
-# rosnode kill op_motion_predictor
-# rosnode kill predicted_objects_visualizer
-# rosnode kill op_trajectory_evaluator
-# rosnode kill op_behavior_selector
-# rosnode kill pure_pursuit
-# rosnode kill twist_filter
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch
index 8929284e..97eddb56 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch
@@ -22,7 +22,7 @@
   </include>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
 
   <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
     <arg name="input_topic" value="$(arg lidar_input_topic)" />
@@ -35,7 +35,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/rubis_ws/src/rubis_autorunner/scripts/transformation_test/_transformation_test_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/transformation_test/_transformation_test_1_sensing.launch
deleted file mode 100644
index 807e075f..00000000
--- a/rubis_ws/src/rubis_autorunner/scripts/transformation_test/_transformation_test_1_sensing.launch
+++ /dev/null
@@ -1,54 +0,0 @@
-<launch>
-  <rosparam command="load" file="$(env USER_HOME)/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml" />
-
-  <arg name="lidar_input_topic" default="/points_raw_origin"/>
-  <arg name="lidar_output_topic" default="/points_raw"/>
-
-  <!-- Map TF Publisher -->	
-	<include file="$(env USER_HOME)/autoware.ai/autoware_files/data/tf/tf.launch" />
-  
-  <!-- Vector Map Loader -->
-	<node pkg="map_file" type="vector_map_loader" name="vector_map_loader" args="
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/dtlane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/idx.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/lane.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/node.csv 
-	    $(env USER_HOME)/autoware.ai/autoware_files/vector_map/cubetown_circle/point.csv"/>
-
-  <!-- Point Map Loader -->
-	<include file="$(find map_file)/launch/points_map_loader.launch">
-    <arg name="scene_num" value="noupdate" />
-    <arg name="path_pcd" value="$(env USER_HOME)/autoware.ai/autoware_files/points_map/CubeTown.pcd"/>
-  </include>
-
-  <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
-
-  <include file="$(find rubis_pkg)/launch/lidar_republisher_params.launch">
-    <arg name="input_topic" value="$(arg lidar_input_topic)" />
-    <arg name="output_topic" value="$(arg lidar_output_topic)" />
-  </include>
-
-  <!-- TF -->
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="1.2 0 2.0 0 0 0 base_link velodyne 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss" args="0.24 0.39 1.135 0 0 0 base_link gnss 10"/>
-  <node pkg="tf" type="static_transform_publisher" name="lidar_to_camera" args="0.029 -0.574 -1.820 4.712 0.009 -1.883 velodyne camera 10" />
-
-
-  <!-- gnss module -->
-  <include file="$(find gnss_module)/launch/gnss_module.launch">
-        <arg name="pose_topic_name" value="/gnss_test_pose" />
-        <arg name="x_offset" value="0.0" />
-        <arg name="y_offset" value="0.0" />
-        <arg name="z_offset" value="-1463" />
-        <arg name="yaw_offset" value="0.0" />    
-        <arg name="use_gnss_tf" value="false" />  <!-- true: base_link TF is published based on gnss_pose-->
-        <arg name="use_sync" value="false" />     <!-- false: use gps data only -->
-        <arg name="use_kalman_filter" value="false" />
-        <arg name="kalman_filter_params" value="$(find gnss_module)/cfg/kalman_filter.yaml" />
-        <arg name="transformation_params" value="$(env USER_HOME)/autoware.ai/autoware_files/transformation/none.yaml" />
-        <arg name="debug" value="false" />        
-  </include>
-
-</launch>
-
diff --git a/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch
index 9393c304..bac37403 100644
--- a/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch
+++ b/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch
@@ -22,9 +22,9 @@
   </include>
 
   <!-- Re-publishing simulator/camera_node/image/compressed topic to /image_raw as expected by Autoware -->
-  <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
+  <node name="republish" type="republish" pkg="image_transport" args="compressed in:=/simulator/camera_node/image raw out:=/image_raw"  />
 
-  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher" output="screen">
+  <node pkg="rubis_pkg" type="lidar_republisher" name="lidar_republisher">
     <param name="/input_topic" value="$(arg lidar_input_topic)" />
     <param name="/output_topic" value="$(arg lidar_output_topic)" />
   </node>
@@ -35,7 +35,7 @@
 
   <!-- gnss localizer -->
   <!-- <arg name="plane" default="0"/>
-  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer" output="screen">
+  <node pkg="rubis_pkg" type="gnss_localizer" name="gnss_localizer">
     <param name="plane" value="$(arg plane)"/>
   </node> -->
 
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp
new file mode 100644
index 00000000..b0891ca0
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp
@@ -0,0 +1,122 @@
+#include "carla_autorunner/carla_autorunner.h"
+
+void CarlaAutorunner::Run(){
+    register_subscribers();             // Register subscribers that shoud check can go next or not
+    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
+    ros::Rate rate(1);                  // Rate can be changed
+    while(ros::ok()){               
+        if(!ros_autorunner_.Run()) break;           // Run Autorunner
+        ros::spinOnce();
+        geometry_msgs::PoseStamped goal_msg;
+        goal_msg.header.stamp = ros::Time::now();
+        goal_msg.header.frame_id = "world";
+        goal_msg.pose.position.x = 302.250;
+        goal_msg.pose.position.y = 118.089;
+        goal_msg.pose.position.z = 0.0;
+        goal_msg.pose.orientation.x = 0.0;
+        goal_msg.pose.orientation.y = 0.0;
+        goal_msg.pose.orientation.z = -0.015;
+        goal_msg.pose.orientation.w = 1.000;
+
+        goal_pub_.publish(goal_msg);
+        rate.sleep();
+    }    
+}
+
+void CarlaAutorunner::register_subscribers(){
+    int total_step_num = nh_.param("/total_step_num", -1);
+    if(total_step_num < 0){
+        std::cout<<"Parameter total_step_num is invalid"<<std::endl;
+        exit(1);
+    }    
+    sub_v_.resize(total_step_num);          // Resizing the subscriber vectors. Its size must be same with number of steps
+
+    // Set the check function(subscriber)
+    sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CarlaAutorunner::points_raw_cb, this);   
+    sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CarlaAutorunner::ndt_pose_cb, this);   
+    sub_v_[STEP(3)] = nh_.subscribe("/detection/lidar_detector/objects_center", 1, &CarlaAutorunner::detection_cb, this);   
+    sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CarlaAutorunner::behavior_state_cb, this);
+
+    initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
+    goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
+}
+
+ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){
+    if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
+        ROS_WARN("[STEP 1] Map and Sensors are prepared");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
+    }
+ }
+
+ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){
+    static int failure_cnt = 0, success_cnt = 0;
+    failure_cnt++;
+    
+    static const double pos_x = 314.072479248;
+    static const double pos_y = 129.654495239;
+    static const double pos_z = 0.044597864151;
+
+    static const double ori_x = 0.0;
+    static const double ori_y = 0.0;
+    static const double ori_z = 0.70715665039;
+    static const double ori_w = 0.70705690407;
+
+
+    if(failure_cnt > 10){        
+        std::cout<<"# Refresh inital pose"<<std::endl;
+        std::cout<<msg.pose.position.x - pos_x <<" "<<msg.pose.position.y - pos_y <<" "
+                 <<msg.pose.orientation.x - ori_x<<" "<<msg.pose.orientation.y - ori_y<<" "
+                 <<msg.pose.orientation.z - ori_z<<" "<<msg.pose.orientation.w - ori_w<<" "<<std::endl;
+        
+        geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
+        initial_pose_msg.header = msg.header;
+        initial_pose_msg.pose.pose.position.x = pos_x;
+        initial_pose_msg.pose.pose.position.y = pos_y;
+        initial_pose_msg.pose.pose.position.z = pos_z;
+        initial_pose_msg.pose.pose.orientation.x = ori_x;
+        initial_pose_msg.pose.pose.orientation.y = ori_y;
+        initial_pose_msg.pose.pose.orientation.z = ori_z;
+        initial_pose_msg.pose.pose.orientation.w = ori_w;
+        initial_pose_pub_.publish(initial_pose_msg);
+        failure_cnt = 0;          
+    }
+
+    if( msg.pose.position.x <= pos_x + 1.5 && msg.pose.position.x >= pos_x - 1.5 &&        
+        msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 &&
+        msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 &&
+        msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 &&
+        msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 &&
+        msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 &&
+        !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){
+        success_cnt++;
+        if(success_cnt < 3) return;
+        ROS_WARN("[STEP 2] Localization is success");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;
+    }
+    else{
+        success_cnt = 0;
+    }
+ }
+
+void CarlaAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){
+    if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){
+        ROS_WARN("[STEP 3] All detection modules are excuted");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
+    }
+}
+
+
+ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
+    std::string state = msg.markers.front().text;    
+
+    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
+        ROS_WARN("[STEP 4] Global & local planning success");
+        ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true;
+    }
+}
+
+
+
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp
new file mode 100644
index 00000000..caac332f
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp
@@ -0,0 +1,11 @@
+#include <carla_autorunner/carla_autorunner.h>
+
+int main(int argc, char* argv[]){
+    ros::init(argc, argv, "carla_autorunner");
+    ros::NodeHandle nh;
+
+    CarlaAutorunner carla_autorunner(nh);
+    carla_autorunner.Run();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp
new file mode 100644
index 00000000..2258ce99
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp
@@ -0,0 +1,111 @@
+#include "carla_autorunner/carla_autorunner.h"
+
+void CarlaAutorunner::Run(){
+    register_subscribers();             // Register subscribers that shoud check can go next or not
+    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
+    ros::Rate rate(1);                  // Rate can be changed
+    while(ros::ok()){               
+        if(!ros_autorunner_.Run()) break;           // Run Autorunner
+        ros::spinOnce();
+        geometry_msgs::PoseStamped goal_msg;
+        goal_msg.header.stamp = ros::Time::now();
+        goal_msg.header.frame_id = "world";
+        goal_msg.pose.position.x = 302.250;
+        goal_msg.pose.position.y = 118.089;
+        goal_msg.pose.position.z = 0.0;
+        goal_msg.pose.orientation.x = 0.0;
+        goal_msg.pose.orientation.y = 0.0;
+        goal_msg.pose.orientation.z = -0.015;
+        goal_msg.pose.orientation.w = 1.000;
+
+        rate.sleep();
+    }    
+}
+
+void CarlaAutorunner::register_subscribers(){
+    int total_step_num = nh_.param("/total_step_num", -1);
+    if(total_step_num < 0){
+        std::cout<<"Parameter total_step_num is invalid"<<std::endl;
+        exit(1);
+    }    
+    sub_v_.resize(total_step_num);          // Resizing the subscriber vectors. Its size must be same with number of steps
+
+    // Set the check function(subscriber)
+    sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CarlaAutorunner::points_raw_cb, this);   
+    sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CarlaAutorunner::ndt_pose_cb, this);   
+    sub_v_[STEP(3)] = nh_.subscribe("/behavior_state", 1, &CarlaAutorunner::behavior_state_cb, this); 
+
+    initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
+    goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
+}
+
+ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){
+    if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
+        ROS_WARN("[STEP 1] Map and Sensors are prepared");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
+    }
+ }
+
+ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){
+    static int failure_cnt = 0, success_cnt = 0;
+    failure_cnt++;
+    
+    static const double pos_x = 314.072479248;
+    static const double pos_y = 129.654495239;
+    static const double pos_z = 0.044597864151;
+
+    static const double ori_x = 0.0;
+    static const double ori_y = 0.0;
+    static const double ori_z = 0.70715665039;
+    static const double ori_w = 0.70705690407;
+
+
+    if(failure_cnt > 10){        
+        std::cout<<"# Refresh inital pose"<<std::endl;
+        std::cout<<msg.pose.position.x - pos_x <<" "<<msg.pose.position.y - pos_y <<" "
+                 <<msg.pose.orientation.x - ori_x<<" "<<msg.pose.orientation.y - ori_y<<" "
+                 <<msg.pose.orientation.z - ori_z<<" "<<msg.pose.orientation.w - ori_w<<" "<<std::endl;
+        
+        geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
+        initial_pose_msg.header = msg.header;
+        initial_pose_msg.pose.pose.position.x = pos_x;
+        initial_pose_msg.pose.pose.position.y = pos_y;
+        initial_pose_msg.pose.pose.position.z = pos_z;
+        initial_pose_msg.pose.pose.orientation.x = ori_x;
+        initial_pose_msg.pose.pose.orientation.y = ori_y;
+        initial_pose_msg.pose.pose.orientation.z = ori_z;
+        initial_pose_msg.pose.pose.orientation.w = ori_w;
+        initial_pose_pub_.publish(initial_pose_msg);
+        failure_cnt = 0;          
+    }
+
+    if( msg.pose.position.x <= pos_x + 1.5 && msg.pose.position.x >= pos_x - 1.5 &&        
+        msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 &&
+        msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 &&
+        msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 &&
+        msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 &&
+        msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 &&
+        !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){
+        success_cnt++;
+        if(success_cnt < 3) return;
+        ROS_WARN("[STEP 2] Localization is success");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;
+    }
+    else{
+        success_cnt = 0;
+    }
+ }
+
+
+ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
+    std::string state = msg.markers.front().text;    
+    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
+        ROS_WARN("[STEP 3] Global & local planning success");
+        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
+    }
+}
+
+
+
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp
new file mode 100644
index 00000000..caac332f
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp
@@ -0,0 +1,11 @@
+#include <carla_autorunner/carla_autorunner.h>
+
+int main(int argc, char* argv[]){
+    ros::init(argc, argv, "carla_autorunner");
+    ros::NodeHandle nh;
+
+    CarlaAutorunner carla_autorunner(nh);
+    carla_autorunner.Run();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner.cpp
new file mode 100644
index 00000000..b0891ca0
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner.cpp
@@ -0,0 +1,122 @@
+#include "carla_autorunner/carla_autorunner.h"
+
+void CarlaAutorunner::Run(){
+    register_subscribers();             // Register subscribers that shoud check can go next or not
+    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
+    ros::Rate rate(1);                  // Rate can be changed
+    while(ros::ok()){               
+        if(!ros_autorunner_.Run()) break;           // Run Autorunner
+        ros::spinOnce();
+        geometry_msgs::PoseStamped goal_msg;
+        goal_msg.header.stamp = ros::Time::now();
+        goal_msg.header.frame_id = "world";
+        goal_msg.pose.position.x = 302.250;
+        goal_msg.pose.position.y = 118.089;
+        goal_msg.pose.position.z = 0.0;
+        goal_msg.pose.orientation.x = 0.0;
+        goal_msg.pose.orientation.y = 0.0;
+        goal_msg.pose.orientation.z = -0.015;
+        goal_msg.pose.orientation.w = 1.000;
+
+        goal_pub_.publish(goal_msg);
+        rate.sleep();
+    }    
+}
+
+void CarlaAutorunner::register_subscribers(){
+    int total_step_num = nh_.param("/total_step_num", -1);
+    if(total_step_num < 0){
+        std::cout<<"Parameter total_step_num is invalid"<<std::endl;
+        exit(1);
+    }    
+    sub_v_.resize(total_step_num);          // Resizing the subscriber vectors. Its size must be same with number of steps
+
+    // Set the check function(subscriber)
+    sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CarlaAutorunner::points_raw_cb, this);   
+    sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CarlaAutorunner::ndt_pose_cb, this);   
+    sub_v_[STEP(3)] = nh_.subscribe("/detection/lidar_detector/objects_center", 1, &CarlaAutorunner::detection_cb, this);   
+    sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CarlaAutorunner::behavior_state_cb, this);
+
+    initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
+    goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
+}
+
+ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){
+    if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
+        ROS_WARN("[STEP 1] Map and Sensors are prepared");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
+    }
+ }
+
+ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){
+    static int failure_cnt = 0, success_cnt = 0;
+    failure_cnt++;
+    
+    static const double pos_x = 314.072479248;
+    static const double pos_y = 129.654495239;
+    static const double pos_z = 0.044597864151;
+
+    static const double ori_x = 0.0;
+    static const double ori_y = 0.0;
+    static const double ori_z = 0.70715665039;
+    static const double ori_w = 0.70705690407;
+
+
+    if(failure_cnt > 10){        
+        std::cout<<"# Refresh inital pose"<<std::endl;
+        std::cout<<msg.pose.position.x - pos_x <<" "<<msg.pose.position.y - pos_y <<" "
+                 <<msg.pose.orientation.x - ori_x<<" "<<msg.pose.orientation.y - ori_y<<" "
+                 <<msg.pose.orientation.z - ori_z<<" "<<msg.pose.orientation.w - ori_w<<" "<<std::endl;
+        
+        geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
+        initial_pose_msg.header = msg.header;
+        initial_pose_msg.pose.pose.position.x = pos_x;
+        initial_pose_msg.pose.pose.position.y = pos_y;
+        initial_pose_msg.pose.pose.position.z = pos_z;
+        initial_pose_msg.pose.pose.orientation.x = ori_x;
+        initial_pose_msg.pose.pose.orientation.y = ori_y;
+        initial_pose_msg.pose.pose.orientation.z = ori_z;
+        initial_pose_msg.pose.pose.orientation.w = ori_w;
+        initial_pose_pub_.publish(initial_pose_msg);
+        failure_cnt = 0;          
+    }
+
+    if( msg.pose.position.x <= pos_x + 1.5 && msg.pose.position.x >= pos_x - 1.5 &&        
+        msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 &&
+        msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 &&
+        msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 &&
+        msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 &&
+        msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 &&
+        !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){
+        success_cnt++;
+        if(success_cnt < 3) return;
+        ROS_WARN("[STEP 2] Localization is success");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;
+    }
+    else{
+        success_cnt = 0;
+    }
+ }
+
+void CarlaAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){
+    if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){
+        ROS_WARN("[STEP 3] All detection modules are excuted");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
+    }
+}
+
+
+ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
+    std::string state = msg.markers.front().text;    
+
+    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
+        ROS_WARN("[STEP 4] Global & local planning success");
+        ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true;
+    }
+}
+
+
+
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner_node.cpp
new file mode 100644
index 00000000..caac332f
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner_node.cpp
@@ -0,0 +1,11 @@
+#include <carla_autorunner/carla_autorunner.h>
+
+int main(int argc, char* argv[]){
+    ros::init(argc, argv, "carla_autorunner");
+    ros::NodeHandle nh;
+
+    CarlaAutorunner carla_autorunner(nh);
+    carla_autorunner.Run();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner.cpp
new file mode 100644
index 00000000..2258ce99
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner.cpp
@@ -0,0 +1,111 @@
+#include "carla_autorunner/carla_autorunner.h"
+
+void CarlaAutorunner::Run(){
+    register_subscribers();             // Register subscribers that shoud check can go next or not
+    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
+    ros::Rate rate(1);                  // Rate can be changed
+    while(ros::ok()){               
+        if(!ros_autorunner_.Run()) break;           // Run Autorunner
+        ros::spinOnce();
+        geometry_msgs::PoseStamped goal_msg;
+        goal_msg.header.stamp = ros::Time::now();
+        goal_msg.header.frame_id = "world";
+        goal_msg.pose.position.x = 302.250;
+        goal_msg.pose.position.y = 118.089;
+        goal_msg.pose.position.z = 0.0;
+        goal_msg.pose.orientation.x = 0.0;
+        goal_msg.pose.orientation.y = 0.0;
+        goal_msg.pose.orientation.z = -0.015;
+        goal_msg.pose.orientation.w = 1.000;
+
+        rate.sleep();
+    }    
+}
+
+void CarlaAutorunner::register_subscribers(){
+    int total_step_num = nh_.param("/total_step_num", -1);
+    if(total_step_num < 0){
+        std::cout<<"Parameter total_step_num is invalid"<<std::endl;
+        exit(1);
+    }    
+    sub_v_.resize(total_step_num);          // Resizing the subscriber vectors. Its size must be same with number of steps
+
+    // Set the check function(subscriber)
+    sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CarlaAutorunner::points_raw_cb, this);   
+    sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CarlaAutorunner::ndt_pose_cb, this);   
+    sub_v_[STEP(3)] = nh_.subscribe("/behavior_state", 1, &CarlaAutorunner::behavior_state_cb, this); 
+
+    initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
+    goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
+}
+
+ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){
+    if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
+        ROS_WARN("[STEP 1] Map and Sensors are prepared");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
+    }
+ }
+
+ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){
+    static int failure_cnt = 0, success_cnt = 0;
+    failure_cnt++;
+    
+    static const double pos_x = 314.072479248;
+    static const double pos_y = 129.654495239;
+    static const double pos_z = 0.044597864151;
+
+    static const double ori_x = 0.0;
+    static const double ori_y = 0.0;
+    static const double ori_z = 0.70715665039;
+    static const double ori_w = 0.70705690407;
+
+
+    if(failure_cnt > 10){        
+        std::cout<<"# Refresh inital pose"<<std::endl;
+        std::cout<<msg.pose.position.x - pos_x <<" "<<msg.pose.position.y - pos_y <<" "
+                 <<msg.pose.orientation.x - ori_x<<" "<<msg.pose.orientation.y - ori_y<<" "
+                 <<msg.pose.orientation.z - ori_z<<" "<<msg.pose.orientation.w - ori_w<<" "<<std::endl;
+        
+        geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
+        initial_pose_msg.header = msg.header;
+        initial_pose_msg.pose.pose.position.x = pos_x;
+        initial_pose_msg.pose.pose.position.y = pos_y;
+        initial_pose_msg.pose.pose.position.z = pos_z;
+        initial_pose_msg.pose.pose.orientation.x = ori_x;
+        initial_pose_msg.pose.pose.orientation.y = ori_y;
+        initial_pose_msg.pose.pose.orientation.z = ori_z;
+        initial_pose_msg.pose.pose.orientation.w = ori_w;
+        initial_pose_pub_.publish(initial_pose_msg);
+        failure_cnt = 0;          
+    }
+
+    if( msg.pose.position.x <= pos_x + 1.5 && msg.pose.position.x >= pos_x - 1.5 &&        
+        msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 &&
+        msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 &&
+        msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 &&
+        msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 &&
+        msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 &&
+        !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){
+        success_cnt++;
+        if(success_cnt < 3) return;
+        ROS_WARN("[STEP 2] Localization is success");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;
+    }
+    else{
+        success_cnt = 0;
+    }
+ }
+
+
+ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
+    std::string state = msg.markers.front().text;    
+    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
+        ROS_WARN("[STEP 3] Global & local planning success");
+        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
+    }
+}
+
+
+
diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp
new file mode 100644
index 00000000..caac332f
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp
@@ -0,0 +1,11 @@
+#include <carla_autorunner/carla_autorunner.h>
+
+int main(int argc, char* argv[]){
+    ros::init(argc, argv, "carla_autorunner");
+    ros::NodeHandle nh;
+
+    CarlaAutorunner carla_autorunner(nh);
+    carla_autorunner.Run();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner.cpp
deleted file mode 100644
index 2be9d167..00000000
--- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-#include "cubetown_autorunner/cubetown_autorunner.h"
-
-void CubetownAutorunner::Run(){
-    register_subscribers();             // Register subscribers that shoud check can go next or not
-    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
-    ros::Rate rate(1);                  // Rate can be changed
-    while(ros::ok()){               
-        ros_autorunner_.Run();           // Run Autorunner
-        ros::spinOnce();
-        rate.sleep();
-    }    
-}
-
-void CubetownAutorunner::register_subscribers(){
-    sub_v_.resize(TOTAL_STEP_NUM);          // Resizing the subscriber vectors. Its size must be same with number of steps
-
-    // Set the check function(subscriber)
-    sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CubetownAutorunner::points_raw_cb, this);   
-    sub_v_[STEP(2)] = nh_.subscribe("/ndt_stat", 1, &CubetownAutorunner::ndt_stat_cb, this);   
-    sub_v_[STEP(3)] = nh_.subscribe("/detection/object_tracker/objects_center", 1, &CubetownAutorunner::detection_cb, this);   
-    sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this);   
-}
-
- void CubetownAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){
-    if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
-        ROS_WARN("[STEP 1] Map and Sensors are prepared");
-    	sleep(SLEEP_PERIOD);
-        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
-    }
- }
-
- void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){
-    if(msg.score < 0.2 && !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){
-        ROS_WARN("[STEP 2] Localization is success");
-    	sleep(SLEEP_PERIOD);
-        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;
-    }
- }
-
-void CubetownAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){
-    if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){
-        ROS_WARN("[STEP 3] All detection modules are excuted");
-    	sleep(SLEEP_PERIOD);
-        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
-    }
-}
-
-
- void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
-    std::string state = msg.markers.front().text;    
-    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
-        ROS_WARN("[STEP 4] Global & local planning success");
-        ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true;
-    }
-}
-
-
-
diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu.cpp
deleted file mode 100644
index 0f28c375..00000000
--- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-#include "cubetown_autorunner/cubetown_autorunner.h"
-
-void CubetownAutorunner::Run(){
-    register_subscribers();             // Register subscribers that shoud check can go next or not
-    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
-    ros::Rate rate(1);                  // Rate can be changed
-    while(ros::ok()){               
-        ros_autorunner_.Run();           // Run Autorunner
-        ros::spinOnce();
-        rate.sleep();
-    }    
-}
-
-void CubetownAutorunner::register_subscribers(){
-    sub_v_.resize(TOTAL_STEP_NUM);          // Resizing the subscriber vectors. Its size must be same with number of steps
-
-    // Set the check function(subscriber)
-    sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CubetownAutorunner::points_raw_cb, this);   
-    sub_v_[STEP(2)] = nh_.subscribe("/ndt_stat", 1, &CubetownAutorunner::ndt_stat_cb, this);   
-    sub_v_[STEP(3)] = nh_.subscribe("/detection/object_tracker/objects_center", 1, &CubetownAutorunner::detection_cb, this);   
-    sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this);   
-}
-
- void CubetownAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){
-    if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
-        ROS_WARN("[STEP 1] Map and Sensors are prepared");
-    	sleep(SLEEP_PERIOD);
-        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
-        ROS_WARN("[STEP 2] check is skiped");
-        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;    
-    }
- }
-
- void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){
-
- }
-
-void CubetownAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){
-    if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){
-        ROS_WARN("[STEP 3] All detection modules are excuted");
-    	sleep(SLEEP_PERIOD);
-        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
-    }
-}
-
-
- void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
-    std::string state = msg.markers.front().text;    
-    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
-        ROS_WARN("[STEP 4] Global & local planning success");
-        ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true;
-    }
-}
-
-
-
diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp
new file mode 100644
index 00000000..a333afdf
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp
@@ -0,0 +1,91 @@
+#include "cubetown_autorunner/cubetown_autorunner.h"
+
+void CubetownAutorunner::Run(){
+    register_subscribers();             // Register subscribers that shoud check can go next or not
+    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
+    ros::Rate rate(1);                  // Rate can be changed
+    while(ros::ok()){               
+        if(!ros_autorunner_.Run()) break;           // Run Autorunner
+        ros::spinOnce();
+        rate.sleep();
+    }    
+}
+
+void CubetownAutorunner::register_subscribers(){
+    int total_step_num = nh_.param("/total_step_num", -1);
+    if(total_step_num < 0){
+        std::cout<<"Parameter total_step_num is invalid"<<std::endl;
+        exit(1);
+    }    
+    sub_v_.resize(total_step_num);          // Resizing the subscriber vectors. Its size must be same with number of steps
+
+    // Set the check function(subscriber)
+    sub_v_[STEP(1)] = nh_.subscribe("/rubis_points_raw", 1, &CubetownAutorunner::points_raw_cb, this);   
+    sub_v_[STEP(2)] = nh_.subscribe("/rubis_current_pose_twist", 1, &CubetownAutorunner::pose_twist_cb, this);   
+    sub_v_[STEP(3)] = nh_.subscribe("/detection/lidar_detector/objects_center", 1, &CubetownAutorunner::detection_cb, this);   
+    sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this);
+
+    initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
+}
+
+ void CubetownAutorunner::points_raw_cb(const rubis_msgs::PointCloud2& msg){
+    if(!msg.msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
+        ROS_WARN("[STEP 1] Map and Sensors are prepared");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
+    }
+ }
+
+ void CubetownAutorunner::pose_twist_cb(const rubis_msgs::PoseTwistStamped& msg){
+    static int failure_cnt = 0, success_cnt = 0;
+    failure_cnt++;    
+
+    if(failure_cnt > 10){        
+        std::cout<<"# Refresh inital pose"<<std::endl;
+        geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
+        initial_pose_msg.header = msg.header;
+        initial_pose_msg.pose.pose.position.x = 56.3796081543;
+        initial_pose_msg.pose.pose.position.y = -0.0106279850006;
+        initial_pose_msg.pose.pose.position.z = 0.465716004372;
+        initial_pose_msg.pose.pose.orientation.x = -0.00171861096474;
+        initial_pose_msg.pose.pose.orientation.y = -0.00120572400155;
+        initial_pose_msg.pose.pose.orientation.z = 0.707457658123;
+        initial_pose_msg.pose.pose.orientation.w = 0.706752612;
+        initial_pose_pub_.publish(initial_pose_msg);
+        failure_cnt = 0;
+    }
+
+    if(msg.pose.pose.position.x <= 57.0 && msg.pose.pose.position.x >= 55.0 &&        
+        msg.pose.pose.position.y >= -1.0 && msg.pose.pose.position.y <= 1.0 &&
+        !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){
+        success_cnt++;
+        if(success_cnt < 3) return;
+        ROS_WARN("[STEP 2] Localization is success");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;
+    }
+    else{
+        success_cnt = 0;
+    }
+    
+ }
+
+void CubetownAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){
+    if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){
+        ROS_WARN("[STEP 3] All detection modules are excuted");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
+    }
+}
+
+
+ void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
+    std::string state = msg.markers.front().text;    
+    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
+        ROS_WARN("[STEP 4] Global & local planning success");
+        ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true;
+    }
+}
+
+
+
diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu_node.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner_node.cpp
similarity index 100%
rename from rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu_node.cpp
rename to rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner_node.cpp
diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp
new file mode 100644
index 00000000..5a07ce75
--- /dev/null
+++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp
@@ -0,0 +1,82 @@
+#include "cubetown_autorunner/cubetown_autorunner.h"
+
+void CubetownAutorunner::Run(){
+    register_subscribers();             // Register subscribers that shoud check can go next or not
+    ros_autorunner_.init(nh_, sub_v_);   // Initialize the ROS-Autorunner
+    ros::Rate rate(1);                  // Rate can be changed
+    while(ros::ok()){               
+        if(!ros_autorunner_.Run()) break;           // Run Autorunner
+        ros::spinOnce();
+        rate.sleep();
+    }    
+}
+
+void CubetownAutorunner::register_subscribers(){
+    int total_step_num = nh_.param("/total_step_num", -1);
+    if(total_step_num < 0){
+        std::cout<<"Parameter total_step_num is invalid"<<std::endl;
+        exit(1);
+    }    
+    sub_v_.resize(total_step_num);          // Resizing the subscriber vectors. Its size must be same with number of steps
+
+    // Set the check function(subscriber)
+    sub_v_[STEP(1)] = nh_.subscribe("/rubis_points_raw", 1, &CubetownAutorunner::points_raw_cb, this);   
+    sub_v_[STEP(2)] = nh_.subscribe("/rubis_current_pose_twist", 1, &CubetownAutorunner::pose_twist_cb, this);   
+    sub_v_[STEP(3)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this); 
+
+    initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
+}
+
+ void CubetownAutorunner::points_raw_cb(const rubis_msgs::PointCloud2& msg){
+    if(!msg.msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){
+        ROS_WARN("[STEP 1] Map and Sensors are prepared");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true;
+    }
+ }
+
+ void CubetownAutorunner::pose_twist_cb(const rubis_msgs::PoseTwistStamped& msg){
+    static int failure_cnt = 0, success_cnt = 0;
+    failure_cnt++;    
+
+    if(failure_cnt > 10){        
+        std::cout<<"# Refresh inital pose"<<std::endl;
+        geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
+        initial_pose_msg.header = msg.header;
+        initial_pose_msg.pose.pose.position.x = 56.3796081543;
+        initial_pose_msg.pose.pose.position.y = -0.0106279850006;
+        initial_pose_msg.pose.pose.position.z = 0.465716004372;
+        initial_pose_msg.pose.pose.orientation.x = -0.00171861096474;
+        initial_pose_msg.pose.pose.orientation.y = -0.00120572400155;
+        initial_pose_msg.pose.pose.orientation.z = 0.707457658123;
+        initial_pose_msg.pose.pose.orientation.w = 0.706752612;
+        initial_pose_pub_.publish(initial_pose_msg);
+        failure_cnt = 0;
+    }
+
+    if(msg.pose.pose.position.x <= 57.0 && msg.pose.pose.position.x >= 55.0 &&        
+        msg.pose.pose.position.y >= -1.0 && msg.pose.pose.position.y <= 1.00 &&
+        !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){
+        success_cnt++;
+        if(success_cnt < 3) return;
+        ROS_WARN("[STEP 2] Localization is success");
+    	sleep(SLEEP_PERIOD);
+        ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true;
+    }
+    else{
+        success_cnt = 0;
+    }
+    
+ }
+
+
+ void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){
+    std::string state = msg.markers.front().text;    
+    if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){
+        ROS_WARN("[STEP 3] Global & local planning success");
+        ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true;
+    }
+}
+
+
+
diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp
similarity index 100%
rename from rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_node.cpp
rename to rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp
diff --git a/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp
index 4d489736..84ac22a5 100755
--- a/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp
+++ b/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp
@@ -83,16 +83,16 @@ void ROSAutorunner::init(ros::NodeHandle nh, Sub_v sub_v){
     print_step_info_list();
 }
 
-void ROSAutorunner::Run(){
+bool ROSAutorunner::Run(){
     if(current_step_ == step_info_list_.end()){
-        return;
+        return false;
     }
 
     if(!current_step_->check_topic)
         current_step_->is_prepared = true;
 
     if(current_step_->is_prepared == true){
-        ROS_WARN("[Step %d] Activated", INT_TO_STEP(current_step_->step_id));
+        ROS_WARN("[STEP %d] Activated", INT_TO_STEP(current_step_->step_id));
         if(current_step_->target_type == RUN)
             run_node(current_step_->step_id);
         else if(current_step_->target_type == LAUNCH)
@@ -109,7 +109,7 @@ void ROSAutorunner::Run(){
         ++current_step_;
     }      
 
-    return;
+    return true;
 }
 
 void ROSAutorunner::run_node(int step_id){
diff --git a/rubis_ws/src/rubis_logger/cfg/rubis_logger.yaml b/rubis_ws/src/rubis_logger/cfg/rubis_logger.yaml
new file mode 100644
index 00000000..2e6ec45e
--- /dev/null
+++ b/rubis_ws/src/rubis_logger/cfg/rubis_logger.yaml
@@ -0,0 +1,18 @@
+target_topics:
+  - "/ctrl_cmd"
+  - "/car_ctrl_output"
+  - "/car_ctrl_input"
+  - "/rubis_log_handler"
+  
+available_topics:
+  - "/ctrl_cmd"
+  - "/car_ctrl_output"
+  - "/car_ctrl_input"
+  - "/odom"
+  - "/vehicle_cmd_test"
+  - "/rubis_log_handler"
+
+logger_rate: 100
+paramtest: 2020
+
+logdirpath: "/home/nvidia/rubis_ws/log/"
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch b/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch
index 7f40bcec..48fe81af 100644
--- a/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch
+++ b/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch
@@ -1,4 +1,4 @@
 <launch>
     <rosparam file="$(find rubis_logger)/cfg/logger_brake_dist.yaml" />
-    <node pkg="rubis_logger" type="logger_brake_dist" name="logger_brake_dist" output="screen" />
+    <node pkg="rubis_logger" type="logger_brake_dist" name="logger_brake_dist" />
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch b/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch
index f00c63fe..531d9ccf 100644
--- a/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch
+++ b/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch
@@ -1,3 +1,3 @@
 <launch>
-    <node pkg="rubis_logger" type="logger_brake_dist" name="logger_brake_dist" output="screen" />
+    <node pkg="rubis_logger" type="logger_brake_dist" name="logger_brake_dist" />
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_logger/launch/logger_topic.launch b/rubis_ws/src/rubis_logger/launch/logger_topic.launch
index d47780a6..5debc56f 100644
--- a/rubis_ws/src/rubis_logger/launch/logger_topic.launch
+++ b/rubis_ws/src/rubis_logger/launch/logger_topic.launch
@@ -1,4 +1,4 @@
 <launch>
     <rosparam file="$(find rubis_logger)/cfg/rubis_logger.yaml" />
-    <node pkg="rubis_logger" type="rubis_logger" name="rubis_logger" clear_params="true" output="screen" />
+    <node pkg="rubis_logger" type="rubis_logger" name="rubis_logger" clear_params="true" />
 </launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_logger/launch/rubis_logger.launch b/rubis_ws/src/rubis_logger/launch/rubis_logger.launch
new file mode 100644
index 00000000..d47780a6
--- /dev/null
+++ b/rubis_ws/src/rubis_logger/launch/rubis_logger.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam file="$(find rubis_logger)/cfg/rubis_logger.yaml" />
+    <node pkg="rubis_logger" type="rubis_logger" name="rubis_logger" clear_params="true" output="screen" />
+</launch>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_logger/package.xml b/rubis_ws/src/rubis_logger/package.xml
index 369c9d97..e1cf690a 100644
--- a/rubis_ws/src/rubis_logger/package.xml
+++ b/rubis_ws/src/rubis_logger/package.xml
@@ -4,58 +4,15 @@
   <version>0.0.0</version>
   <description>The rubis_logger package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="hypark@todo.todo">hypark</maintainer>
   <maintainer email="akyeast@gmail.com">dmshin</maintainer>
+  <license>Apache 2</license>
 
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/rubis_logger</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <depend>autoware_msgs</depend>
   <depend>geometry_msgs</depend>
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
+  <depend>nav_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>can_data_msgs</depend>
+  <depend>rubis_logger_msgs</depend>
 </package>
diff --git a/rubis_ws/src/rubis_logger_msgs/package.xml b/rubis_ws/src/rubis_logger_msgs/package.xml
index ce2e5380..f5187634 100644
--- a/rubis_ws/src/rubis_logger_msgs/package.xml
+++ b/rubis_ws/src/rubis_logger_msgs/package.xml
@@ -13,7 +13,7 @@
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
+  <license>Apache 2</license>
 
   <buildtool_depend>catkin</buildtool_depend>
 
diff --git a/rubis_ws/src/rubis_pkg/CMakeLists.txt b/rubis_ws/src/rubis_pkg/CMakeLists.txt
index 4048c8db..8dd94645 100644
--- a/rubis_ws/src/rubis_pkg/CMakeLists.txt
+++ b/rubis_ws/src/rubis_pkg/CMakeLists.txt
@@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
   gnss
   rubis_lib
   rubis_msgs
-  geometry_msgs
+  visualization_msgs
 )
 
 catkin_package(
@@ -100,4 +100,31 @@ add_dependencies(rubis_pose_relay ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_E
 
 target_link_libraries(rubis_pose_relay
   ${catkin_LIBRARIES}
-)
\ No newline at end of file
+)
+
+install(
+  TARGETS
+    lidar_republisher
+    camera_republisher
+    gnss_republisher
+    gnss_localizer
+    fake_current_pose
+    fake_traffic_signal_generator
+    rubis_pose_relay
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY cfg/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+)
diff --git a/rubis_ws/src/rubis_pkg/include/gnss_localizer.h b/rubis_ws/src/rubis_pkg/include/gnss_localizer.h
index 317a5987..1edf9048 100644
--- a/rubis_ws/src/rubis_pkg/include/gnss_localizer.h
+++ b/rubis_ws/src/rubis_pkg/include/gnss_localizer.h
@@ -19,6 +19,7 @@
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
 #include <eigen3/Eigen/Eigen>
+#include <rubis_lib/sched.hpp>
 
 namespace gnss_localizer
 {
diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch
index fa7019a5..3f5a0762 100644
--- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch
+++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch
@@ -2,11 +2,8 @@
     <arg name="input_topic" default="/points_raw_origin"/>
     <arg name="output_topic" default="/points_raw"/>
     <arg name="node_name" default="lidar_republisher"/>
-    <arg name="instance_mode" default="0" />
     <node pkg="rubis_pkg" type="lidar_republisher" name="$(arg node_name)" output="screen">
         <param name="/input_topic" value="$(arg input_topic)" />
         <param name="/output_topic" value="$(arg output_topic)" />
-        <param name="/instance_mode" value="$(arg instance_mode)" />
-        
     </node>
 </launch>
diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch
index 59639703..f4f92931 100644
--- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch
+++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch
@@ -2,10 +2,11 @@
     <arg name="input_topic" default="/points_raw_origin"/>
     <arg name="output_topic" default="/points_raw"/>
     <arg name="node_name" default="lidar_republisher"/>
-    <!-- <arg name="instance_mode" default="0" /> -->
+    <arg name="task_response_time_filename" default="~/Documents/profiling/response_time/lidar_republisher.csv"/>
     <node pkg="rubis_pkg" type="lidar_republisher" name="$(arg node_name)" output="screen">
         <param name="/input_topic" value="$(arg input_topic)" />
         <param name="/output_topic" value="$(arg output_topic)" />
-        <!-- <param name="/instance_mode" value="$(arg instance_mode)" /> -->
+        <param name="/node_name" value="$(arg node_name)" />
+        <param name="/task_response_time_filename" value="$(arg task_response_time_filename)" />
     </node>
 </launch>
diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch.ori b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch.ori
new file mode 100644
index 00000000..3f5a0762
--- /dev/null
+++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch.ori
@@ -0,0 +1,9 @@
+<launch>
+    <arg name="input_topic" default="/points_raw_origin"/>
+    <arg name="output_topic" default="/points_raw"/>
+    <arg name="node_name" default="lidar_republisher"/>
+    <node pkg="rubis_pkg" type="lidar_republisher" name="$(arg node_name)" output="screen">
+        <param name="/input_topic" value="$(arg input_topic)" />
+        <param name="/output_topic" value="$(arg output_topic)" />
+    </node>
+</launch>
diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch
index cb72d0b5..6f819e29 100644
--- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch
+++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch
@@ -5,11 +5,8 @@
   <!-- send table.xml to param server -->
   <arg name="topic_pose_stamped" default="" />
   <arg name="topic_twist_stamped" default="" />
-  <arg name="instance_mode" default="0" />
 
-  <node pkg="rubis_pkg" type="rubis_pose_relay" name="pose_relay" output="screen" args="$(arg topic_pose_stamped) /current_pose">
-    <param name="instance_mode" value="$(arg instance_mode)" />
-  </node>
+  <node pkg="rubis_pkg" type="rubis_pose_relay" name="pose_relay" output="screen" args="$(arg topic_pose_stamped) /current_pose"/>
   <node pkg="topic_tools" type="relay" name="vel_relay" output="screen" args="$(arg topic_twist_stamped) /current_velocity"/>
   
 
diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch
index d2272d1d..6f819e29 100644
--- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch
+++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch
@@ -5,11 +5,8 @@
   <!-- send table.xml to param server -->
   <arg name="topic_pose_stamped" default="" />
   <arg name="topic_twist_stamped" default="" />
-  <!-- <arg name="instance_mode" default="0" /> -->
 
-  <node pkg="rubis_pkg" type="rubis_pose_relay" name="pose_relay" output="screen" args="$(arg topic_pose_stamped) /current_pose">
-    <!-- <param name="instance_mode" value="$(arg instance_mode)" /> -->
-  </node>
+  <node pkg="rubis_pkg" type="rubis_pose_relay" name="pose_relay" output="screen" args="$(arg topic_pose_stamped) /current_pose"/>
   <node pkg="topic_tools" type="relay" name="vel_relay" output="screen" args="$(arg topic_twist_stamped) /current_velocity"/>
   
 
diff --git a/rubis_ws/src/rubis_pkg/package.xml b/rubis_ws/src/rubis_pkg/package.xml
index 875b0aa6..0e22ccbe 100644
--- a/rubis_ws/src/rubis_pkg/package.xml
+++ b/rubis_ws/src/rubis_pkg/package.xml
@@ -10,37 +10,28 @@
   <export>
   </export>
 <build_depend>rospy</build_depend>
-<build_depend>python-lgsvl</build_depend>
-<build_depend>python-os</build_depend>
-<build_depend>python-time</build_depend>
-<build_depend>python-json</build_depend>
-<build_depend>python-threading</build_depend>
 <build_depend>geometry_msgs</build_depend>
 <build_depend>autoware_msgs</build_depend>
 <build_depend>std_msgs</build_depend>
 <build_depend>jsk_recognition_msgs</build_depend>
 <build_depend>rubis_msgs</build_depend>
 <build_export_depend>rospy</build_export_depend>
-<build_export_depend>python-lgsvl</build_export_depend>
-<build_export_depend>python-os</build_export_depend>
-<build_export_depend>python-time</build_export_depend>
-<build_export_depend>python-json</build_export_depend>
-<build_export_depend>python-threading</build_export_depend>
 <build_export_depend>geometry_msgs</build_export_depend>
 <build_export_depend>autoware_msgs</build_export_depend>
 <build_export_depend>std_msgs</build_export_depend>
 <build_export_depend>jsk_recognition_msgs</build_export_depend>
 <build_export_depend>rubis_msgs</build_export_depend>
 <exec_depend>rospy</exec_depend>
-<exec_depend>python-lgsvl</exec_depend>
-<exec_depend>python-os</exec_depend>
-<exec_depend>python-time</exec_depend>
-<exec_depend>python-json</exec_depend>
-<exec_depend>python-threading</exec_depend>
 <exec_depend>geometry_msgs</exec_depend>
 <exec_depend>nmea_msgs</exec_depend>
 <exec_depend>autoware_msgs</exec_depend>
 <exec_depend>std_msgs</exec_depend>
 <exec_depend>jsk_recognition_msgs</exec_depend>
 <exec_depend>rubis_msgs</exec_depend>
+<depend>roscpp</depend>
+<depend>tf</depend>
+<depend>gnss</depend>
+<depend>rubis_lib</depend>
+<depend>visualization_msgs</depend>
+
 </package>
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp b/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp
index 7f094c5d..a7c7a342 100644
--- a/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp
+++ b/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp
@@ -70,17 +70,39 @@ void Nmea2TFPoseNode::initForROS()
   }
 
   // setup subscriber
-  sub1_ = nh_.subscribe("nmea_sentence", 100, &Nmea2TFPoseNode::callbackFromNmeaSentence, this);
-  sub2_ = nh_.subscribe("imu_raw", 100, &Nmea2TFPoseNode::callbackFromIMU, this);
+  sub1_ = nh_.subscribe("nmea_sentence", 1, &Nmea2TFPoseNode::callbackFromNmeaSentence, this);
+  sub2_ = nh_.subscribe("imu_raw", 1, &Nmea2TFPoseNode::callbackFromIMU, this);
 
   // setup publisher
   if(enable_offset_){
-    pub1_ = nh_.advertise<geometry_msgs::PoseStamped>("gnss_offset_pose", 10);
-    pub2_ = nh_.advertise<geometry_msgs::PoseStamped>("gnss_transformed_pose", 10);
+    pub1_ = nh_.advertise<geometry_msgs::PoseStamped>("gnss_offset_pose", 1);
+    pub2_ = nh_.advertise<geometry_msgs::PoseStamped>("gnss_transformed_pose", 1);
   }
   else
-    pub1_ = nh_.advertise<geometry_msgs::PoseStamped>("gnss_pose", 10);
-  vel_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("gnss_vel", 10);
+    pub1_ = nh_.advertise<geometry_msgs::PoseStamped>("gnss_pose", 1);
+  vel_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("gnss_vel", 1);
+
+  // Scheduling & Profiling Setup
+  std::string node_name = ros::this_node::getName();
+  std::string task_response_time_filename;
+  nh_.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/gnss_localizer.csv");
+
+  int rate;
+  nh_.param<int>(node_name+"/rate", rate, 10);
+
+  struct rubis::sched_attr attr;
+  std::string policy;
+  int priority, exec_time ,deadline, period;
+    
+  nh_.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+  nh_.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+  nh_.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+  nh_.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+  nh_.param(node_name+"/task_scheduling_configs/period", period, 0);
+  attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+  rubis::init_task_scheduling(policy, attr);
+
+  rubis::init_task_profiling(task_response_time_filename);
 }
 
 void Nmea2TFPoseNode::run()
diff --git a/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp b/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp
index 8d24cf4e..65a40eef 100644
--- a/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp
+++ b/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp
@@ -9,6 +9,8 @@ static ros::Publisher pub, pub_rubis;
 int is_topic_ready = 1;
 
 void points_cb(const sensor_msgs::PointCloud2ConstPtr& msg){
+    rubis::start_task_profiling_at_initial_node(msg->header.stamp.sec, msg->header.stamp.nsec);
+
     sensor_msgs::PointCloud2 msg_with_intensity = *msg;
     
     msg_with_intensity.fields.at(3).datatype = 7;
@@ -16,15 +18,31 @@ void points_cb(const sensor_msgs::PointCloud2ConstPtr& msg){
 
     pub.publish(msg_with_intensity);
 
-    if(rubis::instance_mode_){
-        rubis_msgs::PointCloud2 rubis_msg_with_intensity;
-        rubis_msg_with_intensity.instance = rubis::instance_;
-        rubis_msg_with_intensity.msg = msg_with_intensity;
-        pub_rubis.publish(rubis_msg_with_intensity);
-    }
+    rubis_msgs::PointCloud2 rubis_msg_with_intensity;
+    rubis_msg_with_intensity.instance = rubis::instance_;
+    rubis_msg_with_intensity.msg = msg_with_intensity;
+    pub_rubis.publish(rubis_msg_with_intensity);
 
-    if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-    rubis::sched::task_state_ = TASK_STATE_DONE;
+    rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+    rubis::instance_ = rubis::instance_+1;
+    rubis::lidar_instance_ = rubis::lidar_instance_+1;
+}
+
+std::string exec(const char* cmd) {
+    char buffer[128];
+    std::string result = "";
+    FILE* pipe = popen(cmd, "r");
+    if (!pipe) throw std::runtime_error("popen() failed!");
+    try {
+        while (fgets(buffer, sizeof(buffer), pipe) != NULL) {
+            result += buffer;
+        }
+    } catch (...) {
+        pclose(pipe);
+        throw;
+    }
+    pclose(pipe);
+    return result;
 }
 
 int main(int argc, char** argv){
@@ -39,72 +57,36 @@ int main(int argc, char** argv){
     std::string output_topic_name = node_name + "/output_topic";
     std::string rubis_output_topic;
 
-    nh.param<int>(node_name+"/instance_mode", rubis::instance_mode_, 0);
     nh.param<std::string>(input_topic_name, input_topic, "/points_raw_origin");
     nh.param<std::string>(output_topic_name, output_topic, "/points_raw");
 
     sub = nh.subscribe(input_topic, 1, points_cb);      
     pub = nh.advertise<sensor_msgs::PointCloud2>(output_topic, 1);
-    if(rubis::instance_mode_){
-        rubis_output_topic = "/rubis_"+output_topic.substr(1);
-        pub_rubis = nh.advertise<rubis_msgs::PointCloud2>(rubis_output_topic, 1);
-    }
-
-    // Scheduling Setup
-    int task_scheduling_flag;
-    int task_profiling_flag;
+    rubis_output_topic = "/rubis_"+output_topic.substr(1);
+    pub_rubis = nh.advertise<rubis_msgs::PointCloud2>(rubis_output_topic, 1);
+    
+    // Scheduling & Profiling Setup
     std::string task_response_time_filename;
-    int rate;
-    double task_minimum_inter_release_time;
-    double task_execution_time;
-    double task_relative_deadline; 
+    private_nh.param<std::string>(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_republisher.csv");
 
-    private_nh.param<int>("/lidar_republisher/task_scheduling_flag", task_scheduling_flag, 0);
-    private_nh.param<int>("/lidar_republisher/task_profiling_flag", task_profiling_flag, 0);
-    private_nh.param<std::string>("/lidar_republisher/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_republisher.csv");
-    private_nh.param<int>("/lidar_republisher/rate", rate, 10);
-    private_nh.param("/lidar_republisher/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
-    private_nh.param("/lidar_republisher/task_execution_time", task_execution_time, (double)10);
-    private_nh.param("/lidar_republisher/task_relative_deadline", task_relative_deadline, (double)10);
-
-    /* For Task scheduling */
-    if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
-
-    if(!task_scheduling_flag && !task_profiling_flag){
-        ros::spin();
-    }
-    else{
-        ros::Rate r(rate);
-        // Initialize task ( Wait until first necessary topic is published )
-        while(ros::ok()){
-            if(rubis::sched::is_task_ready_ == TASK_READY) break;
-            ros::spinOnce();
-            r.sleep();      
-        }
-
-        // Executing task
-        while(ros::ok()){
-            if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-            if(rubis::sched::task_state_ == TASK_STATE_READY){                
-                if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-                rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-            }
+    int rate;
+    private_nh.param<int>(node_name+"/rate", rate, 10);
 
-            ros::spinOnce();
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time ,deadline, period;
 
-            if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
+    private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE"));    
+    private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99);
+    private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0);
+    private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0);
+    private_nh.param(node_name+"/task_scheduling_configs/period", period, 0);
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);    
+    rubis::init_task_scheduling(policy, attr);
 
-            if(rubis::sched::task_state_ == TASK_STATE_DONE){
-                if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-                rubis::sched::task_state_ = TASK_STATE_READY;
-                rubis::instance_ = rubis::instance_+1;
-            }
-            
-        
-            r.sleep();
-        }
-    }
+    rubis::init_task_profiling(task_response_time_filename);
+    
+    ros::spin();
     
     return 0;
 }
\ No newline at end of file
diff --git a/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp b/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp
index 54e6cd23..feb0d728 100644
--- a/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp
+++ b/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp
@@ -8,20 +8,15 @@ ros::Subscriber rubis_sub_, sub_;
 ros::Publisher rubis_pub_, pub_;
 
 inline void relay(const geometry_msgs::PoseStampedConstPtr& msg){
-    if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){
-        rubis_msgs::PoseStamped rubis_msg;
-        rubis_msg.instance = rubis::instance_;
-        rubis_msg.msg = *msg;
-        rubis_pub_.publish(rubis_msg);
-    }
+    rubis_msgs::PoseStamped rubis_msg;
+    rubis_msg.instance = rubis::instance_;
+    rubis_msg.msg = *msg;
+    rubis_pub_.publish(rubis_msg);
     pub_.publish(msg);
-
-    if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-    rubis::sched::task_state_ = TASK_STATE_DONE;
 }
 
 void cb(const geometry_msgs::PoseStampedConstPtr& msg){
-    rubis::instance_ = RUBIS_NO_INSTANCE;
+    rubis::instance_ = 0;
     relay(msg);
 }
 
@@ -35,8 +30,6 @@ int main(int argc, char* argv[]){
     ros::init(argc, argv, "pose_relay");  
 
     // Scheduling Setup
-    int task_scheduling_flag;
-    int task_profiling_flag;
     std::string task_response_time_filename;
     int rate;
     double task_minimum_inter_release_time;
@@ -45,65 +38,34 @@ int main(int argc, char* argv[]){
 
     ros::NodeHandle nh;
 
-    nh.param<int>("/pose_relay/task_scheduling_flag", task_scheduling_flag, 0);
-    nh.param<int>("/pose_relay/task_profiling_flag", task_profiling_flag, 0);
     nh.param<std::string>("/pose_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pose_relay.csv");
     nh.param<int>("/pose_relay/rate", rate, 10);
     nh.param("/pose_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10);
     nh.param("/pose_relay/task_execution_time", task_execution_time, (double)10);
     nh.param("/pose_relay/task_relative_deadline", task_relative_deadline, (double)10);
-    nh.param<int>("/pose_relay/instance_mode", rubis::instance_mode_, 0);
 
     input_topic_ = std::string(argv[1]);
 
     std::cout<<"!!! input topic  "<<input_topic_<<std::endl;
 
-    if(rubis::instance_mode_){
-        rubis_input_topic_ = "/rubis_"+input_topic_.substr(1);
-        rubis_sub_ = nh.subscribe(rubis_input_topic_, 10, rubis_cb);
-        rubis_pub_ = nh.advertise<rubis_msgs::PoseStamped>("/rubis_current_pose", 10);
-    }
-    else{
-        rubis_sub_ = nh.subscribe(input_topic_, 10, cb);
-    }
+    rubis_input_topic_ = "/rubis_"+input_topic_.substr(1);
+    rubis_sub_ = nh.subscribe(rubis_input_topic_, 10, rubis_cb);
+    rubis_pub_ = nh.advertise<rubis_msgs::PoseStamped>("/rubis_current_pose", 10);
 
     pub_ = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 10);
 
     /* For Task scheduling */
-    if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
+    rubis::init_task_profiling(task_response_time_filename);
 
-    if(!task_scheduling_flag && !task_profiling_flag){
-        ros::spin();
-    }
-    else{
-        ros::Rate r(rate);
-        // Initialize task ( Wait until first necessary topic is published )
-        while(ros::ok()){
-            if(rubis::sched::is_task_ready_ == TASK_READY) break;
-            ros::spinOnce();
-            r.sleep();      
-        }
-
-        // Executing task
-        while(ros::ok()){
-            if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-            if(rubis::sched::task_state_ == TASK_STATE_READY){                
-                if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-                rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-            }
-
-            ros::spinOnce();
-
-            if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_);
-
-            if(rubis::sched::task_state_ == TASK_STATE_DONE){
-                if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-                rubis::sched::task_state_ = TASK_STATE_READY;
-            }
-        
-            r.sleep();
-        }
+    ros::Rate r(rate);
+    while(ros::ok()){
+        rubis::start_task_profiling();
+
+        ros::spinOnce();
+
+        rubis::stop_task_profiling(rubis::instance_, rubis::lidar_instance_, rubis::vision_instance_);
+
+        r.sleep();
     }
 
     return 0;
diff --git a/rubis_ws/src/sensing_utils/CMakeLists.txt b/rubis_ws/src/sensing_utils/CMakeLists.txt
index b61899c1..e5b37975 100644
--- a/rubis_ws/src/sensing_utils/CMakeLists.txt
+++ b/rubis_ws/src/sensing_utils/CMakeLists.txt
@@ -4,36 +4,8 @@ project(sensing_utils)
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED)
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   geometry_msgs
@@ -41,69 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   inertiallabs_msgs
   tf
 )
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
 
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES sensing_utils
@@ -111,104 +21,10 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
 include_directories(
   include ${catkin_INCLUDE_DIRS}
-# include
-# ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/sensing_utils.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/sensing_utils_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_sensing_utils.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
-
 add_executable(odom_converter
   src/odom_converter.cpp
 )
@@ -239,4 +55,20 @@ add_executable(yaw_offset_calculator
 add_dependencies(yaw_offset_calculator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(yaw_offset_calculator
   ${catkin_LIBRARIES}
-)
\ No newline at end of file
+)
+
+install(
+  TARGETS
+    odom_converter
+    quaternion_to_rpy
+    ins_sync_test
+    yaw_offset_calculator
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/rubis_ws/src/sensing_utils/launch/velodyne_hdl32e.launch b/rubis_ws/src/sensing_utils/launch/velodyne_hdl32e.launch
new file mode 100644
index 00000000..ac68f576
--- /dev/null
+++ b/rubis_ws/src/sensing_utils/launch/velodyne_hdl32e.launch
@@ -0,0 +1,30 @@
+<!--
+    published topics:
+    /velodyne_points(sensor_msgs/PointCloud2)
+    /velodyne_packets(velodyne_msgs/VelodyneScan)
+-->
+<launch>
+  <!-- declare arguments with default values -->
+  <arg name="model" default="32E"/>
+  <arg name="pcap" default="" />
+  
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/32db.yaml"/>
+  <arg name="min_range" default="0.1"/>
+  <arg name="max_range" default="130.0"/>
+  <arg name="topic_name" default="points_raw"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <node pkg="nodelet" type="nodelet" name="velodyne_nodelet"
+        args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
+    <param name="calibration" value="$(arg calibration)"/>
+    <param name="min_range" value="$(arg min_range)" />
+    <param name="max_range" value="$(arg max_range)" />
+    <remap from="velodyne_points" to="$(arg topic_name)"/>
+  </node>
+</launch>
diff --git a/rubis_ws/src/sensing_utils/launch/velodyne_hdl64e_s2.launch b/rubis_ws/src/sensing_utils/launch/velodyne_hdl64e_s2.launch
new file mode 100644
index 00000000..869c445f
--- /dev/null
+++ b/rubis_ws/src/sensing_utils/launch/velodyne_hdl64e_s2.launch
@@ -0,0 +1,30 @@
+<!--
+    published topics:
+    /velodyne_points(sensor_msgs/PointCloud2)
+    /velodyne_packets(velodyne_msgs/VelodyneScan)
+-->
+<launch>
+  <!-- declare arguments with default values -->
+  <arg name="model" default="64E_S2"/>
+  <arg name="pcap" default="" />
+  
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/64e_s2.1-sztaki.yaml"/>
+  <arg name="min_range" default="2.0"/>
+  <arg name="max_range" default="250.0"/>
+  <arg name="topic_name" default="points_raw"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <node pkg="nodelet" type="nodelet" name="velodyne_nodelet"
+        args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
+    <param name="calibration" value="$(arg calibration)"/>
+    <param name="min_range" value="$(arg min_range)" />
+    <param name="max_range" value="$(arg max_range)" />
+    <remap from="velodyne_points" to="$(arg topic_name)"/>
+  </node>
+</launch>
diff --git a/rubis_ws/src/sensing_utils/launch/velodyne_hdl64e_s3.launch b/rubis_ws/src/sensing_utils/launch/velodyne_hdl64e_s3.launch
new file mode 100644
index 00000000..85a58e13
--- /dev/null
+++ b/rubis_ws/src/sensing_utils/launch/velodyne_hdl64e_s3.launch
@@ -0,0 +1,30 @@
+<!--
+    published topics:
+    /velodyne_points(sensor_msgs/PointCloud2)
+    /velodyne_packets(velodyne_msgs/VelodyneScan)
+-->
+<launch>
+  <!-- declare arguments with default values -->
+  <arg name="model" default="64E_S3"/>
+  <arg name="pcap" default="" />
+  
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/64e_s3-xiesc.yaml"/>
+  <arg name="min_range" default="2.0"/>
+  <arg name="max_range" default="250.0"/>
+  <arg name="topic_name" default="points_raw"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <node pkg="nodelet" type="nodelet" name="velodyne_nodelet"
+        args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
+    <param name="calibration" value="$(arg calibration)"/>
+    <param name="min_range" value="$(arg min_range)" />
+    <param name="max_range" value="$(arg max_range)" />
+    <remap from="/velodyne_points" to="$(arg topic_name)"/>
+  </node>
+</launch>
diff --git a/rubis_ws/src/sensing_utils/launch/velodyne_vlp16.launch b/rubis_ws/src/sensing_utils/launch/velodyne_vlp16.launch
new file mode 100644
index 00000000..0dde4004
--- /dev/null
+++ b/rubis_ws/src/sensing_utils/launch/velodyne_vlp16.launch
@@ -0,0 +1,33 @@
+<!-- -*- mode: XML -*- -->
+<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an VLP-16
+
+arg: calibration = path to calibration file (default: standard VLP16db.yaml)
+pcap = path to packet capture file (default: use real device)
+
+$Id$
+-->
+
+<launch>
+  <!-- declare arguments with default values -->
+  <arg name="pcap" default="" />
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
+  <arg name="min_range" default="0.4" />
+  <arg name="max_range" default="130.0" />
+  <arg name="model" default="VLP16"/>
+  <arg name="topic_name" default="points_raw"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <node pkg="nodelet" type="nodelet" name="cloud_nodelet"
+        args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
+    <param name="calibration" value="$(arg calibration)"/>
+    <param name="min_range" value="$(arg min_range)"/>
+    <param name="max_range" value="$(arg max_range)"/>
+    <remap from="velodyne_points" to="$(arg topic_name)"/>
+  </node>
+</launch>
diff --git a/rubis_ws/src/sensing_utils/launch/velodyne_vlp16_hires.launch b/rubis_ws/src/sensing_utils/launch/velodyne_vlp16_hires.launch
new file mode 100644
index 00000000..0b3681ba
--- /dev/null
+++ b/rubis_ws/src/sensing_utils/launch/velodyne_vlp16_hires.launch
@@ -0,0 +1,33 @@
+<!-- -*- mode: XML -*- -->
+<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an VLP-16 HiRes
+
+arg: calibration = path to calibration file (default: standard VLP16_hires_db.yaml)
+pcap = path to packet capture file (default: use real device)
+
+$Id$
+-->
+
+<launch>
+  <!-- declare arguments with default values -->
+  <arg name="pcap" default="" />
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16_hires_db.yaml"/>
+  <arg name="min_range" default="0.4" />
+  <arg name="max_range" default="130.0" />
+  <arg name="model" default="VLP16"/>
+  <arg name="topic_name" default="points_raw"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="$(arg model)"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <node pkg="nodelet" type="nodelet" name="cloud_nodelet"
+        args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
+    <param name="calibration" value="$(arg calibration)"/>
+    <param name="min_range" value="$(arg min_range)"/>
+    <param name="max_range" value="$(arg max_range)"/>
+    <remap from="velodyne_points" to="$(arg topic_name)"/>
+  </node>
+</launch>
diff --git a/rubis_ws/src/sensing_utils/launch/velodyne_vlp32c.launch b/rubis_ws/src/sensing_utils/launch/velodyne_vlp32c.launch
new file mode 100644
index 00000000..f65caa1a
--- /dev/null
+++ b/rubis_ws/src/sensing_utils/launch/velodyne_vlp32c.launch
@@ -0,0 +1,46 @@
+<!-- -*- mode: XML -*- -->
+<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an VLP-32C -->
+
+<launch>
+
+  <!-- declare arguments with default values -->
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/VeloView-VLP-32C.yaml"/>
+  <arg name="device_ip" default="" />
+  <arg name="frame_id" default="velodyne" />
+  <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
+  <arg name="max_range" default="130.0" />
+  <arg name="min_range" default="0.4" />
+  <arg name="pcap" default="" />
+  <arg name="port" default="2368" />
+  <arg name="read_fast" default="false" />
+  <arg name="read_once" default="false" />
+  <arg name="repeat_delay" default="0.0" />
+  <arg name="rpm" default="600.0" />
+  <arg name="laserscan_ring" default="-1" />
+  <arg name="laserscan_resolution" default="0.007" />
+  <arg name="topic_name" default="points_raw"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="device_ip" value="$(arg device_ip)"/>
+    <arg name="frame_id" value="$(arg frame_id)"/>
+    <arg name="manager" value="$(arg manager)" />
+    <arg name="model" value="32C"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+    <arg name="port" value="$(arg port)"/>
+    <arg name="read_fast" value="$(arg read_fast)"/>
+    <arg name="read_once" value="$(arg read_once)"/>
+    <arg name="repeat_delay" value="$(arg repeat_delay)"/>
+    <arg name="rpm" value="$(arg rpm)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <node pkg="nodelet" type="nodelet" name="cloud_nodelet"
+        args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
+    <param name="calibration" value="$(arg calibration)"/>
+    <param name="min_range" value="$(arg min_range)"/>
+    <param name="max_range" value="$(arg max_range)"/>
+    <remap from="velodyne_points" to="$(arg topic_name)"/>
+  </node>
+  
+</launch>
diff --git a/rubis_ws/src/sensing_utils/package.xml b/rubis_ws/src/sensing_utils/package.xml
index c49ce6b5..e438e78a 100644
--- a/rubis_ws/src/sensing_utils/package.xml
+++ b/rubis_ws/src/sensing_utils/package.xml
@@ -4,60 +4,15 @@
   <version>0.0.0</version>
   <description>The sensing_utils package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="hypark@todo.todo">hypark</maintainer>
 
+  <license>Apache 2</license>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/sensing_utils</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>tf</depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
+  <depend>inertiallabs_msgs</depend>
+  <depend>roscpp</depend>
 </package>
diff --git a/rubis_ws/src/svl_pkg/CMakeLists.txt b/rubis_ws/src/svl_pkg/CMakeLists.txt
new file mode 100644
index 00000000..36d6624a
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/CMakeLists.txt
@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(svl_pkg)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roscpp
+  jsk_recognition_msgs
+  autoware_msgs
+  geometry_msgs
+  nmea_msgs
+  std_msgs
+  tf
+  gnss
+  rubis_lib
+  rubis_msgs
+  visualization_msgs
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES rubis_pkg
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include ${catkin_INCLUDE_DIRS}
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(svl_sensing
+src/svl_sensing/svl_sensing_node.cpp
+  src/svl_sensing/svl_sensing.cpp
+)
+add_dependencies(svl_sensing ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(svl_sensing
+  ${catkin_LIBRARIES}
+)
+
+install(
+  TARGETS
+    svl_sensing
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/rubis_ws/src/svl_pkg/backup/include/svl_sensing.h b/rubis_ws/src/svl_pkg/backup/include/svl_sensing.h
new file mode 100644
index 00000000..621755af
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/backup/include/svl_sensing.h
@@ -0,0 +1,70 @@
+#ifndef SVL_SENSING_H
+#define SVL_SENSING_H
+
+#include <algorithm>
+#include <cstdlib>
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <rubis_lib/sched.hpp>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <rubis_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <rubis_msgs/Image.h>
+#include <nav_msgs/Odometry.h>
+#include <rubis_msgs/PoseTwistStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> SyncPolicyNoImage;
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, sensor_msgs::Image> SyncPolicyImage1;
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image> SyncPolicyImage2;
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> SyncPolicyImage3;
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> SyncPolicyImage4;
+typedef message_filters::Synchronizer<SyncPolicyNoImage> Sync0;
+typedef message_filters::Synchronizer<SyncPolicyImage1> Sync1;
+typedef message_filters::Synchronizer<SyncPolicyImage2> Sync2;
+typedef message_filters::Synchronizer<SyncPolicyImage3> Sync3;
+typedef message_filters::Synchronizer<SyncPolicyImage4> Sync4;
+
+class SvlSensing
+{
+public:
+    SvlSensing();
+    ~SvlSensing();
+    void run();
+private:
+    int* data_;
+    int rate_;
+    int data_size_, n_;
+    ros::NodeHandle nh_;
+       
+    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub_;
+    message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;    
+    message_filters::Subscriber<sensor_msgs::Image> image_sub_1_;
+    message_filters::Subscriber<sensor_msgs::Image> image_sub_2_;
+    message_filters::Subscriber<sensor_msgs::Image> image_sub_3_;
+    message_filters::Subscriber<sensor_msgs::Image> image_sub_4_;
+    boost::shared_ptr<Sync0> sync0_;
+    boost::shared_ptr<Sync1> sync1_;
+    boost::shared_ptr<Sync2> sync2_;
+    boost::shared_ptr<Sync3> sync3_;
+    boost::shared_ptr<Sync4> sync4_;
+
+    ros::Publisher lidar_pub_, pose_twist_pub_, image_pub1_, image_pub2_, image_pub3_, image_pub4_; 
+
+    void callback_no_image(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg, const nav_msgs::Odometry::ConstPtr& odom_msg);
+    void callback_image1(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg, const nav_msgs::Odometry::ConstPtr& odom_msg, const sensor_msgs::Image::ConstPtr& image_msg1);
+    void callback_image2(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg, const nav_msgs::Odometry::ConstPtr& odom_msg, const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2);
+    void callback_image3(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg, const nav_msgs::Odometry::ConstPtr& odom_msg, const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2, const sensor_msgs::Image::ConstPtr& image_msg3);
+    void callback_image4(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg, const nav_msgs::Odometry::ConstPtr& odom_msg, const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2, const sensor_msgs::Image::ConstPtr& image_msg3, const sensor_msgs::Image::ConstPtr& image_msg4);
+    void read_data(int n);
+    void read_sequential_data(int m);
+};
+
+#endif
\ No newline at end of file
diff --git a/rubis_ws/src/svl_pkg/backup/launch/svl_sensing.launch b/rubis_ws/src/svl_pkg/backup/launch/svl_sensing.launch
new file mode 100644
index 00000000..f8dff592
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/backup/launch/svl_sensing.launch
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="lidar_topic" default="/points_raw_origin" />
+  <arg name="odom_topic" default="/odom"/>
+  <arg name="image_topic_1" default="none" />
+  <arg name="image_topic_2" default="none" />
+  <arg name="image_topic_3" default="none" />
+  <arg name="image_topic_4" default="none" />
+  <arg name="pose_twist_topic" default="/rubis_current_pose_twist" />
+  <arg name="rate" default="5" />
+  <arg name="n" default="0" />
+
+  <node pkg="svl_pkg" type="svl_sensing" name="svl_sensing" output="screen">
+    <param name="lidar_topic" value="$(arg lidar_topic)" />
+    <param name="odom_topic" value="$(arg odom_topic)" />
+    <param name="image_topic_1" value="$(arg image_topic_1)" />
+    <param name="image_topic_2" value="$(arg image_topic_2)" />
+    <param name="image_topic_3" value="$(arg image_topic_3)" />
+    <param name="image_topic_4" value="$(arg image_topic_4)" />
+    <param name="rate" value="$(arg rate)" />
+    <param name="pose_twist_topic" value="$(arg pose_twist_topic)" /> 
+    <param name="n" value="$(arg n)" /> 
+  </node>
+
+</launch>
diff --git a/rubis_ws/src/svl_pkg/backup/src/svl_sensing/svl_sensing.cpp b/rubis_ws/src/svl_pkg/backup/src/svl_sensing/svl_sensing.cpp
new file mode 100644
index 00000000..fc44bfb0
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/backup/src/svl_sensing/svl_sensing.cpp
@@ -0,0 +1,468 @@
+#include "svl_sensing.h"
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <cstdlib>
+
+SvlSensing::SvlSensing() {
+    ros::NodeHandle private_nh("~");
+    std::string node_name = ros::this_node::getName();
+    std::string lidar_topic, odom_topic, image_topic_1, image_topic_2, image_topic_3, image_topic_4, pose_twist_topic;
+    int image_topic_cnt = 0;
+
+    private_nh.param<int>(node_name + "/rate", rate_, 5);
+    
+    private_nh.param<std::string>(node_name + "/pose_twist_topic", pose_twist_topic, "/rubis_current_pose_twist");
+
+    private_nh.param<std::string>(node_name + "/lidar_topic", lidar_topic, "/points_raw_origin");
+    lidar_sub_.subscribe(nh_, lidar_topic, 2);
+
+    private_nh.param<std::string>(node_name + "/odom_topic", odom_topic, "/odom");
+    odom_sub_.subscribe(nh_, odom_topic, 2);
+
+    private_nh.param<std::string>(node_name + "/image_topic_1", image_topic_1, "none");
+
+    if (image_topic_1.compare(std::string("none"))) {
+        image_sub_1_.subscribe(nh_, image_topic_1, 2);
+        image_topic_cnt++;
+    }
+
+    private_nh.param<std::string>(node_name + "/image_topic_2", image_topic_2, "none");
+    if (image_topic_2.compare(std::string("none"))) {
+        if (image_topic_cnt != 1) {
+            std::cout << "# [ERROR] Set image_topic_1 first" << std::endl;
+            exit(0);
+        }
+        image_sub_2_.subscribe(nh_, image_topic_2, 2);
+        image_topic_cnt++;
+    }
+
+    private_nh.param<std::string>(node_name + "/image_topic_3", image_topic_3, "none");
+    if (image_topic_3.compare(std::string("none"))) {
+        if (image_topic_cnt != 2) {
+            std::cout << "# [ERROR] Set image_topic_2 first" << std::endl;
+            exit(0);
+        }
+        image_sub_3_.subscribe(nh_, image_topic_3, 2);
+        image_topic_cnt++;
+    }
+
+    private_nh.param<std::string>(node_name + "/image_topic_4", image_topic_4, "none");
+    if (image_topic_4.compare(std::string("none"))) {
+        if (image_topic_cnt != 3) {
+            std::cout << "# [ERROR] Set image_topic_3 first" << std::endl;
+            exit(0);
+        }
+        image_sub_4_.subscribe(nh_, image_topic_4, 2);
+        image_topic_cnt++;
+    }
+
+    if (image_topic_cnt == 0) {
+        sync0_.reset(new message_filters::Synchronizer<SyncPolicyNoImage>(SyncPolicyNoImage(1), lidar_sub_, odom_sub_));
+        sync0_->registerCallback(boost::bind(&SvlSensing::callback_no_image, this, _1, _2));
+    } else if (image_topic_cnt == 1) {
+        sync1_.reset(new message_filters::Synchronizer<SyncPolicyImage1>(SyncPolicyImage1(1), lidar_sub_, odom_sub_, image_sub_1_));
+        sync1_->registerCallback(boost::bind(&SvlSensing::callback_image1, this, _1, _2, _3));
+        image_pub1_ = nh_.advertise<rubis_msgs::Image>("/image_raw1", 1);
+    } else if (image_topic_cnt == 2) {
+        sync2_.reset(new message_filters::Synchronizer<SyncPolicyImage2>(SyncPolicyImage2(1), lidar_sub_, odom_sub_, image_sub_1_, image_sub_2_));
+        sync2_->registerCallback(boost::bind(&SvlSensing::callback_image2, this, _1, _2, _3, _4));
+        image_pub1_ = nh_.advertise<rubis_msgs::Image>("/image_raw1", 1);
+        image_pub2_ = nh_.advertise<rubis_msgs::Image>("/image_raw2", 1);
+    } else if (image_topic_cnt == 3) {
+        sync3_.reset(new message_filters::Synchronizer<SyncPolicyImage3>(SyncPolicyImage3(1), lidar_sub_, odom_sub_, image_sub_1_, image_sub_2_,
+                                                                         image_sub_3_));
+        sync3_->registerCallback(boost::bind(&SvlSensing::callback_image3, this, _1, _2, _3, _4, _5));
+        image_pub1_ = nh_.advertise<rubis_msgs::Image>("/image_raw1", 1);
+        image_pub2_ = nh_.advertise<rubis_msgs::Image>("/image_raw2", 1);
+        image_pub3_ = nh_.advertise<rubis_msgs::Image>("/image_raw3", 1);
+    } else if (image_topic_cnt == 4) {
+        sync4_.reset(new message_filters::Synchronizer<SyncPolicyImage4>(SyncPolicyImage4(1), lidar_sub_, odom_sub_, image_sub_1_, image_sub_2_,
+                                                                         image_sub_3_, image_sub_4_));
+        sync4_->registerCallback(boost::bind(&SvlSensing::callback_image4, this, _1, _2, _3, _4, _5, _6));
+        image_pub1_ = nh_.advertise<rubis_msgs::Image>("/image_raw1", 1);
+        image_pub2_ = nh_.advertise<rubis_msgs::Image>("/image_raw2", 1);
+        image_pub3_ = nh_.advertise<rubis_msgs::Image>("/image_raw3", 1);
+        image_pub4_ = nh_.advertise<rubis_msgs::Image>("/image_raw4", 1);
+    }
+
+    lidar_pub_ = nh_.advertise<rubis_msgs::PointCloud2>("/rubis_points_raw", 1);
+    pose_twist_pub_ = nh_.advertise<rubis_msgs::PoseTwistStamped>(pose_twist_topic, 1);
+
+    std::string task_response_time_filename;
+    nh_.param<std::string>(node_name + "/task_response_time_filename", task_response_time_filename,
+                           "~/Documents/profiling/response_time/svl_sensing.csv");
+
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time, deadline, period;
+
+    nh_.param(node_name + "/task_scheduling_configs/policy", policy, std::string("NONE"));
+    nh_.param(node_name + "/task_scheduling_configs/priority", priority, 99);
+    nh_.param(node_name + "/task_scheduling_configs/exec_time", exec_time, 0);
+    nh_.param(node_name + "/task_scheduling_configs/deadline", deadline, 0);
+    nh_.param(node_name + "/task_scheduling_configs/period", period, 0);
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);
+    rubis::init_task_scheduling(policy, attr);
+    rubis::init_task_profiling(task_response_time_filename);
+
+
+    nh_.param(node_name + "/n", n_, 10);
+    data_size_ = data_size_mb*1024*1024;
+    data_ = (int *)aligned_alloc(32, data_size_);
+    memset(data_, 0, data_size_);
+}
+
+SvlSensing::~SvlSensing() {}
+
+void SvlSensing::read_data(int n){    
+    int stride_size = 64;
+    int stride = 64 / sizeof(int);
+    int data_num = data_size_ / sizeof(int);    
+    int index_stride = (int)(data_num / 16);
+
+    int index = 0;
+    int limit = index_stride;
+    
+    std::cout<<"n: "<<n<<std::endl;
+    for(int j = 0; j < n; j++){
+        for(int i = 0; i <16; i++){
+            for(; index < limit; index += stride){
+                data_[index] = data_[index] + 1;
+            }
+            limit += index_stride;
+        }
+    }
+
+    return;
+}
+
+void SvlSensing::read_sequential_data(int m){    
+    int stride_size = 64;
+    int stride = 64 / sizeof(int);
+    int data_num = data_size_ / sizeof(int);    
+    int index_stride = (int)(data_num / 16);
+
+    int index = 0;
+    int limit = index_stride;
+    
+    for(int i = 0; i <16; i++){
+        for(; index < limit; index += 1){
+            data_[index] = data_[index] + 1;
+        }
+        limit += index_stride;
+    }
+
+    return;
+}
+
+
+void SvlSensing::callback_no_image(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg, const nav_msgs::Odometry::ConstPtr &odom_msg) {
+    rubis::start_task_profiling_at_initial_node(std::max(lidar_msg->header.stamp.sec, odom_msg->header.stamp.sec),
+                                                std::max(lidar_msg->header.stamp.nsec, odom_msg->header.stamp.nsec));
+
+    auto cur_time = ros::Time::now();
+    rubis_msgs::PointCloud2 out_lidar_msg;
+    out_lidar_msg.header = lidar_msg->header;
+    out_lidar_msg.header.stamp = cur_time;
+    out_lidar_msg.instance = rubis::instance_;
+    out_lidar_msg.msg = *lidar_msg;
+    out_lidar_msg.msg.header.stamp = cur_time;
+    out_lidar_msg.msg.fields.at(3).datatype = 7;
+
+    rubis_msgs::PoseTwistStamped out_pose_twist_msg;
+    out_pose_twist_msg.header = odom_msg->header;
+    out_pose_twist_msg.header.stamp = cur_time;
+    out_pose_twist_msg.instance = rubis::instance_;
+    out_pose_twist_msg.pose.header = odom_msg->header;
+    out_pose_twist_msg.pose.header.stamp = cur_time;
+    out_pose_twist_msg.pose.header.frame_id = "/map";
+    out_pose_twist_msg.pose.pose = odom_msg->pose.pose;
+    out_pose_twist_msg.twist.header.frame_id = "/map";
+    out_pose_twist_msg.twist.header.stamp = cur_time;
+    out_pose_twist_msg.twist.twist = odom_msg->twist.twist;
+
+    read_data(n_);
+
+    lidar_pub_.publish(out_lidar_msg);
+    pose_twist_pub_.publish(out_pose_twist_msg);
+
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion gnss_q(out_pose_twist_msg.pose.pose.orientation.x, out_pose_twist_msg.pose.pose.orientation.y, out_pose_twist_msg.pose.pose.orientation.z,
+                          out_pose_twist_msg.pose.pose.orientation.w);
+    transform.setOrigin(tf::Vector3(out_pose_twist_msg.pose.pose.position.x, out_pose_twist_msg.pose.pose.position.y, out_pose_twist_msg.pose.pose.position.z));
+    transform.setRotation(gnss_q);
+    
+    br.sendTransform(tf::StampedTransform(transform, cur_time, "/map", "/base_link"));
+
+
+    rubis::stop_task_profiling(rubis::instance_++, rubis::lidar_instance_++, rubis::vision_instance_++);
+    return;
+}
+
+void SvlSensing::callback_image1(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg, const nav_msgs::Odometry::ConstPtr &odom_msg,
+                                 const sensor_msgs::Image::ConstPtr &image_msg1) {
+    rubis::start_task_profiling_at_initial_node(std::max(lidar_msg->header.stamp.sec, odom_msg->header.stamp.sec),
+                                                std::max(lidar_msg->header.stamp.nsec, odom_msg->header.stamp.nsec));
+
+    auto cur_time = ros::Time::now();
+    rubis_msgs::PointCloud2 out_lidar_msg;
+    out_lidar_msg.header = lidar_msg->header;
+    out_lidar_msg.header.stamp = cur_time;
+    out_lidar_msg.instance = rubis::instance_;
+    out_lidar_msg.msg = *lidar_msg;
+    out_lidar_msg.msg.header.stamp = cur_time;
+    out_lidar_msg.msg.fields.at(3).datatype = 7;
+
+    rubis_msgs::PoseTwistStamped out_pose_twist_msg;
+    out_pose_twist_msg.header = odom_msg->header;
+    out_pose_twist_msg.header.stamp = cur_time;
+    out_pose_twist_msg.instance = rubis::instance_;
+    out_pose_twist_msg.pose.header = odom_msg->header;
+    out_pose_twist_msg.pose.header.stamp = cur_time;
+    out_pose_twist_msg.pose.header.frame_id = "/map";
+    out_pose_twist_msg.pose.pose = odom_msg->pose.pose;
+    out_pose_twist_msg.twist.header.frame_id = "/map";
+    out_pose_twist_msg.twist.header.stamp = cur_time;
+    out_pose_twist_msg.twist.twist = odom_msg->twist.twist;
+
+    rubis_msgs::Image out_image_msg1;
+    out_image_msg1.header = odom_msg->header;
+    out_image_msg1.header.stamp = cur_time;
+    out_image_msg1.instance = rubis::instance_;
+    out_image_msg1.msg = *image_msg1;
+
+    // read_data(n_);
+
+    lidar_pub_.publish(out_lidar_msg);
+    pose_twist_pub_.publish(out_pose_twist_msg);
+    image_pub1_.publish(out_image_msg1);
+
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion gnss_q(out_pose_twist_msg.pose.pose.orientation.x, out_pose_twist_msg.pose.pose.orientation.y, out_pose_twist_msg.pose.pose.orientation.z,
+                          out_pose_twist_msg.pose.pose.orientation.w);
+    transform.setOrigin(tf::Vector3(out_pose_twist_msg.pose.pose.position.x, out_pose_twist_msg.pose.pose.position.y, out_pose_twist_msg.pose.pose.position.z));
+    transform.setRotation(gnss_q);
+    
+    br.sendTransform(tf::StampedTransform(transform, cur_time, "/map", "/base_link"));
+
+
+    rubis::stop_task_profiling(rubis::instance_++, rubis::lidar_instance_++, rubis::vision_instance_++);
+    return;
+}
+
+void SvlSensing::callback_image2(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg, const nav_msgs::Odometry::ConstPtr &odom_msg,
+                                 const sensor_msgs::Image::ConstPtr &image_msg1, const sensor_msgs::Image::ConstPtr &image_msg2) {
+    rubis::start_task_profiling_at_initial_node(std::max(lidar_msg->header.stamp.sec, odom_msg->header.stamp.sec),
+                                                std::max(lidar_msg->header.stamp.nsec, odom_msg->header.stamp.nsec));
+
+    auto cur_time = ros::Time::now();
+    rubis_msgs::PointCloud2 out_lidar_msg;
+    out_lidar_msg.header = lidar_msg->header;
+    out_lidar_msg.header.stamp = cur_time;
+    out_lidar_msg.instance = rubis::instance_;
+    out_lidar_msg.msg = *lidar_msg;
+    out_lidar_msg.msg.header.stamp = cur_time;
+    out_lidar_msg.msg.fields.at(3).datatype = 7;
+
+    rubis_msgs::PoseTwistStamped out_pose_twist_msg;
+    out_pose_twist_msg.header = odom_msg->header;
+    out_pose_twist_msg.header.stamp = cur_time;
+    out_pose_twist_msg.instance = rubis::instance_;
+    out_pose_twist_msg.pose.header = odom_msg->header;
+    out_pose_twist_msg.pose.header.stamp = cur_time;
+    out_pose_twist_msg.pose.header.frame_id = "/map";
+    out_pose_twist_msg.pose.pose = odom_msg->pose.pose;
+    out_pose_twist_msg.twist.header.frame_id = "/map";
+    out_pose_twist_msg.twist.header.stamp = cur_time;
+    out_pose_twist_msg.twist.twist = odom_msg->twist.twist;
+
+    rubis_msgs::Image out_image_msg1;
+    out_image_msg1.header = odom_msg->header;
+    out_image_msg1.header.stamp = cur_time;
+    out_image_msg1.instance = rubis::instance_;
+    out_image_msg1.msg = *image_msg1;
+
+    rubis_msgs::Image out_image_msg2;
+    out_image_msg2.header = odom_msg->header;
+    out_image_msg2.header.stamp = cur_time;
+    out_image_msg2.instance = rubis::instance_;
+    out_image_msg2.msg = *image_msg2;
+
+    read_data(n_);
+
+    lidar_pub_.publish(out_lidar_msg);
+    pose_twist_pub_.publish(out_pose_twist_msg);
+    image_pub1_.publish(out_image_msg1);
+    image_pub2_.publish(out_image_msg2);
+
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion gnss_q(out_pose_twist_msg.pose.pose.orientation.x, out_pose_twist_msg.pose.pose.orientation.y, out_pose_twist_msg.pose.pose.orientation.z,
+                          out_pose_twist_msg.pose.pose.orientation.w);
+    transform.setOrigin(tf::Vector3(out_pose_twist_msg.pose.pose.position.x, out_pose_twist_msg.pose.pose.position.y, out_pose_twist_msg.pose.pose.position.z));
+    transform.setRotation(gnss_q);
+    
+    br.sendTransform(tf::StampedTransform(transform, cur_time, "/map", "/base_link"));
+
+
+    rubis::stop_task_profiling(rubis::instance_++, rubis::lidar_instance_++, rubis::vision_instance_++);
+    return;
+}
+
+void SvlSensing::callback_image3(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg, const nav_msgs::Odometry::ConstPtr &odom_msg,
+                                 const sensor_msgs::Image::ConstPtr &image_msg1, const sensor_msgs::Image::ConstPtr &image_msg2,
+                                 const sensor_msgs::Image::ConstPtr &image_msg3) {
+    rubis::start_task_profiling_at_initial_node(std::max(lidar_msg->header.stamp.sec, odom_msg->header.stamp.sec),
+                                                std::max(lidar_msg->header.stamp.nsec, odom_msg->header.stamp.nsec));
+
+    auto cur_time = ros::Time::now();
+    rubis_msgs::PointCloud2 out_lidar_msg;
+    out_lidar_msg.header = lidar_msg->header;
+    out_lidar_msg.header.stamp = cur_time;
+    out_lidar_msg.instance = rubis::instance_;
+    out_lidar_msg.msg = *lidar_msg;
+    out_lidar_msg.msg.header.stamp = cur_time;
+    out_lidar_msg.msg.fields.at(3).datatype = 7;
+
+    rubis_msgs::PoseTwistStamped out_pose_twist_msg;
+    out_pose_twist_msg.header = odom_msg->header;
+    out_pose_twist_msg.header.stamp = cur_time;
+    out_pose_twist_msg.instance = rubis::instance_;
+    out_pose_twist_msg.pose.header = odom_msg->header;
+    out_pose_twist_msg.pose.header.stamp = cur_time;
+    out_pose_twist_msg.pose.header.frame_id = "/map";
+    out_pose_twist_msg.pose.pose = odom_msg->pose.pose;
+    out_pose_twist_msg.twist.header.frame_id = "/map";
+    out_pose_twist_msg.twist.header.stamp = cur_time;
+    out_pose_twist_msg.twist.twist = odom_msg->twist.twist;
+
+    rubis_msgs::Image out_image_msg1;
+    out_image_msg1.header = odom_msg->header;
+    out_image_msg1.header.stamp = cur_time;
+    out_image_msg1.instance = rubis::instance_;
+    out_image_msg1.msg = *image_msg1;
+
+    rubis_msgs::Image out_image_msg2;
+    out_image_msg2.header = odom_msg->header;
+    out_image_msg2.header.stamp = cur_time;
+    out_image_msg2.instance = rubis::instance_;
+    out_image_msg2.msg = *image_msg2;
+
+    rubis_msgs::Image out_image_msg3;
+    out_image_msg3.header = odom_msg->header;
+    out_image_msg3.header.stamp = cur_time;
+    out_image_msg3.instance = rubis::instance_;
+    out_image_msg3.msg = *image_msg3;
+
+    read_data(n_);
+
+    lidar_pub_.publish(out_lidar_msg);
+    pose_twist_pub_.publish(out_pose_twist_msg);
+    image_pub1_.publish(out_image_msg1);
+    image_pub2_.publish(out_image_msg2);
+    image_pub3_.publish(out_image_msg3);
+
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion gnss_q(out_pose_twist_msg.pose.pose.orientation.x, out_pose_twist_msg.pose.pose.orientation.y, out_pose_twist_msg.pose.pose.orientation.z,
+                          out_pose_twist_msg.pose.pose.orientation.w);
+    transform.setOrigin(tf::Vector3(out_pose_twist_msg.pose.pose.position.x, out_pose_twist_msg.pose.pose.position.y, out_pose_twist_msg.pose.pose.position.z));
+    transform.setRotation(gnss_q);
+    
+    br.sendTransform(tf::StampedTransform(transform, cur_time, "/map", "/base_link"));
+
+
+    rubis::stop_task_profiling(rubis::instance_++, rubis::lidar_instance_++, rubis::vision_instance_++);
+    return;
+}
+
+void SvlSensing::callback_image4(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg, const nav_msgs::Odometry::ConstPtr &odom_msg,
+                                 const sensor_msgs::Image::ConstPtr &image_msg1, const sensor_msgs::Image::ConstPtr &image_msg2,
+                                 const sensor_msgs::Image::ConstPtr &image_msg3, const sensor_msgs::Image::ConstPtr &image_msg4) {
+    rubis::start_task_profiling_at_initial_node(std::max(lidar_msg->header.stamp.sec, odom_msg->header.stamp.sec),
+                                                std::max(lidar_msg->header.stamp.nsec, odom_msg->header.stamp.nsec));
+
+    auto cur_time = ros::Time::now();
+    rubis_msgs::PointCloud2 out_lidar_msg;
+    out_lidar_msg.header = lidar_msg->header;
+    out_lidar_msg.header.stamp = cur_time;
+    out_lidar_msg.instance = rubis::instance_;
+    out_lidar_msg.msg = *lidar_msg;
+    out_lidar_msg.msg.header.stamp = cur_time;
+    out_lidar_msg.msg.fields.at(3).datatype = 7;
+
+    rubis_msgs::PoseTwistStamped out_pose_twist_msg;
+    out_pose_twist_msg.header = odom_msg->header;
+    out_pose_twist_msg.header.stamp = cur_time;
+    out_pose_twist_msg.instance = rubis::instance_;
+    out_pose_twist_msg.pose.header = odom_msg->header;
+    out_pose_twist_msg.pose.header.stamp = cur_time;
+    out_pose_twist_msg.pose.header.frame_id = "/map";
+    out_pose_twist_msg.pose.pose = odom_msg->pose.pose;
+    out_pose_twist_msg.twist.header.frame_id = "/map";
+    out_pose_twist_msg.twist.header.stamp = cur_time;
+    out_pose_twist_msg.twist.twist = odom_msg->twist.twist;
+
+    rubis_msgs::Image out_image_msg1;
+    out_image_msg1.header = odom_msg->header;
+    out_image_msg1.header.stamp = cur_time;
+    out_image_msg1.instance = rubis::instance_;
+    out_image_msg1.msg = *image_msg1;
+
+    rubis_msgs::Image out_image_msg2;
+    out_image_msg2.header = odom_msg->header;
+    out_image_msg2.header.stamp = cur_time;
+    out_image_msg2.instance = rubis::instance_;
+    out_image_msg2.msg = *image_msg2;
+
+    rubis_msgs::Image out_image_msg3;
+    out_image_msg3.header = odom_msg->header;
+    out_image_msg3.header.stamp = cur_time;
+    out_image_msg3.instance = rubis::instance_;
+    out_image_msg3.msg = *image_msg3;
+
+    rubis_msgs::Image out_image_msg4;
+    out_image_msg4.header = odom_msg->header;
+    out_image_msg4.header.stamp = cur_time;
+    out_image_msg4.instance = rubis::instance_;
+    out_image_msg4.msg = *image_msg4;
+
+    read_data(n_);
+
+    lidar_pub_.publish(out_lidar_msg);
+    pose_twist_pub_.publish(out_pose_twist_msg);
+    image_pub1_.publish(out_image_msg1);
+    image_pub2_.publish(out_image_msg2);
+    image_pub3_.publish(out_image_msg3);
+    image_pub4_.publish(out_image_msg4);
+
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion gnss_q(out_pose_twist_msg.pose.pose.orientation.x, out_pose_twist_msg.pose.pose.orientation.y, out_pose_twist_msg.pose.pose.orientation.z,
+                          out_pose_twist_msg.pose.pose.orientation.w);
+    transform.setOrigin(tf::Vector3(out_pose_twist_msg.pose.pose.position.x, out_pose_twist_msg.pose.pose.position.y, out_pose_twist_msg.pose.pose.position.z));
+    transform.setRotation(gnss_q);
+    
+    br.sendTransform(tf::StampedTransform(transform, cur_time, "/map", "/base_link"));
+
+    rubis::stop_task_profiling(rubis::instance_++, rubis::lidar_instance_++, rubis::vision_instance_++);
+    return;
+}
+
+void SvlSensing::run() {
+    if(rate_ < 0)
+        ros::spin();
+    else{
+    
+    ros::Rate r(rate_);
+    while (ros::ok()) {
+        ros::spinOnce();
+        r.sleep();
+    }
+
+    }
+}
\ No newline at end of file
diff --git a/rubis_ws/src/svl_pkg/backup/src/svl_sensing/svl_sensing_node.cpp b/rubis_ws/src/svl_pkg/backup/src/svl_sensing/svl_sensing_node.cpp
new file mode 100644
index 00000000..3e7631cf
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/backup/src/svl_sensing/svl_sensing_node.cpp
@@ -0,0 +1,12 @@
+#include <ros/ros.h>
+
+#include "svl_sensing.h"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "svl_sensing");  
+  SvlSensing svl_sensing;
+  svl_sensing.run();
+
+  return 0;
+}
diff --git a/rubis_ws/src/svl_pkg/include/svl_sensing.h b/rubis_ws/src/svl_pkg/include/svl_sensing.h
new file mode 100644
index 00000000..a7470474
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/include/svl_sensing.h
@@ -0,0 +1,58 @@
+#ifndef SVL_SENSING_H
+#define SVL_SENSING_H
+
+#include <algorithm>
+#include <cstdlib>
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <rubis_lib/sched.hpp>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <rubis_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <rubis_msgs/Image.h>
+#include <nav_msgs/Odometry.h>
+#include <rubis_msgs/PoseTwistStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+
+class SvlSensing
+{
+public:
+    SvlSensing();
+    ~SvlSensing();
+    void run();
+private:
+    int* data_;
+    int rate_;
+    int data_size_, n_, last_publish_id_, id_;
+    ros::NodeHandle nh_;
+
+
+    nav_msgs::Odometry::ConstPtr latest_odom_msg_;
+    sensor_msgs::Image::ConstPtr latest_image_msg1_;
+    sensor_msgs::Image::ConstPtr latest_image_msg2_;
+    sensor_msgs::Image::ConstPtr latest_image_msg3_;
+    sensor_msgs::Image::ConstPtr latest_image_msg4_;
+    sensor_msgs::PointCloud2::ConstPtr latest_lidar_msg_;
+ 
+    void callbackOdom(const nav_msgs::Odometry::ConstPtr &odom_msg);
+    void callbackImage1(const sensor_msgs::Image::ConstPtr &image_msg1);
+    void callbackImage2(const sensor_msgs::Image::ConstPtr &image_msg2);
+    void callbackImage3(const sensor_msgs::Image::ConstPtr &image_msg3);
+    void callbackImage4(const sensor_msgs::Image::ConstPtr &image_msg4);
+    void callbackLidar(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg);
+    ros::Publisher lidar_pub_, pose_twist_pub_, image_pub1_, image_pub2_, image_pub3_, image_pub4_; 
+    ros::Subscriber lidar_sub_, odom_sub_, image_sub_1_, image_sub_2_, image_sub_3_, image_sub_4_; 
+
+    
+    void read_data(int n);
+    void read_sequential_data(int m);
+};
+
+#endif
\ No newline at end of file
diff --git a/rubis_ws/src/svl_pkg/launch/svl_sensing.launch b/rubis_ws/src/svl_pkg/launch/svl_sensing.launch
new file mode 100644
index 00000000..af216213
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/launch/svl_sensing.launch
@@ -0,0 +1,26 @@
+<launch>
+  <arg name="lidar_topic" default="/points_raw_origin" />
+  <arg name="odom_topic" default="/odom"/>
+  <arg name="image_topic_1" default="none" />
+  <arg name="image_topic_2" default="none" />
+  <arg name="image_topic_3" default="none" />
+  <arg name="image_topic_4" default="none" />
+  <arg name="pose_twist_topic" default="/rubis_current_pose_twist" />
+  <arg name="rate" default="5" />
+  <arg name="data_size_mb" default="32" />
+  <arg name="n" default="50" />
+
+  <node pkg="svl_pkg" type="svl_sensing" name="svl_sensing" output="screen">
+    <param name="lidar_topic" value="$(arg lidar_topic)" />
+    <param name="odom_topic" value="$(arg odom_topic)" />
+    <param name="image_topic_1" value="$(arg image_topic_1)" />
+    <param name="image_topic_2" value="$(arg image_topic_2)" />
+    <param name="image_topic_3" value="$(arg image_topic_3)" />
+    <param name="image_topic_4" value="$(arg image_topic_4)" />
+    <param name="rate" value="$(arg rate)" />
+    <param name="pose_twist_topic" value="$(arg pose_twist_topic)" /> 
+    <param name="data_size_mb" value="$(arg data_size_mb)" /> 
+    <param name="n" value="$(arg n)" /> 
+  </node>
+
+</launch>
diff --git a/rubis_ws/src/svl_pkg/package.xml b/rubis_ws/src/svl_pkg/package.xml
new file mode 100644
index 00000000..22b11781
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/package.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>svl_pkg</name>
+  <version>0.0.0</version>
+  <description>Package for SVL simulation</description>
+  
+  <maintainer email="kite9240@gmail.com">Hayeon Park</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <export>
+  </export>
+<build_depend>rospy</build_depend>
+<build_depend>geometry_msgs</build_depend>
+<build_depend>autoware_msgs</build_depend>
+<build_depend>std_msgs</build_depend>
+<build_depend>jsk_recognition_msgs</build_depend>
+<build_depend>rubis_msgs</build_depend>
+<build_export_depend>rospy</build_export_depend>
+<build_export_depend>geometry_msgs</build_export_depend>
+<build_export_depend>autoware_msgs</build_export_depend>
+<build_export_depend>std_msgs</build_export_depend>
+<build_export_depend>jsk_recognition_msgs</build_export_depend>
+<build_export_depend>rubis_msgs</build_export_depend>
+<exec_depend>rospy</exec_depend>
+<exec_depend>geometry_msgs</exec_depend>
+<exec_depend>nmea_msgs</exec_depend>
+<exec_depend>autoware_msgs</exec_depend>
+<exec_depend>std_msgs</exec_depend>
+<exec_depend>jsk_recognition_msgs</exec_depend>
+<exec_depend>rubis_msgs</exec_depend>
+<depend>roscpp</depend>
+<depend>tf</depend>
+<depend>gnss</depend>
+<depend>rubis_lib</depend>
+<depend>visualization_msgs</depend>
+<depend>nav_msgs</depend>
+
+</package>
\ No newline at end of file
diff --git a/rubis_ws/src/svl_pkg/src/svl_sensing/svl_sensing.cpp b/rubis_ws/src/svl_pkg/src/svl_sensing/svl_sensing.cpp
new file mode 100644
index 00000000..be719f06
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/src/svl_sensing/svl_sensing.cpp
@@ -0,0 +1,293 @@
+#include "svl_sensing.h"
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <cstdlib>
+#include <mutex>
+#include <atomic>
+
+// Constructor
+SvlSensing::SvlSensing() : id_(0), last_publish_id_(0) {
+    ros::NodeHandle private_nh("~");
+    std::string node_name = ros::this_node::getName();
+    std::string lidar_topic, odom_topic, image_topic_1, image_topic_2, image_topic_3, image_topic_4, pose_twist_topic;
+    int image_topic_cnt = 0;
+    int data_size_mb = 0;
+
+    // Parameters
+    private_nh.param<int>(node_name + "/rate", rate_, 5);
+    private_nh.param<std::string>(node_name + "/pose_twist_topic", pose_twist_topic, "/rubis_current_pose_twist");
+    private_nh.param<std::string>(node_name + "/lidar_topic", lidar_topic, "/points_raw_origin");
+    private_nh.param<std::string>(node_name + "/odom_topic", odom_topic, "/odom");
+    private_nh.param<std::string>(node_name + "/image_topic_1", image_topic_1, "none");
+    private_nh.param<std::string>(node_name + "/image_topic_2", image_topic_2, "none");
+    private_nh.param<std::string>(node_name + "/image_topic_3", image_topic_3, "none");
+    private_nh.param<std::string>(node_name + "/image_topic_4", image_topic_4, "none");
+    private_nh.param<int>(node_name + "/data_size_mb", data_size_mb, 64);
+
+    // Subscribers
+    lidar_sub_ = nh_.subscribe(lidar_topic, 2, &SvlSensing::callbackLidar, this);
+    odom_sub_ = nh_.subscribe(odom_topic, 10, &SvlSensing::callbackOdom, this);
+
+    if (image_topic_1 != "none") {
+        image_sub_1_ = nh_.subscribe(image_topic_1, 10, &SvlSensing::callbackImage1, this);
+        image_topic_cnt++;
+    }
+
+    if (image_topic_2 != "none") {
+        if (image_topic_cnt != 1) {
+            ROS_ERROR("# [ERROR] Set image_topic_1 first");
+            exit(EXIT_FAILURE);
+        }
+        image_sub_2_ = nh_.subscribe(image_topic_2, 10, &SvlSensing::callbackImage2, this);
+        image_topic_cnt++;
+    }
+
+    if (image_topic_3 != "none") {
+        if (image_topic_cnt != 2) {
+            ROS_ERROR("# [ERROR] Set image_topic_2 first");
+            exit(EXIT_FAILURE);
+        }
+        image_sub_3_ = nh_.subscribe(image_topic_3, 10, &SvlSensing::callbackImage3, this);
+        image_topic_cnt++;
+    }
+
+    if (image_topic_4 != "none") {
+        if (image_topic_cnt != 3) {
+            ROS_ERROR("# [ERROR] Set image_topic_3 first");
+            exit(EXIT_FAILURE);
+        }
+        image_sub_4_ = nh_.subscribe(image_topic_4, 10, &SvlSensing::callbackImage4, this);
+        image_topic_cnt++;
+    }
+
+    // Publishers
+    lidar_pub_ = nh_.advertise<rubis_msgs::PointCloud2>("/rubis_points_raw", 1);
+    pose_twist_pub_ = nh_.advertise<rubis_msgs::PoseTwistStamped>(pose_twist_topic, 1);
+
+    if (image_topic_cnt >= 1) {
+        image_pub1_ = nh_.advertise<rubis_msgs::Image>("/image_raw1", 1);
+    }
+    if (image_topic_cnt >= 2) {
+        image_pub2_ = nh_.advertise<rubis_msgs::Image>("/image_raw2", 1);
+    }
+    if (image_topic_cnt >= 3) {
+        image_pub3_ = nh_.advertise<rubis_msgs::Image>("/image_raw3", 1);
+    }
+    if (image_topic_cnt >= 4) {
+        image_pub4_ = nh_.advertise<rubis_msgs::Image>("/image_raw4", 1);
+    }
+
+    // Task Profiling Configuration
+    std::string task_response_time_filename;
+    nh_.param<std::string>(node_name + "/task_response_time_filename", task_response_time_filename,
+                           "~/Documents/profiling/response_time/svl_sensing.csv");
+
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time, deadline, period;
+
+    nh_.param(node_name + "/task_scheduling_configs/policy", policy, std::string("NONE"));
+    nh_.param(node_name + "/task_scheduling_configs/priority", priority, 99);
+    nh_.param(node_name + "/task_scheduling_configs/exec_time", exec_time, 0);
+    nh_.param(node_name + "/task_scheduling_configs/deadline", deadline, 0);
+    nh_.param(node_name + "/task_scheduling_configs/period", period, 0);
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);
+    rubis::init_task_scheduling(policy, attr);
+    rubis::init_task_profiling(task_response_time_filename);
+
+    // Data Initialization
+    nh_.param(node_name + "/n", n_, 10);
+    data_size_ = data_size_mb*1024*1024;
+    data_ = (int *)aligned_alloc(32, data_size_);
+    memset(data_, 0, data_size_);
+}
+
+// Destructor
+SvlSensing::~SvlSensing() {
+    if (data_) {
+        free(data_);
+    }
+}
+
+// Non-LiDAR Callbacks: Update data and increment id_
+void SvlSensing::callbackOdom(const nav_msgs::Odometry::ConstPtr &odom_msg) {
+    latest_odom_msg_ = odom_msg;
+}
+
+void SvlSensing::callbackImage1(const sensor_msgs::Image::ConstPtr &image_msg1) {
+    latest_image_msg1_ = image_msg1;
+}
+
+void SvlSensing::callbackImage2(const sensor_msgs::Image::ConstPtr &image_msg2) {
+    latest_image_msg2_ = image_msg2;
+}
+
+void SvlSensing::callbackImage3(const sensor_msgs::Image::ConstPtr &image_msg3) {
+    latest_image_msg3_ = image_msg3;
+    id_++;
+}
+
+void SvlSensing::callbackImage4(const sensor_msgs::Image::ConstPtr &image_msg4) {
+    latest_image_msg4_ = image_msg4;
+}
+
+// LiDAR Callback: Check for updates and publish if necessary
+void SvlSensing::callbackLidar(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg) {
+
+    // Check if id_ has changed since last publish
+    if (id_ == last_publish_id_) {
+        // No new data, skip publishing
+        return;
+    }
+
+    // Ensure that odom message is available
+    if (!latest_odom_msg_) {
+        ROS_WARN("Odom message not received yet. Skipping publish.");
+        return;
+    }
+
+    // Update last_publish_id_
+    last_publish_id_ = id_;
+
+    // Start task profiling
+    rubis::start_task_profiling_at_initial_node(
+        std::max(lidar_msg->header.stamp.sec, latest_odom_msg_->header.stamp.sec),
+        std::max(lidar_msg->header.stamp.nsec, latest_odom_msg_->header.stamp.nsec));
+
+    // Current time for synchronization
+    auto cur_time = ros::Time::now();
+
+    // Prepare and publish PointCloud2 message
+    rubis_msgs::PointCloud2 out_lidar_msg;
+    out_lidar_msg.header = lidar_msg->header;
+    out_lidar_msg.header.stamp = cur_time;
+    out_lidar_msg.instance = rubis::instance_;
+    out_lidar_msg.msg = *lidar_msg;
+    out_lidar_msg.msg.header.stamp = cur_time;
+    if (out_lidar_msg.msg.fields.size() > 3) {
+        out_lidar_msg.msg.fields.at(3).datatype = 7;
+    }
+
+    // Prepare and publish PoseTwistStamped message
+    rubis_msgs::PoseTwistStamped out_pose_twist_msg;
+    out_pose_twist_msg.header = latest_odom_msg_->header;
+    out_pose_twist_msg.header.stamp = cur_time;
+    out_pose_twist_msg.instance = rubis::instance_;
+    out_pose_twist_msg.pose.header = latest_odom_msg_->header;
+    out_pose_twist_msg.pose.header.stamp = cur_time;
+    out_pose_twist_msg.pose.header.frame_id = "/map";
+    out_pose_twist_msg.pose.pose = latest_odom_msg_->pose.pose;
+    out_pose_twist_msg.twist.header.frame_id = "/map";
+    out_pose_twist_msg.twist.header.stamp = cur_time;
+    out_pose_twist_msg.twist.twist = latest_odom_msg_->twist.twist;
+
+    // Prepare and publish Image messages if available
+    rubis_msgs::Image out_image_msg1, out_image_msg2, out_image_msg3, out_image_msg4;
+    bool has_image1 = false, has_image2 = false, has_image3 = false, has_image4 = false;
+
+    if (latest_image_msg1_) {
+        out_image_msg1.header = latest_odom_msg_->header;
+        out_image_msg1.header.stamp = cur_time;
+        out_image_msg1.instance = rubis::instance_;
+        out_image_msg1.msg = *latest_image_msg1_;
+        has_image1 = true;
+    }
+
+    if (latest_image_msg2_) {
+        out_image_msg2.header = latest_odom_msg_->header;
+        out_image_msg2.header.stamp = cur_time;
+        out_image_msg2.instance = rubis::instance_;
+        out_image_msg2.msg = *latest_image_msg2_;
+        has_image2 = true;
+    }
+
+    if (latest_image_msg3_) {
+        out_image_msg3.header = latest_odom_msg_->header;
+        out_image_msg3.header.stamp = cur_time;
+        out_image_msg3.instance = rubis::instance_;
+        out_image_msg3.msg = *latest_image_msg3_;
+        has_image3 = true;
+    }
+
+    if (latest_image_msg4_) {
+        out_image_msg4.header = latest_odom_msg_->header;
+        out_image_msg4.header.stamp = cur_time;
+        out_image_msg4.instance = rubis::instance_;
+        out_image_msg4.msg = *latest_image_msg4_;
+        has_image4 = true;
+    }
+
+    // Perform data processing
+    read_data(n_);
+
+    // Publish LiDAR and PoseTwist messages
+    lidar_pub_.publish(out_lidar_msg);
+    pose_twist_pub_.publish(out_pose_twist_msg);
+
+    // Publish Image messages if available
+    if (has_image1) {
+        image_pub1_.publish(out_image_msg1);
+    }
+    if (has_image2) {
+        image_pub2_.publish(out_image_msg2);
+    }
+    if (has_image3) {
+        image_pub3_.publish(out_image_msg3);
+    }
+    if (has_image4) {
+        image_pub4_.publish(out_image_msg4);
+    }
+
+    // Broadcast TF Transform
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion gnss_q(out_pose_twist_msg.pose.pose.orientation.x,
+                          out_pose_twist_msg.pose.pose.orientation.y,
+                          out_pose_twist_msg.pose.pose.orientation.z,
+                          out_pose_twist_msg.pose.pose.orientation.w);
+    transform.setOrigin(tf::Vector3(out_pose_twist_msg.pose.pose.position.x,
+                                    out_pose_twist_msg.pose.pose.position.y,
+                                    out_pose_twist_msg.pose.pose.position.z));
+    transform.setRotation(gnss_q);
+
+    br.sendTransform(tf::StampedTransform(transform, cur_time, "/map", "/base_link"));
+
+    // Stop task profiling
+    rubis::stop_task_profiling(rubis::instance_++, rubis::lidar_instance_++, rubis::vision_instance_++);
+}
+
+void SvlSensing::read_data(int n){    
+    int stride_size = 64;
+    int stride = 64 / sizeof(int);
+    int data_num = data_size_ / sizeof(int);    
+    int index_stride = (int)(data_num / 16);
+
+    for(int j = 0; j < n; j++){
+        int index = 0;
+        int limit = index_stride;
+        for(int i = 0; i <16; i++){
+            for(; index < limit; index += stride){        
+                data_[index] = data_[index] + 1;                          
+            }
+            limit += index_stride;
+        }
+    }
+
+    return;
+}
+
+// Run method remains largely unchanged
+void SvlSensing::run() {
+    if(rate_ < 0)
+        ros::spin();
+    else{
+        ros::Rate r(rate_);
+        while (ros::ok()) {
+            ros::spinOnce();
+            id_++;
+            r.sleep();
+        }
+    }
+}
diff --git a/rubis_ws/src/svl_pkg/src/svl_sensing/svl_sensing_node.cpp b/rubis_ws/src/svl_pkg/src/svl_sensing/svl_sensing_node.cpp
new file mode 100644
index 00000000..3e7631cf
--- /dev/null
+++ b/rubis_ws/src/svl_pkg/src/svl_sensing/svl_sensing_node.cpp
@@ -0,0 +1,12 @@
+#include <ros/ros.h>
+
+#include "svl_sensing.h"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "svl_sensing");  
+  SvlSensing svl_sensing;
+  svl_sensing.run();
+
+  return 0;
+}
diff --git a/rubis_ws/src/topic_tools/CMakeLists.txt b/rubis_ws/src/topic_tools/CMakeLists.txt
index 1eda16fd..c6bcabd5 100644
--- a/rubis_ws/src/topic_tools/CMakeLists.txt
+++ b/rubis_ws/src/topic_tools/CMakeLists.txt
@@ -76,45 +76,4 @@ catkin_install_python(PROGRAMS
   scripts/mux_select
   scripts/relay_field
   scripts/transform
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
-#Testing
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest)
-  find_package(rosunit)
-  catkin_add_gtest(${PROJECT_NAME}-utest test/utest.cpp)
-  if(TARGET ${PROJECT_NAME}-utest)
-    target_link_libraries(${PROJECT_NAME}-utest topic_tools)
-  endif()
-
-  if(GTEST_FOUND)
-    include_directories(${GTEST_INCLUDE_DIRS})
-    add_executable(${PROJECT_NAME}-test_shapeshifter EXCLUDE_FROM_ALL test/test_shapeshifter.cpp)
-    target_link_libraries(${PROJECT_NAME}-test_shapeshifter ${GTEST_LIBRARIES} topic_tools)
-  endif()
-  if(TARGET tests)
-    add_dependencies(tests ${PROJECT_NAME}-test_shapeshifter)
-  endif()
-
-  add_rostest(test/shapeshifter.test)
-  add_rostest(test/throttle.test)
-  add_rostest(test/throttle_simtime.test)
-  add_rostest(test/throttle_simtime_loop.test)
-  add_rostest(test/drop.test)
-  add_rostest(test/relay.test)
-  add_rostest(test/relay_stealth.test)
-  add_rostest(test/lazy_transport.test)
-  add_rostest(test/mux_initial_none.test)
-  add_rostest(test/mux_initial_other.test)
-  add_rostest(test/transform.test)
-  ## Latched test disabled until underlying issue in roscpp is resolved,
-  ## #3385, #3434.
-  #rosbuild_add_rostest(test/relay_latched.test)
-  add_rostest(test/mux.test)
-  add_rostest(test/switch_mux.test)
-  add_rostest(test/switch_mux_leading_slash.test)
-  add_rostest(test/switch_mux_none.test)
-  #rosbuild_add_rostest(test/mux_add.test)
-
-  catkin_add_nosetests(test/args.py)
-endif()
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
\ No newline at end of file
diff --git a/rubis_ws/src/topic_tools/src/relay.cpp b/rubis_ws/src/topic_tools/src/relay.cpp
index 60862697..4a12f860 100644
--- a/rubis_ws/src/topic_tools/src/relay.cpp
+++ b/rubis_ws/src/topic_tools/src/relay.cpp
@@ -110,10 +110,6 @@ void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event)
   }
   else
     g_pub.publish(msg);
-  
-  if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task();
-  rubis::sched::task_state_ = TASK_STATE_DONE;
-
 }
 
 void timer_cb(const ros::TimerEvent&)
@@ -184,73 +180,10 @@ int main(int argc, char **argv)
     pnh.param<double>("monitor_rate", monitor_rate, 1.0);
     monitor_timer = n.createTimer(ros::Duration(monitor_rate), &timer_cb);
   }
-  
-
-  // scheduling
-  int task_scheduling_flag = 0;
-  int task_profiling_flag = 0;
-  std::string task_response_time_filename;
-  int rate = 0;
-  double task_minimum_inter_release_time = 0;
-  double task_execution_time = 0;
-  double task_relative_deadline = 0;
-
-  if(g_output_topic == std::string("/current_velocity")){
-    pnh.param<int>("/vel_relay/task_scheduling_flag", task_scheduling_flag, 0);
-    pnh.param<int>("/vel_relay/task_profiling_flag", task_profiling_flag, 0);
-    pnh.param<std::string>("/vel_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/vel_relay.csv");
-    pnh.param<int>("/vel_relay/rate", rate, 10);
-    pnh.param("/vel_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000);
-    pnh.param("/vel_relay/task_execution_time", task_execution_time, (double)100000000);
-    pnh.param("/vel_relay/task_relative_deadline", task_relative_deadline, (double)100000000);
-  }
-  else if (g_output_topic == std::string("/current_pose")){
-    pnh.param<int>("/pose_relay/task_scheduling_flag", task_scheduling_flag, 0);
-    pnh.param<int>("/pose_relay/task_profiling_flag", task_profiling_flag, 0);
-    pnh.param<std::string>("/pose_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pose_relay.csv");
-    pnh.param<int>("/pose_relay/rate", rate, 10);
-    pnh.param("/pose_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000);
-    pnh.param("/pose_relay/task_execution_time", task_execution_time, (double)100000000);
-    pnh.param("/pose_relay/task_relative_deadline", task_relative_deadline, (double)100000000);
-  }
-
-  if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename);
 
   subscribe();
 
-  if(!task_scheduling_flag && !task_profiling_flag){
-    ros::spin();
-  }
-  else{
-    ros::Rate r(rate);
-    // Initialize task ( Wait until first necessary topic is published )
-    while(ros::ok()){
-      if(rubis::sched::is_task_ready_ == TASK_READY) break;
-      ros::spinOnce();
-      r.sleep();      
-    }
-
-    // Executing task
-    while(ros::ok()){
-      if(task_profiling_flag) rubis::sched::start_task_profiling();
-
-      if(rubis::sched::task_state_ == TASK_STATE_READY){        
-        if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); 
-        rubis::sched::task_state_ = TASK_STATE_RUNNING;     
-      }
-
-      ros::spinOnce();
-
-      if(task_profiling_flag) rubis::sched::stop_task_profiling(RUBIS_NO_INSTANCE, rubis::sched::task_state_);
-
-      if(rubis::sched::task_state_ == TASK_STATE_DONE){        
-        if(task_scheduling_flag) rubis::sched::yield_task_scheduling();
-        rubis::sched::task_state_ = TASK_STATE_READY;
-      }
-      
-      r.sleep();
-    }
-  }
+  ros::spin();
 
   return 0;
 }
diff --git a/rubis_ws/src/vesc_ackermann/CMakeLists.txt b/rubis_ws/src/vesc_ackermann/CMakeLists.txt
new file mode 100644
index 00000000..5cea706e
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/CMakeLists.txt
@@ -0,0 +1,90 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vesc_ackermann)
+
+find_package(catkin REQUIRED COMPONENTS
+  nodelet
+  pluginlib
+  roscpp
+  nav_msgs
+  std_msgs
+  geometry_msgs
+  tf
+  ackermann_msgs
+  vesc_msgs
+  nav_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS
+    nodelet
+    pluginlib
+    roscpp
+    nav_msgs
+    std_msgs
+    geometry_msgs
+    tf
+    ackermann_msgs
+    vesc_msgs
+    nav_msgs
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# node executable
+add_executable(vesc_to_odom_node src/vesc_to_odom_node.cpp
+                                 src/vesc_to_odom.cpp)
+add_dependencies(vesc_to_odom_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(vesc_to_odom_node
+  ${catkin_LIBRARIES}
+)
+
+add_executable(ackermann_to_vesc_node src/ackermann_to_vesc_node.cpp
+                                      src/ackermann_to_vesc.cpp)
+add_dependencies(ackermann_to_vesc_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(ackermann_to_vesc_node
+  ${catkin_LIBRARIES}
+)
+
+# nodelet library
+add_library(vesc_ackermann_nodelet src/ackermann_to_vesc_nodelet.cpp
+                                   src/ackermann_to_vesc.cpp
+                                   src/vesc_to_odom_nodelet.cpp
+                                   src/vesc_to_odom.cpp)
+add_dependencies(vesc_ackermann_nodelet ${catkin_EXPORTED_TARGETS})
+target_link_libraries(vesc_ackermann_nodelet
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(TARGETS vesc_to_odom_node ackermann_to_vesc_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS vesc_ackermann_nodelet
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(FILES vesc_ackermann_nodelet.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY launch/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+
+#############
+## Testing ##
+#############
+
+# TODO
\ No newline at end of file
diff --git a/rubis_ws/src/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/rubis_ws/src/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h
new file mode 100644
index 00000000..3edb06e8
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h
@@ -0,0 +1,37 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_
+#define VESC_ACKERMANN_ACKERMANN_TO_VESC_H_
+
+#include <ros/ros.h>
+#include <ackermann_msgs/AckermannDriveStamped.h>
+
+namespace vesc_ackermann
+{
+
+class AckermannToVesc
+{
+public:
+
+  AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh);
+
+private:
+  // ROS parameters
+  // conversion gain and offset
+  double speed_to_erpm_gain_, speed_to_erpm_offset_;
+  double steering_to_servo_gain_, steering_to_servo_offset_;
+
+  /** @todo consider also providing an interpolated look-up table conversion */
+
+  // ROS services
+  ros::Publisher erpm_pub_;
+  ros::Publisher servo_pub_;
+  ros::Subscriber ackermann_sub_;
+
+  // ROS callbacks
+  void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd);
+};
+
+} // namespace vesc_ackermann
+
+#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_
diff --git a/rubis_ws/src/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/rubis_ws/src/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
new file mode 100644
index 00000000..7866a5aa
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
@@ -0,0 +1,51 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_
+#define VESC_ACKERMANN_VESC_TO_ODOM_H_
+
+#include <ros/ros.h>
+#include <vesc_msgs/VescStateStamped.h>
+#include <std_msgs/Float64.h>
+#include <boost/shared_ptr.hpp>
+#include <tf/transform_broadcaster.h>
+
+namespace vesc_ackermann
+{
+
+class VescToOdom
+{
+public:
+
+  VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh);
+
+private:
+  // ROS parameters
+  std::string odom_frame_;
+  std::string base_frame_;
+  /** State message does not report servo position, so use the command instead */
+  bool use_servo_cmd_;
+  // conversion gain and offset
+  double speed_to_erpm_gain_, speed_to_erpm_offset_;
+  double steering_to_servo_gain_, steering_to_servo_offset_;
+  double wheelbase_;
+  bool publish_tf_;
+
+  // odometry state
+  double x_, y_, yaw_;
+  std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value
+  vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message
+
+  // ROS services
+  ros::Publisher odom_pub_;
+  ros::Subscriber vesc_state_sub_;
+  ros::Subscriber servo_sub_;
+  boost::shared_ptr<tf::TransformBroadcaster> tf_pub_;
+
+  // ROS callbacks
+  void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state);
+  void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo);
+};
+
+} // namespace vesc_ackermann
+
+#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_
diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch b/rubis_ws/src/vesc_ackermann/launch/ackermann_to_vesc_node.launch
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch
rename to rubis_ws/src/vesc_ackermann/launch/ackermann_to_vesc_node.launch
diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch b/rubis_ws/src/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch
rename to rubis_ws/src/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch
diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch b/rubis_ws/src/vesc_ackermann/launch/vesc_to_odom_node.launch
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch
rename to rubis_ws/src/vesc_ackermann/launch/vesc_to_odom_node.launch
diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch b/rubis_ws/src/vesc_ackermann/launch/vesc_to_odom_nodelet.launch
similarity index 100%
rename from rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch
rename to rubis_ws/src/vesc_ackermann/launch/vesc_to_odom_nodelet.launch
diff --git a/rubis_ws/src/vesc_ackermann/package.xml b/rubis_ws/src/vesc_ackermann/package.xml
new file mode 100644
index 00000000..b66f2ebe
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/package.xml
@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<package>
+  <name>vesc_ackermann</name>
+  <version>0.0.1</version>
+  <description>
+    Translate between VESC messages and ROS ackermann and odometry messages.
+  </description>
+
+  <maintainer email="boulet@ll.mit.edu">Michael T. Boulet</maintainer>
+  <author email="boulet@ll.mit.edu">Michael T. Boulet</author>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/vesc_ackermann</url>
+  <url type="repository">https://github.mit.edu/racecar/racecar-iap</url>
+  <url type="bugtracker">https://github.mit.edu/racecar/racecar-iap/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>nodelet</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>ackermann_msgs</build_depend>
+  <build_depend>vesc_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+
+  <run_depend>nodelet</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>ackermann_msgs</run_depend>
+  <run_depend>vesc_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/vesc_ackermann_nodelet.xml"/>
+  </export>
+
+</package>
diff --git a/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc.cpp b/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc.cpp
new file mode 100644
index 00000000..0a00aaaf
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc.cpp
@@ -0,0 +1,64 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#include "vesc_ackermann/ackermann_to_vesc.h"
+
+#include <cmath>
+#include <sstream>
+
+#include <std_msgs/Float64.h>
+
+namespace vesc_ackermann
+{
+
+template <typename T>
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value);
+
+AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh)
+{
+  // get conversion parameters
+  if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_))
+    return;
+  if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_))
+    return;
+  if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_))
+    return;
+  if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_))
+    return;
+
+  // create publishers to vesc electric-RPM (speed) and servo commands
+  erpm_pub_ = nh.advertise<std_msgs::Float64>("commands/motor/speed", 10);
+  servo_pub_ = nh.advertise<std_msgs::Float64>("commands/servo/position", 10);
+
+  // subscribe to ackermann topic
+  ackermann_sub_ = nh.subscribe("ackermann_cmd", 10, &AckermannToVesc::ackermannCmdCallback, this);
+}
+
+typedef ackermann_msgs::AckermannDriveStamped::ConstPtr AckermannMsgPtr;
+void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd)
+{
+  // calc vesc electric RPM (speed)
+  std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64);
+  erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_;
+
+  // calc steering angle (servo)
+  std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64);
+  servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_;
+
+  // publish
+  if (ros::ok()) {
+    erpm_pub_.publish(erpm_msg);
+    servo_pub_.publish(servo_msg);
+  }
+}
+
+template <typename T>
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value)
+{
+  if (nh.getParam(name, value))
+    return true;
+
+  ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str());
+  return false;
+}
+
+} // namespace vesc_ackermann
diff --git a/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc_node.cpp b/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc_node.cpp
new file mode 100644
index 00000000..cfe2268c
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc_node.cpp
@@ -0,0 +1,16 @@
+#include <ros/ros.h>
+
+#include "vesc_ackermann/ackermann_to_vesc.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "ackermann_to_vesc_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  vesc_ackermann::AckermannToVesc ackermann_to_vesc(nh, private_nh);
+
+  ros::spin();
+
+  return 0;
+}
diff --git a/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp
new file mode 100644
index 00000000..129df4f2
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp
@@ -0,0 +1,33 @@
+#include <boost/shared_ptr.hpp>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "vesc_ackermann/ackermann_to_vesc.h"
+
+namespace vesc_ackermann
+{
+
+class AckermannToVescNodelet: public nodelet::Nodelet
+{
+public:
+
+  AckermannToVescNodelet() {}
+
+private:
+
+  virtual void onInit(void);
+
+  boost::shared_ptr<AckermannToVesc> ackermann_to_vesc_;
+
+}; // class AckermannToVescNodelet
+
+void AckermannToVescNodelet::onInit()
+{
+  NODELET_DEBUG("Initializing ackermann to VESC nodelet");
+  ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle()));
+}
+
+} // namespace vesc_ackermann
+
+PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet);
diff --git a/rubis_ws/src/vesc_ackermann/src/vesc_to_odom.cpp b/rubis_ws/src/vesc_ackermann/src/vesc_to_odom.cpp
new file mode 100644
index 00000000..eba416a0
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/src/vesc_to_odom.cpp
@@ -0,0 +1,151 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#include "vesc_ackermann/vesc_to_odom.h"
+
+#include <cmath>
+
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/TransformStamped.h>
+
+namespace vesc_ackermann
+{
+
+template <typename T>
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value);
+
+VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) :
+  odom_frame_("odom"), base_frame_("base_link"),
+  use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0)
+{
+  // get ROS parameters
+  private_nh.param("odom_frame", odom_frame_, odom_frame_);
+  private_nh.param("base_frame", base_frame_, base_frame_);
+  private_nh.param("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_, use_servo_cmd_);
+  if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_))
+    return;
+  if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_))
+    return;
+  if (use_servo_cmd_) {
+    if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_))
+      return;
+    if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_))
+      return;
+    if (!getRequiredParam(nh, "wheelbase", wheelbase_))
+      return;
+  }
+  private_nh.param("publish_tf", publish_tf_, publish_tf_);
+
+  // create odom publisher
+  odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 10);
+
+  // create tf broadcaster
+  if (publish_tf_) {
+    tf_pub_.reset(new tf::TransformBroadcaster);
+  }
+
+  // subscribe to vesc state and. optionally, servo command
+  vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this);
+  if (use_servo_cmd_) {
+    servo_sub_ = nh.subscribe("sensors/servo_position_command", 10,
+                              &VescToOdom::servoCmdCallback, this);
+  }
+}
+
+void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state)
+{
+  // check that we have a last servo command if we are depending on it for angular velocity
+  if (use_servo_cmd_ && !last_servo_cmd_)
+    return;
+
+  // convert to engineering units
+  double current_speed = ( state->state.speed - speed_to_erpm_offset_ ) / speed_to_erpm_gain_;
+  double current_steering_angle(0.0), current_angular_velocity(0.0);
+  if (use_servo_cmd_) {
+    current_steering_angle =
+      ( last_servo_cmd_->data - steering_to_servo_offset_ ) / steering_to_servo_gain_;
+    current_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_;
+  }
+
+  // use current state as last state if this is our first time here
+  if (!last_state_)
+    last_state_ = state;
+
+  // calc elapsed time
+  ros::Duration dt = state->header.stamp - last_state_->header.stamp;
+
+  /** @todo could probably do better propigating odometry, e.g. trapezoidal integration */
+
+  // propigate odometry
+  double x_dot = current_speed * cos(yaw_);
+  double y_dot = current_speed * sin(yaw_);
+  x_ += x_dot * dt.toSec();
+  y_ += y_dot * dt.toSec();
+  if (use_servo_cmd_)
+    yaw_ += current_angular_velocity * dt.toSec();
+
+  // save state for next time
+  last_state_ = state;
+
+  // publish odometry message
+  nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry);
+  odom->header.frame_id = odom_frame_;
+  odom->header.stamp = state->header.stamp;
+  odom->child_frame_id = base_frame_;
+
+  // Position
+  odom->pose.pose.position.x = x_;
+  odom->pose.pose.position.y = y_;
+  odom->pose.pose.orientation.x = 0.0;
+  odom->pose.pose.orientation.y = 0.0;
+  odom->pose.pose.orientation.z = sin(yaw_/2.0);
+  odom->pose.pose.orientation.w = cos(yaw_/2.0);
+
+  // Position uncertainty
+  /** @todo Think about position uncertainty, perhaps get from parameters? */
+  odom->pose.covariance[0]  = 0.2; ///< x
+  odom->pose.covariance[7]  = 0.2; ///< y
+  odom->pose.covariance[35] = 0.4; ///< yaw
+
+  // Velocity ("in the coordinate frame given by the child_frame_id")
+  odom->twist.twist.linear.x = current_speed;
+  odom->twist.twist.linear.y = 0.0;
+  odom->twist.twist.angular.z = current_angular_velocity;
+
+  // Velocity uncertainty
+  /** @todo Think about velocity uncertainty */
+
+  if (publish_tf_) {
+    geometry_msgs::TransformStamped tf;
+    tf.header.frame_id = odom_frame_;
+    tf.child_frame_id = base_frame_;
+    tf.header.stamp = ros::Time::now();
+    tf.transform.translation.x = x_;
+    tf.transform.translation.y = y_;
+    tf.transform.translation.z = 0.0;
+    tf.transform.rotation = odom->pose.pose.orientation;
+    if (ros::ok()) {
+      tf_pub_->sendTransform(tf);
+    }
+  }
+
+  if (ros::ok()) {
+    odom_pub_.publish(odom);
+  }
+}
+
+void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo)
+{
+  last_servo_cmd_ = servo;
+}
+
+template <typename T>
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value)
+{
+  if (nh.getParam(name, value))
+    return true;
+
+  ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str());
+  return false;
+}
+
+} // namespace vesc_ackermann
diff --git a/rubis_ws/src/vesc_ackermann/src/vesc_to_odom_node.cpp b/rubis_ws/src/vesc_ackermann/src/vesc_to_odom_node.cpp
new file mode 100644
index 00000000..9ce992fd
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/src/vesc_to_odom_node.cpp
@@ -0,0 +1,16 @@
+#include <ros/ros.h>
+
+#include "vesc_ackermann/vesc_to_odom.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "vesc_to_odom_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  vesc_ackermann::VescToOdom vesc_to_odom(nh, private_nh);
+
+  ros::spin();
+
+  return 0;
+}
diff --git a/rubis_ws/src/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/rubis_ws/src/vesc_ackermann/src/vesc_to_odom_nodelet.cpp
new file mode 100644
index 00000000..3fc37e45
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/src/vesc_to_odom_nodelet.cpp
@@ -0,0 +1,33 @@
+#include <boost/shared_ptr.hpp>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "vesc_ackermann/vesc_to_odom.h"
+
+namespace vesc_ackermann
+{
+
+class VescToOdomNodelet: public nodelet::Nodelet
+{
+public:
+
+  VescToOdomNodelet() {}
+
+private:
+
+  virtual void onInit(void);
+
+  boost::shared_ptr<VescToOdom> vesc_to_odom_;
+
+}; // class VescToOdomNodelet
+
+void VescToOdomNodelet::onInit()
+{
+  NODELET_DEBUG("Initializing RACECAR VESC odometry estimator nodelet");
+  vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle()));
+}
+
+} // namespace vesc_ackermann
+
+PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet);
diff --git a/rubis_ws/src/vesc_ackermann/vesc_ackermann_nodelet.xml b/rubis_ws/src/vesc_ackermann/vesc_ackermann_nodelet.xml
new file mode 100644
index 00000000..b12358aa
--- /dev/null
+++ b/rubis_ws/src/vesc_ackermann/vesc_ackermann_nodelet.xml
@@ -0,0 +1,8 @@
+<library path="lib/libvesc_ackermann_nodelet">
+  <class type="vesc_ackermann::AckermannToVescNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet for translating ackermann messages to VESC motor controller commands.</description>
+  </class>
+  <class type="vesc_ackermann::VescToOdomNodelet" base_class_type="nodelet::Nodelet">
+    <description>Nodelet for translating VESC motor controller state to odometry messages.</description>
+  </class>
+</library>
diff --git a/rubis_ws/src/vesc_driver/CMakeLists.txt b/rubis_ws/src/vesc_driver/CMakeLists.txt
new file mode 100644
index 00000000..ece8de18
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/CMakeLists.txt
@@ -0,0 +1,73 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vesc_driver)
+
+find_package(catkin REQUIRED COMPONENTS
+  nodelet
+  pluginlib
+  roscpp
+  std_msgs
+  vesc_msgs
+  serial
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS nodelet pluginlib roscpp std_msgs vesc_msgs serial
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# node executable
+add_executable(vesc_driver_node src/vesc_driver_node.cpp
+                                src/vesc_driver.cpp
+                                src/vesc_interface.cpp
+                                src/vesc_packet.cpp
+                                src/vesc_packet_factory.cpp)
+add_dependencies(vesc_driver_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(vesc_driver_node
+  ${catkin_LIBRARIES}
+)
+
+# nodelet library
+add_library(vesc_driver_nodelet src/vesc_driver_nodelet.cpp
+                                src/vesc_driver.cpp
+                                src/vesc_interface.cpp
+                                src/vesc_packet.cpp
+                                src/vesc_packet_factory.cpp)
+add_dependencies(vesc_driver_nodelet ${catkin_EXPORTED_TARGETS})
+target_link_libraries(vesc_driver_nodelet
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(TARGETS vesc_driver_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS vesc_driver_nodelet
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(FILES vesc_driver_nodelet.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY launch/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+
+#############
+## Testing ##
+#############
+
+# TODO
\ No newline at end of file
diff --git a/rubis_ws/src/vesc_driver/include/vesc_driver/datatypes.h b/rubis_ws/src/vesc_driver/include/vesc_driver/datatypes.h
new file mode 100644
index 00000000..d71df934
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/include/vesc_driver/datatypes.h
@@ -0,0 +1,371 @@
+/*
+        Copyright 2012-2014 Benjamin Vedder     benjamin@vedder.se
+
+        This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    */
+
+/*
+ * datatypes.h
+ *
+ *  Created on: 14 sep 2014
+ *      Author: benjamin
+ */
+
+#ifndef DATATYPES_H_
+#define DATATYPES_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+//#include "ch.h"
+typedef uint32_t systime_t; // defined in ch.h
+
+// Data types
+typedef enum {
+   MC_STATE_OFF = 0,
+   MC_STATE_DETECTING,
+   MC_STATE_RUNNING,
+   MC_STATE_FULL_BRAKE,
+} mc_state;
+
+typedef enum {
+        PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended
+        PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode
+        PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs
+} mc_pwm_mode;
+
+typedef enum {
+        COMM_MODE_INTEGRATE = 0,
+        COMM_MODE_DELAY
+} mc_comm_mode;
+
+typedef enum {
+        SENSOR_MODE_SENSORLESS = 0,
+        SENSOR_MODE_SENSORED,
+        SENSOR_MODE_HYBRID
+} mc_sensor_mode;
+
+typedef enum {
+        MOTOR_TYPE_BLDC = 0,
+        MOTOR_TYPE_DC,
+} mc_motor_type;
+
+typedef enum {
+        FAULT_CODE_NONE = 0,
+        FAULT_CODE_OVER_VOLTAGE,
+        FAULT_CODE_UNDER_VOLTAGE,
+        FAULT_CODE_DRV8302,
+        FAULT_CODE_ABS_OVER_CURRENT,
+        FAULT_CODE_OVER_TEMP_FET,
+        FAULT_CODE_OVER_TEMP_MOTOR
+} mc_fault_code;
+
+typedef enum {
+        CONTROL_MODE_DUTY = 0,
+        CONTROL_MODE_SPEED,
+        CONTROL_MODE_CURRENT,
+        CONTROL_MODE_CURRENT_BRAKE,
+        CONTROL_MODE_POS,
+        CONTROL_MODE_NONE
+} mc_control_mode;
+
+typedef struct {
+        float cycle_int_limit;
+        float cycle_int_limit_running;
+        float cycle_int_limit_max;
+        float comm_time_sum;
+        float comm_time_sum_min_rpm;
+        int32_t comms;
+        uint32_t time_at_comm;
+} mc_rpm_dep_struct;
+
+typedef struct {
+        // Switching and drive
+        mc_pwm_mode pwm_mode;
+        mc_comm_mode comm_mode;
+        mc_motor_type motor_type;
+        mc_sensor_mode sensor_mode;
+        // Limits
+        float l_current_max;
+        float l_current_min;
+        float l_in_current_max;
+        float l_in_current_min;
+        float l_abs_current_max;
+        float l_min_erpm;
+        float l_max_erpm;
+        float l_max_erpm_fbrake;
+        float l_max_erpm_fbrake_cc;
+        float l_min_vin;
+        float l_max_vin;
+        bool l_slow_abs_current;
+        bool l_rpm_lim_neg_torque;
+        float l_temp_fet_start;
+        float l_temp_fet_end;
+        float l_temp_motor_start;
+        float l_temp_motor_end;
+        float l_min_duty;
+        float l_max_duty;
+        // Overridden limits (Computed during runtime)
+        float lo_current_max;
+        float lo_current_min;
+        float lo_in_current_max;
+        float lo_in_current_min;
+        // Sensorless
+        float sl_min_erpm;
+        float sl_min_erpm_cycle_int_limit;
+        float sl_max_fullbreak_current_dir_change;
+        float sl_cycle_int_limit;
+        float sl_phase_advance_at_br;
+        float sl_cycle_int_rpm_br;
+        float sl_bemf_coupling_k;
+        // Hall sensor
+        int8_t hall_table[8];
+        float hall_sl_erpm;
+        // Speed PID
+        float s_pid_kp;
+        float s_pid_ki;
+        float s_pid_kd;
+        float s_pid_min_rpm;
+        // Pos PID
+        float p_pid_kp;
+        float p_pid_ki;
+        float p_pid_kd;
+        // Current controller
+        float cc_startup_boost_duty;
+        float cc_min_current;
+        float cc_gain;
+        float cc_ramp_step_max;
+        // Misc
+        int32_t m_fault_stop_time_ms;
+} mc_configuration;
+
+// Applications to use
+typedef enum {
+        APP_NONE = 0,
+        APP_PPM,
+        APP_ADC,
+        APP_UART,
+        APP_PPM_UART,
+        APP_ADC_UART,
+        APP_NUNCHUK,
+        APP_NRF,
+        APP_CUSTOM
+} app_use;
+
+// PPM control types
+typedef enum {
+        PPM_CTRL_TYPE_NONE = 0,
+        PPM_CTRL_TYPE_CURRENT,
+        PPM_CTRL_TYPE_CURRENT_NOREV,
+        PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE,
+        PPM_CTRL_TYPE_DUTY,
+        PPM_CTRL_TYPE_DUTY_NOREV,
+        PPM_CTRL_TYPE_PID,
+        PPM_CTRL_TYPE_PID_NOREV
+} ppm_control_type;
+
+typedef struct {
+        ppm_control_type ctrl_type;
+        float pid_max_erpm;
+        float hyst;
+        float pulse_start;
+        float pulse_end;
+        bool median_filter;
+        bool safe_start;
+        float rpm_lim_start;
+        float rpm_lim_end;
+        bool multi_esc;
+        bool tc;
+        float tc_max_diff;
+} ppm_config;
+
+// ADC control types
+typedef enum {
+        ADC_CTRL_TYPE_NONE = 0,
+        ADC_CTRL_TYPE_CURRENT,
+        ADC_CTRL_TYPE_CURRENT_REV_CENTER,
+        ADC_CTRL_TYPE_CURRENT_REV_BUTTON,
+        ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER,
+        ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON,
+        ADC_CTRL_TYPE_DUTY,
+        ADC_CTRL_TYPE_DUTY_REV_CENTER,
+        ADC_CTRL_TYPE_DUTY_REV_BUTTON
+} adc_control_type;
+
+typedef struct {
+        adc_control_type ctrl_type;
+        float hyst;
+        float voltage_start;
+        float voltage_end;
+        bool use_filter;
+        bool safe_start;
+        bool button_inverted;
+        bool voltage_inverted;
+        float rpm_lim_start;
+        float rpm_lim_end;
+        bool multi_esc;
+        bool tc;
+        float tc_max_diff;
+        uint32_t update_rate_hz;
+} adc_config;
+
+// Nunchuk control types
+typedef enum {
+        CHUK_CTRL_TYPE_NONE = 0,
+        CHUK_CTRL_TYPE_CURRENT,
+        CHUK_CTRL_TYPE_CURRENT_NOREV
+} chuk_control_type;
+
+typedef struct {
+        chuk_control_type ctrl_type;
+        float hyst;
+        float rpm_lim_start;
+        float rpm_lim_end;
+        float ramp_time_pos;
+        float ramp_time_neg;
+        bool multi_esc;
+        bool tc;
+        float tc_max_diff;
+} chuk_config;
+
+typedef struct {
+        // Settings
+        uint8_t controller_id;
+        uint32_t timeout_msec;
+        float timeout_brake_current;
+        bool send_can_status;
+        uint32_t send_can_status_rate_hz;
+
+        // Application to use
+        app_use app_to_use;
+
+        // PPM application settings
+        ppm_config app_ppm_conf;
+
+        // ADC application settings
+        adc_config app_adc_conf;
+
+        // UART application settings
+        uint32_t app_uart_baudrate;
+
+        // Nunchuk application settings
+        chuk_config app_chuk_conf;
+} app_configuration;
+
+// Communication commands
+typedef enum {
+        COMM_FW_VERSION = 0,
+        COMM_JUMP_TO_BOOTLOADER,
+        COMM_ERASE_NEW_APP,
+        COMM_WRITE_NEW_APP_DATA,
+        COMM_GET_VALUES,
+        COMM_SET_DUTY,
+        COMM_SET_CURRENT,
+        COMM_SET_CURRENT_BRAKE,
+        COMM_SET_RPM,
+        COMM_SET_POS,
+        COMM_SET_DETECT,
+        COMM_SET_SERVO_POS,
+        COMM_SET_MCCONF,
+        COMM_GET_MCCONF,
+        COMM_SET_APPCONF,
+        COMM_GET_APPCONF,
+        COMM_SAMPLE_PRINT,
+        COMM_TERMINAL_CMD,
+        COMM_PRINT,
+        COMM_ROTOR_POSITION,
+        COMM_EXPERIMENT_SAMPLE,
+        COMM_DETECT_MOTOR_PARAM,
+        COMM_REBOOT,
+        COMM_ALIVE,
+        COMM_GET_DECODED_PPM,
+        COMM_GET_DECODED_ADC,
+        COMM_GET_DECODED_CHUK,
+        COMM_FORWARD_CAN
+} COMM_PACKET_ID;
+
+// CAN commands
+typedef enum {
+        CAN_PACKET_SET_DUTY = 0,
+        CAN_PACKET_SET_CURRENT,
+        CAN_PACKET_SET_CURRENT_BRAKE,
+        CAN_PACKET_SET_RPM,
+        CAN_PACKET_SET_POS,
+        CAN_PACKET_FILL_RX_BUFFER,
+        CAN_PACKET_FILL_RX_BUFFER_LONG,
+        CAN_PACKET_PROCESS_RX_BUFFER,
+        CAN_PACKET_PROCESS_SHORT_BUFFER,
+        CAN_PACKET_STATUS
+} CAN_PACKET_ID;
+
+// Logged fault data
+typedef struct {
+        mc_fault_code fault;
+        float current;
+        float current_filtered;
+        float voltage;
+        float duty;
+        float rpm;
+        int tacho;
+        int tim_pwm_cnt;
+        int tim_samp_cnt;
+        int comm_step;
+        float temperature;
+} fault_data;
+
+// External LED state
+typedef enum {
+        LED_EXT_OFF = 0,
+        LED_EXT_NORMAL,
+        LED_EXT_BRAKE,
+        LED_EXT_TURN_LEFT,
+        LED_EXT_TURN_RIGHT,
+        LED_EXT_BRAKE_TURN_LEFT,
+        LED_EXT_BRAKE_TURN_RIGHT,
+        LED_EXT_BATT
+} LED_EXT_STATE;
+
+typedef struct {
+        int js_x;
+        int js_y;
+        int acc_x;
+        int acc_y;
+        int acc_z;
+        bool bt_c;
+        bool bt_z;
+} chuck_data;
+
+typedef struct {
+        int id;
+        systime_t rx_time;
+        float rpm;
+        float current;
+        float duty;
+} can_status_msg;
+
+typedef struct {
+        uint8_t js_x;
+        uint8_t js_y;
+        bool bt_c;
+        bool bt_z;
+        bool bt_push;
+        float vbat;
+} mote_state;
+
+typedef enum {
+        MOTE_PACKET_BATT_LEVEL = 0,
+        MOTE_PACKET_BUTTONS,
+        MOTE_PACKET_ALIVE
+} MOTE_PACKET;
+
+#endif /* DATATYPES_H_ */
diff --git a/rubis_ws/src/vesc_driver/include/vesc_driver/v8stdint.h b/rubis_ws/src/vesc_driver/include/vesc_driver/v8stdint.h
new file mode 100644
index 00000000..f3be96b1
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/include/vesc_driver/v8stdint.h
@@ -0,0 +1,57 @@
+// This header is from the v8 google project:
+// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h
+
+// Copyright 2012 the V8 project authors. All rights reserved.
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above
+//       copyright notice, this list of conditions and the following
+//       disclaimer in the documentation and/or other materials provided
+//       with the distribution.
+//     * Neither the name of Google Inc. nor the names of its
+//       contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Load definitions of standard types.
+
+#ifndef V8STDINT_H_
+#define V8STDINT_H_
+
+#include <stddef.h>
+#include <stdio.h>
+
+#if defined(_WIN32) && !defined(__MINGW32__)
+
+typedef signed char int8_t;
+typedef unsigned char uint8_t;
+typedef short int16_t;  // NOLINT
+typedef unsigned short uint16_t;  // NOLINT
+typedef int int32_t;
+typedef unsigned int uint32_t;
+typedef __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+// intptr_t and friends are defined in crtdefs.h through stdio.h.
+
+#else
+
+#include <stdint.h>
+
+#endif
+
+#endif  // V8STDINT_H_
diff --git a/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_driver.h b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_driver.h
new file mode 100644
index 00000000..005c0be3
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_driver.h
@@ -0,0 +1,83 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#ifndef VESC_DRIVER_VESC_DRIVER_H_
+#define VESC_DRIVER_VESC_DRIVER_H_
+
+#include <string>
+
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <boost/optional.hpp>
+
+#include "vesc_driver/vesc_interface.h"
+#include "vesc_driver/vesc_packet.h"
+
+namespace vesc_driver
+{
+
+class VescDriver
+{
+public:
+
+  VescDriver(ros::NodeHandle nh,
+             ros::NodeHandle private_nh);
+
+private:
+  // interface to the VESC
+  VescInterface vesc_;
+  void vescPacketCallback(const boost::shared_ptr<VescPacket const>& packet);
+  void vescErrorCallback(const std::string& error);
+
+  // limits on VESC commands
+  struct CommandLimit
+  {
+    CommandLimit(const ros::NodeHandle& nh, const std::string& str,
+                 const boost::optional<double>& min_lower = boost::optional<double>(),
+                 const boost::optional<double>& max_upper = boost::optional<double>());
+    double clip(double value);
+    std::string name;
+    boost::optional<double> lower;
+    boost::optional<double> upper;
+  };
+  CommandLimit duty_cycle_limit_;
+  CommandLimit current_limit_;
+  CommandLimit brake_limit_;
+  CommandLimit speed_limit_;
+  CommandLimit position_limit_;
+  CommandLimit servo_limit_;
+
+  // ROS services
+  ros::Publisher state_pub_;
+  ros::Publisher servo_sensor_pub_;
+  ros::Subscriber duty_cycle_sub_;
+  ros::Subscriber current_sub_;
+  ros::Subscriber brake_sub_;
+  ros::Subscriber speed_sub_;
+  ros::Subscriber position_sub_;
+  ros::Subscriber servo_sub_;
+  ros::Timer timer_;
+
+  // driver modes (possible states)
+  typedef enum {
+    MODE_INITIALIZING,
+    MODE_OPERATING
+  } driver_mode_t;
+
+  // other variables
+  driver_mode_t driver_mode_;           ///< driver state machine mode (state)
+  int fw_version_major_;                ///< firmware major version reported by vesc
+  int fw_version_minor_;                ///< firmware minor version reported by vesc
+
+  // ROS callbacks
+  void timerCallback(const ros::TimerEvent& event);
+  void dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle);
+  void currentCallback(const std_msgs::Float64::ConstPtr& current);
+  void brakeCallback(const std_msgs::Float64::ConstPtr& brake);
+  void speedCallback(const std_msgs::Float64::ConstPtr& speed);
+  void positionCallback(const std_msgs::Float64::ConstPtr& position);
+  void servoCallback(const std_msgs::Float64::ConstPtr& servo);
+};
+
+} // namespace vesc_driver
+
+#endif // VESC_DRIVER_VESC_DRIVER_H_
diff --git a/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_interface.h b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_interface.h
new file mode 100644
index 00000000..3bc50d09
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_interface.h
@@ -0,0 +1,122 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#ifndef VESC_DRIVER_VESC_INTERFACE_H_
+#define VESC_DRIVER_VESC_INTERFACE_H_
+
+#include <string>
+#include <sstream>
+#include <exception>
+#include <stdexcept>
+
+#include <boost/function.hpp>
+#include <boost/noncopyable.hpp>
+#include <boost/scoped_ptr.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include "vesc_driver/vesc_packet.h"
+
+namespace vesc_driver
+{
+
+/**
+ * Class providing an interface to the Vedder VESC motor controller via a serial port interface.
+ */
+class VescInterface : private boost::noncopyable
+{
+public:
+
+  typedef boost::function<void (const VescPacketConstPtr&)> PacketHandlerFunction;
+  typedef boost::function<void (const std::string&)> ErrorHandlerFunction;
+
+  /**
+   * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not
+   * empty, otherwise the serial port remains closed until connect() is called.
+   *
+   * @param port Address of the serial port, e.g. '/dev/ttyUSB0'.
+   * @param packet_handler Function this class calls when a VESC packet is received.
+   * @param error_handler Function this class calls when an error is detected, such as a bad
+   *                      checksum.
+   *
+   * @throw SerialException
+   */
+  VescInterface(const std::string& port = std::string(),
+                const PacketHandlerFunction& packet_handler = PacketHandlerFunction(),
+                const ErrorHandlerFunction& error_handler = ErrorHandlerFunction());
+
+  /**
+   * VescInterface destructor.
+   */
+  ~VescInterface();
+
+  /**
+   * Sets / updates the function that this class calls when a VESC packet is received.
+   */
+  void setPacketHandler(const PacketHandlerFunction& handler);
+
+  /**
+   * Sets / updates the function that this class calls when an error is detected, such as a bad
+   * checksum.
+   */
+  void setErrorHandler(const ErrorHandlerFunction& handler);
+
+  /**
+   * Opens the serial port interface to the VESC.
+   *
+   * @throw SerialException
+   */
+  void connect(const std::string& port);
+
+  /**
+   * Closes the serial port interface to the VESC.
+   */
+  void disconnect();
+
+  /**
+   * Gets the status of the serial interface to the VESC.
+   *
+   * @return Returns true if the serial port is open, false otherwise.
+   */
+  bool isConnected() const;
+
+  /**
+   * Send a VESC packet.
+   */
+  void send(const VescPacket& packet);
+
+  void requestFWVersion();
+  void requestState();
+  void setDutyCycle(double duty_cycle);
+  void setCurrent(double current);
+  void setBrake(double brake);
+  void setSpeed(double speed);
+  void setPosition(double position);
+  void setServo(double servo);
+
+private:
+  // Pimpl - hide serial port members from class users
+  class Impl;
+  boost::scoped_ptr<Impl> impl_;
+};
+
+// todo: review
+class SerialException : public std::exception
+{
+  // Disable copy constructors
+  SerialException& operator=(const SerialException&);
+  std::string e_what_;
+public:
+  SerialException (const char *description) {
+      std::stringstream ss;
+      ss << "SerialException " << description << " failed.";
+      e_what_ = ss.str();
+  }
+  SerialException (const SerialException& other) : e_what_(other.e_what_) {}
+  virtual ~SerialException() throw() {}
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+} // namespace vesc_driver
+
+#endif // VESC_DRIVER_VESC_INTERFACE_H_
diff --git a/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_packet.h b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_packet.h
new file mode 100644
index 00000000..630634e9
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_packet.h
@@ -0,0 +1,194 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#ifndef VESC_DRIVER_VESC_PACKET_H_
+#define VESC_DRIVER_VESC_PACKET_H_
+
+#include <string>
+#include <vector>
+#include <utility>
+
+#include <boost/crc.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include "vesc_driver/v8stdint.h"
+
+namespace vesc_driver
+{
+
+typedef std::vector<uint8_t> Buffer;
+typedef std::pair<Buffer::iterator, Buffer::iterator> BufferRange;
+typedef std::pair<Buffer::const_iterator, Buffer::const_iterator> BufferRangeConst;
+
+/** The raw frame for communicating with the VESC */
+class VescFrame
+{
+public:
+  virtual ~VescFrame() {}
+
+  // getters
+  virtual const Buffer& frame() const {return *frame_;}
+
+  // VESC packet properties
+  static const int VESC_MAX_PAYLOAD_SIZE = 1024;          ///< Maximum VESC payload size, in bytes
+  static const int VESC_MIN_FRAME_SIZE = 5;               ///< Smallest VESC frame size, in bytes
+  static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes
+  static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value
+  static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value
+  static const unsigned int VESC_EOF_VAL = 3;             ///< VESC end-of-frame value
+
+  /** CRC parameters for the VESC */
+  typedef boost::crc_optimal<16, 0x1021, 0, 0, false, false> CRC;
+
+protected:
+  /** Construct frame with specified payload size. */
+  VescFrame(int payload_size);
+
+  boost::shared_ptr<Buffer> frame_; ///< Stores frame data, shared_ptr for shallow copy
+  BufferRange payload_;             ///< View into frame's payload section
+
+private:
+  /** Construct from buffer. Used by VescPacketFactory factory. */
+  VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload);
+
+  /** Give VescPacketFactory access to private constructor. */
+  friend class VescPacketFactory;
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+/** A VescPacket is a VescFrame with a non-zero length payload */
+class VescPacket : public VescFrame
+{
+public:
+  virtual ~VescPacket() {}
+
+  virtual const std::string& name() const {return name_;}
+
+protected:
+  VescPacket(const std::string& name, int payload_size, int payload_id);
+  VescPacket(const std::string& name, boost::shared_ptr<VescFrame> raw);
+
+private:
+  std::string name_;
+};
+
+typedef boost::shared_ptr<VescPacket> VescPacketPtr;
+typedef boost::shared_ptr<VescPacket const> VescPacketConstPtr;
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketFWVersion : public VescPacket
+{
+public:
+  VescPacketFWVersion(boost::shared_ptr<VescFrame> raw);
+
+  int fwMajor() const;
+  int fwMinor() const;
+
+};
+
+class VescPacketRequestFWVersion : public VescPacket
+{
+public:
+  VescPacketRequestFWVersion();
+
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketValues : public VescPacket
+{
+public:
+  VescPacketValues(boost::shared_ptr<VescFrame> raw);
+
+  double v_in() const;
+  double temp_mos1() const;
+  double temp_mos2() const;
+  double temp_mos3() const;
+  double temp_mos4() const;
+  double temp_mos5() const;
+  double temp_mos6() const;
+  double temp_pcb() const;
+  double current_motor() const;
+  double current_in() const;
+  double rpm() const;
+  double duty_now() const;
+  double amp_hours() const;
+  double amp_hours_charged() const;
+  double watt_hours() const;
+  double watt_hours_charged() const;
+  double tachometer() const;
+  double tachometer_abs() const;
+  int fault_code() const;
+
+};
+
+class VescPacketRequestValues : public VescPacket
+{
+public:
+  VescPacketRequestValues();
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketSetDuty : public VescPacket
+{
+public:
+  VescPacketSetDuty(double duty);
+
+  //  double duty() const;
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketSetCurrent : public VescPacket
+{
+public:
+  VescPacketSetCurrent(double current);
+
+  //  double current() const;
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketSetCurrentBrake : public VescPacket
+{
+public:
+  VescPacketSetCurrentBrake(double current_brake);
+
+  //  double current_brake() const;
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketSetRPM : public VescPacket
+{
+public:
+  VescPacketSetRPM(double rpm);
+
+  //  double rpm() const;
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketSetPos : public VescPacket
+{
+public:
+  VescPacketSetPos(double pos);
+
+  //  double pos() const;
+};
+
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketSetServoPos : public VescPacket
+{
+public:
+  VescPacketSetServoPos(double servo_pos);
+
+  //  double servo_pos() const;
+};
+
+} // namespace vesc_driver
+
+#endif // VESC_DRIVER_VESC_PACKET_H_
diff --git a/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_packet_factory.h
new file mode 100644
index 00000000..f856f44c
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/include/vesc_driver/vesc_packet_factory.h
@@ -0,0 +1,82 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#ifndef VESC_DRIVER_VESC_PACKET_FACTORY_H_
+#define VESC_DRIVER_VESC_PACKET_FACTORY_H_
+
+#include <vector>
+#include <map>
+#include <string>
+
+#include <boost/noncopyable.hpp>
+#include <boost/function.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include "vesc_driver/v8stdint.h"
+#include "vesc_driver/vesc_packet.h"
+
+namespace vesc_driver
+{
+
+/**
+ * Class for creating VESC packets from raw data.
+ */
+class VescPacketFactory : private boost::noncopyable
+{
+public:
+  /** Return the global factory object */
+  static VescPacketFactory* getFactory();
+
+  /**
+   * Create a VescPacket from a buffer (factory function). Packet must start (start of frame
+   * character) at @p begin and complete (end of frame character) before *p end. The buffer element
+   * at @p end is not examined, i.e. it can be the past-the-end element. Only returns a packet if
+   * the packet is valid, i.e. valid size, matching checksum, complete etc. An empty pointer is
+   * returned if a packet cannot be found or if it is invalid. If a valid packet is not found,
+   * optional output parameter @what is set to a string providing a reason why a packet was not
+   * found. If a packet was not found because additional bytes are needed on the buffer, optional
+   * output parameter @p num_bytes_needed will contain the number of bytes needed to either
+   * determine the size of the packet or complete the packet. Output parameters @p num_bytes_needed
+   * and @p what will be set to 0 and empty if a valid packet is found.
+   *
+   * @param begin[in] Iterator to a buffer at the start-of-frame character
+   * @param end[in] Iterator to the buffer past-the-end element.
+   * @param num_bytes_needed[out] Number of bytes needed to determine the packet size or complete
+   *                              the frame.
+   * @param what[out] Human readable string giving a reason why the packet was not found.
+   *
+   * @return Pointer to a valid VescPacket if successful. Otherwise, an empty pointer.
+   */
+  static VescPacketPtr createPacket(const Buffer::const_iterator& begin,
+                                    const Buffer::const_iterator& end,
+                                    int* num_bytes_needed, std::string* what);
+
+  typedef boost::function<VescPacketPtr(boost::shared_ptr<VescFrame>)> CreateFn;
+
+  /** Register a packet type with the factory. */
+  static void registerPacketType(int payload_id, CreateFn fn);
+
+private:
+
+  typedef std::map<int, CreateFn > FactoryMap;
+  static FactoryMap* getMap();
+};
+
+/** Use this macro to register packets */
+#define REGISTER_PACKET_TYPE(id, klass)   \
+class klass##Factory \
+{ \
+public: \
+  klass##Factory() \
+  { \
+    VescPacketFactory::registerPacketType((id), &klass##Factory::create); \
+  } \
+  static VescPacketPtr create(boost::shared_ptr<VescFrame> frame) \
+  { \
+    return VescPacketPtr(new klass(frame)); \
+  } \
+}; \
+static klass##Factory global_##klass##Factory;
+
+} // namespace vesc_driver
+
+#endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_
diff --git a/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_node.launch b/rubis_ws/src/vesc_driver/launch/vesc_driver_node.launch
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_node.launch
rename to rubis_ws/src/vesc_driver/launch/vesc_driver_node.launch
diff --git a/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_nodelet.launch b/rubis_ws/src/vesc_driver/launch/vesc_driver_nodelet.launch
similarity index 100%
rename from rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_nodelet.launch
rename to rubis_ws/src/vesc_driver/launch/vesc_driver_nodelet.launch
diff --git a/rubis_ws/src/vesc_driver/package.xml b/rubis_ws/src/vesc_driver/package.xml
new file mode 100644
index 00000000..2ed45e3d
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/package.xml
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package>
+  <name>vesc_driver</name>
+  <version>0.0.1</version>
+  <description>
+    ROS device driver for the Vedder VESC open source motor driver.
+  </description>
+
+  <maintainer email="boulet@ll.mit.edu">Michael T. Boulet</maintainer>
+  <author email="boulet@ll.mit.edu">Michael T. Boulet</author>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/vesc_driver</url>
+  <url type="repository">https://github.mit.edu/racecar/vesc</url>
+  <url type="bugtracker">https://github.mit.edu/racecar/vesc/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>nodelet</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>vesc_msgs</build_depend>
+  <build_depend>serial</build_depend>
+
+  <run_depend>nodelet</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>vesc_msgs</run_depend>
+  <run_depend>serial</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/vesc_driver_nodelet.xml"/>
+  </export>
+
+</package>
diff --git a/rubis_ws/src/vesc_driver/src/vesc_driver.cpp b/rubis_ws/src/vesc_driver/src/vesc_driver.cpp
new file mode 100644
index 00000000..3c4788a4
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/src/vesc_driver.cpp
@@ -0,0 +1,304 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#include "vesc_driver/vesc_driver.h"
+
+#include <cassert>
+#include <cmath>
+#include <sstream>
+
+#include <boost/bind.hpp>
+#include <vesc_msgs/VescStateStamped.h>
+
+namespace vesc_driver
+{
+
+VescDriver::VescDriver(ros::NodeHandle nh,
+                       ros::NodeHandle private_nh) :
+  vesc_(std::string(),
+        boost::bind(&VescDriver::vescPacketCallback, this, _1),
+        boost::bind(&VescDriver::vescErrorCallback, this, _1)),
+  duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0), current_limit_(private_nh, "current"),
+  brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"),
+  position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0),
+  driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1)
+{
+  // get vesc serial port address
+  std::string port;
+  if (!private_nh.getParam("port", port)) {
+    ROS_FATAL("VESC communication port parameter required.");
+    ros::shutdown();
+    return;
+  }
+
+  // attempt to connect to the serial port
+  try {
+    vesc_.connect(port);
+  }
+  catch (SerialException e) {
+    ROS_FATAL("Failed to connect to the VESC, %s.", e.what());
+    ros::shutdown();
+    return;
+  }
+
+  // create vesc state (telemetry) publisher
+  state_pub_ = nh.advertise<vesc_msgs::VescStateStamped>("sensors/core", 10);
+
+  // since vesc state does not include the servo position, publish the commanded
+  // servo position as a "sensor"
+  servo_sensor_pub_ = nh.advertise<std_msgs::Float64>("sensors/servo_position_command", 10);
+
+  // subscribe to motor and servo command topics
+  duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10,
+                                 &VescDriver::dutyCycleCallback, this);
+  current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this);
+  brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this);
+  speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this);
+  position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this);
+  servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this);
+
+  // create a 50Hz timer, used for state machine & polling VESC telemetry
+  timer_ = nh.createTimer(ros::Duration(1.0/50.0), &VescDriver::timerCallback, this);
+}
+
+  /* TODO or TO-THINKABOUT LIST
+    - what should we do on startup? send brake or zero command?
+    - what to do if the vesc interface gives an error?
+    - check version number against know compatable?
+    - should we wait until we receive telemetry before sending commands?
+    - should we track the last motor command
+    - what to do if no motor command received recently?
+    - what to do if no servo command received recently?
+    - what is the motor safe off state (0 current?)
+    - what to do if a command parameter is out of range, ignore?
+    - try to predict vesc bounds (from vesc config) and command detect bounds errors
+  */
+
+void VescDriver::timerCallback(const ros::TimerEvent& event)
+{
+  // VESC interface should not unexpectedly disconnect, but test for it anyway
+  if (!vesc_.isConnected()) {
+    ROS_FATAL("Unexpectedly disconnected from serial port.");
+    timer_.stop();
+    ros::shutdown();
+    return;
+  }
+
+  /*
+   * Driver state machine, modes:
+   *  INITIALIZING - request and wait for vesc version
+   *  OPERATING - receiving commands from subscriber topics
+   */
+  if (driver_mode_ == MODE_INITIALIZING) {
+    // request version number, return packet will update the internal version numbers
+    vesc_.requestFWVersion();
+    if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) {
+      ROS_INFO("Connected to VESC with firmware version %d.%d",
+               fw_version_major_, fw_version_minor_);
+      driver_mode_ = MODE_OPERATING;
+    }
+  }
+  else if (driver_mode_ == MODE_OPERATING) {
+    // poll for vesc state (telemetry)
+    vesc_.requestState();
+  }
+  else {
+    // unknown mode, how did that happen?
+    assert(false && "unknown driver mode");
+  }
+}
+
+void VescDriver::vescPacketCallback(const boost::shared_ptr<VescPacket const>& packet)
+{
+  if (packet->name() == "Values") {
+    boost::shared_ptr<VescPacketValues const> values =
+      boost::dynamic_pointer_cast<VescPacketValues const>(packet);
+
+    vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped);
+    state_msg->header.stamp = ros::Time::now();
+    state_msg->state.voltage_input = values->v_in();
+    state_msg->state.temperature_pcb = values->temp_pcb();
+    state_msg->state.current_motor = values->current_motor();
+    state_msg->state.current_input = values->current_in();
+    state_msg->state.speed = values->rpm();
+    state_msg->state.duty_cycle = values->duty_now();
+    state_msg->state.charge_drawn = values->amp_hours();
+    state_msg->state.charge_regen = values->amp_hours_charged();
+    state_msg->state.energy_drawn = values->watt_hours();
+    state_msg->state.energy_regen = values->watt_hours_charged();
+    state_msg->state.displacement = values->tachometer();
+    state_msg->state.distance_traveled = values->tachometer_abs();
+    state_msg->state.fault_code = values->fault_code();
+
+    state_pub_.publish(state_msg);
+  }
+  else if (packet->name() == "FWVersion") {
+    boost::shared_ptr<VescPacketFWVersion const> fw_version =
+      boost::dynamic_pointer_cast<VescPacketFWVersion const>(packet);
+    // todo: might need lock here
+    fw_version_major_ = fw_version->fwMajor();
+    fw_version_minor_ = fw_version->fwMinor();
+  }
+}
+
+void VescDriver::vescErrorCallback(const std::string& error)
+{
+  ROS_ERROR("%s", error.c_str());
+}
+
+/**
+ * @param duty_cycle Commanded VESC duty cycle. Valid range for this driver is -1 to +1. However,
+ *                   note that the VESC may impose a more restrictive bounds on the range depending
+ *                   on its configuration, e.g. absolute value is between 0.05 and 0.95.
+ */
+void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle)
+{
+  if (driver_mode_ = MODE_OPERATING) {
+    vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data));
+  }
+}
+
+/**
+ * @param current Commanded VESC current in Amps. Any value is accepted by this driver. However,
+ *                note that the VESC may impose a more restrictive bounds on the range depending on
+ *                its configuration.
+ */
+void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current)
+{
+  if (driver_mode_ = MODE_OPERATING) {
+    vesc_.setCurrent(current_limit_.clip(current->data));
+  }
+}
+
+/**
+ * @param brake Commanded VESC braking current in Amps. Any value is accepted by this driver.
+ *              However, note that the VESC may impose a more restrictive bounds on the range
+ *              depending on its configuration.
+ */
+void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake)
+{
+  if (driver_mode_ = MODE_OPERATING) {
+    vesc_.setBrake(brake_limit_.clip(brake->data));
+  }
+}
+
+/**
+ * @param speed Commanded VESC speed in electrical RPM. Electrical RPM is the mechanical RPM
+ *              multiplied by the number of motor poles. Any value is accepted by this
+ *              driver. However, note that the VESC may impose a more restrictive bounds on the
+ *              range depending on its configuration.
+ */
+void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed)
+{
+  if (driver_mode_ = MODE_OPERATING) {
+    vesc_.setSpeed(speed_limit_.clip(speed->data));
+  }
+}
+
+/**
+ * @param position Commanded VESC motor position in radians. Any value is accepted by this driver.
+ *                 Note that the VESC must be in encoder mode for this command to have an effect.
+ */
+void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position)
+{
+  if (driver_mode_ = MODE_OPERATING) {
+    // ROS uses radians but VESC seems to use degrees. Convert to degrees.
+    double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI;
+    vesc_.setPosition(position_deg);
+  }
+}
+
+/**
+ * @param servo Commanded VESC servo output position. Valid range is 0 to 1.
+ */
+void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo)
+{
+  if (driver_mode_ = MODE_OPERATING) {
+    double servo_clipped(servo_limit_.clip(servo->data));
+    vesc_.setServo(servo_clipped);
+    // publish clipped servo value as a "sensor"
+    std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64);
+    servo_sensor_msg->data = servo_clipped;
+    servo_sensor_pub_.publish(servo_sensor_msg);
+  }
+}
+
+VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str,
+                                       const boost::optional<double>& min_lower,
+                                       const boost::optional<double>& max_upper) :
+  name(str)
+{
+  // check if user's minimum value is outside of the range min_lower to max_upper
+  double param_min;
+  if (nh.getParam(name + "_min", param_min)) {
+    if (min_lower && param_min < *min_lower) {
+      lower = *min_lower;
+      ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min <<
+                      ") is less than the feasible minimum (" << *min_lower << ").");
+    }
+    else if (max_upper && param_min > *max_upper) {
+      lower = *max_upper;
+      ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min <<
+                      ") is greater than the feasible maximum (" << *max_upper << ").");
+    }
+    else {
+      lower = param_min;
+    }
+  }
+  else if (min_lower) {
+    lower = *min_lower;
+  }
+
+  // check if the uers' maximum value is outside of the range min_lower to max_upper
+  double param_max;
+  if (nh.getParam(name + "_max", param_max)) {
+    if (min_lower && param_max < *min_lower) {
+      upper = *min_lower;
+      ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max <<
+                      ") is less than the feasible minimum (" << *min_lower << ").");
+    }
+    else if (max_upper && param_max > *max_upper) {
+      upper = *max_upper;
+      ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max <<
+                      ") is greater than the feasible maximum (" << *max_upper << ").");
+    }
+    else {
+      upper = param_max;
+    }
+  }
+  else if (max_upper) {
+    upper = *max_upper;
+  }
+
+  // check for min > max
+  if (upper && lower && *lower > *upper) {
+    ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper
+                    << ") is less than parameter " << name << "_min (" << *lower << ").");
+    double temp(*lower);
+    lower = *upper;
+    upper = temp;
+  }
+
+  std::ostringstream oss;
+  oss << "  " << name << " limit: ";
+  if (lower) oss << *lower << " "; else oss << "(none) ";
+  if (upper) oss << *upper; else oss << "(none)";
+  ROS_DEBUG_STREAM(oss.str());
+}
+
+double VescDriver::CommandLimit::clip(double value)
+{
+  if (lower && value < lower) {
+    ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.",
+                      name.c_str(), value, *lower);
+    return *lower;
+  }
+  if (upper && value > upper) {
+    ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.",
+                      name.c_str(), value, *upper);
+    return *upper;
+  }
+  return value;
+}
+
+
+} // namespace vesc_driver
diff --git a/rubis_ws/src/vesc_driver/src/vesc_driver_node.cpp b/rubis_ws/src/vesc_driver/src/vesc_driver_node.cpp
new file mode 100644
index 00000000..115191e2
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/src/vesc_driver_node.cpp
@@ -0,0 +1,16 @@
+#include <ros/ros.h>
+
+#include "vesc_driver/vesc_driver.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "vesc_driver_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  vesc_driver::VescDriver vesc_driver(nh, private_nh);
+
+  ros::spin();
+
+  return 0;
+}
diff --git a/rubis_ws/src/vesc_driver/src/vesc_driver_nodelet.cpp b/rubis_ws/src/vesc_driver/src/vesc_driver_nodelet.cpp
new file mode 100644
index 00000000..78c8d3ba
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/src/vesc_driver_nodelet.cpp
@@ -0,0 +1,33 @@
+#include <boost/shared_ptr.hpp>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "vesc_driver/vesc_driver.h"
+
+namespace vesc_driver
+{
+
+class VescDriverNodelet: public nodelet::Nodelet
+{
+public:
+
+  VescDriverNodelet() {}
+
+private:
+
+  virtual void onInit(void);
+
+  boost::shared_ptr<VescDriver> vesc_driver_;
+
+}; // class VescDriverNodelet
+
+void VescDriverNodelet::onInit()
+{
+  NODELET_DEBUG("Initializing VESC driver nodelet");
+  vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle()));
+}
+
+} // namespace vesc_driver
+
+PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet);
diff --git a/rubis_ws/src/vesc_driver/src/vesc_interface.cpp b/rubis_ws/src/vesc_driver/src/vesc_interface.cpp
new file mode 100644
index 00000000..84a2cf2c
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/src/vesc_interface.cpp
@@ -0,0 +1,244 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#include "vesc_driver/vesc_interface.h"
+
+#include <pthread.h>
+
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <iomanip>
+
+#include <serial/serial.h>
+#include <boost/crc.hpp>
+
+#include "vesc_driver/vesc_packet_factory.h"
+
+namespace vesc_driver
+{
+
+class VescInterface::Impl
+{
+public:
+  Impl() :
+    serial_(std::string(), 115200, serial::Timeout::simpleTimeout(100),
+            serial::eightbits, serial::parity_none, serial::stopbits_one, serial::flowcontrol_none)
+  {}
+
+  void* rxThread(void);
+
+  static void* rxThreadHelper(void *context)
+  {
+    return ((VescInterface::Impl*)context)->rxThread();
+  }
+
+  pthread_t rx_thread_;
+  bool rx_thread_run_;
+  PacketHandlerFunction packet_handler_;
+  ErrorHandlerFunction error_handler_;
+  serial::Serial serial_;
+  VescFrame::CRC send_crc_;
+};
+
+void* VescInterface::Impl::rxThread(void)
+{
+  Buffer buffer;
+  buffer.reserve(4096);
+
+  while (rx_thread_run_) {
+
+    int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE;
+    if (!buffer.empty()) {
+
+      // search buffer for valid packet(s)
+      Buffer::iterator iter(buffer.begin());
+      Buffer::iterator iter_begin(buffer.begin());
+      while (iter != buffer.end()) {
+
+        // check if valid start-of-frame character
+        if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter ||
+            VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) {
+
+          // good start, now attempt to create packet
+          std::string error;
+          VescPacketConstPtr packet =
+            VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error);
+          if (packet) {
+            // good packet, check if we skipped any data
+            if (std::distance(iter_begin, iter) > 0) {
+              std::ostringstream ss;
+              ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding "
+                 << std::distance(iter_begin, iter) << " bytes.";
+              error_handler_(ss.str());
+            }
+            // call packet handler
+            packet_handler_(packet);
+            // update state
+            iter = iter + packet->frame().size();
+            iter_begin = iter;
+            // continue to look for another frame in buffer
+            continue;
+          }
+          else if (bytes_needed > 0) {
+            // need more data, break out of while loop
+            break; // for (iter_sof...
+          }
+          else {
+            // else, this was not a packet, move on to next byte
+            error_handler_(error);
+          }
+        }
+
+        iter++;
+      }
+
+      // if iter is at the end of the buffer, more bytes are needed
+      if (iter == buffer.end())
+        bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE;
+
+      // erase "used" buffer
+      if (std::distance(iter_begin, iter) > 0) {
+        std::ostringstream ss;
+        ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes.";
+        error_handler_(ss.str());
+      }
+      buffer.erase(buffer.begin(), iter);
+    }
+
+    // attempt to read at least bytes_needed bytes from the serial port
+    int bytes_to_read =
+      std::max(bytes_needed, std::min(4096, static_cast<int>(serial_.available())));
+    int bytes_read = serial_.read(buffer, bytes_to_read);
+    if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) {
+      error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame.");
+    }
+
+  }
+}
+
+
+VescInterface::VescInterface(const std::string& port,
+                             const PacketHandlerFunction& packet_handler,
+                             const ErrorHandlerFunction& error_handler) :
+  impl_(new Impl())
+{
+  setPacketHandler(packet_handler);
+  setErrorHandler(error_handler);
+  // attempt to conect if the port is specified
+  if (!port.empty())
+    connect(port);
+}
+
+VescInterface::~VescInterface()
+{
+  disconnect();
+}
+
+void VescInterface::setPacketHandler(const PacketHandlerFunction& handler)
+{
+  // todo - definately need mutex
+  impl_->packet_handler_ = handler;
+}
+
+void VescInterface::setErrorHandler(const ErrorHandlerFunction& handler)
+{
+  // todo - definately need mutex
+  impl_->error_handler_ = handler;
+}
+
+void VescInterface::connect(const std::string& port)
+{
+  // todo - mutex?
+
+  if (isConnected()) {
+    throw SerialException("Already connected to serial port.");
+  }
+
+  // connect to serial port
+  try {
+    impl_->serial_.setPort(port);
+    impl_->serial_.open();
+  }
+  catch (const std::exception& e) {
+      std::stringstream ss;
+      ss << "Failed to open the serial port to the VESC. " << e.what();
+      throw SerialException(ss.str().c_str());
+  }
+
+  // start up a monitoring thread
+  impl_->rx_thread_run_ = true;
+  int result =
+    pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get());
+  assert(0 == result);
+}
+
+void VescInterface::disconnect()
+{
+  // todo - mutex?
+
+  if (isConnected()) {
+    // bring down read thread
+    impl_->rx_thread_run_ = false;
+    int result = pthread_join(impl_->rx_thread_, NULL);
+    assert(0 == result);
+
+    impl_->serial_.close();
+  }
+}
+
+bool VescInterface::isConnected() const
+{
+  return impl_->serial_.isOpen();
+}
+
+void VescInterface::send(const VescPacket& packet)
+{
+  size_t written = impl_->serial_.write(packet.frame());
+  if (written != packet.frame().size()) {
+    std::stringstream ss;
+    ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << ".";
+    throw SerialException(ss.str().c_str());
+  }
+}
+
+void VescInterface::requestFWVersion()
+{
+  send(VescPacketRequestFWVersion());
+}
+
+void VescInterface::requestState()
+{
+  send(VescPacketRequestValues());
+}
+
+void VescInterface::setDutyCycle(double duty_cycle)
+{
+  send(VescPacketSetDuty(duty_cycle));
+}
+
+void VescInterface::setCurrent(double current)
+{
+  send(VescPacketSetCurrent(current));
+}
+
+void VescInterface::setBrake(double brake)
+{
+  send(VescPacketSetCurrentBrake(brake));
+}
+
+void VescInterface::setSpeed(double speed)
+{
+  send(VescPacketSetRPM(speed));
+}
+
+void VescInterface::setPosition(double position)
+{
+  send(VescPacketSetPos(position));
+}
+
+void VescInterface::setServo(double servo)
+{
+  send(VescPacketSetServoPos(servo));
+}
+
+} // namespace vesc_driver
diff --git a/rubis_ws/src/vesc_driver/src/vesc_packet.cpp b/rubis_ws/src/vesc_driver/src/vesc_packet.cpp
new file mode 100644
index 00000000..80328a7e
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/src/vesc_packet.cpp
@@ -0,0 +1,368 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#include "vesc_driver/vesc_packet.h"
+
+#include <cassert>
+#include <iterator>
+
+#include <boost/range/begin.hpp>
+#include <boost/range/distance.hpp>
+#include <boost/range/end.hpp>
+
+#include "vesc_driver/vesc_packet_factory.h"
+#include "vesc_driver/datatypes.h"
+
+namespace vesc_driver
+{
+
+VescFrame::VescFrame(int payload_size)
+{
+  assert(payload_size >= 0 && payload_size <= 1024);
+
+  if (payload_size < 256) {
+    // single byte payload size
+    frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size));
+    *frame_->begin() = 2;
+    *(frame_->begin() + 1) = payload_size;
+    payload_.first = frame_->begin() + 2;
+  }
+  else {
+    // two byte payload size
+    frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size));
+    *frame_->begin() = 3;
+    *(frame_->begin() + 1) = payload_size >> 8;
+    *(frame_->begin() + 2) = payload_size & 0xFF;
+    payload_.first = frame_->begin() + 3;
+  }
+
+  payload_.second = payload_.first + payload_size;
+  *(frame_->end() - 1) = 3;
+}
+
+VescFrame::VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload)
+{
+  /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap
+     checks anyway */
+  assert(boost::distance(frame) >= VESC_MIN_FRAME_SIZE);
+  assert(boost::distance(frame) <= VESC_MAX_FRAME_SIZE);
+  assert(boost::distance(payload) <= VESC_MAX_PAYLOAD_SIZE);
+  assert(std::distance(frame.first, payload.first) > 0 &&
+         std::distance(payload.second, frame.second) > 0);
+
+  frame_.reset(new Buffer(boost::begin(frame), boost::end(frame)));
+  payload_.first = frame_->begin() + std::distance(frame.first, payload.first);
+  payload_.second = frame_->begin() + std::distance(frame.first, payload.second);
+}
+
+VescPacket::VescPacket(const std::string& name, int payload_size, int payload_id) :
+  VescFrame(payload_size), name_(name)
+{
+  assert(payload_id >= 0 && payload_id < 256);
+  assert(boost::distance(payload_) > 0);
+  *payload_.first = payload_id;
+}
+
+VescPacket::VescPacket(const std::string& name, boost::shared_ptr<VescFrame> raw) :
+  VescFrame(*raw), name_(name)
+{
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketFWVersion::VescPacketFWVersion(boost::shared_ptr<VescFrame> raw) :
+  VescPacket("FWVersion", raw)
+{
+}
+
+int VescPacketFWVersion::fwMajor() const
+{
+  return *(payload_.first + 1);
+}
+
+int VescPacketFWVersion::fwMinor() const
+{
+  return *(payload_.first + 2);
+}
+
+REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion)
+
+VescPacketRequestFWVersion::VescPacketRequestFWVersion() :
+  VescPacket("RequestFWVersion", 1, COMM_FW_VERSION)
+{
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketValues::VescPacketValues(boost::shared_ptr<VescFrame> raw) :
+  VescPacket("Values", raw)
+{
+}
+
+double VescPacketValues::temp_mos1() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 1)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 2)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::temp_mos2() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 3)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 4)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::temp_mos3() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 5)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 6)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::temp_mos4() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 7)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 8)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::temp_mos5() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 9)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 10)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::temp_mos6() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 11)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 12)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::temp_pcb() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 13)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 14)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::current_motor() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 15)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 16)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 17)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 18)));
+  return static_cast<double>(v) / 100.0;
+}
+double VescPacketValues::current_in() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 19)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 20)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 21)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 22)));
+  return static_cast<double>(v) / 100.0;
+}
+double VescPacketValues::duty_now() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 23)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 24)));
+  return static_cast<double>(v) / 1000.0;
+}
+double VescPacketValues::rpm() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 25)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 26)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 27)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 28)));
+  return static_cast<double>(v);
+}
+double VescPacketValues::v_in() const
+{
+  int16_t v = static_cast<int16_t>((static_cast<uint16_t>(*(payload_.first + 29)) << 8) +
+                                   static_cast<uint16_t>(*(payload_.first + 30)));
+  return static_cast<double>(v) / 10.0;
+}
+double VescPacketValues::amp_hours() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 31)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 32)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 33)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 34)));
+  return static_cast<double>(v);
+}
+double VescPacketValues::amp_hours_charged() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 35)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 36)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 37)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 38)));
+  return static_cast<double>(v);
+}
+double VescPacketValues::watt_hours() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 39)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 40)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 41)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 42)));
+  return static_cast<double>(v);
+}
+double VescPacketValues::watt_hours_charged() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 43)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 44)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 45)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 46)));
+  return static_cast<double>(v);
+}
+double VescPacketValues::tachometer() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 47)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 48)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 49)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 50)));
+  return static_cast<double>(v);
+}
+double VescPacketValues::tachometer_abs() const
+{
+  int32_t v = static_cast<int32_t>((static_cast<uint32_t>(*(payload_.first + 51)) << 24) +
+                                   (static_cast<uint32_t>(*(payload_.first + 52)) << 16) +
+                                   (static_cast<uint32_t>(*(payload_.first + 53)) << 8) +
+                                   static_cast<uint32_t>(*(payload_.first + 54)));
+  return static_cast<double>(v);
+}
+int VescPacketValues::fault_code() const
+{
+  return static_cast<int32_t>(*(payload_.first + 55));
+}
+
+REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues)
+
+VescPacketRequestValues::VescPacketRequestValues() :
+  VescPacket("RequestFWVersion", 1, COMM_GET_VALUES)
+{
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+
+VescPacketSetDuty::VescPacketSetDuty(double duty) :
+  VescPacket("SetDuty", 5, COMM_SET_DUTY)
+{
+  /** @todo range check duty */
+
+  int32_t v = static_cast<int32_t>(duty * 100000.0);
+
+  *(payload_.first + 1) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 24) & 0xFF);
+  *(payload_.first + 2) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 16) & 0xFF);
+  *(payload_.first + 3) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 8) & 0xFF);
+  *(payload_.first + 4) = static_cast<uint8_t>(static_cast<uint32_t>(v) & 0xFF);
+
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketSetCurrent::VescPacketSetCurrent(double current) :
+  VescPacket("SetCurrent", 5, COMM_SET_CURRENT)
+{
+  int32_t v = static_cast<int32_t>(current * 1000.0);
+
+  *(payload_.first + 1) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 24) & 0xFF);
+  *(payload_.first + 2) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 16) & 0xFF);
+  *(payload_.first + 3) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 8) & 0xFF);
+  *(payload_.first + 4) = static_cast<uint8_t>(static_cast<uint32_t>(v) & 0xFF);
+
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) :
+  VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE)
+{
+  int32_t v = static_cast<int32_t>(current_brake * 1000.0);
+
+  *(payload_.first + 1) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 24) & 0xFF);
+  *(payload_.first + 2) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 16) & 0xFF);
+  *(payload_.first + 3) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 8) & 0xFF);
+  *(payload_.first + 4) = static_cast<uint8_t>(static_cast<uint32_t>(v) & 0xFF);
+
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketSetRPM::VescPacketSetRPM(double rpm) :
+  VescPacket("SetRPM", 5, COMM_SET_RPM)
+{
+  int32_t v = static_cast<int32_t>(rpm);
+
+  *(payload_.first + 1) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 24) & 0xFF);
+  *(payload_.first + 2) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 16) & 0xFF);
+  *(payload_.first + 3) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 8) & 0xFF);
+  *(payload_.first + 4) = static_cast<uint8_t>(static_cast<uint32_t>(v) & 0xFF);
+
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketSetPos::VescPacketSetPos(double pos) :
+  VescPacket("SetPos", 5, COMM_SET_POS)
+{
+  /** @todo range check pos */
+
+  int32_t v = static_cast<int32_t>(pos * 1000000.0);
+
+  *(payload_.first + 1) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 24) & 0xFF);
+  *(payload_.first + 2) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 16) & 0xFF);
+  *(payload_.first + 3) = static_cast<uint8_t>((static_cast<uint32_t>(v) >> 8) & 0xFF);
+  *(payload_.first + 4) = static_cast<uint8_t>(static_cast<uint32_t>(v) & 0xFF);
+
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) :
+  VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS)
+{
+  /** @todo range check pos */
+
+  int16_t v = static_cast<int16_t>(servo_pos * 1000.0);
+
+  *(payload_.first + 1) = static_cast<uint8_t>((static_cast<uint16_t>(v) >> 8) & 0xFF);
+  *(payload_.first + 2) = static_cast<uint8_t>(static_cast<uint16_t>(v) & 0xFF);
+
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
+} // namespace vesc_driver
diff --git a/rubis_ws/src/vesc_driver/src/vesc_packet_factory.cpp b/rubis_ws/src/vesc_driver/src/vesc_packet_factory.cpp
new file mode 100644
index 00000000..470f9c52
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/src/vesc_packet_factory.cpp
@@ -0,0 +1,124 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#include "vesc_driver/vesc_packet_factory.h"
+
+#include <cassert>
+#include <iterator>
+
+#include <boost/range/begin.hpp>
+#include <boost/range/distance.hpp>
+#include <boost/range/end.hpp>
+
+#include "vesc_driver/vesc_packet.h"
+
+namespace vesc_driver
+{
+
+/** Construct map on first use */
+VescPacketFactory::FactoryMap* VescPacketFactory::getMap()
+{
+  static FactoryMap m;
+  return &m;
+}
+
+void VescPacketFactory::registerPacketType(int payload_id, CreateFn fn)
+{
+  FactoryMap* p_map(getMap());
+  assert(0 == p_map->count(payload_id));
+  (*p_map)[payload_id] = fn;
+}
+
+/** Helper function for when createPacket can not create a packet */
+VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what,
+                           const std::string& what, int num_bytes_needed = 0)
+{
+  if (p_num_bytes_needed != NULL) *p_num_bytes_needed = num_bytes_needed;
+  if (p_what != NULL) *p_what = what;
+  return VescPacketPtr();
+}
+
+VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin,
+                                              const Buffer::const_iterator& end,
+                                              int* num_bytes_needed, std::string* what)
+{
+  // initialize output variables
+  if (num_bytes_needed != NULL) *num_bytes_needed = 0;
+  if (what != NULL) what->clear();
+
+  // need at least VESC_MIN_FRAME_SIZE bytes in buffer
+  int buffer_size(std::distance(begin, end));
+  if (buffer_size < VescFrame::VESC_MIN_FRAME_SIZE)
+    return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame",
+                        VescFrame::VESC_MIN_FRAME_SIZE - buffer_size);
+
+  // buffer must begin with a start-of-frame
+  if (VescFrame::VESC_SOF_VAL_SMALL_FRAME != *begin &&
+      VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin)
+    return createFailed(num_bytes_needed, what, "Buffer must begin with start-of-frame character");
+
+  // get a view of the payload
+  BufferRangeConst view_payload;
+  if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) {
+    // payload size field is one byte
+    view_payload.first = begin + 2;
+    view_payload.second = view_payload.first + *(begin + 1);
+  }
+  else {
+    assert(VescFrame::VESC_SOF_VAL_LARGE_FRAME == *begin);
+    // payload size field is two bytes
+    view_payload.first = begin + 3;
+    view_payload.second = view_payload.first + (*(begin + 1) << 8) + *(begin + 2);
+  }
+
+  // check length
+  if (boost::distance(view_payload) > VescFrame::VESC_MAX_PAYLOAD_SIZE)
+    return createFailed(num_bytes_needed, what, "Invalid payload length");
+
+  // get iterators to crc field, end-of-frame field, and a view of the whole frame
+  Buffer::const_iterator iter_crc(view_payload.second);
+  Buffer::const_iterator iter_eof(iter_crc + 2);
+  BufferRangeConst view_frame(begin, iter_eof + 1);
+
+  // do we have enough data in the buffer to complete the frame?
+  int frame_size = boost::distance(view_frame);
+  if (buffer_size < frame_size)
+    return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame",
+                        frame_size - buffer_size);
+
+  // is the end-of-frame character valid?
+  if (VescFrame::VESC_EOF_VAL != *iter_eof)
+    return createFailed(num_bytes_needed, what, "Invalid end-of-frame character");
+
+  // is the crc valid?
+  unsigned short crc = (static_cast<unsigned short>(*iter_crc) << 8) + *(iter_crc + 1);
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*view_payload.first), boost::distance(view_payload));
+  if (crc != crc_calc.checksum())
+    return createFailed(num_bytes_needed, what, "Invalid checksum");
+
+  // frame looks good, construct the raw frame
+  boost::shared_ptr<VescFrame> raw_frame(new VescFrame(view_frame, view_payload));
+
+  // if the packet has a payload, construct the corresponding subclass
+  if (boost::distance(view_payload) > 0) {
+
+    // get constructor function from payload id
+    FactoryMap* p_map(getMap());
+    FactoryMap::const_iterator search(p_map->find(*view_payload.first));
+    if (search != p_map->end()) {
+      return search->second(raw_frame);
+    }
+    else {
+      // no subclass constructor for this packet
+      return createFailed(num_bytes_needed, what, "Unkown payload type.");
+    }
+
+  }
+  else {
+    // no payload
+    return createFailed(num_bytes_needed, what, "Frame does not have a payload");
+  }
+}
+
+
+} // namesapce vesc_driver
diff --git a/rubis_ws/src/vesc_driver/vesc_driver_nodelet.xml b/rubis_ws/src/vesc_driver/vesc_driver_nodelet.xml
new file mode 100644
index 00000000..5660e73a
--- /dev/null
+++ b/rubis_ws/src/vesc_driver/vesc_driver_nodelet.xml
@@ -0,0 +1,5 @@
+<library path="lib/libvesc_driver_nodelet">
+  <class type="vesc_driver::VescDriverNodelet" base_class_type="nodelet::Nodelet">
+    <description>Vedder VESC motor controller interface nodelet.</description>
+  </class>
+</library>
diff --git a/rubis_ws/src/vesc_msgs/CMakeLists.txt b/rubis_ws/src/vesc_msgs/CMakeLists.txt
new file mode 100644
index 00000000..d1365bd0
--- /dev/null
+++ b/rubis_ws/src/vesc_msgs/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vesc_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  std_msgs
+  message_generation
+)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  VescState.msg
+  VescStateStamped.msg
+)
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS std_msgs message_runtime
+)
diff --git a/rubis_ws/src/vesc_msgs/msg/VescState.msg b/rubis_ws/src/vesc_msgs/msg/VescState.msg
new file mode 100644
index 00000000..276d62ac
--- /dev/null
+++ b/rubis_ws/src/vesc_msgs/msg/VescState.msg
@@ -0,0 +1,24 @@
+# Vedder VESC open source motor controller state (telemetry)
+
+# fault codes
+int32 FAULT_CODE_NONE=0
+int32 FAULT_CODE_OVER_VOLTAGE=1
+int32 FAULT_CODE_UNDER_VOLTAGE=2
+int32 FAULT_CODE_DRV8302=3
+int32 FAULT_CODE_ABS_OVER_CURRENT=4
+int32 FAULT_CODE_OVER_TEMP_FET=5
+int32 FAULT_CODE_OVER_TEMP_MOTOR=6
+
+float64 voltage_input        # input voltage (volt)
+float64 temperature_pcb      # temperature of printed circuit board (degrees Celsius)
+float64 current_motor        # motor current (ampere)
+float64 current_input        # input current (ampere)
+float64 speed                # motor electrical speed (revolutions per minute) 
+float64 duty_cycle           # duty cycle (0 to 1)
+float64 charge_drawn         # electric charge drawn from input (ampere-hour)
+float64 charge_regen         # electric charge regenerated to input (ampere-hour)
+float64 energy_drawn         # energy drawn from input (watt-hour)
+float64 energy_regen         # energy regenerated to input (watt-hour)
+float64 displacement         # net tachometer (counts)
+float64 distance_traveled    # total tachnometer (counts)
+int32   fault_code
diff --git a/rubis_ws/src/vesc_msgs/msg/VescStateStamped.msg b/rubis_ws/src/vesc_msgs/msg/VescStateStamped.msg
new file mode 100644
index 00000000..086d5b81
--- /dev/null
+++ b/rubis_ws/src/vesc_msgs/msg/VescStateStamped.msg
@@ -0,0 +1,4 @@
+# Timestamped VESC open source motor controller state (telemetry)
+
+Header  header
+VescState state
\ No newline at end of file
diff --git a/rubis_ws/src/vesc_msgs/package.xml b/rubis_ws/src/vesc_msgs/package.xml
new file mode 100644
index 00000000..fa82a73c
--- /dev/null
+++ b/rubis_ws/src/vesc_msgs/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package>
+  <name>vesc_msgs</name>
+  <version>0.0.1</version>
+  <description>
+    ROS message definitions for the Vedder VESC open source motor controller.
+  </description>
+
+  <maintainer email="boulet@ll.mit.edu">Michael T. Boulet</maintainer>
+  <author email="boulet@ll.mit.edu">Michael T. Boulet</author>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/vesc_msgs</url>
+  <url type="repository">https://github.mit.edu/racecar/vesc</url>
+  <url type="bugtracker">https://github.mit.edu/racecar/vesc/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+</package>
diff --git a/rubis_ws/src/vision_workloads/CMakeLists.txt b/rubis_ws/src/vision_workloads/CMakeLists.txt
new file mode 100644
index 00000000..fb590b61
--- /dev/null
+++ b/rubis_ws/src/vision_workloads/CMakeLists.txt
@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vision_workloads)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roscpp  
+  autoware_msgs  
+  rubis_lib
+  rubis_msgs
+  cv_bridge
+)
+find_package(OpenCV REQUIRED)
+
+find_package(OpenCL REQUIRED)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES rubis_pkg
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+add_executable(lane_detector
+  src/lane_detector/lane_detector_node.cpp
+  src/lane_detector/lane_detector.cpp
+)
+add_dependencies(lane_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(lane_detector
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  OpenCL::OpenCL
+)
+
+install(
+  TARGETS
+  lane_detector
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
diff --git a/rubis_ws/src/vision_workloads/include/lane_detector.hpp b/rubis_ws/src/vision_workloads/include/lane_detector.hpp
new file mode 100644
index 00000000..8f390182
--- /dev/null
+++ b/rubis_ws/src/vision_workloads/include/lane_detector.hpp
@@ -0,0 +1,134 @@
+#ifndef LANE_DETECTOR_H
+#define LANE_DETECTOR_H
+
+#include <cmath>
+#include <CL/cl.h>
+#include "regression.hpp"
+#include <ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+#include <rubis_lib/sched.hpp>
+#include <rubis_msgs/Image.h>
+#include <rubis_msgs/Bool.h>
+#include <chrono>
+
+using namespace cv;
+
+void printOpenCLInfo();
+Mat getImage();
+
+class LaneDetector
+{
+public:
+    LaneDetector();
+    ~LaneDetector();
+    void run();
+private:
+    cl_platform_id platform;
+    cl_device_id device;
+    cl_context context;
+    cl_command_queue queue;
+    cl_program program;
+    cl_kernel kernel;
+    cl_mem inputImageBuffer;
+    cl_mem outputImageBuffer;
+    cl_kernel sobelGradientKernel;  // Sobel 커널
+    cl_mem gradientImageBuffer;     // Gradient 결과 저장 버퍼
+    cl_mem gradientDirectionBuffer; // Gradient 방향 저장 버퍼
+
+    bool debug_, filter_image_, gray_scale_, gaussian_blur_, canny_, roi_, opencl_;
+    ros::NodeHandle nh_;
+    ros::Publisher lane_pub_;
+    ros::Subscriber image_sub_; 
+
+    void imageCallback(const rubis_msgs::Image& msg);
+
+    /**
+     * @brief Apply grayscale transform on image.
+     * 
+     * @param source image that needs to be transformed
+     * @return grayscale image
+     */
+    Mat applyGrayscale(Mat source);
+
+    Mat applyOpenCLGrayscale(Mat source);
+    void initializeOpenCL();
+    void releaseOpenCL();
+
+    /**
+     * @brief Apply Gaussian blur to image.
+     * 
+     * @param source image that needs to be blurred
+     * @return blurred image
+     */
+    Mat applyGaussianBlur(Mat source);
+
+    /**
+     * @brief Detect edges of image by applying canny edge detection.
+     * 
+     * @param source image of which the edges needs to be detected
+     * @return image with detected edges
+     */
+    Mat applyCanny(Mat source);
+    Mat applyOpenCLCanny(Mat source);
+
+    /**
+     * @brief Filter source image so that only the white and yellow pixels remain.
+     * A gray filter will also be added if the source image is classified as taken during the night.
+     * One assumption for lane detection here is that lanes are either white or yellow.
+     * 
+     * @param source source image
+     * @param isDayTime true if image is taken during the day, false if at night
+     * @see isDayTime
+     * @return Mat filtered image
+     */
+    Mat filterColors(Mat source, bool isDayTime);
+
+    /**
+     * @brief Apply an image mask. 
+     * Only keep the part of the image defined by the
+     * polygon formed from four points. The rest of the image is set to black.
+     * 
+     * @param source image on which to apply the mask
+     * @return Mat image with mask
+     */
+    Mat RegionOfInterest(Mat source);
+
+    /**
+     * @brief Creates mask and blends it with source image so that the lanes
+     * are drawn on the source image.
+     * 
+     * @param source source image
+     * @param lines vector < vec4i > holding the lines
+     * @return Mat image with lines drawn on it
+     */
+    Mat drawLanes(Mat source, std::vector<Vec4i> lines);
+
+    /**
+     * @brief Returns a vector with the detected hough lines.
+     * 
+     * @param canny image resulted from a canny transform
+     * @param source image on which hough lines are drawn
+     * @param drawHough draw detected lines on source image if true. 
+     * It will also show the image with de lines drawn on it, which is why
+     * it is not recommended to pass in true when working with a video. 
+     * @see applyCanny
+     * @return vector<Vec4i> contains hough lines.
+     */
+    std::vector<Vec4i> houghLines(Mat canny, Mat source, bool drawHough);
+
+    /**
+     * @brief Determine whether a picture is taken during day or night time.
+     * Returns true when the image is classified as daytime. 
+     * Note: this is based on the mean pixel value of an image and might not
+     * always lead to accurate results.
+     * 
+     * @param source image
+     * @return true 
+     * @return false 
+     */
+    bool isDayTime(Mat source);
+};
+
+
+
+#endif
\ No newline at end of file
diff --git a/rubis_ws/src/vision_workloads/include/regression.hpp b/rubis_ws/src/vision_workloads/include/regression.hpp
new file mode 100644
index 00000000..6ee1ac22
--- /dev/null
+++ b/rubis_ws/src/vision_workloads/include/regression.hpp
@@ -0,0 +1,71 @@
+/*                                  
+ * regression.h
+ *
+ * Created on: 11 July 2022
+ * Author: EtoileScintillante
+ */
+
+#ifndef __REGRESSION__
+#define __REGRESSION__
+
+#include <vector>
+#include <numeric>
+#include <opencv2/opencv.hpp>
+
+/**
+ * @brief Multiplies two vectors and then calculates the sum of the multiplied values.
+ * vector A and B must be the same size and their values must be of the same type.
+ * 
+ * @param A vector<T>.
+ * @param B vector<T>.
+ * @return X sum of the multiplied values.
+ */
+template <typename T, typename X>
+X multiplyAndSum(std::vector<T> A, std::vector<T> B)
+{
+    X sum;
+    std::vector<T> temp;
+    for (int i = 0; i < A.size(); i++)
+    {
+        temp.push_back(A[i]*B[i]);
+    }
+    sum = std::accumulate(temp.begin(), temp.end(), 0.0);
+
+    return sum;
+}
+
+/**
+ * @brief Calculates the coefficients (slope and intercept) of the best fitting line
+ * given independent and dependent values. Vector A and B must be the same size 
+ * and their values must be of the same type.
+ * 
+ * @param A vector<T> (independent values).
+ * @param B vector<T> (dependent values).
+ * @return vector<X> where first element is the slope (b1), second element is intercept (b0).
+ */
+template <typename T, typename X>
+std::vector< X > estimateCoefficients(std::vector<T> A, std::vector<T> B)
+{
+    // Sample size
+    int N = A.size();
+
+    // Calculate mean of X and Y
+    X meanA= std::accumulate(A.begin(), A.end(), 0.0) / A.size();
+    X meanB = std::accumulate(B.begin(), B.end(), 0.0) / B.size();
+
+    // Calculating cross-deviation and deviation about x
+    X SSxy = multiplyAndSum<T, T>(A, B) - (N * meanA * meanB);
+    X SSxx = multiplyAndSum<T, T>(A, A) - (N * meanA * meanA);
+
+    // Calculating regression coefficients
+    X slopeB1 = SSxy / SSxx;
+    X interceptB0 = meanB - (slopeB1 * meanA);
+
+    // Return vector, insert slope first and then intercept
+    std::vector< X > coef;
+    coef.push_back(slopeB1);
+    coef.push_back(interceptB0);
+    return coef;
+}
+
+#endif /*__REGRESSION__*/
\ No newline at end of file
diff --git a/rubis_ws/src/vision_workloads/launch/lane_detector.launch b/rubis_ws/src/vision_workloads/launch/lane_detector.launch
new file mode 100644
index 00000000..e4fa4b5e
--- /dev/null
+++ b/rubis_ws/src/vision_workloads/launch/lane_detector.launch
@@ -0,0 +1,22 @@
+<launch>    
+    <arg name="debug" default="false" />
+    <arg name="node_name" default="lane_detector" />
+    <arg name="input_topic" default="/svl_image_raw"/>
+    <arg name="filter_image" default="true" />
+    <arg name="gray_scale" default="true" />
+    <arg name="gaussian_blur" default="true" />
+    <arg name="canny" default="true" />
+    <arg name="roi" default="true" />
+    <arg name="opencl" default="false" />
+    
+    <node pkg="vision_workloads" type="lane_detector" name="$(arg node_name)" output="screen">
+        <param name="input_topic" value="$(arg input_topic)" />
+        <param name="debug" value="$(arg debug)" />
+        <param name="filter_image" value="$(arg filter_image)" />
+        <param name="gray_scale" value="$(arg gray_scale)" />
+        <param name="gaussian_blur" value="$(arg gaussian_blur)" />
+        <param name="canny" value="$(arg canny)" />
+        <param name="roi" value="$(arg roi)" />
+        <param name="opencl" value="$(arg opencl)" />
+    </node>
+</launch>
diff --git a/rubis_ws/src/vision_workloads/package.xml b/rubis_ws/src/vision_workloads/package.xml
new file mode 100644
index 00000000..897f4d41
--- /dev/null
+++ b/rubis_ws/src/vision_workloads/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>vision_workloads</name>
+  <version>0.0.0</version>
+  <description>Vision workloads</description>
+  
+  <maintainer email="kite9240@gmail.com">Hayeon Park</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <export>
+  </export>
+  <build_depend>rospy</build_depend>
+  <build_depend>autoware_msgs</build_depend>
+  <build_depend>rubis_msgs</build_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>autoware_msgs</build_export_depend>
+  <build_export_depend>rubis_msgs</build_export_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>autoware_msgs</exec_depend>
+  <exec_depend>rubis_msgs</exec_depend>
+  <depend>roscpp</depend>
+  <depend>rubis_lib</depend>
+  <depend>libopencv-dev</depend>
+  <depend>cv_bridge</depend>
+</package>
\ No newline at end of file
diff --git a/rubis_ws/src/vision_workloads/src/lane_detector/lane_detector.cpp b/rubis_ws/src/vision_workloads/src/lane_detector/lane_detector.cpp
new file mode 100644
index 00000000..c9d0f5b6
--- /dev/null
+++ b/rubis_ws/src/vision_workloads/src/lane_detector/lane_detector.cpp
@@ -0,0 +1,604 @@
+#include "lane_detector.hpp"
+
+#include <CL/cl.h>
+#include <opencv2/opencv.hpp>
+#include <iostream>
+#include <chrono>
+
+#define CL_TARGET_OPENCL_VERSION 120
+
+#define MAX_WIDTH 1920
+#define MAX_HEIGHT 1080
+#define MAX_IMAGE_SIZE (MAX_WIDTH * MAX_HEIGHT * 3)  // 3채널 BGR 이미지 크기
+#define MAX_OUTPUT_SIZE (MAX_WIDTH * MAX_HEIGHT)     // 그레이스케일 이미지 크기
+
+const char* kernelSource = 
+"__kernel void grayscale(__global uchar* inputImage, __global uchar* outputImage, int width, int height) {"
+"    int x = get_global_id(0);"
+"    int y = get_global_id(1);"
+"    int idx = (y * width + x) * 3;"
+"    int gray_idx = y * width + x;"
+"    uchar r = inputImage[idx];"
+"    uchar g = inputImage[idx + 1];"
+"    uchar b = inputImage[idx + 2];"
+"    outputImage[gray_idx] = (uchar)(0.299f * r + 0.587f * g + 0.114f * b);"
+"}";
+
+LaneDetector::LaneDetector() {
+    if(opencl_) initializeOpenCL();
+
+    ros::NodeHandle private_nh("~");
+    std::string node_name = ros::this_node::getName();
+    std::string input_topic;
+    std::string output_topic;
+    output_topic = node_name + std::string("_lane");
+
+    private_nh.param<std::string>(node_name + "/input_topic", input_topic, "/svl_image_raw");
+    private_nh.param<bool>(node_name + "/debug", debug_, false);
+    private_nh.param<bool>(node_name + "/filter_image", filter_image_, false);
+    private_nh.param<bool>(node_name + "/gray_scale", gray_scale_, false);
+    private_nh.param<bool>(node_name + "/gaussian_blur", gaussian_blur_, false);
+    private_nh.param<bool>(node_name + "/canny", canny_, false);
+    private_nh.param<bool>(node_name + "/roi", roi_, false);
+    private_nh.param<bool>(node_name + "/opencl", opencl_, false);
+    lane_pub_ = nh_.advertise<rubis_msgs::Bool>(output_topic.c_str(), 1);
+    image_sub_ =
+        nh_.subscribe(input_topic, 1, &LaneDetector::imageCallback, this);
+
+    return;
+}
+
+LaneDetector::~LaneDetector() {
+    if(opencl_) releaseOpenCL();
+}
+
+Mat getImage() {
+    static Mat smallImage(64, 64, CV_8UC3);
+
+    static bool initialized = false;
+    if (!initialized) {
+        smallImage.at<Vec3b>(0, 0) = Vec3b(255, 0, 0);    
+        smallImage.at<Vec3b>(0, 1) = Vec3b(0, 255, 0);    
+        smallImage.at<Vec3b>(1, 0) = Vec3b(0, 0, 255);    
+        smallImage.at<Vec3b>(1, 1) = Vec3b(255, 255, 255);
+        initialized = true;
+    }
+
+    return smallImage;
+}
+
+void LaneDetector::initializeOpenCL() {
+    cl_int err;
+
+    // 플랫폼 및 장치 검색
+    err = clGetPlatformIDs(2, &platform, NULL);
+    err = clGetDeviceIDs(platform, CL_DEVICE_TYPE_GPU, 1, &device, NULL);
+
+    // 컨텍스트 및 명령 큐 생성
+    context = clCreateContext(NULL, 1, &device, NULL, NULL, &err);
+    queue = clCreateCommandQueueWithProperties(context, device, 0, &err);
+
+    // 프로그램 및 커널 생성
+    program = clCreateProgramWithSource(context, 1, (const char**)&kernelSource, NULL, &err);
+    clBuildProgram(program, 1, &device, NULL, NULL, NULL);
+    kernel = clCreateKernel(program, "grayscale", &err);
+
+    inputImageBuffer = clCreateBuffer(context, CL_MEM_READ_ONLY, MAX_IMAGE_SIZE, NULL, &err);
+    outputImageBuffer = clCreateBuffer(context, CL_MEM_WRITE_ONLY, MAX_OUTPUT_SIZE, NULL, &err);
+}
+
+void LaneDetector::releaseOpenCL() {
+    // OpenCL 자원 해제
+    clReleaseMemObject(inputImageBuffer);
+    clReleaseMemObject(outputImageBuffer);
+    clReleaseKernel(kernel);
+    clReleaseProgram(program);
+    clReleaseCommandQueue(queue);
+    clReleaseContext(context);
+}
+
+void LaneDetector::run() {
+    ros::NodeHandle private_nh("~");
+    std::string node_name = ros::this_node::getName();
+    std::string task_response_time_filename;
+    private_nh.param<std::string>(
+        node_name + "/task_response_time_filename", task_response_time_filename,
+        "~/Documents/profiling/response_time/lane_detector.csv");
+
+    struct rubis::sched_attr attr;
+    std::string policy;
+    int priority, exec_time, deadline, period;
+
+    private_nh.param(node_name + "/task_scheduling_configs/policy", policy,
+                     std::string("NONE"));
+    private_nh.param(node_name + "/task_scheduling_configs/priority", priority,
+                     99);
+    private_nh.param(node_name + "/task_scheduling_configs/exec_time",
+                     exec_time, 0);
+    private_nh.param(node_name + "/task_scheduling_configs/deadline", deadline,
+                     0);
+    private_nh.param(node_name + "/task_scheduling_configs/period", period, 0);
+
+    attr = rubis::create_sched_attr(priority, exec_time, deadline, period);
+    rubis::init_task_scheduling(policy, attr);
+    rubis::init_task_profiling(task_response_time_filename);
+
+    while (ros::ok()) {
+        ros::spinOnce();
+    }
+
+    return;
+}
+
+void LaneDetector::imageCallback(const rubis_msgs::Image &image) {
+    rubis::start_task_profiling();
+
+    rubis::instance_ = image.instance;
+    rubis::vision_instance_ = image.instance;
+
+    cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image.msg, "bgr8");
+    Mat frame = cv_image->image;
+
+    // Determine if video is taken during daytime or not
+    bool isDay = isDayTime(frame);
+
+    // Filter image
+    Mat filteredIMG;
+    if (filter_image_)
+        filteredIMG = filterColors(frame, isDay);
+    else
+        filteredIMG = frame;
+
+    // Apply grayscale
+    Mat gray;
+    if (gray_scale_){
+        gray = applyGrayscale(frame);
+        if(opencl_) applyOpenCLGrayscale(getImage());
+    }
+    else{
+        gray = filteredIMG;
+    }
+
+    // Apply Gaussian blur
+    Mat gBlur;
+    if (gaussian_blur_)
+        gBlur = applyGaussianBlur(gray);
+    else
+        gBlur = gray;
+
+    // Find edges
+    Mat edges;
+    if (canny_)
+        edges = applyCanny(gBlur);
+    else
+        edges = gBlur;
+
+    // Create mask (Region of Interest)
+    Mat maskedIMG;
+    if (roi_)
+        maskedIMG = RegionOfInterest(edges);
+    else
+        maskedIMG = edges;
+
+    // Detect straight lines and draw the lanes if possible
+    std::vector<Vec4i> linesP = houghLines(maskedIMG, frame.clone(), false);
+
+    // Draw lines
+    Mat lanes = drawLanes(frame, linesP);
+
+    // cv::resize(lanes, lanes, cv::Size(640, 480));
+    // imshow("Lanes", lanes);
+    // waitKey(1);
+
+    rubis_msgs::Bool lane_msg;
+    lane_msg.header = image.header;
+    lane_msg.instance = rubis::instance_;
+    lane_pub_.publish(lane_msg);
+
+    rubis::stop_task_profiling(rubis::instance_++, rubis::lidar_instance_++,
+                               rubis::vision_instance_++);
+}
+
+Mat LaneDetector::applyGrayscale(Mat source) {
+    Mat dst;
+    cvtColor(source, dst, COLOR_BGR2GRAY);
+
+    return dst;
+}
+
+Mat LaneDetector::applyOpenCLGrayscale(Mat source) {
+    cl_int err;
+    
+    // 이미지 크기 설정
+    int width = source.cols;
+    int height = source.rows;
+    size_t imageSize = width * height * 3 * sizeof(unsigned char);
+    size_t outputSize = width * height * sizeof(unsigned char);
+
+    // 입력 이미지를 GPU로 비동기적으로 복사
+    cl_event writeEvent;
+    clEnqueueWriteBuffer(queue, inputImageBuffer, CL_FALSE, 0, imageSize, source.data, 0, NULL, &writeEvent);
+
+    // 커널 매개변수 설정
+    clSetKernelArg(kernel, 0, sizeof(cl_mem), &inputImageBuffer);
+    clSetKernelArg(kernel, 1, sizeof(cl_mem), &outputImageBuffer);
+    clSetKernelArg(kernel, 2, sizeof(int), &width);
+    clSetKernelArg(kernel, 3, sizeof(int), &height);
+
+    // 글로벌 및 로컬 작업 크기 설정
+    size_t globalSize[2] = {width, height};
+    size_t localSize[2] = {16, 16};  // 워크 그룹 크기
+
+    // 커널 실행 (이벤트 기반 비동기 실행)
+    cl_event kernelEvent;
+    err = clEnqueueNDRangeKernel(queue, kernel, 2, NULL, globalSize, localSize, 1, &writeEvent, &kernelEvent);
+
+    // GPU 연산이 완료된 후 결과를 비동기적으로 CPU로 복사
+    unsigned char* outputImage = (unsigned char*)malloc(outputSize);
+    cl_event readEvent;
+    clEnqueueReadBuffer(queue, outputImageBuffer, CL_FALSE, 0, outputSize, outputImage, 1, &kernelEvent, &readEvent);
+
+    // GPU 연산이 끝나기를 기다림 (이벤트 기반 비동기 처리)
+    clWaitForEvents(1, &readEvent);
+
+    // OpenCV의 Mat 형식으로 출력 이미지 저장
+    Mat grayImage = Mat(height, width, CV_8UC1, outputImage);
+
+    // 이벤트 해제 (clean-up)
+    clReleaseEvent(writeEvent);
+    clReleaseEvent(kernelEvent);
+    clReleaseEvent(readEvent);
+
+    // 더 이상 메모리 해제가 필요하지 않으면 free 제거 가능
+    free(outputImage);
+
+    return grayImage;
+}
+
+Mat LaneDetector::applyGaussianBlur(Mat source) {
+    Mat dst;
+    GaussianBlur(source, dst, Size(3, 3), 0);
+
+    return dst;
+}
+
+Mat LaneDetector::applyCanny(Mat source) {
+    Mat dst;
+    Canny(source, dst, 50, 150);
+
+    return dst;
+}
+
+Mat LaneDetector::filterColors(Mat source, bool isDayTime) {
+    Mat hsv, whiteMask, whiteImage, yellowMask, yellowImage, whiteYellow;
+
+    // White mask
+    std::vector<int> lowerWhite = {130, 130, 130};
+    std::vector<int> upperWhite = {255, 255, 255};
+    inRange(source, lowerWhite, upperWhite, whiteMask);
+    bitwise_and(source, source, whiteImage, whiteMask);
+
+    // Yellow mask
+    cvtColor(source, hsv, COLOR_BGR2HSV);
+    std::vector<int> lowerYellow = {20, 100, 110};
+    std::vector<int> upperYellow = {30, 180, 240};
+    inRange(hsv, lowerYellow, upperYellow, yellowMask);
+    bitwise_and(source, source, yellowImage, yellowMask);
+
+    // Blend yellow and white together
+    addWeighted(whiteImage, 1., yellowImage, 1., 0., whiteYellow);
+
+    // Add gray filter if image is not taken during the day
+    if (isDayTime == false) {
+        // Gray mask
+        Mat grayMask, grayImage, grayAndWhite, dst;
+        std::vector<int> lowerGray = {80, 80, 80};
+        std::vector<int> upperGray = {130, 130, 130};
+        inRange(source, lowerGray, upperGray, grayMask);
+        bitwise_and(source, source, grayImage, grayMask);
+
+        // Blend gray, yellow and white together and return the result
+        addWeighted(grayImage, 1., whiteYellow, 1., 0., dst);
+        return dst;
+    }
+
+    // Return white and yellow mask if image is taken during the day
+    return whiteYellow;
+}
+
+Mat LaneDetector::RegionOfInterest(Mat source) {
+    /* In an ideal situation, the ROI should only contain the road lanes.
+    We want to filter out all the other stuff, including things like arrow road
+    markings. We try to achieve that by creating two trapezoid masks: one big
+    trapezoid and a smaller one. The smaller one goes inside the bigger one. The
+    pixels in the space between them will be kept and all the other pixels will
+    be masked. If it goes well, the space between the two trapezoids contains
+    only the lanes. */
+
+    // Parameters big trapezoid
+    float trapezoidBottomWidth = 1.0; // Width of bottom edge of trapezoid,
+                                      // expressed as percentage of image width
+    float trapezoidTopWidth = 0.07; // Above comment also applies here, but then
+                                    // for the top edge of trapezoid
+    float trapezoidHeight =
+        0.5; // Height of the trapezoid expressed as percentage of image height
+
+    // Parameters small trapezoid
+    float smallBottomWidth = 0.45; // This will be added to trapezoidBottomWidth
+                                   // to create a less wide bottom edge
+    float smallTopWidth =
+        0.3; // We multiply the percentage trapoezoidTopWidth with this
+             // parameter to create a less wide top edge
+    float smallHeight = 1.0; // Height of the small trapezoid expressed as
+                             // percentage of height of big trapezoid
+
+    // This parameter will make the trapezoids float just above the bottom edge
+    // of the image
+    float bar = 0.97;
+
+    // Vector which holds all the points of the two trapezoids
+    std::vector<Point> pts;
+
+    // Large trapezoid
+    pts.push_back(cv::Point((source.cols * (1 - trapezoidBottomWidth)) / 2,
+                            source.rows * bar)); // Bottom left
+    pts.push_back(
+        cv::Point((source.cols * (1 - trapezoidTopWidth)) / 2,
+                  source.rows - source.rows * trapezoidHeight)); // Top left
+    pts.push_back(
+        cv::Point(source.cols - (source.cols * (1 - trapezoidTopWidth)) / 2,
+                  source.rows - source.rows * trapezoidHeight)); // Top right
+    pts.push_back(
+        cv::Point(source.cols - (source.cols * (1 - trapezoidBottomWidth)) / 2,
+                  source.rows * bar)); // Bottom right
+
+    // Small trapezoid
+    pts.push_back(cv::Point(
+        (source.cols * (1 - trapezoidBottomWidth + smallBottomWidth)) / 2,
+        source.rows * bar)); // Bottom left
+    pts.push_back(cv::Point(
+        (source.cols * (1 - trapezoidTopWidth * smallTopWidth)) / 2,
+        source.rows - source.rows * trapezoidHeight * smallHeight)); // Top left
+    pts.push_back(cv::Point(
+        source.cols -
+            (source.cols * (1 - trapezoidTopWidth * smallTopWidth)) / 2,
+        source.rows -
+            source.rows * trapezoidHeight * smallHeight)); // Top right
+    pts.push_back(cv::Point(
+        source.cols -
+            (source.cols * (1 - trapezoidBottomWidth + smallBottomWidth)) / 2,
+        source.rows * bar)); // Bottom right
+
+    std::vector<std::vector<Point>> contours;
+    contours.push_back(pts);
+
+    // Create the mask
+    Mat mask = Mat::zeros(source.size(), source.type());
+    fillPoly(mask, contours, Scalar(255, 255, 255));
+
+    /* And here we basically put the mask over the source image,
+    meaning we return an all black image, except for the part where the mask
+    image has nonzero pixels: all the pixels in the space between the two
+    trapezoids */
+    Mat maskedImage;
+    bitwise_and(source, mask, maskedImage);
+
+    return maskedImage;
+}
+
+Mat LaneDetector::drawLanes(Mat source, std::vector<Vec4i> lines) {
+    // Stop if there are no lines, just return original image without lines
+    if (lines.size() == 0) {
+        return source;
+    }
+
+    // Set drawing lanes to true
+    bool drawRightLane = true;
+    bool drawLeftLane = true;
+
+    // Find lines with a slope higher than the slope threshold
+    float slopeThreshold = 0.5;
+    std::vector<float> slopes;
+    std::vector<Vec4i> goodLines;
+
+    for (int i = 0; i < lines.size(); i++) {
+        /* Each line is represented by a 4-element vector (x_1, y_1, x_2, y_2),
+        where (x_1,y_1) is the line's starting point and (x_2, y_2) is the
+        ending point */
+        Vec4i l = lines[i];
+        double slope;
+
+        // Calculate slope
+        if (l[2] - l[0] == 0) // Avoid division by zero
+        {
+            slope = 999; // Basically infinte slope
+        } else {
+            slope = (l[3] - l[1]) / (l[2] / l[0]);
+        }
+        if (abs(slope) > 0.5) {
+            slopes.push_back(slope);
+            goodLines.push_back(l);
+        }
+    }
+
+    /* Split the good lines into two categories: right and left
+    The right lines have a positive slope and the left lines have a negative
+    slope */
+    std::vector<Vec4i> rightLines;
+    std::vector<Vec4i> leftLines;
+    int imgCenter = source.cols / 2;
+
+    for (int i = 0; i < slopes.size(); i++) {
+        if (slopes[i] > 0 && goodLines[i][0] > imgCenter &&
+            goodLines[i][2] > imgCenter) {
+            rightLines.push_back(goodLines[i]);
+        }
+        if (slopes[i] < 0 && goodLines[i][0] < imgCenter &&
+            goodLines[i][2] < imgCenter) {
+            leftLines.push_back(goodLines[i]);
+        }
+    }
+
+    /* Now that we've isolated the right lane lines from the left lane lines,
+    it is time to form two lane lines out of all the lines we've detected.
+    A line is defined as 2 points: a starting point and an ending point.
+    So up to this point the right and left lane basically consist of multiple
+    hough lines. Our goal at this step is to use linear regression to find the
+    two best fitting lines: one through the points at the left side to form the
+    left lane and one through the points at the right side to form the right
+    lane */
+
+    // We start with the right side points
+    std::vector<int> rightLinesX;
+    std::vector<int> rightLinesY;
+    double rightB1, rightB0; // Slope and intercept
+
+    for (int i = 0; i < rightLines.size(); i++) {
+        rightLinesX.push_back(rightLines[i][0]); // X of starting point of line
+        rightLinesX.push_back(rightLines[i][2]); // X of ending point of line
+        rightLinesY.push_back(rightLines[i][1]); // Y of starting point of line
+        rightLinesY.push_back(rightLines[i][3]); // Y of ending point of line
+    }
+
+    if (rightLinesX.size() > 0) {
+        std::vector<double> coefRight = estimateCoefficients<int, double>(
+            rightLinesX, rightLinesY); // y = b1x + b0
+        rightB1 = coefRight[0];
+        rightB0 = coefRight[1];
+    } else {
+        rightB1 = 1;
+        rightB0 = 1;
+        drawRightLane = false;
+    }
+
+    // Now the points at the left side
+    std::vector<int> leftLinesX;
+    std::vector<int> leftLinesY;
+    double leftB1, leftB0; // Slope and intercept
+
+    for (int i = 0; i < leftLines.size(); i++) {
+        leftLinesX.push_back(leftLines[i][0]); // X of starting point of line
+        leftLinesX.push_back(leftLines[i][2]); // X of ending point of line
+        leftLinesY.push_back(leftLines[i][1]); // Y of starting point of line
+        leftLinesY.push_back(leftLines[i][3]); // Y of ending point of line
+    }
+
+    if (leftLinesX.size() > 0) {
+        std::vector<double> coefLeft = estimateCoefficients<int, double>(
+            leftLinesX, leftLinesY); // y = b1x + b0
+        leftB1 = coefLeft[0];
+        leftB0 = coefLeft[1];
+    } else {
+        leftB1 = 1;
+        leftB0 = 1;
+        drawLeftLane = false;
+    }
+
+    /* Now we need to find the two points for the right and left lane:
+    starting points and ending points */
+
+    int y1 = source.rows; // Y coordinate of starting point of both the left and
+                          // right lane
+
+    /* 0.5 = trapezoidHeight (see RegionOfInterest), we set the y coordinate of
+    the ending point below the trapezoid height (0.4) to draw shorter lanes. I
+    think that looks nicer. */
+
+    int y2 =
+        source.rows *
+        (1 -
+         0.4); // Y coordinate of ending point of both the left and right lane
+
+    // y = b1x + b0 --> x = (y - b0) / b1
+    int rightX1 = (y1 - rightB0) /
+                  rightB1; // X coordinate of starting point of right lane
+    int rightX2 =
+        (y2 - rightB0) / rightB1; // X coordinate of ending point of right lane
+
+    int leftX1 =
+        (y1 - leftB0) / leftB1; // X coordinate of starting point of left lane
+    int leftX2 =
+        (y2 - leftB0) / leftB1; // X coordinate of ending point of left lane
+
+    /* If the ending point of the right lane is on the left side of the left
+    lane (or vice versa), return source image without drawings, because this
+    should not be happening in real life. */
+    if (rightX2 < leftX2 || leftX2 > rightX2) {
+        return source;
+    }
+
+    // Create the mask
+    Mat mask = Mat::zeros(source.size(), source.type());
+
+    // Draw lines and fill poly made up of the four points described above if
+    // both bools are true
+    Mat dst; // Holds blended image
+    if (drawRightLane == true && drawLeftLane == true) {
+        line(source, Point(rightX1, y1), Point(rightX2, y2), Scalar(255, 0, 0),
+             7);
+        line(source, Point(leftX1, y1), Point(leftX2, y2), Scalar(255, 0, 0),
+             7);
+
+        Point pts[4] = {
+            Point(leftX1, y1),  // Starting point left lane
+            Point(leftX2, y2),  // Ending point left lane
+            Point(rightX2, y2), // Ending point right lane
+            Point(rightX1, y1)  // Starting point right lane
+        };
+
+        fillConvexPoly(mask, pts, 4,
+                       Scalar(235, 229, 52)); // Color is light blue
+
+        // Blend the mask and source image together
+        addWeighted(source, 0.9, mask, 0.3, 0.0, dst);
+
+        // Return blended image
+        return dst;
+    }
+
+    return source; // Return source if drawing lanes did not happen
+}
+
+std::vector<Vec4i> LaneDetector::houghLines(Mat canny, Mat source,
+                                            bool drawHough) {
+    double rho = 2; // Distance resolution in pixels of the Hough grid
+    double theta =
+        1 * M_PI / 180; // Angular resolution in radians of the Hough grid
+    int thresh =
+        15; // Minimum number of votes (intersections in Hough grid cell)
+    double minLineLength = 10; // Minimum number of pixels making up a line
+    double maxGapLength =
+        20; // Maximum gap in pixels between connectable line segments
+
+    std::vector<Vec4i> linesP; // Will hold the results of the detection
+    HoughLinesP(canny, linesP, rho, theta, thresh, minLineLength, maxGapLength);
+
+    if (drawHough == true) {
+        for (size_t i = 0; i < linesP.size(); i++) {
+            Vec4i l = linesP[i];
+            line(source, Point(l[0], l[1]), Point(l[2], l[3]),
+                 Scalar(0, 0, 255), 3, LINE_AA);
+        }
+        imshow("Hough Lines", source);
+        waitKey(0);
+    }
+
+    return linesP;
+}
+
+bool LaneDetector::isDayTime(Mat source) {
+    /* I've noticed that, in general, daytime images/videos require different
+    color filters than nighttime images/videos. For example, in darker light it
+    is better to add a gray color filter in addition to the white and yellow one
+  */
+
+    Scalar s = mean(source); // Mean pixel values
+
+    /* I chose these cut off values by looking at the mean pixel values of
+    multiple daytime and nighttime images */
+    if (s[0] < 30 || s[1] < 33 && s[2] < 30) {
+        return false;
+    }
+
+    return true;
+}
\ No newline at end of file
diff --git a/rubis_ws/src/vision_workloads/src/lane_detector/lane_detector_node.cpp b/rubis_ws/src/vision_workloads/src/lane_detector/lane_detector_node.cpp
new file mode 100644
index 00000000..26e038ef
--- /dev/null
+++ b/rubis_ws/src/vision_workloads/src/lane_detector/lane_detector_node.cpp
@@ -0,0 +1,10 @@
+#include <ros/ros.h>
+#include "lane_detector.hpp"
+
+int main(int argc, char* argv[]){
+    ros::init(argc, argv, "lane_detector");
+    LaneDetector lane_detector;
+    lane_detector.run();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/setup/setup.sh b/setup/setup.sh
index 50df55a6..29fb165c 100755
--- a/setup/setup.sh
+++ b/setup/setup.sh
@@ -42,21 +42,6 @@ if [ ! -d ~/Documents/profiling/response_time ]; then
     printf "~/Documents/profiling/response_time is created.\n"
 fi
 
-if [ ! -d ~/Documents/gpu_profiling ]; then
-    mkdir ~/Documents/gpu_profiling
-    printf "~/Documents/gpu_profiling is created.\n"
-fi
-
-if [ ! -d ~/Documents/gpu_profiling ]; then
-    mkdir ~/Documents/gpu_profiling
-    printf "~/Documents/gpu_profiling is created.\n"
-fi
-
-if [ ! -d ~/Documents/gpu_deadline ]; then
-    mkdir ~/Documents/gpu_deadline
-    printf "~/Documents/gpu_deadline is created.\n"
-fi
-
 echo "Necessary directory paths are created to /home/${1}/Documents"
 
 sudo ./setup_bashrc.sh $1
diff --git a/setup/setup_bashrc.sh b/setup/setup_bashrc.sh
index 1afb4e29..800bb5ef 100755
--- a/setup/setup_bashrc.sh
+++ b/setup/setup_bashrc.sh
@@ -31,31 +31,31 @@ rubis_ws_path="$(cd -P /home/${1}/rubis_ws && pwd)"
 
 user_bash_rubis_pkg=`cat /home/${1}/.bashrc | grep "source ${rubis_ws_path}/devel/setup.bash" | head -1`
 root_bash_rubis_pkg=`cat /root/.bashrc | grep "source ${rubis_ws_path}/devel/setup.bash" | head -1`
-user_bash_AUTWOARE=`cat /home/${1}/.bashrc | grep "source ${autoware_path}/install/setup.bash" | head -1`
-root_bash_AUTWOARE=`cat /root/.bashrc | grep "source ${autoware_path}/install/setup.bash" | head -1`
+user_bash_AUTWOARE=`cat /home/${1}/.bashrc | grep "source ${autoware_path}/devel/setup.bash" | head -1`
+root_bash_AUTWOARE=`cat /root/.bashrc | grep "source ${autoware_path}/devel/setup.bash" | head -1`
 
-if [ "$user_bash_rubis_pkg" != "source ${rubis_ws_path}/devel/setup.bash" ]
-  then echo "source ${rubis_ws_path}/devel/setup.bash" >> /home/${1}/.bashrc
-  echo "rubis_pkg variable setup is updated to /home/${1}/.bashrc"
+if [ "$user_bash_AUTWOARE" != "source ${autoware_path}/devel/setup.bash" ]
+  then echo "source ${autoware_path}/devel/setup.bash"  >> /home/${1}/.bashrc
+  echo "AUTOWAER variable setup is updated to /home/${1}/.bashrc"
   change_flag="True"
 fi
 
-if [ "$root_bash_rubis_pkg" != "source ${rubis_ws_path}/devel/setup.bash" ]
-  then echo "source ${rubis_ws_path}/devel/setup.bash" >> /root/.bashrc
-  echo "rubis_pkg variable setup is updated to /root/.bashrc"
+if [ "$root_bash_AUTWOARE" != "source ${autoware_path}/devel/setup.bash" ]
+  then echo "source ${autoware_path}/devel/setup.bash" >> /root/.bashrc
+  echo "AUTOWAER variable setup is updated to /root/.bashrc"
   change_flag="True"
 fi
 
 
-if [ "$user_bash_AUTWOARE" != "source ${autoware_path}/install/setup.bash" ]
-  then echo "source ${autoware_path}/install/setup.bash"  >> /home/${1}/.bashrc
-  echo "AUTOWAER variable setup is updated to /home/${1}/.bashrc"
+if [ "$user_bash_rubis_pkg" != "source ${rubis_ws_path}/devel/setup.bash" ]
+  then echo "source ${rubis_ws_path}/devel/setup.bash" >> /home/${1}/.bashrc
+  echo "rubis_pkg variable setup is updated to /home/${1}/.bashrc"
   change_flag="True"
 fi
 
-if [ "$root_bash_AUTWOARE" != "source ${autoware_path}/install/setup.bash" ]
-  then echo "source ${autoware_path}/install/setup.bash" >> /root/.bashrc
-  echo "AUTOWAER variable setup is updated to /root/.bashrc"
+if [ "$root_bash_rubis_pkg" != "source ${rubis_ws_path}/devel/setup.bash" ]
+  then echo "source ${rubis_ws_path}/devel/setup.bash" >> /root/.bashrc
+  echo "rubis_pkg variable setup is updated to /root/.bashrc"
   change_flag="True"
 fi
 
@@ -72,7 +72,7 @@ alias_user_gb="alias gb='gedit /home/${1}/.bashrc'"
 alias_user_sb="alias sb='source /home/${1}/.bashrc'"
 alias_root_gb="alias gb='gedit /root/.bashrc'"
 alias_root_sb="alias sb='source /root/.bashrc'"
-alias_sa="alias sa='source ${autoware_path}/install/setup.bash'"
+alias_sa="alias sa='source ${autoware_path}/devel/setup.bash'"
 alias_sr="alias sa='source ${rubis_ws_path}/devel/setup.bash'"
 alias_lglaunch="alias lglaunch='cd /home/${1}/autoware.ai/autoware_files/lgsvl_file/launch/${lglaunch_dir_name}'"
 
diff --git a/test.cpp b/test.cpp
new file mode 100644
index 00000000..946c2b3f
--- /dev/null
+++ b/test.cpp
@@ -0,0 +1,49 @@
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+class MultiTopicSyncSubscriber {
+public:
+    MultiTopicSyncSubscriber() : nh("~") {
+        topic1_sub.subscribe(nh, "/topic1", 1);
+        topic2_sub.subscribe(nh, "/topic2", 1);
+        topic3_sub.subscribe(nh, "/topic3", 1);
+
+        typedef message_filters::sync_policies::ApproximateTime<std_msgs::String, std_msgs::String, std_msgs::String> MySyncPolicy;
+        sync.reset(new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10), topic1_sub, topic2_sub, topic3_sub));
+        sync->registerCallback(boost::bind(&MultiTopicSyncSubscriber::callback, this, _1, _2, _3));
+    }
+
+    void callback(const std_msgs::String::ConstPtr& msg1, 
+                  const std_msgs::String::ConstPtr& msg2,
+                  const std_msgs::String::ConstPtr& msg3) {
+        ROS_INFO("Received from topic1: %s", msg1->data.c_str());
+        ROS_INFO("Received from topic2: %s", msg2->data.c_str());
+        ROS_INFO("Received from topic3: %s", msg3->data.c_str());
+    }
+
+    void spin() {
+        ros::spin();
+    }
+
+private:
+    ros::NodeHandle nh;
+    message_filters::Subscriber<std_msgs::String> topic1_sub;
+    message_filters::Subscriber<std_msgs::String> topic2_sub;
+    message_filters::Subscriber<std_msgs::String> topic3_sub;
+
+    typedef message_filters::sync_policies::ApproximateTime<std_msgs::String, std_msgs::String, std_msgs::String> MySyncPolicy;
+    typedef message_filters::Synchronizer<MySyncPolicy> Sync;
+    boost::shared_ptr<Sync> sync;
+};
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "multi_topic_sync_subscriber_class_example");
+
+    MultiTopicSyncSubscriber instance;
+    instance.spin();
+
+    return 0;
+}
\ No newline at end of file