From 5c4fe3310650f9c256da149d1e60aebb31ac8d34 Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Thu, 7 Mar 2024 16:41:37 -0800 Subject: [PATCH] Multiple improvements from testing survey_manager (#156) * improvements on feedback * make task time more reasonable + improve output * adding berth positions for granite table * updating submodule * reverting pano distance --- .../inspection/include/inspection/inspection.h | 3 +++ .../behaviors/inspection/src/inspection_node.cc | 15 +++++++++++++-- .../survey_dependencies/ros2_planning_system | 2 +- .../data/granite_survey_static.yaml | 2 ++ .../survey/survey_manager/pddl/domain_survey.pddl | 2 +- .../src/survey_manager/command_astrobee.py | 13 ++++++++++--- 6 files changed, 30 insertions(+), 7 deletions(-) diff --git a/astrobee/behaviors/inspection/include/inspection/inspection.h b/astrobee/behaviors/inspection/include/inspection/inspection.h index c06c2b7f..8b774042 100644 --- a/astrobee/behaviors/inspection/include/inspection/inspection.h +++ b/astrobee/behaviors/inspection/include/inspection/inspection.h @@ -105,6 +105,9 @@ class Inspection { bool RedoInspectionPose(); geometry_msgs::PoseArray GetInspectionPoses(); + int GetCurrentCounter() {return inspection_counter_ + 1;} + int GetSurveySize() {return points_.size();} + // Get distance from camera to target double GetDistanceToTarget(); diff --git a/astrobee/behaviors/inspection/src/inspection_node.cc b/astrobee/behaviors/inspection/src/inspection_node.cc index 3bb61c20..235e0cb5 100644 --- a/astrobee/behaviors/inspection/src/inspection_node.cc +++ b/astrobee/behaviors/inspection/src/inspection_node.cc @@ -549,7 +549,14 @@ class InspectionNode : public ff_util::FreeFlyerNodelet { sci_cam_req_ = 0; // If the action was cancelled stop taking more pictures - if (fsm_.GetState() == STATE::WAITING) return; + if (fsm_.GetState() == STATE::WAITING) { + // If we stopped inspection while the flashlight was on, turn it off + if (flashlight_intensity_current_ != 0) { + flashlight_intensity_current_ = 0.0; + Flashlight(flashlight_intensity_current_); + } + return; + } result_.inspection_result.push_back(isaac_msgs::InspectionResult::PIC_ACQUIRED); result_.picture_time.push_back(msg->header.stamp); @@ -844,7 +851,11 @@ class InspectionNode : public ff_util::FreeFlyerNodelet { case STATE::INIT_INSPECTION: msg.fsm_state = "INIT_INSPECTION"; break; case STATE::MOVING_TO_APPROACH_POSE: - msg.fsm_state = "MOVING_TO_APPROACH_POSE"; break; + msg.fsm_state = "MOVING_TO_APPROACH_POSE "; + msg.fsm_state += std::to_string(inspection_->GetCurrentCounter()); + msg.fsm_state += "/"; + msg.fsm_state += std::to_string(inspection_->GetSurveySize()); + break; case STATE::VISUAL_INSPECTION: msg.fsm_state = "VISUAL_INSPECTION"; break; case STATE::RETURN_INSPECTION: diff --git a/astrobee/survey/survey_dependencies/ros2_planning_system b/astrobee/survey/survey_dependencies/ros2_planning_system index e9c14625..11302c00 160000 --- a/astrobee/survey/survey_dependencies/ros2_planning_system +++ b/astrobee/survey/survey_dependencies/ros2_planning_system @@ -1 +1 @@ -Subproject commit e9c14625cd9d0c609f59837dc979eca6d74202d9 +Subproject commit 11302c0069fb0165bc63b98bf8981e34758f7e48 diff --git a/astrobee/survey/survey_manager/data/granite_survey_static.yaml b/astrobee/survey/survey_manager/data/granite_survey_static.yaml index 6b84411f..52229c3d 100644 --- a/astrobee/survey/survey_manager/data/granite_survey_static.yaml +++ b/astrobee/survey/survey_manager/data/granite_survey_static.yaml @@ -45,6 +45,8 @@ bays_move: gra_bay5: ["-pos", "0.1 0.1 -0.68", "-att", "3.14 1 0 0"] gra_bay6: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"] gra_bay7: ["-pos", "0.1 -0.5 -0.68", "-att", "3.14 1 0 0"] + berth1: ["-pos", "0.1 0.3 -0.68"] + berth2: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"] bays_pano: diff --git a/astrobee/survey/survey_manager/pddl/domain_survey.pddl b/astrobee/survey/survey_manager/pddl/domain_survey.pddl index 00f50c7d..0adbe252 100644 --- a/astrobee/survey/survey_manager/pddl/domain_survey.pddl +++ b/astrobee/survey/survey_manager/pddl/domain_survey.pddl @@ -107,7 +107,7 @@ (:durative-action dock :parameters (?robot - robot ?from ?to - location) ;; from bay7 to berth1 or berth2 - :duration (= ?duration 30) + :duration (= ?duration 90) :condition (and ;; Check robot mutex diff --git a/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py b/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py index b3b0c1fb..eb16ce9c 100755 --- a/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py +++ b/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py @@ -318,7 +318,10 @@ def thread_read_input(self, process): loginfo(f"reader exiting on exception: {e}") def send_command(self, command): - loginfo(f"send_command: {command}") + formatted_command = " ".join( + f'"{arg}"' if " " in arg else arg for arg in command + ) + loginfo(f"send_command: {formatted_command}") return_code = 1 try: @@ -385,8 +388,6 @@ def send_command(self, command): return return_code def send_command_recursive(self, command): - loginfo(f"Sending recursive command: {command}") - exit_code = self.send_command(command) loginfo("send_command exit code " + str(exit_code)) @@ -689,6 +690,12 @@ def survey_manager_executor(args, run, config_static, process_executor, quick: b exit_code = sm_exec.move(args["from_name"], args["to_name"]) elif args["type"] == "panorama": + # In some cases if the robot is not at the module position + # it might not perform a localization maneuver + exit_code = first_non_zero( + exit_code, sm_exec.move(args["location_name"], args["location_name"]) + ) + exit_code = first_non_zero( exit_code, command_executor.start_recording(