Handle sensor_id via roslaunch arg/param and set_sensor_id service

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
 

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_;
 };
 }
 

launch/ars_40X.launch

@@ -1,18 +1,24 @@
 <launch>
   <arg name="visualize" default="true"/>
   <arg name="obstacle_array" default="false"/>
+  <arg name="publish_radardetection" default="true"/>
+  <arg name="frame_id" default="radar" />
+  <arg name="sensor_id" default="0" />
 
-  <node pkg="ars_40X" type="ars_40X_ros" name="ars_40X_ros" output="screen">
-    <!--<param name="frame_id" value="radar_link"/>-->
+  <node pkg="ars_40X" type="ars_40X_ros" name="$(anon ars_40X_ros)" output="screen">
+    <param name="frame_id" value="$(arg frame_id)"/>
+    <param name="sensor_id" value="$(arg sensor_id)"/>
   </node>
+
+  <node pkg="ars_40X" type="convert_to_radardetectionlist.py" name="$(anon object_list_converter)" output="screen" if="$(arg publish_radardetection)" />
 
   <group if="$(arg visualize)">
-    <node pkg="ars_40X" type="ars_40X_rviz" name="ars_40X_rviz"/>
-    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ars_40X)/rviz_cfg/ars_40X.rviz"/>
+    <node pkg="ars_40X" type="ars_40X_rviz" name="$(anon ars_40X_rviz)"/>
+    <node pkg="rviz" type="rviz" name="$(anon rviz)" args="-d $(find ars_40X)/rviz_cfg/ars_40X.rviz"/>
   </group>
 
   <group if="$(arg obstacle_array)">
-    <node pkg="ars_40X" type="ars_40X_obstacle_array" name="ars_40X_obstacle_array">
+    <node pkg="ars_40X" type="ars_40X_obstacle_array" name="$(anon ars_40X_obstacle_array)">
       <!--<remap from="obstacles" to="/move_base/TebLocalPlannerROS/obstacles"/>-->
     </node>
   </group>

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()

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;
+}
 }

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<std::string>("frame_id", frame_id, std::string("radar"));
+  private_nh.param<int>("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);
 }

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;
 }