From 47ad6e9b8b124004a6f36cfbdd23f158165b326c Mon Sep 17 00:00:00 2001 From: swaroophs Date: Wed, 11 May 2022 09:29:39 +0800 Subject: [PATCH 1/5] Fixing some typos/grammar --- autodock_core/scripts/simple_autodock.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autodock_core/scripts/simple_autodock.py b/autodock_core/scripts/simple_autodock.py index e87d076..7bf9661 100755 --- a/autodock_core/scripts/simple_autodock.py +++ b/autodock_core/scripts/simple_autodock.py @@ -172,7 +172,7 @@ def start(self) -> bool: # 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(): @@ -270,7 +270,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 +282,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 +308,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): From 6239f987c0c733b313c504fff8f2eb9ad78b0dba Mon Sep 17 00:00:00 2001 From: swaroophs Date: Wed, 11 May 2022 09:35:45 +0800 Subject: [PATCH 2/5] Add service client and utility function to set detections --- .../scripts/autodock_core/autodock_server.py | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/autodock_core/scripts/autodock_core/autodock_server.py b/autodock_core/scripts/autodock_core/autodock_server.py index 3caf47d..9599910 100755 --- a/autodock_core/scripts/autodock_core/autodock_server.py +++ b/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 From 3a4d141dcab04b58b5ae2bb87909f0cabcce65c8 Mon Sep 17 00:00:00 2001 From: swaroophs Date: Wed, 11 May 2022 09:36:32 +0800 Subject: [PATCH 3/5] Set aruco detections based on autodock state --- autodock_core/scripts/simple_autodock.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/autodock_core/scripts/simple_autodock.py b/autodock_core/scripts/simple_autodock.py index 7bf9661..b2529ce 100755 --- a/autodock_core/scripts/simple_autodock.py +++ b/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,10 +166,12 @@ 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 @@ -177,6 +181,7 @@ def start(self) -> bool: 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 From c27c8b4077b135af85bf19025691f8e9d23c063e Mon Sep 17 00:00:00 2001 From: swaroophs Date: Thu, 26 May 2022 09:48:58 +0800 Subject: [PATCH 4/5] Replace print with rospy.log --- autodock_core/scripts/autodock_core/autodock_server.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autodock_core/scripts/autodock_core/autodock_server.py b/autodock_core/scripts/autodock_core/autodock_server.py index 9599910..1cdd273 100755 --- a/autodock_core/scripts/autodock_core/autodock_server.py +++ b/autodock_core/scripts/autodock_core/autodock_server.py @@ -352,10 +352,10 @@ def set_aruco_detections(self, detection_state) -> bool: try: rospy.wait_for_service('/enable_detections', timeout=3.0) resp = self.__enable_detections_srv(detection_state) - print("Enable detections response: ", resp.message) + rospy.loginfo("Enable detections response: " + resp.message) return resp.success except rospy.ServiceException as e: - print("Service call failed: %s"%e) + rospy.logerr("Service call failed: " + str(e)) def __pause_dock_cb(self, msg): self.is_pause = msg.data From f1683348e7a1e1325a68055d328dbe37bb1b42af Mon Sep 17 00:00:00 2001 From: swaroophs Date: Thu, 26 May 2022 14:28:34 +0800 Subject: [PATCH 5/5] 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 | 3 +++ .../scripts/autodock_core/autodock_server.py | 26 ++++++++++++------- autodock_core/scripts/simple_autodock.py | 4 +++ autodock_sim/launch/tb3_nav_dock_sim.launch | 2 ++ 4 files changed, 25 insertions(+), 10 deletions(-) diff --git a/autodock_core/launch/autodock_server.launch b/autodock_core/launch/autodock_server.launch index 855d5a9..6151568 100644 --- a/autodock_core/launch/autodock_server.launch +++ b/autodock_core/launch/autodock_server.launch @@ -1,6 +1,7 @@ + @@ -9,6 +10,7 @@ + @@ -17,6 +19,7 @@ + diff --git a/autodock_core/scripts/autodock_core/autodock_server.py b/autodock_core/scripts/autodock_core/autodock_server.py index 1cdd273..b203481 100755 --- a/autodock_core/scripts/autodock_core/autodock_server.py +++ b/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 diff --git a/autodock_core/scripts/simple_autodock.py b/autodock_core/scripts/simple_autodock.py index b2529ce..76ee5ad 100755 --- a/autodock_core/scripts/simple_autodock.py +++ b/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): """ diff --git a/autodock_sim/launch/tb3_nav_dock_sim.launch b/autodock_sim/launch/tb3_nav_dock_sim.launch index 6f0b3b2..d5b09fb 100644 --- a/autodock_sim/launch/tb3_nav_dock_sim.launch +++ b/autodock_sim/launch/tb3_nav_dock_sim.launch @@ -3,6 +3,7 @@ + @@ -17,6 +18,7 @@ +