Add quick:=true option for sim.launch (#148)

* fixing inspection tool reading input and exiting * better performance in simulation * fixing exit 0 * cleaning out output * adding localization attitudes to jem static * - sim.launch: Add quick:=true argument, switching to short versions of longer actions. - command_astrobee: Add final auto-move after stereo and pano actions to canonicalize attitude and get nav features. - command_astrobee: Add auto-move to berth approach point prior to docking to avoid failures * Take more care with exit codes * Fix inspection_tool exit code * Switch to astrobee_ops plan commands for stereo survey --------- Co-authored-by: Marina Moreira <[email protected]>
nasa · Feb 22, 2024 · d0cc317 · d0cc317
1 parent dd27f6a
commit d0cc317
Show file tree

Hide file tree

Showing 15 changed files with 362 additions and 180 deletions.
diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc
@@ -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
@@ -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)) {
@@ -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);
@@ -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();
     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;
 }
diff --git a/astrobee/description/description/urdf/sensor_RFID_receiver.urdf.xacro b/astrobee/description/description/urdf/sensor_RFID_receiver.urdf.xacro
@@ -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>

diff --git a/astrobee/description/description/urdf/sensor_RFID_transmitter.urdf.xacro b/astrobee/description/description/urdf/sensor_RFID_transmitter.urdf.xacro
@@ -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>
@@ -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>

diff --git a/astrobee/description/description/urdf/sensor_wifi_receiver.urdf.xacro b/astrobee/description/description/urdf/sensor_wifi_receiver.urdf.xacro
@@ -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>

diff --git a/astrobee/description/description/urdf/sensor_wifi_transmitter.urdf.xacro b/astrobee/description/description/urdf/sensor_wifi_transmitter.urdf.xacro
@@ -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>
@@ -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>

diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml
@@ -38,30 +38,33 @@ 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"]
   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_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_to_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "0 0 1 0"]
+  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 0 1"]
+  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 0 1"]
   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"]
   usl_bay3: ["-pos", "2.6 0 4.8", "-att", "0 0 0 1"]
   usl_bay4: ["-pos", "1.55 0 4.8", "-att", "0 0 0 1"]
   usl_bay5: ["-pos", "0.5 0 4.8", "-att", "0 0 0 1"]
   usl_bay6: ["-pos", "-0.5 0 4.8", "-att", "0 0 0 1"]
+  # berth approach points from gds_configs/IssWorld/BookmarksList.json
+  berth1: ["-pos", "10.5 -9.25 4.5", "-att", "0 0 0 1"]
+  berth2: ["-pos", "10.5 -9.75 4.5", "-att", "3.14 1 0 0"]
 
 bays_pano:
   jem_bay1: "isaac9/jem_bay1_std_panorama.txt"

diff --git a/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h b/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h
@@ -28,7 +28,7 @@ namespace plansys2_actions {
 
 class IsaacAction : public plansys2::ActionExecutorClient {
  public:
-  IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate);
+  IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate, bool quick);
   ~IsaacAction();
 
  protected:
@@ -40,6 +40,7 @@ class IsaacAction : public plansys2::ActionExecutorClient {
   std::string robot_name_, action_name_;
   int pid_;
   std::string command_;
+  bool quick_;
 };
 }  // namespace plansys2_actions
 

diff --git a/astrobee/survey_manager/survey_planner/launch/survey.launch b/astrobee/survey_manager/survey_planner/launch/survey.launch
@@ -6,6 +6,7 @@
   <arg name="lifecycle" default="false" />  <!-- lifecycle node -->
   <arg name="robot1" default="" />  <!-- lifecycle node -->
   <arg name="robot2" default="honey" />  <!-- lifecycle node -->
+  <arg name="quick" default="false" /> <!-- abbreviate longer actions for faster testing -->
 
 
   <arg name="model_file" default="$(find survey_planner)/pddl/domain_survey.pddl"/>
@@ -48,29 +49,29 @@
   </group>
 
   <!-- Actions -->
-  <node name="move_action_node1" pkg="survey_planner" type="move_action_node" output="screen">
+  <node name="move_action_node1" pkg="survey_planner" type="move_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
-  <node name="move_action_node2" pkg="survey_planner" type="move_action_node" output="screen">
+  <node name="move_action_node2" pkg="survey_planner" type="move_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
 
-  <node name="dock_action_node1" pkg="survey_planner" type="dock_action_node" output="screen">
+  <node name="dock_action_node1" pkg="survey_planner" type="dock_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
-  <node name="dock_action_node2" pkg="survey_planner" type="dock_action_node" output="screen">
+  <node name="dock_action_node2" pkg="survey_planner" type="dock_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
 
-  <node name="undock_action_node1" pkg="survey_planner" type="undock_action_node" output="screen">
+  <node name="undock_action_node1" pkg="survey_planner" type="undock_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
-  <node name="undock_action_node2" pkg="survey_planner" type="undock_action_node" output="screen">
+  <node name="undock_action_node2" pkg="survey_planner" type="undock_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
 
-  <node name="panorama_action_node1" pkg="survey_planner" type="panorama_action_node" output="screen">
+  <node name="panorama_action_node1" pkg="survey_planner" type="panorama_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
-  <node name="panorama_action_node2" pkg="survey_planner" type="panorama_action_node" output="screen">
+  <node name="panorama_action_node2" pkg="survey_planner" type="panorama_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
 
-  <node name="stereo_action_node1" pkg="survey_planner" type="stereo_action_node" output="screen">
+  <node name="stereo_action_node1" pkg="survey_planner" type="stereo_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
-  <node name="stereo_action_node2" pkg="survey_planner" type="stereo_action_node" output="screen">
+  <node name="stereo_action_node2" pkg="survey_planner" type="stereo_action_node" output="screen" args="$(eval '--quick' if arg('quick') else '')">
   </node>
 </launch>
 
diff --git a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp
@@ -148,8 +148,9 @@ double get_action_duration(const std::string& action_name) {
   return duration;
 }
 
-IsaacAction::IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate)
-    : ActionExecutorClient(nh, action, rate) {
+IsaacAction::IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate,
+                         bool quick)
+    : ActionExecutorClient(nh, action, rate), quick_(quick) {
   action_name_ = action;
   progress_ = 0.0;
   pid_ = 0;
@@ -174,7 +175,8 @@ void IsaacAction::do_work() {
       args_str += " " + arg;
     }
     command_ = std::string("(") + args_str + ")";
-    std::string command_astrobee_call = std::string("rosrun survey_planner command_astrobee ") + args_str;
+    std::string quick_str = quick_ ? "--quick " : "";
+    std::string command_astrobee_call = std::string("rosrun survey_planner command_astrobee ") + quick_str + args_str;
 
     start_time_ = ros::Time::now();
     pid_ = fork();
@@ -241,6 +243,12 @@ int isaac_action_main(int argc, char *argv[], const char* action_name) {
   // Initialize a ros node
   ros::init(argc, argv, (std::string(action_name) + "_action").c_str());
 
+  bool quick = false;
+  if (argc == 2 && std::string(argv[1]) == "--quick") {
+    printf("isaac_action_node[%s]: Got --quick; running longer actions in quick mode\n", action_name);
+    quick = true;
+  }
+
   std::string name = ros::this_node::getName();
   if (name.empty() || (name.size() == 1 && name[0] == '/'))
     name = "default";
@@ -253,7 +261,7 @@ int isaac_action_main(int argc, char *argv[], const char* action_name) {
   // Start action node
   // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner
   // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41)
-  auto action_node = std::make_shared<plansys2_actions::IsaacAction>(nh, action_name,  std::chrono::seconds(2));
+  auto action_node = std::make_shared<plansys2_actions::IsaacAction>(nh, action_name,  std::chrono::seconds(2), quick);
   action_node->trigger_transition(ros::lifecycle::CONFIGURE);
 
   // Synchronous mode