diff --git a/README.md b/README.md index 79703de..755daa8 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,9 @@ 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 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 013595f..0c212e9 100644 --- a/launch/ars_40X.launch +++ b/launch/ars_40X.launch @@ -1,18 +1,24 @@ + + + - - + + + + + - - + + - + diff --git a/scripts/convert_to_radardetectionlist.py b/scripts/convert_to_radardetectionlist.py new file mode 100755 index 0000000..616e0e8 --- /dev/null +++ b/scripts/convert_to_radardetectionlist.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python + +import rospy +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 + 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/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 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; }