diff --git a/config/blue_conf.yaml b/config/blue_conf.yaml index beeba6d8104202545953e52886ec12cb9fd717b5..87d1cd2e432de7e35ce18a5bb7bfa4ba5edc9b3f 100644 --- a/config/blue_conf.yaml +++ b/config/blue_conf.yaml @@ -1,9 +1,9 @@ color_id: blue H_center: 240 H_window: 10 -S_center: 250 +S_center: 255 S_window: 10 V_center: 102 -V_window: 8 +V_window: 10 max_area: 100000 -min_area: 10 +min_area: 10 \ No newline at end of file diff --git a/config/sim_position.yaml b/config/sim_position.yaml index 99d7282e599e7fd65a4e62d482029cc1844cf03b..5652a755dd5e1199d60753f1bb254cc66c5d9840 100644 --- a/config/sim_position.yaml +++ b/config/sim_position.yaml @@ -9,9 +9,9 @@ color2_z: 4.0 color3_id: yellow color3_x: -2.5 color3_y: -5.0 -color3_z: 2.0 +color3_z: 4.0 color4_id: blue color4_x: 0.0 color4_y: -7.5 -color4_z: 2.0 +color4_z: 4.0 diff --git a/launch/visual_gps.launch b/launch/visual_gps.launch index ed0c943049377cbb7fe7506fce48d497defdfe2d..22bcbb4782ad668d994340a71a87957ab4675c65 100644 --- a/launch/visual_gps.launch +++ b/launch/visual_gps.launch @@ -1,3 +1,4 @@ +<!-- --> <launch> <node pkg="nodelet" type="nodelet" name="visual_gps_nodelet" args="manager" output="screen"> @@ -7,25 +8,25 @@ <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/default_params.yaml" command="load" /> + <rosparam file="$(find iri_blob_detector)/config/red_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_blob_detector)/config/default_params.yaml" command="load" /> + <rosparam file="$(find iri_blob_detector)/config/blue_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_blob_detector)/config/default_params.yaml" command="load" /> + <rosparam file="$(find iri_blob_detector)/config/green_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_blob_detector)/config/default_params.yaml" command="load" /> + <rosparam file="$(find iri_blob_detector)/config/yellow_conf.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"/> diff --git a/launch/visual_gps_sim.launch b/launch/visual_gps_sim.launch index b4a5e0bd190d866fc51c0f99c1d8646ea895c24d..75c7f08c653e072e4cb20e80d6c4d314d7763a36 100644 --- a/launch/visual_gps_sim.launch +++ b/launch/visual_gps_sim.launch @@ -1,32 +1,49 @@ +<!-- --> <launch> - <node pkg="nodelet" type="nodelet" name="visual_gps_nodelet" args="manager" output="screen"> - - </node> + <arg name="positions_file" default="sim_position_track18.yaml"/> + <arg name="positions_path" default="$(find iri_visual_gps)/config"/> + + <arg name="config_yaml_file_location" default="$(find iri_visual_gps)/config"/> - <node pkg="nodelet" type="nodelet" name="blob_detector_color1" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen"> + <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="$(find iri_visual_gps)/config/green_conf.yaml" command="load" /> + <rosparam file="$(arg config_yaml_file_location)/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"> + + <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="$(find iri_visual_gps)/config/red_conf.yaml" command="load" /> + <rosparam file="$(arg config_yaml_file_location)/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"> + + <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="$(find iri_visual_gps)/config/yellow_conf.yaml" command="load" /> + <rosparam file="$(arg config_yaml_file_location)/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"> + + <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="$(find iri_visual_gps)/config/blue_conf.yaml" command="load" /> + <rosparam file="$(arg config_yaml_file_location)/blue_conf.yaml" command="load" /> </node> <node pkg="iri_visual_gps" @@ -35,6 +52,6 @@ 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" /> + <rosparam file="$(arg positions_path)/$(arg positions_file)" command="load" /> </node> </launch> diff --git a/launch/visual_gps_sim_nodelets.launch b/launch/visual_gps_sim_nodelets.launch new file mode 100644 index 0000000000000000000000000000000000000000..aa0f395032e179b1e2a8747ab6d3c9b60ad9849c --- /dev/null +++ b/launch/visual_gps_sim_nodelets.launch @@ -0,0 +1,41 @@ +<!-- --> +<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>