mirror of
https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git
synced 2026-05-21 01:12:08 +00:00
Merge pull request #7 in MITL/mmwave_ti_ros from multi-sensor-coarse-sync to master
* commit '910d31ec3886380656865b9b6de8a0eb61f0865c': Added mmWaveSync node with HW and SW based sync methods
This commit is contained in:
Pedrhom Nafisi
@@ -220,6 +220,10 @@ add_executable(mmWaveQuickConfig src/mmWaveQuickConfig.cpp)
|
||||
target_link_libraries(mmWaveQuickConfig ${catkin_LIBRARIES} mmwave ${PCL_LIBRARIES})
|
||||
add_dependencies(mmWaveQuickConfig ${catkin_EXPORTED_TARGETS} mmwave)
|
||||
|
||||
add_executable(mmWaveSync src/mmWaveSync.cpp)
|
||||
target_link_libraries(mmWaveSync ${catkin_LIBRARIES} mmwave ti_gpio)
|
||||
add_dependencies(mmWaveSync ${catkin_EXPORTED_TARGETS} mmwave)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
51
ros_driver/src/ti_mmwave_rospkg/cfg/1843_3d_hw_trig.cfg
Normal file
51
ros_driver/src/ti_mmwave_rospkg/cfg/1843_3d_hw_trig.cfg
Normal file
@@ -0,0 +1,51 @@
|
||||
% ***************************************************************
|
||||
% Created for SDK ver:03.03
|
||||
% Created using Visualizer ver:3.3.0.0
|
||||
% Frequency:77
|
||||
% Platform:xWR18xx
|
||||
% Scene Classifier:best_range_res
|
||||
% Azimuth Resolution(deg):15 + Elevation
|
||||
% Range Resolution(m):0.044
|
||||
% Maximum unambiguous Range(m):9.02
|
||||
% Maximum Radial Velocity(m/s):1
|
||||
% Radial velocity resolution(m/s):0.13
|
||||
% Frame Duration(msec):100
|
||||
% Range Detection Threshold (dB):15
|
||||
% Doppler Detection Threshold (dB):15
|
||||
% Range Peak Grouping:enabled
|
||||
% Doppler Peak Grouping:enabled
|
||||
% Static clutter removal:disabled
|
||||
% Angle of Arrival FoV: Full FoV
|
||||
% Range FoV: Full FoV
|
||||
% Doppler FoV: Full FoV
|
||||
% ***************************************************************
|
||||
sensorStop
|
||||
flushCfg
|
||||
dfeDataOutputMode 1
|
||||
channelCfg 15 7 0
|
||||
adcCfg 2 1
|
||||
adcbufCfg -1 0 1 1 1
|
||||
profileCfg 0 77 267 7 57.14 0 0 70 1 256 5209 0 0 30
|
||||
chirpCfg 0 0 0 0 0 0 0 1
|
||||
chirpCfg 1 1 0 0 0 0 0 4
|
||||
chirpCfg 2 2 0 0 0 0 0 2
|
||||
frameCfg 0 2 16 1000 99.75 2 0
|
||||
lowPower 0 0
|
||||
guiMonitor -1 1 1 0 0 0 1
|
||||
cfarCfg -1 0 2 8 4 3 0 15 1
|
||||
cfarCfg -1 1 0 4 2 3 1 15 1
|
||||
multiObjBeamForming -1 1 0.5
|
||||
clutterRemoval -1 0
|
||||
calibDcRangeSig -1 0 -5 8 256
|
||||
extendedMaxVelocity -1 0
|
||||
lvdsStreamCfg -1 0 0 0
|
||||
compRangeBiasAndRxChanPhase 0.0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
|
||||
measureRangeBiasAndRxChanPhase 0 1.5 0.2
|
||||
CQRxSatMonitor 0 3 5 121 0
|
||||
CQSigImgMonitor 0 127 4
|
||||
analogMonitor 0 0
|
||||
aoaFovCfg -1 -90 90 -90 90
|
||||
cfarFovCfg -1 0 0 8.92
|
||||
cfarFovCfg -1 1 -1 1.00
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
@@ -0,0 +1,106 @@
|
||||
Time Sync of Frames from Multiple Sensors
|
||||
=========================================
|
||||
|
||||
The purpose for this page is to document two methods to enable synchronization in time for frames from multiple TI mmWave radar devices with the ROS radar driver. Time based synchronization can be very useful for applications in which data from multiple sensors is used.
|
||||
|
||||
In this scenario four (4) of TI's mmWave EVM's are connected to a host machine via USB. It is assumed that all of the sensors are flashed and running the exact same firmware.
|
||||
|
||||
# Software Based Method
|
||||
|
||||
The software based approach is very simple and enables coarse synchronization (1ms) among frames from multiple mmWave devices for a period of time. It works by configuring all sensors, waiting to send the start command to each device until all devices are fully configured. At that point the start command is sent to each device with an optional user defined delay value between subsequent start commands. The delay can be used to offset frames from different radars in time. One of the limitations of this approach is that the triggering of each frame following the first frame, for each sensor, is controlled by that sensor's own clock. Because of slight variations, over a long period of time, the timing of the frames for one device may become out of sync with that of another.
|
||||
|
||||
# Hardware Based Method
|
||||
|
||||
**Note**: The hardware sync method applies only when the host machine is TI's [SK-TDA4VM](https://www.ti.com/tool/SK-TDA4VM). It is also assumed that the setup of the Robotics SDK and Docker container is complete. If this has not been completed, follow the instructions [here](https://software-dl.ti.com/jacinto7/esd/robotics-sdk/08_02_00/docs/source/docker/README.html).
|
||||
|
||||
This approach utilizes the GPIO PWM functionality on the SK-TDA4VM to generate a triggering signal. The mmWave devices can be configured for HW Trigger mode which means that each frame is triggered at the rising edge of a pulse signal fed into the SYNC_IN pin. The mmWave devices can also be configured with a trigger delay, which allows for the triggering of frames for multiple radars to be offset from one another. This can be beneficial in reducing the probability of seeing "ghost targets" caused by parallel interference.
|
||||
|
||||
This approach has clear advantages in terms of time synchronization. This is because the triggering of frames for all the connected sensors is controlled by a single source. Also, assuming an equal length for the path of the triggering pulse signal to each sensor, the sensors all recieve the signal at the same instant.
|
||||
|
||||
**Note**: HW based synchronization was tested with TI's IWR1843BOOST EVM due to the SYNC_IN pin being accessible without board modification. Modification may be required in order to access the SYNC_IN pin on other EVMs.
|
||||
|
||||
# Using the mmWaveSync node
|
||||
|
||||
## SW based synchronization
|
||||
To use the SW based sync, launch the driver with `sw_sync_quad_sensor.launch`. Below is the part of the launch file relating to the sync node.
|
||||
```
|
||||
<node pkg="ti_mmwave_rospkg" type="mmWaveSync" name="mmWaveSync" ns="ti_mmwave_sync" output="screen">
|
||||
<param name="sync_type" value="SW" />
|
||||
<param name="num_sensors" value="4" />
|
||||
<param name="trigger_delay" value="0" />
|
||||
<param name="serial_ports" value="/dev/ttyACM0 /dev/ttyACM2" />
|
||||
<param name="config_file" value="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" />
|
||||
</node>
|
||||
```
|
||||
|
||||
## HW based synchronization
|
||||
The hardware based synchronization method requires the ti-gpio-cpp package. Run the following on the TDA4 host linux:
|
||||
```
|
||||
git clone https://github.com/TexasInstruments/ti-gpio-cpp.git
|
||||
mkdir -p ti-gpio-cpp/build && cd ti-gpio-cpp/build
|
||||
cmake ..
|
||||
make
|
||||
make install
|
||||
```
|
||||
|
||||
The hardware synchronization method requires that a signal is fed from a pin on the 40-pin header on the SK board to the SYNC_IN pin on all of the connected sensors. On the xWR1843BOOST evm the SYNC_IN pin can be accessed via pin 9 of J6. For other devices, use the schematics to find where the SYNC_IN pin can be accessed.
|
||||
|
||||
|
||||
|
||||
Figure 1. Radar Connections for HW Sync.
|
||||
|
||||
|
||||
|
||||
Figure 2. SK Board Connections for HW Sync.
|
||||
|
||||
By default, the 40-pin header is not enabled on TDA4VM SK board. This can be enabled by specifying the dtb overlay file `k3-j721e-sk-rpi-exp-header.dtbo` in `/run/media/mmcblk0p1/uenv.txt` as given below:
|
||||
```
|
||||
name_overlays=k3-j721e-edgeai-apps.dtbo k3-j721e-sk-rpi-exp-header.dtbo
|
||||
```
|
||||
|
||||
To use the HW based sync, launch the driver with `hw_sync_quad_sensor.launch`. Below is the part of the launch file relating to the sync node.
|
||||
```
|
||||
<node pkg="ti_mmwave_rospkg" type="mmWaveSync" name="mmWaveSync" ns="ti_mmwave_sync" output="screen">
|
||||
<param name="sync_type" value="HW" />
|
||||
<param name="num_sensors" value="4" />
|
||||
<param name="pwm_pin" value="32" />
|
||||
<param name="pulse_freq" value="10" />
|
||||
<param name="duty_cycle" value="1" />
|
||||
<param name="trigger_delay" value="0" />
|
||||
<param name="config_file" value="$(find ti_mmwave_rospkg)/cfg/1843_3d_hw_trig.cfg" />
|
||||
</node>
|
||||
```
|
||||
|
||||
**Note**: For HW based sync, user must define valid parameters for triggering pulse generation:
|
||||
* Pulse periodicity should be greater than the sum of the frame period and the trigger_delay
|
||||
* Pulse width must be greater than 25ns
|
||||
* HW PWM pins on SK-TDA4VM are {29, 31, 32, 33}
|
||||
|
||||
## mmWaveSync Parameters
|
||||
|
||||
### Common Parameters
|
||||
Parameter | Description | Value
|
||||
----------|-------------|------
|
||||
sync_type | Select synchronization type | "SW", "HW", "none"
|
||||
num_sensors | Total number of mmWave sensors connected to Host | int
|
||||
trigger_delay | Duration in ms to offset the start of each frame from sensor to sensor | float
|
||||
config_file | Path to cfg file used to configure sensors | string
|
||||
|
||||
### SW Parameters
|
||||
Parameter | Description | Value
|
||||
----------|-------------|------
|
||||
serial_ports | list of serial ports (com_user) for each device | string (each port should be separated by a space)
|
||||
|
||||
### HW Parameters
|
||||
Parameter | Description | Value
|
||||
----------|-------------|------
|
||||
pwm_pin | GPIO pin from 40-pin header on SK-TDA4VM used to generate triggering pulse | int
|
||||
pulse_freq | Frequency of the triggering pulse in Hz | int
|
||||
duty_cycle | Duty cycle percentage of the triggering pulse | float
|
||||
|
||||
## Limitations
|
||||
|
||||
* For SW based synchronization, actual delay value will be accurate to +-1ms
|
||||
* Known issue for SW based sync where radar's first frame will take much longer than expected voiding synchronization
|
||||
* `trigger_delay` for HW based sync is limmited to a max of 100us due to device firmware constraints. It may be possible to increase this maximum by modifying the firmware flashed to the device.
|
||||
* HW based sync requires that the SYNC_IN pin be acessible on the TI mmWave device. Some EVMs may require modification to access the SYNC_IN pin.
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 151 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 234 KiB |
93
ros_driver/src/ti_mmwave_rospkg/include/mmWaveSync.h
Normal file
93
ros_driver/src/ti_mmwave_rospkg/include/mmWaveSync.h
Normal file
@@ -0,0 +1,93 @@
|
||||
#include <stdio.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <regex>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <iterator>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <signal.h>
|
||||
|
||||
#include <GPIO.h>
|
||||
#include <serial/serial.h>
|
||||
#include "ParameterParser.h"
|
||||
#include "ti_mmwave_rospkg/mmWaveCLI.h"
|
||||
|
||||
class Sync {
|
||||
public:
|
||||
Sync (ros::NodeHandle, ros::NodeHandle);
|
||||
~Sync() {};
|
||||
|
||||
virtual void start () {configureSensors();};
|
||||
|
||||
/** Boolean to indicate if the node should terminate */
|
||||
bool m_done;
|
||||
|
||||
private:
|
||||
/** Function which uses the mmWaveCLI service to configure mmWave sensors */
|
||||
void configureSensors();
|
||||
|
||||
/** Synchronization type: HW, SW, or none */
|
||||
std::string m_syncType;
|
||||
|
||||
/** Path to .cfg file used to configure sensors */
|
||||
std::string m_configFile;
|
||||
|
||||
protected:
|
||||
ros::NodeHandle m_nh;
|
||||
|
||||
/** Total number of sensors */
|
||||
int32_t m_numSensors;
|
||||
|
||||
/** Duration to wait between sending start command to subsequent sensors */
|
||||
float m_delay;
|
||||
};
|
||||
|
||||
/* SW based synchronization object. */
|
||||
class SWSync : public Sync {
|
||||
public:
|
||||
SWSync (ros::NodeHandle, ros::NodeHandle);
|
||||
~SWSync();
|
||||
|
||||
void start () override { startSyncSW(); };
|
||||
|
||||
private:
|
||||
/** Contains pointers to serial object for each sensor */
|
||||
std::vector<serial::Serial*> m_serialObjects;
|
||||
|
||||
/** SW based sync start function. */
|
||||
void startSyncSW();
|
||||
};
|
||||
|
||||
/* HW based synchronization object. */
|
||||
class HWSync : public Sync {
|
||||
public:
|
||||
HWSync(ros::NodeHandle, ros::NodeHandle);
|
||||
~HWSync() {};
|
||||
|
||||
void start () override { startSyncHW(); };
|
||||
|
||||
private:
|
||||
/** Pin on SK-TDA4VM 40-pin header for generating triggering pulse */
|
||||
int32_t m_pwmPin;
|
||||
|
||||
/** Duty cycle percentage for the triggering pulse */
|
||||
float m_dutyCycle;
|
||||
|
||||
/** Frequency in Hz for the triggering pulse */
|
||||
int32_t m_pulseFreq;
|
||||
|
||||
std::map<std::string, int32_t> m_outputPins;
|
||||
|
||||
int32_t get_output_pin();
|
||||
|
||||
/** HW based sync start function. */
|
||||
void startSyncHW();
|
||||
};
|
||||
@@ -14,11 +14,10 @@
|
||||
<arg name="device_num" default="0"/>
|
||||
<arg name="com_user" default="/dev/ttyUSB0"/>
|
||||
<arg name="com_data" default="/dev/ttyUSB1"/>
|
||||
<arg name="node_start_delay" default="0"/>
|
||||
<arg name="tf_args" default="0 0 0 0 0 0" doc="Values to be used for tf transformation"/>
|
||||
|
||||
<!-- mmWave_Manager node -->
|
||||
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_$(arg device_num)" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'">
|
||||
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_$(arg device_num)" output="screen">
|
||||
<param name="command_port" value="$(arg com_user)" />
|
||||
<param name="command_rate" value="115200" />
|
||||
<param name="data_port" value="$(arg com_data)" />
|
||||
@@ -31,10 +30,10 @@
|
||||
</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" >
|
||||
<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 $@'"/>
|
||||
<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>
|
||||
|
||||
@@ -11,7 +11,6 @@
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB0"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB1"/>
|
||||
<arg name="node_start_delay" value="0"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
|
||||
@@ -19,7 +18,6 @@
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB2"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB3"/>
|
||||
<arg name="node_start_delay" value="0"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
|
||||
@@ -27,7 +25,6 @@
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB4"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB5"/>
|
||||
<arg name="node_start_delay" value="0"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
|
||||
@@ -35,7 +32,6 @@
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB6"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB7"/>
|
||||
<arg name="node_start_delay" value="0"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
<!--
|
||||
This file will launch four instances of the mmWave sensor node and configure each TI mmWave sensor using a 3D config. This launch file is intended
|
||||
to be used to launch the mmWave sensor node on TI's TDA4VM Jacinto Processors, as such a separate command must be used to launch RViz
|
||||
for visualization on a PC. This File also launches mmWaveSync, a node which implements a user selected HW or SW based sync.
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<node pkg="ti_mmwave_rospkg" type="mmWaveSync" name="mmWaveSync" ns="ti_mmwave_sync" output="screen">
|
||||
<param name="sync_type" value="HW" />
|
||||
<param name="num_sensors" value="4" />
|
||||
<param name="pwm_pin" value="32" />
|
||||
<param name="pulse_freq" value="10" />
|
||||
<param name="duty_cycle" value="0.001" />
|
||||
<param name="trigger_delay" value="0" />
|
||||
<param name="config_file" value="$(find ti_mmwave_rospkg)/cfg/1843_3d_hw_trig.cfg" />
|
||||
</node>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="0" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyACM0"/>
|
||||
<arg name="com_data" value="/dev/ttyACM1"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="1" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyACM2"/>
|
||||
<arg name="com_data" value="/dev/ttyACM3"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="2" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyACM4"/>
|
||||
<arg name="com_data" value="/dev/ttyACM5"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="3" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyACM6"/>
|
||||
<arg name="com_data" value="/dev/ttyACM7"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,45 @@
|
||||
<!--
|
||||
This file will launch four instances of the mmWave sensor node and configure each TI mmWave 6843 sensor using a 3D config. This launch file is intended
|
||||
to be used to launch the mmWave sensor node on TI's TDA4VM Jacinto Processors, as such a separate command must be used to launch rViz
|
||||
for visualization on a PC. This File also launches mmWaveSync, a node which implements a user selected HW or SW based sync.
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<node pkg="ti_mmwave_rospkg" type="mmWaveSync" name="mmWaveSync" ns="ti_mmwave_sync" output="screen">
|
||||
<param name="sync_type" value="SW" />
|
||||
<param name="num_sensors" value="4" />
|
||||
<param name="trigger_delay" value="0" />
|
||||
<param name="serial_ports" value="/dev/ttyUSB0 /dev/ttyUSB2 /dev/ttyUSB4 /dev/ttyUSB6" />
|
||||
<param name="config_file" value="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" />
|
||||
</node>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="0" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB0"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB1"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="1" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB2"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB3"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="2" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB4"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB5"/>
|
||||
</include>
|
||||
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/sync_single_sensor.launch">
|
||||
<arg name="device_num" value="3" />
|
||||
<arg name="tf_args" value="0 0 0 0 0 0" />
|
||||
<arg name="com_user" value="/dev/ttyUSB6"/>
|
||||
<arg name="com_data" value="/dev/ttyUSB7"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,32 @@
|
||||
<!--
|
||||
This file will launch the mmWave sensor node and configure a TI mmWave sensor using a 3D config. This launch file is intended
|
||||
to be used to launch the mmWave sensor node on TI's TDA4VM Jacinto Processors, as such a separate command must be used to launch rViz
|
||||
for visualization on a PC.
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- Input arguments -->
|
||||
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
<arg name="device_num" default="0"/>
|
||||
<arg name="com_user" default="/dev/ttyUSB0"/>
|
||||
<arg name="com_data" default="/dev/ttyUSB1"/>
|
||||
<arg name="tf_args" default="0 0 0 0 0 0" doc="Values to be used for tf transformation"/>
|
||||
|
||||
<!-- mmWave_Manager node -->
|
||||
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_$(arg device_num)" output="screen" >
|
||||
<param name="command_port" value="$(arg com_user)" />
|
||||
<param name="command_rate" value="115200" />
|
||||
<param name="data_port" value="$(arg com_data)" />
|
||||
<param name="data_rate" value="921600" />
|
||||
<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>
|
||||
|
||||
<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>
|
||||
270
ros_driver/src/ti_mmwave_rospkg/src/mmWaveSync.cpp
Normal file
270
ros_driver/src/ti_mmwave_rospkg/src/mmWaveSync.cpp
Normal file
@@ -0,0 +1,270 @@
|
||||
#include "mmWaveSync.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
Sync *g_syncObjPtr;
|
||||
|
||||
void signalHandler(int32_t s)
|
||||
{
|
||||
g_syncObjPtr->m_done = true;
|
||||
}
|
||||
|
||||
Sync::Sync (ros::NodeHandle nh, ros::NodeHandle private_nh)
|
||||
{
|
||||
m_nh = nh;
|
||||
m_done = false;
|
||||
m_configFile = "";
|
||||
|
||||
private_nh.getParam("trigger_delay", m_delay);
|
||||
private_nh.getParam("num_sensors", m_numSensors);
|
||||
private_nh.getParam("sync_type", m_syncType);
|
||||
private_nh.getParam("config_file", m_configFile);
|
||||
}
|
||||
|
||||
void Sync::configureSensors()
|
||||
{
|
||||
ti_mmwave_rospkg::mmWaveCLI srv;
|
||||
if (m_configFile == "")
|
||||
{
|
||||
ROS_ERROR("mmWaveSync: Missing config file!");
|
||||
m_done = true;
|
||||
return;
|
||||
}
|
||||
|
||||
std::string mmWaveCLIName;
|
||||
ros::ServiceClient client;
|
||||
ti_mmwave_rospkg::ParameterParser parser;
|
||||
|
||||
for (int32_t i = 0; i < m_numSensors; i++)
|
||||
{
|
||||
std::ifstream myParams;
|
||||
myParams.open(m_configFile);
|
||||
|
||||
mmWaveCLIName = "/mmWaveCLI_" + std::to_string(i);
|
||||
|
||||
//wait for service to become available
|
||||
ros::service::waitForService(mmWaveCLIName, 10000);
|
||||
client = m_nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>(mmWaveCLIName);
|
||||
|
||||
if (myParams.is_open())
|
||||
{
|
||||
while( std::getline(myParams, srv.request.comm))
|
||||
{
|
||||
// Remove Windows carriage-return if present
|
||||
srv.request.comm.erase(std::remove(srv.request.comm.begin(), srv.request.comm.end(), '\r'), srv.request.comm.end());
|
||||
// Ignore comment lines (first non-space char is '%') or blank lines
|
||||
if (!(std::regex_match(srv.request.comm, std::regex("^\\s*%.*")) || std::regex_match(srv.request.comm, std::regex("^\\s*"))))
|
||||
{
|
||||
if (std::regex_search(srv.request.comm, std::regex("frameCfg")) && m_syncType == "HW")
|
||||
{
|
||||
std::istringstream iss(srv.request.comm);
|
||||
std::vector<std::string> tokens{std::istream_iterator<std::string>{iss},
|
||||
std::istream_iterator<std::string>{}};
|
||||
|
||||
// Set Trigger Delay value in frameCfg
|
||||
tokens[7] = std::to_string(i * m_delay);
|
||||
|
||||
srv.request.comm = "";
|
||||
for (const auto &piece : tokens) srv.request.comm += (piece + ' ');
|
||||
}
|
||||
|
||||
if (std::regex_search(srv.request.comm, std::regex("sensorStart")) && m_syncType == "SW")
|
||||
{
|
||||
// exit without sending sensor start
|
||||
break;
|
||||
}
|
||||
|
||||
if (client.call(srv))
|
||||
{
|
||||
if (std::regex_search(srv.response.resp, std::regex("Done")))
|
||||
{
|
||||
parser.ParamsParser(srv, m_nh);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("mmWaveSync: Command failed (mmWave sensor did not respond with 'Done')");
|
||||
ROS_ERROR("mmWaveSync: Response: '%s'", srv.response.resp.c_str() );
|
||||
m_done = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("mmWaveSync: Failed to call service mmWaveCLI");
|
||||
ROS_ERROR("%s", srv.request.comm.c_str() );
|
||||
m_done = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
parser.CalParams(m_nh);
|
||||
myParams.close();
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("mmWaveSync: Failed to open File %s", m_configFile.c_str());
|
||||
m_done = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
HWSync::HWSync (ros::NodeHandle nh, ros::NodeHandle private_nh) : Sync::Sync(nh, private_nh)
|
||||
{
|
||||
// Populate pulse parameters with user arguments from launch file
|
||||
private_nh.getParam("pulse_freq", m_pulseFreq);
|
||||
private_nh.getParam("duty_cycle", m_dutyCycle);
|
||||
private_nh.getParam("pwm_pin", m_pwmPin);
|
||||
}
|
||||
|
||||
int32_t HWSync::get_output_pin()
|
||||
{
|
||||
if (m_outputPins.find(GPIO::model) == m_outputPins.end())
|
||||
{
|
||||
ROS_INFO("PWM not supported on this board\n");
|
||||
terminate();
|
||||
}
|
||||
|
||||
return m_outputPins.at(GPIO::model);
|
||||
}
|
||||
|
||||
void HWSync::startSyncHW()
|
||||
{
|
||||
Sync::start();
|
||||
|
||||
m_outputPins["J721E_SK"] = m_pwmPin;
|
||||
|
||||
// Pin Definitions
|
||||
int32_t output_pin = get_output_pin();
|
||||
|
||||
// Pin Setup
|
||||
// Board pin-numbering scheme
|
||||
GPIO::setmode(GPIO::BOARD);
|
||||
|
||||
// set pin as an output pin with optional initial state of LOW
|
||||
GPIO::setup(output_pin, GPIO::OUT, GPIO::LOW);
|
||||
GPIO::PWM p(output_pin, m_pulseFreq);
|
||||
|
||||
// Enable the PWM
|
||||
ROS_INFO("mmWaveSync: Starting HW triggering pulse with frequency: %dhz and duty cycle: %f pct.", m_pulseFreq, m_dutyCycle);
|
||||
p.start(m_dutyCycle);
|
||||
|
||||
// Wait for user interrupt via CTRL+C
|
||||
while (!m_done)
|
||||
{
|
||||
ros::Duration(1).sleep();
|
||||
}
|
||||
|
||||
// Clean up gpio pins
|
||||
p.stop();
|
||||
GPIO::cleanup();
|
||||
}
|
||||
|
||||
SWSync::SWSync (ros::NodeHandle nh, ros::NodeHandle private_nh) : Sync::Sync(nh, private_nh)
|
||||
{
|
||||
m_delay = m_delay / 1000;
|
||||
|
||||
std::vector<serial::Serial*> nulled(m_numSensors);
|
||||
m_serialObjects = nulled;
|
||||
|
||||
std::string port_names;
|
||||
private_nh.getParam("serial_ports", port_names);
|
||||
ROS_INFO("%s", port_names.c_str());
|
||||
|
||||
std::istringstream iss(port_names);
|
||||
std::vector<std::string> ports{std::istream_iterator<std::string>{iss},
|
||||
std::istream_iterator<std::string>{}};
|
||||
|
||||
for (int32_t i = 0; i < m_numSensors; i++)
|
||||
{
|
||||
/* Create Serial port object for each device */
|
||||
serial::Serial *mySerialObject = new serial::Serial("", 115200, serial::Timeout::simpleTimeout(1000));
|
||||
mySerialObject->setPort(ports[i]);
|
||||
m_serialObjects[i] = mySerialObject;
|
||||
}
|
||||
}
|
||||
|
||||
SWSync::~SWSync()
|
||||
{
|
||||
for (int32_t i = 0; i < m_numSensors; i++)
|
||||
{
|
||||
delete m_serialObjects[i];
|
||||
}
|
||||
}
|
||||
|
||||
void SWSync::startSyncSW()
|
||||
{
|
||||
Sync::start();
|
||||
|
||||
std::string response;
|
||||
|
||||
for (int32_t i = 0; i < m_numSensors; i++)
|
||||
{
|
||||
/* Open Serial port and error check */
|
||||
try
|
||||
{
|
||||
m_serialObjects[i]->open();
|
||||
}
|
||||
catch (std::exception &e1)
|
||||
{
|
||||
ROS_INFO("mmWaveSync: Failed to open User serial port with error: %s", e1.what());
|
||||
}
|
||||
}
|
||||
|
||||
/* Send start command to each device */
|
||||
ROS_INFO("mmWaveSync: Sending syncronized sensorStart command to devices.");
|
||||
for (int32_t i = 0; i < m_numSensors; i++)
|
||||
{
|
||||
int32_t bytesSent = m_serialObjects[i]->write("sensorStart\n");
|
||||
ros::Duration(m_delay).sleep();
|
||||
}
|
||||
|
||||
/* Read response from each device and close serial port */
|
||||
for (int32_t i = 0; i < m_numSensors; i++)
|
||||
{
|
||||
response = "";
|
||||
m_serialObjects[i]->readline(response, 1024, ":/>");
|
||||
ROS_INFO("mmWaveSync: Received response from sensor %d: '%s'", i, response.c_str());
|
||||
|
||||
m_serialObjects[i]->close();
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "mmWaveSync");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle private_nh("~");
|
||||
|
||||
std::string sync_type;
|
||||
private_nh.getParam("sync_type", sync_type);
|
||||
if (sync_type == "none")
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else if (sync_type == "SW")
|
||||
{
|
||||
ROS_INFO("mmWaveSync: SW Synchronization enabled");
|
||||
|
||||
g_syncObjPtr = new SWSync(nh, private_nh);
|
||||
}
|
||||
else if (sync_type == "HW")
|
||||
{
|
||||
ROS_INFO("mmWaveSync: HW Synchronization enabled");
|
||||
|
||||
g_syncObjPtr = new HWSync(nh, private_nh);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_INFO("mmWaveSync: Sync not enabled! 'sync_type' must be 'SW' 'HW' or 'none'");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// When CTRL+C pressed, signalHandler will be called
|
||||
signal(SIGINT, signalHandler);
|
||||
|
||||
g_syncObjPtr->start();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user