diff --git a/CMakeLists.txt b/CMakeLists.txt index bb9f4a1ba..9eafa91aa 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/cmake/modules/FindTinyXML2.cmake b/cmake/modules/FindTinyXML2.cmake new file mode 100644 index 000000000..791a3a3ad --- /dev/null +++ b/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() \ No newline at end of file diff --git a/launch/launch_utils/launch_base.py b/launch/launch_utils/launch_base.py index abaaf2ed8..056f19c8c 100644 --- a/launch/launch_utils/launch_base.py +++ b/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( diff --git a/modules/ATOSBase/inc/ATOSbase.hpp b/modules/ATOSBase/inc/ATOSbase.hpp index 07a8ef696..a1872d7c4 100644 --- a/modules/ATOSBase/inc/ATOSbase.hpp +++ b/modules/ATOSBase/inc/ATOSbase.hpp @@ -10,6 +10,7 @@ #include #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::SharedPtr initDataDictionaryService; rclcpp::Service::SharedPtr getObjectIdsService; + rclcpp::Service::SharedPtr getObjectIpService; rclcpp::Service::SharedPtr getTestOriginService; void onExitMessage(const ROSChannels::Exit::message_type::SharedPtr) override; @@ -31,10 +33,13 @@ class ATOSBase : public Module { std::shared_ptr); void onRequestObjectIDs(const std::shared_ptr, std::shared_ptr); + void onRequestObjectIP(const std::shared_ptr, + std::shared_ptr); void onRequestTestOrigin(const std::shared_ptr, std::shared_ptr); bool isInitialized = false; ROSChannels::Exit::Sub exitSub; -}; + std::map getObjectsInfo(); + }; #endif \ No newline at end of file diff --git a/modules/ATOSBase/src/ATOSbase.cpp b/modules/ATOSBase/src/ATOSbase.cpp index ddd047464..115ce1a85 100644 --- a/modules/ATOSBase/src/ATOSbase.cpp +++ b/modules/ATOSBase/src/ATOSbase.cpp @@ -7,6 +7,7 @@ #include "datadictionary.h" #include "objectconfig.hpp" #include +#include #include @@ -31,6 +32,8 @@ ATOSBase::ATOSBase() std::bind(&ATOSBase::onInitDataDictionary, this, _1, _2)); getObjectIdsService = create_service(ServiceNames::getObjectIds, std::bind(&ATOSBase::onRequestObjectIDs, this, _1, _2)); + getObjectIpService = create_service(ServiceNames::getObjectIp, + std::bind(&ATOSBase::onRequestObjectIP, this, _1, _2)); getTestOriginService = create_service(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 req, - std::shared_ptr res) +std::map ATOSBase::getObjectsInfo() { char path[PATH_MAX]; std::vector errors; @@ -94,7 +95,7 @@ void ATOSBase::onRequestObjectIDs( throw std::ios_base::failure("Object directory does not exist"); } - std::vector objectIDs; + std::map objectIps; for (const auto& entry : fs::directory_iterator(objectDir)) { if (!fs::is_regular_file(entry.status())) { continue; @@ -105,9 +106,10 @@ 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()) @@ -115,8 +117,43 @@ void ATOSBase::onRequestObjectIDs( throw std::invalid_argument(errMsg); } } + return objectIps; +} - res->ids = objectIDs; +void ATOSBase::onRequestObjectIDs( + const std::shared_ptr req, + std::shared_ptr res) +{ + std::vector 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 req, + std::shared_ptr 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( diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt new file mode 100644 index 000000000..d836f843b --- /dev/null +++ b/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} +) diff --git a/modules/DroneControl/README.md b/modules/DroneControl/README.md new file mode 100644 index 000000000..2700208f3 --- /dev/null +++ b/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 +``` \ No newline at end of file diff --git a/modules/DroneControl/inc/dronecontrol.hpp b/modules/DroneControl/inc/dronecontrol.hpp new file mode 100644 index 000000000..fde61bade --- /dev/null +++ b/modules/DroneControl/inc/dronecontrol.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#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 droneTrajectories; // map from drone object-id to trajectory + + // Provided services + std::shared_ptr> 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); + + + +}; diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp new file mode 100644 index 000000000..bc294fa25 --- /dev/null +++ b/modules/DroneControl/inc/path.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include "tinyxml2.h" +#include +#include +#include + +namespace ABD +{ + class Segment + { + public: + ~Segment() = default; + std::string description; + float x; + float y; + float z; + float time; + float velocity; + int heading; + }; + + class Path + { + public: + Path(const std::string &path); + ~Path() = default; + float OriginLatitiude; + float OriginLongitude; + float OriginAltitude; + float bearing; + std::string path_to_file; + void getPath(void); + std::list offsetPath(float xOffset, float yOffset, float zOffset); + std::list segmentsList; + std::list dronePath; + }; + +} diff --git a/modules/DroneControl/src/dronecontrol.cpp b/modules/DroneControl/src/dronecontrol.cpp new file mode 100644 index 000000000..1856d01f2 --- /dev/null +++ b/modules/DroneControl/src/dronecontrol.cpp @@ -0,0 +1,82 @@ +#include "dronecontrol.hpp" +#include "path.hpp" + +using namespace ROSChannels; +using namespace std::placeholders; +using ObjectTrajectorySrv = atos_interfaces::srv::GetObjectTrajectory; + +DroneControl::DroneControl() : + Module(moduleName) +{ + // Entry-point to the module + + // Initialize services that this module provides to other modules + objectTrajectoryService = create_service(ServiceNames::getObjectTrajectory, + std::bind(&DroneControl::onRequestObjectTrajectory, this, _1, _2)); +} + +// Some module has requested the object trajectories +void DroneControl::onRequestObjectTrajectory(const ObjectTrajectorySrv::Request::SharedPtr req, + const ObjectTrajectorySrv::Response::SharedPtr res) +{ + + res->id = req->id; + try { + // TODO: Parse .path file + auto path = createPath("path/path.path"); + + // TODO: Create trajectory object for drone with id req->id + auto traj = createDroneTrajectory(path,req->id); + RCLCPP_INFO(get_logger(), "Esmini trajectory service called, returning trajectory for object %s", traj.toString().c_str()); + res->trajectory = traj.toCartesianTrajectory(); + res->success = true; + } + catch (std::out_of_range& e){ + RCLCPP_ERROR(get_logger(), "Esmini trajectory service called, no trajectory found for object %d", req->id); + res->success = false; + } +} + +// Creates Path objects from .path file +ABD::Path DroneControl::createPath(const std::string& path) +{ + return ABD::Path(path); +} +ATOS::Trajectory DroneControl::createDroneTrajectory(ABD::Path& path, uint32_t id) +{ + // TODO: Automate creation of multiple points + auto traj = ATOS::Trajectory(get_logger()); + + // The starting point + auto point1 = ATOS::Trajectory::TrajectoryPoint(get_logger()); + point1.setXCoord(0); + point1.setYCoord(0); + point1.setZCoord(0); + point1.setHeading(0); + point1.setLongitudinalVelocity(3); + point1.setLateralVelocity(3); + point1.setLongitudinalAcceleration(3); + point1.setLateralAcceleration(3); + point1.setTime(0); + traj.points.push_back(point1); + + // The ending point + auto point2 = ATOS::Trajectory::TrajectoryPoint(get_logger()); + point2.setXCoord(1); + point2.setYCoord(1); + point2.setZCoord(1); + point2.setHeading(0); + point2.setLongitudinalVelocity(0); + point2.setLateralVelocity(0); + point2.setLongitudinalAcceleration(0); + point2.setLateralAcceleration(0); + point2.setTime(100); + traj.points.push_back(point2); + + return traj; +} + + +DroneControl::~DroneControl() +{ +} diff --git a/modules/DroneControl/src/main.cpp b/modules/DroneControl/src/main.cpp new file mode 100644 index 000000000..20004445b --- /dev/null +++ b/modules/DroneControl/src/main.cpp @@ -0,0 +1,11 @@ +#include "rclcpp/rclcpp.hpp" +#include "dronecontrol.hpp" + + +int main(int argc, char** argv) { + rclcpp::init(argc,argv); + auto dc = std::make_shared(); + rclcpp::spin(dc); + rclcpp::shutdown(); + return 0; +} diff --git a/modules/DroneControl/src/path.cpp b/modules/DroneControl/src/path.cpp new file mode 100644 index 000000000..3605ea2a7 --- /dev/null +++ b/modules/DroneControl/src/path.cpp @@ -0,0 +1,105 @@ +#include "path.hpp" + +namespace ABD +{ + + Path::Path(const std::string &path) + { + // Todo construct path using a filepath to a .path file by calling getPath() + path_to_file = path; + } + + // This function writes to the path's segments list, returns nothing + void Path::getPath(void) + { + // load xml doc based on path argument + tinyxml2::XMLDocument doc; + doc.LoadFile(path_to_file.c_str()); + + // get root element ("Path" in this case) + tinyxml2::XMLNode *root = doc.RootElement(); + if (root == NULL) + { + return; + } + + // get OriginDatum element + tinyxml2::XMLNode *origin = root->FirstChildElement()->FirstChildElement(); + + // assign origin values from OriginDatum attributes + const tinyxml2::XMLAttribute *attribute = origin->ToElement()->FirstAttribute(); + + // gathering each attribute by steping through the header + // doing it this way may present issues for other xml files with different + OriginLatitiude = std::stof(attribute->Value(), NULL); + attribute = attribute->Next(); + OriginLongitude = std::stof(attribute->Value(), NULL); + attribute = attribute->Next(); + OriginAltitude = std::stof(attribute->Value(), NULL); + attribute = attribute->Next(); + bearing = std::stof(attribute->Value(), NULL); + + tinyxml2::XMLNode *segmentStart = origin->Parent()->NextSibling()->FirstChild(); + + + while (segmentStart) + { // iterate through all waypoints in the .path file + + Segment segment; + + // get segment description + const tinyxml2::XMLAttribute *description = + segmentStart->ToElement()->FirstAttribute(); + + segment.description = description->Value(); + + // get start point xyz values + const tinyxml2::XMLAttribute *startpoint = + segmentStart->FirstChild()->ToElement()->FirstAttribute(); + + segment.x = std::stof(startpoint->Value(), NULL); + startpoint = startpoint->Next(); + segment.y = std::stof(startpoint->Value(), NULL); + segment.z = 0; + + // get time + startpoint = startpoint->Next(); + segment.time = std::stof(startpoint->Value(), NULL); + + // get velocity + startpoint = startpoint->Next(); + segment.velocity = std::stof(startpoint->Value(), NULL); + + // get heading + startpoint = startpoint->Next(); + segment.heading = std::stoi(startpoint->Value(), NULL); + + // add to list + segmentsList.push_back(segment); + + // move to next segment + segmentStart = segmentStart->NextSibling(); + } + + } + + // This function returns a list of segments with the specified offset from the paths segment list + std::list Path::offsetPath(double xOffset, double yOffset, double zOffset) + { + // This will change to Path::drone path to fly above the segments list + std::list newPath; + std::list::iterator it; + for (it = segmentsList.begin(); it != segmentsList.end(); ++it) { + Segment s; + s.description = "Drone Path: " + it->description; + s.x = it->x + x_offset; + s.y = it->y + y_offset; + s.z = it->z + z_offset; + s.time = it->time; + s.velocity = it->velocity; + s.heading = it->heading; + newPath.push_back(s); + } + return newPath; + } +} \ No newline at end of file diff --git a/modules/ObjectControl/src/objectcontrol.cpp b/modules/ObjectControl/src/objectcontrol.cpp index a748151d9..61573f000 100644 --- a/modules/ObjectControl/src/objectcontrol.cpp +++ b/modules/ObjectControl/src/objectcontrol.cpp @@ -258,6 +258,7 @@ void ObjectControl::loadScenario() { objects.emplace(id, object); objects.at(id)->setTransmitterID(id); + // RESPONSE REQUIRED auto trajectoryCallback = [id, this](const rclcpp::Client::SharedFuture future) { auto trajResponse = future.get(); if (!trajResponse->success) { @@ -273,6 +274,7 @@ void ObjectControl::loadScenario() { trajRequest->id = id; trajectoryClient->async_send_request(trajRequest, trajectoryCallback); + // RESPONSE REQUIRED auto ipCallback = [id, this](const rclcpp::Client::SharedFuture future) { auto ipResponse = future.get(); if (!ipResponse->success) { @@ -290,7 +292,7 @@ void ObjectControl::loadScenario() { ipRequest->id = id; ipClient->async_send_request(ipRequest, ipCallback); - // Get delayed start + // REPONSE OPTIONAL auto triggerCallback = [id, this](const rclcpp::Client::SharedFuture future) { auto triggerResponse = future.get(); if (!triggerResponse->success) { @@ -304,7 +306,7 @@ void ObjectControl::loadScenario() { triggerRequest->id = id; triggerClient->async_send_request(triggerRequest, triggerCallback); - // Get test origin + // RESPONSE REQUIRED auto originCallback = [id, this](const rclcpp::Client::SharedFuture future) { auto origin = future.get(); if (!origin->success) {