Skip to content

Commit

Permalink
Isaac13 qol fixes + release 0.2.8 (#160)
Browse files Browse the repository at this point in the history
* don't update focus if not needed + reset focus start panorama

* make monitor exit cleanly

* force stop if something happens to retry command; declutter output

* update submodule

* making berth names unique such that pos is not overwritten

* make move output not flood the entire output

* adding another \r

* upping fsw version to only have one PR

* reset focus for geometry too
  • Loading branch information
marinagmoreira authored Apr 2, 2024
1 parent f5cc1b4 commit 2550c10
Show file tree
Hide file tree
Showing 12 changed files with 151 additions and 84 deletions.
5 changes: 5 additions & 0 deletions RELEASE.md
Original file line number Diff line number Diff line change
@@ -1,5 +1,10 @@
# Releases

## Release 0.2.8

* Improvements to survey_manager output
* Bug fixes

## Release 0.2.7

* Survey manager
Expand Down
31 changes: 23 additions & 8 deletions astrobee/behaviors/inspection/src/inspection_node.cc
Original file line number Diff line number Diff line change
Expand Up @@ -482,9 +482,9 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
pub_guest_sci_.publish(cmd);
}

void SendPicture(double focus_distance) {
ROS_DEBUG_STREAM("Send picture");
// Take picture

void SendSciCamCommand(std::string command) {
// Chnage focus
ff_msgs::CommandArg arg;
std::vector<ff_msgs::CommandArg> cmd_args;

Expand All @@ -496,7 +496,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
cmd_args.push_back(arg);

arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING;
arg.s = "{\"name\": \"takePicture\", \"haz_dist\": " + std::to_string(focus_distance) +"}";
arg.s = command;
cmd_args.push_back(arg);

ff_msgs::CommandStamped cmd;
Expand All @@ -508,7 +508,12 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
cmd.args = cmd_args;

pub_guest_sci_.publish(cmd);
}


void SendPicture(double focus_distance) {
ROS_DEBUG_STREAM("Send picture");
SendSciCamCommand("{\"name\": \"takePicture\", \"haz_dist\": " + std::to_string(focus_distance) +"}");
// Timer for the sci cam camera
sci_cam_timeout_.start();
}
Expand Down Expand Up @@ -571,6 +576,8 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
if (flashlight_intensity_current_ != cfg_.Get<double>("toggle_flashlight")) {
flashlight_intensity_current_ = cfg_.Get<double>("toggle_flashlight");
Flashlight(flashlight_intensity_current_);
sci_cam_req_ = 1;
SendPicture(-1);
} else {
// Move on in focus distance iteration
flashlight_intensity_current_ = 0.0;
Expand All @@ -588,9 +595,9 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
// When the anomaly detection returns a result it will go to the next inspection
finished_anomaly_ = true;
}
sci_cam_req_ = 1;
SendPicture(focus_distance_current_);
}
sci_cam_req_ = 1;
SendPicture(focus_distance_current_);
} else {
return fsm_.Update(NEXT_INSPECT);
}
Expand Down Expand Up @@ -748,14 +755,22 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
// Geometry command
case isaac_msgs::InspectionGoal::GEOMETRY:
NODELET_DEBUG("Received Goal Geometry");
if (inspection_->GenerateGeometrySurvey(goal_.inspect_poses))
if (inspection_->GenerateGeometrySurvey(goal_.inspect_poses)) {
// Reset the focus distance
SendSciCamCommand("{\"name\": \"setFocusDistance\", \"distance\": " +
std::to_string(cfg_.Get<double>("sci_cam_startup_focus")) + "}");
return fsm_.Update(GOAL_INSPECT);
}
break;
// Panorama command
case isaac_msgs::InspectionGoal::PANORAMA:
NODELET_DEBUG("Received Goal Panorama");
if (inspection_->GeneratePanoramaSurvey(goal_.inspect_poses))
if (inspection_->GeneratePanoramaSurvey(goal_.inspect_poses)) {
// Reset the focus distance
SendSciCamCommand("{\"name\": \"setFocusDistance\", \"distance\": " +
std::to_string(cfg_.Get<double>("sci_cam_startup_focus")) + "}");
return fsm_.Update(GOAL_INSPECT);
}
break;
// Volumetric command
case isaac_msgs::InspectionGoal::VOLUMETRIC:
Expand Down
2 changes: 1 addition & 1 deletion astrobee/survey/survey_dependencies/ros1_lifecycle
Original file line number Diff line number Diff line change
Expand Up @@ -45,14 +45,18 @@ 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"]
berth1_g: ["-pos", "0.1 0.3 -0.68"]
berth2_g: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"]


bays_pano:
gra_bay2: "panorama_granite_bsharp.txt"
gra_bay6: "panorama_granite_wannabee.txt"

berth:
berth1_g: "1"
berth2_g: "2"

bogus_bays: [gra_bay0, gra_bay8]
berths: [berth1, berth2]
robots: [bsharp, wannabee]
Expand Down
8 changes: 4 additions & 4 deletions astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ goals:
- {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3}
# This is one of the goals we previously had to comment out for POPF to return a halfway decent
# plan. Adding a let_other_robot_reach goal mostly fixed the problem.
- {type: robot_at, robot: bumble, location: berth1}
- {type: robot_at, robot: bumble, location: berth1_g}

# This let_other_robot_reach goal is effectively a very specific kind of between-robot ordering
# constraint. It tells honey to let bumble get to bay 5 before taking its first panorama. Without
Expand All @@ -45,10 +45,10 @@ goals:
- {type: panorama, robot: honey, order: 3, location: jem_bay5}
# This is the other objective we previously had to comment out for POPF to return a decent plan.
- {type: stereo, robot: honey, order: 4, trajectory: jem_bay7_to_bay4}
- {type: robot_at, robot: honey, location: berth2}
- {type: robot_at, robot: honey, location: berth2_g}

init:
bumble:
location: berth1
location: berth1_g
honey:
location: berth2
location: berth2_g
Original file line number Diff line number Diff line change
Expand Up @@ -7,16 +7,16 @@ set instance gra_bay5 location
set instance gra_bay6 location
set instance gra_bay7 location
set instance gra_bay8 location
set instance berth1 location
set instance berth2 location
set instance berth1_g location
set instance berth2_g location
set instance bsharp robot
set instance wannabee robot
set instance o0 order
set instance o1 order
set instance o2 order
set instance o3 order
set instance o4 order
set goal (and (completed-panorama bsharp o0 gra_bay2) (completed-stereo bsharp o1 gra_bay1 gra_bay3) (robot-at bsharp berth1) (completed-panorama wannabee o1 gra_bay6) (completed-stereo wannabee o4 gra_bay5 gra_bay7) (robot-at wannabee berth2))
set goal (and (completed-panorama bsharp o0 gra_bay2) (completed-stereo bsharp o1 gra_bay1 gra_bay3) (robot-at bsharp berth1_g) (completed-panorama wannabee o1 gra_bay6) (completed-stereo wannabee o4 gra_bay5 gra_bay7) (robot-at wannabee berth2_g))
set predicate (move-connected gra_bay0 gra_bay1)
set predicate (move-connected gra_bay1 gra_bay0)
set predicate (move-connected gra_bay1 gra_bay2)
Expand All @@ -40,8 +40,8 @@ set predicate (location-real gra_bay4)
set predicate (location-real gra_bay5)
set predicate (location-real gra_bay6)
set predicate (location-real gra_bay7)
set predicate (dock-connected gra_bay3 berth1)
set predicate (dock-connected gra_bay5 berth2)
set predicate (dock-connected gra_bay3 berth1_g)
set predicate (dock-connected gra_bay5 berth2_g)
set predicate (robots-different bsharp wannabee)
set predicate (robots-different wannabee bsharp)
set predicate (locations-different gra_bay0 gra_bay1)
Expand Down Expand Up @@ -118,8 +118,8 @@ set predicate (locations-different gra_bay8 gra_bay6)
set predicate (locations-different gra_bay8 gra_bay7)
set predicate (robot-available bsharp)
set predicate (robot-available wannabee)
set predicate (robot-at bsharp berth1)
set predicate (robot-at wannabee berth2)
set predicate (robot-at bsharp berth1_g)
set predicate (robot-at wannabee berth2_g)
set predicate (location-available gra_bay0)
set predicate (location-available gra_bay1)
set predicate (location-available gra_bay2)
Expand Down
3 changes: 2 additions & 1 deletion astrobee/survey/survey_manager/src/isaac_action_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,8 @@ void IsaacAction::do_work() {
progress_ = (ros::Time::now() - start_time_).toSec() / action_duration_;
send_feedback(progress_, command_ + " running");

printf("\t ** %s [%5.1f%%] \n", command_.c_str(), progress_ * 100.0);
// Status gets printed on terminal
// printf("\t ** %s [%5.1f%%] \n", command_.c_str(), progress_ * 100.0);=
int status;
int result = waitpid(-1, &status, WNOHANG);
// printf("Result: %d %d %d\n", result, pid_, status);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -183,26 +183,27 @@ def __del__(self):
self.sock_output.close()

def write_output_once(self, output):
while not self.sock_output_connected:
while not self._stop_event.is_set():
try:
# If socket is not connected try to connect
self.sock_output_conn, addr = self.sock_output.accept()
self.sock_output_conn.setblocking(False)
if not self.sock_output_connected:
# If socket is not connected try to connect
self.sock_output_conn, addr = self.sock_output.accept()
self.sock_output_conn.setblocking(False)

self.sock_output_connected = True
self.sock_output_connected = True
except socket.timeout:
continue

try:
if self.sock_output_connected:
encoded_message = output.encode("ascii", errors="replace")
for i in range(0, len(encoded_message), CHUNK_SIZE):
chunk = encoded_message[i : i + CHUNK_SIZE]
self.sock_output_conn.sendall(chunk)

except (socket.error, BrokenPipeError):
loginfo("Error sending data. Receiver may have disconnected.")
self.sock_output_connected = False
try:
if self.sock_output_connected:
encoded_message = output.encode("ascii", errors="replace")
for i in range(0, len(encoded_message), CHUNK_SIZE):
chunk = encoded_message[i : i + CHUNK_SIZE]
self.sock_output_conn.sendall(chunk)
break
except (socket.error, BrokenPipeError):
loginfo("Error sending data. Receiver may have disconnected.")
self.sock_output_connected = False

def thread_write_output(self, process):
# loginfo("starting thread_write_output...")
Expand All @@ -218,13 +219,13 @@ def thread_write_output(self, process):
if output == "":
continue
if output and not output.startswith("pos: x:"):
loginfo(f"writer received: {output}")
output_total += output

try:
# If socket is not connected try to connect
if not self.sock_output_connected:
# loginfo("trying to connect")
if not output.startswith("pos: x:"):
loginfo(f"writer received: {output}")
self.sock_output_conn, addr = self.sock_output.accept()
self.sock_output_conn.setblocking(False)

Expand Down Expand Up @@ -258,9 +259,7 @@ def read_input_once(self) -> str:
# loginfo("waiting for connection")
try:
self.sock_input_conn, addr = self.sock_input.accept()
self.sock_input_conn.settimeout(
1
) # Set a timeout for socket operations
self.sock_input_conn.settimeout(1)
self.sock_input_connected = True
break
except socket.timeout:
Expand All @@ -270,11 +269,17 @@ def read_input_once(self) -> str:
request = self.sock_input_conn.recv(CHUNK_SIZE).decode(
"ascii", errors="replace"
)
return request
if request:
return request
else:
self.sock_input_connected = False
break
loginfo("request")
except socket.timeout:
continue
except ConnectionResetError:
# Connection was reset, set sock_input_connected to False
loginfo("disconnected")
self.sock_input_connected = False
break
return ""
Expand Down Expand Up @@ -311,6 +316,8 @@ def thread_read_input(self, process):
# If broken pipe connect
if not request:
break
if request == "stop":
return
loginfo("reader sending: " + request)
process.stdin.write(request + "\n")
process.stdin.flush()
Expand Down Expand Up @@ -347,7 +354,11 @@ def send_command(self, command):
output_thread.start()

# When the process finishes, te output thread automatically closes
while output_thread.is_alive() and process.poll() is None:
while (
output_thread.is_alive()
and input_thread.is_alive()
and process.poll() is None
):
rospy.sleep(1)

except Exception as e:
Expand Down Expand Up @@ -802,11 +813,12 @@ def survey_manager_executor_recursive(
)

while exit_code != 0 and not rospy.is_shutdown():
loginfo(f"Exit code non-zero: Do you want to repeat the survey? (yes/no/skip):")
process_executor.write_output_once(
"Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): \n"
)
repeat = process_executor.read_input_once().lower()

loginfo(f"Got response: {repeat}")
if repeat == "yes":
run_number += 1
exit_code = survey_manager_executor_recursive(
Expand Down
Loading

0 comments on commit 2550c10

Please sign in to comment.