Parametrized CLI service name

This commit is contained in:
Josh Dye
2022-03-21 15:31:37 -05:00
parent 2e4f4523a1
commit 5f4426fe55
23 changed files with 90 additions and 24 deletions

View File

@@ -76,6 +76,8 @@ class mmWaveCommSrv : public nodelet::Nodelet
std::string mySerialPort;
int myBaudRate;
std::string mmWaveCLIName;
}; //Class mmWaveCommSrv
} //namespace ti_mmwave_rospkg

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -19,12 +19,15 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 -1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -26,11 +26,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="ti_mmwave_$(arg device_num)"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI_$(arg device_num)" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_$(arg device_num)"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_$(arg device_num)" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'"/>
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_$(arg device_num)" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'">
<param name="mmWaveCLI_name" value="/mmWaveCLI_$(arg device_num)" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_$(arg device_num)" args="$(arg tf_args) ti_mmwave_pcl ti_mmwave_$(arg device_num) 100" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="5 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

View File

@@ -25,10 +25,13 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642es2_short_range.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642es2_short_range.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

View File

@@ -15,11 +15,12 @@ void mmWaveCommSrv::onInit()
private_nh2.getParam("command_port", mySerialPort);
private_nh2.getParam("command_rate", myBaudRate);
private_nh2.getParam("mmWaveCLI_name", mmWaveCLIName);
ROS_INFO("mmWaveCommSrv: command_port = %s", mySerialPort.c_str());
ROS_INFO("mmWaveCommSrv: command_rate = %d", myBaudRate);
commSrv = private_nh.advertiseService("/mmWaveCLI", &mmWaveCommSrv::commSrv_cb, this);
commSrv = private_nh.advertiseService(mmWaveCLIName, &mmWaveCommSrv::commSrv_cb, this);
NODELET_DEBUG("mmWaveCommsrv: Finished onInit function");
}

View File

@@ -9,6 +9,7 @@
int main(int argc, char **argv) {
ros::init(argc, argv, "mmWaveQuickConfig");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
ti_mmwave_rospkg::mmWaveCLI srv;
if (argc != 2) {
ROS_INFO("mmWaveQuickConfig: usage: mmWaveQuickConfig /file_directory/params.cfg");
@@ -16,11 +17,13 @@ int main(int argc, char **argv) {
} else
ROS_INFO("mmWaveQuickConfig: Configuring mmWave device using config file: %s", argv[1]);
ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>("/mmWaveCLI");
std::string mmWaveCLIName;
private_nh.getParam("mmWaveCLI_name", mmWaveCLIName);
ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>(mmWaveCLIName);
std::ifstream myParams;
ti_mmwave_rospkg::ParameterParser parser;
//wait for service to become available
ros::service::waitForService("/mmWaveCLI", 10000);
ros::service::waitForService(mmWaveCLIName, 10000);
//wait 0.5 secs to avoid multi-sensor conflicts
ros::Duration(0.5).sleep();