Merge branch 'dev' into 'master'

Dev

See merge request SoftwareTeam/GeneralSoftware/HesaiLidar_Swift_ROS!70

README.md

@@ -138,6 +138,7 @@ $ roslaunch pandar_pointcloud transform_nodelet.launch data_type:=rosbag
 |---------|---------------|
 |calibration|Path of correction file, will be used when not able to get correction file from a connected Liar|
 |device_ip_ip|The IP address of connected Lidar, will be used to get correction file|
+|host_ip|The IP address of host pc, will be used to get udp data from lidar|
 |frame_id|frame id of published messages|
 |firetime_file|Path of firetime files|
 |pcap|Path of the pcap file, once not empty, driver will get data from pcap file instead of a connected Lidar|
@@ -149,4 +150,5 @@ $ roslaunch pandar_pointcloud transform_nodelet.launch data_type:=rosbag
 |channel_config_file|Path of channel config file, will be used when not able to get channel config file from a connected Liar|
 |cert_file|Path of the user's certificate|
 |private_key_file|Path of the user's private key|
-|ca_file|Path of the root certificate|
+|ca_file|Path of the root certificate|
+|multicast_ip|The multicast IP address of connected Lidar, will be used to get udp packets from multicast ip address|

change notes.md

@@ -139,5 +139,12 @@ PandarSwiftROS_1.0.31
 1. Fix bug in publish points mode
 2. Support 0x3a and 0x3e return mode of QT128
 
+Monday December 5th, 2022 17:30
+##version
+PandarSwiftROS_1.0.36
+
+##modify
+1. Support multicast
+
 
 

pandar_pointcloud/include/pandar_pointcloud/input.h

@@ -123,8 +123,8 @@ typedef struct PandarPacket_s {
 
 namespace pandar_pointcloud
 {
-  static uint16_t DATA_PORT_NUMBER = 8080;     // default data port
-  static uint16_t POSITION_PORT_NUMBER = 8308; // default position port
+  static uint16_t DATA_PORT_NUMBER = 2368;     // default data port
+  static uint16_t GPS_PORT_NUMBER = 8308; // default position port
 
   #define PANDAR128_SEQUENCE_NUMBER_OFFSET (831) 
   /** @brief pandar input base class */
@@ -164,7 +164,9 @@ namespace pandar_pointcloud
   {
   public:
     InputSocket(ros::NodeHandle private_nh,
-                uint16_t port = DATA_PORT_NUMBER);
+                std::string host_ip = "",
+                uint16_t port = DATA_PORT_NUMBER,
+                uint16_t gpsport = GPS_PORT_NUMBER, std::string multicast_ip = "");
     virtual ~InputSocket();
 
     virtual int getPacket(PandarPacket *pkt);

pandar_pointcloud/launch/PandarSwift_points.launch

@@ -6,6 +6,7 @@
   <!-- declare arguments with default values -->
   <arg name="calibration" default="$(find pandar_pointcloud)/params/Pandar128_Correction.csv"/>
   <arg name="device_ip" default="192.168.1.201" />
+  <arg name="host_ip" default="" />
   <arg name="frame_id" default="PandarSwift" />
   <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
   <arg name="max_range" default="130.0" />
@@ -20,6 +21,7 @@
   <arg name="start_angle" default="0.0" />
   <arg name="publish_model" default="both_point_raw" />
   <arg name="namespace" default="hesai"/>
+  <arg name="multicast_ip"  default=""/>
   <arg name="coordinate_correction_flag" default="false" />
   <arg name="channel_config_file" default="$(find pandar_pointcloud)/params/QT128C2X_Channel_Cofig.csv" />
   <!--"cert_file" represents the path of the user's certificate-->
@@ -40,6 +42,7 @@
     <arg name="min_range" value="$(arg min_range)"/>
     <arg name="start_angle" value="$(arg start_angle)"/>
     <arg name="device_ip" value="$(arg device_ip)" />
+    <arg name="host_ip" value="$(arg host_ip)" />
     <arg name="frame_id" value="$(arg frame_id)"/>
     <arg name="publish_model" value="$(arg publish_model)"/>
     <arg name="pcap" value="$(arg pcap)"/>
@@ -50,6 +53,7 @@
     <arg name="rpm" value="$(arg rpm)"/>
     <arg name="firetime_file" value="$(arg firetime_file)"/>
     <arg name="namespace" value="$(arg namespace)"/>
+    <arg name="multicast_ip" value="$(arg multicast_ip)"/>
     <arg name="coordinate_correction_flag" value="$(arg coordinate_correction_flag)"/>
     <arg name="channel_config_file" value="$(arg channel_config_file)"/>
     <arg name="cert_file" value="$(arg cert_file)" />

pandar_pointcloud/launch/cloud_nodelet.launch

@@ -8,6 +8,7 @@
   <arg name="start_angle" default="0" />
 
   <arg name="device_ip" default="" />
+  <arg name="host_ip" default="" />
   <arg name="frame_id" default="Pandar128" />
   <arg name="publish_model" default="both_point_raw" />
   <arg name="pcap" default="" />
@@ -19,6 +20,7 @@
   <arg name="firetime_file"  default="$(find pandar_pointcloud)/params/Pandar128_Firetimes.csv"/>
   <arg name="namespace" default="hesai"/>
   <arg name="coordinate_correction_flag" default="false" />
+  <arg name="multicast_ip"  default=""/>
   <arg name="channel_config_file" default="$(find pandar_pointcloud)/params/QT128C2X_Channel_Cofig.csv" />
   <arg name="cert_file" default="" />
   <arg name="private_key_file" default="" />
@@ -33,6 +35,7 @@
     <param name="start_angle" value="$(arg start_angle)"/>
 
     <param name="device_ip" value="$(arg device_ip)" />
+    <param name="host_ip" value="$(arg host_ip)" />
     <param name="frame_id" value="$(arg frame_id)"/>
     <param name="publish_model" value="$(arg publish_model)"/>
     <param name="pcap" value="$(arg pcap)"/>
@@ -47,6 +50,7 @@
     <param name="cert_file" value="$(arg cert_file)" />
     <param name="private_key_file" value="$(arg private_key_file)" />
     <param name="ca_file" value="$(arg ca_file)" />
+    <param name="multicast_ip" value="$(arg multicast_ip)"/>
   </node>
 
   <!--node pkg="nodelet" type="nodelet" name="$(arg manager)_color"

pandar_pointcloud/src/conversions/convert.cc

@@ -86,7 +86,7 @@ Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh,
     : data_(new pandar_rawdata::RawData()),
       drv(node, private_nh, node_type, this) {
 
-  m_sRosVersion = "PandarSwiftROS_1.0.31";
+  m_sRosVersion = "PandarSwiftROS_1.0.36";
   ROS_WARN("--------PandarSwift ROS version: %s--------\n\n",m_sRosVersion.c_str());
 
   publishmodel = "";
@@ -1307,7 +1307,8 @@ void Convert::calcPointXYZIT(pandar_msgs::PandarPacket &packet, int cursor) {
         }
         else{
           pthread_mutex_lock(&m_RedundantPointLock);
-          m_RedundantPointBuffer.push_back(RedundantPoint{point_index, point});
+          if (point.timestamp - m_OutMsgArray[cursor]->points[point_index].timestamp  > 0.01 && point.timestamp - m_OutMsgArray[cursor]->points[point_index].timestamp  < 0.11)
+						m_RedundantPointBuffer.push_back(RedundantPoint{point_index, point});
           pthread_mutex_unlock(&m_RedundantPointLock);
         }
       }
@@ -1483,7 +1484,8 @@ void Convert::calcQT128PointXYZIT(pandar_msgs::PandarPacket &packet, int cursor)
       }
       else{
         pthread_mutex_lock(&m_RedundantPointLock);
-        m_RedundantPointBuffer.push_back(RedundantPoint{point_index, point});
+        if (point.timestamp - m_OutMsgArray[cursor]->points[point_index].timestamp  > 0.01 && point.timestamp - m_OutMsgArray[cursor]->points[point_index].timestamp  < 0.11)
+						m_RedundantPointBuffer.push_back(RedundantPoint{point_index, point});
         pthread_mutex_unlock(&m_RedundantPointLock);
       }
     }

pandar_pointcloud/src/conversions/driver.cc

@@ -62,6 +62,12 @@ PandarDriver::PandarDriver(ros::NodeHandle node, ros::NodeHandle private_nh,
   int udp_port;
   private_nh.param("port", udp_port, (int)DATA_PORT_NUMBER);
 
+  std::string multicast_ip;
+  private_nh.param("multicast_ip", multicast_ip, std::string(""));
+
+  std::string host_ip;
+  private_nh.param("host_ip", host_ip, std::string(""));
+
   pthread_mutex_init(&piclock, NULL);
 
   m_bNeedPublish = false;
@@ -107,7 +113,7 @@ PandarDriver::PandarDriver(ros::NodeHandle node, ros::NodeHandle private_nh,
                                                   packet_rate, dump_file));
   } else {
     // read data from live socket
-    m_spInput.reset(new pandar_pointcloud::InputSocket(private_nh, udp_port));
+    m_spInput.reset(new pandar_pointcloud::InputSocket(private_nh, host_ip, udp_port, 10010, multicast_ip));
   }
   // ROS_WARN("drive nodeType[%s]", nodeType.c_str());
   // ROS_WARN("drive publishmodel[%s]", publishmodel.c_str());

pandar_pointcloud/src/lib/input.cc

@@ -187,7 +187,7 @@ std::string Input::getUdpVersion() {
  *  @param private_nh ROS private handle for calling node.
  *  @param port UDP port number
  */
-InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port)
+InputSocket::InputSocket(ros::NodeHandle private_nh, std::string host_ip, uint16_t port, uint16_t gpsport, std::string multicast_ip)
     : Input(private_nh, port) {
   sockfd_ = -1;
   m_u32Sequencenum = 0;
@@ -234,6 +234,18 @@ InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port)
   ROS_DEBUG("Pandar socket fd is %d\n", sockfd_);
   int nRecvBuf = 26214400;
 	setsockopt(sockfd_, SOL_SOCKET, SO_RCVBUF, (const char*)&nRecvBuf, sizeof(int));
+  if(multicast_ip != ""){
+    struct ip_mreq mreq;                    
+    mreq.imr_multiaddr.s_addr=inet_addr(multicast_ip.c_str());
+    mreq.imr_interface.s_addr = host_ip == "" ? htons(INADDR_ANY) : inet_addr(host_ip.c_str());
+    int ret = setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char *)&mreq, sizeof(mreq));
+    if (ret < 0) {
+      printf("Multicast IP error,set correct multicast ip address or keep it empty %s %s\n", multicast_ip.c_str(), host_ip.c_str());
+    } 
+    else {
+      printf("Recive data from multicast ip address %s %s\n", multicast_ip.c_str(), host_ip.c_str());
+    }
+  }
 }
 
 /** @brief destructor */