Merge pull request #5 from cognicept-admin/feature/selective_aruco_de…

…tect

Feature/selective aruco detect

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

@@ -32,6 +32,7 @@
 from std_msgs.msg import Bool
 from autodock_core.msg import AutoDockingAction, AutoDockingFeedback
 from autodock_core.msg import AutoDockingGoal, AutoDockingResult
+from std_srvs.srv import SetBool
 
 
 ##############################################################################
@@ -59,6 +60,8 @@ class AutoDockConfig:
     # stop thresh
     stop_yaw_diff: float         # radian
     stop_trans_diff: float      # meters
+    # debug state
+    debug_mode: bool
 
 ##############################################################################
 ##############################################################################
@@ -94,15 +97,23 @@ 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
         self.start_time = rospy.Time.now()
         self.sleep_period = rospy.Duration(1/self.cfg.controller_rate)
 
+        # create service client to 
+        # 1. enable aruco detections only when action is started
+        # 2. disable aruco detections when action is idle
+        self.__enable_detections_srv = rospy.ServiceProxy('/enable_detections', SetBool)
+        # disable on initialize
+        self.set_aruco_detections(detection_state=False)
+
         if run_server:
             self.__as = actionlib.SimpleActionServer(
                 "autodock_action",
@@ -336,6 +347,22 @@ def rotate_with_odom(self, rotate: float) -> bool:
             rospy.sleep(self.sleep_period)
         exit(0)
 
+    def set_aruco_detections(self, detection_state) -> bool:
+        """
+        Set aruco detections to True or False
+        :return : success
+        """
+        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):
     """
@@ -141,6 +145,7 @@ def start(self) -> bool:
         """
         rospy.loginfo("Start Docking Action Now")
         self.init_params()
+        self.set_aruco_detections(detection_state=True)
         rospy.sleep(self.sleep_period)
 
         current_retry_count = 0
@@ -156,6 +161,7 @@ def start(self) -> bool:
             ):
                 self.publish_cmd()
                 self.set_state(DockState.IDLE, "All Success!")
+                self.set_aruco_detections(detection_state=False)
                 return True
 
             # If Dock failed
@@ -164,19 +170,22 @@ def start(self) -> bool:
 
             # Break from a retry
             if(current_retry_count >= self.cfg.retry_count):
+                self.set_aruco_detections(detection_state=False)
                 break
 
             # check again if it failed because of canceled
             if self.check_cancel():
+                self.set_aruco_detections(detection_state=False)
                 break
 
             # Attempt a Retry
             current_retry_count += 1
-            rospy.logwarn("Attemping retry: "
+            rospy.logwarn("Attempting retry: "
                           f"{current_retry_count}/{self.cfg.retry_count}")
 
             if not self.do_retry():
                 rospy.logwarn("Not able to retry")
+                self.set_aruco_detections(detection_state=False)
                 break
 
         # Note: will not set_state IDLE here since we will wish to capture
@@ -270,7 +279,7 @@ def do_predock(self) -> bool:
 
         # start predock loop
         _pose_list = []
-        rospy.loginfo("Both Markers are detected, runnning predock loop")
+        rospy.loginfo("Both Markers are detected, running predock loop")
         while not rospy.is_shutdown():
             if self.check_cancel():
                 return False
@@ -282,7 +291,7 @@ def do_predock(self) -> bool:
             centre_tf = self.get_centre_of_side_markers()
 
             if centre_tf is None:
-                rospy.logerr("Not detecting two sides marker, exit state")
+                rospy.logerr("Not detecting two side markers, exit state")
                 return False
 
             if self.cfg.front_dock:
@@ -308,7 +317,7 @@ def do_predock(self) -> bool:
                     y_offset *= -1
                 _pose_list = []
 
-                # if robot y axis is way off, we will do parellel correction
+                # if robot y axis is way off, we will do parallel correction
                 # after this, will repeat predock
                 if abs(y_offset) > self.cfg.max_parallel_offset:
                     if not self.do_parallel_correction(y_offset):

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>