Survey manager execute goals script

astrobee/behaviors/inspection/CMakeLists.txt

@@ -106,7 +106,7 @@ target_link_libraries(test_pano
 #############
 
 # Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}
+install(TARGETS pano_survey ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

astrobee/behaviors/inspection/src/inspection_node.cc

@@ -368,7 +368,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     ff_msgs::MotionResultConstPtr const& result) {
     // Check for invalid results
     if (result == nullptr) {
-      ROS_ERROR_STREAM("Invalid result received Motion");
+      ROS_INFO_STREAM("Invalid result received Motion");
       return fsm_.Update(MOTION_FAILED);
     }
 
@@ -607,7 +607,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     if (result != nullptr)
       result_.anomaly_result.push_back(result->anomaly_result);
     else
-      ROS_ERROR_STREAM("Invalid result received Image Analysis");
+      ROS_INFO_STREAM("Invalid result received Image Analysis");
     return fsm_.Update(NEXT_INSPECT);
   }
 

astrobee/behaviors/inspection/tools/inspection_tool.cc

@@ -66,6 +66,7 @@ DEFINE_bool(anomaly, false, "Send the inspection command");
 DEFINE_bool(geometry, false, "Send the inspection command");
 DEFINE_bool(panorama, false, "Send the inspection command");
 DEFINE_bool(volumetric, false, "Send the inspection command");
+DEFINE_bool(remote, false, "Whether target command is remote robot");
 
 // General parameters
 DEFINE_string(camera, "sci_cam", "Camera to use");
@@ -113,6 +114,7 @@ DEFINE_double(deadline, -1.0, "Action deadline timeout");
 // Match the internal states and responses with the message definition
 using STATE = isaac_msgs::InspectionState;
 bool stopflag_ = false;
+std::string feedback_old = "";
 
 bool has_only_whitespace_or_comments(const std::string & str) {
   for (std::string::const_iterator it = str.begin(); it != str.end(); it++) {
@@ -185,7 +187,7 @@ geometry_msgs::PoseArray ReadPosesFile(std::string file) {
           quat_robot.setRPY(euler_roll * DEG2RAD, euler_pitch * DEG2RAD, euler_yaw * DEG2RAD);
 
         } else {
-          std::cout << "Ignoring invalid line: " << line  << std::endl;
+          // std::cout << "Ignoring invalid line: " << line  << std::endl;
           continue;
         }
       }
@@ -209,6 +211,8 @@ void FeedbackCallback(isaac_msgs::InspectionFeedbackConstPtr const& feedback) {
     + " -> " + feedback->state.fsm_state
     + " (" + feedback->state.fsm_subevent
     + " -> " + feedback->state.fsm_substate + ")";
+  if (s == feedback_old) return;
+  feedback_old = s;
   if (s.size() < 70) s.append(70 - s.size(), ' ');
   std::cout << "\r" << s.substr(0, 70) << "|Input: " << std::flush;
 }
@@ -307,10 +311,37 @@ 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()) {
     std::string line, val;
-    std::getline(std::cin, line);
+
+    if (!GetlineAsync(std::cin, line))
+      continue;
+
     std::string s;
     try {
       switch (std::stoi(line)) {
@@ -327,7 +358,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         case 2:
           FLAGS_pause = false;
@@ -344,7 +375,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing and repeating pose (needs resume)";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         case 4:
           FLAGS_pause = false;
@@ -354,7 +385,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing and skipping pose (needs resume)";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         case 5:
           FLAGS_pause = false;
@@ -365,7 +396,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing and saving (needs resume)";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         default:
           s = "\r Input: " + line + ") Invalid option";
@@ -424,20 +455,22 @@ int main(int argc, char *argv[]) {
   // Create a node handle
   ros::NodeHandle nh(std::string("/") + FLAGS_ns);
   // Setup SWITCH action
-  client.SetConnectedTimeout(FLAGS_connect);
-  client.SetActiveTimeout(FLAGS_active);
+  if (!FLAGS_remote) {
+    client.SetConnectedTimeout(FLAGS_connect);
+    client.SetActiveTimeout(FLAGS_active);
+    client.SetConnectedCallback(std::bind(ConnectedCallback, &client));
+  }
   client.SetResponseTimeout(FLAGS_response);
   if (FLAGS_deadline > 0)
     client.SetDeadlineTimeout(FLAGS_deadline);
   client.SetFeedbackCallback(std::bind(FeedbackCallback,
     std::placeholders::_1));
   client.SetResultCallback(std::bind(ResultCallback,
     std::placeholders::_1, std::placeholders::_2));
-  client.SetConnectedCallback(std::bind(ConnectedCallback, &client));
   client.Create(&nh, ACTION_BEHAVIORS_INSPECTION);
 
   // Configure panorama anomaly parameters
-  if (FLAGS_anomaly) {
+  if (FLAGS_anomaly && !FLAGS_remote) {
     ff_util::ConfigClient cfg(&nh, NODE_INSPECTION);
     cfg.Set<double>("target_distance", FLAGS_target_distance);
     cfg.Set<double>("min_distance", FLAGS_min_distance);
@@ -456,7 +489,7 @@ int main(int argc, char *argv[]) {
   }
 
   // Configure panorama inspection parameters
-  if (FLAGS_panorama) {
+  if (FLAGS_panorama && !FLAGS_remote) {
     ff_util::ConfigClient cfg(&nh, NODE_INSPECTION);
 
     if (FLAGS_panorama_mode == "") {
@@ -503,6 +536,17 @@ int main(int argc, char *argv[]) {
 
   // Start input thread
   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();
+
+    SendGoal(&client);
+  }
   // Synchronous mode
   while (!stopflag_) {
     ros::spinOnce();

astrobee/survey_manager/survey_planner/CMakeLists.txt

@@ -16,23 +16,83 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(survey_planner)
 
-add_compile_options(-std=c++17)
+set(CMAKE_CXX_STANDARD 17)
 
 find_package(catkin REQUIRED)
 SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake")
 find_package(catkin2 REQUIRED COMPONENTS
   roscpp
   ff_msgs
+  lifecycle
+  plansys2_executor
 )
 
 catkin_package(
   CATKIN_DEPENDS
 )
 
-foreach( dir pddl launch)
+###########
+## Build ##
+###########
+
+# Specify additional locations of header files
+# Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(isaac_action_node src/isaac_action_node.cpp)
+# Action for move
+add_executable(move_action_node src/move_action_node.cpp)
+target_link_libraries(move_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for dock
+add_executable(dock_action_node src/dock_action_node.cpp)
+target_link_libraries(dock_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for undock
+add_executable(undock_action_node src/undock_action_node.cpp)
+target_link_libraries(undock_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for panorama
+add_executable(panorama_action_node src/panorama_action_node.cpp)
+target_link_libraries(panorama_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for stereo
+add_executable(stereo_action_node src/stereo_action_node.cpp)
+target_link_libraries(stereo_action_node isaac_action_node ${catkin_LIBRARIES} )
+
+#############
+## Install ##
+#############
+
+# Allow other packages to use python scripts from this package
+catkin_python_setup()
+
+catkin_package()
+
+catkin_install_python(PROGRAMS
+  tools/survey_planner/command_astrobee
+  tools/survey_planner/monitor_astrobee
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+# Mark executables and/or libraries for installation
+install(TARGETS isaac_action_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Install actions
+install(TARGETS
+  move_action_node
+  dock_action_node
+  undock_action_node
+  panorama_action_node
+  stereo_action_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+foreach( dir pddl launch data)
    install( DIRECTORY ${dir}/
          DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir} )
 endforeach(dir)