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