Expose navgraph interfaces to ROS2

cfg/conf.d/meta_plugins.yaml

@@ -7,6 +7,13 @@ fawkes/meta_plugins:
 
   # Meta plugin with useful base plugins for the Robotino
   robotino_default: robotino,joystick,robotino-joystick,webview
+  # Meta plugin for running as client for the ros2 clips executive
+  m-ros2-skiller:
+    - flite
+    - ros2
+    - skiller
+    - ros2-cx-skiller
+    - ros2-cx-navgraph
 
   # Meta plugin for the clips-executive without planner integration
   m-clips-executive-test:

cfg/conf.d/navgraph.yaml

@@ -71,7 +71,7 @@ navgraph:
   # essentially means that no blackboard interfaces for path
   # instruction will be provided and the remaining API is kept
   # as-is.
-  path_execution: true
+  path_execution: false
 
   visualization:
 

src/lua/skiller/fawkes/init.lua

@@ -86,7 +86,7 @@ end
 function process_skiller_messages()
 	 local write_skiller_if = false
 	 local skill_enqueued = false
-	 
+
 	 while not skiller_if:msgq_empty() do
 			local m = skiller_if:msgq_first()
 			local mtype = m:type()
@@ -115,7 +115,7 @@ function process_skiller_messages()
 						write_skiller_if = true
 				 elseif skiller_if:exclusive_controller() ~= "" then
 						print_warn("%s tried to release exclusive control, but it's not the controller",
-											 m:sender_thread_name())						
+											 m:sender_thread_name())
 				 end
 
 			elseif mtype == "ExecSkillMessage" then
@@ -186,14 +186,15 @@ function process_skiller_messages()
 			skiller_if:msgq_pop()
 	 end
 
-	 if write_skiller_if then 
+	 if write_skiller_if then
 			skiller_if:write()
 	 end
 end
 
 function publish_skill_status()
 	 local old_status = skiller_if:status()
 	 local new_status = skillenv.get_overall_status()
+	 skiller_if:set_status(old_status)
 
 	 if old_status ~= new_status then
       skiller_if:set_status(new_status)

src/plugins/ros2/CMakeLists.txt

@@ -21,47 +21,50 @@
 # *****************************************************************************
 
 set(PLUGIN_ros2
-    ON
-    CACHE BOOL "Build ros2 plugin")
+  ON
+  CACHE BOOL "Build ros2 plugin")
+set(PLUGIN_ros2-cx-skiller
+  ON
+  CACHE BOOL "Build ros2 cx-skiller plugin")
 set(PLUGIN_ros2-talkerpub
-    ON
-    CACHE BOOL "Build ros2-talkerpub plugin")
+  ON
+  CACHE BOOL "Build ros2-talkerpub plugin")
 set(PLUGIN_ros2-tf2
-    ON
-    CACHE BOOL "Build ros2-tf2 plugin")
+  ON
+  CACHE BOOL "Build ros2-tf2 plugin")
 set(PLUGIN_ros2-images
-    ON
-    CACHE BOOL "Build ros2-images plugin")
+  ON
+  CACHE BOOL "Build ros2-images plugin")
 set(PLUGIN_ros2-laserscan
-    ON
-    CACHE BOOL "Build ros2-laserscan plugin")
+  ON
+  CACHE BOOL "Build ros2-laserscan plugin")
 set(PLUGIN_ros2-irscan
-    ON
-    CACHE BOOL "Build ros2-irscan plugin")
+  ON
+  CACHE BOOL "Build ros2-irscan plugin")
 set(PLUGIN_ros2-odometry
-    ON
-    CACHE BOOL "Build ros2-odometry plugin")
+  ON
+  CACHE BOOL "Build ros2-odometry plugin")
 set(PLUGIN_ros2-cmdvel
-    ON
-    CACHE BOOL "Build ros2-cmdvel plugin")
+  ON
+  CACHE BOOL "Build ros2-cmdvel plugin")
 set(PLUGIN_ros2-navigator
-    ON
-    CACHE BOOL "Build ros2-navigator plugin")
+  ON
+  CACHE BOOL "Build ros2-navigator plugin")
 set(PLUGIN_ros2-dynamic-reconfigure
-    ON
-    CACHE BOOL "Build ros2-dynamic-reconfigure plugin")
+  ON
+  CACHE BOOL "Build ros2-dynamic-reconfigure plugin")
 set(PLUGIN_ros2-joint
-    ON
-    CACHE BOOL "Build ros2-joint plugin")
+  ON
+  CACHE BOOL "Build ros2-joint plugin")
 set(PLUGIN_ros2-robot-description
-    ON
-    CACHE BOOL "Build ros2-robot-description plugin")
+  ON
+  CACHE BOOL "Build ros2-robot-description plugin")
 set(PLUGIN_ros2-clock
-    ON
-    CACHE BOOL "Build ros2-clock plugin")
+  ON
+  CACHE BOOL "Build ros2-clock plugin")
 set(PLUGIN_ros2-imu
-    ON
-    CACHE BOOL "Build ros2-imu plugin")
+  ON
+  CACHE BOOL "Build ros2-imu plugin")
 add_subdirectory(aspect)
 
 if(PLUGIN_ros2)
@@ -80,6 +83,27 @@ else()
   plugin_disabled_message(ros2)
 endif()
 
+if(PLUGIN_ros2-cx-skiller)
+  add_library(ros2-cx-skiller MODULE cx_skiller_plugin.cpp
+    cx_skiller_thread.cpp skill_node.cpp)
+  depend_on_ros2(ros2-cx-skiller)
+  depend_on_ros2_libs(
+    ros2-cx-skiller
+    "rmw;rosidl_typesupport_interface;rcl_interfaces;cx_msgs;cx_skill_execution"
+  )
+  target_link_libraries(
+    ros2-cx-skiller
+    fawkescore
+    fawkesutils
+    fawkesaspects
+    fawkesblackboard
+    fawkesinterface
+    SkillerInterface
+    fawkesros2aspect)
+else()
+  plugin_disabled_message(ros2-cx-skiller)
+endif()
+
 if(PLUGIN_ros2-talkerpub)
   add_library(ros2-talkerpub MODULE talkerpub_plugin.cpp talkerpub_thread.cpp)
   depend_on_ros2(ros2-talkerpub)
@@ -253,7 +277,7 @@ endif()
 
 if(PLUGIN_ros2-dynamic-reconfigure)
   add_library(ros2-dynamic-reconfigure MODULE dynamic_reconfigure_plugin.cpp
-                                              dynamic_reconfigure_thread.cpp)
+    dynamic_reconfigure_thread.cpp)
   depend_on_ros2(ros2-dynamic-reconfigure)
   depend_on_ros2_libs(ros2-dynamic-reconfigure dynamic_reconfigure)
   target_link_libraries(
@@ -315,10 +339,10 @@ endif()
 
 if(PLUGIN_ros2-robot-description)
   add_library(ros2-robot-description MODULE robot_description_plugin.cpp
-                                            robot_description_thread.cpp)
+    robot_description_thread.cpp)
   depend_on_ros2(ros2-robot-description)
   target_link_libraries(ros2-robot-description fawkescore fawkesutils
-                        fawkesaspects fawkesros2aspect)
+    fawkesaspects fawkesros2aspect)
 
 else()
   plugin_disabled_message(ros2-robot-description)

src/plugins/ros2/aspect/ros2.cpp

@@ -59,9 +59,11 @@ ROS2Aspect::~ROS2Aspect()
  * @param node_handle ROS2 node handle
  */
 void
-ROS2Aspect::init_ROS2Aspect(rclcpp::Node::SharedPtr node_handle)
+ROS2Aspect::init_ROS2Aspect(rclcpp::Node::SharedPtr                                   node_handle,
+                            std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor)
 {
 	this->node_handle = node_handle;
+	this->executor    = executor;
 	//        cfg_tf_prefix_ = config->get_string_or_default("/ros2/tf/tf_prefix", "");
 	//        cfg_tf_prefix_exclusions_ = config->get_strings_or_defaults("/ros2/tf/tf_prefix_exclusions", std::vector<std::string>());
 	//

src/plugins/ros2/aspect/ros2.h

@@ -47,10 +47,12 @@ class ROS2Aspect : public virtual Aspect
 	void add_tf_prefix(std::string &frame_id);
 
 protected:
-	rclcpp::Node::SharedPtr node_handle;
+	rclcpp::Node::SharedPtr                                   node_handle;
+	std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor;
 
 private:
-	void init_ROS2Aspect(rclcpp::Node::SharedPtr node_handle);
+	void init_ROS2Aspect(rclcpp::Node::SharedPtr                                   node_handle,
+	                     std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor);
 	void finalize_ROS2Aspect();
 	//	bool tf_prefix_enabled_;
 	//	std::string cfg_tf_prefix_;

src/plugins/ros2/aspect/ros2_inifin.cpp

@@ -52,8 +52,11 @@ ROS2AspectIniFin::init(Thread *thread)
 	if (!node_handle_) {
 		throw CannotInitializeThreadException("ROS2 node handle has not been set.");
 	}
+	if (!executor_) {
+		throw CannotInitializeThreadException("ROS2 executor has not been set.");
+	}
 
-	ros2_thread->init_ROS2Aspect(node_handle_);
+	ros2_thread->init_ROS2Aspect(node_handle_, executor_);
 }
 
 void
@@ -79,4 +82,13 @@ ROS2AspectIniFin::set_node_handle(rclcpp::Node::SharedPtr node_handle)
 	node_handle_ = node_handle;
 }
 
+/** Set the ROS2 executor to use for aspect initialization.
+ * @param executor ROS2 executor to pass to threads with ROS2Aspect.
+ */
+void
+ROS2AspectIniFin::set_executor(std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor)
+{
+	executor_ = executor;
+}
+
 } // end namespace fawkes

src/plugins/ros2/aspect/ros2_inifin.h

@@ -27,6 +27,7 @@
 #include <aspect/inifins/inifin.h>
 #include <plugins/ros2/aspect/ros2.h>
 
+#include <rclcpp/executors/multi_threaded_executor.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 namespace fawkes {
@@ -40,10 +41,12 @@ class ROS2AspectIniFin : public AspectIniFin
 	virtual void finalize(Thread *thread);
 
 	void set_node_handle(rclcpp::Node::SharedPtr node_handle);
+	void set_executor(std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor);
 
 private:
-	rclcpp::Node::SharedPtr node_handle_;
-	std::string             tf_prefix_;
+	rclcpp::Node::SharedPtr                                   node_handle_;
+	std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+	std::string                                               tf_prefix_;
 };
 
 } // end namespace fawkes

src/plugins/ros2/cx_skiller_plugin.cpp

@@ -0,0 +1,43 @@
+/***************************************************************************
+ *  cx_skiller_plugin.cpp - Relay exec requests from ROS CX to the Skiller
+ *
+ *  Created: Oct 2023
+ *  Copyright  2023  Tarik Viehmann <viehmann@kbsg.rwth-aachen.de>
+ ****************************************************************************/
+
+/*  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 "cx_skiller_thread.h"
+
+#include <core/plugin.h>
+
+using namespace fawkes;
+
+/** Plugin to publish IMU data to ROS.
+ * @author Till Hofmann
+ */
+class ROS2CXSkillerPlugin : public fawkes::Plugin
+{
+public:
+	/** Constructor.
+    * @param config Fawkes configuration
+    */
+	explicit ROS2CXSkillerPlugin(Configuration *config) : Plugin(config)
+	{
+		thread_list.push_back(new ROS2CXSkillerThread());
+	}
+};
+
+PLUGIN_DESCRIPTION("ROS CX Skiller Interface Plugin")
+EXPORT_PLUGIN(ROS2CXSkillerPlugin)

src/plugins/ros2/cx_skiller_thread.cpp

@@ -0,0 +1,76 @@
+/***************************************************************************
+ *  imu_thread.cpp - Thread to publish CXSkiller data to ROS
+ *
+ *  Created: oct 2023
+ *  Copyright  2023  Tarik Viehmann <viehmann@kbsg.rwth-aachen.de>
+ ****************************************************************************/
+
+/*  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 "cx_skiller_thread.h"
+
+using namespace fawkes;
+
+/** @class ROS2CXSkillerThread "imu_thread.h"
+ * Thread to publish CXSkiller data to ROS.
+ * This thread reads data from the CXSkiller blackboard interface and publishes the
+ * data to ROS.
+ * @author Till Hofmann
+ */
+
+/** Constructor. */
+ROS2CXSkillerThread::ROS2CXSkillerThread()
+: Thread("ROS2CXSkillerThread", Thread::OPMODE_WAITFORWAKEUP)
+{
+}
+
+/** Destructor. */
+ROS2CXSkillerThread::~ROS2CXSkillerThread()
+{
+}
+
+/** Initialize the thread.
+ * Open the blackboard interface and advertise the ROS topic.
+ * Register as listener for the blackboard interface.
+ */
+void
+ROS2CXSkillerThread::init()
+{
+	skiller_iface_ = blackboard->open_for_reading<SkillerInterface>("Skiller");
+	//logger->log_info(name(),
+	//                 "Publishing CXSkiller '%s' to ROS topic '%s'.",
+	//                 skiller_iface_name.c_str(),
+	//                 ros2_topic.c_str());
+	std::string robot_name = config->get_string_or_default("fawkes/agent/name", "");
+	std::string executor_name =
+	  config->get_string_or_default("ros2/cx-skiller/executor-name", "fawkes_skiller");
+	skill_node_ = std::make_shared<SkillNode>(
+	  "FawkesSkillNode", robot_name, executor_name, std::chrono::nanoseconds(5000), skiller_iface_);
+	skill_node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+	executor->add_node(skill_node_->get_node_base_interface());
+	//ros2_pub_ = node_handle->create_publisher<sensor_msgs::msg::Imu>(ros2_topic, 100);
+
+	blackboard->register_listener(skill_node_.get());
+}
+
+/** Close all interfaces and ROS handles. */
+void
+ROS2CXSkillerThread::finalize()
+{
+	blackboard->unregister_listener(skill_node_.get());
+	// call destructor before closing interface to release skiller control
+	executor->remove_node(skill_node_->get_node_base_interface());
+	skill_node_.reset();
+	blackboard->close(skiller_iface_);
+}