From 2d216aaf88eac5402ebe26f4f3c7b00fbde1c23d Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 09:46:35 +0200 Subject: [PATCH 01/22] modify the way IP addresses are loaded, now read from .opro files --- modules/ATOSBase/inc/ATOSbase.hpp | 7 ++++- modules/ATOSBase/src/ATOSbase.cpp | 51 ++++++++++++++++++++++++++----- 2 files changed, 49 insertions(+), 9 deletions(-) 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 5e3b4f8df..2ba31e7a7 100644 --- a/modules/ATOSBase/src/ATOSbase.cpp +++ b/modules/ATOSBase/src/ATOSbase.cpp @@ -31,6 +31,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 +82,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 +94,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 +105,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 +116,42 @@ void ATOSBase::onRequestObjectIDs( throw std::invalid_argument(errMsg); } } + return objectIps; +} + +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; + } +} - res->ids = objectIDs; +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 = objinfo[req->id]; + } + catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to get object IPs: %s", e.what()); + res->success = false; + } } void ATOSBase::onRequestTestOrigin( From 261f1d845723153782dff94783a232aa07e43f15 Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 09:48:58 +0200 Subject: [PATCH 02/22] added DroneControl module with some boilerplate code --- CMakeLists.txt | 4 ++ .../CMakeLists.txt | 20 +++---- .../README.md | 0 modules/DroneControl/inc/dronecontrol.hpp | 41 ++++++++++++++ modules/DroneControl/inc/path.hpp | 23 ++++++++ modules/DroneControl/src/dronecontrol.cpp | 55 +++++++++++++++++++ .../src/main.cpp | 4 +- modules/DroneControl/src/path.cpp | 15 +++++ .../SampleModule copy/inc/samplemodule.hpp | 31 ----------- .../SampleModule copy/src/samplemodule.cpp | 55 ------------------- 10 files changed, 150 insertions(+), 98 deletions(-) rename modules/{SampleModule copy => DroneControl}/CMakeLists.txt (66%) rename modules/{SampleModule copy => DroneControl}/README.md (100%) create mode 100644 modules/DroneControl/inc/dronecontrol.hpp create mode 100644 modules/DroneControl/inc/path.hpp create mode 100644 modules/DroneControl/src/dronecontrol.cpp rename modules/{SampleModule copy => DroneControl}/src/main.cpp (66%) create mode 100644 modules/DroneControl/src/path.cpp delete mode 100644 modules/SampleModule copy/inc/samplemodule.hpp delete mode 100644 modules/SampleModule copy/src/samplemodule.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 202f1fb63..c35589cc2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,6 +36,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") @@ -64,6 +65,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/modules/SampleModule copy/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt similarity index 66% rename from modules/SampleModule copy/CMakeLists.txt rename to modules/DroneControl/CMakeLists.txt index 06eba02e5..828352ec6 100644 --- a/modules/SampleModule copy/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -4,46 +4,46 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) -project(sample_module) +project(DRONE_CONTROL) find_package(atos_interfaces REQUIRED) # Define target names -set(SAMPLE_MODULE_TARGET ${PROJECT_NAME}) +set(DRONE_CONTROL_TARGET ${PROJECT_NAME}) set(COMMON_LIBRARY ATOSCommon) # Common library for ATOS with e.g. Trajectory class -set(SOCKET_LIBRARY TCPUDPSocket) # Socket library for TCP/UDP communication get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES) include(GNUInstallDirs) # Create project main executable target -add_executable(${SAMPLE_MODULE_TARGET} +add_executable(${DRONE_CONTROL_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/samplemodule.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/dronecontrol.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/path.cpp ) # Link project executable to common libraries -target_link_libraries(${SAMPLE_MODULE_TARGET} +target_link_libraries(${DRONE_CONTROL_TARGET} ${COREUTILS_LIBRARY} ${SOCKET_LIBRARY} ${COMMON_LIBRARY} ) -target_include_directories(${SAMPLE_MODULE_TARGET} PUBLIC +target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${COMMON_HEADERS} ) # ROS specific rules -ament_target_dependencies(${SAMPLE_MODULE_TARGET} +ament_target_dependencies(${DRONE_CONTROL_TARGET} rclcpp std_msgs atos_interfaces ) # Installation rules -install(CODE "MESSAGE(STATUS \"Installing target ${SAMPLE_MODULE_TARGET}\")") -install(TARGETS ${SAMPLE_MODULE_TARGET} +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} diff --git a/modules/SampleModule copy/README.md b/modules/DroneControl/README.md similarity index 100% rename from modules/SampleModule copy/README.md rename to modules/DroneControl/README.md diff --git a/modules/DroneControl/inc/dronecontrol.hpp b/modules/DroneControl/inc/dronecontrol.hpp new file mode 100644 index 000000000..a0d48b79e --- /dev/null +++ b/modules/DroneControl/inc/dronecontrol.hpp @@ -0,0 +1,41 @@ +#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::vector droneTrajectories; + + // State subscribers + ROSChannels::Init::Sub initSub; + + // State callbacks + void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; + + // Provided services + std::shared_ptr> objectTrajectoryService; + + // Service callbacks + void onRequestObjectTrajectory(const atos_interfaces::srv::GetObjectTrajectory::Request::SharedPtr request, + const atos_interfaces::srv::GetObjectTrajectory::Response::SharedPtr response); + + // Business logic + ABD::Path createPath(const std::string& path); + std::vector createDroneTrajectories(ABD::Path&); + + + +}; diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp new file mode 100644 index 000000000..84ce8c3ce --- /dev/null +++ b/modules/DroneControl/inc/path.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +namespace ABD +{ + class Segment{ + public: + Segment(); + ~Segment() = default; + }; + + class Path{ + public: + Path(const std::string& path); + ~Path() = default; + + private: + std::vector segments; + }; + +} diff --git a/modules/DroneControl/src/dronecontrol.cpp b/modules/DroneControl/src/dronecontrol.cpp new file mode 100644 index 000000000..f3f5806f3 --- /dev/null +++ b/modules/DroneControl/src/dronecontrol.cpp @@ -0,0 +1,55 @@ +#include "dronecontrol.hpp" +#include "roschannels/commandchannels.hpp" +#include "path.hpp" + +using namespace ROSChannels; +using namespace std::placeholders; +using ObjectTrajectorySrv = atos_interfaces::srv::GetObjectTrajectory; + +DroneControl::DroneControl() : + Module(moduleName), + initSub(*this, std::bind(&DroneControl::onInitMessage, this, _1)) +{ + // 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)); +} + +// Object control requests the object trajectories +void DroneControl::onRequestObjectTrajectory(const ObjectTrajectorySrv::Request::SharedPtr request, + const ObjectTrajectorySrv::Response::SharedPtr response) +{ + +} + +// Creates Path objects from .path file +ABD::Path DroneControl::createPath(const std::string& path) +{ + +} + +std::vector DroneControl::createDroneTrajectories(ABD::Path& path) +{ + // TODO: Create trajectory object(s) for drone(s) + // droneTrajectories = somefunction(path); +} + + +DroneControl::~DroneControl() +{ +} + +//! Init command callback +void DroneControl::onInitMessage(const Init::message_type::SharedPtr) +{ + // Callback executed when this module receives a test-is-initialized command + + // TODO: Parse .path file + auto path = createPath("path/path.path"); + + // TODO: Create trajectory object(s) for drone(s) + droneTrajectories = createDroneTrajectories(path); +} + diff --git a/modules/SampleModule copy/src/main.cpp b/modules/DroneControl/src/main.cpp similarity index 66% rename from modules/SampleModule copy/src/main.cpp rename to modules/DroneControl/src/main.cpp index fcdbc8f07..e804ab2ce 100644 --- a/modules/SampleModule copy/src/main.cpp +++ b/modules/DroneControl/src/main.cpp @@ -1,10 +1,10 @@ #include "rclcpp/rclcpp.hpp" -#include "samplemodule.hpp" +#include "dronecontrol.hpp" int main(int argc, char** argv) { rclcpp::init(argc,argv); - auto sm = std::make_shared(); + auto sm = std::make_shared(); rclcpp::spin(sm); rclcpp::shutdown(); return 0; diff --git a/modules/DroneControl/src/path.cpp b/modules/DroneControl/src/path.cpp new file mode 100644 index 000000000..e7c543fe0 --- /dev/null +++ b/modules/DroneControl/src/path.cpp @@ -0,0 +1,15 @@ +#include "path.hpp" + +namespace ABD{ + + Path::Path(const std::string& path) + { + // Todo construct path using a filepath to a .path file + } + + Segment::Segment() + { + // Todo construct segment + } + +} \ No newline at end of file diff --git a/modules/SampleModule copy/inc/samplemodule.hpp b/modules/SampleModule copy/inc/samplemodule.hpp deleted file mode 100644 index 5bcf1663a..000000000 --- a/modules/SampleModule copy/inc/samplemodule.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include -#include "module.hpp" -#include "server.hpp" - -/*! - * \brief The SampleModule is a ros2 node that demonstrates how to use the Module class - */ -class SampleModule : public Module{ -public: - static inline std::string const moduleName = "sample_module"; - SampleModule(); - ~SampleModule(); - -private: - static inline const int TCPPort = 1337; - - void tcpSocketProcedure(); - ROSChannels::Init::Sub initSub; - ROSChannels::Abort::Sub abortSub; - ROSChannels::AllClear::Sub allClearSub; - - void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; - void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) override; - void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr) override; - - std::unique_ptr tcpThread; - TCPServer tcpServer; - bool quit = false; -}; diff --git a/modules/SampleModule copy/src/samplemodule.cpp b/modules/SampleModule copy/src/samplemodule.cpp deleted file mode 100644 index d5776ca47..000000000 --- a/modules/SampleModule copy/src/samplemodule.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "samplemodule.hpp" -#include "roschannel.hpp" -#include "rclcpp/wait_for_message.hpp" - -using namespace ROSChannels; -using namespace std::chrono_literals; -using namespace std::placeholders; - -SampleModule::SampleModule() : - Module(moduleName), - initSub(*this, std::bind(&SampleModule::onInitMessage, this, _1)), - abortSub(*this, std::bind(&SampleModule::onAbortMessage, this, _1)), - allClearSub(*this, std::bind(&SampleModule::onAllClearMessage, this, _1)), - tcpServer("0.0.0.0",TCPPort) -{ - tcpThread = std::make_unique(&SampleModule::tcpSocketProcedure, this); - RCLCPP_INFO(get_logger(), "%s task running with PID: %d",moduleName.c_str(), getpid()); -} - -SampleModule::~SampleModule() { - // When the SampleModule object is destroyed, make sure that socket thread is joined - quit = true; - if (tcpThread != nullptr) { - tcpThread->join(); - } -} - -//! Message queue callbacks -void SampleModule::onInitMessage(const Init::message_type::SharedPtr) { - // Callback executed when this module receives a test is initialized - - // Get the object id of the objects in the scenario - ConnectedObjectIds::message_type connectedObjectIds; - // Wait for a single message containing a vector of ids, at most 1000ms - rclcpp::wait_for_message(connectedObjectIds, shared_from_this(), std::string(get_namespace()) + "/connected_object_ids", 1000ms); - // Do something with IDs -} - -void SampleModule::onAbortMessage(const Abort::message_type::SharedPtr) {} - -void SampleModule::onAllClearMessage(const AllClear::message_type::SharedPtr) {} - -/*! \brief This function is executed in a separate thread - * It is used to handle incoming TCP connections - * and receive data from them -*/ -void SampleModule::tcpSocketProcedure() { - while (!quit) { - auto socket = tcpServer.await(); - auto bytes = socket.recv(); - for (auto b : bytes) { - RCLCPP_INFO(get_logger(), "Received byte: %d", b); - } - } -} From 5e6797609177a1c20d1bffd55a5b667f9ea4c5fc Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 09:49:49 +0200 Subject: [PATCH 03/22] commented required/optional service calls in OBC --- modules/ObjectControl/src/objectcontrol.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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) { From 6e2d5cd50cd9625c4c68cc3ac6578fa76d658024 Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 09:52:24 +0200 Subject: [PATCH 04/22] updated variable naming --- modules/DroneControl/src/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/DroneControl/src/main.cpp b/modules/DroneControl/src/main.cpp index e804ab2ce..20004445b 100644 --- a/modules/DroneControl/src/main.cpp +++ b/modules/DroneControl/src/main.cpp @@ -4,8 +4,8 @@ int main(int argc, char** argv) { rclcpp::init(argc,argv); - auto sm = std::make_shared(); - rclcpp::spin(sm); + auto dc = std::make_shared(); + rclcpp::spin(dc); rclcpp::shutdown(); return 0; } From 680ae5ed8f22d78f2267d73aeb97bb44329d67ef Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 12:40:18 +0200 Subject: [PATCH 05/22] remove esminiAdapter and add DroneControl in launch file --- launch/launch_utils/launch_base.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/launch/launch_utils/launch_base.py b/launch/launch_utils/launch_base.py index cf249cfec..2bf81e4d7 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( From 8c534a5528de4081e7ce278331755c2c26830dc7 Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 12:40:51 +0200 Subject: [PATCH 06/22] send a string ip address instead of an uint32_t --- modules/ATOSBase/src/ATOSbase.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/modules/ATOSBase/src/ATOSbase.cpp b/modules/ATOSBase/src/ATOSbase.cpp index 2ba31e7a7..22fe91fc5 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 @@ -146,7 +147,8 @@ void ATOSBase::onRequestObjectIP( if (objinfo.find(req->id) == objinfo.end()) { throw std::invalid_argument("Object ID not found"); } - res->ip = objinfo[req->id]; + 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()); From 1cef9315e6666afc5c76481531c170f843b4572a Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 12:41:12 +0200 Subject: [PATCH 07/22] correctly named module --- modules/DroneControl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index 828352ec6..248d15cb9 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -4,7 +4,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) -project(DRONE_CONTROL) +project(drone_control) find_package(atos_interfaces REQUIRED) # Define target names From fac0452eb7129dddb2550bd044eee473fc1ed7d0 Mon Sep 17 00:00:00 2001 From: Victor Jarlow Date: Sun, 2 Apr 2023 12:47:26 +0200 Subject: [PATCH 08/22] DroneControl responds to service call by producing a drone trajectory --- modules/DroneControl/inc/dronecontrol.hpp | 16 ++--- modules/DroneControl/src/dronecontrol.cpp | 75 +++++++++++++++-------- 2 files changed, 56 insertions(+), 35 deletions(-) diff --git a/modules/DroneControl/inc/dronecontrol.hpp b/modules/DroneControl/inc/dronecontrol.hpp index a0d48b79e..fde61bade 100644 --- a/modules/DroneControl/inc/dronecontrol.hpp +++ b/modules/DroneControl/inc/dronecontrol.hpp @@ -17,24 +17,18 @@ class DroneControl : public Module{ private: // Member variables - std::vector droneTrajectories; - - // State subscribers - ROSChannels::Init::Sub initSub; - - // State callbacks - void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; + 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 request, - const atos_interfaces::srv::GetObjectTrajectory::Response::SharedPtr response); + void onRequestObjectTrajectory(const atos_interfaces::srv::GetObjectTrajectory::Request::SharedPtr, + const atos_interfaces::srv::GetObjectTrajectory::Response::SharedPtr); // Business logic - ABD::Path createPath(const std::string& path); - std::vector createDroneTrajectories(ABD::Path&); + ABD::Path createPath(const std::string&); + ATOS::Trajectory createDroneTrajectory(ABD::Path&, uint32_t); diff --git a/modules/DroneControl/src/dronecontrol.cpp b/modules/DroneControl/src/dronecontrol.cpp index f3f5806f3..1856d01f2 100644 --- a/modules/DroneControl/src/dronecontrol.cpp +++ b/modules/DroneControl/src/dronecontrol.cpp @@ -1,5 +1,4 @@ #include "dronecontrol.hpp" -#include "roschannels/commandchannels.hpp" #include "path.hpp" using namespace ROSChannels; @@ -7,8 +6,7 @@ using namespace std::placeholders; using ObjectTrajectorySrv = atos_interfaces::srv::GetObjectTrajectory; DroneControl::DroneControl() : - Module(moduleName), - initSub(*this, std::bind(&DroneControl::onInitMessage, this, _1)) + Module(moduleName) { // Entry-point to the module @@ -17,39 +15,68 @@ DroneControl::DroneControl() : std::bind(&DroneControl::onRequestObjectTrajectory, this, _1, _2)); } -// Object control requests the object trajectories -void DroneControl::onRequestObjectTrajectory(const ObjectTrajectorySrv::Request::SharedPtr request, - const ObjectTrajectorySrv::Response::SharedPtr response) +// 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); } - -std::vector DroneControl::createDroneTrajectories(ABD::Path& path) +ATOS::Trajectory DroneControl::createDroneTrajectory(ABD::Path& path, uint32_t id) { - // TODO: Create trajectory object(s) for drone(s) - // droneTrajectories = somefunction(path); -} + // 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); -DroneControl::~DroneControl() -{ -} + // 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); -//! Init command callback -void DroneControl::onInitMessage(const Init::message_type::SharedPtr) -{ - // Callback executed when this module receives a test-is-initialized command + return traj; +} - // TODO: Parse .path file - auto path = createPath("path/path.path"); - // TODO: Create trajectory object(s) for drone(s) - droneTrajectories = createDroneTrajectories(path); +DroneControl::~DroneControl() +{ } - From 4e54dc9a558c6be689f050af7c9c8214f2fee3c1 Mon Sep 17 00:00:00 2001 From: dandeandean Date: Wed, 12 Apr 2023 08:51:17 -0400 Subject: [PATCH 09/22] added CMake for tinyXML2 --- cmake/modules/FindTinyXML2.cmake | 74 +++++++++++++++++++++++++++++ modules/DroneControl/CMakeLists.txt | 6 ++- 2 files changed, 79 insertions(+), 1 deletion(-) create mode 100644 cmake/modules/FindTinyXML2.cmake 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/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index 248d15cb9..c74e87dd6 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -4,9 +4,11 @@ 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) # TODO: FIND PACKAGE FOR TINYXML2 + project(drone_control) find_package(atos_interfaces REQUIRED) - # Define target names set(DRONE_CONTROL_TARGET ${PROJECT_NAME}) @@ -22,11 +24,13 @@ add_executable(${DRONE_CONTROL_TARGET} ${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_LIBRARY} #TODO: ADD LIB TINYXML ) target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC From 2bc4aeeb85d60f4b0044481c6e954a61d10a60f5 Mon Sep 17 00:00:00 2001 From: dandeandean Date: Wed, 19 Apr 2023 11:46:17 -0400 Subject: [PATCH 10/22] added in the class definitions --- modules/DroneControl/inc/path.hpp | 38 +++++++++++++++++++++---------- 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp index 84ce8c3ce..af62c320f 100644 --- a/modules/DroneControl/inc/path.hpp +++ b/modules/DroneControl/inc/path.hpp @@ -2,22 +2,36 @@ #include #include +#include +#include "tinyxml2.h" +#include +#include +#include +#include -namespace ABD +namespace ABD { - class Segment{ - public: - Segment(); - ~Segment() = default; + class Segment + { + public: + Segment(); + ~Segment() = default; }; - class Path{ - public: - Path(const std::string& path); - ~Path() = default; - - private: - std::vector segments; + class Path + { + public: + Path(const std::string &path); + ~Path() = default; + float OriginLatitiude; + float OriginLongitude; + float OriginAltitude; + float bearing; + std::string path_to_file; + void get_path(void); + void get_traj(float x_offset, float y_offset, float z_offset); + std::list segmentsList; + std::list dronePath; }; } From ae4176912c4d232ca5ac6e2518d4e6414008e248 Mon Sep 17 00:00:00 2001 From: dandeandean Date: Thu, 20 Apr 2023 09:19:59 -0400 Subject: [PATCH 11/22] added path messages --- modules/DroneControl/src/path.cpp | 102 +++++++++++++++++++++++++++++- 1 file changed, 100 insertions(+), 2 deletions(-) diff --git a/modules/DroneControl/src/path.cpp b/modules/DroneControl/src/path.cpp index e7c543fe0..b31b1aa4c 100644 --- a/modules/DroneControl/src/path.cpp +++ b/modules/DroneControl/src/path.cpp @@ -1,15 +1,113 @@ #include "path.hpp" -namespace ABD{ +namespace ABD +{ - Path::Path(const std::string& path) + Path::Path(const std::string &path) { // Todo construct path using a filepath to a .path file + path_to_file = path; } + 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) + { + //cout << "root in NULL, check the path" << endl; + return; + } + + // get OriginDatum element + tinyxml2::XMLNode *origin = root->FirstChildElement()->FirstChildElement(); + //cout << origin->ToElement()->Name() << endl; + + // assign origin values from OriginDatum attributes + const tinyxml2::XMLAttribute *attribute = origin->ToElement()->FirstAttribute(); + + // const XMLAttribute *attribute = element->FindAttribute("OriginLatitiude"); + // trying to get search to work instead + // not having search may present issues for other xml files with different + // formats + + OriginLatitiude = std::stof(attribute->Value(), NULL); + //cout << attribute->Name() << ": " << OriginLatitiude << endl; + + attribute = attribute->Next(); + OriginLongitude = std::stof(attribute->Value(), NULL); + //cout << attribute->Name() << ": " << OriginLongitude << endl; + + attribute = attribute->Next(); + OriginAltitude = std::stof(attribute->Value(), NULL); + //cout << attribute->Name() << ": " << OriginAltitude << endl;I + + attribute = attribute->Next(); + bearing = std::stof(attribute->Value(), NULL); + //cout << attribute->Name() << ": " << bearing << endl; + + tinyxml2::XMLNode *segmentStart = origin->Parent()->NextSibling()->FirstChild(); + + + while (segmentStart) + { // while loop for all waypoints + + Segment segment; + + // get segment description + const tinyxml2::XMLAttribute *description = + segmentStart->ToElement()->FirstAttribute(); + + segment.description = description->Value(); + + //cout << description->Name() << ": " << segment.description << endl; + + // 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(); + } + + } + + /* Segment::Segment() { // Todo construct segment } + */ } \ No newline at end of file From 802845d4d9d7f6ee93d0afb7df914b8c8353f571 Mon Sep 17 00:00:00 2001 From: dandeandean Date: Thu, 20 Apr 2023 11:12:59 -0400 Subject: [PATCH 12/22] changed cmake to targets --- modules/DroneControl/CMakeLists.txt | 2 ++ modules/DroneControl/inc/path.hpp | 12 +++++++++--- modules/DroneControl/src/dronecontrol.cpp | 1 + 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index c74e87dd6..206c20cd2 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -23,6 +23,7 @@ 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 + #${CMAKE_CURRENT_SOURCE_DIR}/src/tinyxml2.cpp ) # Link project executable to common libraries @@ -36,6 +37,7 @@ target_link_libraries(${DRONE_CONTROL_TARGET} target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${COMMON_HEADERS} + ${tinyxml2_INCLUDE_DIRS} #added ) # ROS specific rules diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp index af62c320f..95db0130d 100644 --- a/modules/DroneControl/inc/path.hpp +++ b/modules/DroneControl/inc/path.hpp @@ -14,8 +14,14 @@ namespace ABD class Segment { public: - Segment(); ~Segment() = default; + std::string description; + float x; + float y; + float z; + float time; + float velocity; + int heading; }; class Path @@ -28,8 +34,8 @@ namespace ABD float OriginAltitude; float bearing; std::string path_to_file; - void get_path(void); - void get_traj(float x_offset, float y_offset, float z_offset); + void getPath(void); + void getTraj(float x_offset, float y_offset, float z_offset); std::list segmentsList; std::list dronePath; }; diff --git a/modules/DroneControl/src/dronecontrol.cpp b/modules/DroneControl/src/dronecontrol.cpp index 1856d01f2..8571d6f9b 100644 --- a/modules/DroneControl/src/dronecontrol.cpp +++ b/modules/DroneControl/src/dronecontrol.cpp @@ -27,6 +27,7 @@ void DroneControl::onRequestObjectTrajectory(const ObjectTrajectorySrv::Request: // TODO: Create trajectory object for drone with id req->id auto traj = createDroneTrajectory(path,req->id); + // EXAMPLE OF LOGGING TO TERMINAL RCLCPP_INFO(get_logger(), "Esmini trajectory service called, returning trajectory for object %s", traj.toString().c_str()); res->trajectory = traj.toCartesianTrajectory(); res->success = true; From 918a8d5988aebd0fc79a488851b24b5511683a30 Mon Sep 17 00:00:00 2001 From: dandeandean Date: Thu, 20 Apr 2023 13:10:54 -0400 Subject: [PATCH 13/22] Cmake fixed --- modules/DroneControl/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index 206c20cd2..3a8fff11c 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -31,13 +31,13 @@ target_link_libraries(${DRONE_CONTROL_TARGET} ${COREUTILS_LIBRARY} ${SOCKET_LIBRARY} ${COMMON_LIBRARY} - ${TINYXML2_LIBRARY} #TODO: ADD LIB TINYXML + ${TinyXML2_LIBRARIES} #TODO: ADD LIB TINYXML ) target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${COMMON_HEADERS} - ${tinyxml2_INCLUDE_DIRS} #added + ${TinyXML2_INCLUDE_DIRS} #added ) # ROS specific rules From ec1e49974a906962ac007ec117eb6b0d82f2cdc6 Mon Sep 17 00:00:00 2001 From: dandeandean Date: Mon, 15 May 2023 06:10:00 -0400 Subject: [PATCH 14/22] discarded unused lines and added offset path --- modules/DroneControl/inc/path.hpp | 2 +- modules/DroneControl/src/path.cpp | 52 +++++++++++++------------------ 2 files changed, 23 insertions(+), 31 deletions(-) diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp index 95db0130d..9c6aafeba 100644 --- a/modules/DroneControl/inc/path.hpp +++ b/modules/DroneControl/inc/path.hpp @@ -35,7 +35,7 @@ namespace ABD float bearing; std::string path_to_file; void getPath(void); - void getTraj(float x_offset, float y_offset, float z_offset); + std::list offsetPath(float x_offset, float y_offset, float z_offset); std::list segmentsList; std::list dronePath; }; diff --git a/modules/DroneControl/src/path.cpp b/modules/DroneControl/src/path.cpp index b31b1aa4c..f3c7a29d1 100644 --- a/modules/DroneControl/src/path.cpp +++ b/modules/DroneControl/src/path.cpp @@ -5,13 +5,13 @@ namespace ABD Path::Path(const std::string &path) { - // Todo construct path using a filepath to a .path file + // 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()); @@ -20,42 +20,30 @@ namespace ABD tinyxml2::XMLNode *root = doc.RootElement(); if (root == NULL) { - //cout << "root in NULL, check the path" << endl; return; } // get OriginDatum element tinyxml2::XMLNode *origin = root->FirstChildElement()->FirstChildElement(); - //cout << origin->ToElement()->Name() << endl; // assign origin values from OriginDatum attributes const tinyxml2::XMLAttribute *attribute = origin->ToElement()->FirstAttribute(); - // const XMLAttribute *attribute = element->FindAttribute("OriginLatitiude"); - // trying to get search to work instead - // not having search may present issues for other xml files with different - // formats - + // 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); - //cout << attribute->Name() << ": " << OriginLatitiude << endl; - attribute = attribute->Next(); OriginLongitude = std::stof(attribute->Value(), NULL); - //cout << attribute->Name() << ": " << OriginLongitude << endl; - attribute = attribute->Next(); OriginAltitude = std::stof(attribute->Value(), NULL); - //cout << attribute->Name() << ": " << OriginAltitude << endl;I - attribute = attribute->Next(); bearing = std::stof(attribute->Value(), NULL); - //cout << attribute->Name() << ": " << bearing << endl; tinyxml2::XMLNode *segmentStart = origin->Parent()->NextSibling()->FirstChild(); while (segmentStart) - { // while loop for all waypoints + { // iterate through all waypoints in the .path file Segment segment; @@ -65,8 +53,6 @@ namespace ABD segment.description = description->Value(); - //cout << description->Name() << ": " << segment.description << endl; - // get start point xyz values const tinyxml2::XMLAttribute *startpoint = segmentStart->FirstChild()->ToElement()->FirstAttribute(); @@ -76,22 +62,16 @@ namespace ABD 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 @@ -103,11 +83,23 @@ namespace ABD } - /* - Segment::Segment() + // This function returns a list of segments with the specified offset from the paths segment list + std::list Path::offsetPath(float x_offset, float y_offset, float z_offset) { - // Todo construct segment + // 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 From 288877819d0bd6a1066389cecf9a50b64a5db049 Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 13:26:08 +0200 Subject: [PATCH 15/22] Update modules/DroneControl/CMakeLists.txt --- modules/DroneControl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index 3a8fff11c..6982cb216 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -5,7 +5,7 @@ set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) # Adding tinyxml2 to the CMake -find_package(TinyXML2 REQUIRED) # TODO: FIND PACKAGE FOR TINYXML2 +find_package(TinyXML2 REQUIRED) project(drone_control) find_package(atos_interfaces REQUIRED) From 891ebc41593a99f31be3e7ebc413b7ce3254ce01 Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 13:26:15 +0200 Subject: [PATCH 16/22] Update modules/DroneControl/CMakeLists.txt --- modules/DroneControl/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index 6982cb216..97c7e1ab1 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -23,7 +23,6 @@ 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 - #${CMAKE_CURRENT_SOURCE_DIR}/src/tinyxml2.cpp ) # Link project executable to common libraries From 3c1913c06df58b580ba78bbeb7f6da13f71dc8f3 Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 13:26:21 +0200 Subject: [PATCH 17/22] Update modules/DroneControl/CMakeLists.txt --- modules/DroneControl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index 97c7e1ab1..bb97d1111 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -30,7 +30,7 @@ target_link_libraries(${DRONE_CONTROL_TARGET} ${COREUTILS_LIBRARY} ${SOCKET_LIBRARY} ${COMMON_LIBRARY} - ${TinyXML2_LIBRARIES} #TODO: ADD LIB TINYXML + ${TinyXML2_LIBRARIES} ) target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC From 5c9e3dfac6783cb26cb5b391298e0e53e9f1a76e Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 13:26:27 +0200 Subject: [PATCH 18/22] Update modules/DroneControl/CMakeLists.txt --- modules/DroneControl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt index bb97d1111..d836f843b 100644 --- a/modules/DroneControl/CMakeLists.txt +++ b/modules/DroneControl/CMakeLists.txt @@ -36,7 +36,7 @@ target_link_libraries(${DRONE_CONTROL_TARGET} target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${COMMON_HEADERS} - ${TinyXML2_INCLUDE_DIRS} #added + ${TinyXML2_INCLUDE_DIRS} ) # ROS specific rules From 6a8e9fdc8f68bfb5d2d7eb40e8c55737aaf3f5bb Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 13:26:32 +0200 Subject: [PATCH 19/22] Update modules/DroneControl/inc/path.hpp --- modules/DroneControl/inc/path.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp index 9c6aafeba..328661262 100644 --- a/modules/DroneControl/inc/path.hpp +++ b/modules/DroneControl/inc/path.hpp @@ -5,7 +5,6 @@ #include #include "tinyxml2.h" #include -#include #include #include From 5693f3628be15d67cea484b0174af131c83ab958 Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 13:26:42 +0200 Subject: [PATCH 20/22] Update modules/DroneControl/src/dronecontrol.cpp --- modules/DroneControl/src/dronecontrol.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/DroneControl/src/dronecontrol.cpp b/modules/DroneControl/src/dronecontrol.cpp index 8571d6f9b..1856d01f2 100644 --- a/modules/DroneControl/src/dronecontrol.cpp +++ b/modules/DroneControl/src/dronecontrol.cpp @@ -27,7 +27,6 @@ void DroneControl::onRequestObjectTrajectory(const ObjectTrajectorySrv::Request: // TODO: Create trajectory object for drone with id req->id auto traj = createDroneTrajectory(path,req->id); - // EXAMPLE OF LOGGING TO TERMINAL RCLCPP_INFO(get_logger(), "Esmini trajectory service called, returning trajectory for object %s", traj.toString().c_str()); res->trajectory = traj.toCartesianTrajectory(); res->success = true; From 89c3fd9a6d39925e31dd181d190b1b6c95141ade Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 13:26:47 +0200 Subject: [PATCH 21/22] Update modules/DroneControl/inc/path.hpp --- modules/DroneControl/inc/path.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp index 328661262..bc294fa25 100644 --- a/modules/DroneControl/inc/path.hpp +++ b/modules/DroneControl/inc/path.hpp @@ -34,7 +34,7 @@ namespace ABD float bearing; std::string path_to_file; void getPath(void); - std::list offsetPath(float x_offset, float y_offset, float z_offset); + std::list offsetPath(float xOffset, float yOffset, float zOffset); std::list segmentsList; std::list dronePath; }; From 574d09e327532276d68ecf6135578c165288f095 Mon Sep 17 00:00:00 2001 From: victorjarlow <97448034+victorjarlow@users.noreply.github.com> Date: Fri, 19 May 2023 14:36:36 +0200 Subject: [PATCH 22/22] Update modules/DroneControl/src/path.cpp --- modules/DroneControl/src/path.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneControl/src/path.cpp b/modules/DroneControl/src/path.cpp index f3c7a29d1..3605ea2a7 100644 --- a/modules/DroneControl/src/path.cpp +++ b/modules/DroneControl/src/path.cpp @@ -84,7 +84,7 @@ namespace ABD } // This function returns a list of segments with the specified offset from the paths segment list - std::list Path::offsetPath(float x_offset, float y_offset, float z_offset) + 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;