Skip to content

Commit

Permalink
Adding back the basler launcher and fixing the model selected in the …
Browse files Browse the repository at this point in the history
…default rw launcher
  • Loading branch information
ViktorWalter committed Apr 5, 2024
1 parent 567ce0b commit 9edb18c
Show file tree
Hide file tree
Showing 2 changed files with 323 additions and 1 deletion.
6 changes: 5 additions & 1 deletion launch/rw_three_sided.launch
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,8 @@

<arg name="beacon" default="false"/>

<arg name="output_frame" default="local_origin">

<arg name="id1" value="0"/>
<arg name="id2" value="1"/>
<arg name="id3" value="2"/>
Expand All @@ -98,7 +100,7 @@
<arg name="id21" value="20"/>

<arg name="sequence_file" default="$(find uvdar_core)/config/selected.txt"/>
<arg name="model_file" default="$(find uvdar_core)/config/models/quadrotor_foursided.txt"/>
<arg name="model_file" default="$(find uvdar_core)/config/models/quadrotor_foursided_px4_motor_aligned.txt"/>

<arg name="use_masks" default="true"/>

Expand Down Expand Up @@ -341,6 +343,8 @@

<remap from="~constituentHypotheses" to="/$(arg uav_name)/uvdar/constituentHypotheses"/>
<remap from="~measuredPoses" to="/$(arg uav_name)/uvdar/measuredPoses"/>

<param name="output_frame" type="string" value="$(arg output_frame)"/>
</node>
</group>

Expand Down
318 changes: 318 additions & 0 deletions launch/rw_three_sided_basler.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,318 @@
<launch>
<arg name="uav_name" default="$(env UAV_NAME)"/>
<arg name="mrs_id" default="$(env MRS_ID)"/>

<arg name="standalone" default="false"/>

<arg unless="$(arg standalone)" name="nodelet" value="load"/>
<arg if="$(arg standalone)" name="nodelet" value="standalone"/>
<arg unless="$(arg standalone)" name="nodelet_manager" value="$(arg uav_name)_uvdar_nodelet_manager"/>
<arg if="$(arg standalone)" name="nodelet_manager" value=""/>


<arg name="debug" default="false"/>

<!-- Basler camera settings: -->

<arg name="BASLER_LEFT_ID" default="$(env BASLER_UV_LEFT)" />
<arg name="BASLER_RIGHT_ID" default="$(env BASLER_UV_RIGHT)" />
<arg name="BASLER_BACK_ID" default="$(env BASLER_UV_BACK)" />


<arg name="camera_name_left" default="basler_left" />
<arg name="camera_name_right" default="basler_right" />
<arg name="camera_name_back" default="basler_back" />

<arg name="expose_us_left" default="$(env BASLER_UV_LEFT_EXPOSE_US)"/>
<arg name="expose_us_right" default="$(env BASLER_UV_RIGHT_EXPOSE_US)"/>
<arg name="expose_us_back" default="$(env BASLER_UV_BACK_EXPOSE_US)"/>

<arg name="camera_config_file" default="$(find uvdar_core)/config/camera_config/basler_grayscale.yaml" />

<arg name="calibrations_folder" default="$(find mrs_uav_deployment)/config/uvdar_calibrations"/>

<arg name="FRAME_RATE" default="120" />


<!-- Point detector settings: -->
<arg name="threshold" default="50"/>

<!-- Blink processor settings: -->

<arg name="blink_process_rate" default="10"/>

<!--AMI values-->
<arg name="max_px_shift_y" default="3"/>
<arg name="max_px_shift_x" default="3"/>
<arg name="max_zeros_consecutive" default="2"/>
<arg name="max_ones_consecutive" default="2"/>
<arg name="stored_seq_len_factor" default="1"/>
<arg name="max_buffer_length" default="5000"/>
<arg name="poly_order" default="2"/>
<arg name="decay_factor" default="0.1"/>
<arg name="confidence_probability" default="95.0"/>
<arg name="allowed_BER_per_seq" default="0"/>

<arg name="use_4DHT" default="false"/>
<!--4DHT values-->
<arg name="accumulator_length" default="18"/>
<arg name="pitch_steps" default="16"/>
<arg name="yaw_steps" default="16"/>
<arg name="max_pixel_shift" default="4"/>

<arg name="visual_debug" default="false"/>
<!-- <arg name="gui" default="true"/> -->
<arg name="gui" default="false"/>
<arg name="publish_visualization" default="true"/>
<arg name="visualization_rate" default="2"/>


<arg name="id1" value="0"/>
<arg name="id2" value="1"/>
<arg name="id3" value="2"/>
<arg name="id4" value="3"/>
<arg name="id5" value="4"/>
<arg name="id6" value="5"/>
<arg name="id7" value="6"/>
<arg name="id8" value="7"/>
<arg name="id9" value="8"/>
<arg name="id10" value="9"/>
<arg name="id11" value="10"/>
<arg name="id12" value="11"/>
<arg name="id13" value="12"/>
<arg name="id14" value="13"/>
<arg name="id15" value="14"/>
<arg name="id16" value="15"/>
<arg name="id17" value="16"/>
<arg name="id18" value="17"/>
<arg name="id19" value="18"/>
<arg name="id20" value="19"/>
<arg name="id21" value="20"/>

<arg name="sequence_file" default="$(find uvdar_core)/config/selected.txt"/>

<!-- Params for Tim's BP_Processor-->
<arg name="max_px_shift_x" default="2"/>
<arg name="max_px_shift_y" default="2"/>
<arg name="max_zeros_consecutive" default="2"/>
<arg name="max_ones_consecutive" default="2"/>
<arg name="stored_seq_len_factor" default="15"/>
<arg name="poly_order" default="4"/>
<arg name="decay_factor" default="0.01"/>
<arg name="confidence_probability" default="95.0"/>
<arg name="frame_tolerance" default="5"/>
<arg name="allowed_BER_per_seq" default="0"/>


<!-- Model settings: -->

<arg name="use_masks" default="true"/>
<arg name="model_file" default="$(find uvdar_core)/config/models/quadrotor_foursided_px4_motor_aligned.txt"/>
<arg name="beacon" default="false"/>

<arg name="blink_throttle_rate" default="10"/>

<node
name="uvcam_left_tf_$(arg uav_name)"
pkg="tf2_ros"
type="static_transform_publisher"
args="0.03 0.10 0.06 -0.3490658504 0.0 -1.57079632679 $(arg uav_name)/fcu $(arg uav_name)/uvcam_left"/>

<node
name="uvcam_right_tf_$(arg uav_name)"
pkg="tf2_ros"
type="static_transform_publisher"
args="0.03 -0.10 0.06 -2.792526803 0.0 -1.57079632679 $(arg uav_name)/fcu $(arg uav_name)/uvcam_right"/>

<node
name="uvcam_back_tf_$(arg uav_name)"
pkg="tf2_ros"
type="static_transform_publisher"
args="-0.10 0.0 -0.02 0.0 1.57079632679 3.14159265 $(arg uav_name)/fcu $(arg uav_name)/uvcam_back"/>
<!-- ideal angle: 1.221730476 - pi/2 -->

<!-- OR -->
<group ns="$(arg uav_name)">

<arg name="respawn" default="false" />
<arg name="startup_user_set" default="CurrentSetting" />
<arg name="enable_status_publisher" default="true" />

<arg name="nodelet_left" value="pylon_camera/PylonCameraNodelet" />
<arg name="nodelet_load_left" value="load $(arg nodelet_left) $(arg nodelet_manager)"/>

<arg name="nodelet_right" value="pylon_camera/PylonCameraNodelet" />
<arg name="nodelet_load_right" value="load $(arg nodelet_right) $(arg nodelet_manager)"/>

<arg name="nodelet_back" value="pylon_camera/PylonCameraNodelet" />
<arg name="nodelet_load_back" value="load $(arg nodelet_back) $(arg nodelet_manager)"/>

<!-- <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen" launch-prefix="debug_roslaunch"> -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen">
<param name="num_worker_threads" value="10" />
</node>


<node pkg="nodelet" type="nodelet" name="$(arg camera_name_left)" args="$(arg nodelet_load_left)" output="screen" launch-prefix="bash -c 'sleep 5; $0 $@'; " respawn="$(arg respawn)" >
<rosparam command="load" file="$(arg camera_config_file)" />
<param name="camera_frame" value="$(arg uav_name)/$(arg camera_name_left)_optical"/>
<param name="frame_rate" type="double" value="$(arg FRAME_RATE)"/>
<param name="device_user_id" type="string" value="$(arg BASLER_LEFT_ID)"/>
<param name="startup_user_set" value="$(arg startup_user_set)"/>
<param name="enable_status_publisher" value="$(arg enable_status_publisher)"/>
<param name="enable_current_params_publisher" value="false"/>
<param name="exposure" type="double" value="$(arg expose_us_left)"/>

</node>

<node pkg="nodelet" type="nodelet" name="$(arg camera_name_right)" args="$(arg nodelet_load_right)" output="screen" launch-prefix="bash -c 'sleep 6; $0 $@'; " respawn="$(arg respawn)" >
<rosparam command="load" file="$(arg camera_config_file)" />
<param name="camera_frame" value="$(arg uav_name)/$(arg camera_name_right)_optical"/>
<param name="frame_rate" type="double" value="$(arg FRAME_RATE)"/>
<param name="device_user_id" type="string" value="$(arg BASLER_RIGHT_ID)"/>
<param name="startup_user_set" value="$(arg startup_user_set)"/>
<param name="enable_status_publisher" value="$(arg enable_status_publisher)"/>
<param name="enable_current_params_publisher" value="false"/>
<param name="exposure" type="double" value="$(arg expose_us_right)"/>
</node>

<node pkg="nodelet" type="nodelet" name="$(arg camera_name_back)" args="$(arg nodelet_load_back)" output="screen" launch-prefix="bash -c 'sleep 7; $0 $@'; " respawn="$(arg respawn)" >
<rosparam command="load" file="$(arg camera_config_file)" />
<param name="camera_frame" value="$(arg uav_name)/$(arg camera_name_back)_optical"/>
<param name="frame_rate" type="double" value="$(arg FRAME_RATE)"/>
<param name="device_user_id" type="string" value="$(arg BASLER_BACK_ID)"/>
<param name="startup_user_set" value="$(arg startup_user_set)"/>
<param name="enable_status_publisher" value="$(arg enable_status_publisher)"/>
<param name="enable_current_params_publisher" value="false"/>
<param name="exposure" type="double" value="$(arg expose_us_back)"/>
</node>


<node name="uv_detect" pkg="nodelet" type="nodelet" args="$(arg nodelet) uvdar/UVDARDetector $(arg nodelet_manager)" output="screen" launch-prefix="bash -c 'sleep 8; $0 $@'; " respawn="false">
<!-- <node name="uv_detect" pkg="nodelet" type="nodelet" args="$(arg nodelet) uvdar/UVDARDetector $(arg nodelet_manager)" output="screen" respawn="false" launch-prefix="debug_roslaunch"> -->
<!-- <node name="uv_detect" pkg="uvdar" type="uv_detector_node" output="screen" launch-prefix="urxvt -e gdb -q -x /home/viktor/gdb.cmds -/-args"> -->
<param name="uav_name" type = "string" value="$(arg uav_name)"/>
<param name="debug" type="bool" value="$(arg debug)"/>
<!-- <param name="debug" type="bool" value="true"/> -->
<param name="gui" type="bool" value="false"/>
<param name="publish_visualization" type="bool" value="false"/>
<param name="justReport" type="bool" value="true"/>
<param name="threshold" type="int" value="$(arg threshold)"/>

<param name="use_masks" type="bool" value="$(arg use_masks)"/>
<param name="body_name" type = "string" value="$(arg mrs_id)"/>
<rosparam param="mask_file_names" subst_value="True"> ["$(arg calibrations_folder)/masks/$(arg mrs_id)_$(arg BASLER_LEFT_ID).png", "$(arg calibrations_folder)/masks/$(arg mrs_id)_$(arg BASLER_RIGHT_ID).png", "$(arg calibrations_folder)/masks/$(arg mrs_id)_$(arg BASLER_BACK_ID).png"] </rosparam>

<rosparam param="camera_topics"> ["camera_left", "camera_right", "camera_back"] </rosparam>
<rosparam param="points_seen_topics"> ["points_seen_left", "points_seen_right", "points_seen_back"] </rosparam>

<remap from="~camera_left" to="/$(arg uav_name)/basler_left/image_raw"/>
<remap from="~camera_right" to="/$(arg uav_name)/basler_right/image_raw"/>
<remap from="~camera_back" to="/$(arg uav_name)/basler_back/image_raw"/>
<remap from="~points_seen_left" to="/$(arg uav_name)/uvdar/points_seen_left"/>
<remap from="~points_seen_right" to="/$(arg uav_name)/uvdar/points_seen_right"/>
<remap from="~points_seen_back" to="/$(arg uav_name)/uvdar/points_seen_back"/>

<remap from="~odometry" to="/$(arg uav_name)/mrs_odometry/new_odom"/>
<remap from="~imu" to="mavros/imu/data"/>
</node>

<node name="blink_processor" pkg="nodelet" type="nodelet" args="load uvdar/UVDARBlinkProcessor $(arg nodelet_manager)" output="screen" respawn="false">

<param name="uav_name" type = "string" value="$(arg uav_name)"/>
<!-- <param name="debug" type="bool" value="$(arg debug)"/> -->
<param name="debug" type="bool" value="false"/>
<param name="visual_debug" type="bool" value="$(arg visual_debug)"/>
<param name="gui" type="bool" value="$(arg gui)"/>
<param name="publish_visualization" type="bool" value="$(arg publish_visualization)"/>
<param name="use_camera_for_visualization" type="bool" value="true"/>
<param name="visualization_rate" type="int" value="$(arg visualization_rate)"/>
<!-- <rosparam param="frequencies" subst_value="true"> [$(arg frequency1), $(arg frequency2), $(arg frequency3), $(arg frequency4)] </rosparam> -->
<rosparam param="signal_ids" subst_value="true"> [$(arg id1), $(arg id2), $(arg id3), $(arg id4), $(arg id5), $(arg id6), $(arg id7), $(arg id8), $(arg id9), $(arg id10), $(arg id11), $(arg id12), $(arg id13), $(arg id14), $(arg id15), $(arg id16), $(arg id17), $(arg id18), $(arg id19), $(arg id20), $(arg id21)] </rosparam>
<param name="sequence_file" type="string" value="$(arg sequence_file)"/>

<param name="blink_process_rate" type="int" value="$(arg blink_process_rate)"/>

<param name="max_px_shift_x" type="int" value="$(arg max_px_shift_x)"/>
<param name="max_px_shift_y" type="int" value="$(arg max_px_shift_y)"/>
<param name="max_zeros_consecutive" type="int" value="$(arg max_zeros_consecutive)"/>
<param name="max_ones_consecutive" type="int" value="$(arg max_ones_consecutive)"/>
<param name="stored_seq_len_factor" type="int" value="$(arg stored_seq_len_factor)"/>
<param name="max_buffer_length" type="int" value="$(arg max_buffer_length)"/>
<param name="poly_order" type="int" value="$(arg poly_order)"/>
<param name="decay_factor" type="double" value="$(arg decay_factor)"/>
<param name="confidence_probability" type="double" value="$(arg confidence_probability)"/>
<param name="allowed_BER_per_seq" type="int" value="$(arg allowed_BER_per_seq)"/>

<param name="accumulator_length" type="int" value="$(arg accumulator_length)"/>
<param name="pitch_steps" type="int" value="$(arg pitch_steps)"/>
<param name="yaw_steps" type="int" value="$(arg yaw_steps)"/>
<param name="max_pixel_shift" type="int" value="$(arg max_pixel_shift)"/>
<param name="nullify_radius" type="int" value="5"/>

<rosparam param="camera_topics"> ["camera_left", "camera_right", "camera_back"] </rosparam>
<rosparam param="points_seen_topics"> ["points_seen_left", "points_seen_right", "points_seen_back"] </rosparam>
<rosparam param="blinkers_seen_topics"> ["blinkers_seen_left", "blinkers_seen_right", "blinkers_seen_back"] </rosparam>
<rosparam param="estimated_framerate_topics"> ["estimated_framerate_left", "estimated_framerate_right", "estimated_framerate_back"] </rosparam>
<rosparam param="ami_logging_topics"> ["ami_logging_left", "ami_logging_right", "ami_logging_back"] </rosparam>
<rosparam param="ami_all_seq_info_topics"> ["ami_all_seq_info_left", "ami_all_seq_info_right", "ami_all_seq_info_back"] </rosparam>

<remap from="~camera_left" to="/$(arg uav_name)/basler_left/image_raw"/>
<remap from="~camera_right" to="/$(arg uav_name)/basler_right/image_raw"/>
<remap from="~camera_back" to="/$(arg uav_name)/basler_back/image_raw"/>
<remap from="~points_seen_left" to="/$(arg uav_name)/uvdar/points_seen_left"/>
<remap from="~points_seen_right" to="/$(arg uav_name)/uvdar/points_seen_right"/>
<remap from="~points_seen_back" to="/$(arg uav_name)/uvdar/points_seen_back"/>
<remap from="~blinkers_seen_left" to="/$(arg uav_name)/uvdar/blinkers_seen_left"/>
<remap from="~blinkers_seen_right" to="/$(arg uav_name)/uvdar/blinkers_seen_right"/>
<remap from="~blinkers_seen_back" to="/$(arg uav_name)/uvdar/blinkers_seen_back"/>
<remap from="~estimated_framerate_left" to="/$(arg uav_name)/uvdar/estimated_framerate_left"/>
<remap from="~estimated_framerate_right" to="/$(arg uav_name)/uvdar/estimated_framerate_right"/>
<remap from="~estimated_framerate_back" to="/$(arg uav_name)/uvdar/estimated_framerate_back"/>
<remap from="~ami_logging_left" to="/$(arg uav_name)/uvdar/ami_logging_left"/>
<remap from="~ami_logging_right" to="/$(arg uav_name)/uvdar/ami_logging_right"/>
<remap from="~ami_logging_back" to="/$(arg uav_name)/uvdar/ami_logging_back"/>
<remap from="~ami_all_seq_info_left" to="/$(arg uav_name)/uvdar/ami_all_seq_info_left"/>
<remap from="~ami_all_seq_info_right" to="/$(arg uav_name)/uvdar/ami_all_seq_info_right"/>
<remap from="~ami_all_seq_info_back" to="/$(arg uav_name)/uvdar/ami_all_seq_info_back"/>
</node>
<node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" output="screen" >
<!-- <node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" launch-prefix="valgrind -/-tool=callgrind -/-callgrind-out-file=/home/viktor/callgrind.out -/-instr-atstart=no -/-collect-atstart=yes"> -->
<!-- <node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" launch-prefix="valgrind -/-tool=callgrind -/-callgrind-out-file=/home/viktor/callgrind.out"> -->
<param name="uav_name" type = "string" value="$(arg uav_name)"/>
<param name="debug" type="bool" value="false"/>
<!-- <param name="debug" type="bool" value="false"/> -->
<!-- <param name="gui" type="bool" value="false"/> -->
<param name="gui" type="bool" value="false"/>
<param name="publish_visualization" type="bool" value="false"/>
<param name="publish_constituents" type="bool" value="true"/>
<!-- <param name="publish_visualization" type="bool" value="false"/> -->
<rosparam param="signal_ids" subst_value="true"> [$(arg id1), $(arg id2), $(arg id3), $(arg id4), $(arg id5), $(arg id6), $(arg id7), $(arg id8), $(arg id9), $(arg id10), $(arg id11), $(arg id12), $(arg id13), $(arg id14), $(arg id15), $(arg id16), $(arg id17), $(arg id18), $(arg id19), $(arg id20), $(arg id21)] </rosparam>

<param name="quadrotor" type="bool" value="true"/>
<param name="custom_model" type="bool" value="true"/>
<param name="model_file" type="string" value="$(arg model_file)"/>
<param name="beacon" type="bool" value="false"/>

<rosparam param="blinkers_seen_topics"> ["blinkers_seen_left", "blinkers_seen_right", "blinkers_seen_back"] </rosparam>
<rosparam param="estimated_framerate_topics"> ["estimated_framerate_left", "estimated_framerate_right", "estimated_framerate_back"] </rosparam>
<rosparam param="camera_frames" subst_value="true"> ["$(arg uav_name)/uvcam_left", "$(arg uav_name)/uvcam_right", "$(arg uav_name)/uvcam_back"] </rosparam>

<remap from="~blinkers_seen_left" to="/$(arg uav_name)/uvdar/blinkers_seen_left/throttled"/>
<remap from="~blinkers_seen_right" to="/$(arg uav_name)/uvdar/blinkers_seen_right/throttled"/>
<remap from="~blinkers_seen_back" to="/$(arg uav_name)/uvdar/blinkers_seen_back/throttled"/>
<remap from="~estimated_framerate_left" to="/$(arg uav_name)/uvdar/estimated_framerate_left"/>
<remap from="~estimated_framerate_right" to="/$(arg uav_name)/uvdar/estimated_framerate_right"/>
<remap from="~estimated_framerate_back" to="/$(arg uav_name)/uvdar/estimated_framerate_back"/>

<rosparam param="calib_files" subst_value="true"> ["$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg left).txt", "$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg right).txt", "$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg back).txt"] </rosparam>

<remap from="~constituentHypotheses" to="/$(arg uav_name)/uvdar/constituentHypotheses"/>
<remap from="~measuredPoses" to="/$(arg uav_name)/uvdar/measuredPoses"/>

<param name="output_frame" type="string" value="$(arg output_frame)"/>
</node>
</group>


</launch>

0 comments on commit 9edb18c

Please sign in to comment.