Merge pull request #28 from pf-telos/fix/allow-param-defaults

Let sensor start scan output with packet_type and watchdog only optionally explicitly set by driver

src/pf_driver/include/pf_driver/pf/pfsdp_base.h

@@ -100,16 +100,14 @@ class PFSDPBase
 
   std::string get_parameter_str(const std::string& param);
 
-  void request_handle_tcp(int port = 0, const std::string& packet_type = "");
+  void request_handle_tcp(int port = 0);
 
-  virtual void request_handle_udp(const std::string& packet_type = "");
+  virtual void request_handle_udp();
 
   bool release_handle(const std::string& handle);
 
   virtual void get_scanoutput_config(const std::string& handle);
 
-  bool set_scanoutput_config(const std::string& handle, const ScanConfig& config);
-
   bool update_scanoutput_config();
 
   bool start_scanoutput();

src/pf_driver/src/pf/pf_interface.cpp

@@ -112,8 +112,14 @@ bool PFInterface::init(std::shared_ptr<HandleInfo> info, std::shared_ptr<ScanCon
   }
 
   prev_handle_ = info_->handle;
-  protocol_interface_->setup_parameters_callback();
+
+  /* Configure sensor from ScanConfig (only explicitly set options) */
   protocol_interface_->update_scanoutput_config();
+
+  /* Update our ScanConfig from sensor (all options) */
+  protocol_interface_->get_scanoutput_config(info_->handle);
+
+  protocol_interface_->setup_parameters_callback();
   protocol_interface_->set_connection_failure_cb(std::bind(&PFInterface::connection_failure_cb, this));
   change_state(PFState::INIT);
   return true;
@@ -158,11 +164,14 @@ bool PFInterface::start_transmission(std::shared_ptr<std::mutex> net_mtx,
     return false;
 
   protocol_interface_->start_scanoutput();
-  // start watchdog timer is watchdog is true in config AND if the device is not R2000
-  // since watchdog for R2300 is always off
-  if (config_->watchdog && protocol_interface_->get_product().find("R2000") != std::string::npos)
+  if (config_->watchdog)
   {
-    start_watchdog_timer(config_->watchdogtimeout / 1000.0);
+    double timeout_s = config_->watchdogtimeout / 1000.0;
+    if (timeout_s < 0.1)
+    {
+      timeout_s = 0.1;
+    }
+    start_watchdog_timer(timeout_s);
   }
   change_state(PFState::RUNNING);
   return true;

src/pf_driver/src/pf/pfsdp_base.cpp

@@ -254,14 +254,10 @@ std::string PFSDPBase::get_parameter_str(const std::string& param)
   return resp[param];
 }
 
-void PFSDPBase::request_handle_tcp(int port, const std::string& packet_type)
+void PFSDPBase::request_handle_tcp(int port)
 {
   param_vector_type query;
-  if (!packet_type.empty())
-  {
-    query.push_back(KV("packet_type", packet_type));
-  }
-  else
+  if (!config_->packet_type.empty())
   {
     query.push_back(KV("packet_type", config_->packet_type));
   }
@@ -273,18 +269,12 @@ void PFSDPBase::request_handle_tcp(int port, const std::string& packet_type)
 
   info_->handle = resp["handle"];
   info_->actual_port = parser_utils::to_long(resp["port"]);
-
-  // TODO: port and pkt_type should be updated in config_
 }
 
-void PFSDPBase::request_handle_udp(const std::string& packet_type)
+void PFSDPBase::request_handle_udp()
 {
   param_vector_type query = { KV("address", info_->endpoint), KV("port", info_->actual_port) };
-  if (!packet_type.empty())
-  {
-    query.push_back(KV("packet_type", packet_type));
-  }
-  else
+  if (!config_->packet_type.empty())
   {
     query.push_back(KV("packet_type", config_->packet_type));
   }
@@ -306,36 +296,28 @@ void PFSDPBase::get_scanoutput_config(const std::string& handle)
   config_->max_num_points_scan = parser_utils::to_long(resp["max_num_points_scan"]);
 }
 
-bool PFSDPBase::set_scanoutput_config(const std::string& handle, const ScanConfig& config)
+bool PFSDPBase::update_scanoutput_config()
 {
-  param_vector_type query = { KV("handle", handle),
-                              KV("start_angle", config.start_angle),
-                              KV("packet_type", config.packet_type),
-                              KV("max_num_points_scan", config.max_num_points_scan),
-                              KV("watchdogtimeout", config.watchdogtimeout),
-                              KV("skip_scans", config.skip_scans),
-                              KV("watchdog", config.watchdog ? "on" : "off") };
-  auto resp = get_request("set_scanoutput_config", { "" }, query);
+  param_vector_type query = { KV("handle", info_->handle) };
 
-  // update global config_
-  get_scanoutput_config(handle);
-  get_scan_parameters();
-  return true;
-}
+  query.push_back(KV("start_angle", config_->start_angle));
+  if (!config_->packet_type.empty())
+  {
+    query.push_back(KV("packet_type", config_->packet_type));
+  }
+  query.push_back(KV("max_num_points_scan", config_->max_num_points_scan));
+  query.push_back(KV("skip_scans", config_->skip_scans));
+  if (config_->watchdogtimeout != 0)
+  {
+    query.push_back(KV("watchdogtimeout", config_->watchdogtimeout));
+  }
+  if (config_->watchdog == false)
+  {
+    /* Force watchdog off. Otherwise (if watchdog==true), use scanner default */
+    query.push_back(KV("watchdog", "off"));
+  }
 
-bool PFSDPBase::update_scanoutput_config()
-{
-  param_vector_type query = { KV("handle", info_->handle),
-                              KV("start_angle", config_->start_angle),
-                              KV("packet_type", config_->packet_type),
-                              KV("max_num_points_scan", config_->max_num_points_scan),
-                              KV("watchdogtimeout", config_->watchdogtimeout),
-                              KV("skip_scans", config_->skip_scans),
-                              KV("watchdog", config_->watchdog ? "on" : "off") };
   auto resp = get_request("set_scanoutput_config", { "" }, query);
-
-  // recalculate scan params
-  get_scan_parameters();
   return true;
 }
 

src/pf_driver/src/ros/ros_main.cpp

@@ -13,12 +13,21 @@ int main(int argc, char* argv[])
   auto node = std::make_shared<rclcpp::Node>("pf_driver");
 
   std::vector<std::string> pfsdp_init;
-  std::string device, transport_str, scanner_ip, topic, frame_id, packet_type;
-  int samples_per_scan, start_angle, max_num_points_scan, watchdogtimeout, skip_scans, num_layers;
-  int port = 0;
-  num_layers = 0;
-  skip_scans = 0;
-  bool watchdog, apply_correction = 0;
+  std::string device;
+  std::string transport_str;
+  std::string scanner_ip;
+  int port = 0; /* 0 means: automatic */
+  std::string topic("/scan");
+  std::string frame_id("scanner_link");
+  std::string packet_type; /* empty means: use scanner default */
+  int samples_per_scan = 0;
+  int start_angle = -1800000;
+  int max_num_points_scan = 0;
+  int watchdogtimeout = 0; /* "0" means: use scanner default */
+  int skip_scans = 0;
+  int num_layers = 1;
+  bool watchdog = true; /* "true" means: use scanner default */
+  bool apply_correction = false;
 
   if (!node->has_parameter("device"))
   {
@@ -132,6 +141,7 @@ int main(int argc, char* argv[])
   std::shared_ptr<HandleInfo> info = std::make_shared<HandleInfo>();
 
   info->handle_type = transport_str == "udp" ? HandleInfo::HANDLE_TYPE_UDP : HandleInfo::HANDLE_TYPE_TCP;
+
   info->hostname = node->get_parameter("scanner_ip").get_parameter_value().get<std::string>();
   info->actual_port = -1;