From 8cb6491d3d56e7ad6dc6854d7a98d242031f86b7 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Fri, 26 Oct 2018 10:13:54 +0200
Subject: [PATCH] Renamed files and launchs

---
 cfg/VisualGps.cfg                             |  2 +-
 config/blue_conf.yaml                         |  9 ---
 config/blue_lab.yaml                          |  9 ---
 config/green_conf.yaml                        |  9 ---
 config/green_lab.yaml                         |  9 ---
 config/{params.yaml => param_sim.yaml}        |  0
 config/params_lab.yaml                        |  4 +
 config/params_sim.yaml                        |  4 +
 .../{lab_position.yaml => position_lab.yaml}  |  0
 .../{sim_position.yaml => position_sim.yaml}  |  0
 config/red_conf.yaml                          |  9 ---
 config/red_lab.yaml                           |  9 ---
 ...default_params.yaml => sample_params.yaml} |  0
 config/sim_position_full.yaml                 | 17 -----
 config/yellow_conf.yaml                       |  9 ---
 config/yellow_lab.yaml                        |  9 ---
 include/visual_gps_alg.h                      |  1 -
 launch/node.launch                            | 31 ++++++++
 launch/real.launch                            | 73 +++++++++++++++++++
 launch/sim.launch                             | 73 +++++++++++++++++++
 launch/visual_gps.launch                      | 63 ----------------
 launch/visual_gps_nodelets.launch             | 61 ----------------
 launch/visual_gps_sim.launch                  | 63 ----------------
 launch/visual_gps_sim_nodelets.launch         | 42 -----------
 launch/visual_gps_undistort.launch            | 73 +++++++++++++++++++
 launch/visual_gps_undistort_sim.launch        | 73 +++++++++++++++++++
 src/visual_gps_alg.cpp                        |  5 +-
 src/visual_gps_alg_node.cpp                   |  8 +-
 28 files changed, 338 insertions(+), 327 deletions(-)
 delete mode 100644 config/blue_conf.yaml
 delete mode 100644 config/blue_lab.yaml
 delete mode 100644 config/green_conf.yaml
 delete mode 100644 config/green_lab.yaml
 rename config/{params.yaml => param_sim.yaml} (100%)
 create mode 100644 config/params_lab.yaml
 create mode 100644 config/params_sim.yaml
 rename config/{lab_position.yaml => position_lab.yaml} (100%)
 rename config/{sim_position.yaml => position_sim.yaml} (100%)
 delete mode 100644 config/red_conf.yaml
 delete mode 100644 config/red_lab.yaml
 rename config/{default_params.yaml => sample_params.yaml} (100%)
 delete mode 100644 config/sim_position_full.yaml
 delete mode 100644 config/yellow_conf.yaml
 delete mode 100644 config/yellow_lab.yaml
 create mode 100644 launch/node.launch
 create mode 100644 launch/real.launch
 create mode 100644 launch/sim.launch
 delete mode 100644 launch/visual_gps.launch
 delete mode 100644 launch/visual_gps_nodelets.launch
 delete mode 100644 launch/visual_gps_sim.launch
 delete mode 100644 launch/visual_gps_sim_nodelets.launch
 create mode 100644 launch/visual_gps_undistort.launch
 create mode 100644 launch/visual_gps_undistort_sim.launch

diff --git a/cfg/VisualGps.cfg b/cfg/VisualGps.cfg
index 1737dae..beeb4bf 100755
--- a/cfg/VisualGps.cfg
+++ b/cfg/VisualGps.cfg
@@ -44,7 +44,7 @@ gen.add("odom_gps_frame"        ,  str_t,     0,                               "
 gen.add("odom_frame"            ,  str_t,     0,                               "Odometry reference frame",       "odom")
 gen.add("base_frame"            ,  str_t,     0,                               "Base frame id",                  "base_link")
 gen.add("camera_frame"          ,  str_t,     0,                               "Camera frame id",                "top_camera_optical")
-gen.add("undistort_points"      ,  bool_t,    0,                               "Enable point undistorsion",      True)
+gen.add("undistort_points"      ,  bool_t,    0,                               "Enable point undistorsion",      False)
 gen.add("publish_tf"            ,  bool_t,    0,                               "Enable tf broadcasting",         True)
 gen.add("publish_markers"       ,  bool_t,    0,                               "Enable marker publishing",       True)
 gen.add("color1_id"             ,  str_t,     0,                               "Color 1 identifier",             "color1")
diff --git a/config/blue_conf.yaml b/config/blue_conf.yaml
deleted file mode 100644
index 87d1cd2..0000000
--- a/config/blue_conf.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: blue
-H_center: 240
-H_window: 10
-S_center: 255
-S_window: 10
-V_center: 102
-V_window: 10
-max_area: 100000
-min_area: 10
\ No newline at end of file
diff --git a/config/blue_lab.yaml b/config/blue_lab.yaml
deleted file mode 100644
index e5f517d..0000000
--- a/config/blue_lab.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: blue
-H_center: 233
-H_window: 3
-S_center: 215
-S_window: 10
-V_center: 131
-V_window: 33
-max_area: 100000
-min_area: 10
diff --git a/config/green_conf.yaml b/config/green_conf.yaml
deleted file mode 100644
index 175431e..0000000
--- a/config/green_conf.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: green
-H_center: 120
-H_window: 10
-S_center: 250
-S_window: 10
-V_center: 108
-V_window: 23
-max_area: 100000
-min_area: 10
diff --git a/config/green_lab.yaml b/config/green_lab.yaml
deleted file mode 100644
index 00d14fb..0000000
--- a/config/green_lab.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: green
-H_center: 134
-H_window: 14
-S_center: 122
-S_window: 35
-V_center: 44
-V_window: 9
-max_area: 100000
-min_area: 10
diff --git a/config/params.yaml b/config/param_sim.yaml
similarity index 100%
rename from config/params.yaml
rename to config/param_sim.yaml
diff --git a/config/params_lab.yaml b/config/params_lab.yaml
new file mode 100644
index 0000000..aada361
--- /dev/null
+++ b/config/params_lab.yaml
@@ -0,0 +1,4 @@
+world_frame:    map
+odom_gps_frame: gps_odom
+base_frame:     base_link
+camera_frame:   top_camera_optical
\ No newline at end of file
diff --git a/config/params_sim.yaml b/config/params_sim.yaml
new file mode 100644
index 0000000..aada361
--- /dev/null
+++ b/config/params_sim.yaml
@@ -0,0 +1,4 @@
+world_frame:    map
+odom_gps_frame: gps_odom
+base_frame:     base_link
+camera_frame:   top_camera_optical
\ No newline at end of file
diff --git a/config/lab_position.yaml b/config/position_lab.yaml
similarity index 100%
rename from config/lab_position.yaml
rename to config/position_lab.yaml
diff --git a/config/sim_position.yaml b/config/position_sim.yaml
similarity index 100%
rename from config/sim_position.yaml
rename to config/position_sim.yaml
diff --git a/config/red_conf.yaml b/config/red_conf.yaml
deleted file mode 100644
index e91af09..0000000
--- a/config/red_conf.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: red
-H_center: 0
-H_window: 10
-S_center: 250
-S_window: 10
-V_center: 102
-V_window: 7
-max_area: 100000
-min_area: 10
diff --git a/config/red_lab.yaml b/config/red_lab.yaml
deleted file mode 100644
index 85f4dc9..0000000
--- a/config/red_lab.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: red
-H_center: 348
-H_window: 6
-S_center: 185
-S_window: 21
-V_center: 90
-V_window: 27
-max_area: 100000
-min_area: 10
diff --git a/config/default_params.yaml b/config/sample_params.yaml
similarity index 100%
rename from config/default_params.yaml
rename to config/sample_params.yaml
diff --git a/config/sim_position_full.yaml b/config/sim_position_full.yaml
deleted file mode 100644
index 5652a75..0000000
--- a/config/sim_position_full.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-color1_id: green
-color1_x: 0.0
-color1_y: -2.5
-color1_z: 4.0
-color2_id: red
-color2_x: 2.5
-color2_y: -5.0
-color2_z: 4.0
-color3_id: yellow
-color3_x: -2.5
-color3_y: -5.0
-color3_z: 4.0
-color4_id: blue
-color4_x: 0.0
-color4_y: -7.5
-color4_z: 4.0
-
diff --git a/config/yellow_conf.yaml b/config/yellow_conf.yaml
deleted file mode 100644
index cbe58fc..0000000
--- a/config/yellow_conf.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: yellow
-H_center: 60
-H_window: 10
-S_center: 251
-S_window: 8
-V_center: 101
-V_window: 7
-max_area: 100000
-min_area: 10
diff --git a/config/yellow_lab.yaml b/config/yellow_lab.yaml
deleted file mode 100644
index 3928b14..0000000
--- a/config/yellow_lab.yaml
+++ /dev/null
@@ -1,9 +0,0 @@
-color_id: yellow
-H_center: 48
-H_window: 6
-S_center: 228
-S_window: 20
-V_center: 132
-V_window: 21
-max_area: 100000
-min_area: 10
diff --git a/include/visual_gps_alg.h b/include/visual_gps_alg.h
index 0528d13..4189814 100644
--- a/include/visual_gps_alg.h
+++ b/include/visual_gps_alg.h
@@ -56,7 +56,6 @@ class VisualGpsAlgorithm
     // private attributes and methods
     cv::Mat K,D,R,P;
     geometry_msgs::Pose pose; 
-    tf::StampedTransform transform_base_to_camera;
     
     Eigen::Vector3d t_base_2_camera;
     Eigen::Matrix3d R_base_camera;
diff --git a/launch/node.launch b/launch/node.launch
new file mode 100644
index 0000000..a6dc003
--- /dev/null
+++ b/launch/node.launch
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <arg name="node_name"      default="iri_visual_gps"/>
+  <arg name="positions_file" default="position_lab.yaml"/>
+  <arg name="param_file"     default="sample_params.yaml"/>
+  <arg name="param_path"     default="$(find iri_visual_gps)/config"/>
+  <arg name="camera_name"    default="usb_cam"/>
+  <arg name="blobs1_topic"   default="blobs1"/>
+  <arg name="blobs2_topic"   default="blobs2"/>
+  <arg name="blobs3_topic"   default="blobs3"/>
+  <arg name="blobs4_topic"   default="blobs4"/>
+  <arg name="camera_info_topic"         default="/$(arg camera_name)/camera_info"/>
+  <arg name="camera_set_params_service" default="/$(arg camera_name)/set_parameters"/>
+
+  <node pkg="iri_visual_gps" 
+        type="iri_visual_gps" 
+        name="$(arg node_name)" 
+        output="screen">
+    <remap from="~camera/camera_info" to="$(arg camera_info_topic)"/>
+    <remap from="~set_camera_params"  to="$(arg camera_set_params_service)"/>
+    <remap from="~blobs1" to="$(arg blobs1_topic)"/>
+    <remap from="~blobs2" to="$(arg blobs2_topic)"/>
+    <remap from="~blobs3" to="$(arg blobs3_topic)"/>
+    <remap from="~blobs4" to="$(arg blobs4_topic)"/>
+    <rosparam file="$(arg param_path)/$(arg positions_file)" command="load" />
+    <rosparam file="$(arg param_path)/$(arg param_file)"     command="load" />
+  </node>
+
+</launch>
\ No newline at end of file
diff --git a/launch/real.launch b/launch/real.launch
new file mode 100644
index 0000000..de71734
--- /dev/null
+++ b/launch/real.launch
@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_red"/>
+    <arg name="param_file"  value="red_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_red/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_green"/>
+    <arg name="param_file"  value="green_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_green/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_blue"/>
+    <arg name="param_file"  value="blue_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_blue/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_yellow"/>
+    <arg name="param_file"  value="yellow_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_yellow/blobs"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_red"/>
+    <arg name="topic_blobs_in"  value="blob_detector_red/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_red/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_green"/>
+    <arg name="topic_blobs_in"  value="blob_detector_green/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_green/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_blue"/>
+    <arg name="topic_blobs_in"  value="blob_detector_blue/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_blue/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_yellow"/>
+    <arg name="topic_blobs_in"  value="blob_detector_yellow/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_yellow/blobs_undist"/>
+  </include>
+  
+  <include file="$(find iri_visual_gps)/launch/node.launch">
+    <arg name="node_name"       value="iri_visual_gps"/>
+    <arg name="positions_file"  value="position_lab.yaml"/>
+    <arg name="param_file"      value="params_lab.yaml"/>
+    <arg name="param_path"      value="$(find iri_visual_gps)/config"/>
+    <arg name="camera_name"     value="usb_cam"/>
+    <arg name="blobs1_topic"    value="blob_detector_red/blobs_undist"/>
+    <arg name="blobs2_topic"    value="blob_detector_green/blobs_undist"/>
+    <arg name="blobs3_topic"    value="blob_detector_blue/blobs_undist"/>
+    <arg name="blobs4_topic"    value="blob_detector_yellow/blobs_undist"/>
+  </include>
+
+</launch>
\ No newline at end of file
diff --git a/launch/sim.launch b/launch/sim.launch
new file mode 100644
index 0000000..89dbfe2
--- /dev/null
+++ b/launch/sim.launch
@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_red"/>
+    <arg name="param_file"  value="red_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_red/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_green"/>
+    <arg name="param_file"  value="green_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_green/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_blue"/>
+    <arg name="param_file"  value="blue_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_blue/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_yellow"/>
+    <arg name="param_file"  value="yellow_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_yellow/blobs"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_red"/>
+    <arg name="topic_blobs_in"  value="blob_detector_red/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_red/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_green"/>
+    <arg name="topic_blobs_in"  value="blob_detector_green/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_green/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_blue"/>
+    <arg name="topic_blobs_in"  value="blob_detector_blue/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_blue/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_yellow"/>
+    <arg name="topic_blobs_in"  value="blob_detector_yellow/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_yellow/blobs_undist"/>
+  </include>
+  
+  <include file="$(find iri_visual_gps)/launch/node.launch">
+    <arg name="node_name"       value="iri_visual_gps"/>
+    <arg name="positions_file"  value="position_sim.yaml"/>
+    <arg name="param_file"      value="params_sim.yaml"/>
+    <arg name="param_path"      value="$(find iri_visual_gps)/config"/>
+    <arg name="camera_name"     value="usb_cam"/>
+    <arg name="blobs1_topic"    value="blob_detector_red/blobs_undist"/>
+    <arg name="blobs2_topic"    value="blob_detector_green/blobs_undist"/>
+    <arg name="blobs3_topic"    value="blob_detector_blue/blobs_undist"/>
+    <arg name="blobs4_topic"    value="blob_detector_yellow/blobs_undist"/>
+  </include>
+
+</launch>
\ No newline at end of file
diff --git a/launch/visual_gps.launch b/launch/visual_gps.launch
deleted file mode 100644
index 572559f..0000000
--- a/launch/visual_gps.launch
+++ /dev/null
@@ -1,63 +0,0 @@
-<!-- -->
-<launch>
-
-  <arg name="positions_file" default="lab_position.yaml"/>
-  <arg name="positions_path" default="$(find iri_visual_gps)/config"/>
-  <arg name="color_sufix" default="_lab"/>
-  <arg name="color1_file" default="green$(arg color_sufix).yaml"/>
-  <arg name="color2_file" default="red$(arg color_sufix).yaml"/>
-  <arg name="color3_file" default="yellow$(arg color_sufix).yaml"/>
-  <arg name="color4_file" default="blue$(arg color_sufix).yaml"/>
-  
-  <arg name="config_yaml_file_location" default="$(find iri_visual_gps)/config"/>
-
-  <node name="blob1"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color1_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color1_file)" command="load" />
-  </node>
-  
-  <node name="blob2"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color2_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color2_file)" command="load" />
-  </node>
-  
-  <node name="blob3"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color3_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color3_file)" command="load" />
-  </node>
-  
-  <node name="blob4"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color4_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color4_file)" command="load" />
-  </node>
-
-  <node pkg="iri_visual_gps" 
-        type="iri_visual_gps" 
-        name="visual_gps" 
-        output="screen">
-    <remap from="~/camera/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/set_camera_params" to="/usb_cam/set_parameters"/>
-    <rosparam file="$(arg positions_path)/$(arg positions_file)" command="load" />
-    <rosparam file="$(arg positions_path)/params.yaml" command="load" />
-  </node>
-</launch>
diff --git a/launch/visual_gps_nodelets.launch b/launch/visual_gps_nodelets.launch
deleted file mode 100644
index c5312ea..0000000
--- a/launch/visual_gps_nodelets.launch
+++ /dev/null
@@ -1,61 +0,0 @@
-<?xml version="1.0"?>
-<!-- -->
-<launch>
-  <node pkg="nodelet" type="nodelet" name="visual_gps_nodelet"  args="manager" output="screen">
-    
-  </node>
-
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color1" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color1_blobs"/>
-    <rosparam file="$(find iri_blob_detector)/config/red.yaml" command="load" />
-  </node>
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color2" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color2_blobs"/>
-    <rosparam file="$(find iri_blob_detector)/config/blue.yaml" command="load" />
-  </node>
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color3" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color3_blobs"/>
-    <rosparam file="$(find iri_blob_detector)/config/green.yaml" command="load" />
-  </node>
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color4" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color4_blobs"/>
-    <rosparam file="$(find iri_blob_detector)/config/yellow.yaml" command="load" />
-  </node>
-  <node pkg="nodelet" type="nodelet" name="camera" args="load libuvc_camera/driver visual_gps_nodelet" output="screen">
-<!--    <param name="vendor" value="0x046d"/>
-    <param name="product" value="0x0805"/>
-    <param name="serial" value="AB659260"/>-->
-    <!-- If the above parameters aren't unique, choose the first match: -->
-    <param name="index" value="0"/>
-    <!-- Image size and type -->
-    <param name="width" value="640"/>
-    <param name="height" value="480"/>
-    <!-- choose whichever uncompressed format the camera supports: -->
-    <param name="video_mode" value="uncompressed"/> <!-- or yuyv/nv12/mjpeg -->
-    <param name="frame_rate" value="15"/>
-    <param name="timestamp_method" value="start"/> <!-- start of frame -->
-    <param name="camera_info_url" value="file:///tmp/cam.yaml"/>
-    <param name="auto_exposure" value="0"/> <!-- use aperture_priority auto exposure -->
-    <param name="auto_exposure_priority" value="0"/>
-    <param name="exposure_absolute" value="0.90"/> <!-- use aperture_priority auto exposure -->
-    <remap from="/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="/camera_info" to="/usb_cam/camera_info"/>
-  </node>
-
-  <node pkg="iri_visual_gps" 
-        type="iri_visual_gps" 
-        name="visual_gps" 
-        output="screen">
-    <remap from="~/camera/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/set_camera_params" to="/usb_cam/set_parameters"/>
-    <rosparam file="$(find iri_visual_gps)/config/lab_position.yaml" command="load" />
-  </node>
-</launch>
diff --git a/launch/visual_gps_sim.launch b/launch/visual_gps_sim.launch
deleted file mode 100644
index 8560430..0000000
--- a/launch/visual_gps_sim.launch
+++ /dev/null
@@ -1,63 +0,0 @@
-<!-- -->
-<launch>
-
-  <arg name="positions_file" default="sim_position_full.yaml"/>
-  <arg name="positions_path" default="$(find iri_visual_gps)/config"/>
-  <arg name="color_sufix" default="_conf"/>
-  <arg name="color1_file" default="green$(arg color_sufix).yaml"/>
-  <arg name="color2_file" default="red$(arg color_sufix).yaml"/>
-  <arg name="color3_file" default="yellow$(arg color_sufix).yaml"/>
-  <arg name="color4_file" default="blue$(arg color_sufix).yaml"/>
-  
-  <arg name="config_yaml_file_location" default="$(find iri_visual_gps)/config"/>
-
-  <node name="blob1"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color1_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color1_file)" command="load" />
-  </node>
-  
-  <node name="blob2"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color2_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color2_file)" command="load" />
-  </node>
-  
-  <node name="blob3"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color3_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color3_file)" command="load" />
-  </node>
-  
-  <node name="blob4"
-        pkg="iri_blob_detector"
-        type="iri_blob_detector"
-        output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color4_blobs"/>
-    <rosparam file="$(arg config_yaml_file_location)/$(arg color4_file)" command="load" />
-  </node>
-
-  <node pkg="iri_visual_gps" 
-        type="iri_visual_gps" 
-        name="visual_gps" 
-        output="screen">
-    <remap from="~/camera/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/set_camera_params" to="/usb_cam/set_parameters"/>
-    <rosparam file="$(arg positions_path)/$(arg positions_file)" command="load" />
-    <rosparam file="$(arg positions_path)/params.yaml" command="load" />
-  </node>
-</launch>
diff --git a/launch/visual_gps_sim_nodelets.launch b/launch/visual_gps_sim_nodelets.launch
deleted file mode 100644
index 40728e1..0000000
--- a/launch/visual_gps_sim_nodelets.launch
+++ /dev/null
@@ -1,42 +0,0 @@
-<?xml version="1.0"?>
-<!-- -->
-<launch>
-
-  <node pkg="nodelet" type="nodelet" name="visual_gps_nodelet"  args="manager" output="screen">
-    
-  </node>
-
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color1" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color1_blobs"/>
-    <rosparam file="$(find iri_visual_gps)/config/green_conf.yaml" command="load" />
-  </node>
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color2" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color2_blobs"/>
-    <rosparam file="$(find iri_visual_gps)/config/red_conf.yaml" command="load" />
-  </node>
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color3" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color3_blobs"/>
-    <rosparam file="$(find iri_visual_gps)/config/yellow_conf.yaml" command="load" />
-  </node>
-  <node pkg="nodelet" type="nodelet" name="blob_detector_color4" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
-    <remap from="~/image_in/image_raw" to="/usb_cam/image_raw"/>
-    <remap from="~/image_in/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/blobs" to="/visual_gps/color4_blobs"/>
-    <rosparam file="$(find iri_visual_gps)/config/blue_conf.yaml" command="load" />
-  </node>
-
-  <node pkg="iri_visual_gps" 
-        type="iri_visual_gps" 
-        name="visual_gps" 
-        output="screen">
-    <remap from="~/camera/camera_info" to="/usb_cam/camera_info"/>
-    <remap from="~/set_camera_params" to="/usb_cam/set_parameters"/>
-    <rosparam file="$(find iri_visual_gps)/config/sim_position.yaml" command="load" />
-  </node>
-</launch>
diff --git a/launch/visual_gps_undistort.launch b/launch/visual_gps_undistort.launch
new file mode 100644
index 0000000..de71734
--- /dev/null
+++ b/launch/visual_gps_undistort.launch
@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_red"/>
+    <arg name="param_file"  value="red_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_red/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_green"/>
+    <arg name="param_file"  value="green_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_green/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_blue"/>
+    <arg name="param_file"  value="blue_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_blue/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_yellow"/>
+    <arg name="param_file"  value="yellow_lab.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_yellow/blobs"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_red"/>
+    <arg name="topic_blobs_in"  value="blob_detector_red/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_red/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_green"/>
+    <arg name="topic_blobs_in"  value="blob_detector_green/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_green/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_blue"/>
+    <arg name="topic_blobs_in"  value="blob_detector_blue/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_blue/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_yellow"/>
+    <arg name="topic_blobs_in"  value="blob_detector_yellow/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_yellow/blobs_undist"/>
+  </include>
+  
+  <include file="$(find iri_visual_gps)/launch/node.launch">
+    <arg name="node_name"       value="iri_visual_gps"/>
+    <arg name="positions_file"  value="position_lab.yaml"/>
+    <arg name="param_file"      value="params_lab.yaml"/>
+    <arg name="param_path"      value="$(find iri_visual_gps)/config"/>
+    <arg name="camera_name"     value="usb_cam"/>
+    <arg name="blobs1_topic"    value="blob_detector_red/blobs_undist"/>
+    <arg name="blobs2_topic"    value="blob_detector_green/blobs_undist"/>
+    <arg name="blobs3_topic"    value="blob_detector_blue/blobs_undist"/>
+    <arg name="blobs4_topic"    value="blob_detector_yellow/blobs_undist"/>
+  </include>
+
+</launch>
\ No newline at end of file
diff --git a/launch/visual_gps_undistort_sim.launch b/launch/visual_gps_undistort_sim.launch
new file mode 100644
index 0000000..89dbfe2
--- /dev/null
+++ b/launch/visual_gps_undistort_sim.launch
@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_red"/>
+    <arg name="param_file"  value="red_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_red/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_green"/>
+    <arg name="param_file"  value="green_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_green/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_blue"/>
+    <arg name="param_file"  value="blue_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_blue/blobs"/>
+  </include>
+  
+  <include file="$(find iri_blob_detector)/launch/node.launch">
+    <arg name="node_name"   value="blob_detector_yellow"/>
+    <arg name="param_file"  value="yellow_sim.yaml"/>
+    <arg name="param_path"  value="$(find iri_blob_detector)/config"/>
+    <arg name="camera_name" value="usb_cam"/>
+    <arg name="blobs_topic" value="blob_detector_yellow/blobs"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_red"/>
+    <arg name="topic_blobs_in"  value="blob_detector_red/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_red/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_green"/>
+    <arg name="topic_blobs_in"  value="blob_detector_green/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_green/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_blue"/>
+    <arg name="topic_blobs_in"  value="blob_detector_blue/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_blue/blobs_undist"/>
+  </include>
+  
+  <include file="$(find undistort_pixels)/launch/node.launch">
+    <arg name="node_name"       value="undistort_blob_yellow"/>
+    <arg name="topic_blobs_in"  value="blob_detector_yellow/blobs"/>
+    <arg name="topic_blobs_out" value="blob_detector_yellow/blobs_undist"/>
+  </include>
+  
+  <include file="$(find iri_visual_gps)/launch/node.launch">
+    <arg name="node_name"       value="iri_visual_gps"/>
+    <arg name="positions_file"  value="position_sim.yaml"/>
+    <arg name="param_file"      value="params_sim.yaml"/>
+    <arg name="param_path"      value="$(find iri_visual_gps)/config"/>
+    <arg name="camera_name"     value="usb_cam"/>
+    <arg name="blobs1_topic"    value="blob_detector_red/blobs_undist"/>
+    <arg name="blobs2_topic"    value="blob_detector_green/blobs_undist"/>
+    <arg name="blobs3_topic"    value="blob_detector_blue/blobs_undist"/>
+    <arg name="blobs4_topic"    value="blob_detector_yellow/blobs_undist"/>
+  </include>
+
+</launch>
\ No newline at end of file
diff --git a/src/visual_gps_alg.cpp b/src/visual_gps_alg.cpp
index 77112d4..b1c33a6 100644
--- a/src/visual_gps_alg.cpp
+++ b/src/visual_gps_alg.cpp
@@ -56,8 +56,8 @@ void VisualGpsAlgorithm::update_points(std::vector<iri_blob_detector::blob> &blo
       // rectify points
       std::vector<cv::Point2f> pixels_img_undist;
       cv::fisheye::undistortPoints(pixels_img,pixels_img_undist,this->K,this->D,this->R,this->P); 
-      ROS_INFO("pixels_img x,y = %f,%f", pixels_img[0].x, pixels_img[0].y);
-      ROS_INFO("pixels_img_undist x,y = %f,%f", pixels_img_undist[0].x, pixels_img_undist[0].y);
+      //ROS_INFO("pixels_img x,y = %f,%f", pixels_img[0].x, pixels_img[0].y);
+      //ROS_INFO("pixels_img_undist x,y = %f,%f", pixels_img_undist[0].x, pixels_img_undist[0].y);
       pixels_img = pixels_img_undist;
     }
     
@@ -170,7 +170,6 @@ void VisualGpsAlgorithm::get_points(std::vector<cv::Point3f> &map_points, std::v
 
 void VisualGpsAlgorithm::set_transform_base_to_camera(tf::StampedTransform t)
 {
-  this->transform_base_to_camera = t;
   this->t_base_2_camera << t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z();
   double yaw_base_2_camera = tf::getYaw(t.getRotation());
   this->R_base_camera = Eigen::Matrix3d::Identity();
diff --git a/src/visual_gps_alg_node.cpp b/src/visual_gps_alg_node.cpp
index 90fd14b..19182ec 100644
--- a/src/visual_gps_alg_node.cpp
+++ b/src/visual_gps_alg_node.cpp
@@ -14,16 +14,16 @@ VisualGpsAlgNode::VisualGpsAlgNode(void) :
   this->odom_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("odom", 1);
   
   // [init subscribers]
-  this->color4_blobs_subscriber_ = this->public_node_handle_.subscribe("color4_blobs", 1, &VisualGpsAlgNode::color4_blobs_callback, this);
+  this->color4_blobs_subscriber_ = this->public_node_handle_.subscribe("blobs4", 1, &VisualGpsAlgNode::color4_blobs_callback, this);
   pthread_mutex_init(&this->color4_blobs_mutex_,NULL);
 
-  this->color3_blobs_subscriber_ = this->public_node_handle_.subscribe("color3_blobs", 1, &VisualGpsAlgNode::color3_blobs_callback, this);
+  this->color3_blobs_subscriber_ = this->public_node_handle_.subscribe("blobs3", 1, &VisualGpsAlgNode::color3_blobs_callback, this);
   pthread_mutex_init(&this->color3_blobs_mutex_,NULL);
 
-  this->color2_blobs_subscriber_ = this->public_node_handle_.subscribe("color2_blobs", 1, &VisualGpsAlgNode::color2_blobs_callback, this);
+  this->color2_blobs_subscriber_ = this->public_node_handle_.subscribe("blobs2", 1, &VisualGpsAlgNode::color2_blobs_callback, this);
   pthread_mutex_init(&this->color2_blobs_mutex_,NULL);
 
-  this->color1_blobs_subscriber_ = this->public_node_handle_.subscribe("color1_blobs", 1, &VisualGpsAlgNode::color1_blobs_callback, this);
+  this->color1_blobs_subscriber_ = this->public_node_handle_.subscribe("blobs1", 1, &VisualGpsAlgNode::color1_blobs_callback, this);
   pthread_mutex_init(&this->color1_blobs_mutex_,NULL);
 
   this->camera_info_subscriber_ = this->public_node_handle_.subscribe("camera/camera_info", 1, &VisualGpsAlgNode::camera_info_callback, this);
-- 
GitLab