Add debug_mode parameter

-------------------------------------------
Default true, when false:
1. selectively enable/disable aruco detections
2. turn off viz markers

autodock_core/launch/autodock_server.launch

@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="use_fake_clock" default="false" doc="Use this with Caution!!!!"/>
+  <arg name="debug_mode" default="true" doc="Use this to have aruco detections always ON (publish fiducial images always) and other useful visualization markers. Turn OFF for better performance in deployment."/>
   <arg name="autodock_config" default="$(find autodock_core)/configs/mock_robot.yaml"/>
 
   <!-- Danger Zone!!! launch simple dock server with fake clock -->
@@ -9,6 +10,7 @@
     <node pkg="autodock_core" name="simple_autodock" type="simple_autodock.py" 
           args="--server --rosparam --fake_clock" output="screen" respawn="false">
       <rosparam command="load" file="$(arg autodock_config)" />
+      <param name="debug_mode" value="$(arg debug_mode)"/>
     </node>
   </group>
 
@@ -17,6 +19,7 @@
     <node pkg="autodock_core" name="simple_autodock" type="simple_autodock.py" 
           args="--server --rosparam" output="screen" respawn="false">
       <rosparam command="load" file="$(arg autodock_config)" />
+      <param name="debug_mode" value="$(arg debug_mode)"/>
     </node>
   </group>
 

autodock_core/scripts/autodock_core/autodock_server.py

@@ -60,6 +60,8 @@ class AutoDockConfig:
     # stop thresh
     stop_yaw_diff: float         # radian
     stop_trans_diff: float      # meters
+    # debug state
+    debug_mode: bool
 
 ##############################################################################
 ##############################################################################
@@ -95,9 +97,10 @@ def __init__(self, config: AutoDockConfig, run_server: bool):
         # create_subscriber to pause dock
         rospy.Subscriber("/pause_dock", Bool, self.__pause_dock_cb)
 
-        # debug timer
-        self.__marker_pub = rospy.Publisher('/sm_maker', Marker, queue_size=1)
-        self.__timer = rospy.Timer(rospy.Duration(0.5), self.__timer_cb)
+        # debug timer for state machine marker
+        if self.cfg.debug_mode:
+            self.__marker_pub = rospy.Publisher('/sm_maker', Marker, queue_size=1)
+            self.__timer = rospy.Timer(rospy.Duration(0.5), self.__timer_cb)
         self.is_pause = False # TODO
 
         self.dock_state = DockState.INVALID
@@ -349,13 +352,16 @@ def set_aruco_detections(self, detection_state) -> bool:
         Set aruco detections to True or False
         :return : success
         """
-        try:
-            rospy.wait_for_service('/enable_detections', timeout=3.0)
-            resp = self.__enable_detections_srv(detection_state)
-            rospy.loginfo("Enable detections response: " + resp.message)
-            return resp.success
-        except rospy.ServiceException as e:
-            rospy.logerr("Service call failed: " + str(e))
+        if self.cfg.debug_mode:
+            return True
+        else:
+            try:
+                rospy.wait_for_service('/enable_detections', timeout=3.0)
+                resp = self.__enable_detections_srv(detection_state)
+                rospy.loginfo("Enable detections response: " + resp.message)
+                return resp.success
+            except rospy.ServiceException as e:
+                rospy.logerr("Service call failed: " + str(e))
 
     def __pause_dock_cb(self, msg):
         self.is_pause = msg.data

autodock_core/scripts/simple_autodock.py

@@ -84,6 +84,10 @@ class DefaultAutoDockConfig(AutoDockConfig):
     retry_count = 1             # how many times to retry
     retry_retreat_dis = 0.4     # meters, distance retreat during retry
 
+    # debug state
+    debug_mode = True           # when False selectively turns on aruco detections only
+                                # a valid action is in progress.
+
 
 class AutoDockStateMachine(AutoDockServer):
     """

autodock_sim/launch/tb3_nav_dock_sim.launch

@@ -3,6 +3,7 @@
   <!-- Global param -->
   <arg name="autodock_server" default="true"/>
   <arg name="use_sim_time" default="true"/>
+  <arg name="debug_mode" default="true" doc="Use this to have aruco detections always ON (publish fiducial images always) and other useful visualization markers. Turn OFF for better performance in deployment."/>
 
   <!-- Spawn tb3 pose -->
   <arg name="x_pos" default="0.5"/>
@@ -17,6 +18,7 @@
   <group if="$(arg autodock_server)">
     <include file="$(find autodock_core)/launch/autodock_server.launch">
       <arg name="autodock_config" default="$(find autodock_examples)/configs/turtlebot3.yaml"/>
+      <arg name="debug_mode" value="$(arg debug_mode)" />
     </include>
   </group>