From a08720ba2294bb3271b9f24489e8adc4d14528ef Mon Sep 17 00:00:00 2001 From: Shahrukh <82996646+smartSRA@users.noreply.github.com> Date: Wed, 19 Oct 2022 13:00:02 +0200 Subject: [PATCH] Incorporate sensor type DRVEGRD 152 Linkt to issue #11 --- CHANGELOG.md | 4 + CONTRIBUTION.md => CONTRIBUTING.md | 0 Readme.md | 9 +- docker-compose.yml | 16 +++ simulator/config_umrr9d/com_lib_config.json | 16 +++ simulator/config_umrr9d/hw_inventory.json | 12 ++ simulator/config_umrr9d/routing_table.json | 14 +++ simulator/simulation/src/simulator.cpp | 103 ++++++++++-------- simulator/targetlist_port_v4_0_0.bin | Bin 0 -> 11696 bytes smart_extract.sh | 2 +- umrr_ros2_driver/CMakeLists.txt | 7 +- umrr_ros2_driver/config/sensor_params.json | 82 +++++++++++++- .../smartmicro_radar_node.hpp | 18 +++ umrr_ros2_driver/param/radar.template.yaml | 13 +++ .../src/smartmicro_radar_node.cpp | 68 +++++++++++- .../test/radar_node_test.launch.py | 12 ++ 16 files changed, 320 insertions(+), 56 deletions(-) rename CONTRIBUTION.md => CONTRIBUTING.md (100%) create mode 100644 simulator/config_umrr9d/com_lib_config.json create mode 100644 simulator/config_umrr9d/hw_inventory.json create mode 100644 simulator/config_umrr9d/routing_table.json create mode 100644 simulator/targetlist_port_v4_0_0.bin diff --git a/CHANGELOG.md b/CHANGELOG.md index bf8f542..c88caee 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,3 +19,7 @@ This release also offers the possibilty of configuring the ip addresses of the s ## v3.0.0 - 2022-09-23 Major release includes the new smartmicro sensor DRVEGRD 169. The driver offers mode changes and configuration of the DRVEGRD 169 along with publishing the radar targets as pointcloud data. The callbacks for datastream now require a clientID. + +## v3.1.0 - 2022-10-19 + +This release includes the new smartmicro sensor DRVEGRD 152. The driver offers mode changes and configuration of the DRVEGRD 152 along with publishing the radar targets as pointcloud data. The callbacks for datastream now require a clientID. \ No newline at end of file diff --git a/CONTRIBUTION.md b/CONTRIBUTING.md similarity index 100% rename from CONTRIBUTION.md rename to CONTRIBUTING.md diff --git a/Readme.md b/Readme.md index 218cd02..e17d4ae 100644 --- a/Readme.md +++ b/Readme.md @@ -22,20 +22,23 @@ ros2 launch umrr_ros2_driver radar.launch.py - ROS2 foxy ### UMRR radars and Smart Access API version -A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96 radar, UMRR11 radar or UMRR9F radar are +A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 152 or DRVEGRD 169 radar are required to run this node. This code is bundled with a version of Smart Access API. Please make sure the version used to publish the data is compatible with this version: -- Date of release: `September 22, 2022` -- Smart Access Automotive version: `v2.0.0` +- Date of release: `October 19, 2022` +- Smart Access Automotive version: `v2.1.0` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.1` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.1` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` +- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.1` ### Sensor Firmwares This ROS2 driver release is compatible with the following sensor firmwares: - UMRR11 Type 132: V5.1.4 - UMRR96 Type 153: V5.2.4 +- UMRR9D Type 152: V1.0.0 +- UMRR9F Type 169: V1.3.0 ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a diff --git a/docker-compose.yml b/docker-compose.yml index 64e2208..f3c20a2 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -64,12 +64,28 @@ services: networks: device_network: ipv4_address: 172.22.10.103 + sensor_3: + depends_on: + build_simulator: + condition: service_completed_successfully + image: umrr-ros:latest + volumes: + - .:/code + environment: + - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9d/com_lib_config.json + - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 + entrypoint: ["/code/simulator/simulation/out/bin/simulator"] + command: ["11", "1", "0", "C"] + networks: + device_network: + ipv4_address: 172.22.10.104 ros_node: image: umrr-ros:latest depends_on: - "sensor_0" - "sensor_1" - "sensor_2" + - "sensor_3" volumes: - .:/code command: ["colcon", "test", "--packages-above", "umrr_ros2_driver"] diff --git a/simulator/config_umrr9d/com_lib_config.json b/simulator/config_umrr9d/com_lib_config.json new file mode 100644 index 0000000..069ef60 --- /dev/null +++ b/simulator/config_umrr9d/com_lib_config.json @@ -0,0 +1,16 @@ +{ + "name": "Com Lib Config", + "version": "1.0.0", + "client_id": 400, + "role": "slave", + "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", + "config_path": "/code/simulator/config_umrr9d", + "user_interface_name": "umrr9d_t152_automotive", + "user_interface_major_v": 1, + "user_interface_minor_v": 0, + "user_interface_patch_v": 1, + "download_path": "", + "instruction_serialization_type": "port_based", + "data_serialization_type": "port_based", + "alive": false +} diff --git a/simulator/config_umrr9d/hw_inventory.json b/simulator/config_umrr9d/hw_inventory.json new file mode 100644 index 0000000..94f370a --- /dev/null +++ b/simulator/config_umrr9d/hw_inventory.json @@ -0,0 +1,12 @@ +{ + "hwItems": [ + { + "type": "eth", + "dev_id": 1, + "iface_name": "eth0", + "port": 55555 + } + ], + "name": "HW inventory List", + "version": "1.1.0" +} diff --git a/simulator/config_umrr9d/routing_table.json b/simulator/config_umrr9d/routing_table.json new file mode 100644 index 0000000..1eaffd3 --- /dev/null +++ b/simulator/config_umrr9d/routing_table.json @@ -0,0 +1,14 @@ +{ + "clients": [ + { + "client_id": 1, + "data_serialization_type": "port_based", + "instruction_serialization_type": "port_based", + "ip": "172.22.10.100", + "link_type": "eth", + "port": 55555 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/simulator/simulation/src/simulator.cpp b/simulator/simulation/src/simulator.cpp index 386cdf9..39003fd 100644 --- a/simulator/simulation/src/simulator.cpp +++ b/simulator/simulation/src/simulator.cpp @@ -1,3 +1,17 @@ +// Copyright (c) 2021, s.m.s, smart microwave sensors GmbH, Brunswick, Germany. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include @@ -5,13 +19,15 @@ #include #include #include +#include +#include +#include + #include #include #include #include -#include -#include -#include +#include #include #include @@ -19,19 +35,21 @@ using namespace com::common; using namespace com::master; using namespace com::types; +namespace +{ uint64_t identifier; uint64_t majorVersion; uint64_t minorVersion; std::string port; std::string portFile; - +} // namespace std::shared_ptr dataServices = - com::common::DataServicesIface::Get(); + com::common::DataServicesIface::Get(); -void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { - InstructionBuffer *receive = - reinterpret_cast(buffer.GetBufferPtr()); +void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) +{ + InstructionBuffer * receive = reinterpret_cast(buffer.GetBufferPtr()); int sizeIncomingBuf = buffer.GetSize(); uint32_t instnumber = receive->GetNumOfInstructions(); auto instructions = receive->GetInstructions(); @@ -41,40 +59,33 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { std::cout << "Base user interface major version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(1); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 21) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 21) { std::cout << "Base user interface minor version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(0); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 22) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 22) { std::cout << "User interface identifier set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(identifier); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 23) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 23) { std::cout << "User interface major version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(majorVersion); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 24) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 24) { std::cout << "User interface minor version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(minorVersion); - } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 2) { - std::cout << "UMRR96 mode frequency_sweep set!" - << std::endl; + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 2) { + std::cout << "UMRR96 mode frequency_sweep set!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 4) { + std::cout << "UMRR11 mode angular_separation set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 4) { - std::cout << "UMRR11 mode angular_separation set!" - << std::endl; + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 5) { + std::cout << "UMRR9F mode range_toggle_mode set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 5) { - std::cout << "UMRR9F mode range_toggle_mode set!" - << std::endl; + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 1) { + std::cout << "UMRR9D mode center_frequency_idx set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); } else { std::cout << "Unknown instruction received from ROS driver!" << std::endl; @@ -84,17 +95,17 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { dataServices->SetInstructionBuffer(clientId, *receive, nullptr); } -void stream_port(std::string portFile) { - +void stream_port(std::string portFile) +{ dataServices->RegisterInstRecvCallback(slave_callback); ClientId masterId = 1; PortId portTargetListId = 66; std::ifstream ifs(portFile, std::ifstream::binary | std::ios::binary); - std::filebuf *pbuf = ifs.rdbuf(); + std::filebuf * pbuf = ifs.rdbuf(); int size = pbuf->pubseekoff(0, ifs.end, ifs.in); pbuf->pubseekpos(0, ifs.in); - char *filebuffer = new (std::nothrow) char[size]; + char * filebuffer = new (std::nothrow) char[size]; if (filebuffer == nullptr) { std::cout << "error assigning memory!" << std::endl; @@ -104,18 +115,17 @@ void stream_port(std::string portFile) { BufferDescriptor bufferdesc((uint8_t *)filebuffer, size); std::this_thread::sleep_for(std::chrono::seconds(1)); - if (ERROR_CODE_OK != - dataServices->StreamDataPort(masterId, portTargetListId, bufferdesc)) { + if (ERROR_CODE_OK != dataServices->StreamDataPort(masterId, portTargetListId, bufferdesc)) { return; } std::cout << "sensor is transmitting data! " << std::endl; ifs.close(); delete[] filebuffer; - } - -int main(int argc, char *argv[]) { + +int main(int argc, char * argv[]) +{ if (argc != 5) { std::cout << "Specifiy User Interface for the sensor" << std::endl; return 1; @@ -125,28 +135,29 @@ int main(int argc, char *argv[]) { majorVersion = strtoll(argv[2], nullptr, 10); minorVersion = strtoll(argv[3], nullptr, 10); port = argv[4]; - + if (!dataServices->Init()) { throw std::runtime_error("Data services have not been initialized!"); } - + auto Start = std::chrono::steady_clock::now(); while (1) { - - if(port == "A") { + if (port == "A") { std::string portFile = "/code/simulator/targetlist_port_v2_1_0.bin"; stream_port(portFile); - } - else if(port == "B") { + } else if (port == "B") { std::string portFile = "/code/simulator/targetlist_port_v3_0_0.bin"; stream_port(portFile); - } - else { + } else if (port == "C") { + std::string portFile = "/code/simulator/targetlist_port_v4_0_0.bin"; + stream_port(portFile); + } else { std::cout << "Invalid input!" << std::endl; } - if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(30)) + if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(30)) { break; + } } return 0; } diff --git a/simulator/targetlist_port_v4_0_0.bin b/simulator/targetlist_port_v4_0_0.bin new file mode 100644 index 0000000000000000000000000000000000000000..c23d2f29f5ddc0666767134a95fd3a0d32e7642d GIT binary patch literal 11696 zcmYkCcRZGV)Wd3QZN- z<^G=MdS0*RkLUjP{^P#x>vhigd_M1UIF9q+B)R|hv8Wa2#JM4juURvfFuwf%zUepD zxG8B_(R$QtJe19tMRkL+=nYp3mtEqS_Qk=Je0eFfh^>X*RYs_`Ly1Z!t$rfLX;ACy zjlgzsTm;b#iDxep#-U8MNOWbKvB+}n`I^#bo<6o- zfbc?&+t%C1-+$nPo`1$dP0BXdf6^KXI(r~THxxcjnMO$q-thV^k45X>Y9GqpHIvu8 zpHDcM!51rGYlvjtzXeQ!HC`xdNOd*JZDEmxr<5 z_LvyUan0Atz&_) zD9|KJjyv#O8Ph@rlWBJZ*1rpa7?r(6M#;TkckGyGaF9O^Zt+00X=ZZ@4_Ug{?tl{p zZw13jMduf%hM$D; zsA_iOF2~sndI+)Co))E4aA+{ug&@}of0fN=TU*D|blEDFRS}9-sUukBvyin{Bs8$( zaVwMT;kf53!uhVp%b3(US(H1kNnb`M;*r1+i1|hA%l5HkuF(YF+VfcI-^$)IF5f|n ztm(sJj*GGyh88|=;Gu#EB-f~tq&kC1TO!2C$ac1j= zjb?WibKIw2PvE?HIqcG%PW#*_OIMXgPVXe*BxXnI-9;%9W-SY zN}+f*gyXUrTyU*ZBWzy&o@uqoFx53N*ry_nF=Clm8atHLI8{L3uQ_@!8S{Vx?ko{@}vbgckFP!D_xv|?R8jBVJ=GcYcZ$rxWct~6UJQA zfn&WKCnY}?>yQ3rwvxAt!v1*E=<`{W8QuitYz!L{yO4dqz~l9^8l+Hp_2uN8IndU> z4{nuloO;toUi0B9fM7c;=H{ZNlOf(H6csjhdefBKjj%r3AJxn^z`BOND?Z4FPk9z|b?+OPP?=B4 zLS0|}+`v!YGlVXMmx0E(>r6K#A4h*Fg7F7c$aKbF)U}Qz@%)1@uI)zUkZlIEvPz$p zRB~KVW*nA`Rit~J`ZUd}3{(~WvEb}Nd|*1CB0Z*HUQGpSYPUyI^PfEj`V?r@oS~HY zo8!h@9)}i(f(CBfQ^AcjnNU3SvS`Y}8vd3e;_qJ^Iz3*)Urk zb(!OQXQ}gwlIgVUyb0{9ehAyUyI5Y83OXH-r-6 zCoDMbZLS91lG-kkaCfE^WxQxG{0E6k2a$N%88+Fyh3%LXLx&}f!V~-N6NhE{V4H_7 z`h{~`LtZg-o2>x1b@Q+`@c<9=2CvBw4+bzPD`ZLAKlA z8+^RLaXmX~Vay^U`Yl_=#B%f~>sJ$d?ly>V0mUMv#n<^kcgN#1n?On)S6VdI;y3HZ zXCftEj+?Xf61-?Mz+K&P?E9x8T0Ykf)87xq+c6&K8XHADAs=9oV+Xq>eNK9cYcy^; ztVUYTI4)K#6RLliFhj@fAU9i)mgM)trq?&w;oPGvv`PsMIM`AB?K^P%vvERD$P@S- z>5X~$f^K^Hq5g~<>^d1qrM7YyebofdO6C<^l1;`D!+hzLRU(UIGl!yXWIbbw9!@5sw0>8RSHYxgESYF>we)AJw zx|PLo;h%Q$N+HYtg~h7i?^;s~XL3}x-j$paXQ1I#DUx3@mEXq?;Y;3&YlcQjldNqJ zyQj@@-8b8Lb=6&>`YxM+f5l?#iH<8-Tvnoho?4OOi{}vkRUbW0rNW9jDo&V0khHTG zDyuoJ_cXwdvt!UH^)Zb4T8N7KFR^do>e%0-hqoFzYCYcr6#>DNSyH9hTU`R4FUL{V zCytX}Esdorim-c>B~EF7BucSQr!rk0RTp~F=BveUGEo|{A(A?C9~;cC@~4Rr`&ox7 z$7y{%%+I{|jOl2Gg7?-Lw5ik-Js10te$Pg};l~uBgw2qm`<}VhOI$f*r9`uuZ^HR^ z95*C>9n?1LW^SQk-czJIWId>1DgJQ#|aF7mtx#W)Lmxa>gaY-=2q+ESZ-xHC^`27&~ zm*Ym*o50!(F*23*W3>|#ak3e(ptxQ^ztovc(g)xq<}$f&cZx1qpBJmImMY2-IpVgL zLLZ%U0{@*|kX0K^$~F%~VbAWekNdZ?0m2Rfia!eAPE^Ea>Nu>uxPd&A5>iX$v9FD>tX5yTHZ|s$25UHC6;l0=}a?@O}-9uvl&i~QE=A|5$ zxuqToG!?PWJdjP!nE==KzJNB<8kl7-h5a$wxadd-s`ZA#!RGp*CMWWXmTkMk46h0K z;mBXeEZxHnsd}^LR>7DTW`OIyEaGKSKZ0nvCAPaq)0Y_+Ma~=YizjW+B|`&$(7woV z`|@7GQsKF?)lzX)w*yV(6v*!JP)Lxt4OjYd@XAyj!Jo%d*RjPDb(~yTN2dm53c2Ig z0(;c)lq2;Eo$Qs<=>fAK8S=3avD!oHKu6XB7a#q`I|hCR4I15STdWojEM(~g0k0U{Mm%iIXV$@+8Og9araN+I(w&_{0XklCVg38{xSR7|cw%LL{ zY;S|z7yhs_Y2&eauPyHDa-m|7Mz7^JSy_hx9(eN~NRB)V3;7XkKi0=n;mVURD_EG< zD$1z0sB7TwG=B={3Zk-46In*Hj>v!GU|cKb2Y*^sXx}I^3Vb+4_EwJqDa<#b1q~ec zH_ZxWPnm)$hRX*!8^3_1%`VZ6ae8F4PLi^B=VR*JL8!M=4rd1|<|+8b(L?7T+^WuT z_wTzw_ZLag_#didvayUUXpdxzce>#m?__4s(a)MQl<`o;c^261{r8ENE^KUafhys> zXzESF*;Ui|<-fFO^qa#hqHH?qlz76Uas`^Va|G3jk7atEi`nf&DbKr3`E=#?U>YRF zanq8Ea9S1MlqrB$58hz=A&Kc)ePc^Tw6eR}^YEqIufoitwIZDZ1E(YkPO>Mq6Ug^A z$JKiug-Q1o72dlN0rG_bl>XEYFLckxsK8!U@zV(tzIM3`a-(i7q(kC6@Xv=`Hswb__ z;W!ze)3Er0B3thFiJg>R&3cjzNp*mveI&W$e z^7P7?hrsS}G*-XwznYH&L#HO<;G6v}-cLrs--&Zz z@jM|vxaHtN_4iC|%nOM1Dlb~`*Pg-_Dr3(LPg<}yoenQF#*6hMaLugh^O-q`l!0-) z+h~r<2r|V3|6K9)E|x}fs=9zoAq&;n)AX+=sP_H zPpsm&MUNxU{LW_f;p-ZvWONSRbo(;3Wa3vk4W>!Y&FQ6AE;VJoW`_f(%H#JuO3?Dh z#o`=y@ip=U%A^$NE}F7QjGl!Sp}&SKX-ud9{qqLo8X3e~me>+klbf{r;5OcQnh!31 z#Bqi*I`}jv6Po&X7dz?l5t{ZsVR=gxaM7i;EZ|!NXzjiMVYZRfEEbm`rD1~CmyV`j zL2svCb;b5)-o)1|V>detY0N5T+BMs@~KYO{may&lV*$5p% zzh{c;V#W(cc(7qA1lIf0=;~NpKSY<}cYbDC2TbVN)d{eCP88mHP%g?9Uk&l%;XZ=(jID@>C<4X^yZc`a54z=Z8;SeR1U}S4CCIMLkCwv^>Fa zj)ET_>>iKD-$qcsoe}xYjmFTTwX8S&F|Q*wR&?z}BUn0&!m->}N~sPayA#0}^n5qC99<*&{3HOqO}4_xz`1C0a}b^W@(C0JcWpjVl|%<8)PS2Z z$ED=$23Ivbd~x(Rly~}}qIwW}Cc6hrT$|XsEfILm?l{EVNF(3(ng&j$MgtcHuPWLg z)b)-#E;uQ%3TmJ6bYY@2j{WnUeX5YbmNF|Eae({h##v)cK|Z`}_h}L3*H8V&ewmho%&8H$GT;(BCKZSGy?P*ULk^tBZ*2XmD%jcNwWc;>D0Q~_Va`5| zb9piuh98ZfvE2s;?n(TG)^$DbJ6WGSS4+n$i^cHsQ6IcfRYbP8o*Z8?@OEHJ^<=y* z$#F+-rQsR7cuFZY#x?J~&~os2_F%FYT2Bx|r?zQyeOxhE3e07Xsc~FTlLPrChLg?; zj#G{H0@VZ)4C>m%OgjBBZjLNX{H#pz2Uak>#m%g|OdlHEjY;YLs3o)AqRIcN9J(DA zxQ+8s*!u1gTVWPX+23^WOi2}-jQ+|3*GSN>Rui<0Ucw%3amFtNeF+hlx|r6rHGK7C zq0XicMb)(Da7>sdqdR0kqwW#ZORj)vQM=g0KkJ#*adWyk%$QUzY;PNw-#pM7d#Gsg zbBGxW=D`y=GxScWc$B*3*(r^vr z1nySzpAx?Fn~c+C0^#g`32+R~WN%EK(a2Y(t za9r@63>-Bnh*qXnfd2zs95>YJEs&BD&4h9R&gB?&&Q z7W_n36fRlH!zW)Sny_*rYHE$(+uEbaqC0|Cwm9S6+hSB+?vC|$6fZ;{i^gLOcK9!b z<2EZOV(Fh1qNtOlP?w)1a^{ACgV$`#*}Vn&Oy4kh$u*#8l1&j=!>;mA1ur+bi666z z&h)Dau;uPbZHr|M=Eco0bmmV`~Kc zd;RDgSj{N~{rJZ$W`QE9m`h;P@JL+KKOG~p6`6;L5BhPrD8rXa-ssGxqZ;4&0wMQD z`6S_y6;-V7oIT1&kEM$?*ZFU9`Y5NphMkYhWgRcXQMs}UVvP284wqLWodPpj`jF$i zc0C5s7ZGdi&Y}o46OtR=$!eeLP{Z+0Y`^?{h+De{UQP(X1P90NO^aq?bxk6zTPSd_ z&O<&2tbr$L}z(tN9Lr6`w@a(n3FY z3%%3$h)*kO7HLK};)))5Y(2gaY_Ft~`}8t?XPz?d_P+?<%Mbj!VUtJ^o`yJTFvlft z55w*PDcTnJZNV&U75wwp30vjeP}bry9IUj&5#QG`(H33Nu?2QLDqlYXD|-h4WrDA3 zc?(W&b`{Nw$fLlZ?fjVyyMRy4rI7Cbz-WpQrA4QZP`_Z`tW++#nie=9W;kW4qUi3njG;aFi*7t`+`J;7f%l0eLY14T6HCy04 zI&JV;EdrBo1f#M{0X^+`1@Si4=u;QVLbtxoK z)o{r{9Z^8H53U`qK}j-hbn#9rd>(##U{c>C>EwE5-(ZcdL+7 zUF&&E6ECW5o5HJn-ocjoM^RT%2373w#W@3)A#RKb$wwKG_p0F9xCReAk}pG(1#VNK z=u`CRZ7DOV$s=&S2$;WSpsO*R=E)h+{&|TM9dsTv!;i4>U1PcyTF+v(Wiwz_fWWWQ zukyEsEoQNb`Ejr(t^&9D z@C;Ua<)F;-GXqxZN7CoUS+Cse(^$)l017tdxV-ib z=-9A>X^)l}IQn$~{`lq1S7a<;K2tp?f6NkAe=rDBo~e?Yhw+Oc#Z%FPGsEP$9JhG> z&LaDCAM`ifz|`K0(~~xN`cRzBsC7K-OmW8LLvHc4=e%jDg6Yoluo+~V>qLe-IqpX5 zT+F_lf~^CaVdq^BbTm+)HQ6~BD;h_(V$a$Aqi$63O^rH7g`W5I^2MgR^J%pm$4#(C zJWz6ht(Gz+akXrcw?5DRH>?J3#LvXUP?2cRsBAvj^)GukTJL*jVLN>A>Vq1Ae@@a; z#`8he7_FI#xg&0Yk?v^p%6?1jif@Pqp! z9WnI9Nj@>$9#zdP=wIk)>h89rcCH9TwHDM^n$--;s;Dr@r2bW&KjG{OmUQ_u-_Bv?6YeUg!a#- zoxiQ|*>qW&Z+4()N7W~Y8-10%Uu6bPgT1j3)L353M+r|00~X_7&TiOnT-V08(09Qe zKmT+RHEfuI8o5_R#%g8k+ldsM<};YwY;@pCaW=)j+bY9OBtrJlmuz_R|LRdZwn@Kb zbitVg%Bn)xl{V(OwF?5YF0wf856n@UL$lLESjhLy=~6D+K;qUg++8H(&1JiaDrU^1 z+yAQBrOfZ5KRRP6OyGrDCK-6F!jvYp?_ifK&9VMJ|Fyi)8a9`bLHV-4Cydwf_l~Xt zQQJ6(lUzWf(}U69Vt~Q1Txz!e%arPzC_f~D(hOg`ku;h@YcFQuY#~p7c;}DmWk1+b zNp0#hSwLV3TO8hpwuS`>eE>N^xe6;q7T1g$mSo%DUTVCzZNQx z-ih&;9&!)N?d!owsgz|GSmXWowXjJx7bo=F(CN_ORkkbLA+p$*rKxe8!J$zYxcnwO zdprmwAE)6fYh#wRdo;co9!(lOD(r5*6W;rVm~rOe0~aG*#1ocKcV5WT1Gc#TO(B+^ zFvauLiZu4*SnRuP0{o|6?AqQWCVN(aDm#-aB^@wZhHDdk>fTgS-KeG9>l7M-&4%gG}bQ7rWe@5Mbl0^{2^z~imH z_|V7`OFd`P`dL!6OU)F0R%qet+fU%g5@a!lF38*cJRwTZs)XJ0949?-Dvn%ii}?;e z`O|te?0KmJ6_n~Q`Dd9JYNt)nvAwWb)|~yUE1!NkvKk7+rRlH0g%cVT;Nh`oeo^#x zHe*s~QGta6-PqO4yBH~wMCA+UdDH}XH5b7ve&fH+)M>belgGk2f?rzefi926!GXTl zpeVMqsIXI=BHo+gCGk5NAYf@PMQ?ake5%;N_h!K}^XF9p;<_ zmyFMFBzg_3%0g5c9*p$;H{{xU6s^=WK#8&^%|klo!FgY8TKAgc>P~%u&hIkRmyv<< zFU_P~FcY~ehHUv6HJBdjK-;V?^Dg6VG5?jxjhD@qi{_QCVq;%$95*Thr9U>n=4a-R zd)y6d#{Gu5i{?@FF$0niZWk1nx}a}pCawH)P;Rr3t92I0h+cmZes9!DsMFU$X5)|f zIu^L=p)8#iA4(Z%VPLDN#D49N$MD`RHe0_b;>dV)`lUaMuGVtgRS$P;sQAKrW%{%0 zQRQshp;@@gXBruOcVhGXzQek9Px@>y2NR1O(^8jTXLFK=(raN}?^H9yOFBx_blVa~ z<^3zN+8PUAdg3uh;LDFXW})b%GUZ4_V6o+qq$MrtxLu(aB!&G^>*gYqON_+($!GW- zuWc}CggneY8pUoqegav=a2$4X$H42GVN`K($K5q%sWfi37ys!a$1PO?Tr|oYqmM-~ zB}FGt4(SnC*=}}WTPA8AF6D<>30(ima5g^cUqOEl2R0W*;$?w{RCxwr?cgy~-4Mw> z=s(~$b&UOIIJ}!pJ#38E%qHWsyAw%ozB_(C(m6M2K-kY0=2GBsj*D+s#LG?b)G*YP z{t5i7LGWMuetdvq#a856J{7AM3UgrmTs*SS(Kv0dIBQ*Smem$>oSZ@<^oUaE{HFi- z)w7<0oAxjo_UHpE&;2Y~^`aai+gG!Z*XF<#+gEb>qCA}FvypXdpeeNyx_7ids5pb*QMw|Dq_51wQUmpWjB?h*2xd3a6ms4Q^Q_VY7Ec+)5+CPM zhjcpCIZD%1)gqid*#-@?a`7>VVc2XVSY6Ro;iug$lAGAhWZZ*Kft+b zg`#`rR_O7jl)ryapZVR2;4L=F9Hl}+u1a2OB|l^ zwjkwP5Y>*G^UumKoVKf|lJ#aC3Kcx?klnvh-;~)=!Fs9OhgjL)+j za%NbYuoo<9jc8@XVYc%4H@2d*4sK|>{kgXHEX@44k-ZS=WlWJFy849D$C-K96}VSq z(p=2WSLjh>$vV~^9!`B{H0g9+3J$lrdpk3#9-3Y#;6+OzcPtV?q*oh@@#4w!QKcwK z(3#z3jW8s^0Awdikkjmm@aKjB&F!C49_I3#86Ez~%v3mTt4%}E$_ue*_V)o~&h|rb zi+=+jaVjm{=t8N6LrD9_0?~_?>bUB^;f*?7vGgOTp8XQ`l(yT4;kCHoV05XE##8bI}$CVtWj30bDTP&BE@4)dOJ+{^lLXuNM18yPH*hHX(4 zSX#ks-+p2nk`ma+A!E_WW%fYNnU72%KJV;h8GrJ8Udme!;ke4Jq44#ZJMDYvfbYx< zF#6SLkVtnL@VxCzQ=5G-&DkA2M@f())Sdn=AHXj=GzO*mIc`8_7Mc~uL5JfdSn#MH z3dWa-92{%4|Dxqa zGlO3#tVezkSS}92HHuOg{xKXKcg3LgQi~g|!7lh^PZ28i33?Qq%*Vt`!}rD(WHOL1 z%tRx4@pUM>H|Y`kxIrElyU9YJ=4v>-$IYf^)*IL%b^^uDmo zS*i!FQR;?X1rPMi_*@ibtVgD$uh`)$CZE$Soye;CGLs1AxS%CzSok!GxZ)&!q#DwQ zGFOZ)zQXDad8)c=UKF>mLDUpv#$Va9)%DDVK{PSYo@EKXpb`G@<*ZP}6N7&Ue78*+EpEPNVg8|%n{2^U zyuzu-O_8Qe(4wW4s!Vy53SNn{f+t}c&Ai{-Va~r|X!shATR(jwp1c>!zJ;og_?u!@ z_bde+L)^*w8B)u3EzD7zfEEpAn18j+wEku-JC{C(G6hb1tzt8j{7}U;&AxPchXT&| z+{zE@KnmLMnkA`QqICNu)+cDO&+75JB@^Uv-KuG1D&n{n|7@()TFlm24nnQ3FTs5A zc2UE>r{MXTu~~mLDP?sQpd?EB=0Kct z202I{WNM+WSjqcUkoU!0RCnYX%4DleT0~nH&!*+a>}ckI zJ{%v^3H_g=amd02_~7DC)uq->cXD6a3&smn_0d&S=bB{TsrJ2jjV4 z1?0ZuFB@BijE)%KA>jriwCNmVKfZbC2us7|xtAd0zOctDaKJ2!QnuT8J*-*gj0tPs zLF?)Z%>0cH-Cv!LiHiE{fX^0)+OjL;G5PVG_~;rNRAh7cDGv&lsw-t^!1;?!d-DPzAzEywdFh<*Kro2%_^Ajj^m(s z+#de?ej{=?@*NtaZ-@@YBx9;*FewZbT{ilajnRc;aHOEOIkk@qx7b)T@_@TZ8MHQg5Y3eyf_6R}H-Ef7s6v@PW5aW#Qb`9Hu4kI`u&j&{<@MpX+8)mc$>B qyLw;b)#y)e*D13-4c@5vLW=BH*|5up;+L #include #include +#include #include #include @@ -122,6 +123,22 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & targetlist_port_umrr9f, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port The target list port + /// @param[in] client_id The client_id of the sensor + /// + + void targetlist_callback_umrr9d( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9d, + const com::types::ClientId client_id); /// /// @brief Read parameters and update the json config files required by @@ -181,6 +198,7 @@ std::shared_ptr m_services{}; std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f{}; +std::shared_ptr data_umrr9d{}; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index 6182304..55a2fa3 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -52,3 +52,16 @@ frame_id: "umrr" # Specify the history size. history_size: 10 + sensor_3: + # The model of the connected sensor. + model: "umrr9d" + # The client_id of the sensor/source, must be a unique integer. + id: 400 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.104" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 40ed817..b92b490 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include @@ -138,6 +140,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option data_umrr11 = com::master::umrr11_t132_automotive_v1_1_1::DataStreamServiceIface::Get(); data_umrr96 = com::master::umrr96_t153_automotive_v1_2_1::DataStreamServiceIface::Get(); data_umrr9f = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); + data_umrr9d = com::master::umrr9d_t152_automotive_v1_0_1::DataStreamServiceIface::Get(); RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); // Wait init time @@ -172,6 +175,15 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } + if ( + sensor.model == "umrr9d" && + com::types::ERROR_CODE_OK != + data_umrr9d->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout <<"Failed to register targetlist callback for sensor umrr9d"<< std::endl; + } m_publishers[i] = create_publisher( "umrr/targets_" + std::to_string(i), sensor.history_size); @@ -468,15 +480,65 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f( const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); modifier.push_back( - {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetRCS(), - target->GetTgtNoise(), target->GetPower()}); + { + range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), + target->GetRCS(), + target->GetTgtNoise(), + target->GetPower() + } + ); } m_publishers[sensor_idx]->publish(msg); } } +void SmartmicroRadarNode::targetlist_callback_umrr9d( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9d, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9d" << std::endl; + if(!check_signal) { + std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9d->GetGenericPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; + const auto sec = std::chrono::duration_cast(timestamp); + const auto nanosec = std::chrono::duration_cast(timestamp - sec); + msg.header.stamp.sec = sec.count(); + msg.header.stamp.nanosec = nanosec.count(); + for (const auto & target : targetlist_port_umrr9d->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + modifier.push_back( + { + range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), + target->GetRCS(), + target->GetTgtNoise(), + target->GetPower() + } + ); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + + void SmartmicroRadarNode::update_config_files_from_params() { const auto dev_id = declare_parameter(kDevIdTag, 0); diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 4bbc74d..b863fd7 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -77,12 +77,24 @@ def generate_test_description(): shell = True ) + center_frequency_idx_service = ExecuteProcess( + cmd = [[ + 'ros2 service call ', + '/smartmicro_radar_node/set_radar_mode ', + 'umrr_ros2_msgs/srv/SetMode ', + '"{param: "center_frequency_idx", value: 1, sensor_id: 400}"' + ]], + output='screen', + shell = True + ) + return ( launch.LaunchDescription([ radar_node, frequency_sweep_service, angular_separation_service, range_toggle_mode_service, + center_frequency_idx_service, launch_testing.actions.ReadyToTest(), ]), {