Feature drone control

CMakeLists.txt

@@ -43,6 +43,7 @@ set(WITH_OSI_ADAPTER ON CACHE BOOL "Enable OSIAdapter module")
 set(WITH_ESMINI_ADAPTER ON CACHE BOOL "Enable EsminiAdapter module")
 set(WITH_MQTT_BRIDGE ON CACHE BOOL "Enable MQTTBridge module")
 set(WITH_POINTCLOUD_PUBLISHER ON CACHE BOOL "Enable PointcloudPublisher module")
+set(WITH_DRONE_CONTROL ON CACHE BOOL "Enable DroneControl module")
 
 set(ENABLE_TESTS ON CACHE BOOL "Enable testing on build")
 
@@ -71,6 +72,9 @@ endif()
 if(WITH_POINTCLOUD_PUBLISHER)
         list(APPEND ENABLED_MODULES PointcloudPublisher)
 endif()
+if(WITH_DRONE_CONTROL)
+        list(APPEND ENABLED_MODULES DroneControl)
+endif()
 
 # Add corresponding subprojects
 add_subdirectory(iso22133)

cmake/modules/FindTinyXML2.cmake

@@ -0,0 +1,74 @@
+##################################################################################################
+# 
+# CMake script for finding TinyXML2.
+# 
+# Input variables:
+# 
+# - TinyXML2_ROOT_DIR (optional): When specified, header files and libraries will be searched for in
+#     ${TinyXML2_ROOT_DIR}/include
+#     ${TinyXML2_ROOT_DIR}/libs
+#   respectively, and the default CMake search order will be ignored. When unspecified, the default
+#   CMake search order is used.
+#   This variable can be specified either as a CMake or environment variable. If both are set,
+#   preference is given to the CMake variable.
+#   Use this variable for finding packages installed in a nonstandard location, or for enforcing
+#   that one of multiple package installations is picked up.
+# 
+# 
+# Cache variables (not intended to be used in CMakeLists.txt files)
+# 
+# - TinyXML2_INCLUDE_DIR: Absolute path to package headers.
+# - TinyXML2_LIBRARY: Absolute path to library.
+# 
+# 
+# Output variables:
+# 
+# - TinyXML2_FOUND: Boolean that indicates if the package was found
+# - TinyXML2_INCLUDE_DIRS: Paths to the necessary header files
+# - TinyXML2_LIBRARIES: Package libraries
+# 
+# 
+# Example usage:
+# 
+#  find_package(TinyXML2)
+#  if(NOT TinyXML2_FOUND)
+#    # Error handling
+#  endif()
+#  ...
+#  include_directories(${TinyXML2_INCLUDE_DIRS} ...)
+#  ...
+#  target_link_libraries(my_target ${TinyXML2_LIBRARIES})
+# 
+##################################################################################################
+
+# Get package location hint from environment variable (if any)
+if(NOT TinyXML2_ROOT_DIR AND DEFINED ENV{TinyXML2_ROOT_DIR})
+  set(TinyXML2_ROOT_DIR "$ENV{TinyXML2_ROOT_DIR}" CACHE PATH
+      "TinyXML2 base directory location (optional, used for nonstandard installation paths)")
+endif()
+
+# Search path for nonstandard package locations
+if(TinyXML2_ROOT_DIR)
+  set(TinyXML2_INCLUDE_PATH PATHS "${TinyXML2_ROOT_DIR}/include" NO_DEFAULT_PATH)
+  set(TinyXML2_LIBRARY_PATH PATHS "${TinyXML2_ROOT_DIR}/lib"     NO_DEFAULT_PATH)
+endif()
+
+# Find headers and libraries
+find_path(TinyXML2_INCLUDE_DIR NAMES tinyxml2.h PATH_SUFFIXES "tinyxml2" ${TinyXML2_INCLUDE_PATH})
+find_library(TinyXML2_LIBRARY  NAMES tinyxml2   PATH_SUFFIXES "tinyxml2" ${TinyXML2_LIBRARY_PATH})
+
+mark_as_advanced(TinyXML2_INCLUDE_DIR
+                 TinyXML2_LIBRARY)
+
+# Output variables generation
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(TinyXML2 DEFAULT_MSG TinyXML2_LIBRARY
+                                                       TinyXML2_INCLUDE_DIR)
+
+set(TinyXML2_FOUND ${TINYXML2_FOUND}) # Enforce case-correctness: Set appropriately cased variable...
+unset(TINYXML2_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args
+
+if(TinyXML2_FOUND)
+  set(TinyXML2_INCLUDE_DIRS ${TinyXML2_INCLUDE_DIR})
+  set(TinyXML2_LIBRARIES ${TinyXML2_LIBRARY})
+endif()

launch/launch_utils/launch_base.py

@@ -71,11 +71,18 @@ def get_base_nodes():
             name='osi_adapter',
             parameters=[files["params"]]
         ),
+        #Node(
+        #    package='atos',
+        #    namespace='atos',
+        #    executable='esmini_adapter',
+        #    name='esmini_adapter',
+        #    parameters=[files["params"]]
+        #),
         Node(
             package='atos',
             namespace='atos',
-            executable='esmini_adapter',
-            name='esmini_adapter',
+            executable='drone_control',
+            name='drone_control',
             parameters=[files["params"]]
         ),
         Node(

modules/ATOSBase/inc/ATOSbase.hpp

@@ -10,6 +10,7 @@
 #include <std_srvs/srv/set_bool.hpp>
 #include "atos_interfaces/srv/get_object_ids.hpp"
 #include "atos_interfaces/srv/get_test_origin.hpp"
+#include "atos_interfaces/srv/get_object_ip.hpp"
 #include "module.hpp"
 
 class ATOSBase : public Module {
@@ -21,6 +22,7 @@ class ATOSBase : public Module {
 	static inline std::string const moduleName = "atos_base";
 	rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr initDataDictionaryService;
 	rclcpp::Service<atos_interfaces::srv::GetObjectIds>::SharedPtr getObjectIdsService;
+	rclcpp::Service<atos_interfaces::srv::GetObjectIp>::SharedPtr getObjectIpService;
 	rclcpp::Service<atos_interfaces::srv::GetTestOrigin>::SharedPtr getTestOriginService;
 
 	void onExitMessage(const ROSChannels::Exit::message_type::SharedPtr) override;
@@ -31,10 +33,13 @@ class ATOSBase : public Module {
 							  std::shared_ptr<std_srvs::srv::SetBool::Response>);
 	void onRequestObjectIDs(const std::shared_ptr<atos_interfaces::srv::GetObjectIds::Request>,
 							std::shared_ptr<atos_interfaces::srv::GetObjectIds::Response>);
+	void onRequestObjectIP(const std::shared_ptr<atos_interfaces::srv::GetObjectIp::Request>,
+							std::shared_ptr<atos_interfaces::srv::GetObjectIp::Response>);
 	void onRequestTestOrigin(const std::shared_ptr<atos_interfaces::srv::GetTestOrigin::Request>,
 							 std::shared_ptr<atos_interfaces::srv::GetTestOrigin::Response>);
 	bool isInitialized = false;
 	ROSChannels::Exit::Sub exitSub;
-};
+	std::map<uint32_t,uint32_t> getObjectsInfo();
+	};
 
 #endif

modules/ATOSBase/src/ATOSbase.cpp

@@ -7,6 +7,7 @@
 #include "datadictionary.h"
 #include "objectconfig.hpp"
 #include <functional>
+#include <arpa/inet.h>
 
 #include <ament_index_cpp/get_package_prefix.hpp>
 
@@ -31,6 +32,8 @@ ATOSBase::ATOSBase()
 		std::bind(&ATOSBase::onInitDataDictionary, this, _1, _2));
 	getObjectIdsService = create_service<atos_interfaces::srv::GetObjectIds>(ServiceNames::getObjectIds,
 		std::bind(&ATOSBase::onRequestObjectIDs, this, _1, _2));
+	getObjectIpService = create_service<atos_interfaces::srv::GetObjectIp>(ServiceNames::getObjectIp,
+		std::bind(&ATOSBase::onRequestObjectIP, this, _1, _2));
 	getTestOriginService = create_service<atos_interfaces::srv::GetTestOrigin>(ServiceNames::getTestOrigin,
 		std::bind(&ATOSBase::onRequestTestOrigin, this, _1, _2));
 }
@@ -80,9 +83,7 @@ void ATOSBase::onExitMessage(const Exit::message_type::SharedPtr)
     rclcpp::shutdown();
 }
 
-void ATOSBase::onRequestObjectIDs(
-	const std::shared_ptr<atos_interfaces::srv::GetObjectIds::Request> req,
-	std::shared_ptr<atos_interfaces::srv::GetObjectIds::Response> res)
+std::map<uint32_t,uint32_t> ATOSBase::getObjectsInfo()
 {
 	char path[PATH_MAX];
 	std::vector<std::invalid_argument> errors;
@@ -94,7 +95,7 @@ void ATOSBase::onRequestObjectIDs(
 		throw std::ios_base::failure("Object directory does not exist");
 	}
 
-	std::vector<uint32_t> objectIDs;
+	std::map<uint32_t,uint32_t> objectIps;
 	for (const auto& entry : fs::directory_iterator(objectDir)) {
 		if (!fs::is_regular_file(entry.status())) {
 			continue;
@@ -105,18 +106,54 @@ void ATOSBase::onRequestObjectIDs(
 
 		RCLCPP_DEBUG(get_logger(), "Loaded configuration: %s", conf.toString().c_str());
 		// Check preexisting
-		auto foundID = std::find(objectIDs.begin(), objectIDs.end(), conf.getTransmitterID());
-		if (foundID == objectIDs.end()) {
-			objectIDs.push_back(conf.getTransmitterID());
+
+		auto foundID = objectIps.find(conf.getTransmitterID());
+		if (foundID == objectIps.end()) {
+			objectIps.emplace(conf.getTransmitterID(), conf.getIP());
 		}
 		else {
 			std::string errMsg = "Duplicate object ID " + std::to_string(conf.getTransmitterID())
 					+ " detected in object files";
 			throw std::invalid_argument(errMsg);
 		}
 	}
+	return objectIps;
+}
 
-	res->ids = objectIDs;
+void ATOSBase::onRequestObjectIDs(
+	const std::shared_ptr<atos_interfaces::srv::GetObjectIds::Request> req,
+	std::shared_ptr<atos_interfaces::srv::GetObjectIds::Response> res)
+{
+	std::vector<uint32_t> objectIDs;
+	try {
+		for(auto const& objs: getObjectsInfo())
+    		objectIDs.push_back(objs.first);
+		res->ids = objectIDs;
+	}
+	catch (const std::exception& e) {
+		RCLCPP_ERROR(get_logger(), "Failed to get object IDs: %s", e.what());
+		res->success = false;
+		return;
+	}
+}
+
+void ATOSBase::onRequestObjectIP(
+	const std::shared_ptr<atos_interfaces::srv::GetObjectIp::Request> req,
+	std::shared_ptr<atos_interfaces::srv::GetObjectIp::Response> res)
+{
+	uint32_t objectIp;
+	try {
+		auto objinfo = getObjectsInfo();
+		if (objinfo.find(req->id) == objinfo.end()) {
+			throw std::invalid_argument("Object ID not found");
+		}
+		res->ip = std::string(inet_ntoa(in_addr{objinfo.at(req->id)}));
+		res->success = true;
+	}
+	catch (const std::exception& e) {
+		RCLCPP_ERROR(get_logger(), "Failed to get object IPs: %s", e.what());
+		res->success = false;
+	}
 }
 
 void ATOSBase::onRequestTestOrigin(

modules/DroneControl/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.8)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_C_STANDARD_REQUIRED ON)
+
+# Adding tinyxml2 to the CMake
+find_package(TinyXML2 REQUIRED)
+
+project(drone_control)
+find_package(atos_interfaces REQUIRED)
+# Define target names
+set(DRONE_CONTROL_TARGET ${PROJECT_NAME})
+
+set(COMMON_LIBRARY ATOSCommon) # Common library for ATOS with e.g. Trajectory class
+
+get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES)
+
+include(GNUInstallDirs)
+
+# Create project main executable target
+add_executable(${DRONE_CONTROL_TARGET}
+	${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/dronecontrol.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/path.cpp
+)
+
+# Link project executable to common libraries
+target_link_libraries(${DRONE_CONTROL_TARGET}
+	${COREUTILS_LIBRARY}
+	${SOCKET_LIBRARY}
+	${COMMON_LIBRARY}
+	${TinyXML2_LIBRARIES}
+)
+
+target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}/inc
+	${COMMON_HEADERS}
+	${TinyXML2_INCLUDE_DIRS}
+)
+
+# ROS specific rules
+ament_target_dependencies(${DRONE_CONTROL_TARGET}
+  rclcpp
+  std_msgs
+  atos_interfaces
+)
+
+# Installation rules
+install(CODE "MESSAGE(STATUS \"Installing target ${DRONE_CONTROL_TARGET}\")")
+install(TARGETS ${DRONE_CONTROL_TARGET} 
+	RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos"
+	LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+)

modules/DroneControl/README.md

@@ -0,0 +1,40 @@
+# Sample module
+This is a sample module to be used as template for creating new modules.
+## Features
+The sample module is a ros2 node that features some basic publishers and subscribers to various topics.
+It also features a TCPServer running in a separate thread. 
+
+## Usage
+In order to compile and launch this module (or any other module created from the template) you need to go to the outer-most CMakeLists.txt file in the root of the repository and add the following line:
+```
+set(WITH_MODULE_X ON CACHE BOOL "Enable ModuleX module")
+```
+
+
+Followed by:
+```
+if(WITH_MODULE_X)
+        list(APPEND ENABLED_MODULES ModuleX)
+endif()
+```
+
+Note: When switching ON/OFF certain modules, it might be nessesscary to remove the CMakeCache.txt file in ~/atos_ws/install/atos/.
+
+It is also necessary to add the module to a launch file, located in the launch directory. This is done by adding the following line to the list of nodes in the appropriate launch file:
+```
+    Node(
+        package='atos',
+        namespace='atos',
+        executable='module_x',
+        name='module_x',
+    )
+```
+
+Then you can compile and launch the module by running the following commands:
+```
+MAKEFLAGS=-j5 colcon build --packages-up-to atos
+```
+(tune -j5 to an approperiate number depending on your availiable RAM memory and CPU cores)
+```
+ros2 launch atos launch_basic.py
+```

modules/DroneControl/inc/dronecontrol.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <vector>
+#include "trajectory.hpp"
+#include "module.hpp"
+#include "path.hpp"
+#include "atos_interfaces/srv/get_object_trajectory.hpp"
+
+/*!
+ * \brief The DroneControl module 
+ */
+class DroneControl : public Module{
+public:
+	static inline std::string const moduleName = "drone_control";
+	DroneControl();
+	~DroneControl();
+
+private:
+	// Member variables
+	std::map<uint32_t,ATOS::Trajectory> droneTrajectories; // map from drone object-id to trajectory
+
+	// Provided services
+	std::shared_ptr<rclcpp::Service<atos_interfaces::srv::GetObjectTrajectory>> objectTrajectoryService;
+
+	// Service callbacks
+	void onRequestObjectTrajectory(const atos_interfaces::srv::GetObjectTrajectory::Request::SharedPtr,
+		const atos_interfaces::srv::GetObjectTrajectory::Response::SharedPtr);
+
+	// Business logic
+	ABD::Path createPath(const std::string&);
+	ATOS::Trajectory createDroneTrajectory(ABD::Path&, uint32_t);
+
+
+
+};