Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixes planning performance #147

Merged
merged 9 commits into from
Feb 22, 2024
Merged

Fixes planning performance #147

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
d10e426
fixing inspection tool reading input and exiting
marinagmoreira Feb 17, 2024
39106d1
better performance in simulation
marinagmoreira Feb 17, 2024
2ccbce0
fixing exit 0
marinagmoreira Feb 17, 2024
98e07cd
cleaning out output
marinagmoreira Feb 17, 2024
209d76e
adding localization attitudes to jem static
marinagmoreira Feb 17, 2024
d5692b8
Fix inspection_tool exit code
trey0 Feb 20, 2024
b7e8a1b
updating submodule
marinagmoreira Feb 22, 2024
7b2a4f1
adding comment and reducing timer
marinagmoreira Feb 22, 2024
50c2c68
Merge branch 'develop' of github.com:nasa/isaac into fixes_planning_p…
marinagmoreira Feb 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 18 additions & 41 deletions astrobee/behaviors/inspection/tools/inspection_tool.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -271,8 +271,9 @@ void ResultCallback(ff_util::FreeFlyerActionState::Enum code,

teardown:
std::cout << std::endl;
stopflag_ = true;
ros::shutdown();
bool success = (code == ff_util::FreeFlyerActionState::Enum::SUCCESS);
exit(success ? EXIT_SUCCESS : EXIT_FAILURE);
}

// Send the inspection goal to the server
Expand Down Expand Up @@ -311,37 +312,10 @@ void SendGoal(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
client->SendGoal(goal);
}

bool GetlineAsync(std::istream& is, std::string& str, char delim = '\n') {
static std::string linesofar;
char inchar;
int charsread = 0;
bool lineread = false;
str = "";

do {
charsread = is.readsome(&inchar, 1);
if (charsread == 1) {
// if the delimiter is read then return the string so far
if (inchar == delim) {
str = linesofar;
linesofar = "";
lineread = true;
} else { // otherwise add it to the string so far
linesofar.append(1, inchar);
}
}
} while (charsread != 0 && !lineread && !!stopflag_);

return lineread;
}

void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *client) {
while (!stopflag_ && ros::ok()) {
while (ros::ok()) {
std::string line, val;

if (!GetlineAsync(std::cin, line))
continue;

std::getline(std::cin, line);
std::string s;
try {
switch (std::stoi(line)) {
Expand All @@ -352,7 +326,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
if (s.size() < 80) s.append(80 - s.size(), ' ');
std::cout << s << std::endl;
stopflag_ = true;
break;
return;
case 1:
FLAGS_pause = true;
SendGoal(client);
Expand Down Expand Up @@ -538,24 +512,27 @@ int main(int argc, char *argv[]) {
boost::thread inp(GetInput, &client);

if (FLAGS_remote) {
ros::Rate loop_rate(10);
ros::Time start_time = ros::Time::now();

// Spin for 3 seconds
while (ros::Time::now() - start_time < ros::Duration(3.0))
loop_rate.sleep();

ros::Duration(3.0).sleep();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It seems worthwhile to log a message saying that it is sleeping (and why).

I'm assuming this is somehow related to ensuring the goal is delivered to the remote robot through the generic bridge? If so, it's not clear to me why it would help. I thought that the bridge didn't start establishing connections for a new message topic/type until the first message was received, so a delay in between creating the publisher and publishing the first message wouldn't help.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'll add a comment on this. When you declare a publisher you have to sleep a bit before publishing a message. This is not an issue when there is a connected_callback involved, but since we're doing remote commanding and waving that requirement, without this, the goal is not published. I can also lower the time involved, in retrospect 3s is too much..

SendGoal(&client);
}
// Synchronous mode
while (!stopflag_) {
while (ros::ok() && !stopflag_) {
ros::spinOnce();
}
// Finish commandline flags
google::ShutDownCommandLineFlags();

// Clean up threads and flush streams
if (!stopflag_) {
const char* msg = "inspection_tool: Exiting when stopflag not set, exit code 1";
ROS_ERROR("%s", msg);
fprintf(stderr, "%s\n", msg);
exit(1);
}

// Wait for thread to exit
inp.join();

// Finish commandline flags
google::ShutDownCommandLineFlags();
// Make for great success
return 0;
}
2 changes: 1 addition & 1 deletion astrobee/description/description/urdf/sensor_RFID_receiver.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
<gazebo reference="${prefix}body">
<sensor name="RFID_receiver" type="rfid">
<always_on>1</always_on>
<update_rate>62.5</update_rate>
<update_rate>1</update_rate>
<visualize>true</visualize>
<plugin name="wifi_pub" filename="libgazebo_sensor_plugin_RFID_receiver.so">
<namespace>/${ns}/</namespace>
Expand Down
4 changes: 2 additions & 2 deletions astrobee/description/description/urdf/sensor_RFID_transmitter.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@
<gazebo reference="body">
<sensor name="${essid}" type="rfidtag">
<always_on>1</always_on>
<update_rate>62.5</update_rate>
<update_rate>1</update_rate>
<visualize>true</visualize>
<plugin name="trans_ros" filename="libgazebo_sensor_plugin_RFID_tag.so"/>
</sensor>
Expand All @@ -46,7 +46,7 @@
<gazebo>
<static>true</static>
<plugin name="truth_ros" filename="libgazebo_model_plugin_truth.so">
<rate>62.5</rate>
<rate>1</rate>
<parent>world</parent>
<child>body</child>
<tf>false</tf>
Expand Down
2 changes: 1 addition & 1 deletion astrobee/description/description/urdf/sensor_wifi_receiver.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
<sensor name="wifi_receiver" type="wireless_receiver">
<pose>0 0 0 0 0 0</pose>
<always_on>true</always_on>
<update_rate>62.5</update_rate>
<update_rate>5</update_rate>
<visualize>true</visualize>
<!-- <transceiver>
<min_frequency>2412.0</min_frequency>
Expand Down
4 changes: 2 additions & 2 deletions astrobee/description/description/urdf/sensor_wifi_transmitter.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@
<gazebo reference="body">
<sensor name="${essid}" type="wireless_transmitter">
<always_on>1</always_on>
<update_rate>62.5</update_rate>
<update_rate>5</update_rate>
<visualize>true</visualize>
<transceiver>
<essid>${essid}</essid>
Expand All @@ -52,7 +52,7 @@
<gazebo>
<static>true</static>
<plugin name="truth_ros" filename="libgazebo_model_plugin_truth.so">
<rate>62.5</rate>
<rate>5</rate>
<parent>world</parent>
<child>body</child>
<tf>false</tf>
Expand Down
26 changes: 13 additions & 13 deletions astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,23 +38,23 @@ bays:
jem_bay7: [11.0, -9.7, 4.8]

bays_move:
jem_bay1: ["-pos", "11 -4 4.8", "-att", "0 0 0 1"]
jem_bay2: ["-pos", "11 -5 4.8", "-att", "0 0 0 1"]
jem_bay3: ["-pos", "11 -6 4.8", "-att", "0 0 0 1"]
jem_bay4: ["-pos", "11 -7 4.8", "-att", "0 0 0 1"]
jem_bay5: ["-pos", "11 -8 4.8", "-att", "0 0 0 1"]
jem_bay6: ["-pos", "11 -9 4.8", "-att", "0 0 0 1"]
jem_bay7: ["-pos", "11 -9.7 4.8", "-att", "0 0 0 1"]
jem_hatch_to_nod2: ["-pos", "11 -3.5 4.8", "-att", "0 0 1 90"]
jem_hatch_from_nod2: ["-pos", "11 -3.5 4.8", "-att", "0 0 1 -90"]
nod2_hatch_from_jem: ["-pos", "11 -1.0 4.8", "-att", "0 0 1 90"]
nod2_hatch_to_jem: ["-pos", "11 -1.0 4.8", "-att", "0 0 1 -90"]
jem_bay1: ["-pos", "11 -4 4.8", "-att", "1.5708 0 0 1"]
jem_bay2: ["-pos", "11 -5 4.8", "-att", "1.5708 0 0 1"]
jem_bay3: ["-pos", "11 -6 4.8", "-att", "1.5708 0 0 1"]
jem_bay4: ["-pos", "11 -7 4.8", "-att", "-1.5708 0 0 1"]
jem_bay5: ["-pos", "11 -8 4.8", "-att", "-1.5708 0 0 1"]
jem_bay6: ["-pos", "11 -9 4.8", "-att", "-1.5708 0 0 1"]
jem_bay7: ["-pos", "11 -9.7 4.8", "-att", "-1.5708 0 0 1"]
jem_hatch_to_nod2: ["-pos", "11 -3.5 4.8", "-att", "1.5708 0 0 1"]
jem_hatch_from_nod2: ["-pos", "11 -3.5 4.8", "-att", "-1.5708 0 0 1"]
nod2_hatch_from_jem: ["-pos", "11 -1.0 4.8", "-att", "1.5708 0 0 1"]
nod2_hatch_to_jem: ["-pos", "11 -1.0 4.8", "-att", "-1.5708 0 0 1"]
marinagmoreira marked this conversation as resolved.
Show resolved Hide resolved
nod2_bay2: ["-pos", "11 0 4.8", "-att", "0 0 0 1"]
nod2_bay3: ["-pos", "10 0 4.8", "-att", "0 0 0 1"]
nod2_bay4: ["-pos", "9 0 4.8", "-att", "0 0 0 1"]
nod2_hatch_to_usl: ["-pos", "7.8 -3.5 4.8", "-att", "0 0 1 180"]
nod2_hatch_to_usl: ["-pos", "7.8 -3.5 4.8", "-att", "3.14 0 0 1"]
nod2_hatch_from_usl: ["-pos", "7.8 -3.5 4.8", "-att", "0 0 1 0"]
usl_hatch_from_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "0 0 1 180"]
usl_hatch_from_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "3.14 0 0 1"]
usl_hatch_to_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "0 0 1 0"]
usl_bay1: ["-pos", "4.7 0 4.8", "-att", "0 0 0 1"]
usl_bay2: ["-pos", "3.65 0 4.8", "-att", "0 0 0 1"]
Expand Down
6 changes: 3 additions & 3 deletions astrobee/survey_manager/survey_planner/src/survey_planner/command_astrobee.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -366,7 +366,7 @@ def send_command_recursive(self, command):

while exit_code != 0 and not rospy.is_shutdown():
self.write_output_once(
"Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): "
"Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): \n"
)
repeat = self.read_input_once().lower()
loginfo(f"user input: {repeat}")
Expand Down Expand Up @@ -394,12 +394,12 @@ def __init__(self, ns):
)
self.ack_needed = False
self.ack_msg = None
self.plan_status_needed = False
self.sub_plan_status = rospy.Subscriber(
self.ns + "/mgt/executive/plan_status",
PlanStatusStamped,
self.plan_status_callback,
)
self.plan_status_needed = False
self.plan_name = ""
self.pub_command = rospy.Publisher(
self.ns + "/command", CommandStamped, queue_size=5
Expand Down Expand Up @@ -648,7 +648,7 @@ def survey_manager_executor_recursive(

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

Expand Down
20 changes: 13 additions & 7 deletions astrobee/survey_manager/survey_planner/src/survey_planner/monitor_astrobee.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,35 +27,40 @@

# Constants
CHUNK_SIZE = 1024

# Declare event that will stop input thread
stop_event = threading.Event()


def thread_write_input(input_path):
print("Start thread_write_input")

user_input = ""
while not stop_event.is_set():
try:
# Attempt to connect to the server
sock_client_input = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
sock_client_input.connect(input_path)
except ConnectionRefusedError:
print("Input Connection refused. Retrying in 5 seconds...")
# print("Input Connection refused. Retrying in 5 seconds...")
time.sleep(5)
continue

try:
while not stop_event.is_set():
user_input = input()
print("user input: " + user_input)
if user_input == "":
user_input = input()
# print("user input: " + user_input)
if user_input.lower().strip() == "exit":
stop_event.set()
break
sock_client_input.send(
user_input.encode("ascii", errors="replace")[:CHUNK_SIZE]
)
user_input = ""
except Exception as e:
print("Exception in thread_write_input:", e)
# print("Exception in thread_write_input:", e)
pass

# Close the sockets
sock_client_input.close()
Expand All @@ -71,23 +76,24 @@ def thread_read_output(output_path):
sock_client_output.connect(output_path)

except ConnectionRefusedError:
print("Output Connection refused. Retrying in 5 seconds...")
# print("Output Connection refused. Retrying in 5 seconds...")
time.sleep(5)
continue

print("connected")

try:
while not stop_event.is_set():
print("get data")
# print("get data")
request = sock_client_output.recv(CHUNK_SIZE)
request = request.decode("ascii", errors="replace")
print(request, end="")
if not request: # If no data received, it means the server disconnected
print("Server disconnected")
break
except Exception as e:
print("Exception in thread_read_output:", e)
# print("Exception in thread_read_output:", e)
pass

# Close the sockets
sock_client_output.close()
Expand Down
2 changes: 2 additions & 0 deletions isaac/launch/isaac_astrobee.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
<arg name="wifi" default="false" /> <!-- Enable Wifi -->
<arg name="streaming_mapper" default="false" /> <!-- Enable the streaming mapper -->
<arg name="acoustics_cam" default="false" /> <!-- Enable the acoustics camera -->
<arg name="vol_mapper" default="false" /> <!-- Enable volume mappers -->

<!-- Remaining options -->
<arg name="output" default="log" /> <!-- Output to screen or log -->
Expand Down Expand Up @@ -260,6 +261,7 @@
<arg name="wifi" value="$(arg wifi)" /> <!-- Enable wifi -->
<arg name="streaming_mapper" value="$(arg streaming_mapper)" /> <!-- Enable the streaming mapper -->
<arg name="acoustics_cam" value="$(arg acoustics_cam)" /> <!-- Enable the acoustics camera -->
<arg name="vol_mapper" value="$(arg vol_mapper)" /> <!-- Enable volume mappers -->
</include>
</group>

Expand Down
41 changes: 22 additions & 19 deletions isaac/launch/robot/GLP.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,17 +19,18 @@
<launch>

<!-- Additional options -->
<arg name="drivers"/> <!-- Should we launch drivers? -->
<arg name="spurn" default=""/> <!-- PRevent a specific node -->
<arg name="nodes" default=""/> <!-- Launch specific nodes -->
<arg name="extra" default=""/> <!-- Inject an additional node -->
<arg name="debug" default=""/> <!-- Debug a node set -->
<arg name="output" default="log"/> <!-- Where to log -->
<arg name="sim_mode" default="false" /> <!-- If in sim mode -->
<arg name="wifi" default="false" /> <!-- Enable Wifi -->
<arg name="drivers"/> <!-- Should we launch drivers? -->
<arg name="spurn" default=""/> <!-- PRevent a specific node -->
<arg name="nodes" default=""/> <!-- Launch specific nodes -->
<arg name="extra" default=""/> <!-- Inject an additional node -->
<arg name="debug" default=""/> <!-- Debug a node set -->
<arg name="output" default="log"/> <!-- Where to log -->
<arg name="sim_mode" default="false" /> <!-- If in sim mode -->
<arg name="wifi" default="false" /> <!-- Enable Wifi -->
<arg name="streaming_mapper" default="false" /><!-- Enable the streaming mapper -->
<arg name="acoustics_cam" default="false" /><!-- Enable the acoustics cam -->
<arg name="ros_gs_bridge" default="true" /><!-- Enable the ros guest science bridge -->
<arg name="acoustics_cam" default="false" /> <!-- Enable the acoustics cam -->
<arg name="vol_mapper" default="false" /> <!-- Enable volume mappers -->
<arg name="ros_gs_bridge" default="true" /> <!-- Enable the ros gs bridge -->

<!-- Launch all nodelet managers -->
<group if="$(eval optenv('ASTROBEE_NODEGRAPH','')=='')">
Expand All @@ -38,15 +39,17 @@
</group>

<!-- Launch dense map nodes -->
<include file="$(find vol_mapper)/launch/air_quality_mapper.launch">
<arg name="output" value="$(arg output)" />
</include>
<include file="$(find vol_mapper)/launch/RFID_mapper.launch">
<arg name="output" value="$(arg output)" />
</include>
<include file="$(find vol_mapper)/launch/wifi_mapper.launch">
<arg name="output" value="$(arg output)" />
</include>
<group if="$(arg vol_mapper)">
<include file="$(find vol_mapper)/launch/air_quality_mapper.launch">
<arg name="output" value="$(arg output)" />
</include>
<include file="$(find vol_mapper)/launch/RFID_mapper.launch">
<arg name="output" value="$(arg output)" />
</include>
<include file="$(find vol_mapper)/launch/wifi_mapper.launch">
<arg name="output" value="$(arg output)" />
</include>
</group>

<!-- Launch the streaming mapper -->
<include if="$(arg streaming_mapper)" file="$(find geometry_mapper)/launch/streaming_mapper.launch">
Expand Down
Loading
Loading