yemai/mmwave_ti_ros
1
0
Fork 0
mirror of https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git synced 2026-05-21 09:22:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial version baselined from ITB 4.5.1

Browse Source 
Signed-off-by: Yogesh Siraswar <yogeshs@ti.com>
This commit is contained in:
Yogesh Siraswar
2020-11-30 15:24:10 -06:00
commit 33b26c8afe
573 changed files with 289126 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
ros_driver/docs/TI_mmWave_ROS_Driver_Release_Notes.html Normal file
View File

@@ -0,0 +1,115 @@
<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<title>TI mmWave ROS Driver Release Notes - Version 2.1 </title>
</head>
<!-- START PRE -->
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/fonts/ubuntu-regular-woff.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/fonts/glyphicons-halflings-regular.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/themes/united2.min.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/themes/bootstrap-responsive.min.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/strapdown.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/styles/link.css"/>
<!-- END PRE -->
<xmp style="display:none;" class="col-xs-6">
TI mmWave ROS Driver Release Notes
===================================
Overview
========
The TI mmWave ROS driver integrates the mmWave out-of-box demo with Robot OS (ROS) in Linux. It sets up a ROS service as an interface to configure the mmWave Evaluation Module and publishes a ROS PointCloud2 message with the objects detected when the sensor is activated.
Features
========
- Compatible with TI mmWave SDK v3.4.0.3 out-of-box demo when using IWR6843AOPEVM
- Compatible with TI mmWave SDK v3.4.0.3 out-of-box demo when using IWR6843 ES2.0 EVM
- Offers the out-of-box demo Command Line Interface (CLI) as a ROS Service
- Reads Radar data from the mmWave EVM and publishes it as a ROS PointCloud2 Message
- Outputs the {X,Y,Z} coordinate and intensity (snr) of each detected point in the ROS PointCloud2 Message
- {X,Y,Z} coordinates use the ROS standard coordinate system where X=forward, Y=left, Z=up (see http://www.ros.org/reps/rep-0103.html for more details)
Limitations
========================
The following is a list of known limitations for this release that were known at the time of release.
- Currently tested for the IWR6843 EVM and IWR6843AOPEVM only
- The mmWave EVM must be configured to only send the "detected objects¨ data. The other possible output data types such as "log magnitude range¨, "noise profile¨, heat maps, etc. are not currently supported by the ROS driver. Also, the Doppler/velocity field is not currently output in the ROS PointCloud2 Message.
Changes in Version 2.0
========================
The following is a list of changes compared to the previous release.
- Added support for multiple devices runing and displaying pointcloud on same RVIZ visualizer
- Launch file for mulitple devices included
- Source code was updated, update sourced from https://github.com/radar-lab/ti_mmwave_rospkg
Changes in Version 1.8
========================
The following is a list of changes compared to the previous release.
- Note on editing the cfg file added to FAQ section
Changes in Version 1.7
========================
The following is a list of changes compared to the previous release.
- Driver source code modified to work with out-of-box demo from TI mmWave SDK 3.2.0.6 (IWR6843AOPEVM only) and TI mmWave SDK 3.3.0.1 (IWR6843 ES2 only and IWR1843)
- Chirp profile config (.cfg) file added for IWR1843 was added
- Launch file added for IWR1843
Changes in Version 1.6
========================
The following is a list of changes compared to the previous release.
- Driver source code modified to work with out-of-box demo from TI mmWave SDK 3.2.0.6 (IWR6843AOPEVM only) and TI mmWave SDK 3.2.0.4 (IWR6843 ES1.0 EVM only)
- Chirp profile config (.cfg) file added for IWR6843AOPEVM
- Launch file added for IWR6843AOPEVM
Changes in Version 1.5
========================
The following is a list of changes compared to the previous release.
- Driver source code modified to work with out-of-box demo from TI mmWave SDK 2.1.0.4 (xWR1443 ES3.0 EVM or xWR1642 ES2.0 EVM only), TI mmWave SDK 3.0.0.8 (IWR6843 ES1.0 EVM only)
- Chirp profile config (.cfg) file added for IWR6843 EVM
Changes in Version 1.4
========================
The following is a list of changes compared to the previous release.
- xWR1642 chirp profile config (.cfg) files modified to work with out-of-box demo from TI mmWave SDK 2.0.0.4 (requires xWR1642 ES2.0 EVM)
Changes in Version 1.3
========================
The following is a list of changes compared to the previous release.
- Chirp profile config (.cfg) files modified to work with out-of-box demo from TI mmWave SDK 1.2.0.5
Changes in Version 1.2.1
========================
The following is a list of changes compared to the previous release.
- Fixed syntax error in mmWave_nodelets.xml file in driver package which caused driver to fail with newer releases of ROS Kinetic distribution
Changes in Version 1.2
========================
The following is a list of changes compared to the previous release.
- Chirp profile config (.cfg) files modified to work with out-of-box demo from TI mmWave SDK 1.1.0.2
- Driver source code modified to be compatible with output data format used in out-of-box demo from TI mmWave SDK 1.1.0.2
</xmp>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/highlight/styles/zenburn.css"/>
<script src="../../../../.metadata/.html/scripts/strapdown/vendor/jquery-1.11.2.min.js"></script>
<script src="../../../../.metadata/.html/scripts/strapdown/vendor/bootstrap.min.js"></script>
<script src="../../../../.metadata/.html/scripts/highlight/highlight.pack.js"></script>
<script src="../../../../.metadata/.html/scripts/strapdown/v/0.2/strapdown.js"></script>
<script src="../../../../.metadata/.html/scripts/toolbox.js"></script>
<script src="../../../../.metadata/.html/scripts/toolbox_fixes.js"></script>
</html>

BIN
ros_driver/docs/TI_mmWave_ROS_Driver_Release_Notes.pdf Normal file
View File

Binary file not shown.

534
ros_driver/docs/TI_mmWave_ROS_Driver_Users_Guide.html Normal file
View File

@@ -0,0 +1,534 @@
<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<title>TI mmWave ROS Driver User's Guide - Version 2.0</title>
</head>
<!-- START PRE -->
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/fonts/ubuntu-regular-woff.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/fonts/glyphicons-halflings-regular.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/themes/united2.min.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/themes/bootstrap-responsive.min.css"/>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/strapdown/v/0.2/strapdown.css"/>
<!-- END PRE -->
<xmp style="display:none;" class="col-xs-6">
## TI mmWave ROS Driver
--------------------------
## Purpose
This document exists to aid TI mmWave Evaluation Module (EVM) users who wish to leverage the Robot Operating System (ROS) framework for their applications.
## Objectives
- Bring up ROS on a Linux machine
- Build the ti_mmwave_rospkg package in the ROS workspace
- Launch the ti_mmwave_rospkg node to start the mmWave EVM
- Visualize the Radar point cloud using Rviz
- Reconfigure the mmWave EVM with mmWaveQuickConfig node (optional)
## Requirements
### Prerequisite
[[y! Run Out of Box Demo
Before continuing with this lab, users should first run the out of box demo for the EVM.
This will enable users to gain familiarity with the sensor's capabilities as well as the various tools used across all labs in the mmWave Industrial Toolbox. ]]
### Required and Supported mmWave Evaluation Modules (EVM)
#### ISK module with MMWAVEICBOOST
Quantity | Item
------------------|-----------------
1 | Antenna Module Board: [IWR6843ISK](http://www.ti.com/tool/IWR6843ISK)
1 | OPTIONAL: [Industrial mmWave Carrier Board](http://www.ti.com/tool/MMWAVEICBOOST) for CCS based debugging
OR
#### AOP EVM
Quantity | Item
------------------|-----------------
1 | [IWR6843AOPEVM](http://www.ti.com/tool/IWR6843AOPEVM)
1 | OPTIONAL: [Industrial mmWave Carrier Board](http://www.ti.com/tool/MMWAVEICBOOST) for CCS based debugging
[[r! IWR6843 ES2.0 Only
This lab is only compatible with ES2.0 version of IWR6843.
On ISK or ODS, check the device version on your IWR6843 using the on-chip device markings as shown below
1. If line 4 reads `678A`, you have an ES2 device. In this case, this lab is compatible with your EVM.
2. If line 4 reads `60 GHZi`, you have an older ES1 device. In this case, the lab is NOT compatible with your EVM. ES2 IWR6843ISK/IWR6843ISK-ODS boards are orderable from the EVM link above.
<img src="images/iwr6843_silicon_revision.png" width="300"/>
On AOP, the EVM must be Rev F or later. This can be distinguished by the shape of the EVM if it is as shown above.
]]
[[r! AoP ES2.0 EVM only
The IWR6843 AoP version of this lab is only compatible with ES2.0 silicon and the corresponding EVM. Please ensure your EVM is the same as in the below image.
<img src="images/iwr6843aopevm1.png" width="300"/>
]]
### Software
#### Host machine
- Ubuntu 16.04 LTS operating system (native Linux machine preferred but will also work with Linux virtual machine running under Windows 7/Windows 10)
- Robot Operating System (ROS)
- ti_mmwave_rospkg source code
#### MMWAVE EVM
BIN Name | Board | Location
------------------------------|-----------------|-----------
xwr68xx_mmw_demo.bin | IWR6843ISK | `<INDUSTRIAL_TOOLBOX_INSTALL_DIR>\mmwave_industrial_toolbox_<VER>\out_of_box_demo\`<br>`68xx_mmwave_sdk_dsp\prebuilt_binaries\xwr68xx_mmw_demo.bin`
xwr64xxAOP_mmw_demo.bin | IWR6843AOPEVM | `<INDUSTRIAL_TOOLBOX_INSTALL_DIR>\mmwave_industrial_toolbox_<VER>\out_of_box_demo\`<br>`68xx_aop_mmwave_sdk_hwa\prebuilt_binaries\xwr64xxAOP_mmw_demo.bin`
## Compatibility
The ti_mmwave_rospkg source code has been tested with ROS Kinetic Kame on an x86_64 machine with Ubuntu 16.04 LTS. It has also been tested under an Ubuntu 16.04 LTS VirtualBox Virtual Machine (VM) under Windows 7 and Windows 10. Please follow all of the instructions in the section titled “Tips for Usage with a Virtual Machine” if you are using an Ubuntu Virtual Machine.
## Why ROS?
The Robot Operating System (ROS) is a flexible framework for writing robot software. It is a collection of tools, libraries, and conventions that aim to simplify the task of creating complex and robust robot behavior across a wide variety of robotic platforms. From a high level perspective the TI mmWave Out-of-Box demo provides these functions:
- Be able to specify desired FMCW Radar chirp profile through command line interface (CLI) on a UART port
- Do 1D, 2D, CFAR, Azimuth and Elevation processing and stream out intensity, relative radial velocity and three spatial coordinates (x,y,z) of the detected objects in real-time.
Because the ROS framework and libraries support point cloud data, ROS is useful for developing and prototyping with the mmWave EVMs for robotics applications.
## Installing ROS
Before installing ROS make sure you have a properly working Ubuntu 16.04 LTS host machine. If you are using an Ubuntu virtual machine please go to the section titled “Tips for Usage with a Virtual Machine” and make sure to read/follow all of the instructions there before continuing.
To install ROS on your machine follow the instructions provided on the following webpage. Please select the full desktop (ros-kinetic-desktop-full) version. http://wiki.ros.org/kinetic/Installation/Ubuntu
After installation, if you are new to ROS it will be helpful to read and complete the tutorials found at the following link to familiarize yourself with the framework.
http://wiki.ros.org/ROS/Tutorials
## Using the ti_mmwave_rospkg ROS driver
### User permissions
Your user account must be a member of the “dialout” unix group in order to have access to the serial ports on Linux. To check if you are a member of this group run the “groups” command.
If you are already a member of the “dialout” group the output should list “dialout” (in addition to other groups).
If you are not a member of the “dialout” group, run the following command to add your user account to that group and then logout and login to Linux to make the change take effect:
```
$ sudo adduser <your_username> dialout
```
After executing this command please logout and login to Linux so the change will take effect.
### Building the TI mmWave ROS driver
1. Follow the instructions on the following webpage to create a new ROS workspace:
http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment
[[d **NOTE:**
For convenience, the commands to source the required ROS environment setup scripts (“source /opt/ros/kinetic/setup.bash” and “source `<workspace_dir>/devel/setup.bash`”) can be placed at the end of your ~/.bashrc file so they are executed automatically every time you open a new Terminal/shell. Otherwise, you will need to source them each time you open a new Terminal/shell. The following commands can be used to add the “source” commands to the end of your ~/.bashrc file. In the second command below make sure to change “`<workspace_dir>`” to your actual workspace directory.
]]
[[y **Caution:**
Copying the commands from a web browser sometimes adds extra newline characters that will cause the commands to fail when pasted into Linux.Open the doc in a stand-alone PDF reader (i.e. Adobe Acrobat, etc.) if this occurs while you copy and paste commands directly into Linux.
]]
```
$ echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
```
```
$ echo "source <workspace_dir>/devel/setup.bash" >> ~/.bashrc
```
After you run these commands to add the scripts to your ~/.bashrc file, you must close and re-open your Terminal/shell for them to take effect.
Please note that the script in the devel directory is specific to the workspace and you would need to change your ~/.bashrc file and re-open your shell in order to work in a different workspace.
Alternatively you can copy and paste the commands to your ~/.bashrc file using an editor. for example the command shown below should open ~/.bashrc using gedit text editor
```
gedit ~/.bashrc
```
2. Install the ROS ‘serial’ package:
a) Use GIT to download (clone) the ROS serial driver into your <`workspace_dir>/src` directory:
```
$ cd <workspace_dir>/src
```
```
$ git clone https://github.com/wjwwood/serial.git
```
b) More documentation on this library can be found at: http://wjwwood.io/serial/
3. Download the TI mmWave ROS Driver (ti_mmwave_rospkg) and copy it to your `<workspace_dir>/src` directory:
a) Point your web browser to the following link:
http://dev.ti.com/tirex/#/?link=Software
Click on “mmWave Sensors” and then click on “Industrial Toolbox"
b) Click on the link for “Labs”
c) Click on the “ROS Point Cloud Visualizer” lab in the table in the center
d) Click on the “Download All” button on the right side as shown in the following image and accept the user license agreement when prompted. Choose to open the downloaded zip file if prompted or save it to disk first and then open it by double-clicking it from the downloaded location.
<img src="images/Ros_TIREX.png" class="img-responsive"/>
e) Navigate into the folder structure of the opened zip file to the path shown in the following figure and then double-click on the `ti_mmwave_rospkg_<ver>`.zip file to open it. (Note that the path may start with `mmwave_training_<ver>` as shown or `mmwave_sensors/industrial_toolbox_<ver>` depending on package version)
<img src="images/Ros_Folder_Content.png" class="img-responsive"/>
f) After opening the `ti_mmave_rospkg_<ver>.zip` file, you should see the ti_mmwave_rospkg folder as shown in the left window in following screenshot. Open a new file browser and navigate to your `<workspace_dir>`/src directory as shown in the right window in the screenshot. Copy the ti_mmwave_rospkg folder to your `<workspace_dir>/src` directory by dragging it from the source (left) window to the destination (right) window.
<img src="images/Copy_paste.png" class="img-responsive"/>
g) You should now see the ti_mmwave_rospkg folder in your `<workspace_dir>/src` directory as shown in the following screenshot.
<img src="images/ti_mmWave_rospkg_folder.png" class="img-responsive"/>
4. Build your project:
In the Terminal/shell, go to your `<workspace_dir>` directory and build the driver:
```
$ cd <workspace_dir>
```
```
$ catkin_make
```
If all of the installation steps were followed and both ROS environment scripts were sourced, the driver should build successfully and you should see something similar to the following at the end of the output. Your output may look slightly different but it should still show “[100%]”.
[[g! build Successful
[100%] Linking CXX executable /home/username/catkin_ws/devel/lib/ti_mmwave_rospkg/ti_mmwave_rospkg
[100%] Built target ti_mmwave_rospkg Scanning dependencies of target ti_mmwave_rospkg_generate_messages
[100%] Built target ti_mmwave_rospkg_generate_messages
]]
 
## Running the driver
1. Close all previously opened Terminal windows/shells (since they may have old/stale environment variables)
2. If you are using a brand new TI mmWave EVM, it may come pre-flashed with an older version of the Out-of-Box Demo. Since the ROS driver is only compatible with mmWave SDK version listed in the ROS Driver release notes, the EVM should be re-flashed with the Out-of-Box demo from that version of the mmWave SDK. You will also need to re-flash it if the EVM has been re-programmed with a different flash image or if you are running into issues. You can follow the steps in the section “How do I Re-flash the TI mmWave EVM with Out-of-Box demo?” to re-flash the EVM with the Out-of-Box Demo.
3. Make sure the SOP2 jumper on the TI mmWave EVM has been removed or switched to off postion and then power it using the 5VDC power supply (which must be rated for at least 2.5Amps) and connect the EVM via USB (port labeled "XDS110 USB" on the MMWAVEICBOOST) to your host machine.
[[d NOTE:
Please note that the IWR6843AOPEVM does not work with the ROS visualizer in standalone mode. It must be mounted on the MMWAVEICBOOST.
]]
4. If you are using an Ubuntu virtual machine (VM) running under Windows, whenever you connect or power-cycle the mmWave EVM you will need to enable the VM to access the mmWave EVM serial ports by going to the Devices menu along the top of the VM window, going to Devices->USB and selecting the Texas Instruments XDS110 option as shown in the picture in the “Tips for usage with an Ubuntu Virtual Machine” section. This is not necessary for native Linux machines.
5. Open a new Terminal window/shell. If you did not add the commands to source the two ROS environment scripts used during the “Setup a ROS Workspace” section to your ~/.bashrc file, then they should be manually sourced now using the following commands (make sure to change `<workspace_dir>` to your actual workspace directory):
```
$ source /opt/ros/kinetic/setup.bash
```
```
$ source <workspace_dir>/devel/setup.bash
```
6. Launch the TI mmWave ROS node and Rviz visualizer as follows. (Please note that the roslaunch command will automatically start a “roscore” ROS master process if there is not one started already.)
**For the TI mmWave 6843AOP EVM use one of the following commands:**
3-D configuration with elevation info:
```
$ roslaunch ti_mmwave_rospkg 6843AOP_multi_3d_0.launch
```
To add the second EVM in 3-D configuration, open a new terminal and enter the command:
```
$ roslaunch ti_mmwave_rospkg 6843AOP_multi_3d_1.launch
```
2-D configuration without elevation info:
```
$ roslaunch ti_mmwave_rospkg 6843AOP_multi_2d_0.launch
```
To add the second EVM in 2-D configuration, open a new terminal and enter the command:
```
$ roslaunch ti_mmwave_rospkg 6843AOP_multi_2d_1.launch
```
**For the TI mmWave 6843 EVM use one of the following commands:**
3-D configuration with elevation info:
```
$ roslaunch ti_mmwave_rospkg 6843_multi_3d_0.launch
```
To add the second EVM in 2-D configuration, open a new terminal and enter the command:
```
$ roslaunch ti_mmwave_rospkg 6843_multi_3d_1.launch
```
2-D configuration without elevation info:
```
$ roslaunch ti_mmwave_rospkg 6843_multi_2d_0.launch
```
To add the second EVM in 2-D configuration, open a new terminal and enter the command:
```
$ roslaunch ti_mmwave_rospkg 6843_multi_2d_1.launch
```
[[d NOTE:
The EVM must be power cycled or reset when switching from a 3-D configuration to a 2-D configuration (or from 2-D to 3-D). Otherwise, it will produce an error and stop working until it has been power cycled or reset.
]]
After up to 20 seconds the LED (labeled ‘GPIO_2’ on the 6843 EVM) should light up (or pulse off then on if it was already lit up) and you should start seeing points being displayed in the Rviz visualizer window as shown in the pictures in the “Visualizing the Data” section. To stop the roslaunch command press CTRL-C in the Terminal window.
If the mmWave EVM was not running the Out-of-box demo firmware or did not successfully connect to the PC via the default Linux serial ports (/dev/ttyACM0 and /dev/ttyACM1) the command above will produce errors in the Terminal window and you will not see any points in the Rviz visualizer window. You will need to press CTRL-C in the Terminal window to stop the roslaunch command.
In case the EVM was not in a good state, try resetting the EVM by pressing the ‘NRST’ button on the EVM and then run the desired roslaunch command again.
You can check that the mmWave EVM is connected and that both COM ports show up using the command
```
ll /dev/serial/by-id
```
from a Linux Terminal window as shown in the following image.
<img src="images/COM_Ports.png" class="img-responsive"/>
If you are already using the default (/dev/ttyACM0 and /dev/ttyACM1) ports for a different USB device or the TI mmWave EVM is connected to different ports you can open the file `<$WORKSPACE>/src/ti_mmwave_rospkg/launch/ti_mmwave_sensor.launch` and edit the following lines to use your own serial ports instead of the default ports:
[[b! Setting the serial port
`<param name=“command_port” value=“/dev/ttyACM0” />`
`<param name=“data_port” value=“/dev/ttyACM1” />`
]]
Please see the “Frequency Asked Questions” section for more troubleshooting hints if needed.
7. When running the demo in multi-EVM mode the offset and position of the EVM relative to the other needs to be set. The launch files utilize a static_transform_publisher to keep track of 3D coordinates over time.
static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
X, Y and Z refer to the offset position of the sensor in meters. Yaw, pitch and roll refer to the angle offset of the sensor in radians (yaw is rotation about Z, pitch is rotation about Y and roll is rotation about X). The period in milliseconds specifies how often to send a transform (100 ms = 10 Hz). Set these arguments relative to one another for each of the sensors. For example, use `multi_<sensor>_0` as the baseline (0 0 0 0 0 0) and update the arguments in `multi_<sensor>_1` to describe the position of the second sensor relative to the baseline sensor.
To display the axes, add the axes feature to the map as shown below. This enables the axes display located at the origin of the target frame, red - x green - y blue -z
<img src="images/Axis_screenshot.jpg" class="img-responsive"/>
## Visualizing the data
Once the sensor is running you should see the point cloud displayed in the Rviz visualizer window. The previous roslaunch command will automatically launch Rviz, but it can also be launched manually if needed using the following command in a new Terminal window/shell (after sourcing the ROS environment scripts).
```
$ rosrun rviz rviz
```
- Click Add->PointCloud2
- Select /mmWaveDataHdl/RScan from the “Topic” field dropdown for the PointCloud2 on the left hand panel
The following figure shows an example Rviz window for the 6843 or 6843AOP sensor in a 3D configuration. In the initial perspective shown, the view is setup with the TI mmWave EVM in the back-middle edge of the grid facing toward you. The color of each point is based on its Z-axis elevation relative to the grid. You can move the perspective around by moving the mouse while holding down the left mouse button while the mouse is over the visualizer display. You can zoom in/out using the mouse scroll wheel or by holding down the right mouse button. Holding down the center scroll wheel allows panning.
<img src="images/ROS_Visualizer.png" class="img-responsive"/>
The previous roslaunch command loads a default Rviz configuration but feel free to modify the visualization settings in the Rviz window as desired. The PointCloud2 point size and shape can be changed by setting the Size (m) parameter and Style parameter. The points can be colored based on elevation (used by default in the 3-D configurations) or by intensity (used by default in the 2-D configurations). Changing the Decay option to a non-zero value will cause Rviz to accumulate and display points over a specified number of seconds (it is set to 0.25 sec in the provided default configurations). Try changing this parameter and note the change in the displayed output.
The following figure shows an example Rviz window for the 6843 or 6843AOP sensor in a 2D configuration. The initial perspective is a top-down (bird’s eye) view with the sensor at the top-middle of the grid facing downward. The color of each point represents the intensity of the reflection where dark blue represents the lowest intensity points displayed and white represents the highest intensity points being displayed. This is configurable in the Rviz settings for the PointCloud2.
<img src="images/ROS_Visualizer_2d.png" class="img-responsive"/>
### Reconfiguring the chirp profile
At this point, the mmWave EVM will be running one of the selected FMCW Radar chirp configurations provided with the driver. If you would like to reconfigure the mmWave device’s chirp profile with a different set of parameters, you can create a new configuration using the mmWave Demo Visualizer (https://dev.ti.com/mmWaveDemoVisualizer) and save the corresponding cfg file using the “Save Config to PC” button. Make sure the device and mmWave SDK version selected in the GUI match your EVM and Out-of-Box firmware.
It is possible to reconfigure the mmWave sensor after it is already running as long as the previous config used the same number of TX and RX antennas. You can send the configuration to the mmWave device with the following command:
```
$ rosrun ti_mmwave_rospkg mmWaveQuickConfig /path/to/profile.cfg
```
It is also possible to replace the default config so that the new config is loaded when the lab is started (required if the number of TX/RX antennas differs) using the following steps.
- Place the new chirp config file in the “~/catkin_ws/src/ti_mmwave_rospkg/cfg” directory
- Rename the original default config file to a different name
- Rename the new config file to match the original default config file
## How it works
### Nodes
### ti_mmwave_rospkg
This node is a nodelet manager which is loaded with the two nodelets listed below when initiated. It must be instantiated with the parameters listed below.
**Parameters**
command_port : Linux port used for the mmWave User UART port
command_rate: Baud rate for the command_port (115200)
data_port: Linux port used for the mmWave Data UART port
data_rate: Baud rate for the data_port (921600)
**mmWaveCommSrv Nodelet**
This nodelet provides the mmWave UART Command Line Interface as a ROS Service.
- Establishes a ROS Service which receives a CLI command and parameters in the form of a std_msgs::string
- Uses ‘serial’ library (dependency) to open/read a COM port in Linux
- Sends the string out to the mmWave board over UART
- Returns the response from the EVM in the form of a std_msgs::string
- mmWaveQuickConfig node uses this service to load a full .cfg file to the EVM.
<img src="images/Nodes.png" class="img-responsive"/>
**mmWaveDataHdl Nodelet**
This nodelet reads and sorts the incoming radar data from the mmWave EVM.
- Uses ‘serial’ library to read incoming data from the mmWave device via a COM port on Linux
- Multi-Threaded Double Buffered read & sort of incoming packets
1. readIncomingData thread : reads incoming data, looks for start-of-packet signal
2. sortIncomingData thread : sorts the incoming data based off of the package structure sent from the mmWave device
3. syncedBufferSwap thread : when 1) and 2) have finished, swaps data buffers so the sorting thread can operate on new data
- Publishes output as a PointCloud<T> type ROS message
<img src="images/ROS_Message.png" class="img-responsive"/>
**mmWaveQuickConfig**
This node will read a user specified radar chirp profile line-by-line and send each line to the mmWaveCommSrv nodelet to configure the mmWave EVM.
### Services
mmWaveCLI
#### Requests
Std_msgs::String comm
#### Response
Std_msgs::String resp
### Messages
Sensor_msgs::PointCloud2 /mmwaveDataHdl/RScan
 
## Frequently Asked Questions (FAQ)
### Tips for usage with an Ubuntu Virtual Machine
Here are some tips when using ROS and the TI mmWave ROS driver with an Ubuntu Virtual Machine (VM).
- The Windows PC should have more than 12GB of free hard drive space to install the VM and all required software on the VM
- The driver was successfully tested using an Ubuntu 16.04 LTS Virtual Machine running under Oracle Virtualbox (www.virtualbox.org) on Windows 7 and Windows 10. The following instructions are specific to Oracle Virtualbox but should be useful for other virtual machine managers as well.
- Download and install Oracle Virtualbox using the link above (to download click the Download button on the webpage and then click the download link for “Windows hosts”)
- Open Virtualbox and click the button to create a new virtual machine (VM)
- Create the VM in Virtualbox using the following settings:
- Name: Ubuntu 16.04
- If not already selected, use Type: “Linux” and Version: “Ubuntu (64-bit)” and click Next
- Select at least 2048 MB for the RAM memory size and click Next
- Choose “Create a Virtual Hard Disk now” when prompted and click Create
- For the following prompts select “VDI”, “Dynamically allocated”, and increase the size to at least 12GB and then click Create
- Download (save to your hard drive) the Ubuntu 16.04 installer ISO image (1.5 GB) from the following link: http://releases.ubuntu.com/16.04.3/ubuntu-16.04.3-desktop-amd64.iso
- Start the newly created VM and point it to the downloaded ISO image when prompted for a startup disk
- Follow the prompts to install Ubuntu on the VM
- It is not required to check the box to “Install third-party software”
- Use the default selected option “Erase disk and install Ubuntu” when prompted
- Press “Continue” when prompted to write changes to disk
- Press “Restart Now” when prompted
- After logging into the Ubuntu VM using your newly created username and password you can open a Terminal window by clicking on the icon on the top of the left bar and searching for “Terminal” and then clicking on the Terminal icon (you can then right-click the Terminal icon on the left bar and choose to lock it to the Launcher bar if desired)
- Whenever you connect or power-cycle the mmWave EVM you will need to enable the VM to access the mmWave EVM serial ports by going to the Devices menu along the top of the VM window, going to Devices->USB and selecting the Texas Instruments XDS110 option as shown in the following picture:
<img src="images/USB_Settings.png" class="img-responsive"/>
- Enable the VM to access the internet by going to the “Devices->Network->Network Settings” menu at the top of the VM window and then select “Bridged Adapter” and pick your active network adapter so the VM can access the network
- If you are behind a corporate firewall, you may also need to setup your Linux proxy configurations to allow proper operation of GIT, web browsing and software installation using apt-get. Please contact your IT department for assistance if needed.
- Do not enable the “Display->Enable 3D Acceleration” option in the VM settings (it is normally disabled by default) or it may cause Rviz (ROS visualizer) to crash
- (Optional) You may want to enable the Bi-directional Shared Clipboard so you can copy/paste text between Windows and the Linux VM. This can be done via the “Devices->Shared Clipboard” menu.
- (Optional) You may want to enable a Shared Folder so you can copy files between Windows and the Linux VM. This can be done via the “Devices->Shared Folders” menu. You will need to restart the Linux VM after adding a new shared folder to make it show up in the Linux file browser.
### I see “not recognized as a CLI command” errors when I start the driver
- This type of error is typically seen when the Out-of-Box demo firmware version running on the mmWave EVM is not compatible with the .cfg files in the ROS driver version in-use. An example error message would be:
[[r! Error Message
“mmWaveCommSrv: Received response from sensor: 'clutterRemoval 0
'clutterRemoval' is not recognized as a CLI command”
]]
- Please re-flash the mmWave EVM with the Out-of-Box demo firmware from the mmWave SDK using the steps in the FAQ section titled “How do I Re-flash the TI mmWave EVM with Out-of-Box demo?”
### I see serial port errors when I start the driver or I do not see the mmWave EVM serial ports at /dev/ttyACM0 and /dev/ttyACM1
- Make sure the mmWave EVM is powered on and connected to the PC. It may take up to 20 seconds for Linux to completely initialize the serial ports for usage after the EVM is plugged in.
- If you are using a Virtual Machine, please make sure you have followed the steps listed in the section on tips for Virtual Machine usage regarding the USB serial port access.
- If the mmWave EVM is connected via different serial ports, follow the steps in the “Running the driver” section to modify the launch file that specifies the ports for the driver to use
### How do I ensure the EVM is properly configured?
For xWR6843AOP EVM and xWR6843 EVM, see users guide
http://www.ti.com/lit/ug/swru546c/swru546c.pdf
### How do I Re-flash the TI mmWave EVM with Out-of-Box demo?
If you are using a brand new TI mmWave EVM, it may come pre-flashed with an older version of the Out-of-Box Demo. Since the ROS driver is only compatible with the mmWave SDK version listed in the ROS Driver release notes, the EVM should be re-flashed with the Out-of-Box demo from the compatible version of the mmWave SDK. You will also need to re-flash it if the EVM has been re-programmed with a different flash image or if you are running into issues.
The following steps can be used to re-flash the EVM.
1. Download Uniflash, TI’s flashing software, via the following link:
http://processors.wiki.ti.com/index.php/Category:CCS_UniFlash
2. Make sure the file “/lib/x86_64-linux-gnu/libudev.so.0” exists. If it does not exist execute the following command to generate a symbolic link necessary for Uniflash to work:
```
$ sudo ln -sf /lib/x86_64-linux-gnu/libudev.so.1 /lib/x86_64-linux-gnu/libudev.so.0
```
3. Navigate to the download directory, make the file executable and run the Uniflash installer:
```
$ cd ~/Downloads
```
```
$ chmod +x uniflash_sl.4.2.1.1562.run
```
```
$ ./uniflash_sl.4.2.1.1562.run
```
[[y! Note:
Your Uniflash executable filename may differ once a newer version becomes available
]]
4. Download the Linux version of the mmWave SDK (required version listed in the ROS Driver release notes) via the following link:
http://www.ti.com/tool/mmwave-sdk
5. Install the following mmWave SDK dependency:
```
$ sudo apt-get install libc6:i386
```
6. Navigate to the download directory, make the file executable and run the mmWave SDK installer (change “<ver>” to the proper version):
```
$ cd ~/Downloads
```
```
$ chmod +x mmwave_sdk_<ver>-Linux-x86-Install.bin
```
```
$ ./mmwave_sdk_<ver>-Linux-x86-Install.bin
```
7. Make sure there is no power to the mmWave EVM and place a jumper on the SOP2 header
8. Run Uniflash and you should see the following screen:
<img src="images/Uniflash_Interface.png" class="img-responsive"/>
Click mmWave and in the list select your device.
9. Specify the following flash image:
For xWR6843AOP EVM:
Meta Image 1:
`<MMWAVE_SDK_INSTALL_DIR>/packages/ti/demo/xwr64xx/mmw/xwr64xxAOP_mmw_demo.bin`
For xWR6843 EVM:
Meta Image 1:
`<MMWAVE_SDK_INSTALL_DIR>/packages/ti/demo/xwr68xx/mmw/xwr68xx_mmw_demo.bin`
10. Supply power to your mmWave EVM and connect the device to the computer with the microUSB cable (use the USB port labeled ‘XDS110 USB’ on the 6843 EVM).
11. Check that the mmWave EVM is connected and that both COM ports show up using the command “ll /dev/serial/by-id” from a Linux Terminal window as shown in the following image.
<img src="images/COM_Ports_2.png" class="img-responsive"/>
12. On the left-hand pane in Uniflash, select Settings and Utilities. Enter the serial port corresponding to the first COM port (typically /dev/ttyACM0)
13. Select Program in the left-hand pane and then click the “Load Images” button. When finished, the console should report “Program Load completed successfully” as in the following image.
<img src="images/Flashing.png" class="img-responsive"/>
[[y!Note:
If Uniflash stops responding after clicking “Load Images”, re-start it and make sure EVM is power cycled after Uniflash is started.
]]
14. After Uniflash has finished flashing the device remove the power supply, remove the SOP2 jumper and reapply power to the EVM.
### How do I edit the cfg file?
- The configuration files are included in the ti_mmwave_rospkg.zip folder, ..\ti_mmwave_rospkg\cfg.
- To change the parameter, edit and save the file using a text editor.
- For example to update the demo to run using the IWR6843ISK-ODS and configuration file used with the ODS point cloud demo
- Edit the "6843_3d.cfg" configuration file to one that works with the ODS point cloud demo.
- Flash the IWR6843ISK-ODS with the ODS point cloud binary.
- Run the demo as documented for 6843 EVM.
</xmp>
<link rel="stylesheet" href="../../../../.metadata/.html/scripts/highlight/styles/zenburn.css"/>
<script src="../../../../.metadata/.html/scripts/strapdown/vendor/jquery-1.11.2.min.js"></script>
<script src="../../../../.metadata/.html/scripts/strapdown/vendor/bootstrap.min.js"></script>
<script src="../../../../.metadata/.html/scripts/highlight/highlight.pack.js"></script>
<script src="../../../../.metadata/.html/scripts/strapdown/v/0.2/strapdown.js"></script>
</html>

BIN
ros_driver/docs/TI_mmWave_ROS_Driver_Users_Guide.pdf Normal file
View File

Binary file not shown.

BIN
ros_driver/docs/images/Axis_screenshot.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 619 KiB

BIN
ros_driver/docs/images/COM_Ports.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 453 KiB

BIN
ros_driver/docs/images/COM_Ports_2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 453 KiB

BIN
ros_driver/docs/images/Copy_paste.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 45 KiB

BIN
ros_driver/docs/images/Flashing.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 314 KiB

BIN
ros_driver/docs/images/Nodes.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 86 KiB

BIN
ros_driver/docs/images/ROS_Message.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 33 KiB

BIN
ros_driver/docs/images/ROS_Visualizer.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 106 KiB

BIN
ros_driver/docs/images/ROS_Visualizer_2d.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 107 KiB

BIN
ros_driver/docs/images/Ros_Folder_Content.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 41 KiB

BIN
ros_driver/docs/images/Ros_TIREX.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 82 KiB

BIN
ros_driver/docs/images/USB_Settings.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 110 KiB

BIN
ros_driver/docs/images/Uniflash_Interface.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 76 KiB

BIN
ros_driver/docs/images/Uniflash_Interface_2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 40 KiB

BIN
ros_driver/docs/images/iwr6843_silicon_revision.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 88 KiB

BIN
ros_driver/docs/images/iwr6843aopevm1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 179 KiB

BIN
ros_driver/docs/images/ti_mmWave_rospkg_folder.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 25 KiB

232
ros_driver/ti_mmwave_rospkg/CMakeLists.txt Normal file
View File

@@ -0,0 +1,232 @@
cmake_minimum_required(VERSION 2.8.3)
project(ti_mmwave_rospkg)
## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
nodelet
roscpp
serial
std_msgs
sensor_msgs
message_generation
pluginlib
)
find_package(Boost REQUIRED)
find_package(Threads REQUIRED)
find_package(PCL 1.7.2 REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
RadarScan.msg
)
## Generate services in the 'srv' folder
add_service_files(
FILES
mmWaveCLI.srv
)
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES mmwave
#CATKIN_DEPENDS nodelet roscpp serial std_msgs
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories( include
${catkin_INCLUDE_DIRS}
${ti_mmwave_rospkg_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${pthread_INCLUDE_DIRS}
${rt_INCLUDE_DIRS}
)
link_directories( ${Boost_LIBRARIES_DIRS} )
link_directories( ${PCL_LIBRARY_DIRS} )
add_definitions(${PCL_DEFINITIONS})
## Declare a C++ library
add_library(mmwave
src/mmWaveDataHdl.cpp
src/mmWaveCommSrv.cpp
src/DataHandlerClass.cpp
src/mmWaveQuickConfig.cpp
src/ParameterParser.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(mmwave ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/ti_mmwave_rospkg_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
set (CMAKE_CXX_STANDARD 11)
add_executable(${PROJECT_NAME} src/mmWaveLoader.cpp)
target_link_libraries(mmwave ${serial_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} mmwave ${serial_LIBRARIES})
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} mmwave ${serial_EXPORTED_TARGETS})
add_executable(mmWaveQuickConfig src/mmWaveQuickConfig.cpp)
target_link_libraries(mmWaveQuickConfig ${catkin_LIBRARIES} mmwave)
add_dependencies(mmWaveQuickConfig ${catkin_EXPORTED_TARGETS} mmwave)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ti_mmwave_rospkg.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

144
ros_driver/ti_mmwave_rospkg/README.md Normal file
View File

@@ -0,0 +1,144 @@
# TI mmWave ROS Package (Customized)
#### Based on updates from Dr. Leo Zhang (University of Arizona)
---
### Most recent change from Dr. Zhang:
Add support for XWR18XX devices. SDK version: 3.2.0.4.
### Most recent change from Allison Wendell:
Add support for XWR68XX devices. SDK version: 3.2.0.4
---
Initially derived from TI's origin ROS package in Industrial Toolbox 2.3.0 (new version available [Industrial Toolbox 2.5.2](http://dev.ti.com/tirex/#/?link=Software%2FmmWave%20Sensors%2FIndustrial%20Toolbox)).
### Differences from origin TI's version:
1. Added all radar parameters from calculations and can be read from `rosparam get`.
2. Added Doppler data from detecting targets and form a customized ROS message `/ti_mmwave/radar_scan`.
3. Added support for multiple radars working together.
4. Added support for camera overlay (for sensor fusion).
5. Working with xWR1443 and xWR1642 ES1.0 and ES2.0 (ES1.0 is deprecated from TI)
---
### Available devices:
```
TI mmWave xWR1443BOOST
TI mmWave xWR1642BOOST
TI mmWave xWR1642BOOST ES2.0/3.0 EVM (not tested)
TI mmWave xWR1642BOOST ES2.0 EVM
TI mmWave AWR1843BOOST ES1.0 EVM
TI mmWave IWR6843ISK ES1.0 EVM
```
---
### Quick start guide (AWR1642BOOST ES2.0 EVM):
1. Mount AWR1642BOOST ES2.0 EVM (as below), connect 5V/2.5A power supply and connect a micro-USB cable to host Ubuntu 16.04 LTS with [ROS Kinetic](http://wiki.ros.org/kinetic).
Note: Tested with Ubuntu 16.04 LTS with ROS Kinectic and Ubuntu 18.04 LTS with [ROS Melodic](http://wiki.ros.org/melodic)
2. Download SDK 2.0 or above (suggested SDK 2.1) from [here](http://www.ti.com/tool/MMWAVE-SDK) and use [UNIFLASH](http://www.ti.com/tool/UNIFLASH) to flash xwr16xx_mmw_demo.bin to your device. **Do not forget SOP2 jumper when flashing.**
3. Clone this repo and ROS serial onto your `<workspace dir>/src`:
```
git clone https://github.com/wjwwood/serial.git
git clone https://bitbucket.itg.ti.com/scm/mmwave_apps/ros_multisensor_demo.git
```
4. Go back to `<workspace dir>`:
```
catkin_make && source devel/setup.bash
echo "source <workspace_dir>/devel/setup.bash" >> ~/.bashrc
```
5. Enable command and data ports on Linux:
```
sudo chmod 666 /dev/ttyACM0
sudo chmod 666 /dev/ttyACM1
```
Note: If multiple sensors are used, enable additional ports `/dev/ttyACM2` and `/dev/ttyACM3`, etc. the same as this step.
6. Launch AWR1642 short range config:
```
roslaunch ti_mmwave_rospkg 1642es2_short_range.launch
```
Note: If you want to build your own config, use [mmWave Demo Visualizer](https://dev.ti.com/mmwavedemovisualizer) and link the launch file to the config.
7. ROS topics can be accessed as follows:
```
rostopic list
rostopic echo /ti_mmwave/radar_scan
```
8. ROS parameters can be accessed as follows:
```
rosparam list
rosparam get /ti_mmwave/max_doppler_vel
```
---
### Message format:
```
header:
seq: 6264
stamp:
secs: 1538888235
nsecs: 712113897
frame_id: "ti_mmwave" # Frame ID used for multi-sensor scenarios
point_id: 17 # Point ID of the detecting frame (Every frame starts with 0)
x: 8.650390625 # Point x coordinates in m (front from antenna)
y: 6.92578125 # Point y coordinates in m (left/right from antenna, right positive)
z: 0.0 # Point z coordinates in m (up/down from antenna, up positive)
range: 11.067276001 # Radar measured range in m
velocity: 0.0 # Radar measured range rate in m/s
doppler_bin: 8 # Doppler bin location of the point (total bins = num of chirps)
bearing: 38.6818885803 # Radar measured angle in degrees (right positive)
intensity: 13.6172780991 # Radar measured intensity in dB
```
---
### Troubleshooting
1.
```
mmWaveCommSrv: Failed to open User serial port with error: IO Exception (13): Permission denied
mmWaveCommSrv: Waiting 20 seconds before trying again...
```
This happens when serial port is called without superuser permission, do the following steps:
```
sudo chmod 666 /dev/ttyACM0
sudo chmod 666 /dev/ttyACM1
```
2.
```
mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')
mmWaveQuickConfig: Response: 'sensorStop
'?`????`????`???~' is not recognized as a CLI command
mmwDemo:/>'
```
When this happens, re-run the command you send to sensor. If it continues, shut down and restart the sensor.
---
### Multiple devices support (dual AWR1642 ES2.0 EVM):
1. Connect two devices and try `ll /dev/serial/by-id` or `ls /dev`. In this case, `/dev/ttyACM0` to `/dev/ttyACM3` should shown.
2. To avoid serial port confliction, you need to launch devices separately. So for first device (it will open rviz):
```
roslaunch ti_mmwave_rospkg multi_1642_0.launch
```
3. Change radars' location in first six arguments `<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"/>` (stands for x,y,z for positions in meters and yaw, pitch, roll for angles in radians) in launch file `multi_1642_1.launch`. And launch second device:
```
roslaunch ti_mmwave_rospkg multi_1642_1.launch
```
Note: As serial connection and the original code, you need to launch devices separately using different launch files.
---
### Camera overlay support (working with USB camera or CV camera):
1. Download and build USB camera repo [here](https://github.com/radar-lab/usb_webcam`). And set parameters of camera in `<usb_webcam dir>/launch/usb_webcam.launch`.
2. To test the device image working, try:
```
roslaunch usb_webcam usb_webcam.launch
rosrun rqt_image_view rqt_image_view
```
3. Make sure you have done [ROS camera calibration](http://wiki.ros.org/camera_calibration) and create a `*.yaml` configuration file accordingly.
4. Launch radar-camera system using:
```
roslaunch ti_mmwave_rospkg camera_overlay.launch
```

39
ros_driver/ti_mmwave_rospkg/cfg/1443_2d.cfg Normal file
View File

@@ -0,0 +1,39 @@
% ***************************************************************
% Created for SDK ver:01.02
% Created using Visualizer ver:1.2.0.0
% Frequency:77
% Platform:xWR14xx
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):15
% Range Resolution(m):0.044
% Maximum unambiguous Range(m):9.01
% Maximum Radial Velocity(m/s):5.06
% Radial velocity resolution(m/s):0.64
% Frame Duration(msec):33.333
% Range Detection Threshold (dB):9
% Range Peak Grouping:disabled
% Doppler Peak Grouping:enabled
% Static clutter removal:disabled
% ***************************************************************
sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 5 0
adcCfg 2 1
adcbufCfg 0 1 0 1
profileCfg 0 77 39 7 57.14 0 0 70 1 240 4884 0 0 30
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 4
frameCfg 0 1 16 0 33.333 1 0
guiMonitor 1 0 0 0 0 0
cfarCfg 0 2 8 4 3 0 768
peakGrouping 1 0 1 1 229
multiObjBeamForming 1 0.5
clutterRemoval 0
calibDcRangeSig 0 -5 8 256
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 123 0
CQSigImgMonitor 0 119 4
analogMonitor 1 1
sensorStart

40
ros_driver/ti_mmwave_rospkg/cfg/1443_3d.cfg Normal file
View File

@@ -0,0 +1,40 @@
% ***************************************************************
% Created for SDK ver:01.02
% Created using Visualizer ver:1.2.0.0
% Frequency:77
% Platform:xWR14xx
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):15 + Elevation
% Range Resolution(m):0.044
% Maximum unambiguous Range(m):9.01
% Maximum Radial Velocity(m/s):5.06
% Radial velocity resolution(m/s):0.64
% Frame Duration(msec):33.333
% Range Detection Threshold (dB):9
% Range Peak Grouping:disabled
% Doppler Peak Grouping:enabled
% Static clutter removal:disabled
% ***************************************************************
sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 7 0
adcCfg 2 1
adcbufCfg 0 1 0 1
profileCfg 0 77 7 7 57.14 0 0 70 1 240 4884 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 0 33.333 1 0
guiMonitor 1 0 0 0 0 0
cfarCfg 0 2 8 4 3 0 768
peakGrouping 1 0 1 1 229
multiObjBeamForming 1 0.5
clutterRemoval 0
calibDcRangeSig 0 -5 8 256
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 123 0
CQSigImgMonitor 0 119 4
analogMonitor 1 1
sensorStart

50
ros_driver/ti_mmwave_rospkg/cfg/1642_2d.cfg Normal file
View File

@@ -0,0 +1,50 @@
% ***************************************************************
% Created for SDK ver:03.03
% Created using Visualizer ver:3.3.0.0
% Frequency:77
% Platform:xWR16xx
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):15
% 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 3 0
adcCfg 2 1
adcbufCfg -1 0 1 1 0
profileCfg 0 77 429 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 2
frameCfg 0 1 16 0 100 1 0
lowPower 0 1
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
bpmCfg -1 0 0 1
lvdsStreamCfg -1 0 0 0
compRangeBiasAndRxChanPhase 0.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
sensorStart

49
ros_driver/ti_mmwave_rospkg/cfg/1843_2d.cfg Normal file
View File

@@ -0,0 +1,49 @@
% ***************************************************************
% 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
% Range Resolution(m):0.044
% Maximum unambiguous Range(m):9.02
% Maximum Radial Velocity(m/s):6.45
% Radial velocity resolution(m/s):0.81
% 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 5 0
adcCfg 2 1
adcbufCfg -1 0 1 1 1
profileCfg 0 77 18 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
frameCfg 0 1 16 0 100 1 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 -6.48 6.48
sensorStart

50
ros_driver/ti_mmwave_rospkg/cfg/1843_3d.cfg Normal file
View File

@@ -0,0 +1,50 @@
% ***************************************************************
% 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 0 100 1 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
sensorStart

50
ros_driver/ti_mmwave_rospkg/cfg/6843AOP_2d.cfg Normal file
View File

@@ -0,0 +1,50 @@
% ***************************************************************
% Created for SDK ver:03.04
% Created using Visualizer ver:3.4.0.0
% Frequency:60
% Platform:xWR68xx_AOP
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):60 + 30
% 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 60 567 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 2
frameCfg 0 1 16 0 100 1 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
sensorStart

50
ros_driver/ti_mmwave_rospkg/cfg/6843AOP_3d.cfg Normal file
View File

@@ -0,0 +1,50 @@
% ***************************************************************
% Created for SDK ver:03.02
% Created using Visualizer ver:3.2.0.0_AOP
% Frequency:60
% Platform:xWR68xx_AOP
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):30 + 30
% Range Resolution(m):0.047
% Maximum unambiguous Range(m):9.02
% Maximum Radial Velocity(m/s):4.99
% Radial velocity resolution(m/s):0.63
% Frame Duration(msec):33.333
% 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 60 43 7 40 0 0 100 1 224 7000 0 0 30
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 2
chirpCfg 2 2 0 0 0 0 0 4
frameCfg 0 2 16 0 33.333 1 0
lowPower 0 0
guiMonitor -1 1 0 0 0 0 0
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 1
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 4 99 0
CQSigImgMonitor 0 111 4
analogMonitor 0 0
aoaFovCfg -1 -90 90 -90 90
cfarFovCfg -1 0 0 8.40
cfarFovCfg -1 1 -5.02 5.02
sensorStart

50
ros_driver/ti_mmwave_rospkg/cfg/6843_2d.cfg Normal file
View File

@@ -0,0 +1,50 @@
% ***************************************************************
% Created for SDK ver:03.03
% Created using Visualizer ver:3.3.0.0
% Frequency:60
% Platform:xWR68xx
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):15
% 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 5 0
adcCfg 2 1
adcbufCfg -1 0 1 1 1
profileCfg 0 60 567 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
frameCfg 0 1 16 0 100 1 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
bpmCfg -1 0 0 1
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
sensorStart

51
ros_driver/ti_mmwave_rospkg/cfg/6843_3d.cfg Normal file
View File

@@ -0,0 +1,51 @@
% ***************************************************************
% Created for SDK ver:03.02
% Created using Visualizer ver:3.2.0.1
% Frequency:60
% Platform:xWR68xx
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):15 + Elevation
% Range Resolution(m):0.047
% Maximum unambiguous Range(m):9.04
% Maximum Radial Velocity(m/s):5.09
% Radial velocity resolution(m/s):0.64
% Frame Duration(msec):33.333
% 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 60 41 7 40 0 0 100 1 240 7500 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 0 33.333 1 0
lowPower 0 0
guiMonitor -1 1 0 0 0 0 0
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 1
calibDcRangeSig -1 0 -5 8 256
extendedMaxVelocity -1 0
bpmCfg -1 0 0 1
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 4 99 0
CQSigImgMonitor 0 119 4
analogMonitor 0 0
aoaFovCfg -1 -90 90 -90 90
cfarFovCfg -1 0 0 9.00
cfarFovCfg -1 1 -5.14 5.14
sensorStart

142
ros_driver/ti_mmwave_rospkg/include/DataHandlerClass.h Normal file
View File

@@ -0,0 +1,142 @@
#ifndef _DATA_HANDLER_CLASS_
#define _DATA_HANDLER_CLASS_
#include <ti_mmwave_rospkg/RadarScan.h>
#include "mmWave.h"
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <boost/shared_ptr.hpp>
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include <pthread.h>
#include <algorithm>
#include "pcl_ros/point_cloud.h"
#include "sensor_msgs/PointField.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/point_cloud2_iterator.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#define COUNT_SYNC_MAX 2
class DataUARTHandler{
public:
/*Constructor*/
//void DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) {}
DataUARTHandler(ros::NodeHandle* nh);
void setFrameID(char* myFrameID);
/*User callable function to set the UARTPort*/
void setUARTPort(char* mySerialPort);
/*User callable function to set the BaudRate*/
void setBaudRate(int myBaudRate);
/*User callable function to set maxAllowedElevationAngleDeg*/
void setMaxAllowedElevationAngleDeg(int myMaxAllowedElevationAngleDeg);
/*User callable function to set maxAllowedElevationAngleDeg*/
void setMaxAllowedAzimuthAngleDeg(int myMaxAllowedAzimuthAngleDeg);
void setNodeHandle(ros::NodeHandle* nh);
/*User callable function to start the handler's internal threads*/
void start(void);
/*Helper functions to allow pthread compatability*/
static void* readIncomingData_helper(void *context);
static void* sortIncomingData_helper(void *context);
static void* syncedBufferSwap_helper(void *context);
/*Sorted mmwDemo Data structure*/
mmwDataPacket mmwData;
private:
int nr;
int nd;
int ntx;
float fs;
float fc;
float BW;
float PRI;
float tfr;
float max_range;
float vrange;
float max_vel;
float vvel;
char* frameID;
/*Contains the name of the serial port*/
char* dataSerialPort;
/*Contains the baud Rate*/
int dataBaudRate;
/*Contains the max_allowed_elevation_angle_deg (points with elevation angles
outside +/- max_allowed_elevation_angle_deg will be removed)*/
int maxAllowedElevationAngleDeg;
/*Contains the max_allowed_azimuth_angle_deg (points with azimuth angles
outside +/- max_allowed_azimuth_angle_deg will be removed)*/
int maxAllowedAzimuthAngleDeg;
/*Mutex protected variable which synchronizes threads*/
int countSync;
/*Read/Write Buffers*/
std::vector<uint8_t> pingPongBuffers[2];
/*Pointer to current data (sort)*/
std::vector<uint8_t>* currentBufp;
/*Pointer to new data (read)*/
std::vector<uint8_t>* nextBufp;
/*Mutex protecting the countSync variable */
pthread_mutex_t countSync_mutex;
/*Mutex protecting the nextBufp pointer*/
pthread_mutex_t nextBufp_mutex;
/*Mutex protecting the currentBufp pointer*/
pthread_mutex_t currentBufp_mutex;
/*Condition variable which blocks the Swap Thread until signaled*/
pthread_cond_t countSync_max_cv;
/*Condition variable which blocks the Read Thread until signaled*/
pthread_cond_t read_go_cv;
/*Condition variable which blocks the Sort Thread until signaled*/
pthread_cond_t sort_go_cv;
/*Swap Buffer Pointers Thread*/
void *syncedBufferSwap(void);
/*Checks if the magic word was found*/
int isMagicWord(uint8_t last8Bytes[8]);
/*Read incoming UART Data Thread*/
void *readIncomingData(void);
/*Sort incoming UART Data Thread*/
void *sortIncomingData(void);
void visualize(const ti_mmwave_rospkg::RadarScan &msg);
ros::NodeHandle* nodeHandle;
ros::Publisher DataUARTHandler_pub;
ros::Publisher radar_scan_pub;
ros::Publisher marker_pub;
};
#endif

35
ros_driver/ti_mmwave_rospkg/include/ParameterParser.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef _PARAM_PARSER_CLASS_
#define _PARAM_PARSER_CLASS_
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "ti_mmwave_rospkg/mmWaveCLI.h"
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <cstdio>
#include <sstream>
#include <string>
#include <vector>
namespace ti_mmwave_rospkg {
class ParameterParser : public nodelet::Nodelet{
public:
ParameterParser();
void ParamsParser(ti_mmwave_rospkg::mmWaveCLI &srv, ros::NodeHandle &n);
void CalParams(ros::NodeHandle &nh);
private:
virtual void onInit();
ti_mmwave_rospkg::mmWaveCLI srv;
};
}
#endif

190
ros_driver/ti_mmwave_rospkg/include/mmWave.h Normal file
View File

@@ -0,0 +1,190 @@
/*
* mmWave.h
*
* This file contains various defines used within this package.
*
*
* Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef _TI_IWR14XX_
#define _TI_IWR14XX_
#include <iostream>
#include <iostream>
#include <cstdio>
#include "serial/serial.h"
#include "ros/ros.h"
#include <boost/thread.hpp>
#include <cstdint>
enum MmwDemo_Output_TLV_Types
{
MMWDEMO_OUTPUT_MSG_NULL = 0,
/*! @brief List of detected points */
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS,
/*! @brief Range profile */
MMWDEMO_OUTPUT_MSG_RANGE_PROFILE,
/*! @brief Noise floor profile */
MMWDEMO_OUTPUT_MSG_NOISE_PROFILE,
/*! @brief Samples to calculate static azimuth heatmap */
MMWDEMO_OUTPUT_MSG_AZIMUTH_STATIC_HEAT_MAP,
/*! @brief Range/Doppler detection matrix */
MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP,
/*! @brief Stats information */
MMWDEMO_OUTPUT_MSG_STATS,
/*! @brief List of detected points side information */
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS_SIDE_INFO,
MMWDEMO_OUTPUT_MSG_MAX
};
enum SorterState{ READ_HEADER,
CHECK_TLV_TYPE,
READ_OBJ_STRUCT,
READ_LOG_MAG_RANGE,
READ_NOISE,
READ_AZIMUTH,
READ_DOPPLER,
READ_STATS,
SWAP_BUFFERS,
READ_SIDE_INFO};
struct MmwDemo_output_message_header_t
{
/*! brief Version: : MajorNum * 2^24 + MinorNum * 2^16 + BugfixNum * 2^8 + BuildNum */
uint32_t version;
/*! @brief Total packet length including header in Bytes */
uint32_t totalPacketLen;
/*! @brief platform type */
uint32_t platform;
/*! @brief Frame number */
uint32_t frameNumber;
/*! @brief Time in CPU cycles when the message was created. For XWR16xx: DSP CPU cycles, for XWR14xx: R4F CPU cycles */
uint32_t timeCpuCycles;
/*! @brief Number of detected objects */
uint32_t numDetectedObj;
/*! @brief Number of TLVs */
uint32_t numTLVs;
/*! @brief Sub-frame Number (not used with XWR14xx) */
uint32_t subFrameNumber;
};
// Detected object structure for mmWave SDK 1.x and 2.x
struct MmwDemo_DetectedObj
{
uint16_t rangeIdx; /*!< @brief Range index */
uint16_t dopplerIdx; /*!< @brief Dopler index */
uint16_t peakVal; /*!< @brief Peak value */
int16_t x; /*!< @brief x - coordinate in meters. Q format depends on the range resolution */
int16_t y; /*!< @brief y - coordinate in meters. Q format depends on the range resolution */
int16_t z; /*!< @brief z - coordinate in meters. Q format depends on the range resolution */
};
// Detected object structures for mmWave SDK 3.x (DPIF_PointCloudCartesian_t and DPIF_PointCloudSideInfo_t)
/**
* @brief
* Point cloud definition in Cartesian coordinate system - floating point format
*
*/
typedef struct DPIF_PointCloudCartesian_t
{
/*! @brief x - coordinate in meters */
float x;
/*! @brief y - coordinate in meters */
float y;
/*! @brief z - coordinate in meters */
float z;
/*! @brief Doppler velocity estimate in m/s. Positive velocity means target
* is moving away from the sensor and negative velocity means target
* is moving towards the sensor. */
float velocity;
}DPIF_PointCloudCartesian;
/**
* @brief
* Point cloud side information such as SNR and noise level
*
*/
typedef struct DPIF_PointCloudSideInfo_t
{
/*! @brief snr - CFAR cell to side noise ratio in dB expressed in 0.1 steps of dB */
int16_t snr;
/*! @brief y - CFAR noise level of the side of the detected cell in dB expressed in 0.1 steps of dB */
int16_t noise;
}DPIF_PointCloudSideInfo;
struct mmwDataPacket{
MmwDemo_output_message_header_t header;
uint16_t numObjOut;
uint16_t xyzQFormat; // only used for SDK 1.x and 2.x
MmwDemo_DetectedObj objOut; // only used for SDK 1.x and 2.x
DPIF_PointCloudCartesian_t newObjOut; // used for SDK 3.x
DPIF_PointCloudSideInfo_t sideInfo; // used for SDK 3.x
};
const uint8_t magicWord[8] = {2, 1, 4, 3, 6, 5, 8, 7};
#endif

83
ros_driver/ti_mmwave_rospkg/include/mmWaveCommSrv.hpp Normal file
View File

@@ -0,0 +1,83 @@
/*
* mmWaveCommSrv.hpp
*
* This file defines a ROS nodelet which will open up a serial port provided by the user
* at a certain baud rate (also provided by user) that will interface with the 1443EVM mmwDemo
* User UART to be used for board configuration.
*
*
* Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef MMWAVE_COMM_SRV_H
#define MMWAVE_COMM_SRV_H
/*Include ROS specific headers*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
/*Include standard C/C++ headers*/
#include <iostream>
#include <cstdio>
#include <sstream>
/*mmWave Driver Headers*/
#include <ti_mmwave_rospkg/mmWaveCLI.h>
namespace ti_mmwave_rospkg
{
class mmWaveCommSrv : public nodelet::Nodelet
{
public:
mmWaveCommSrv();
private:
virtual void onInit();
bool commSrv_cb(ti_mmwave_rospkg::mmWaveCLI::Request &req, ti_mmwave_rospkg::mmWaveCLI::Response &res);
ros::ServiceServer commSrv;
std::string mySerialPort;
int myBaudRate;
}; //Class mmWaveCommSrv
} //namespace ti_mmwave_rospkg
#endif

80
ros_driver/ti_mmwave_rospkg/include/mmWaveDataHdl.hpp Normal file
View File

@@ -0,0 +1,80 @@
/*
* mmWaveDataHdl.hpp
*
* This file defines a ROS nodelet which will open up a serial port provided by the user
* at a certain baud rate (also provided by user) that will interface with the 1443EVM mmwDemo
* Data UART to be used for board configuration.
*
*
* Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef MMWAVE_DATA_HDL_H
#define MMWAVE_DATA_HDL_H
/*Include ROS specific headers*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
/*Include standard C/C++ headers*/
#include <iostream>
#include <cstdio>
#include <sstream>
/*mmWave Driver Headers*/
#include "DataHandlerClass.h"
namespace ti_mmwave_rospkg
{
class mmWaveDataHdl : public nodelet::Nodelet
{
public:
mmWaveDataHdl();
private:
virtual void onInit();
//char* mySerialPort;
//int myBaudRate;
}; //Class mmWaveDataHdl
} //namespace ti_mmwave_rospkg
#endif

27
ros_driver/ti_mmwave_rospkg/launch/1443_multi_2d_0.launch Normal file
View File

@@ -0,0 +1,27 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

26
ros_driver/ti_mmwave_rospkg/launch/1443_multi_2d_1.launch Normal file
View File

@@ -0,0 +1,26 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

27
ros_driver/ti_mmwave_rospkg/launch/1443_multi_3d_0.launch Normal file
View File

@@ -0,0 +1,27 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

26
ros_driver/ti_mmwave_rospkg/launch/1443_multi_3d_1.launch Normal file
View File

@@ -0,0 +1,26 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

31
ros_driver/ti_mmwave_rospkg/launch/1642_multi_2d_0.launch Normal file
View File

@@ -0,0 +1,31 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 1642 sensor using a 2D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="1642" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="2d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

30
ros_driver/ti_mmwave_rospkg/launch/1642_multi_2d_1.launch Normal file
View File

@@ -0,0 +1,30 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 1642 sensor using a 2D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="1642" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="2d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 -1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

27
ros_driver/ti_mmwave_rospkg/launch/1843_multi_2d_0.launch Normal file
View File

@@ -0,0 +1,27 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

26
ros_driver/ti_mmwave_rospkg/launch/1843_multi_2d_1.launch Normal file
View File

@@ -0,0 +1,26 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

27
ros_driver/ti_mmwave_rospkg/launch/1843_multi_3d_0.launch Normal file
View File

@@ -0,0 +1,27 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

26
ros_driver/ti_mmwave_rospkg/launch/1843_multi_3d_1.launch Normal file
View File

@@ -0,0 +1,26 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1443" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

31
ros_driver/ti_mmwave_rospkg/launch/6843AOP_multi_2d_0.launch Normal file
View File

@@ -0,0 +1,31 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

30
ros_driver/ti_mmwave_rospkg/launch/6843AOP_multi_2d_1.launch Normal file
View File

@@ -0,0 +1,30 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

31
ros_driver/ti_mmwave_rospkg/launch/6843AOP_multi_3d_0.launch Normal file
View File

@@ -0,0 +1,31 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

30
ros_driver/ti_mmwave_rospkg/launch/6843AOP_multi_3d_1.launch Normal file
View File

@@ -0,0 +1,30 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

31
ros_driver/ti_mmwave_rospkg/launch/6843_multi_2d_0.launch Normal file
View File

@@ -0,0 +1,31 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

30
ros_driver/ti_mmwave_rospkg/launch/6843_multi_2d_1.launch Normal file
View File

@@ -0,0 +1,30 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

31
ros_driver/ti_mmwave_rospkg/launch/6843_multi_3d_0.launch Normal file
View File

@@ -0,0 +1,31 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="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"/>
</launch>

30
ros_driver/ti_mmwave_rospkg/launch/6843_multi_3d_1.launch Normal file
View File

@@ -0,0 +1,30 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyACM2" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM3" />
<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_1"/>
<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="tf" type="static_transform_publisher" name="radar_baselink_1" args="5 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
</launch>

40
ros_driver/ti_mmwave_rospkg/launch/camera_overlay.launch Normal file
View File

@@ -0,0 +1,40 @@
<launch>
<arg name="device" value="1642" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="2d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<node name="usb_webcam" pkg="usb_webcam" type="usb_webcam_node" output="screen" >
<param name="video_device" value="/dev/video1" />
<param name="camera_info_url" value="file:///home/leo/workspace/usb_webcam/src/cfg/elp-usb8mp02g-l36.yaml" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_webcam" />
<param name="io_method" value="mmap"/>
<param name="framerate" value="30" />
<param name="autoexposure" value="true" />
<param name="autofocus" value="true" />
</node>
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<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_0"/>
<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="tf" type="static_transform_publisher" name="radar_baselink" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
<node pkg="tf" type="static_transform_publisher" name="camera_baselink" args="-2 0 .95 -1.5707963267948966 0 -1.5707963267948966 ti_mmwave_pcl usb_webcam 100"/>
<!-- Launch Rviz with pre-defined configuration to view mmWave sensor detected object data (color by intensity) -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_camera_overlay.rviz"/>
</launch>

15
ros_driver/ti_mmwave_rospkg/launch/rosbag_show.launch Normal file
View File

@@ -0,0 +1,15 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1642" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="2d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<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]}"/>
<!-- Static transform from map to base_radar_link for visualization of stand-alone mmWave sensor using Rviz -->
<node pkg="tf" type="static_transform_publisher" name="static_tf_map_to_base_radar_link" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave 30"/>
<!-- Launch Rviz with pre-defined configuration to view mmWave sensor detected object data (color by intensity) -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave.rviz"/>
</launch>

174
ros_driver/ti_mmwave_rospkg/launch/rviz/ti_mmwave.rviz Normal file
View File

@@ -0,0 +1,174 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Grid1
- /Grid1/Offset1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 569
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.0399999991
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 35.2955856
Min Color: 255; 255; 0
Min Intensity: 9.54242516
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /usb_webcam/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
- Class: rviz/Image
Enabled: false
Image Topic: /usb_webcam/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 12.8599997
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.56979632
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.14159274
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000208000002c8fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000028000002c8000000dd00fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000005c00fffffffb000000100044006900730070006c0061007900730100000000000002080000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650200000156000000000000042b00000384000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff00000058fc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003f1000002c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 65
Y: 24

148
ros_driver/ti_mmwave_rospkg/launch/rviz/ti_mmwave_3d.rviz Normal file
View File

@@ -0,0 +1,148 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Grid1
- /Grid1/Offset1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 595
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0.100000001
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 37.8426056
Min Color: 0; 0; 255
Min Intensity: 3.01029992
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 0.0181882642
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.0697968528
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.20659256
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000020b000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003ee000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 65
Y: 24

164
ros_driver/ti_mmwave_rospkg/launch/rviz/ti_mmwave_camera_overlay.rviz Normal file
View File

@@ -0,0 +1,164 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Grid1
- /Grid1/Offset1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /Camera1
Splitter Ratio: 0.5
Tree Height: 1430
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.0399999991
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 31.970047
Min Color: 255; 0; 0
Min Intensity: 16.3346844
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_0
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Class: rviz/Camera
Enabled: true
Image Rendering: background and overlay
Image Topic: /usb_webcam/image_raw
Name: Camera
Overlay Alpha: 0.5
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Visibility:
Grid: true
PointCloud2: true
Value: true
Zoom Factor: 1
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 12.8599997
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.56979632
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.14159274
Saved: ~
Window Geometry:
Camera:
collapsed: false
Displays:
collapsed: false
Height: 1764
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001ad00000637fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003400000637000000e900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061030000024f0000009a000009ef00000610000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000ca00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000c1e0000004afc0100000002fb0000000800540069006d0065010000000000000c1e0000043900fffffffb0000000800540069006d0065010000000000000450000000000000000000000a680000063700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 3102
X: 98
Y: 36

178
ros_driver/ti_mmwave_rospkg/launch/rviz/ti_mmwave_multi.rviz Normal file
View File

@@ -0,0 +1,178 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1/Offset1
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 583
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: Intensity
Decay Time: 0.400000006
Enabled: true
Invert Rainbow: false
Max Color: 102; 0; 0
Max Intensity: 21.2000008
Min Color: 255; 102; 102
Min Intensity: 11.3999996
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_0
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 204; 204; 0
Color Transformer: Intensity
Decay Time: 0.400000006
Enabled: true
Invert Rainbow: false
Max Color: 102; 102; 0
Max Intensity: 27.7999992
Min Color: 255; 255; 51
Min Intensity: 12.8999996
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_1
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 13.5551214
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.234797031
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.07158875
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001ad000002d6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002d6000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000004afc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000044c000002d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 65
Y: 24

16
ros_driver/ti_mmwave_rospkg/mmWave_nodelets.xml Normal file
View File

@@ -0,0 +1,16 @@
<library path="lib/libmmwave">
<class name="ti_mmwave_rospkg/mmWaveCommSrv" type="ti_mmwave_rospkg::mmWaveCommSrv" base_class_type="nodelet::Nodelet">
<description>
Command Service Nodelet
</description>
</class>
<class name="ti_mmwave_rospkg/mmWaveDataHdl" type="ti_mmwave_rospkg::mmWaveDataHdl" base_class_type="nodelet::Nodelet">
<description>
Data Handler Nodelet
</description>
</class>
<depend>serial</depend>
</library>

10
ros_driver/ti_mmwave_rospkg/msg/RadarScan.msg Normal file
View File

@@ -0,0 +1,10 @@
Header header
uint16 point_id
float32 x
float32 y
float32 z
float32 range
float32 velocity
uint16 doppler_bin
float32 bearing
float32 intensity

60
ros_driver/ti_mmwave_rospkg/package.xml Normal file
View File

@@ -0,0 +1,60 @@
<?xml version="1.0"?>
<package>
<name>ti_mmwave_rospkg</name>
<version>3.3.0</version>
<description>The ti_mmwave_rospkg package. Publish messages with doppler information. Multi-sensor and camera overlay support. Modified by Leo Zhang</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dr.leo.zhang@outlook.com">Leo Zhang</maintainer>
<maintainer email="a-wendell@ti.com">Allison Wendell</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">https://github.com/radar-lab/ti_mmwave_rospkg</url>
<url type="website">https://bitbucket.itg.ti.com/scm/mmwave_apps/ros_multisensor_demo/ti_mmwave_rospkg</url>
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<author email="dr.leo.zhang@outlook.com">Leo Zhang</author>
<author email="a-wendell@ti.com">Allison Wendell</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>serial</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>nodelet</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>serial</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<nodelet plugin="${prefix}/mmWave_nodelets.xml" />
</export>
</package>

852
ros_driver/ti_mmwave_rospkg/src/DataHandlerClass.cpp Normal file
View File

@@ -0,0 +1,852 @@
#include <DataHandlerClass.h>
DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) {
DataUARTHandler_pub = nh->advertise<sensor_msgs::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);
radar_scan_pub = nh->advertise<ti_mmwave_rospkg::RadarScan>("/ti_mmwave/radar_scan", 100);
marker_pub = nh->advertise<visualization_msgs::Marker>("/ti_mmwave/radar_scan_markers", 100);
maxAllowedElevationAngleDeg = 90; // Use max angle if none specified
maxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified
// Wait for parameters
while(!nh->hasParam("/ti_mmwave/doppler_vel_resolution")){}
nh->getParam("/ti_mmwave/numAdcSamples", nr);
nh->getParam("/ti_mmwave/numLoops", nd);
nh->getParam("/ti_mmwave/num_TX", ntx);
nh->getParam("/ti_mmwave/f_s", fs);
nh->getParam("/ti_mmwave/f_c", fc);
nh->getParam("/ti_mmwave/BW", BW);
nh->getParam("/ti_mmwave/PRI", PRI);
nh->getParam("/ti_mmwave/t_fr", tfr);
nh->getParam("/ti_mmwave/max_range", max_range);
nh->getParam("/ti_mmwave/range_resolution", vrange);
nh->getParam("/ti_mmwave/max_doppler_vel", max_vel);
nh->getParam("/ti_mmwave/doppler_vel_resolution", vvel);
ROS_INFO("\n\n==============================\nList of parameters\n==============================\nNumber of range samples: %d\nNumber of chirps: %d\nf_s: %.3f MHz\nf_c: %.3f GHz\nBandwidth: %.3f MHz\nPRI: %.3f us\nFrame time: %.3f ms\nMax range: %.3f m\nRange resolution: %.3f m\nMax Doppler: +-%.3f m/s\nDoppler resolution: %.3f m/s\n==============================\n", \
nr, nd, fs/1e6, fc/1e9, BW/1e6, PRI*1e6, tfr*1e3, max_range, vrange, max_vel/2, vvel);
}
void DataUARTHandler::setFrameID(char* myFrameID)
{
frameID = myFrameID;
}
/*Implementation of setUARTPort*/
void DataUARTHandler::setUARTPort(char* mySerialPort)
{
dataSerialPort = mySerialPort;
}
/*Implementation of setBaudRate*/
void DataUARTHandler::setBaudRate(int myBaudRate)
{
dataBaudRate = myBaudRate;
}
/*Implementation of setMaxAllowedElevationAngleDeg*/
void DataUARTHandler::setMaxAllowedElevationAngleDeg(int myMaxAllowedElevationAngleDeg)
{
maxAllowedElevationAngleDeg = myMaxAllowedElevationAngleDeg;
}
/*Implementation of setMaxAllowedAzimuthAngleDeg*/
void DataUARTHandler::setMaxAllowedAzimuthAngleDeg(int myMaxAllowedAzimuthAngleDeg)
{
maxAllowedAzimuthAngleDeg = myMaxAllowedAzimuthAngleDeg;
}
/*Implementation of readIncomingData*/
void *DataUARTHandler::readIncomingData(void)
{
int firstPacketReady = 0;
uint8_t last8Bytes[8] = {0};
/*Open UART Port and error checking*/
serial::Serial mySerialObject("", dataBaudRate, serial::Timeout::simpleTimeout(100));
mySerialObject.setPort(dataSerialPort);
try
{
mySerialObject.open();
} catch (std::exception &e1) {
ROS_INFO("DataUARTHandler Read Thread: Failed to open Data serial port with error: %s", e1.what());
ROS_INFO("DataUARTHandler Read Thread: Waiting 20 seconds before trying again...");
try
{
// Wait 20 seconds and try to open serial port again
ros::Duration(20).sleep();
mySerialObject.open();
} catch (std::exception &e2) {
ROS_ERROR("DataUARTHandler Read Thread: Failed second time to open Data serial port, error: %s", e1.what());
ROS_ERROR("DataUARTHandler Read Thread: Port could not be opened. Port is \"%s\" and baud rate is %d", dataSerialPort, dataBaudRate);
pthread_exit(NULL);
}
}
if(mySerialObject.isOpen())
ROS_INFO("DataUARTHandler Read Thread: Port is open");
else
ROS_ERROR("DataUARTHandler Read Thread: Port could not be opened");
/*Quick magicWord check to synchronize program with data Stream*/
while(!isMagicWord(last8Bytes))
{
last8Bytes[0] = last8Bytes[1];
last8Bytes[1] = last8Bytes[2];
last8Bytes[2] = last8Bytes[3];
last8Bytes[3] = last8Bytes[4];
last8Bytes[4] = last8Bytes[5];
last8Bytes[5] = last8Bytes[6];
last8Bytes[6] = last8Bytes[7];
mySerialObject.read(&last8Bytes[7], 1);
}
/*Lock nextBufp before entering main loop*/
pthread_mutex_lock(&nextBufp_mutex);
while(ros::ok())
{
/*Start reading UART data and writing to buffer while also checking for magicWord*/
last8Bytes[0] = last8Bytes[1];
last8Bytes[1] = last8Bytes[2];
last8Bytes[2] = last8Bytes[3];
last8Bytes[3] = last8Bytes[4];
last8Bytes[4] = last8Bytes[5];
last8Bytes[5] = last8Bytes[6];
last8Bytes[6] = last8Bytes[7];
mySerialObject.read(&last8Bytes[7], 1);
nextBufp->push_back( last8Bytes[7] ); //push byte onto buffer
//ROS_INFO("DataUARTHandler Read Thread: last8bytes = %02x%02x %02x%02x %02x%02x %02x%02x", last8Bytes[7], last8Bytes[6], last8Bytes[5], last8Bytes[4], last8Bytes[3], last8Bytes[2], last8Bytes[1], last8Bytes[0]);
/*If a magicWord is found wait for sorting to finish and switch buffers*/
if( isMagicWord(last8Bytes) )
{
//ROS_INFO("Found magic word");
/*Lock countSync Mutex while unlocking nextBufp so that the swap thread can use it*/
pthread_mutex_lock(&countSync_mutex);
pthread_mutex_unlock(&nextBufp_mutex);
/*increment countSync*/
countSync++;
/*If this is the first packet to be found, increment countSync again since Sort thread is not reading data yet*/
if(firstPacketReady == 0)
{
countSync++;
firstPacketReady = 1;
}
/*Signal Swap Thread to run if countSync has reached its max value*/
if(countSync == COUNT_SYNC_MAX)
{
pthread_cond_signal(&countSync_max_cv);
}
/*Wait for the Swap thread to finish swapping pointers and signal us to continue*/
pthread_cond_wait(&read_go_cv, &countSync_mutex);
/*Unlock countSync so that Swap Thread can use it*/
pthread_mutex_unlock(&countSync_mutex);
pthread_mutex_lock(&nextBufp_mutex);
nextBufp->clear();
memset(last8Bytes, 0, sizeof(last8Bytes));
}
}
mySerialObject.close();
pthread_exit(NULL);
}
int DataUARTHandler::isMagicWord(uint8_t last8Bytes[8])
{
int val = 0, i = 0, j = 0;
for(i = 0; i < 8 ; i++)
{
if( last8Bytes[i] == magicWord[i])
{
j++;
}
}
if( j == 8)
{
val = 1;
}
return val;
}
void *DataUARTHandler::syncedBufferSwap(void)
{
while(ros::ok())
{
pthread_mutex_lock(&countSync_mutex);
while(countSync < COUNT_SYNC_MAX)
{
pthread_cond_wait(&countSync_max_cv, &countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
pthread_mutex_lock(&nextBufp_mutex);
std::vector<uint8_t>* tempBufp = currentBufp;
this->currentBufp = this->nextBufp;
this->nextBufp = tempBufp;
pthread_mutex_unlock(&currentBufp_mutex);
pthread_mutex_unlock(&nextBufp_mutex);
countSync = 0;
pthread_cond_signal(&sort_go_cv);
pthread_cond_signal(&read_go_cv);
}
pthread_mutex_unlock(&countSync_mutex);
}
pthread_exit(NULL);
}
void *DataUARTHandler::sortIncomingData( void )
{
MmwDemo_Output_TLV_Types tlvType = MMWDEMO_OUTPUT_MSG_NULL;
uint32_t tlvLen = 0;
uint32_t headerSize;
unsigned int currentDatap = 0;
SorterState sorterState = READ_HEADER;
int i = 0, tlvCount = 0, offset = 0;
int j = 0;
float maxElevationAngleRatioSquared;
float maxAzimuthAngleRatio;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>> RScan(new pcl::PointCloud<pcl::PointXYZI>);
ti_mmwave_rospkg::RadarScan radarscan;
//wait for first packet to arrive
pthread_mutex_lock(&countSync_mutex);
pthread_cond_wait(&sort_go_cv, &countSync_mutex);
pthread_mutex_unlock(&countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
while(ros::ok())
{
switch(sorterState)
{
case READ_HEADER:
//init variables
mmwData.numObjOut = 0;
//make sure packet has at least first three fields (12 bytes) before we read them (does not include magicWord since it was already removed)
if(currentBufp->size() < 12)
{
sorterState = SWAP_BUFFERS;
break;
}
//get version (4 bytes)
memcpy( &mmwData.header.version, &currentBufp->at(currentDatap), sizeof(mmwData.header.version));
currentDatap += ( sizeof(mmwData.header.version) );
//get totalPacketLen (4 bytes)
memcpy( &mmwData.header.totalPacketLen, &currentBufp->at(currentDatap), sizeof(mmwData.header.totalPacketLen));
currentDatap += ( sizeof(mmwData.header.totalPacketLen) );
//get platform (4 bytes)
memcpy( &mmwData.header.platform, &currentBufp->at(currentDatap), sizeof(mmwData.header.platform));
currentDatap += ( sizeof(mmwData.header.platform) );
//if packet doesn't have correct header size (which is based on platform), throw it away
// (does not include magicWord since it was already removed)
if ((mmwData.header.platform & 0xFFFF) == 0x1443) // platform is xWR1443)
{
headerSize = 7 * 4; // xWR1443 SDK demo header does not have subFrameNumber field
}
else
{
headerSize = 8 * 4; // header includes subFrameNumber field
}
if(currentBufp->size() < headerSize) {
sorterState = SWAP_BUFFERS;
break;
}
//get frameNumber (4 bytes)
memcpy( &mmwData.header.frameNumber, &currentBufp->at(currentDatap), sizeof(mmwData.header.frameNumber));
currentDatap += ( sizeof(mmwData.header.frameNumber) );
//get timeCpuCycles (4 bytes)
memcpy( &mmwData.header.timeCpuCycles, &currentBufp->at(currentDatap), sizeof(mmwData.header.timeCpuCycles));
currentDatap += ( sizeof(mmwData.header.timeCpuCycles) );
//get numDetectedObj (4 bytes)
memcpy( &mmwData.header.numDetectedObj, &currentBufp->at(currentDatap), sizeof(mmwData.header.numDetectedObj));
currentDatap += ( sizeof(mmwData.header.numDetectedObj) );
//get numTLVs (4 bytes)
memcpy( &mmwData.header.numTLVs, &currentBufp->at(currentDatap), sizeof(mmwData.header.numTLVs));
currentDatap += ( sizeof(mmwData.header.numTLVs) );
//get subFrameNumber (4 bytes) (not used for XWR1443)
if((mmwData.header.platform & 0xFFFF) != 0x1443) {
memcpy( &mmwData.header.subFrameNumber, &currentBufp->at(currentDatap), sizeof(mmwData.header.subFrameNumber));
currentDatap += ( sizeof(mmwData.header.subFrameNumber) );
}
//if packet lengths do not match, throw it away
if(mmwData.header.totalPacketLen == currentBufp->size() )
{
sorterState = CHECK_TLV_TYPE;
}
else sorterState = SWAP_BUFFERS;
break;
case READ_OBJ_STRUCT:
// CHECK_TLV_TYPE code has already read tlvType and tlvLen
i = 0;
offset = 0;
if (((mmwData.header.version >> 24) & 0xFF) < 3) // SDK version is older than 3.x
{
//get number of objects
memcpy( &mmwData.numObjOut, &currentBufp->at(currentDatap), sizeof(mmwData.numObjOut));
currentDatap += ( sizeof(mmwData.numObjOut) );
//get xyzQFormat
memcpy( &mmwData.xyzQFormat, &currentBufp->at(currentDatap), sizeof(mmwData.xyzQFormat));
currentDatap += ( sizeof(mmwData.xyzQFormat) );
}
else // SDK version is at least 3.x
{
mmwData.numObjOut = mmwData.header.numDetectedObj;
}
// RScan->header.seq = 0;
// RScan->header.stamp = (uint64_t)(ros::Time::now());
// RScan->header.stamp = (uint32_t) mmwData.header.timeCpuCycles;
RScan->header.frame_id = frameID;
RScan->height = 1;
RScan->width = mmwData.numObjOut;
RScan->is_dense = 1;
RScan->points.resize(RScan->width * RScan->height);
// Calculate ratios for max desired elevation and azimuth angles
if ((maxAllowedElevationAngleDeg >= 0) && (maxAllowedElevationAngleDeg < 90)) {
maxElevationAngleRatioSquared = tan(maxAllowedElevationAngleDeg * M_PI / 180.0);
maxElevationAngleRatioSquared = maxElevationAngleRatioSquared * maxElevationAngleRatioSquared;
} else maxElevationAngleRatioSquared = -1;
if ((maxAllowedAzimuthAngleDeg >= 0) && (maxAllowedAzimuthAngleDeg < 90)) maxAzimuthAngleRatio = tan(maxAllowedAzimuthAngleDeg * M_PI / 180.0);
else maxAzimuthAngleRatio = -1;
//ROS_INFO("maxElevationAngleRatioSquared = %f", maxElevationAngleRatioSquared);
//ROS_INFO("maxAzimuthAngleRatio = %f", maxAzimuthAngleRatio);
//ROS_INFO("mmwData.numObjOut before = %d", mmwData.numObjOut);
// Populate pointcloud
while( i < mmwData.numObjOut ) {
if (((mmwData.header.version >> 24) & 0xFF) < 3) { // SDK version is older than 3.x
//get object range index
memcpy( &mmwData.objOut.rangeIdx, &currentBufp->at(currentDatap), sizeof(mmwData.objOut.rangeIdx));
currentDatap += ( sizeof(mmwData.objOut.rangeIdx) );
//get object doppler index
memcpy( &mmwData.objOut.dopplerIdx, &currentBufp->at(currentDatap), sizeof(mmwData.objOut.dopplerIdx));
currentDatap += ( sizeof(mmwData.objOut.dopplerIdx) );
//get object peak intensity value
memcpy( &mmwData.objOut.peakVal, &currentBufp->at(currentDatap), sizeof(mmwData.objOut.peakVal));
currentDatap += ( sizeof(mmwData.objOut.peakVal) );
//get object x-coordinate
memcpy( &mmwData.objOut.x, &currentBufp->at(currentDatap), sizeof(mmwData.objOut.x));
currentDatap += ( sizeof(mmwData.objOut.x) );
//get object y-coordinate
memcpy( &mmwData.objOut.y, &currentBufp->at(currentDatap), sizeof(mmwData.objOut.y));
currentDatap += ( sizeof(mmwData.objOut.y) );
//get object z-coordinate
memcpy( &mmwData.objOut.z, &currentBufp->at(currentDatap), sizeof(mmwData.objOut.z));
currentDatap += ( sizeof(mmwData.objOut.z) );
float temp[7];
temp[0] = (float) mmwData.objOut.x;
temp[1] = (float) mmwData.objOut.y;
temp[2] = (float) mmwData.objOut.z;
temp[3] = (float) mmwData.objOut.dopplerIdx;
for (int j = 0; j < 4; j++) {
if (temp[j] > 32767) temp[j] -= 65536;
if (j < 3) temp[j] = temp[j] / pow(2 , mmwData.xyzQFormat);
}
temp[7] = temp[3] * vvel;
temp[4] = (float) mmwData.objOut.rangeIdx * vrange;
temp[5] = 10 * log10(mmwData.objOut.peakVal + 1); // intensity
temp[6] = std::atan2(-temp[0], temp[1]) / M_PI * 180;
uint16_t tmp = (uint16_t)(temp[3] + nd / 2);
// Map mmWave sensor coordinates to ROS coordinate system
RScan->points[i].x = temp[1]; // ROS standard coordinate system X-axis is forward which is the mmWave sensor Y-axis
RScan->points[i].y = -temp[0]; // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis)
RScan->points[i].z = temp[2]; // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis
RScan->points[i].intensity = temp[5];
radarscan.header.frame_id = frameID;
radarscan.header.stamp = ros::Time::now();
radarscan.point_id = i;
radarscan.x = temp[1];
radarscan.y = -temp[0];
radarscan.z = temp[2];
radarscan.range = temp[4];
radarscan.velocity = temp[7];
radarscan.doppler_bin = tmp;
radarscan.bearing = temp[6];
radarscan.intensity = temp[5];
} else { // SDK version is 3.x+
//get object x-coordinate (meters)
memcpy( &mmwData.newObjOut.x, &currentBufp->at(currentDatap), sizeof(mmwData.newObjOut.x));
currentDatap += ( sizeof(mmwData.newObjOut.x) );
//get object y-coordinate (meters)
memcpy( &mmwData.newObjOut.y, &currentBufp->at(currentDatap), sizeof(mmwData.newObjOut.y));
currentDatap += ( sizeof(mmwData.newObjOut.y) );
//get object z-coordinate (meters)
memcpy( &mmwData.newObjOut.z, &currentBufp->at(currentDatap), sizeof(mmwData.newObjOut.z));
currentDatap += ( sizeof(mmwData.newObjOut.z) );
//get object velocity (m/s)
memcpy( &mmwData.newObjOut.velocity, &currentBufp->at(currentDatap), sizeof(mmwData.newObjOut.velocity));
currentDatap += ( sizeof(mmwData.newObjOut.velocity) );
// Map mmWave sensor coordinates to ROS coordinate system
RScan->points[i].x = mmwData.newObjOut.y; // ROS standard coordinate system X-axis is forward which is the mmWave sensor Y-axis
RScan->points[i].y = -mmwData.newObjOut.x; // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis)
RScan->points[i].z = mmwData.newObjOut.z; // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis
radarscan.header.frame_id = frameID;
radarscan.header.stamp = ros::Time::now();
radarscan.point_id = i;
radarscan.x = mmwData.newObjOut.y;
radarscan.y = -mmwData.newObjOut.x;
radarscan.z = mmwData.newObjOut.z;
// radarscan.range = temp[4];
radarscan.velocity = mmwData.newObjOut.velocity;
// radarscan.doppler_bin = tmp;
// radarscan.bearing = temp[6];
// radarscan.intensity = temp[5];
// For SDK 3.x, intensity is replaced by snr in sideInfo and is parsed in the READ_SIDE_INFO code
}
if (((maxElevationAngleRatioSquared == -1) ||
(((RScan->points[i].z * RScan->points[i].z) / (RScan->points[i].x * RScan->points[i].x +
RScan->points[i].y * RScan->points[i].y)
) < maxElevationAngleRatioSquared)
) &&
((maxAzimuthAngleRatio == -1) || (fabs(RScan->points[i].y / RScan->points[i].x) < maxAzimuthAngleRatio)) &&
(RScan->points[i].x != 0)
)
{
radar_scan_pub.publish(radarscan);
}
i++;
}
sorterState = CHECK_TLV_TYPE;
break;
case READ_SIDE_INFO:
// Make sure we already received and parsed detected obj list (READ_OBJ_STRUCT)
if (mmwData.numObjOut > 0)
{
for (i = 0; i < mmwData.numObjOut; i++)
{
//get snr (unit is 0.1 steps of dB)
memcpy( &mmwData.sideInfo.snr, &currentBufp->at(currentDatap), sizeof(mmwData.sideInfo.snr));
currentDatap += ( sizeof(mmwData.sideInfo.snr) );
//get noise (unit is 0.1 steps of dB)
memcpy( &mmwData.sideInfo.noise, &currentBufp->at(currentDatap), sizeof(mmwData.sideInfo.noise));
currentDatap += ( sizeof(mmwData.sideInfo.noise) );
RScan->points[i].intensity = (float) mmwData.sideInfo.snr / 10.0; // Use snr for "intensity" field (divide by 10 since unit of snr is 0.1dB)
}
}
else // else just skip side info section if we have not already received and parsed detected obj list
{
i = 0;
while (i++ < tlvLen - 1)
{
//ROS_INFO("DataUARTHandler Sort Thread : Parsing Side Info i=%d and tlvLen = %u", i, tlvLen);
}
currentDatap += tlvLen;
}
sorterState = CHECK_TLV_TYPE;
break;
case READ_LOG_MAG_RANGE:
{
sorterState = CHECK_TLV_TYPE;
}
break;
case READ_NOISE:
{
i = 0;
while (i++ < tlvLen - 1)
{
//ROS_INFO("DataUARTHandler Sort Thread : Parsing Noise Profile i=%d and tlvLen = %u", i, tlvLen);
}
currentDatap += tlvLen;
sorterState = CHECK_TLV_TYPE;
}
break;
case READ_AZIMUTH:
{
i = 0;
while (i++ < tlvLen - 1)
{
//ROS_INFO("DataUARTHandler Sort Thread : Parsing Azimuth Profile i=%d and tlvLen = %u", i, tlvLen);
}
currentDatap += tlvLen;
sorterState = CHECK_TLV_TYPE;
}
break;
case READ_DOPPLER:
{
i = 0;
while (i++ < tlvLen - 1)
{
//ROS_INFO("DataUARTHandler Sort Thread : Parsing Doppler Profile i=%d and tlvLen = %u", i, tlvLen);
}
currentDatap += tlvLen;
sorterState = CHECK_TLV_TYPE;
}
break;
case READ_STATS:
{
i = 0;
while (i++ < tlvLen - 1)
{
//ROS_INFO("DataUARTHandler Sort Thread : Parsing Stats Profile i=%d and tlvLen = %u", i, tlvLen);
}
currentDatap += tlvLen;
sorterState = CHECK_TLV_TYPE;
}
break;
case CHECK_TLV_TYPE:
//ROS_INFO("DataUARTHandler Sort Thread : tlvCount = %d, numTLV = %d", tlvCount, mmwData.header.numTLVs);
if(tlvCount++ >= mmwData.header.numTLVs) // Done parsing all received TLV sections
{
// Publish detected object pointcloud
if (mmwData.numObjOut > 0)
{
j = 0;
for (i = 0; i < mmwData.numObjOut; i++)
{
// Keep point if elevation and azimuth angles are less than specified max values
// (NOTE: The following calculations are done using ROS standard coordinate system axis definitions where X is forward and Y is left)
if (((maxElevationAngleRatioSquared == -1) ||
(((RScan->points[i].z * RScan->points[i].z) / (RScan->points[i].x * RScan->points[i].x +
RScan->points[i].y * RScan->points[i].y)
) < maxElevationAngleRatioSquared)
) &&
((maxAzimuthAngleRatio == -1) || (fabs(RScan->points[i].y / RScan->points[i].x) < maxAzimuthAngleRatio)) &&
(RScan->points[i].x != 0)
)
{
//ROS_INFO("Kept point");
// copy: points[i] => points[j]
memcpy( &RScan->points[j], &RScan->points[i], sizeof(RScan->points[i]));
j++;
}
}
mmwData.numObjOut = j; // update number of objects as some points may have been removed
// Resize point cloud since some points may have been removed
RScan->width = mmwData.numObjOut;
RScan->points.resize(RScan->width * RScan->height);
//ROS_INFO("mmwData.numObjOut after = %d", mmwData.numObjOut);
//ROS_INFO("DataUARTHandler Sort Thread: number of obj = %d", mmwData.numObjOut );
DataUARTHandler_pub.publish(RScan);
}
//ROS_INFO("DataUARTHandler Sort Thread : CHECK_TLV_TYPE state says tlvCount max was reached, going to switch buffer state");
sorterState = SWAP_BUFFERS;
}
else // More TLV sections to parse
{
//get tlvType (32 bits) & remove from queue
memcpy( &tlvType, &currentBufp->at(currentDatap), sizeof(tlvType));
currentDatap += ( sizeof(tlvType) );
//ROS_INFO("DataUARTHandler Sort Thread : sizeof(tlvType) = %d", sizeof(tlvType));
//get tlvLen (32 bits) & remove from queue
memcpy( &tlvLen, &currentBufp->at(currentDatap), sizeof(tlvLen));
currentDatap += ( sizeof(tlvLen) );
//ROS_INFO("DataUARTHandler Sort Thread : sizeof(tlvLen) = %d", sizeof(tlvLen));
//ROS_INFO("DataUARTHandler Sort Thread : tlvType = %d, tlvLen = %d", (int) tlvType, tlvLen);
switch(tlvType)
{
case MMWDEMO_OUTPUT_MSG_NULL:
break;
case MMWDEMO_OUTPUT_MSG_DETECTED_POINTS:
//ROS_INFO("DataUARTHandler Sort Thread : Object TLV");
sorterState = READ_OBJ_STRUCT;
break;
case MMWDEMO_OUTPUT_MSG_RANGE_PROFILE:
//ROS_INFO("DataUARTHandler Sort Thread : Range TLV");
sorterState = READ_LOG_MAG_RANGE;
break;
case MMWDEMO_OUTPUT_MSG_NOISE_PROFILE:
//ROS_INFO("DataUARTHandler Sort Thread : Noise TLV");
sorterState = READ_NOISE;
break;
case MMWDEMO_OUTPUT_MSG_AZIMUTH_STATIC_HEAT_MAP:
//ROS_INFO("DataUARTHandler Sort Thread : Azimuth Heat TLV");
sorterState = READ_AZIMUTH;
break;
case MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP:
//ROS_INFO("DataUARTHandler Sort Thread : R/D Heat TLV");
sorterState = READ_DOPPLER;
break;
case MMWDEMO_OUTPUT_MSG_STATS:
//ROS_INFO("DataUARTHandler Sort Thread : Stats TLV");
sorterState = READ_STATS;
break;
case MMWDEMO_OUTPUT_MSG_DETECTED_POINTS_SIDE_INFO:
//ROS_INFO("DataUARTHandler Sort Thread : Side info TLV");
sorterState = READ_SIDE_INFO;
break;
case MMWDEMO_OUTPUT_MSG_MAX:
//ROS_INFO("DataUARTHandler Sort Thread : Header TLV");
sorterState = READ_HEADER;
break;
default: break;
}
}
break;
case SWAP_BUFFERS:
pthread_mutex_lock(&countSync_mutex);
pthread_mutex_unlock(&currentBufp_mutex);
countSync++;
if(countSync == COUNT_SYNC_MAX)
{
pthread_cond_signal(&countSync_max_cv);
}
pthread_cond_wait(&sort_go_cv, &countSync_mutex);
pthread_mutex_unlock(&countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
currentDatap = 0;
tlvCount = 0;
sorterState = READ_HEADER;
break;
default: break;
}
}
pthread_exit(NULL);
}
void DataUARTHandler::start(void)
{
pthread_t uartThread, sorterThread, swapThread;
int iret1, iret2, iret3;
pthread_mutex_init(&countSync_mutex, NULL);
pthread_mutex_init(&nextBufp_mutex, NULL);
pthread_mutex_init(&currentBufp_mutex, NULL);
pthread_cond_init(&countSync_max_cv, NULL);
pthread_cond_init(&read_go_cv, NULL);
pthread_cond_init(&sort_go_cv, NULL);
countSync = 0;
/* Create independent threads each of which will execute function */
iret1 = pthread_create( &uartThread, NULL, this->readIncomingData_helper, this);
if(iret1)
{
ROS_INFO("Error - pthread_create() return code: %d\n",iret1);
ros::shutdown();
}
iret2 = pthread_create( &sorterThread, NULL, this->sortIncomingData_helper, this);
if(iret2)
{
ROS_INFO("Error - pthread_create() return code: %d\n",iret1);
ros::shutdown();
}
iret3 = pthread_create( &swapThread, NULL, this->syncedBufferSwap_helper, this);
if(iret3)
{
ROS_INFO("Error - pthread_create() return code: %d\n",iret1);
ros::shutdown();
}
ros::spin();
pthread_join(iret1, NULL);
ROS_INFO("DataUARTHandler Read Thread joined");
pthread_join(iret2, NULL);
ROS_INFO("DataUARTHandler Sort Thread joined");
pthread_join(iret3, NULL);
ROS_INFO("DataUARTHandler Swap Thread joined");
pthread_mutex_destroy(&countSync_mutex);
pthread_mutex_destroy(&nextBufp_mutex);
pthread_mutex_destroy(&currentBufp_mutex);
pthread_cond_destroy(&countSync_max_cv);
pthread_cond_destroy(&read_go_cv);
pthread_cond_destroy(&sort_go_cv);
}
void* DataUARTHandler::readIncomingData_helper(void *context)
{
return (static_cast<DataUARTHandler*>(context)->readIncomingData());
}
void* DataUARTHandler::sortIncomingData_helper(void *context)
{
return (static_cast<DataUARTHandler*>(context)->sortIncomingData());
}
void* DataUARTHandler::syncedBufferSwap_helper(void *context)
{
return (static_cast<DataUARTHandler*>(context)->syncedBufferSwap());
}
void DataUARTHandler::visualize(const ti_mmwave_rospkg::RadarScan &msg){
visualization_msgs::Marker marker;
marker.header.frame_id = frameID;
marker.header.stamp = ros::Time::now();
marker.id = msg.point_id;
marker.type = visualization_msgs::Marker::SPHERE;
marker.lifetime = ros::Duration(tfr);
marker.action = marker.ADD;
marker.pose.position.x = msg.x;
marker.pose.position.y = msg.y;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 0;
marker.scale.x = .03;
marker.scale.y = .03;
marker.scale.z = .03;
marker.color.a = 1;
marker.color.r = (int) 255 * msg.intensity;
marker.color.g = (int) 255 * msg.intensity;
marker.color.b = 1;
marker_pub.publish(marker);
}

117
ros_driver/ti_mmwave_rospkg/src/ParameterParser.cpp Normal file
View File

@@ -0,0 +1,117 @@
#include "ParameterParser.h"
namespace ti_mmwave_rospkg {
PLUGINLIB_EXPORT_CLASS(ti_mmwave_rospkg::ParameterParser, nodelet::Nodelet);
ParameterParser::ParameterParser() {}
void ParameterParser::onInit() {}
void ParameterParser::ParamsParser(ti_mmwave_rospkg::mmWaveCLI &srv, ros::NodeHandle &nh) {
// ROS_ERROR("%s",srv.request.comm.c_str());
// ROS_ERROR("%s",srv.response.resp.c_str());
std::vector <std::string> v;
std::string s = srv.request.comm.c_str();
std::istringstream ss(s);
std::string token;
std::string req;
int i = 0;
while (std::getline(ss, token, ' ')) {
v.push_back(token);
if (i > 0) {
if (!req.compare("profileCfg")) {
switch (i) {
case 2:
nh.setParam("/ti_mmwave/startFreq", std::stof(token)); break;
case 3:
nh.setParam("/ti_mmwave/idleTime", std::stof(token)); break;
case 4:
nh.setParam("/ti_mmwave/adcStartTime", std::stof(token)); break;
case 5:
nh.setParam("/ti_mmwave/rampEndTime", std::stof(token)); break;
case 8:
nh.setParam("/ti_mmwave/freqSlopeConst", std::stof(token)); break;
case 10:
nh.setParam("/ti_mmwave/numAdcSamples", std::stoi(token)); break;
case 11:
nh.setParam("/ti_mmwave/digOutSampleRate", std::stof(token)); break;
case 14:
nh.setParam("/ti_mmwave/rxGain", std::stof(token)); break;
}
} else if (!req.compare("frameCfg")) {
switch (i) {
case 1:
nh.setParam("/ti_mmwave/chirpStartIdx", std::stoi(token)); break;
case 2:
nh.setParam("/ti_mmwave/chirpEndIdx", std::stoi(token)); break;
case 3:
nh.setParam("/ti_mmwave/numLoops", std::stoi(token)); break;
case 4:
nh.setParam("/ti_mmwave/numFrames", std::stoi(token)); break;
case 5:
nh.setParam("/ti_mmwave/framePeriodicity", std::stof(token)); break;
}
}
} else req = token;
i++;
}
}
void ParameterParser::CalParams(ros::NodeHandle &nh) {
float c0 = 299792458;
int chirpStartIdx;
int chirpEndIdx;
int numLoops;
float framePeriodicity;
float startFreq;
float idleTime;
float adcStartTime;
float rampEndTime;
float digOutSampleRate;
float freqSlopeConst;
float numAdcSamples;
nh.getParam("/ti_mmwave/startFreq", startFreq);
nh.getParam("/ti_mmwave/idleTime", idleTime);
nh.getParam("/ti_mmwave/adcStartTime", adcStartTime);
nh.getParam("/ti_mmwave/rampEndTime", rampEndTime);
nh.getParam("/ti_mmwave/digOutSampleRate", digOutSampleRate);
nh.getParam("/ti_mmwave/freqSlopeConst", freqSlopeConst);
nh.getParam("/ti_mmwave/numAdcSamples", numAdcSamples);
nh.getParam("/ti_mmwave/chirpStartIdx", chirpStartIdx);
nh.getParam("/ti_mmwave/chirpEndIdx", chirpEndIdx);
nh.getParam("/ti_mmwave/numLoops", numLoops);
nh.getParam("/ti_mmwave/framePeriodicity", framePeriodicity);
int ntx = chirpEndIdx - chirpStartIdx + 1;
int nd = numLoops;
int nr = numAdcSamples;
float tfr = framePeriodicity * 1e-3;
float fs = digOutSampleRate * 1e3;
float kf = freqSlopeConst * 1e12;
float adc_duration = nr / fs;
float BW = adc_duration * kf;
float PRI = (idleTime + rampEndTime) * 1e-6;
float fc = startFreq * 1e9 + kf * (adcStartTime * 1e-6 + adc_duration / 2);
float vrange = c0 / (2 * BW);
float max_range = nr * vrange;
float max_vel = c0 / (2 * fc * PRI) / ntx;
float vvel = max_vel / nd;
nh.setParam("/ti_mmwave/num_TX", ntx);
nh.setParam("/ti_mmwave/f_s", fs);
nh.setParam("/ti_mmwave/f_c", fc);
nh.setParam("/ti_mmwave/BW", BW);
nh.setParam("/ti_mmwave/PRI", PRI);
nh.setParam("/ti_mmwave/t_fr", tfr);
nh.setParam("/ti_mmwave/max_range", max_range);
nh.setParam("/ti_mmwave/range_resolution", vrange);
nh.setParam("/ti_mmwave/max_doppler_vel", max_vel);
nh.setParam("/ti_mmwave/doppler_vel_resolution", vvel);
}
}

75
ros_driver/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp Normal file
View File

@@ -0,0 +1,75 @@
#include "mmWaveCommSrv.hpp"
namespace ti_mmwave_rospkg
{
PLUGINLIB_EXPORT_CLASS(ti_mmwave_rospkg::mmWaveCommSrv, nodelet::Nodelet);
mmWaveCommSrv::mmWaveCommSrv() {}
void mmWaveCommSrv::onInit()
{
ros::NodeHandle private_nh = getPrivateNodeHandle();
ros::NodeHandle private_nh2("~"); // follow namespace for multiple sensors
private_nh2.getParam("command_port", mySerialPort);
private_nh2.getParam("command_rate", myBaudRate);
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);
NODELET_DEBUG("mmWaveCommsrv: Finished onInit function");
}
bool mmWaveCommSrv::commSrv_cb(mmWaveCLI::Request &req , mmWaveCLI::Response &res) {
NODELET_DEBUG("mmWaveCommSrv: Port is \"%s\" and baud rate is %d", mySerialPort.c_str(), myBaudRate);
/*Open Serial port and error check*/
serial::Serial mySerialObject("", myBaudRate, serial::Timeout::simpleTimeout(1000));
mySerialObject.setPort(mySerialPort.c_str());
try {
mySerialObject.open();
} catch (std::exception &e1) {
ROS_INFO("mmWaveCommSrv: Failed to open User serial port with error: %s", e1.what());
ROS_INFO("mmWaveCommSrv: Waiting 20 seconds before trying again...");
try {
// Wait 20 seconds and try to open serial port again
ros::Duration(20).sleep();
mySerialObject.open();
} catch (std::exception &e2) {
ROS_ERROR("mmWaveCommSrv: Failed second time to open User serial port, error: %s", e1.what());
NODELET_ERROR("mmWaveCommSrv: Port could not be opened. Port is \"%s\" and baud rate is %d", mySerialPort.c_str(), myBaudRate);
return false;
}
}
/*Read any previous pending response(s)*/
while (mySerialObject.available() > 0)
{
mySerialObject.readline(res.resp, 1024, ":/>");
ROS_INFO("mmWaveCommSrv: Received (previous) response from sensor: '%s'", res.resp.c_str());
res.resp = "";
}
/*Send out command received from the client*/
ROS_INFO("mmWaveCommSrv: Sending command to sensor: '%s'", req.comm.c_str());
req.comm.append("\n");
int bytesSent = mySerialObject.write(req.comm.c_str());
/*Read output from mmwDemo*/
mySerialObject.readline(res.resp, 1024, ":/>");
ROS_INFO("mmWaveCommSrv: Received response from sensor: '%s'", res.resp.c_str());
mySerialObject.close();
return true;
}
}

51
ros_driver/ti_mmwave_rospkg/src/mmWaveDataHdl.cpp Normal file
View File

@@ -0,0 +1,51 @@
#include "mmWaveDataHdl.hpp"
#include "DataHandlerClass.h"
namespace ti_mmwave_rospkg
{
PLUGINLIB_EXPORT_CLASS(ti_mmwave_rospkg::mmWaveDataHdl, nodelet::Nodelet);
mmWaveDataHdl::mmWaveDataHdl() {}
void mmWaveDataHdl::onInit()
{
ros::NodeHandle private_nh("~");
std::string mySerialPort;
std::string myFrameID;
int myBaudRate;
int myMaxAllowedElevationAngleDeg;
int myMaxAllowedAzimuthAngleDeg;
private_nh.getParam("data_port", mySerialPort);
private_nh.getParam("data_rate", myBaudRate);
private_nh.getParam("frame_id", myFrameID);
if (!(private_nh.getParam("max_allowed_elevation_angle_deg", myMaxAllowedElevationAngleDeg))) {
myMaxAllowedElevationAngleDeg = 90; // Use max angle if none specified
}
if (!(private_nh.getParam("max_allowed_azimuth_angle_deg", myMaxAllowedAzimuthAngleDeg))) {
myMaxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified
}
ROS_INFO("mmWaveDataHdl: data_port = %s", mySerialPort.c_str());
ROS_INFO("mmWaveDataHdl: data_rate = %d", myBaudRate);
ROS_INFO("mmWaveDataHdl: max_allowed_elevation_angle_deg = %d", myMaxAllowedElevationAngleDeg);
ROS_INFO("mmWaveDataHdl: max_allowed_azimuth_angle_deg = %d", myMaxAllowedAzimuthAngleDeg);
DataUARTHandler DataHandler(&private_nh);
DataHandler.setFrameID( (char*) myFrameID.c_str() );
DataHandler.setUARTPort( (char*) mySerialPort.c_str() );
DataHandler.setBaudRate( myBaudRate );
DataHandler.setMaxAllowedElevationAngleDeg( myMaxAllowedElevationAngleDeg );
DataHandler.setMaxAllowedAzimuthAngleDeg( myMaxAllowedAzimuthAngleDeg );
DataHandler.start();
NODELET_DEBUG("mmWaveDataHdl: Finished onInit function");
}
}

22
ros_driver/ti_mmwave_rospkg/src/mmWaveLoader.cpp Normal file
View File

@@ -0,0 +1,22 @@
#include "ros/ros.h"
#include "nodelet/loader.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "mmWave_Manager");
nodelet::Loader manager(true);
nodelet::M_string remap(ros::names::getRemappings());
nodelet::V_string nargv;
manager.load("mmWaveCommSrv", "ti_mmwave_rospkg/mmWaveCommSrv", remap, nargv);
manager.load("mmWaveDataHdl", "ti_mmwave_rospkg/mmWaveDataHdl", remap, nargv);
ros::spin();
return 0;
}

61
ros_driver/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp Normal file
View File

@@ -0,0 +1,61 @@
#include "ros/ros.h"
#include "ti_mmwave_rospkg/mmWaveCLI.h"
#include <cstdlib>
#include <fstream>
#include <stdio.h>
#include <regex>
#include "ParameterParser.h"
int main(int argc, char **argv) {
ros::init(argc, argv, "mmWaveQuickConfig");
ros::NodeHandle nh;
ti_mmwave_rospkg::mmWaveCLI srv;
if (argc != 2) {
ROS_INFO("mmWaveQuickConfig: usage: mmWaveQuickConfig /file_directory/params.cfg");
return 1;
} else
ROS_INFO("mmWaveQuickConfig: Configuring mmWave device using config file: %s", argv[1]);
ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>("/mmWaveCLI");
std::ifstream myParams;
ti_mmwave_rospkg::ParameterParser parser;
//wait for service to become available
ros::service::waitForService("/mmWaveCLI", 10000);
//wait 0.5 secs to avoid multi-sensor conflicts
ros::Duration(0.5).sleep();
myParams.open(argv[1]);
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*")))) {
// ROS_INFO("mmWaveQuickConfig: Sending command: '%s'", srv.request.comm.c_str() );
if (client.call(srv)) {
if (std::regex_search(srv.response.resp, std::regex("Done"))) {
// ROS_INFO("mmWaveQuickConfig: Command successful (mmWave sensor responded with 'Done')");
parser.ParamsParser(srv, nh);
} else {
ROS_ERROR("mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')");
ROS_ERROR("mmWaveQuickConfig: Response: '%s'", srv.response.resp.c_str() );
return 1;
}
} else {
ROS_ERROR("mmWaveQuickConfig: Failed to call service mmWaveCLI");
ROS_ERROR("%s", srv.request.comm.c_str() );
return 1;
}
}
}
parser.CalParams(nh);
myParams.close();
} else {
ROS_ERROR("mmWaveQuickConfig: Failed to open File %s", argv[1]);
return 1;
}
ROS_INFO("mmWaveQuickConfig: mmWaveQuickConfig will now terminate. Done configuring mmWave device using config file: %s", argv[1]);
return 0;
}

3
ros_driver/ti_mmwave_rospkg/srv/mmWaveCLI.srv Normal file
View File

@@ -0,0 +1,3 @@
string comm
---
string resp