Merge remote-tracking branch 'origin/timn/ros-navgraph-breakout'

cfg/conf.d/navgraph.yaml

@@ -54,6 +54,17 @@ navgraph:
   # Write graph information to log on (re-)loading?
   log_graph: false
 
+  # The following enables or disables path execution.
+  # In either case a navgraph is maintained internally, paths may be
+  # planned, and searches performed. However, if disabled, paths may
+  # not be executed by the navgraph plugin. This can be useful, for
+  # example, to keep the navgraph data structure around and use
+  # ros-navgraph-breakout for external execution. Disabling
+  # essentially means that no blackboard interfaces for path
+  # instruction will be provided and the remaining API is kept
+  # as-is.
+  path_execution: true
+
   visualization:
 
     # Set to true to enable visualization by publishing messages for rviz

cfg/conf.d/ros.yaml

@@ -49,3 +49,5 @@ ros:
     # This setting is only relevant is use_tf2 is set to false above.
     static-update-interval: 1.0
 
+  navgraph-breakout:
+    action-topic: place_goto

src/libs/netcomm/dns-sd/avahi_thread.cpp

@@ -683,7 +683,6 @@ AvahiThread::call_handler_service_added( const char *name,
 	    if (getaddrinfo(addr_with_scope.c_str(), port_s.c_str(), &hints, &res) == 0) {
 		    if (slen == res[0].ai_addrlen) {
 			    memcpy(sin, res[0].ai_addr, slen);
-			    printf("Scope %u  interface %u\n", sin->sin6_scope_id, interface);
 			    freeaddrinfo(res);
 		    } else {
 			    fprintf(stderr, "AvahiThread::call_handler_service_added: IPv6 address lengths different");

src/plugins/navgraph/navgraph_thread.cpp

@@ -90,6 +90,11 @@ NavGraphThread::init()
     cfg_monitor_file_ = config->get_bool("/navgraph/monitor_file");
   } catch (Exception &e) {} // ignored
 
+  cfg_enable_path_execution_ = true;
+  try {
+    cfg_enable_path_execution_ = config->get_bool("/navgraph/path_execution");
+  } catch (Exception &e) {} // ignored
+
   if (config->exists("/navgraph/travel_tolerance") ||
       config->exists("/navgraph/target_tolerance") ||
       config->exists("/navgraph/orientation_tolerance") ||
@@ -105,9 +110,11 @@ NavGraphThread::init()
     throw Exception("Navgraph tolerances may no longer be set in the config");
   }
 
-  pp_nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Pathplan");
-  nav_if_    = blackboard->open_for_reading<NavigatorInterface>(cfg_nav_if_id_.c_str());
-  path_if_   = blackboard->open_for_writing<NavPathInterface>("NavPath");
+  if (cfg_enable_path_execution_) {
+	  pp_nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Pathplan");
+	  nav_if_    = blackboard->open_for_reading<NavigatorInterface>(cfg_nav_if_id_.c_str());
+	  path_if_   = blackboard->open_for_writing<NavPathInterface>("NavPath");
+  }
 
 
   if (! cfg_graph_file_.empty()) {
@@ -193,9 +200,11 @@ NavGraphThread::finalize()
   }
 #endif
   graph_.clear();
-  blackboard->close(pp_nav_if_);
-  blackboard->close(nav_if_);
-  blackboard->close(path_if_);
+  if (cfg_enable_path_execution_) {
+	  blackboard->close(pp_nav_if_);
+	  blackboard->close(nav_if_);
+	  blackboard->close(path_if_);
+  }
 }
 
 void
@@ -214,7 +223,7 @@ NavGraphThread::loop()
 {
   // process messages
   bool needs_write = false;
-  while (! pp_nav_if_->msgq_empty()) {
+  while (cfg_enable_path_execution_ && ! pp_nav_if_->msgq_empty()) {
     needs_write = true;
 
     if (pp_nav_if_->msgq_first_is<NavigatorInterface::StopMessage>()) {
@@ -269,7 +278,7 @@ NavGraphThread::loop()
     fam_->process_events();
   }
 
-  if (exec_active_) {
+  if (cfg_enable_path_execution_ && exec_active_) {
     // check if current was target reached
     size_t shortcut_to;
 
@@ -404,7 +413,7 @@ NavGraphThread::loop()
   }
 #endif
 
-  if (needs_write) {
+  if (cfg_enable_path_execution_ && needs_write) {
     pp_nav_if_->write();
   }
 }
@@ -431,8 +440,10 @@ NavGraphThread::generate_plan(std::string goal_name)
 	if (! tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose_)) {
 		logger->log_warn(name(),
 		                 "Failed to compute pose, cannot generate plan");
-    pp_nav_if_->set_final(true);
-    pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		if (cfg_enable_path_execution_) {
+			pp_nav_if_->set_final(true);
+			pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		}
 		return false;
 	}
 
@@ -443,8 +454,10 @@ NavGraphThread::generate_plan(std::string goal_name)
 	if (! goal.is_valid()) {
 		logger->log_error(name(), "Failed to generate path from (%.2f,%.2f) to %s: goal is unknown",
 		                  init.x(), init.y(), goal_name.c_str()); 
-    pp_nav_if_->set_final(true);
-    pp_nav_if_->set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
+		if (cfg_enable_path_execution_) {
+			pp_nav_if_->set_final(true);
+			pp_nav_if_->set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
+		}
     return false;
 	}
 
@@ -456,8 +469,10 @@ NavGraphThread::generate_plan(std::string goal_name)
 	} catch (Exception &e) {
 		logger->log_error(name(), "Failed to generate path from (%.2f,%.2f) to %s: %s",
 		                  init.x(), init.y(), goal_name.c_str(), e.what_no_backtrace());
-    pp_nav_if_->set_final(true);
-    pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		if (cfg_enable_path_execution_) {
+			pp_nav_if_->set_final(true);
+			pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		}
 		return false;
 	}
 
@@ -469,8 +484,10 @@ NavGraphThread::generate_plan(std::string goal_name)
 		try {
 			path_ = graph_->search_path(init, goal, /* use constraints */ false);
 		} catch (Exception &e) {
-			pp_nav_if_->set_final(true);
-			pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+			if (cfg_enable_path_execution_) {
+				pp_nav_if_->set_final(true);
+				pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+			}
 			return false;
 		}
 	}
@@ -496,8 +513,10 @@ NavGraphThread::generate_plan(std::string goal_name, float ori)
 		traversal_ = path_.traversal();
 		return true;
 	} else {
-	  pp_nav_if_->set_final(true);
-	  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		if (cfg_enable_path_execution_) {
+			pp_nav_if_->set_final(true);
+			pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		}
 	  return false;
 	}
 }
@@ -518,8 +537,10 @@ NavGraphThread::generate_plan(float x, float y, float ori)
 	  return true;
 
   } else {
-	  pp_nav_if_->set_final(true);
-	  pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		if (cfg_enable_path_execution_) {
+			pp_nav_if_->set_final(true);
+			pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
+		}
 	  return false;
   }
 }
@@ -609,6 +630,8 @@ NavGraphThread::optimize_plan()
 void
 NavGraphThread::start_plan()
 {
+	if (! cfg_enable_path_execution_)  return;
+
   path_planned_at_->stamp();
 
   target_reached_ = false;
@@ -667,6 +690,8 @@ NavGraphThread::start_plan()
 void
 NavGraphThread::stop_motion()
 {
+	if (! cfg_enable_path_execution_)  return;
+
   NavigatorInterface::StopMessage *stop = new NavigatorInterface::StopMessage();
   try {
     nav_if_->msgq_enqueue(stop);
@@ -694,7 +719,9 @@ NavGraphThread::stop_motion()
 void
 NavGraphThread::send_next_goal()
 {
-  bool stop_at_target   = false;
+	if (! cfg_enable_path_execution_)  return;
+
+	bool stop_at_target   = false;
   bool orient_at_target = false;
 
   if (! traversal_.running()) {
@@ -1000,6 +1027,8 @@ NavGraphThread::log_graph()
 void
 NavGraphThread::publish_path()
 {
+	if (! cfg_enable_path_execution_)  return;
+
   std::vector<std::string> vpath(40, "");
 
   if (traversal_) {

src/plugins/navgraph/navgraph_thread.h

@@ -112,6 +112,7 @@ class NavGraphThread
   float        cfg_target_ori_time_;
   bool         cfg_log_graph_;
   bool         cfg_abort_on_error_;
+  bool         cfg_enable_path_execution_;
 
   fawkes::NavigatorInterface *nav_if_;
   fawkes::NavigatorInterface *pp_nav_if_;

src/plugins/ros/Makefile

@@ -61,6 +61,10 @@ LIBS_ros_navigator = m pthread fawkescore fawkesutils fawkesaspects fawkesblackb
 		     fawkesinterface fawkesrosaspect NavigatorInterface
 OBJS_ros_navigator = navigator_plugin.o navigator_thread.o
 
+LIBS_ros_navgraph_breakout = m pthread fawkescore fawkesutils fawkesaspects fawkesblackboard \
+		     fawkesinterface fawkesrosaspect NavigatorInterface
+OBJS_ros_navgraph_breakout = navgraph_breakout_plugin.o navgraph_breakout_thread.o
+
 LIBS_ros_joint = fawkescore fawkesutils fawkesaspects fawkesblackboard \
 	      fawkesinterface fawkesrosaspect JointInterface
 OBJS_ros_joint = joint_plugin.o joint_thread.o
@@ -232,16 +236,21 @@ ifeq ($(HAVE_ROS),1)
   ifeq ($(call ros-have-pkg,sensor_msgs),1)
     CFLAGS_joint_thread = $(CFLAGS) $(call ros-pkg-cflags,sensor_msgs)
     CFLAGS_joint_plugin = $(CFLAGS_joint_thread)
-    LDFLAGS_joint += $(call ros-pkg-lflags,sensor_msgs)
+    LDFLAGS_ros_joint += $(call ros-pkg-lflags,sensor_msgs)
     PLUGINS_all += $(PLUGINDIR)/ros-joint.so
   else
     WARN_TARGETS += warning_sensor_msgs
   endif
   ifeq ($(call ros-have-pkg,fawkes_msgs),1)
     CFLAGS_position_3d_thread += $(CFLAGS) $(call ros-pkg-cflags,fawkes_msgs)
     CFLAGS_position_3d_plugin = $(CFLAGS_position_3d_thread)
-    LDFLAGS_position_3d += $(call ros-pkg-lflags,fawkes_msgs)
+    LDFLAGS_ros_position_3d += $(call ros-pkg-lflags,fawkes_msgs)
     PLUGINS_all += $(PLUGINDIR)/ros-position-3d.so
+
+    CFLAGS_navgraph_breakout_thread += $(CFLAGS) $(call ros-pkg-cflags,fawkes_msgs)
+    CFLAGS_navgraph_breakout_plugin = $(CFLAGS_navgraph_breakout_thread)
+    LDFLAGS_ros_navgraph_breakout += $(call ros-pkg-lflags,fawkes_msgs)
+    PLUGINS_all += $(PLUGINDIR)/ros-navgraph-breakout.so
   else
     WARN_TARGETS += warning_fawkes_msgs
   endif

src/plugins/ros/navgraph_breakout_plugin.cpp

@@ -0,0 +1,44 @@
+
+/***************************************************************************
+ *  navgraph_breakout_plugin.cpp - Provide navgraph-like API through ROS
+ *
+ *  Created: Fri Jan 27 11:19:24 2017
+ *  Copyright  2017  Tim Niemueller
+ ****************************************************************************/
+
+/*  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  Read the full text in the LICENSE.GPL file in the doc directory.
+ */
+
+#include <core/plugin.h>
+#include "navgraph_breakout_thread.h"
+
+using namespace fawkes;
+
+/** Provide navgraph-like API through ROS.
+ * @author Tim Niemueller
+ */
+class RosNavgraphBreakoutPlugin : public fawkes::Plugin
+{
+ public:
+  /** Constructor.
+   * @param config Fawkes configuration
+   */
+  RosNavgraphBreakoutPlugin(Configuration *config)
+    : Plugin(config)
+  {
+    thread_list.push_back(new RosNavgraphBreakoutThread());
+  }
+};
+
+PLUGIN_DESCRIPTION("Use external ROS node for place goto")
+EXPORT_PLUGIN(RosNavgraphBreakoutPlugin)