Feature ros node test objects

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)

common/roschannels/monitorchannel.hpp

@@ -7,6 +7,9 @@
 
 #include "roschannel.hpp"
 #include "atos_interfaces/msg/monitor.hpp"
+#include "positioning.h"
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 namespace ROSChannels {
     namespace Monitor {
@@ -29,5 +32,49 @@ namespace ROSChannels {
                 objectId(id),
                 BaseSub<message_type>(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos) {}
         };
+        static message_type fromMonr(const uint32_t id, const ObjectMonitorType& monrMessage) {
+            atos_interfaces::msg::Monitor msg;
+            auto txid = id;
+            auto indata = monrMessage;
+            auto stamp = rclcpp::Time(indata.timestamp.tv_sec, indata.timestamp.tv_usec*1000);
+
+            // Set stamp for all subtypes
+            msg.atos_header.header.stamp = stamp;
+            msg.pose.header.stamp = stamp;
+            msg.velocity.header.stamp = stamp;
+            msg.acceleration.header.stamp = stamp;
+
+            // Set frame ids
+            msg.atos_header.header.frame_id = "map"; // TODO
+            msg.pose.header.frame_id = "map"; // TODO
+            msg.velocity.header.frame_id = "map"; // TODO vehicle local
+            msg.acceleration.header.frame_id = "map"; // TODO vehicle local
+
+            msg.atos_header.object_id = txid;
+            msg.object_state.state = indata.state;
+            if (indata.position.isPositionValid) {
+                msg.pose.pose.position.x = indata.position.xCoord_m;
+                msg.pose.pose.position.y = indata.position.yCoord_m;
+                msg.pose.pose.position.z = indata.position.isZcoordValid ? indata.position.zCoord_m : 0.0;
+            }
+            if (indata.position.isHeadingValid) {
+                tf2::Quaternion orientation;
+                orientation.setRPY(0, 0, indata.position.heading_rad);
+                msg.pose.pose.orientation = tf2::toMsg(orientation);
+            }
+            msg.velocity.twist.linear.x = indata.speed.isLongitudinalValid ? indata.speed.longitudinal_m_s : 0;
+            msg.velocity.twist.linear.y = indata.speed.isLateralValid ? indata.speed.lateral_m_s : 0;
+            msg.velocity.twist.linear.z = 0;
+            msg.velocity.twist.angular.x = 0;
+            msg.velocity.twist.angular.y = 0;
+            msg.velocity.twist.angular.z = 0;
+            msg.acceleration.accel.linear.x = indata.acceleration.isLongitudinalValid ? indata.acceleration.longitudinal_m_s2 : 0;
+            msg.acceleration.accel.linear.y = indata.acceleration.isLateralValid ? indata.acceleration.lateral_m_s2 : 0;
+            msg.acceleration.accel.linear.z = 0;
+            msg.acceleration.accel.angular.x = 0;
+            msg.acceleration.accel.angular.y = 0;
+            msg.acceleration.accel.angular.z = 0;
+            return msg;
+        }
     }
 }

common/roschannels/navsatfixchannel.hpp

@@ -7,6 +7,8 @@
 
 #include "roschannel.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
+#include "monitorchannel.hpp" // TODO: remove this when making translator node that translates monr to various other msg types..
+#include "util/coordinateutils.hpp" // TODO: also remove this
 
 namespace ROSChannels {
     namespace NavSatFix {
@@ -29,5 +31,23 @@ namespace ROSChannels {
                 objectId(id),
                 BaseSub<message_type>(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos) {}
         };
+        // TODO: Remove below..
+        static message_type fromMonr(std::array<double,3> origin, const ROSChannels::Monitor::message_type &monr) {
+            sensor_msgs::msg::NavSatFix msg;
+            msg.header.stamp = monr.atos_header.header.stamp;
+
+            // Local coordinates to global coordinates
+            double offset[3] = {monr.pose.pose.position.x, monr.pose.pose.position.y, monr.pose.pose.position.z};
+            double llh_0[3] = {origin[0], origin[1], origin[2]};
+            llhOffsetMeters(llh_0,offset);
+            msg.header.frame_id = "map"; // TODO
+
+            // Fill in the rest of the message
+            msg.latitude = llh_0[0];
+            msg.longitude = llh_0[1];
+            msg.altitude = llh_0[2];
+            msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
+            return msg;
+        }
     }
 }

common/roschannels/objstatechangechannel.hpp

@@ -0,0 +1,27 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "roschannel.hpp"
+#include "atos_interfaces/msg/object_state_change.hpp"
+
+namespace ROSChannels {
+    namespace ObjectStateChange {
+        const std::string topicName = "object_state_change";
+        using message_type = atos_interfaces::msg::ObjectStateChange;
+        const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll());
+
+        class Pub : public BasePub<message_type> {
+        public:
+            Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub<message_type>(node, topicName, qos) {}
+        };
+
+        class Sub : public BaseSub<message_type> {
+        public:
+            Sub(rclcpp::Node& node, std::function<void(const message_type::SharedPtr)> callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub<message_type>(node, topicName, callback, qos) {}
+        };
+    }
+}

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,51 @@
+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)
+
+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}
+)
+
+target_include_directories(${DRONE_CONTROL_TARGET} PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}/inc
+	${COMMON_HEADERS}
+)
+
+# 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
+```