mirror of
https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git
synced 2026-05-21 01:12:08 +00:00
complete merged source of turtlebot labs
This commit is contained in:
Sabeeh Khan
1
autonomous_robotics_ros/src/CMakeLists.txt
Symbolic link
1
autonomous_robotics_ros/src/CMakeLists.txt
Symbolic link
@@ -0,0 +1 @@
|
||||
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
|
||||
0
autonomous_robotics_ros/navigation/move_base/CHANGELOG.rst → autonomous_robotics_ros/src/navigation/move_base/CHANGELOG.rst
Normal file → Executable file
0
autonomous_robotics_ros/navigation/move_base/CHANGELOG.rst → autonomous_robotics_ros/src/navigation/move_base/CHANGELOG.rst
Normal file → Executable file
0
autonomous_robotics_ros/navigation/move_base/CMakeLists.txt → autonomous_robotics_ros/src/navigation/move_base/CMakeLists.txt
Normal file → Executable file
0
autonomous_robotics_ros/navigation/move_base/CMakeLists.txt → autonomous_robotics_ros/src/navigation/move_base/CMakeLists.txt
Normal file → Executable file
0
autonomous_robotics_ros/navigation/move_base/package.xml → autonomous_robotics_ros/src/navigation/move_base/package.xml
Normal file → Executable file
0
autonomous_robotics_ros/navigation/move_base/package.xml → autonomous_robotics_ros/src/navigation/move_base/package.xml
Normal file → Executable file
1
autonomous_robotics_ros/src/serial
Submodule
1
autonomous_robotics_ros/src/serial
Submodule
Submodule autonomous_robotics_ros/src/serial added at cbcca7c837
10
autonomous_robotics_ros/ti_mmwave_rospkg/CMakeLists.txt → autonomous_robotics_ros/src/ti_mmwave_rospkg/CMakeLists.txt
Normal file → Executable file
10
autonomous_robotics_ros/ti_mmwave_rospkg/CMakeLists.txt → autonomous_robotics_ros/src/ti_mmwave_rospkg/CMakeLists.txt
Normal file → Executable file
@@ -57,10 +57,10 @@ find_package(PCL 1.7.2 REQUIRED)
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# RadarScan.msg
|
||||
#)
|
||||
add_message_files(
|
||||
FILES
|
||||
RadarScan.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
add_service_files(
|
||||
@@ -144,6 +144,8 @@ add_definitions(${PCL_DEFINITIONS})
|
||||
src/mmWaveDataHdl.cpp
|
||||
src/mmWaveCommSrv.cpp
|
||||
src/DataHandlerClass.cpp
|
||||
src/mmWaveQuickConfig.cpp
|
||||
src/ParameterParser.cpp
|
||||
)
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
183
autonomous_robotics_ros/src/ti_mmwave_rospkg/README.md
Executable file
183
autonomous_robotics_ros/src/ti_mmwave_rospkg/README.md
Executable file
@@ -0,0 +1,183 @@
|
||||
# TI mmWave ROS Package (Customized)
|
||||
|
||||
#### Auhor and Maintainer: Leo Zhang
|
||||
#### Organization: University of Arizona
|
||||
#### Email: dr.leo.zhang@outlook.com
|
||||
---
|
||||
### Most recent change:
|
||||
|
||||
Add support for XWR18XX 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 AWR1443BOOST
|
||||
TI mmWave AWR1642BOOST
|
||||
TI mmWave AWR1642BOOST ES2.0/3.0 EVM (not tested)
|
||||
TI mmWave AWR1642BOOST ES2.0 EVM
|
||||
TI mmWave AWR1843BOOST 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.**
|
||||
|
||||
Note:
|
||||
AWR1642 ES1.0 (usually purchased before May 2018) uses SDK 1.2. AWR1642 ES2.0 (usually purchased after May 2018) uses SDK 2.0. Same applies to AWR1443. (You can refer to [this thread](https://e2e.ti.com/support/sensors/f/1023/t/692195?tisearch=e2e-sitesearch&keymatch=%20user:356347))
|
||||
|
||||
3. Clone this repo and ROS serial onto your `<workspace dir>/src`:
|
||||
|
||||
```
|
||||
git clone https://github.com/radar-lab/ti_mmwave_rospkg.git
|
||||
git clone https://github.com/wjwwood/serial.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
|
||||
```
|
||||
|
||||
Note: AWR1843 requires SDK 3.2.0.4, which has different output format. The later release will improve this part.
|
||||
|
||||
---
|
||||
### 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 three arguments `<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 -1 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>` (stands for x,y,z for positions) 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
|
||||
```
|
||||
|
||||
---
|
||||
### Changelog:
|
||||
|
||||
```
|
||||
v3.3.0
|
||||
|
||||
Add support for XWR18XX devices. SDK version: 3.2.0.4.
|
||||
|
||||
v3.2.2
|
||||
Fix bugs and update README.
|
||||
|
||||
v3.2.1
|
||||
Support camera overlay over 3D 1443s.
|
||||
|
||||
v3.2.0
|
||||
Added camera overlay support.
|
||||
|
||||
v3.1.0
|
||||
Strengthened code.
|
||||
|
||||
v3.0.0
|
||||
Added README.
|
||||
Improved rviz looking for point cloud data.
|
||||
Added support for multiple radars working together.
|
||||
Improved radar's all around working conditions.
|
||||
|
||||
v2.0.0
|
||||
Added support for ES2.0 EVM devices.
|
||||
|
||||
v1.0.0
|
||||
Added Doppler from TI's mmWave radars.
|
||||
```
|
||||
BIN
autonomous_robotics_ros/src/ti_mmwave_rospkg/auxiliary/mounting.jpg
Executable file
BIN
autonomous_robotics_ros/src/ti_mmwave_rospkg/auxiliary/mounting.jpg
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 32 KiB |
@@ -36,4 +36,5 @@ measureRangeBiasAndRxChanPhase 0 1.5 0.2
|
||||
CQRxSatMonitor 0 3 5 123 0
|
||||
CQSigImgMonitor 0 119 4
|
||||
analogMonitor 1 1
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
@@ -37,4 +37,5 @@ measureRangeBiasAndRxChanPhase 0 1.5 0.2
|
||||
CQRxSatMonitor 0 3 5 123 0
|
||||
CQSigImgMonitor 0 119 4
|
||||
analogMonitor 1 1
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
51
autonomous_robotics_ros/src/ti_mmwave_rospkg/cfg/1642_2d.cfg
Normal file
51
autonomous_robotics_ros/src/ti_mmwave_rospkg/cfg/1642_2d.cfg
Normal file
@@ -0,0 +1,51 @@
|
||||
% ***************************************************************
|
||||
% 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
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
50
autonomous_robotics_ros/src/ti_mmwave_rospkg/cfg/1843_2d.cfg
Normal file
50
autonomous_robotics_ros/src/ti_mmwave_rospkg/cfg/1843_2d.cfg
Normal 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
|
||||
% 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
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
@@ -1,6 +1,6 @@
|
||||
% ***************************************************************
|
||||
% Created for SDK ver:03.02
|
||||
% Created using Visualizer ver:3.2.0.1
|
||||
% Created for SDK ver:03.03
|
||||
% Created using Visualizer ver:3.3.0.0
|
||||
% Frequency:77
|
||||
% Platform:xWR18xx
|
||||
% Scene Classifier:best_range_res
|
||||
@@ -32,7 +32,7 @@ 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 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 0
|
||||
@@ -47,4 +47,5 @@ analogMonitor 0 0
|
||||
aoaFovCfg -1 -90 90 -90 90
|
||||
cfarFovCfg -1 0 0 8.92
|
||||
cfarFovCfg -1 1 -1 1.00
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
@@ -0,0 +1,51 @@
|
||||
% ***************************************************************
|
||||
% 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
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
|
||||
@@ -32,10 +32,10 @@ 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 12 0
|
||||
cfarCfg -1 1 0 4 2 3 1 12 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
|
||||
clutterRemoval -1 1
|
||||
calibDcRangeSig -1 0 -5 8 256
|
||||
extendedMaxVelocity -1 0
|
||||
lvdsStreamCfg -1 0 0 0
|
||||
@@ -47,4 +47,5 @@ analogMonitor 0 0
|
||||
aoaFovCfg -1 -90 90 -90 90
|
||||
cfarFovCfg -1 0 0 8.40
|
||||
cfarFovCfg -1 1 -5.02 5.02
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
@@ -35,11 +35,11 @@ guiMonitor -1 1 0 0 0 0 0
|
||||
cfarCfg -1 0 2 8 4 3 0 15 0
|
||||
cfarCfg -1 1 0 4 2 3 1 15 1
|
||||
multiObjBeamForming -1 1 0.5
|
||||
clutterRemoval -1 0
|
||||
clutterRemoval -1 1
|
||||
calibDcRangeSig -1 0 -5 8 256
|
||||
extendedMaxVelocity -1 0
|
||||
bpmCfg -1 0 0 1
|
||||
lvdsStreamCfg -1 0 0 0
|
||||
lvdsStreamCfg -1 1 1 1
|
||||
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
|
||||
@@ -48,4 +48,6 @@ analogMonitor 0 0
|
||||
aoaFovCfg -1 -90 90 -90 90
|
||||
cfarFovCfg -1 0 0 9.00
|
||||
cfarFovCfg -1 1 -5.14 5.14
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
|
||||
@@ -1,18 +1,18 @@
|
||||
% ***************************************************************
|
||||
% Created for SDK ver:03.00
|
||||
% Created using Visualizer ver:3.0.0.1
|
||||
% Frequency:60.25
|
||||
% 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.045
|
||||
% Range Resolution(m):0.044
|
||||
% Maximum unambiguous Range(m):9.02
|
||||
% Maximum Radial Velocity(m/s):5.06
|
||||
% Radial velocity resolution(m/s):0.64
|
||||
% Frame Duration(msec):33.333
|
||||
% Range Detection Threshold (dB):9
|
||||
% Doppler Detection Threshold (dB):9
|
||||
% Range Peak Grouping:disabled
|
||||
% 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
|
||||
@@ -25,20 +25,27 @@ dfeDataOutputMode 1
|
||||
channelCfg 15 5 0
|
||||
adcCfg 2 1
|
||||
adcbufCfg -1 0 1 1 1
|
||||
profileCfg 0 60.25 48 7 75 0 0 50 1 240 3582 0 0 30
|
||||
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 33.333 1 0
|
||||
frameCfg 0 1 16 0 100 1 0
|
||||
lowPower 0 0
|
||||
guiMonitor -1 1 0 0 0 0 0
|
||||
cfarCfg -1 0 0 8 4 4 0 768 0
|
||||
cfarCfg -1 1 0 4 2 3 1 768 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 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.59
|
||||
cfarFovCfg -1 1 -5.06 5.06
|
||||
cfarFovCfg -1 0 0 8.92
|
||||
cfarFovCfg -1 1 -1 1.00
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
@@ -31,11 +31,11 @@ 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 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
|
||||
clutterRemoval -1 1
|
||||
calibDcRangeSig -1 0 -5 8 256
|
||||
extendedMaxVelocity -1 0
|
||||
bpmCfg -1 0 0 1
|
||||
@@ -48,4 +48,5 @@ analogMonitor 0 0
|
||||
aoaFovCfg -1 -90 90 -90 90
|
||||
cfarFovCfg -1 0 0 9.00
|
||||
cfarFovCfg -1 1 -5.14 5.14
|
||||
calibData 0 0 0
|
||||
sensorStart
|
||||
@@ -1,46 +1,7 @@
|
||||
/*
|
||||
* DataHandlerClass.cpp
|
||||
*
|
||||
* This file defines the DataUARTHandler class.
|
||||
*
|
||||
*
|
||||
* 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 _DATA_HANDLER_CLASS_
|
||||
#define _DATA_HANDLER_CLASS_
|
||||
|
||||
|
||||
#include <ti_mmwave_rospkg/RadarScan.h>
|
||||
#include "mmWave.h"
|
||||
#include <iostream>
|
||||
#include <cstdio>
|
||||
@@ -48,6 +9,16 @@
|
||||
#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{
|
||||
@@ -58,6 +29,8 @@ public:
|
||||
//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);
|
||||
|
||||
@@ -87,6 +60,20 @@ public:
|
||||
|
||||
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;
|
||||
|
||||
@@ -143,10 +130,13 @@ private:
|
||||
/*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
autonomous_robotics_ros/src/ti_mmwave_rospkg/include/ParameterParser.h
Executable file
35
autonomous_robotics_ros/src/ti_mmwave_rospkg/include/ParameterParser.h
Executable 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
|
||||
4
autonomous_robotics_ros/ti_mmwave_rospkg/include/mmWave.h → autonomous_robotics_ros/src/ti_mmwave_rospkg/include/mmWave.h
Normal file → Executable file
4
autonomous_robotics_ros/ti_mmwave_rospkg/include/mmWave.h → autonomous_robotics_ros/src/ti_mmwave_rospkg/include/mmWave.h
Normal file → Executable file
@@ -166,13 +166,9 @@ typedef struct DPIF_PointCloudSideInfo_t
|
||||
|
||||
|
||||
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
|
||||
@@ -0,0 +1,9 @@
|
||||
<launch>
|
||||
|
||||
<!-- Call mmWave sensor launch file -->
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_north.launch"> </include>
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_east.launch"> </include>
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_south.launch"> </include>
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_west.launch"> </include>
|
||||
|
||||
</launch>
|
||||
@@ -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" doc="TI mmWave sensor device type [1443, 1642]"/>
|
||||
<arg name="config" 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/mmWave_00D83B95_00" />
|
||||
<param name="command_rate" value="115200" />
|
||||
<param name="data_port" value="/dev/mmWave_00D83B95_01" />
|
||||
<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/$(arg device)_$(arg config).cfg" output="screen" />
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 -0.19 0.18 1.57 3.1415 0 base_link ti_mmwave_1 100"/>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,32 @@
|
||||
<!--
|
||||
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" doc="TI mmWave sensor device type [1443, 1642]"/>
|
||||
<arg name="config" 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/mmWave_00CE0FCA_00" />
|
||||
<param name="command_rate" value="115200" />
|
||||
<param name="data_port" value="/dev/mmWave_00CE0FCA_01" />
|
||||
<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/$(arg device)_$(arg config).cfg" output="screen" />
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0.19 0 0.18 0 0 3.1415 base_link ti_mmwave_0 100"/>
|
||||
<!--
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>
|
||||
-->
|
||||
</launch>
|
||||
@@ -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" doc="TI mmWave sensor device type [1443, 1642]"/>
|
||||
<arg name="config" 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_2" output="screen">
|
||||
<param name="command_port" value="/dev/mmWave_00CE0E81_00" />
|
||||
<param name="command_rate" value="115200" />
|
||||
<param name="data_port" value="/dev/mmWave_00CE0E81_01" />
|
||||
<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_2"/>
|
||||
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_2"/>
|
||||
</node>
|
||||
|
||||
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
|
||||
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_2" args="$(find ti_mmwave_rospkg)/cfg/$(arg device)_$(arg config).cfg" output="screen" />
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_2" args="-.19 0 0.18 3.14 0 3.14 base_link ti_mmwave_2 100"/>
|
||||
|
||||
</launch>
|
||||
@@ -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" doc="TI mmWave sensor device type [1443, 1642]"/>
|
||||
<arg name="config" 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_3" output="screen">
|
||||
<param name="command_port" value="/dev/mmWave_00CE0FCD_00" />
|
||||
<param name="command_rate" value="115200" />
|
||||
<param name="data_port" value="/dev/mmWave_00CE0FCD_01" />
|
||||
<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_3"/>
|
||||
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_3"/>
|
||||
</node>
|
||||
|
||||
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
|
||||
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_3" args="$(find ti_mmwave_rospkg)/cfg/$(arg device)_$(arg config).cfg" output="screen" />
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_3" args="0 0.19 0.18 -1.57 3.1415 0 base_link ti_mmwave_3 100"/>
|
||||
|
||||
</launch>
|
||||
40
autonomous_robotics_ros/src/ti_mmwave_rospkg/launch/camera_overlay.launch
Executable file
40
autonomous_robotics_ros/src/ti_mmwave_rospkg/launch/camera_overlay.launch
Executable 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>
|
||||
@@ -12,15 +12,19 @@
|
||||
|
||||
<!-- mmWave_Manager node -->
|
||||
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="mmWave_Manager" output="screen">
|
||||
<param name="command_port" value="/dev/ttyACM0" />
|
||||
<param name="command_port" value="/dev/mmWave_00CE0FCA_00" />
|
||||
<param name="command_rate" value="115200" />
|
||||
<param name="data_port" value="/dev/ttyACM1" />
|
||||
<param name="data_port" value="/dev/mmWave_00CE0FCA_01" />
|
||||
<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"/>
|
||||
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl"/>
|
||||
</node>
|
||||
|
||||
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
|
||||
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" args="$(find ti_mmwave_rospkg)/cfg/$(arg device)_$(arg config).cfg" output="screen" />
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0.19 0 0.18 0 0 3.1415 base_link ti_mmwave 100"/>
|
||||
|
||||
</launch>
|
||||
0
autonomous_robotics_ros/ti_mmwave_rospkg/mmWave_nodelets.xml → autonomous_robotics_ros/src/ti_mmwave_rospkg/mmWave_nodelets.xml
Normal file → Executable file
0
autonomous_robotics_ros/ti_mmwave_rospkg/mmWave_nodelets.xml → autonomous_robotics_ros/src/ti_mmwave_rospkg/mmWave_nodelets.xml
Normal file → Executable file
10
autonomous_robotics_ros/src/ti_mmwave_rospkg/msg/RadarScan.msg
Executable file
10
autonomous_robotics_ros/src/ti_mmwave_rospkg/msg/RadarScan.msg
Executable 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
|
||||
10
autonomous_robotics_ros/ti_mmwave_rospkg/package.xml → autonomous_robotics_ros/src/ti_mmwave_rospkg/package.xml
Normal file → Executable file
10
autonomous_robotics_ros/ti_mmwave_rospkg/package.xml → autonomous_robotics_ros/src/ti_mmwave_rospkg/package.xml
Normal file → Executable file
@@ -1,13 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>ti_mmwave_rospkg</name>
|
||||
<version>1.5.0</version>
|
||||
<description>The ti_mmwave_rospkg package</description>
|
||||
<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="maintainer@example.com">maintainer</maintainer>
|
||||
<maintainer email="dr.leo.zhang@outlook.com">Leo Zhang</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
@@ -19,13 +19,13 @@
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/ti_mmwave_rospkg</url> -->
|
||||
<url type="website">https://github.com/radar-lab/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="jane.doe@example.com">Jane Doe</author> -->
|
||||
<author email="dr.leo.zhang@outlook.com">Leo Zhang</author>
|
||||
|
||||
|
||||
<!-- The *_depend tags are used to specify dependencies -->
|
||||
@@ -1,18 +1,12 @@
|
||||
/*
|
||||
* DataHandlerClass.cpp
|
||||
* @file DataHandlerClass.cpp
|
||||
*
|
||||
* This is the implementation of the DataHandlerClass.h
|
||||
* Three threads are spawned when start() is called.
|
||||
* 1) readIncomingData() thread
|
||||
* 2) sortIncomingData() thread
|
||||
* 3) syncedBufferSwap() thread
|
||||
*
|
||||
* Together they implement a double-buffered read from the data serial port
|
||||
* which sorts the data into the class's mmwDataPacket struct.
|
||||
*
|
||||
*
|
||||
* Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
|
||||
* @brief
|
||||
* Handles and publishes incoming data from the sensor and .
|
||||
*
|
||||
* \par
|
||||
* NOTE:
|
||||
* (C) Copyright 2020 Texas Instruments, Inc.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -41,28 +35,39 @@
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include <DataHandlerClass.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 <cmath>
|
||||
|
||||
|
||||
DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1])
|
||||
{
|
||||
nodeHandle = nh;
|
||||
DataUARTHandler_pub = nodeHandle->advertise< sensor_msgs::PointCloud2 >("RScan", 100);
|
||||
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*/
|
||||
@@ -274,6 +279,7 @@ void *DataUARTHandler::sortIncomingData( void )
|
||||
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);
|
||||
@@ -322,8 +328,7 @@ void *DataUARTHandler::sortIncomingData( void )
|
||||
{
|
||||
headerSize = 8 * 4; // header includes subFrameNumber field
|
||||
}
|
||||
if(currentBufp->size() < headerSize)
|
||||
{
|
||||
if(currentBufp->size() < headerSize) {
|
||||
sorterState = SWAP_BUFFERS;
|
||||
break;
|
||||
}
|
||||
@@ -345,12 +350,7 @@ void *DataUARTHandler::sortIncomingData( void )
|
||||
currentDatap += ( sizeof(mmwData.header.numTLVs) );
|
||||
|
||||
//get subFrameNumber (4 bytes) (not used for XWR1443)
|
||||
if ((mmwData.header.platform & 0xFFFF) == 0x1443) // platform is xWR1443)
|
||||
{
|
||||
// xWR1443 SDK demo header does not have subFrameNumber field
|
||||
}
|
||||
else
|
||||
{
|
||||
if((mmwData.header.platform & 0xFFFF) != 0x1443) {
|
||||
memcpy( &mmwData.header.subFrameNumber, ¤tBufp->at(currentDatap), sizeof(mmwData.header.subFrameNumber));
|
||||
currentDatap += ( sizeof(mmwData.header.subFrameNumber) );
|
||||
}
|
||||
@@ -386,44 +386,30 @@ void *DataUARTHandler::sortIncomingData( void )
|
||||
mmwData.numObjOut = mmwData.header.numDetectedObj;
|
||||
}
|
||||
|
||||
RScan->header.seq = 0;
|
||||
// RScan->header.seq = 0;
|
||||
// RScan->header.stamp = (uint64_t)(ros::Time::now());
|
||||
// RScan->header.stamp = (uint32_t) mmwData.header.timeCpuCycles;
|
||||
RScan->header.frame_id = "base_radar_link";
|
||||
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))
|
||||
{
|
||||
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;
|
||||
}
|
||||
} 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
|
||||
{
|
||||
|
||||
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, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.rangeIdx));
|
||||
currentDatap += ( sizeof(mmwData.objOut.rangeIdx) );
|
||||
@@ -448,33 +434,45 @@ void *DataUARTHandler::sortIncomingData( void )
|
||||
memcpy( &mmwData.objOut.z, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.z));
|
||||
currentDatap += ( sizeof(mmwData.objOut.z) );
|
||||
|
||||
//convert from Qformat to float(meters)
|
||||
float temp[4];
|
||||
float temp[7];
|
||||
|
||||
temp[0] = (float) mmwData.objOut.x;
|
||||
temp[1] = (float) mmwData.objOut.y;
|
||||
temp[2] = (float) mmwData.objOut.z;
|
||||
//temp[4] = //doppler
|
||||
temp[3] = (float) mmwData.objOut.dopplerIdx;
|
||||
|
||||
for(int j = 0; j < 3; j++)
|
||||
{
|
||||
if(temp[j] > 32767)
|
||||
temp[j] -= 65535;
|
||||
|
||||
temp[j] = temp[j] / pow(2,mmwData.xyzQFormat);
|
||||
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);
|
||||
}
|
||||
|
||||
// Convert intensity to dB
|
||||
temp[3] = 10 * log10(mmwData.objOut.peakVal + 1); // intensity
|
||||
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[3];
|
||||
}
|
||||
else // SDK version is 3.x+
|
||||
{
|
||||
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, ¤tBufp->at(currentDatap), sizeof(mmwData.newObjOut.x));
|
||||
currentDatap += ( sizeof(mmwData.newObjOut.x) );
|
||||
@@ -496,10 +494,35 @@ void *DataUARTHandler::sortIncomingData( void )
|
||||
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
|
||||
}
|
||||
i++;
|
||||
|
||||
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;
|
||||
@@ -645,12 +668,6 @@ void *DataUARTHandler::sortIncomingData( void )
|
||||
memcpy( &RScan->points[j], &RScan->points[i], sizeof(RScan->points[i]));
|
||||
j++;
|
||||
}
|
||||
|
||||
// Otherwise, remove the point
|
||||
else
|
||||
{
|
||||
//ROS_INFO("Removed point");
|
||||
}
|
||||
}
|
||||
mmwData.numObjOut = j; // update number of objects as some points may have been removed
|
||||
|
||||
@@ -840,3 +857,34 @@ 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);
|
||||
}
|
||||
155
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/ParameterParser.cpp
Executable file
155
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/ParameterParser.cpp
Executable file
@@ -0,0 +1,155 @@
|
||||
/*
|
||||
* @file ParameterParser.cpp
|
||||
*
|
||||
* @brief
|
||||
* Calculates parameters from QuickConfig.
|
||||
*
|
||||
* \par
|
||||
* NOTE:
|
||||
* (C) Copyright 2020 Texas Instruments, Inc.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
#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);
|
||||
}
|
||||
|
||||
}
|
||||
113
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp
Executable file
113
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp
Executable file
@@ -0,0 +1,113 @@
|
||||
/*
|
||||
* @file mmWaveCommSrv.cpp
|
||||
*
|
||||
* @brief
|
||||
* Communication service responsible for sending CLI commands to sensor.
|
||||
*
|
||||
* \par
|
||||
* NOTE:
|
||||
* (C) Copyright 2020 Texas Instruments, Inc.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
#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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
89
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveDataHdl.cpp
Executable file
89
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveDataHdl.cpp
Executable file
@@ -0,0 +1,89 @@
|
||||
/*
|
||||
* @file mmWaveDataHdl.cpp
|
||||
*
|
||||
* @brief
|
||||
* Creates the data handler node and sets parameters.
|
||||
*
|
||||
* \par
|
||||
* NOTE:
|
||||
* (C) Copyright 2020 Texas Instruments, Inc.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
#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");
|
||||
}
|
||||
|
||||
}
|
||||
60
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveLoader.cpp
Executable file
60
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveLoader.cpp
Executable file
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* @file mmWaveLoader.cpp
|
||||
*
|
||||
* @brief
|
||||
* Creates the mmWave Manager node.
|
||||
*
|
||||
* \par
|
||||
* NOTE:
|
||||
* (C) Copyright 2019 Texas Instruments, Inc.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
#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;
|
||||
}
|
||||
99
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp
Executable file
99
autonomous_robotics_ros/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp
Executable file
@@ -0,0 +1,99 @@
|
||||
/*
|
||||
* @file mmWaveQuickConfig.cpp
|
||||
*
|
||||
* @brief
|
||||
* Reads the cfg file and calls service to send commands.
|
||||
*
|
||||
* \par
|
||||
* NOTE:
|
||||
* (C) Copyright 2020 Texas Instruments, Inc.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
#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;
|
||||
}
|
||||
0
autonomous_robotics_ros/ti_mmwave_rospkg/srv/mmWaveCLI.srv → autonomous_robotics_ros/src/ti_mmwave_rospkg/srv/mmWaveCLI.srv
Normal file → Executable file
0
autonomous_robotics_ros/ti_mmwave_rospkg/srv/mmWaveCLI.srv → autonomous_robotics_ros/src/ti_mmwave_rospkg/srv/mmWaveCLI.srv
Normal file → Executable file
211
autonomous_robotics_ros/src/ti_safety_bubble/CMakeLists.txt
Normal file
211
autonomous_robotics_ros/src/ti_safety_bubble/CMakeLists.txt
Normal file
@@ -0,0 +1,211 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(ti_safety_bubble)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-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
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
message_generation
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
## 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 exec_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 exec_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
|
||||
Num.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
add_service_files(
|
||||
FILES
|
||||
AddTwoInts.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
|
||||
)
|
||||
|
||||
################################################
|
||||
## 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 exec_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 your 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 beginner_tutorials
|
||||
# CATKIN_DEPENDS roscpp rospy std_msgs
|
||||
# DEPENDS system_lib
|
||||
CATKIN_DEPENDS message_runtime
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/beginner_tutorials.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(${PROJECT_NAME} ${${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/beginner_tutorials_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
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
# install(TARGETS ${PROJECT_NAME}
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_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}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.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)
|
||||
|
||||
add_executable(odomListener src/odomListener.cpp)
|
||||
target_link_libraries(odomListener ${catkin_LIBRARIES})
|
||||
add_dependencies(odomListener ti_safety_bubble_generate_messages_cpp)
|
||||
@@ -0,0 +1,10 @@
|
||||
<launch>
|
||||
<node pkg="tf" type="static_transform_publisher" name="slow_tf" args="0 0 0 0 0 0 base_link slowzone 100"/>
|
||||
<node pkg="tf" type="static_transform_publisher" name="stop_tf" args="0 0 0 0 0 0 base_link stopzone 100"/>
|
||||
|
||||
<node name="odomListener" pkg="ti_safety_bubble" type="odomListener" output="screen" >
|
||||
<param name="slow_radius" value="2.0" />
|
||||
<param name="stop_radius" value="1.0" />
|
||||
<param name="clear_costmap_period_secs" value="5" />
|
||||
</node>
|
||||
</launch>
|
||||
1
autonomous_robotics_ros/src/ti_safety_bubble/msg/Num.msg
Normal file
1
autonomous_robotics_ros/src/ti_safety_bubble/msg/Num.msg
Normal file
@@ -0,0 +1 @@
|
||||
int64 num
|
||||
77
autonomous_robotics_ros/src/ti_safety_bubble/package.xml
Normal file
77
autonomous_robotics_ros/src/ti_safety_bubble/package.xml
Normal file
@@ -0,0 +1,77 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>ti_safety_bubble</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The beginner_tutorials package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="sabeeh@todo.todo">sabeeh</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>TODO</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">http://wiki.ros.org/beginner_tutorials</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,211 @@
|
||||
/*
|
||||
* @file odomListener.cpp
|
||||
*
|
||||
* @brief
|
||||
* Subscribes to the global obstacle layers's costmap and footprint nodes,
|
||||
* publishes velocities based on the .
|
||||
*
|
||||
* \par
|
||||
* NOTE:
|
||||
* (C) Copyright 2020 Texas Instruments, Inc.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include "ros/ros.h"
|
||||
#include "std_msgs/String.h"
|
||||
#include "std_srvs/Empty.h"
|
||||
#include "nav_msgs/OccupancyGrid.h"
|
||||
#include "geometry_msgs/PolygonStamped.h"
|
||||
#include "geometry_msgs/Point32.h"
|
||||
#include "geometry_msgs/Twist.h"
|
||||
|
||||
double robotX, robotY;
|
||||
nav_msgs::OccupancyGrid gMsg;
|
||||
|
||||
/**
|
||||
* This callback receives the robot's polygon and calculates the center.
|
||||
* The center point will be used to calculate distances to objects.
|
||||
*/
|
||||
void footprintCallback(const geometry_msgs::PolygonStamped::ConstPtr& msg)
|
||||
{
|
||||
//ROS_INFO("I heard: [%s]", msg->data.c_str());
|
||||
//ROS_INFO("[%3.3f], [%3.3f]", msg->pose.pose.position.x,msg->pose.pose.position.y);
|
||||
//geometry_msgs::Point32 p
|
||||
double x, y;
|
||||
int s = (int) msg->polygon.points.size();
|
||||
if (s == 0)
|
||||
return;
|
||||
x = 0.0;
|
||||
y = 0.0;
|
||||
for (int i = 0; i < s; i++)
|
||||
{
|
||||
x += msg->polygon.points[i].x;
|
||||
y += msg->polygon.points[i].y;
|
||||
}
|
||||
robotX = x / (double) s;
|
||||
robotY = y / (double) s;
|
||||
}
|
||||
|
||||
/**
|
||||
* This callback saves the global costmap to the gMsg variable.
|
||||
*/
|
||||
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
|
||||
{
|
||||
gMsg = *msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the distance between p1 and p2.
|
||||
*/
|
||||
double calculateDistance(double p1x, double p1y, double p2x, double p2y)
|
||||
{
|
||||
return sqrt(pow(p1x - p2x, 2) + pow(p1y - p2y, 2));
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
|
||||
ros::init(argc, argv, "odomListener");
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::NodeHandle n2("~");
|
||||
|
||||
ros::Subscriber footprintSub = n.subscribe("/move_base/global_costmap/footprint", 10, footprintCallback);
|
||||
ros::Subscriber mapSub = n.subscribe("/move_base/global_costmap/costmap", 10, mapCallback);
|
||||
|
||||
ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
|
||||
std_srvs::Empty srv;
|
||||
|
||||
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);
|
||||
ros::Publisher slowzone_pub = n.advertise<geometry_msgs::PolygonStamped>("/ti_base/slowzone", 1);
|
||||
ros::Publisher stopzone_pub = n.advertise<geometry_msgs::PolygonStamped>("/ti_base/stopzone", 1);
|
||||
|
||||
double outerLimit = 1.5; //radius in meters
|
||||
double innerLimit = 1.0; //radius in meters
|
||||
double mapsClearTime = 7.5; //seconds
|
||||
|
||||
n2.getParam("slow_radius", outerLimit);
|
||||
n2.getParam("stop_radius", innerLimit);
|
||||
n2.getParam("clear_costmap_period_secs", mapsClearTime);
|
||||
|
||||
|
||||
double rate = 20;
|
||||
ros::Rate loop_rate(rate);
|
||||
int counter = 0;
|
||||
bool stopFlag = false;
|
||||
bool slowFlag = false;
|
||||
while (ros::ok())
|
||||
{
|
||||
if (gMsg.info.width != 0) {
|
||||
double tempRobotX = robotX;
|
||||
double tempRobotY = robotY;
|
||||
stopFlag = false;
|
||||
slowFlag = false;
|
||||
|
||||
// create polygons for zones
|
||||
geometry_msgs::PolygonStamped slowzone;
|
||||
geometry_msgs::PolygonStamped stopzone;
|
||||
slowzone.header.frame_id = "slowzone";
|
||||
stopzone.header.frame_id = "stopzone";
|
||||
int numPoints = 12; // number of points for polygon
|
||||
for (int i = 0; i < numPoints; ++i) {
|
||||
double angle = i * ( 360.0 / (double) numPoints) * M_PI / 180.0;
|
||||
geometry_msgs::Point32 slow_point, stop_point;
|
||||
slow_point.z = 0.0;
|
||||
stop_point.z = 0.0;
|
||||
slow_point.x = cos(angle) * outerLimit;
|
||||
slow_point.y = sin(angle) * outerLimit;
|
||||
stop_point.x = cos(angle) * innerLimit;
|
||||
stop_point.y = sin(angle) * innerLimit;
|
||||
slowzone.polygon.points.push_back(slow_point);
|
||||
stopzone.polygon.points.push_back(stop_point);
|
||||
}
|
||||
slowzone_pub.publish(slowzone);
|
||||
stopzone_pub.publish(stopzone);
|
||||
|
||||
// calculate distance to objects
|
||||
for (int i = 0; i < gMsg.info.height; i++)
|
||||
{
|
||||
for (int j = 0; j < gMsg.info.width; j++)
|
||||
{
|
||||
int ind = (i * gMsg.info.width) + j;
|
||||
// grid spaces with probability of 80 or higher will have distance calculated
|
||||
if (gMsg.data[ind] >= 80)
|
||||
{
|
||||
double px = (i * gMsg.info.resolution) + gMsg.info.origin.position.x;
|
||||
double py = (j * gMsg.info.resolution) + gMsg.info.origin.position.y;
|
||||
double dist = calculateDistance(tempRobotX, tempRobotY, py, px);
|
||||
if (outerLimit > dist && dist > innerLimit)
|
||||
{
|
||||
slowFlag = true;
|
||||
} else if (innerLimit >= dist)
|
||||
{
|
||||
stopFlag = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// publish velocity command
|
||||
geometry_msgs::Twist vel;
|
||||
// hard-coded angular velocity to offset turtlebot2 from straying to the left
|
||||
vel.angular.z = -0.0075;
|
||||
if (stopFlag) {
|
||||
ROS_INFO("STOP");
|
||||
vel.linear.x = 0.0;
|
||||
vel.angular.z = 0.0;
|
||||
} else if (slowFlag) {
|
||||
ROS_INFO("SLOW");
|
||||
vel.linear.x = 0.1;
|
||||
} else {
|
||||
ROS_INFO("FULL");
|
||||
vel.linear.x = 0.2;
|
||||
}
|
||||
vel_pub.publish(vel);
|
||||
}
|
||||
|
||||
// calls move_base service to clear costmaps
|
||||
counter += 1;
|
||||
if (counter > (int) (rate * mapsClearTime)) {
|
||||
if (client.call(srv))
|
||||
{
|
||||
ROS_INFO("maps cleared");
|
||||
} else {
|
||||
ROS_INFO("can't call service");
|
||||
}
|
||||
counter = 0;
|
||||
}
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,4 @@
|
||||
int64 a
|
||||
int64 b
|
||||
---
|
||||
int64 sum
|
||||
0
autonomous_robotics_ros/turtlebot/turtlebot/README.md → autonomous_robotics_ros/src/turtlebot/turtlebot/README.md
Normal file → Executable file
0
autonomous_robotics_ros/turtlebot/turtlebot/README.md → autonomous_robotics_ros/src/turtlebot/turtlebot/README.md
Normal file → Executable file
0
autonomous_robotics_ros/turtlebot/turtlebot/setup_create.sh → autonomous_robotics_ros/src/turtlebot/turtlebot/setup_create.sh
Normal file → Executable file
0
autonomous_robotics_ros/turtlebot/turtlebot/setup_create.sh → autonomous_robotics_ros/src/turtlebot/turtlebot/setup_create.sh
Normal file → Executable file
0
autonomous_robotics_ros/turtlebot/turtlebot/setup_kobuki.sh → autonomous_robotics_ros/src/turtlebot/turtlebot/setup_kobuki.sh
Normal file → Executable file
0
autonomous_robotics_ros/turtlebot/turtlebot/setup_kobuki.sh → autonomous_robotics_ros/src/turtlebot/turtlebot/setup_kobuki.sh
Normal file → Executable file
|
Before Width: | Height: | Size: 43 KiB After Width: | Height: | Size: 43 KiB |
@@ -0,0 +1,82 @@
|
||||
<!--
|
||||
Collection of robot-centric definitions and nodes for the turtlebot.
|
||||
-->
|
||||
<launch>
|
||||
<arg name="base"/>
|
||||
<arg name="stacks"/>
|
||||
<arg name="3d_sensor"/>
|
||||
<arg name="mmwave_device"/>
|
||||
|
||||
<include file="$(find turtlebot_bringup)/launch/includes/description.launch.xml">
|
||||
<arg name="base" value="$(arg base)" />
|
||||
<arg name="stacks" value="$(arg stacks)" />
|
||||
<arg name="3d_sensor" value="$(arg 3d_sensor)" />
|
||||
</include>
|
||||
|
||||
<!-- important generally, but specifically utilised by the current app manager -->
|
||||
<param name="robot/name" value="$(optenv ROBOT turtlebot)"/>
|
||||
<param name="robot/type" value="turtlebot"/>
|
||||
|
||||
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
|
||||
<param name="publish_frequency" type="double" value="5.0" />
|
||||
</node>
|
||||
<node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
|
||||
<rosparam command="load" file="$(find turtlebot_bringup)/param/$(arg base)/diagnostics.yaml" />
|
||||
</node>
|
||||
|
||||
<!-- Call mmWave sensor launch file -->
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_north.launch">
|
||||
<arg name="device" value="$(arg mmwave_device)" doc="TI mmWave sensor device type [1443, 1642, 1843, 6843ISK, 6843AOP]"/>
|
||||
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d (not supported by 1642 EVM), 2d]"/>
|
||||
<arg name="max_allowed_elevation_angle_deg" value="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
<arg name="max_allowed_azimuth_angle_deg" value="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
</include>
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_south.launch">
|
||||
<arg name="device" value="$(arg mmwave_device)" doc="TI mmWave sensor device type [1443, 1642, 1843, 6843ISK, 6843AOP]"/>
|
||||
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d (not supported by 1642 EVM), 2d]"/>
|
||||
<arg name="max_allowed_elevation_angle_deg" value="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
<arg name="max_allowed_azimuth_angle_deg" value="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
</include>
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_west.launch">
|
||||
<arg name="device" value="$(arg mmwave_device)" doc="TI mmWave sensor device type [1443, 1642, 1843, 6843ISK, 6843AOP]"/>
|
||||
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d (not supported by 1642 EVM), 2d]"/>
|
||||
<arg name="max_allowed_elevation_angle_deg" value="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
<arg name="max_allowed_azimuth_angle_deg" value="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
</include>
|
||||
<include file="$(find ti_mmwave_rospkg)/launch/bubble_sensor_east.launch">
|
||||
<arg name="device" value="$(arg mmwave_device)" doc="TI mmWave sensor device type [1443, 1642, 1843, 6843ISK, 6843AOP]"/>
|
||||
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d (not supported by 1642 EVM), 2d]"/>
|
||||
<arg name="max_allowed_elevation_angle_deg" value="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
<arg name="max_allowed_azimuth_angle_deg" value="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
|
||||
</include>
|
||||
|
||||
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" />
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="tf_base_link_to_base_footprint" args="0 0 0 0 0 0 base_footprint base_link 100"/>
|
||||
|
||||
<!-- -->
|
||||
<!--
|
||||
<node pkg="tf" type="static_transform_publisher" name="static_transorm_publisher_map" args="0 0 0 0 0 0 map odom 100"/>
|
||||
-->
|
||||
<!--
|
||||
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="Radar_to_Laser">
|
||||
<param name="min_range" value="0"/>
|
||||
<param name="angle_min" value="-3.14"/>
|
||||
<param name="angle_max" value="3.14"/>
|
||||
</node>
|
||||
|
||||
<node pkg="nodelet" type="nodelet" name="passthru_manager" args="manager" />
|
||||
|
||||
Run a passthrough filter to clean NaNs
|
||||
<node pkg="nodelet" type="nodelet" name="i_filter" args="load pcl/PassThrough passthru_manager" output="screen">
|
||||
<remap from="~input" to="/mmWaveDataHdl/RScan" />
|
||||
<remap from="~output" to="/cloud_in" />
|
||||
<rosparam>
|
||||
filter_field_name: intensity
|
||||
filter_limit_min: 30
|
||||
filter_limit_max: 100
|
||||
filter_limit_negative: False
|
||||
</rosparam>
|
||||
</node>-->
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,26 @@
|
||||
<launch>
|
||||
<!-- Turtlebot -->
|
||||
<arg name="base" default="kobuki" doc="mobile base type [create, roomba]"/>
|
||||
<arg name="battery" default="$(env TURTLEBOT_BATTERY)" doc="kernel provided locatio for battery info, use /proc/acpi/battery/BAT0 in 2.6 or earlier kernels." />
|
||||
<arg name="stacks" default="$(env TURTLEBOT_STACKS)" doc="stack type displayed in visualisation/simulation [circles, hexagons]"/>
|
||||
<arg name="3d_sensor" default="radar" doc="3d sensor types [kinect, asux_xtion_pro, radar]"/>
|
||||
<arg name="simulation" default="$(env TURTLEBOT_SIMULATION)" doc="set flags to indicate this turtle is run in simulation mode."/>
|
||||
<arg name="serialport" default="$(env TURTLEBOT_SERIAL_PORT)" doc="used by create to configure the port it is connected on [/dev/ttyUSB0, /dev/ttyS0]"/>
|
||||
<arg name="mmwave_device" default="1443" doc="TI mmWave sensor device type [1443, 1642, 1843, 6843ISK, 6843AOP]"/>
|
||||
|
||||
<param name="/use_sim_time" value="$(arg simulation)"/>
|
||||
|
||||
<include file="$(find turtlebot_bringup)/launch/includes/robot_quad_sensor.launch.xml">
|
||||
<arg name="base" value="$(arg base)" />
|
||||
<arg name="stacks" value="$(arg stacks)" />
|
||||
<arg name="3d_sensor" value="$(arg 3d_sensor)" />
|
||||
<arg name="mmwave_device" value="$(arg mmwave_device)" />
|
||||
</include>
|
||||
<include file="$(find turtlebot_bringup)/launch/includes/mobile_base.launch.xml">
|
||||
<arg name="base" value="$(arg base)" />
|
||||
<arg name="serialport" value="$(arg serialport)" />
|
||||
</include>
|
||||
<include unless="$(eval arg('battery') == 'None')" file="$(find turtlebot_bringup)/launch/includes/netbook.launch.xml">
|
||||
<arg name="battery" value="$(arg battery)" />
|
||||
</include>
|
||||
</launch>
|
||||
@@ -0,0 +1,26 @@
|
||||
<launch>
|
||||
<!-- Turtlebot -->
|
||||
<arg name="base" default="kobuki" doc="mobile base type [create, roomba]"/>
|
||||
<arg name="battery" default="$(env TURTLEBOT_BATTERY)" doc="kernel provided locatio for battery info, use /proc/acpi/battery/BAT0 in 2.6 or earlier kernels." />
|
||||
<arg name="stacks" default="$(env TURTLEBOT_STACKS)" doc="stack type displayed in visualisation/simulation [circles, hexagons]"/>
|
||||
<arg name="3d_sensor" default="radar" doc="3d sensor types [kinect, asux_xtion_pro, radar]"/>
|
||||
<arg name="simulation" default="$(env TURTLEBOT_SIMULATION)" doc="set flags to indicate this turtle is run in simulation mode."/>
|
||||
<arg name="serialport" default="$(env TURTLEBOT_SERIAL_PORT)" doc="used by create to configure the port it is connected on [/dev/ttyUSB0, /dev/ttyS0]"/>
|
||||
<arg name="mmwave_device" default="1443" doc="TI mmWave sensor device type [1443, 1642, 1843, 6843ISK, 6843AOP]"/>
|
||||
|
||||
<param name="/use_sim_time" value="$(arg simulation)"/>
|
||||
|
||||
<include file="$(find turtlebot_bringup)/launch/includes/robot_single_sensor.launch.xml">
|
||||
<arg name="base" value="$(arg base)" />
|
||||
<arg name="stacks" value="$(arg stacks)" />
|
||||
<arg name="3d_sensor" value="$(arg 3d_sensor)" />
|
||||
<arg name="mmwave_device" value="$(arg mmwave_device)" />
|
||||
</include>
|
||||
<include file="$(find turtlebot_bringup)/launch/includes/mobile_base.launch.xml">
|
||||
<arg name="base" value="$(arg base)" />
|
||||
<arg name="serialport" value="$(arg serialport)" />
|
||||
</include>
|
||||
<include unless="$(eval arg('battery') == 'None')" file="$(find turtlebot_bringup)/launch/includes/netbook.launch.xml">
|
||||
<arg name="battery" value="$(arg battery)" />
|
||||
</include>
|
||||
</launch>
|
||||
@@ -10,12 +10,14 @@
|
||||
|
||||
<param name="/use_sim_time" value="$(arg simulation)"/>
|
||||
|
||||
<!--
|
||||
<include file="$(find turtlebot_bringup)/launch/includes/robot.launch.xml">
|
||||
<arg name="base" value="$(arg base)" />
|
||||
<arg name="stacks" value="$(arg stacks)" />
|
||||
<arg name="3d_sensor" value="$(arg 3d_sensor)" />
|
||||
<arg name="mmwave_device" value="$(arg mmwave_device)" />
|
||||
</include>
|
||||
-->
|
||||
<include file="$(find turtlebot_bringup)/launch/includes/mobile_base.launch.xml">
|
||||
<arg name="base" value="$(arg base)" />
|
||||
<arg name="serialport" value="$(arg serialport)" />
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user