Support auto reset setting (#27)

* Support auto reset setting * Don't send auto reset command if the parameter is not given
at-wat · Feb 24, 2018 · d74a85b · d74a85b
1 parent 1e10f40
commit d74a85b
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
diff --git a/include/vssp.h b/include/vssp.h
@@ -129,6 +129,13 @@ class VsspDriver
   {
     cb_ping_ = cb;
   }
+  void setAutoReset(const bool enable)
+  {
+    if (enable)
+      send(std::string("SET:_ars=1\n"));
+    else
+      send(std::string("SET:_ars=0\n"));
+  }
   [[deprecated("use setHorizontalInterlace() instead of setInterlace()")]]
   void setInterlace(const int itl)
   {

diff --git a/src/hokuyo3d.cpp b/src/hokuyo3d.cpp
@@ -200,6 +200,8 @@ class Hokuyo3dNode
     {
       ROS_INFO("Connection established");
       ping();
+      if (set_auto_reset_)
+        driver_.setAutoReset(auto_reset_);
       driver_.setHorizontalInterlace(horizontal_interlace_);
       driver_.requestHorizontalTable();
       driver_.setVerticalInterlace(vertical_interlace_);
@@ -235,6 +237,8 @@ class Hokuyo3dNode
     pnh_.param("imu_frame_id", imu_frame_id_, frame_id_ + "_imu");
     pnh_.param("mag_frame_id", mag_frame_id_, frame_id_ + "_mag");
     pnh_.param("range_min", range_min_, 0.0);
+    set_auto_reset_ = pnh_.hasParam("auto_reset");
+    pnh_.param("auto_reset", auto_reset_, false);
 
     std::string output_cycle;
     pnh_.param("output_cycle", output_cycle, std::string("field"));
@@ -402,6 +406,8 @@ class Hokuyo3dNode
   std::string frame_id_;
   std::string imu_frame_id_;
   std::string mag_frame_id_;
+  bool auto_reset_;
+  bool set_auto_reset_;
 };
 
 int main(int argc, char **argv)