From 12c8f3bf4e890d2fe3ee75b31555e99b4bf55311 Mon Sep 17 00:00:00 2001 From: "m.eschenbach" Date: Thu, 14 May 2020 14:46:38 +0300 Subject: [PATCH 1/7] Add converter to RadarDetectionArray. --- launch/ars_40X.launch | 3 +++ scripts/convert_to_radardetectionlist.py | 25 ++++++++++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100755 scripts/convert_to_radardetectionlist.py diff --git a/launch/ars_40X.launch b/launch/ars_40X.launch index 013595f..e870f32 100644 --- a/launch/ars_40X.launch +++ b/launch/ars_40X.launch @@ -1,10 +1,13 @@ + + + diff --git a/scripts/convert_to_radardetectionlist.py b/scripts/convert_to_radardetectionlist.py new file mode 100755 index 0000000..2f75c76 --- /dev/null +++ b/scripts/convert_to_radardetectionlist.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python + +import rospy +from ars_40X.msg import Object, ObjectList, Cluster, ClusterList +from radar_msgs.msg import RadarDetection, RadarDetectionArray + +def object_list_callback(object_list, detection_array_publisher): + radar_detection_array = RadarDetectionArray() + radar_detection_array.header = object_list.header + for obj in object_list.objects: + radar_detection = RadarDetection() + radar_detection.detection_id = obj.id + radar_detection.position = obj.position.pose.position + radar_detection.velocity = obj.relative_velocity.twist.linear + radar_detection.amplitude = obj.rcs + radar_detection_array.detections.append(radar_detection) + detection_array_publisher.publish(radar_detection_array) + + +if __name__ == '__main__': + rospy.init_node('object_list_converter') + publisher = rospy.Publisher("/radar_converter/detections", RadarDetectionArray, queue_size=10) + callback_closure = lambda object_list: object_list_callback(object_list, publisher) + rospy.Subscriber('/ars_40X/objects', ObjectList, callback_closure) + rospy.spin() \ No newline at end of file From 4fb432475067ef248447a1228d4cc24c69bad4ca Mon Sep 17 00:00:00 2001 From: "m.eschenbach" Date: Mon, 6 Jul 2020 12:36:28 +0300 Subject: [PATCH 2/7] Add possibility to use a radar with different sensorID. --- include/ars_40X/ars_40X_can.hpp | 8 ++- launch/ars_40X.launch | 5 +- src/ars_40X_can.cpp | 90 +++++++++++++++++---------------- src/ros/ars_40X_ros.cpp | 3 ++ src/ros/radar_cfg_ros.cpp | 1 + 5 files changed, 62 insertions(+), 45 deletions(-) diff --git a/include/ars_40X/ars_40X_can.hpp b/include/ars_40X/ars_40X_can.hpp index 5b2e30a..175f637 100644 --- a/include/ars_40X/ars_40X_can.hpp +++ b/include/ars_40X/ars_40X_can.hpp @@ -46,7 +46,9 @@ class ARS_40X_CAN { public: ARS_40X_CAN(); - ARS_40X_CAN(std::string port); + ARS_40X_CAN(uint8_t sensor_id); + + ARS_40X_CAN(std::string port, uint8_t sensor_id); ~ARS_40X_CAN(); @@ -92,6 +94,8 @@ class ARS_40X_CAN { virtual void send_radar_state() {}; + void update_sensor_id(uint8_t sensor_id); + private: socket_can::SocketCAN can_; @@ -116,6 +120,8 @@ class ARS_40X_CAN { radar_state::RadarState radar_state_; radar_cfg::RadarCfg radar_cfg_; + + uint8_t sensor_id_offset_; }; } diff --git a/launch/ars_40X.launch b/launch/ars_40X.launch index e870f32..7766836 100644 --- a/launch/ars_40X.launch +++ b/launch/ars_40X.launch @@ -2,9 +2,12 @@ + + - + + diff --git a/src/ars_40X_can.cpp b/src/ars_40X_can.cpp index 70b9424..24672fe 100644 --- a/src/ars_40X_can.cpp +++ b/src/ars_40X_can.cpp @@ -6,11 +6,14 @@ namespace ars_40X { ARS_40X_CAN::ARS_40X_CAN() : - can_("can0") { + can_("can0"), + sensor_id_offset_(0) { } -ARS_40X_CAN::ARS_40X_CAN(std::string port) : - can_(port.c_str()) { +ARS_40X_CAN::ARS_40X_CAN(std::string port, uint8_t sensor_id) : + can_(port.c_str()), + sensor_id_offset_(0x010 * sensor_id) { + if (sensor_id > 7) throw std::invalid_argument("Invalid sensor id, must be in [0, 7]."); } ARS_40X_CAN::~ARS_40X_CAN() { @@ -24,60 +27,56 @@ bool ARS_40X_CAN::receive_radar_data() { if (!read_status) { return false; } - switch (frame_id) { - case RadarState:memcpy(radar_state_.get_radar_state()->raw_data, data, dlc); - send_radar_state(); - break; - - case Cluster_0_Status:memcpy(cluster_0_status_.get_cluster_0_status()->raw_data, data, dlc); - send_cluster_0_status(); - break; - - case Cluster_1_General:memcpy(cluster_1_general_.get_cluster_1_general()->raw_data, data, dlc); - send_cluster_1_general(); - break; - - case Cluster_2_Quality:memcpy(cluster_2_quality_.get_cluster_2_quality()->raw_data, data, dlc); - send_cluster_2_quality(); - break; - - case Object_0_Status:memcpy(object_0_status_.get_object_0_status()->raw_data, data, dlc); - send_object_0_status(); - break; - - case Object_1_General:memcpy(object_1_general_.get_object_1_general()->raw_data, data, dlc); - send_object_1_general(); - break; - - case Object_2_Quality:memcpy(object_2_quality_.get_object_2_quality()->raw_data, data, dlc); - send_object_2_quality(); - break; - case Object_3_Extended:memcpy(object_3_extended_.get_object_3_extended()->raw_data, data, dlc); - send_object_3_extended(); - break; - - default: { + if (frame_id == RadarState + sensor_id_offset_) { + memcpy(radar_state_.get_radar_state()->raw_data, data, dlc); + send_radar_state(); + } + else if (frame_id == Cluster_0_Status + sensor_id_offset_) { + memcpy(cluster_0_status_.get_cluster_0_status()->raw_data, data, dlc); + send_cluster_0_status(); + } + else if (frame_id == Cluster_1_General + sensor_id_offset_) { + memcpy(cluster_1_general_.get_cluster_1_general()->raw_data, data, dlc); + send_cluster_1_general(); + } + else if (frame_id == Cluster_2_Quality + sensor_id_offset_) { + memcpy(cluster_2_quality_.get_cluster_2_quality()->raw_data, data, dlc); + send_cluster_2_quality(); + } + else if (frame_id == Object_0_Status + sensor_id_offset_) { + memcpy(object_0_status_.get_object_0_status()->raw_data, data, dlc); + send_object_0_status(); + } + else if (frame_id == Object_1_General + sensor_id_offset_) { + memcpy(object_1_general_.get_object_1_general()->raw_data, data, dlc); + send_object_1_general(); + } + else if (frame_id == Object_2_Quality + sensor_id_offset_) { + memcpy(object_2_quality_.get_object_2_quality()->raw_data, data, dlc); + send_object_2_quality(); + } + else if (frame_id == Object_3_Extended + sensor_id_offset_) { + memcpy(object_3_extended_.get_object_3_extended()->raw_data, data, dlc); + send_object_3_extended(); + } #if DEBUG - printf("Unidentified Message: %d\n", frame_id); + else printf("Unidentified Message: %d\n", frame_id); #endif - break; - } - } return true; } bool ARS_40X_CAN::send_radar_data(uint32_t frame_id) { switch (frame_id) { - case RadarCfg:can_.write(frame_id, 8, radar_cfg_.get_radar_cfg()->raw_data); + case RadarCfg:can_.write(frame_id + sensor_id_offset_, 8, radar_cfg_.get_radar_cfg()->raw_data); break; case SpeedInformation: - can_.write(frame_id, + can_.write(frame_id + sensor_id_offset_, 2, speed_information_.get_speed_information()->raw_data); break; case YawRateInformation: - can_.write(frame_id, + can_.write(frame_id + sensor_id_offset_, 2, yaw_rate_information_.get_yaw_rate_information()->raw_data); break; @@ -131,4 +130,9 @@ radar_state::RadarState *ARS_40X_CAN::get_radar_state() { radar_cfg::RadarCfg *ARS_40X_CAN::get_radar_cfg() { return &radar_cfg_; } + +void ARS_40X_CAN::update_sensor_id(uint8_t sensor_id) { + if (sensor_id > 7) throw std::invalid_argument("Invalid sensor id, must be in [0, 7]."); + sensor_id_offset_ = 0x010 * sensor_id; +} } diff --git a/src/ros/ars_40X_ros.cpp b/src/ros/ars_40X_ros.cpp index 6c51bb3..16ce6a1 100644 --- a/src/ros/ars_40X_ros.cpp +++ b/src/ros/ars_40X_ros.cpp @@ -14,7 +14,10 @@ ARS_40X_ROS::ARS_40X_ROS(ros::NodeHandle &nh) : radar_state_ros_(nh_, this) { ros::NodeHandle private_nh("~"); std::string frame_id; + int sensor_id; private_nh.param("frame_id", frame_id, std::string("radar")); + private_nh.param("sensor_id", sensor_id, 0); + update_sensor_id(sensor_id); cluster_list_ros_.set_frame_id(frame_id); object_list_ros_.set_frame_id(frame_id); } diff --git a/src/ros/radar_cfg_ros.cpp b/src/ros/radar_cfg_ros.cpp index c0d8a22..a5dfd12 100644 --- a/src/ros/radar_cfg_ros.cpp +++ b/src/ros/radar_cfg_ros.cpp @@ -49,6 +49,7 @@ bool RadarCfgROS::set_sensor_id( return false; } ars_40X_can_->send_radar_data(can_messages::RadarCfg); + ars_40X_can_->update_sensor_id(req.sensor_id); return true; } From 58bc521c7fe05c1b87af2165d43df94e804c6ad7 Mon Sep 17 00:00:00 2001 From: Marcus Ebner von Eschenbach Date: Mon, 6 Jul 2020 12:43:22 +0300 Subject: [PATCH 3/7] Update README.md --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/README.md b/README.md index 79703de..f97de75 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,13 @@ roslaunch ars_40X ars_40X.launch visualize:=true obstacle_array:=true - **visualize** *(default:"true")* : Launches RViz to display the clusters/obstacles as markers. - **obstacle_array** *(default:"false")* : Launches ars_40X_obstacle_array node which publishes obstacles as geometry_msgs/Polygon +- **frame_id** *(default:"radar")* : Sets the `frame_id` for published markers +- **sensor_id** *(default:0)* : The sensor_id of the radar to use (0-7), determines the message IDs on the CAN bus (see radar documentation) +- **publish_radardetection** *(default:"true")* : Launches a converter, which converts from object lists to ROS radar detection messages + + + + #### Publications From 076fc069f9753bad607d927b5ca499cebc75e7df Mon Sep 17 00:00:00 2001 From: Marcus Ebner von Eschenbach Date: Mon, 6 Jul 2020 12:43:22 +0300 Subject: [PATCH 4/7] Update README.md --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/README.md b/README.md index 79703de..f97de75 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,13 @@ roslaunch ars_40X ars_40X.launch visualize:=true obstacle_array:=true - **visualize** *(default:"true")* : Launches RViz to display the clusters/obstacles as markers. - **obstacle_array** *(default:"false")* : Launches ars_40X_obstacle_array node which publishes obstacles as geometry_msgs/Polygon +- **frame_id** *(default:"radar")* : Sets the `frame_id` for published markers +- **sensor_id** *(default:0)* : The sensor_id of the radar to use (0-7), determines the message IDs on the CAN bus (see radar documentation) +- **publish_radardetection** *(default:"true")* : Launches a converter, which converts from object lists to ROS radar detection messages + + + + #### Publications From ffd5cbd59471011d2c01d33c3e7016f4c5d1be8d Mon Sep 17 00:00:00 2001 From: "m.eschenbach" Date: Mon, 6 Jul 2020 13:25:57 +0300 Subject: [PATCH 5/7] Use anonymous names in launch file to be able to start it multiple times for multiple radars. --- launch/ars_40X.launch | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/ars_40X.launch b/launch/ars_40X.launch index 7766836..0c212e9 100644 --- a/launch/ars_40X.launch +++ b/launch/ars_40X.launch @@ -5,20 +5,20 @@ - + - + - - + + - + From c7b5dfa011e289ed55c45db96d1c322462b4423d Mon Sep 17 00:00:00 2001 From: Marcus Ebner von Eschenbach Date: Mon, 6 Jul 2020 15:44:45 +0300 Subject: [PATCH 6/7] Update README.md --- README.md | 4 ---- 1 file changed, 4 deletions(-) diff --git a/README.md b/README.md index f97de75..755daa8 100644 --- a/README.md +++ b/README.md @@ -21,10 +21,6 @@ roslaunch ars_40X ars_40X.launch visualize:=true obstacle_array:=true - **sensor_id** *(default:0)* : The sensor_id of the radar to use (0-7), determines the message IDs on the CAN bus (see radar documentation) - **publish_radardetection** *(default:"true")* : Launches a converter, which converts from object lists to ROS radar detection messages - - - - #### Publications |Message|Type|Description|Message Box| From fdff37a0f8fbccaf82545205d62ca93336e6336d Mon Sep 17 00:00:00 2001 From: "m.eschenbach" Date: Thu, 9 Jul 2020 12:20:34 +0300 Subject: [PATCH 7/7] Extend converter to convert also cluster lists. --- scripts/convert_to_radardetectionlist.py | 32 ++++++++++++++++-------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/scripts/convert_to_radardetectionlist.py b/scripts/convert_to_radardetectionlist.py index 2f75c76..616e0e8 100755 --- a/scripts/convert_to_radardetectionlist.py +++ b/scripts/convert_to_radardetectionlist.py @@ -4,22 +4,34 @@ from ars_40X.msg import Object, ObjectList, Cluster, ClusterList from radar_msgs.msg import RadarDetection, RadarDetectionArray + +def convert_to_detection(obj): + radar_detection = RadarDetection() + radar_detection.detection_id = obj.id + radar_detection.position = obj.position.pose.position + radar_detection.velocity = obj.relative_velocity.twist.linear + radar_detection.amplitude = obj.rcs + return radar_detection + def object_list_callback(object_list, detection_array_publisher): radar_detection_array = RadarDetectionArray() radar_detection_array.header = object_list.header - for obj in object_list.objects: - radar_detection = RadarDetection() - radar_detection.detection_id = obj.id - radar_detection.position = obj.position.pose.position - radar_detection.velocity = obj.relative_velocity.twist.linear - radar_detection.amplitude = obj.rcs - radar_detection_array.detections.append(radar_detection) + try: + objects = getattr(object_list, 'objects') + except AttributeError: + objects = getattr(object_list, 'clusters') + radar_detection_array.detections = [convert_to_detection(obj) for obj in objects] detection_array_publisher.publish(radar_detection_array) if __name__ == '__main__': rospy.init_node('object_list_converter') - publisher = rospy.Publisher("/radar_converter/detections", RadarDetectionArray, queue_size=10) - callback_closure = lambda object_list: object_list_callback(object_list, publisher) - rospy.Subscriber('/ars_40X/objects', ObjectList, callback_closure) + publisher = rospy.Publisher("/radar_converter/detections123", RadarDetectionArray, queue_size=10) + + object_list_callback_closure = lambda object_list: object_list_callback(object_list, publisher) + rospy.Subscriber('/ars_40X/objects', ObjectList, object_list_callback_closure) + + cluster_list_callback_closure = lambda cluster_list: object_list_callback(cluster_list, publisher) + rospy.Subscriber('/ars_40X/clusters', ClusterList, cluster_list_callback_closure) + rospy.spin() \ No newline at end of file