Feature/selective aruco detect

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
 
 
 ##############################################################################
@@ -103,6 +104,13 @@ def __init__(self, config: AutoDockConfig, run_server: bool):
         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 +344,19 @@ 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
+        """
+        try:
+            rospy.wait_for_service('/enable_detections', timeout=3.0)
+            resp = self.__enable_detections_srv(detection_state)
+            print("Enable detections response: ", resp.message)
+            return resp.success
+        except rospy.ServiceException as e:
+            print("Service call failed: %s"%e)
+
     def __pause_dock_cb(self, msg):
         self.is_pause = msg.data
 

autodock_core/scripts/simple_autodock.py

@@ -141,6 +141,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 +157,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 +166,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 +275,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 +287,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 +313,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):