mlx support.

ZVISION-lidar · Sep 9, 2020 · 37b1a88 · 37b1a88
1 parent 217e9b4
commit 37b1a88
Show file tree

Hide file tree

Showing 51 changed files with 33,666 additions and 225 deletions.
diff --git a/LICENSE b/LICENSE
diff --git a/Release note.txt b/Release note.txt
@@ -4,4 +4,4 @@ V2.2
 V2.5
 -ML30S-A1 support
 V2.6
--Compile error when the dir name contains special characters, '+', '(', ')' etc.
+-Compile error when the dir name contains special characters, '+', '(', ')' etc.
diff --git a/readme.md b/readme.md
diff --git a/zvision_lidar/zvision_lidar/CHANGELOG.rst b/zvision_lidar/zvision_lidar/CHANGELOG.rst
diff --git a/zvision_lidar/zvision_lidar/CMakeLists.txt b/zvision_lidar/zvision_lidar/CMakeLists.txt
diff --git a/zvision_lidar/zvision_lidar/package.xml b/zvision_lidar/zvision_lidar/package.xml
diff --git a/zvision_lidar/zvision_lidar_driver/CMakeLists.txt b/zvision_lidar/zvision_lidar_driver/CMakeLists.txt
diff --git a/zvision_lidar/zvision_lidar_driver/cfg/zvisionLidarNode.cfg b/zvision_lidar/zvision_lidar_driver/cfg/zvisionLidarNode.cfg
diff --git a/zvision_lidar/zvision_lidar_driver/nodelet_zvision_lidar.xml b/zvision_lidar/zvision_lidar_driver/nodelet_zvision_lidar.xml
diff --git a/zvision_lidar/zvision_lidar_driver/package.xml b/zvision_lidar/zvision_lidar_driver/package.xml
diff --git a/zvision_lidar/zvision_lidar_driver/src/CMakeLists.txt b/zvision_lidar/zvision_lidar_driver/src/CMakeLists.txt
diff --git a/zvision_lidar/zvision_lidar_driver/src/input.cc b/zvision_lidar/zvision_lidar_driver/src/input.cc
diff --git a/zvision_lidar/zvision_lidar_driver/src/input.h b/zvision_lidar/zvision_lidar_driver/src/input.h
diff --git a/zvision_lidar/zvision_lidar_driver/src/nodelet.cc b/zvision_lidar/zvision_lidar_driver/src/nodelet.cc
diff --git a/zvision_lidar/zvision_lidar_driver/src/zvision_driver.cpp b/zvision_lidar/zvision_lidar_driver/src/zvision_driver.cpp
@@ -28,18 +28,22 @@ zvisionLidarDriver::zvisionLidarDriver(ros::NodeHandle node, ros::NodeHandle pri
   config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
 
 
-  private_nh.param("model", config_.model, std::string("ML30S-A1"));
+  private_nh.param("model", config_.model, std::string("ML30SA1"));
   double packet_rate_ML30 = 2500;
   double packet_rate_ML30S_A1 = 3200;
+  double packet_rate_MLX = 4000;
   std::string model_full_name;
 
   // product model
-  if (config_.model == "ML30")
+  if (config_.model == "ML30B1")
   {
     model_full_name = "ZVISION-LiDAR-ML30";
   }
-  else if(config_.model == "ML30S-A1"){
-      model_full_name = "ZVISION-LiDAR-ML30S-A1";
+  else if(config_.model == "ML30SA1"){
+    model_full_name = "ZVISION-LiDAR-ML30SA1";
+  }
+  else if(config_.model == "MLX"){
+    model_full_name = "ZVISION-LiDAR-MLX";
   }
   else
   {
@@ -48,7 +52,7 @@ zvisionLidarDriver::zvisionLidarDriver(ros::NodeHandle node, ros::NodeHandle pri
   std::string deviceName(std::string("Zvision ") + model_full_name);
 
 
-  if(config_.model == "ML30"){
+  if(config_.model == "ML30B1"){
 	  int npackets = 125;
 	  private_nh.param("npackets", config_.npackets, npackets);
 	  ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
@@ -91,7 +95,7 @@ zvisionLidarDriver::zvisionLidarDriver(ros::NodeHandle node, ros::NodeHandle pri
 	  output_ = node.advertise<zvision_lidar_msgs::zvisionLidarScan>("zvision_lidar_packets", 20);
 
   }
-  else if(config_.model == "ML30S-A1"){
+  else if(config_.model == "ML30SA1"){
 
 	  int npackets = 160;
 	  private_nh.param("npackets", config_.npackets, npackets);
@@ -111,7 +115,7 @@ zvisionLidarDriver::zvisionLidarDriver(ros::NodeHandle node, ros::NodeHandle pri
 
 	  // initialize diagnostics
 	  diagnostics_.setHardwareID(deviceName);
-	  const double diag_freq = packet_rate_ML30S_A1 / config_.npackets;
+      const double diag_freq = packet_rate_ML30S_A1 / config_.npackets;
 	  diag_max_freq_ = diag_freq;
 	  diag_min_freq_ = diag_freq;
 
@@ -134,6 +138,48 @@ zvisionLidarDriver::zvisionLidarDriver(ros::NodeHandle node, ros::NodeHandle pri
 	  // raw packet output topic
 	  output_ = node.advertise<zvision_lidar_msgs::zvisionLidarScan>("zvision_lidar_packets", 20);
   }
+  else if(config_.model == "MLX"){
+      int npackets = 400;
+      private_nh.param("npackets", config_.npackets, npackets);
+      ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
+
+      std::string dump_file;
+      private_nh.param("pcap", dump_file, std::string(""));
+
+      int port_to_recv_udppkt;
+      private_nh.param("udp_port", port_to_recv_udppkt, (int)UDP_DATA_PORT_NUMBER);
+
+      // Initialize dynamic reconfigure
+      srv_ = boost::make_shared<dynamic_reconfigure::Server<zvision_lidar_driver::zvisionLidarNodeConfig> >(private_nh);
+      dynamic_reconfigure::Server<zvision_lidar_driver::zvisionLidarNodeConfig>::CallbackType f;
+      f = boost::bind(&zvisionLidarDriver::callback, this, _1, _2);
+      srv_->setCallback(f);  // Set callback function und call initially
+
+      // initialize diagnostics
+      diagnostics_.setHardwareID(deviceName);
+      const double diag_freq = packet_rate_MLX / config_.npackets;
+      diag_max_freq_ = diag_freq;
+      diag_min_freq_ = diag_freq;
+
+      using namespace diagnostic_updater;
+      diag_topic_.reset(new TopicDiagnostic("zvision_lidar_packets", diagnostics_,
+                                            FrequencyStatusParam(&diag_min_freq_, &diag_max_freq_, 0.1, 10),
+                                            TimeStampStatusParam()));
+
+      // open zvision lidar input device or file
+      if (dump_file != "")  // have PCAP file?
+      {
+          // read data from packet capture file
+          input_.reset(new zvision_lidar_driver::InputPCAP(private_nh, port_to_recv_udppkt, packet_rate_MLX, dump_file));
+      }
+      else
+      {
+          // read data from live socket
+          input_.reset(new zvision_lidar_driver::InputSocket(private_nh, port_to_recv_udppkt));
+      }
+      // raw packet output topic
+      output_ = node.advertise<zvision_lidar_msgs::zvisionLidarScan>("zvision_lidar_packets", 20);
+  }
   else{
   	ROS_ERROR_STREAM("unknown LIDAR model!");
   }
@@ -170,7 +216,7 @@ bool zvisionLidarDriver::poll(void)
 
 	  if (rc == 0)
 	  {
-		  udp_seq = (int)(scan->packets[i].data[3]) + ((scan->packets[i].data[2] & 0xF) << 8);
+          udp_seq = (int)(scan->packets[i].data[3]) + ((scan->packets[i].data[2] & 0xF) << 8);
 	  }
 
 	  if (i == udp_seq)

diff --git a/zvision_lidar/zvision_lidar_driver/src/zvision_driver.h b/zvision_lidar/zvision_lidar_driver/src/zvision_driver.h
diff --git a/zvision_lidar/zvision_lidar_driver/src/zvision_lidar_node.cpp b/zvision_lidar/zvision_lidar_driver/src/zvision_lidar_node.cpp
diff --git a/zvision_lidar/zvision_lidar_msgs/CMakeLists.txt b/zvision_lidar/zvision_lidar_msgs/CMakeLists.txt
diff --git a/zvision_lidar/zvision_lidar_msgs/msg/zvisionLidarPacket.msg b/zvision_lidar/zvision_lidar_msgs/msg/zvisionLidarPacket.msg
diff --git a/zvision_lidar/zvision_lidar_msgs/msg/zvisionLidarScan.msg b/zvision_lidar/zvision_lidar_msgs/msg/zvisionLidarScan.msg
diff --git a/zvision_lidar/zvision_lidar_msgs/package.xml b/zvision_lidar/zvision_lidar_msgs/package.xml
diff --git a/zvision_lidar/zvision_lidar_pointcloud/CMakeLists.txt b/zvision_lidar/zvision_lidar_pointcloud/CMakeLists.txt
diff --git a/zvision_lidar/zvision_lidar_pointcloud/cfg/CloudNode.cfg b/zvision_lidar/zvision_lidar_pointcloud/cfg/CloudNode.cfg
diff --git a/zvision_lidar/zvision_lidar_pointcloud/cloud_nodelets.xml b/zvision_lidar/zvision_lidar_pointcloud/cloud_nodelets.xml
diff --git a/zvision_lidar/zvision_lidar_pointcloud/data/ML30S_A1_Default.cal b/zvision_lidar/zvision_lidar_pointcloud/data/ML30S_A1_Default.cal
diff --git a/zvision_lidar/zvision_lidar_pointcloud/data/ML30S_A1_Default.pcap b/zvision_lidar/zvision_lidar_pointcloud/data/ML30S_A1_Default.pcap
diff --git a/zvision_lidar/zvision_lidar_pointcloud/data/ML30_Default.cal b/zvision_lidar/zvision_lidar_pointcloud/data/ML30_Default.cal
diff --git a/zvision_lidar/zvision_lidar_pointcloud/data/ML30_Default.pcap b/zvision_lidar/zvision_lidar_pointcloud/data/ML30_Default.pcap