diff --git a/.gitmodules b/.gitmodules index d92292b26..6cff3e1ec 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "atos/iso22133"] path = atos/iso22133 - url = https://github.com/RI-SE/iso22133.git + url = git@github.com:RI-SE/iso22133.git [submodule "atos_interfaces"] path = atos_interfaces - url = https://github.com/RI-SE/atos_interfaces.git + url = git@github.com:RI-SE/atos_interfaces.git diff --git a/atos/CMakeLists.txt b/atos/CMakeLists.txt index 0945133c7..8b9144045 100644 --- a/atos/CMakeLists.txt +++ b/atos/CMakeLists.txt @@ -65,6 +65,7 @@ set(WITH_INTEGRATION_TESTING ON CACHE BOOL "Enable IntegrationTesting module") set(WITH_BACK_TO_START ON CACHE BOOL "Enable BackToStart module") set(WITH_OPEN_SCENARIO_GATEWAY ON CACHE BOOL "Enable OpenScenario Gateway module") set(WITH_REST_BRIDGE ON CACHE BOOL "Enable RESTBridge module") +set(WITH_MONR_RELAY ON CACHE BOOL "Enable MonrRelay module") set(ENABLE_TESTS ON CACHE BOOL "Enable testing on build") @@ -102,6 +103,9 @@ endif() if(WITH_REST_BRIDGE) list(APPEND ENABLED_MODULES RESTBridge) endif() +if(WITH_MONR_RELAY) + list(APPEND ENABLED_MODULES MonrRelay) +endif() # Add corresponding subprojects diff --git a/atos/common/roschannels/monitorchannel.hpp b/atos/common/roschannels/monitorchannel.hpp index f5d957dfd..8d32b6ddf 100644 --- a/atos/common/roschannels/monitorchannel.hpp +++ b/atos/common/roschannels/monitorchannel.hpp @@ -5,9 +5,9 @@ */ #pragma once -#include "roschannel.hpp" #include "atos_interfaces/msg/monitor.hpp" #include "positioning.h" +#include "roschannel.hpp" #include #if ROS_FOXY #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -15,114 +15,138 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif - namespace ROSChannels { - namespace Monitor { - const std::string topicName = "object_monitor"; - using message_type = atos_interfaces::msg::Monitor; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); +namespace Monitor { +const std::string topicName = "object_monitor"; +using message_type = atos_interfaces::msg::Monitor; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); + +class Pub : public BasePub { +public: + const uint32_t objectId; + Pub(rclcpp::Node& node, const uint32_t id, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, "object_" + std::to_string(id) + "/" + topicName, qos), + objectId(id) {} +}; + +class PubAll : public BasePub { +public: + PubAll(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class AnchorPub : public BasePub { +public: + AnchorPub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, "object_anchor/" + topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + const uint32_t objectId; + Sub(rclcpp::Node& node, + const uint32_t id, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos), + objectId(id) {} +}; + +class SubAll : public BaseSub { +public: + SubAll(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; - class Pub : public BasePub { - public: - const uint32_t objectId; - Pub(rclcpp::Node& node, const uint32_t id, const rclcpp::QoS& qos = defaultQoS) : - BasePub(node, "object_" + std::to_string(id) + "/" + topicName, qos), - objectId(id) {} - }; +class AnchorSub : public BaseSub { +public: + AnchorSub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, "object_anchor/" + topicName, callback, qos) {} +}; - class AnchorPub : public BasePub { - public: - AnchorPub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : - BasePub(node, "object_anchor/" + topicName, qos) {} - }; +inline message_type fromISOMonr(const uint32_t id, + const ObjectMonitorType& indata, + const std::vector& raw_data) { + atos_interfaces::msg::Monitor outdata; + auto txid = id; + auto stamp = rclcpp::Time(indata.timestamp.tv_sec, indata.timestamp.tv_usec * 1000); - class Sub : public BaseSub { - public: - const uint32_t objectId; - Sub(rclcpp::Node& node, const uint32_t id, std::function callback, const rclcpp::QoS& qos = defaultQoS) : - BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos), - objectId(id) {} - }; + // Set stamp for all subtypes + outdata.atos_header.header.stamp = stamp; + outdata.pose.header.stamp = stamp; + outdata.velocity.header.stamp = stamp; + outdata.acceleration.header.stamp = stamp; - class AnchorSub : public BaseSub { - public: - AnchorSub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : - BaseSub(node, "object_anchor/" + topicName, callback, qos) {} - }; + // Set frame ids + outdata.atos_header.header.frame_id = "map"; // TODO + outdata.pose.header.frame_id = "map"; // TODO + outdata.velocity.header.frame_id = "map"; // TODO vehicle local + outdata.acceleration.header.frame_id = "map"; // TODO vehicle local - inline message_type fromISOMonr(const uint32_t id, const ObjectMonitorType& indata) { - atos_interfaces::msg::Monitor outdata; - auto txid = id; - auto stamp = rclcpp::Time(indata.timestamp.tv_sec, indata.timestamp.tv_usec*1000); - - // Set stamp for all subtypes - outdata.atos_header.header.stamp = stamp; - outdata.pose.header.stamp = stamp; - outdata.velocity.header.stamp = stamp; - outdata.acceleration.header.stamp = stamp; + outdata.atos_header.object_id = txid; + outdata.object_state.state = indata.state; + if (indata.position.isPositionValid) { + outdata.pose.pose.position.x = indata.position.xCoord_m; + outdata.pose.pose.position.y = indata.position.yCoord_m; + outdata.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); + outdata.pose.pose.orientation = tf2::toMsg(orientation); + } + outdata.velocity.twist.linear.x = indata.speed.isLongitudinalValid ? indata.speed.longitudinal_m_s : 0; + outdata.velocity.twist.linear.y = indata.speed.isLateralValid ? indata.speed.lateral_m_s : 0; + outdata.velocity.twist.linear.z = 0; + outdata.velocity.twist.angular.x = 0; + outdata.velocity.twist.angular.y = 0; + outdata.velocity.twist.angular.z = 0; + outdata.acceleration.accel.linear.x = + indata.acceleration.isLongitudinalValid ? indata.acceleration.longitudinal_m_s2 : 0; + outdata.acceleration.accel.linear.y = indata.acceleration.isLateralValid ? indata.acceleration.lateral_m_s2 : 0; + outdata.acceleration.accel.linear.z = 0; + outdata.acceleration.accel.angular.x = 0; + outdata.acceleration.accel.angular.y = 0; + outdata.acceleration.accel.angular.z = 0; - // Set frame ids - outdata.atos_header.header.frame_id = "map"; // TODO - outdata.pose.header.frame_id = "map"; // TODO - outdata.velocity.header.frame_id = "map"; // TODO vehicle local - outdata.acceleration.header.frame_id = "map"; // TODO vehicle local + outdata.raw_data = raw_data; - outdata.atos_header.object_id = txid; - outdata.object_state.state = indata.state; - if (indata.position.isPositionValid) { - outdata.pose.pose.position.x = indata.position.xCoord_m; - outdata.pose.pose.position.y = indata.position.yCoord_m; - outdata.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); - outdata.pose.pose.orientation = tf2::toMsg(orientation); - } - outdata.velocity.twist.linear.x = indata.speed.isLongitudinalValid ? indata.speed.longitudinal_m_s : 0; - outdata.velocity.twist.linear.y = indata.speed.isLateralValid ? indata.speed.lateral_m_s : 0; - outdata.velocity.twist.linear.z = 0; - outdata.velocity.twist.angular.x = 0; - outdata.velocity.twist.angular.y = 0; - outdata.velocity.twist.angular.z = 0; - outdata.acceleration.accel.linear.x = indata.acceleration.isLongitudinalValid ? indata.acceleration.longitudinal_m_s2 : 0; - outdata.acceleration.accel.linear.y = indata.acceleration.isLateralValid ? indata.acceleration.lateral_m_s2 : 0; - outdata.acceleration.accel.linear.z = 0; - outdata.acceleration.accel.angular.x = 0; - outdata.acceleration.accel.angular.y = 0; - outdata.acceleration.accel.angular.z = 0; - return outdata; - } + return outdata; +} - inline ObjectMonitorType toISOMonr(message_type& indata) { - ObjectMonitorType outdata; - outdata.timestamp.tv_sec = indata.atos_header.header.stamp.sec; - outdata.timestamp.tv_usec = indata.atos_header.header.stamp.nanosec / 1000; - outdata.state = static_cast(indata.object_state.state); - outdata.position.isPositionValid = true; - outdata.position.isXcoordValid = true; - outdata.position.isYcoordValid = true; - outdata.position.isZcoordValid = true; - outdata.position.xCoord_m = indata.pose.pose.position.x; - outdata.position.yCoord_m = indata.pose.pose.position.y; - outdata.position.zCoord_m = indata.pose.pose.position.z; - outdata.position.isHeadingValid = true; - tf2::Quaternion quat_tf; - geometry_msgs::msg::Quaternion quat_msg = indata.pose.pose.orientation; - tf2::fromMsg(quat_msg, quat_tf); - double r{}, p{}, y{}; - tf2::Matrix3x3 m(quat_tf); - m.getRPY(r, p, y); - outdata.position.heading_rad = y; - outdata.speed.isLongitudinalValid = true; - outdata.speed.longitudinal_m_s = indata.velocity.twist.linear.x; - outdata.speed.isLateralValid = true; - outdata.speed.lateral_m_s = indata.velocity.twist.linear.y; - outdata.acceleration.isLongitudinalValid = true; - outdata.acceleration.longitudinal_m_s2 = indata.acceleration.accel.linear.x; - outdata.acceleration.isLateralValid = true; - outdata.acceleration.lateral_m_s2 = indata.acceleration.accel.linear.y; - return outdata; - } - } -} \ No newline at end of file +inline ObjectMonitorType toISOMonr(message_type& indata) { + ObjectMonitorType outdata; + outdata.timestamp.tv_sec = indata.atos_header.header.stamp.sec; + outdata.timestamp.tv_usec = indata.atos_header.header.stamp.nanosec / 1000; + outdata.state = static_cast(indata.object_state.state); + outdata.position.isPositionValid = true; + outdata.position.isXcoordValid = true; + outdata.position.isYcoordValid = true; + outdata.position.isZcoordValid = true; + outdata.position.xCoord_m = indata.pose.pose.position.x; + outdata.position.yCoord_m = indata.pose.pose.position.y; + outdata.position.zCoord_m = indata.pose.pose.position.z; + outdata.position.isHeadingValid = true; + tf2::Quaternion quat_tf; + geometry_msgs::msg::Quaternion quat_msg = indata.pose.pose.orientation; + tf2::fromMsg(quat_msg, quat_tf); + double r{}, p{}, y{}; + tf2::Matrix3x3 m(quat_tf); + m.getRPY(r, p, y); + outdata.position.heading_rad = y; + outdata.speed.isLongitudinalValid = true; + outdata.speed.longitudinal_m_s = indata.velocity.twist.linear.x; + outdata.speed.isLateralValid = true; + outdata.speed.lateral_m_s = indata.velocity.twist.linear.y; + outdata.acceleration.isLongitudinalValid = true; + outdata.acceleration.longitudinal_m_s2 = indata.acceleration.accel.linear.x; + outdata.acceleration.isLateralValid = true; + outdata.acceleration.lateral_m_s2 = indata.acceleration.accel.linear.y; + return outdata; +} +} // namespace Monitor +} // namespace ROSChannels diff --git a/atos/launch/launch_experimental.py b/atos/launch/launch_experimental.py index 82df91f04..aec4cdf8a 100644 --- a/atos/launch/launch_experimental.py +++ b/atos/launch/launch_experimental.py @@ -1,5 +1,6 @@ -import sys import os +import sys + from ament_index_python.packages import get_package_prefix sys.path.insert( @@ -8,9 +9,10 @@ get_package_prefix("atos"), "share", "atos", "launch" ), ) -from launch_ros.actions import Node -from launch import LaunchDescription + import launch_utils.launch_base as launch_base +from launch import LaunchDescription +from launch_ros.actions import Node def get_experimental_nodes(): @@ -53,6 +55,13 @@ def get_experimental_nodes(): name="rest_bridge", parameters=[files["params"]], ), + Node( + package="atos", + namespace="atos", + executable="monr_relay", + name="monr_relay", + parameters=[files["params"]], + ), ] diff --git a/atos/modules/MonrRelay/CMakeLists.txt b/atos/modules/MonrRelay/CMakeLists.txt new file mode 100644 index 000000000..5b737f598 --- /dev/null +++ b/atos/modules/MonrRelay/CMakeLists.txt @@ -0,0 +1,58 @@ +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(monr_relay) +find_package(atos_interfaces REQUIRED) +find_package(std_srvs REQUIRED) + +# Define target names +set(MODULE_TARGET ${PROJECT_NAME}) + +set(COREUTILS_LIBRARY ATOSCoreUtil) +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) +get_target_property(COREUTILS_HEADERS ${COREUTILS_LIBRARY} INCLUDE_DIRECTORIES) + +include(GNUInstallDirs) + +# Create project main executable target +add_executable(${MODULE_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/monr_relay.cpp +) +# Link project executable to common libraries +set(TARGET_LIBRARIES + ${COREUTILS_LIBRARY} + ${SOCKET_LIBRARY} + ${COMMON_LIBRARY} +) +target_link_libraries(${MODULE_TARGET} ${TARGET_LIBRARIES}) + +# Add include directories +target_include_directories(${MODULE_TARGET} PUBLIC SYSTEM + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${COMMON_HEADERS} + ${COREUTILS_HEADERS} +) + +# ROS specific rules +ament_target_dependencies(${MODULE_TARGET} + rclcpp + std_msgs + std_srvs + atos_interfaces +) + +# Installation rules +install(CODE "MESSAGE(STATUS \"Installing target ${MODULE_TARGET}\")") +install(TARGETS ${MODULE_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/atos/modules/MonrRelay/inc/monr_relay.hpp b/atos/modules/MonrRelay/inc/monr_relay.hpp new file mode 100644 index 000000000..e1412b256 --- /dev/null +++ b/atos/modules/MonrRelay/inc/monr_relay.hpp @@ -0,0 +1,34 @@ +/* + * 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 + +#include + +#include "module.hpp" +#include "roschannels/monitorchannel.hpp" +#include "server.hpp" + +namespace monr_relay { + +/*! + * \brief MonrRelay + */ +class MonrRelayModule : public Module { +public: + static inline std::string const moduleName = "monr_relay"; + MonrRelayModule(); + +private: + void relayMonitorMessage(ROSChannels::Monitor::message_type::SharedPtr message); + ROSChannels::Monitor::SubAll monrSub; + UDPServer udp_server; + BasicSocket::HostInfo remote_host{}; +}; + +} // namespace monr_relay diff --git a/atos/modules/MonrRelay/src/main.cpp b/atos/modules/MonrRelay/src/main.cpp new file mode 100644 index 000000000..d0541efef --- /dev/null +++ b/atos/modules/MonrRelay/src/main.cpp @@ -0,0 +1,11 @@ +#include "monr_relay.hpp" + +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto sm = std::make_shared(); + rclcpp::spin(sm); + rclcpp::shutdown(); + return 0; +} diff --git a/atos/modules/MonrRelay/src/monr_relay.cpp b/atos/modules/MonrRelay/src/monr_relay.cpp new file mode 100644 index 000000000..d0a445de4 --- /dev/null +++ b/atos/modules/MonrRelay/src/monr_relay.cpp @@ -0,0 +1,39 @@ +#include "monr_relay.hpp" + +#include "rclcpp/wait_for_message.hpp" +#include "roschannels/commandchannels.hpp" + +namespace monr_relay { + +MonrRelayModule::MonrRelayModule() : + Module(moduleName), + monrSub(*this, [this](auto&& param) { MonrRelayModule::relayMonitorMessage(std::forward(param)); }) { + BasicSocket::Address client_host{}; + declare_parameter("client_host", "127.0.0.1"); + get_parameter("client_host", client_host); + + BasicSocket::Port client_port{}; + declare_parameter("client_port", 51234); + get_parameter("client_port", client_port); + + remote_host = {client_host, client_port}; + + std::string server_host{}; + declare_parameter("server_host", "127.0.0.1"); + get_parameter("server_host", server_host); + + std::uint16_t server_port{}; + declare_parameter("server_port", 51122); + get_parameter("server_port", server_port); + + udp_server = UDPServer(server_host, server_port); +} + +void MonrRelayModule::relayMonitorMessage(ROSChannels::Monitor::message_type::SharedPtr message) { + std::vector out; + out.reserve(message->raw_data.size()); + std::copy(message->raw_data.begin(), message->raw_data.end(), std::back_inserter(out)); + udp_server.sendto({out, remote_host}); +} + +} // namespace monr_relay diff --git a/atos/modules/ObjectControl/inc/channel.hpp b/atos/modules/ObjectControl/inc/channel.hpp index c79fed271..cea686860 100644 --- a/atos/modules/ObjectControl/inc/channel.hpp +++ b/atos/modules/ObjectControl/inc/channel.hpp @@ -5,58 +5,62 @@ */ #pragma once -#include +#include "iso22133.h" +#include "loggable.hpp" +#include "roschannels/controlsignalchannel.hpp" +#include "trajectory.hpp" #include #include #include -#include "loggable.hpp" -#include "iso22133.h" -#include "trajectory.hpp" -#include "roschannels/controlsignalchannel.hpp" +#include -struct MonitorMessage : std::pair {}; +struct MonitorMessage { + uint32_t id; + ObjectMonitorType object_monitor; + std::vector raw_data{}; +}; -class Channel : public Loggable -{ +class Channel : public Loggable { public: - Channel(const size_t bufferLength, const int type, rclcpp::Logger log, int id = 0, int transmitter = 0) - : Loggable(log), - channelType(type), - objectId(id), - transmitterId(transmitter), - transmitBuffer(bufferLength, 0), - receiveBuffer(bufferLength, 0) - {} - Channel(int type, rclcpp::Logger log, int id = 0) : Channel(1024, type, log, id) {} - struct sockaddr_in addr = {}; - int socket = -1; - int channelType = 0; //!< SOCK_STREAM or SOCK_DGRAM - int objectId = 0; - int transmitterId = 0; - int sentMessageCounter = 0; + Channel(const size_t bufferLength, const int type, rclcpp::Logger log, int id = 0, int transmitter = 0) : + Loggable(log), + channelType(type), + objectId(id), + transmitterId(transmitter), + transmitBuffer(bufferLength, 0), + receiveBuffer(bufferLength, 0) {} + Channel(int type, rclcpp::Logger log, int id = 0) : + Channel(1024, type, log, id) {} + struct sockaddr_in addr = {}; + int socket = -1; + int channelType = 0; //!< SOCK_STREAM or SOCK_DGRAM + int objectId = 0; + int transmitterId = 0; + int sentMessageCounter = 0; int receivedMessageCounter = 0; std::vector transmitBuffer; std::vector receiveBuffer; ISOMessageID pendingMessageType(bool awaitNext = false); std::string remoteIP() const; - bool isValid() const { return socket != -1; } - void connect(std::shared_future stopRequest, - const std::chrono::milliseconds retryPeriod); + bool isValid() const { + return socket != -1; + } + void connect(std::shared_future stopRequest, const std::chrono::milliseconds retryPeriod); void disconnect(); - friend Channel& operator<<(Channel&,const HeabMessageDataType&); - friend Channel& operator<<(Channel&,const ObjectSettingsType&); - friend Channel& operator<<(Channel&,const ATOS::Trajectory&); - friend Channel& operator<<(Channel&,const ObjectCommandType&); - friend Channel& operator<<(Channel&,const StartMessageType&); - friend Channel& operator<<(Channel&,const std::vector&); - friend Channel& operator<<(Channel&,const ROSChannels::ControlSignal::message_type::SharedPtr csp); + friend Channel& operator<<(Channel&, const HeabMessageDataType&); + friend Channel& operator<<(Channel&, const ObjectSettingsType&); + friend Channel& operator<<(Channel&, const ATOS::Trajectory&); + friend Channel& operator<<(Channel&, const ObjectCommandType&); + friend Channel& operator<<(Channel&, const StartMessageType&); + friend Channel& operator<<(Channel&, const std::vector&); + friend Channel& operator<<(Channel&, const ROSChannels::ControlSignal::message_type::SharedPtr csp); - friend Channel& operator>>(Channel&,MonitorMessage&); - friend Channel& operator>>(Channel&,ObjectPropertiesType&); - friend Channel& operator>>(Channel&,GeneralResponseMessageType&); + friend Channel& operator>>(Channel&, MonitorMessage&); + friend Channel& operator>>(Channel&, ObjectPropertiesType&); + friend Channel& operator>>(Channel&, GeneralResponseMessageType&); protected: - MessageHeaderType *populateHeaderType(MessageHeaderType *header); -}; \ No newline at end of file + MessageHeaderType* populateHeaderType(MessageHeaderType* header); +}; diff --git a/atos/modules/ObjectControl/inc/testobject.hpp b/atos/modules/ObjectControl/inc/testobject.hpp index 93d0e6851..6886ca3cd 100644 --- a/atos/modules/ObjectControl/inc/testobject.hpp +++ b/atos/modules/ObjectControl/inc/testobject.hpp @@ -5,21 +5,21 @@ */ #pragma once -#include "objectconnection.hpp" - -#include #include #include -#include "trajectory.hpp" + +#include + +#include "loggable.hpp" #include "objectconfig.hpp" +#include "objectconnection.hpp" #include "osi_handler.hpp" #include "roschannels/controlsignalchannel.hpp" -#include "roschannels/navsatfixchannel.hpp" -#include "roschannels/pathchannel.hpp" #include "roschannels/monitorchannel.hpp" +#include "roschannels/navsatfixchannel.hpp" #include "roschannels/objstatechangechannel.hpp" - -#include "loggable.hpp" +#include "roschannels/pathchannel.hpp" +#include "trajectory.hpp" using atos_interfaces::msg::ControlSignalPercentage; @@ -41,21 +41,39 @@ class TestObject : public rclcpp::Node { TestObject(TestObject&&); TestObject& operator=(const TestObject&) = delete; - TestObject& operator=(TestObject&&) = default; + TestObject& operator=(TestObject&&) = default; virtual void parseConfigurationFile(const fs::path& file); - virtual uint32_t getTransmitterID() const { return conf.getTransmitterID(); } - virtual std::string getTrajectoryFileName() const { return conf.getTrajectoryFileName(); } - virtual ATOS::Trajectory getTrajectory() const { return conf.getTrajectory(); } - virtual GeographicPositionType getOrigin() const { return conf.getOrigin(); } + virtual uint32_t getTransmitterID() const { + return conf.getTransmitterID(); + } + virtual std::string getTrajectoryFileName() const { + return conf.getTrajectoryFileName(); + } + virtual ATOS::Trajectory getTrajectory() const { + return conf.getTrajectory(); + } + virtual GeographicPositionType getOrigin() const { + return conf.getOrigin(); + } virtual ObjectStateType getState(const bool awaitUpdate); virtual ObjectStateType getState(const bool awaitUpdate, const std::chrono::milliseconds timeout); - virtual ObjectStateType getState() const { return isConnected() ? state : OBJECT_STATE_UNKNOWN; } - virtual ObjectMonitorType getLastMonitorData() const { return lastMonitor; } - virtual ObjectConfig getObjectConfig() const { return conf; } - virtual void setTrajectory(const ATOS::Trajectory& newTrajectory) { conf.setTrajectory(newTrajectory); } - virtual void setTransmitterID(const uint32_t newID) { conf.setTransmitterID(newID); } + virtual ObjectStateType getState() const { + return isConnected() ? state : OBJECT_STATE_UNKNOWN; + } + virtual ObjectMonitorType getLastMonitorData() const { + return lastMonitor; + } + virtual ObjectConfig getObjectConfig() const { + return conf; + } + virtual void setTrajectory(const ATOS::Trajectory& newTrajectory) { + conf.setTrajectory(newTrajectory); + } + virtual void setTransmitterID(const uint32_t newID) { + conf.setTransmitterID(newID); + } virtual void setLastReceivedPath(ROSChannels::Path::message_type::SharedPtr); virtual void setObjectIP(const in_addr_t newIP); virtual void setCommandAddress(const sockaddr_in& newAddr); @@ -64,20 +82,31 @@ class TestObject : public rclcpp::Node { virtual void setObjectConfig(ObjectConfig& newObjectConfig); virtual void setTriggerStart(const bool startOnTrigger = true); virtual void setOrigin(const GeographicPositionType&); - virtual void interruptSocket() { comms.interruptSocket();} - - virtual bool isAnchor() const { return conf.isAnchor(); } - virtual bool isOsiCompatible() const { return conf.isOSI(); } - virtual bool isStartingOnTrigger() const { return startOnTrigger; } + virtual void interruptSocket() { + comms.interruptSocket(); + } + + virtual bool isAnchor() const { + return conf.isAnchor(); + } + virtual bool isOsiCompatible() const { + return conf.isOSI(); + } + virtual bool isStartingOnTrigger() const { + return startOnTrigger; + } virtual std::string toString() const; - virtual std::string getProjString() const { return conf.getProjString(); } + virtual std::string getProjString() const { + return conf.getProjString(); + } virtual ObjectDataType getAsObjectData() const; - virtual bool isConnected() const { return comms.isConnected(); } + virtual bool isConnected() const { + return comms.isConnected(); + } virtual void establishConnection(std::shared_future stopRequest); virtual void disconnect() { - RCLCPP_INFO(get_logger(), "Disconnecting object %u", - this->getTransmitterID()); + RCLCPP_INFO(get_logger(), "Disconnecting object %u", this->getTransmitterID()); this->comms.disconnect(); } @@ -90,8 +119,8 @@ class TestObject : public rclcpp::Node { virtual void sendStart(std::chrono::system_clock::time_point timestamp); virtual void sendAllClear(); virtual void sendOsiData(const OsiHandler::LocalObjectGroundTruth_t& osidata, - const std::string& projStr, - const std::chrono::system_clock::time_point& timestamp); + const std::string& projStr, + const std::chrono::system_clock::time_point& timestamp); virtual void sendControlSignal(const ControlSignalPercentage::SharedPtr csp); virtual void publishMonr(const ROSChannels::Monitor::message_type); @@ -101,8 +130,7 @@ class TestObject : public rclcpp::Node { if (lastMonitorTime.time_since_epoch().count() == 0) { return std::chrono::milliseconds(0); } - return std::chrono::duration_cast( - clock::now() - lastMonitorTime); + return std::chrono::duration_cast(clock::now() - lastMonitorTime); } virtual std::chrono::milliseconds getMaxAllowedMonitorPeriod() const { @@ -131,10 +159,11 @@ class TestObject : public rclcpp::Node { protected: using clock = std::chrono::steady_clock; - ObjectConnection comms; //!< Channel for communication with object over the ISO 22133 protocol - Channel osiChannel; //!< Channel for communication with object over the OSI protocol + ObjectConnection comms; //!< Channel for communication with object over the ISO 22133 protocol + Channel osiChannel; //!< Channel for communication with object over the OSI protocol ObjectStateType state = OBJECT_STATE_UNKNOWN; std::shared_ptr monrPub; + std::shared_ptr monrPubAll; std::shared_ptr navSatFixPub; std::shared_ptr pathSub; std::shared_ptr stateChangePub; @@ -142,7 +171,7 @@ class TestObject : public rclcpp::Node { virtual void onPathMessage(const ROSChannels::Path::message_type::SharedPtr msg, int id); virtual void publishMonitor(MonitorMessage& monr); - virtual void publishStateChange(ObjectStateType &prevObjState); + virtual void publishStateChange(ObjectStateType& prevObjState); ObjectConfig conf; @@ -155,15 +184,16 @@ class TestObject : public rclcpp::Node { clock::time_point lastMonitorTime; static constexpr auto connRetryPeriod = std::chrono::milliseconds(1000); - std::chrono::milliseconds maxAllowedMonitorPeriod = std::chrono::milliseconds(static_cast(1000.0 * 100.0 / MONR_EXPECTED_FREQUENCY_HZ )); + std::chrono::milliseconds maxAllowedMonitorPeriod = + std::chrono::milliseconds(static_cast(1000.0 * 100.0 / MONR_EXPECTED_FREQUENCY_HZ)); }; // Template specialisation of std::less for TestObject namespace std { - template<> struct less { - bool operator() (const TestObject& lhs, const TestObject& rhs) const { - return lhs.getTransmitterID() < rhs.getTransmitterID(); - } - }; -} - +template<> +struct less { + bool operator()(const TestObject& lhs, const TestObject& rhs) const { + return lhs.getTransmitterID() < rhs.getTransmitterID(); + } +}; +} // namespace std diff --git a/atos/modules/ObjectControl/src/channel.cpp b/atos/modules/ObjectControl/src/channel.cpp index fda40a598..20cb5ea9f 100644 --- a/atos/modules/ObjectControl/src/channel.cpp +++ b/atos/modules/ObjectControl/src/channel.cpp @@ -4,17 +4,21 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "channel.hpp" -#include "iso22133.h" -#include #include "atosTime.h" #include "header.h" +#include "iso22133.h" +#include using namespace ROSChannels; Channel& operator<<(Channel& chnl, const HeabMessageDataType& heartbeat) { MessageHeaderType header; - auto nBytes = encodeHEABMessage(chnl.populateHeaderType(&header), &heartbeat.dataTimestamp, heartbeat.controlCenterStatus, - chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + auto nBytes = encodeHEABMessage(chnl.populateHeaderType(&header), + &heartbeat.dataTimestamp, + heartbeat.controlCenterStatus, + chnl.transmitBuffer.data(), + chnl.transmitBuffer.size(), + false); if (nBytes < 0) { throw std::invalid_argument(std::string("Failed to encode HEAB message: ") + strerror(errno)); } @@ -27,7 +31,8 @@ Channel& operator<<(Channel& chnl, const HeabMessageDataType& heartbeat) { Channel& operator<<(Channel& chnl, const ObjectSettingsType& settings) { MessageHeaderType header; - auto nBytes = encodeOSEMMessage(chnl.populateHeaderType(&header), &settings, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + auto nBytes = encodeOSEMMessage( + chnl.populateHeaderType(&header), &settings, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); if (nBytes < 0) { throw std::invalid_argument(std::string("Failed to encode OSEM message: ") + strerror(errno)); } @@ -44,9 +49,14 @@ Channel& operator<<(Channel& chnl, const ATOS::Trajectory& traj) { // TRAJ header MessageHeaderType header; nBytes = encodeTRAJMessageHeader(chnl.populateHeaderType(&header), - traj.id, TRAJECTORY_INFO_RELATIVE_TO_ORIGIN, traj.name.c_str(),traj.name.length(), - static_cast(traj.points.size()), chnl.transmitBuffer.data(), - chnl.transmitBuffer.size(), false); + traj.id, + TRAJECTORY_INFO_RELATIVE_TO_ORIGIN, + traj.name.c_str(), + traj.name.length(), + static_cast(traj.points.size()), + chnl.transmitBuffer.data(), + chnl.transmitBuffer.size(), + false); if (nBytes < 0) { throw std::invalid_argument(std::string("Failed to encode TRAJ message: ") + strerror(errno)); } @@ -59,13 +69,19 @@ Channel& operator<<(Channel& chnl, const ATOS::Trajectory& traj) { for (const auto& pt : traj.points) { struct timeval relTime; CartesianPosition pos = pt.getISOPosition(); - SpeedType spd = pt.getISOVelocity(); - AccelerationType acc = pt.getISOAcceleration(); + SpeedType spd = pt.getISOVelocity(); + AccelerationType acc = pt.getISOAcceleration(); relTime = to_timeval(pt.getTime()); - nBytes = encodeTRAJMessagePoint(&relTime, pos, spd, acc, static_cast(pt.getCurvature()), - chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + nBytes = encodeTRAJMessagePoint(&relTime, + pos, + spd, + acc, + static_cast(pt.getCurvature()), + chnl.transmitBuffer.data(), + chnl.transmitBuffer.size(), + false); if (nBytes < 0) { // TODO what to do here? throw std::invalid_argument(std::string("Failed to encode TRAJ message point: ") + strerror(errno)); @@ -92,7 +108,8 @@ Channel& operator<<(Channel& chnl, const ATOS::Trajectory& traj) { Channel& operator<<(Channel& chnl, const ObjectCommandType& cmd) { MessageHeaderType header; - auto nBytes = encodeOSTMMessage(chnl.populateHeaderType(&header), cmd, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + auto nBytes = encodeOSTMMessage( + chnl.populateHeaderType(&header), cmd, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); if (nBytes < 0) { throw std::invalid_argument(std::string("Failed to encode OSTM message: ") + strerror(errno)); } @@ -105,7 +122,8 @@ Channel& operator<<(Channel& chnl, const ObjectCommandType& cmd) { Channel& operator<<(Channel& chnl, const StartMessageType& strt) { MessageHeaderType header; - auto nBytes = encodeSTRTMessage(chnl.populateHeaderType(&header), &strt, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + auto nBytes = encodeSTRTMessage( + chnl.populateHeaderType(&header), &strt, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); if (nBytes < 0) { throw std::invalid_argument(std::string("Failed to encode STRT message: ") + strerror(errno)); } @@ -123,13 +141,15 @@ Channel& operator>>(Channel& chnl, MonitorMessage& monitor) { TimeSetToCurrentSystemTime(&tv); HeaderType header; decodeISOHeader(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), &header, false); - monitor.first = header.transmitterID; - auto nBytes = decodeMONRMessage(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), tv, - &monitor.second, false); + monitor.id = header.transmitterID; + auto nBytes = + decodeMONRMessage(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), tv, &monitor.object_monitor, false); + + std::copy(chnl.receiveBuffer.begin(), chnl.receiveBuffer.end(), std::back_inserter(monitor.raw_data)); + if (nBytes < 0) { throw std::invalid_argument("Failed to decode MONR message"); - } - else { + } else { nBytes = recv(chnl.socket, chnl.receiveBuffer.data(), static_cast(nBytes), 0); if (nBytes <= 0) { throw std::runtime_error("Unable to clear from socket buffer"); @@ -144,8 +164,7 @@ Channel& operator>>(Channel& chnl, ObjectPropertiesType& prop) { auto nBytes = decodeOPROMessage(&prop, chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), false); if (nBytes < 0) { throw std::invalid_argument(strerror(errno)); - } - else { + } else { nBytes = recv(chnl.socket, chnl.receiveBuffer.data(), static_cast(nBytes), 0); if (nBytes <= 0) { throw std::runtime_error("Unable to clear from socket buffer"); @@ -160,8 +179,7 @@ Channel& operator>>(Channel& chnl, GeneralResponseMessageType& grem) { auto nBytes = decodeGREMMessage(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), &grem, false); if (nBytes < 0) { throw std::invalid_argument(strerror(errno)); - } - else { + } else { nBytes = recv(chnl.socket, chnl.receiveBuffer.data(), static_cast(nBytes), 0); if (nBytes <= 0) { throw std::runtime_error("Unable to clear from socket buffer"); @@ -173,19 +191,20 @@ Channel& operator>>(Channel& chnl, GeneralResponseMessageType& grem) { Channel& operator<<(Channel& chnl, const ControlSignal::message_type::SharedPtr csp) { RemoteControlManoeuvreMessageType rcmm; - rcmm.command = MANOEUVRE_NONE; + rcmm.command = MANOEUVRE_NONE; rcmm.isThrottleManoeuvreValid = true; - rcmm.isBrakeManoeuvreValid = true; + rcmm.isBrakeManoeuvreValid = true; rcmm.isSteeringManoeuvreValid = true; - rcmm.throttleUnit = ISO_UNIT_TYPE_THROTTLE_PERCENTAGE; - rcmm.brakeUnit = ISO_UNIT_TYPE_BRAKE_PERCENTAGE; - rcmm.steeringUnit = ISO_UNIT_TYPE_STEERING_PERCENTAGE; - rcmm.throttleManoeuvre.pct = csp->throttle; - rcmm.brakeManoeuvre.pct = csp->brake; - rcmm.steeringManoeuvre.pct = csp->steering_angle; + rcmm.throttleUnit = ISO_UNIT_TYPE_THROTTLE_PERCENTAGE; + rcmm.brakeUnit = ISO_UNIT_TYPE_BRAKE_PERCENTAGE; + rcmm.steeringUnit = ISO_UNIT_TYPE_STEERING_PERCENTAGE; + rcmm.throttleManoeuvre.pct = csp->throttle; + rcmm.brakeManoeuvre.pct = csp->brake; + rcmm.steeringManoeuvre.pct = csp->steering_angle; MessageHeaderType header; - auto nBytes = encodeRCMMMessage(chnl.populateHeaderType(&header),&rcmm, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + auto nBytes = encodeRCMMMessage( + chnl.populateHeaderType(&header), &rcmm, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); if (nBytes < 0) { throw std::invalid_argument(std::string("Failed to encode RCM message: ") + strerror(errno)); } @@ -206,23 +225,20 @@ Channel& operator<<(Channel& chnl, const std::vector& data) { std::string Channel::remoteIP() const { char ipString[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &this->addr.sin_addr, ipString, sizeof (ipString)); + inet_ntop(AF_INET, &this->addr.sin_addr, ipString, sizeof(ipString)); return std::string(ipString); } ISOMessageID Channel::pendingMessageType(bool awaitNext) { - auto result = recv(this->socket, this->receiveBuffer.data(), this->receiveBuffer.size(), (awaitNext ? 0 : MSG_DONTWAIT) | MSG_PEEK); + auto result = recv( + this->socket, this->receiveBuffer.data(), this->receiveBuffer.size(), (awaitNext ? 0 : MSG_DONTWAIT) | MSG_PEEK); if (result < 0 && !awaitNext && (errno == EAGAIN || errno == EWOULDBLOCK)) { return MESSAGE_ID_INVALID; - } - else if (result < 0) { - throw std::runtime_error(std::string("Failed to check pending message type (recv: ") - + strerror(errno) + ")"); - } - else if (result == 0) { + } else if (result < 0) { + throw std::runtime_error(std::string("Failed to check pending message type (recv: ") + strerror(errno) + ")"); + } else if (result == 0) { throw std::runtime_error("Connection reset by peer"); - } - else { + } else { ISOMessageID retval = getISOMessageType(this->receiveBuffer.data(), this->receiveBuffer.size(), false); if (retval == MESSAGE_ID_INVALID) { throw std::runtime_error("Non-ISO message received from " + this->remoteIP()); @@ -231,22 +247,19 @@ ISOMessageID Channel::pendingMessageType(bool awaitNext) { } } -MessageHeaderType *Channel::populateHeaderType(MessageHeaderType *header) { - memset(header, 0, sizeof (MessageHeaderType)); - header->transmitterID = this->transmitterId; - header->receiverID = this->objectId; +MessageHeaderType* Channel::populateHeaderType(MessageHeaderType* header) { + memset(header, 0, sizeof(MessageHeaderType)); + header->transmitterID = this->transmitterId; + header->receiverID = this->objectId; header->messageCounter = this->sentMessageCounter++; return header; } -void Channel::connect( - std::shared_future stopRequest, - const std::chrono::milliseconds retryPeriod) { +void Channel::connect(std::shared_future stopRequest, const std::chrono::milliseconds retryPeriod) { char ipString[INET_ADDRSTRLEN]; std::stringstream errMsg; - if (inet_ntop(AF_INET, &this->addr.sin_addr, ipString, sizeof (ipString)) - == nullptr) { + if (inet_ntop(AF_INET, &this->addr.sin_addr, ipString, sizeof(ipString)) == nullptr) { errMsg << "inet_ntop: " << strerror(errno); throw std::invalid_argument(errMsg.str()); } @@ -259,8 +272,7 @@ void Channel::connect( std::string type = "???"; if (this->channelType == SOCK_STREAM) { type = "TCP"; - } - else if (this->channelType == SOCK_DGRAM) { + } else if (this->channelType == SOCK_DGRAM) { type = "UDP"; } @@ -272,20 +284,19 @@ void Channel::connect( } // Begin connection attempt - RCLCPP_INFO(get_logger(), "Attempting %s connection to %s:%u", type.c_str(), ipString, - ntohs(this->addr.sin_port)); + RCLCPP_INFO(get_logger(), "Attempting %s connection to %s:%u", type.c_str(), ipString, ntohs(this->addr.sin_port)); while (true) { - if (::connect(this->socket, - reinterpret_cast(&this->addr), - sizeof (this->addr)) == 0) { + if (::connect(this->socket, reinterpret_cast(&this->addr), sizeof(this->addr)) == 0) { break; - } - else { - RCLCPP_ERROR(get_logger(), "Failed %s connection attempt to %s:%u, retrying in %.3f s ...", - type.c_str(), ipString, ntohs(this->addr.sin_port), retryPeriod.count() / 1000.0); - if (stopRequest.wait_for(retryPeriod) - != std::future_status::timeout) { + } else { + RCLCPP_ERROR(get_logger(), + "Failed %s connection attempt to %s:%u, retrying in %.3f s ...", + type.c_str(), + ipString, + ntohs(this->addr.sin_port), + retryPeriod.count() / 1000.0); + if (stopRequest.wait_for(retryPeriod) != std::future_status::timeout) { errMsg << "Connection attempt interrupted"; throw std::runtime_error(errMsg.str()); } @@ -303,4 +314,4 @@ void Channel::disconnect() { } this->socket = -1; } -} \ No newline at end of file +} diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp index 31026e83f..9b3f01fce 100644 --- a/atos/modules/ObjectControl/src/objectcontrol.cpp +++ b/atos/modules/ObjectControl/src/objectcontrol.cpp @@ -532,17 +532,20 @@ OsiHandler::LocalObjectGroundTruth_t ObjectControl::buildOSILocalGroundTruth(con OsiHandler::LocalObjectGroundTruth_t gt; - gt.id = monr.first; - gt.pos_m.x = monr.second.position.xCoord_m; - gt.pos_m.y = monr.second.position.yCoord_m; - gt.pos_m.z = monr.second.position.zCoord_m; - gt.vel_m_s.lon = monr.second.speed.isLongitudinalValid ? monr.second.speed.longitudinal_m_s : 0.0; - gt.vel_m_s.lat = monr.second.speed.isLateralValid ? monr.second.speed.lateral_m_s : 0.0; - gt.vel_m_s.up = 0.0; - gt.acc_m_s2.lon = monr.second.acceleration.isLongitudinalValid ? monr.second.acceleration.longitudinal_m_s2 : 0.0; - gt.acc_m_s2.lat = monr.second.acceleration.isLateralValid ? monr.second.acceleration.lateral_m_s2 : 0.0; - gt.acc_m_s2.up = 0.0; - gt.orientation_rad.yaw = monr.second.position.isHeadingValid ? monr.second.position.heading_rad : 0.0; + gt.id = monr.id; + gt.pos_m.x = monr.object_monitor.position.xCoord_m; + gt.pos_m.y = monr.object_monitor.position.yCoord_m; + gt.pos_m.z = monr.object_monitor.position.zCoord_m; + gt.vel_m_s.lon = monr.object_monitor.speed.isLongitudinalValid ? monr.object_monitor.speed.longitudinal_m_s : 0.0; + gt.vel_m_s.lat = monr.object_monitor.speed.isLateralValid ? monr.object_monitor.speed.lateral_m_s : 0.0; + gt.vel_m_s.up = 0.0; + gt.acc_m_s2.lon = + monr.object_monitor.acceleration.isLongitudinalValid ? monr.object_monitor.acceleration.longitudinal_m_s2 : 0.0; + gt.acc_m_s2.lat = + monr.object_monitor.acceleration.isLateralValid ? monr.object_monitor.acceleration.lateral_m_s2 : 0.0; + gt.acc_m_s2.up = 0.0; + gt.orientation_rad.yaw = + monr.object_monitor.position.isHeadingValid ? monr.object_monitor.position.heading_rad : 0.0; gt.orientation_rad.roll = 0.0; gt.orientation_rad.pitch = 0.0; @@ -551,13 +554,13 @@ OsiHandler::LocalObjectGroundTruth_t ObjectControl::buildOSILocalGroundTruth(con // TODO (NOT WORKING ATM!): Replace this with a subscriber that listens for monr messages. void ObjectControl::injectObjectData(const MonitorMessage& monr) { - if (!objects.at(monr.first)->getObjectConfig().getInjectionMap().targetIDs.empty()) { + if (!objects.at(monr.id)->getObjectConfig().getInjectionMap().targetIDs.empty()) { std::chrono::system_clock::time_point ts; - auto secs = std::chrono::seconds(monr.second.timestamp.tv_sec); - auto usecs = std::chrono::microseconds(monr.second.timestamp.tv_usec); + auto secs = std::chrono::seconds(monr.object_monitor.timestamp.tv_sec); + auto usecs = std::chrono::microseconds(monr.object_monitor.timestamp.tv_usec); ts += secs + usecs; auto osiGtData = buildOSILocalGroundTruth(monr); - for (const auto& targetID : objects.at(monr.first)->getObjectConfig().getInjectionMap().targetIDs) { + for (const auto& targetID : objects.at(monr.id)->getObjectConfig().getInjectionMap().targetIDs) { if (objects.at(targetID)->isOsiCompatible()) { objects.at(targetID)->sendOsiData(osiGtData, objects.at(targetID)->getProjString(), ts); } diff --git a/atos/modules/ObjectControl/src/relativeanchor.cpp b/atos/modules/ObjectControl/src/relativeanchor.cpp index f8fbe5169..4b264a1f5 100644 --- a/atos/modules/ObjectControl/src/relativeanchor.cpp +++ b/atos/modules/ObjectControl/src/relativeanchor.cpp @@ -7,19 +7,16 @@ #include "testobject.hpp" #include -RelativeAnchor::RelativeAnchor(uint32_t id) : - TestObject(id), - anchorPub(*this) -{ -} +RelativeAnchor::RelativeAnchor(uint32_t id) : + TestObject(id), + anchorPub(*this) {} -RelativeAnchor::RelativeAnchor(RelativeAnchor&& other) : - TestObject(std::move(other)), - anchorPub(std::move(other.anchorPub)) -{ -} +RelativeAnchor::RelativeAnchor(RelativeAnchor&& other) : + TestObject(std::move(other)), + anchorPub(std::move(other.anchorPub)) {} RelativeAnchor::publishMonr(const ROSChannels::Monitor::message_type monr) { monrPub.publish(monr); + monrPubAll.publish(monr); anchorPub.publish(monr); -} \ No newline at end of file +} diff --git a/atos/modules/ObjectControl/src/relativetestobject.cpp b/atos/modules/ObjectControl/src/relativetestobject.cpp index ba9575a88..a22c5fee3 100644 --- a/atos/modules/ObjectControl/src/relativetestobject.cpp +++ b/atos/modules/ObjectControl/src/relativetestobject.cpp @@ -11,17 +11,13 @@ using std::placeholders::_1; -RelativeTestObject::RelativeTestObject(uint32_t id) : - TestObject(id), - anchorSub(*this, std::bind(&RelativeTestObject::updateAnchor, this, _1)) -{ -} +RelativeTestObject::RelativeTestObject(uint32_t id) : + TestObject(id), + anchorSub(*this, std::bind(&RelativeTestObject::updateAnchor, this, _1)) {} RelativeTestObject::RelativeTestObject(RelativeTestObject&& other) : - TestObject(std::move(other)), - anchorSub(std::move(other.anchorSub)) -{ -} + TestObject(std::move(other)), + anchorSub(std::move(other.anchorSub)) {} /*! \brief updateAnchor Updates the position of the anchor object * \param monr Monitor data of anchor point @@ -31,8 +27,8 @@ void RelativeTestObject::updateAnchor(const ROSChannels::Monitor::message_type:: } /*! - * \brief RelativeTestObject Transforms monitor data in positions/speeds relative to - * an anchor point into being relative to the anchor points reference. + * \brief RelativeTestObject Transforms monitor data in positions/speeds relative to + * an anchor point into being relative to the anchor points reference. * \param point Monitor data to be transformed * \param anchor Monitor data of anchor to which point is relative * \return Transformed monitor data @@ -43,11 +39,10 @@ MonitorMessage RelativeTestObject::readMonitorMessage() { lastMonitorTime = clock::now(); updateMonitor(retval); // transform the monitor data relative to anchor - transformCoordinate(retval.second,lastAnchorMonr,true); + transformCoordinate(retval.object_monitor, lastAnchorMonr, true); return retval; } - /*! * \brief transformCoordinate Transforms monitor data in positions/speeds relative to * an anchor point into being relative to the anchor points reference. @@ -55,25 +50,23 @@ MonitorMessage RelativeTestObject::readMonitorMessage() { * \param anchor Monitor data of anchor to which point is relative * \return Transformed monitor data */ -ObjectMonitorType RelativeTestObject::transformCoordinate( - const ObjectMonitorType& point, - const ObjectMonitorType& anchor, - const bool debug) { +ObjectMonitorType RelativeTestObject::transformCoordinate(const ObjectMonitorType& point, + const ObjectMonitorType& anchor, + const bool debug) { using namespace Eigen; using namespace std::chrono; - static auto nextPrintTime = steady_clock::now(); + static auto nextPrintTime = steady_clock::now(); static constexpr auto printInterval = seconds(5); - bool print = false; + bool print = false; if (steady_clock::now() > nextPrintTime) { - print = debug; - nextPrintTime = steady_clock::now() - nextPrintTime > printInterval ? - steady_clock::now() + printInterval - : nextPrintTime + printInterval; + print = debug; + nextPrintTime = steady_clock::now() - nextPrintTime > printInterval ? steady_clock::now() + printInterval + : nextPrintTime + printInterval; } std::stringstream dbg; ObjectMonitorType retval = point; - struct timeval tvdiff = {0, 0}; + struct timeval tvdiff = {0, 0}; std::chrono::milliseconds diff; if (point.isTimestampValid && anchor.isTimestampValid) { timersub(&point.timestamp, &anchor.timestamp, &tvdiff); @@ -91,38 +84,38 @@ ObjectMonitorType RelativeTestObject::transformCoordinate( if (print) { dbg << std::endl - << "pt: " << pointPos << ", " << point.position.heading_rad*180.0/M_PI << "deg" << std::endl - << "anch: " << anchorPos << ", " << anchor.position.heading_rad*180.0/M_PI << "deg" << std::endl; + << "pt: " << pointPos << ", " << point.position.heading_rad * 180.0 / M_PI << "deg" << std::endl + << "anch: " << anchorPos << ", " << anchor.position.heading_rad * 180.0 / M_PI << "deg" << std::endl; } // TODO check on timestamps - Vector3d pointInAnchorFrame = anchorPos + pointPos; // TODO subtract also anchor velocity vector times tdiff - anchorVel = anchorToGlobal*anchorVel; // To x/y speeds - pointVel = pointToGlobal*pointVel + anchorVel; // To x/y speeds - pointVel = pointToGlobal.inverse()*pointVel; // To long/lat speeds - anchorAcc = anchorToGlobal*anchorAcc; // To x/y accelerations - pointAcc = pointToGlobal*pointAcc + anchorAcc; // To x/y accelerations - pointAcc = pointToGlobal.inverse()*pointAcc; // To long/lat accelerations + Vector3d pointInAnchorFrame = anchorPos + pointPos; // TODO subtract also anchor velocity vector times tdiff + anchorVel = anchorToGlobal * anchorVel; // To x/y speeds + pointVel = pointToGlobal * pointVel + anchorVel; // To x/y speeds + pointVel = pointToGlobal.inverse() * pointVel; // To long/lat speeds + anchorAcc = anchorToGlobal * anchorAcc; // To x/y accelerations + pointAcc = pointToGlobal * pointAcc + anchorAcc; // To x/y accelerations + pointAcc = pointToGlobal.inverse() * pointAcc; // To long/lat accelerations retval.position.xCoord_m = pointInAnchorFrame[0]; retval.position.yCoord_m = pointInAnchorFrame[1]; retval.position.zCoord_m = pointInAnchorFrame[2]; - retval.position.isPositionValid = anchor.position.isPositionValid && anchor.position.isHeadingValid - && point.position.isPositionValid; - retval.position.heading_rad = point.position.heading_rad; + retval.position.isPositionValid = + anchor.position.isPositionValid && anchor.position.isHeadingValid && point.position.isPositionValid; + retval.position.heading_rad = point.position.heading_rad; retval.position.isHeadingValid = anchor.position.isHeadingValid && point.position.isHeadingValid; - retval.speed.longitudinal_m_s = pointVel[0]; - retval.speed.lateral_m_s = pointVel[1]; - retval.speed.isLateralValid = retval.speed.isLongitudinalValid = - anchor.speed.isLateralValid && anchor.speed.isLongitudinalValid - && point.speed.isLateralValid && point.speed.isLongitudinalValid; + retval.speed.longitudinal_m_s = pointVel[0]; + retval.speed.lateral_m_s = pointVel[1]; + retval.speed.isLateralValid = retval.speed.isLongitudinalValid = + anchor.speed.isLateralValid && anchor.speed.isLongitudinalValid && point.speed.isLateralValid && + point.speed.isLongitudinalValid; retval.acceleration.longitudinal_m_s2 = pointAcc[0]; - retval.acceleration.lateral_m_s2 = pointAcc[1]; - retval.acceleration.isLateralValid = retval.acceleration.isLongitudinalValid = - anchor.acceleration.isLateralValid && anchor.acceleration.isLongitudinalValid - && point.acceleration.isLateralValid && point.acceleration.isLongitudinalValid; + retval.acceleration.lateral_m_s2 = pointAcc[1]; + retval.acceleration.isLateralValid = retval.acceleration.isLongitudinalValid = + anchor.acceleration.isLateralValid && anchor.acceleration.isLongitudinalValid && + point.acceleration.isLateralValid && point.acceleration.isLongitudinalValid; if (print) { - dbg << "res: " << pointInAnchorFrame << ", " << retval.position.heading_rad*180.0/M_PI << "deg"; + dbg << "res: " << pointInAnchorFrame << ", " << retval.position.heading_rad * 180.0 / M_PI << "deg"; std::cerr << dbg.str(); } return retval; diff --git a/atos/modules/ObjectControl/src/testobject.cpp b/atos/modules/ObjectControl/src/testobject.cpp index b647a3e75..a5de07a20 100644 --- a/atos/modules/ObjectControl/src/testobject.cpp +++ b/atos/modules/ObjectControl/src/testobject.cpp @@ -4,52 +4,50 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "testobject.hpp" -#include "util.h" #include "atosTime.h" -#include "osi_handler.hpp" #include "journal.hpp" +#include "osi_handler.hpp" +#include "util.h" using namespace ATOS; using std::placeholders::_1; TestObject::TestObject(uint32_t id) : - Node("object_" + std::to_string(id)), - comms(get_logger(), id), - osiChannel(SOCK_STREAM, get_logger()), - conf(get_logger()) - { - pathSub = std::make_shared(*this, id, std::bind(&TestObject::onPathMessage, this, _1, id)); - monrPub = std::make_shared(*this, id); - navSatFixPub = std::make_shared(*this, id); - stateChangePub = std::make_shared(*this); -} -void TestObject::onPathMessage(const ROSChannels::Path::message_type::SharedPtr, int){ + Node("object_" + std::to_string(id)), + comms(get_logger(), id), + osiChannel(SOCK_STREAM, get_logger()), + conf(get_logger()) { + pathSub = std::make_shared(*this, id, std::bind(&TestObject::onPathMessage, this, _1, id)); + monrPub = std::make_shared(*this, id); + monrPubAll = std::make_shared(*this); + navSatFixPub = std::make_shared(*this, id); + stateChangePub = std::make_shared(*this); +} +void TestObject::onPathMessage(const ROSChannels::Path::message_type::SharedPtr, int) { ; } TestObject::TestObject(TestObject&& other) : - rclcpp::Node(other.get_name()), - comms(other.get_logger(), other.getTransmitterID()), - osiChannel(SOCK_STREAM, other.get_logger()), - state(other.state), - conf(other.conf), - lastMonitor(other.lastMonitor), - maxAllowedMonitorPeriod(other.maxAllowedMonitorPeriod) -{ - this->comms = other.comms; - this->osiChannel = other.osiChannel; - other.comms.cmd.socket = 0; + rclcpp::Node(other.get_name()), + comms(other.get_logger(), other.getTransmitterID()), + osiChannel(SOCK_STREAM, other.get_logger()), + state(other.state), + conf(other.conf), + lastMonitor(other.lastMonitor), + maxAllowedMonitorPeriod(other.maxAllowedMonitorPeriod) { + this->comms = other.comms; + this->osiChannel = other.osiChannel; + other.comms.cmd.socket = 0; other.comms.mntr.socket = 0; other.osiChannel.socket = 0; - this->nextMonitor = std::move(other.nextMonitor); + this->nextMonitor = std::move(other.nextMonitor); } -void TestObject::setObjectIP( - const in_addr_t newIP) { +void TestObject::setObjectIP(const in_addr_t newIP) { struct sockaddr_in addr; addr.sin_addr.s_addr = newIP; - addr.sin_family = AF_INET; - addr.sin_port = htons(ISO_22133_DEFAULT_OBJECT_TCP_PORT); + addr.sin_family = AF_INET; + addr.sin_port = htons(ISO_22133_DEFAULT_OBJECT_TCP_PORT); this->setCommandAddress(addr); addr.sin_port = htons(ISO_22133_OBJECT_UDP_PORT); this->setMonitorAddress(addr); @@ -57,49 +55,39 @@ void TestObject::setObjectIP( this->setOsiAddress(addr); } -void TestObject::setOrigin(const GeographicPositionType& pos){ +void TestObject::setOrigin(const GeographicPositionType& pos) { this->conf.setOrigin(pos); } -void TestObject::setCommandAddress( - const sockaddr_in &newAddr) { +void TestObject::setCommandAddress(const sockaddr_in& newAddr) { if (!this->comms.isConnected()) { this->comms.cmd.addr = newAddr; } } -void TestObject::setMonitorAddress( - const sockaddr_in &newAddr) { +void TestObject::setMonitorAddress(const sockaddr_in& newAddr) { if (!this->comms.isConnected()) { this->comms.mntr.addr = newAddr; } } -void TestObject::setOsiAddress( - const sockaddr_in &newAddr) { +void TestObject::setOsiAddress(const sockaddr_in& newAddr) { if (!this->comms.isConnected()) { this->osiChannel.addr = newAddr; } } -void TestObject::setObjectConfig( - ObjectConfig& newObjectConfig) { +void TestObject::setObjectConfig(ObjectConfig& newObjectConfig) { this->conf = newObjectConfig; } -void TestObject::setTriggerStart( - const bool startOnTrigger) { +void TestObject::setTriggerStart(const bool startOnTrigger) { auto st = this->getState(); - if (st != OBJECT_STATE_ARMED && - st != OBJECT_STATE_RUNNING && - st != OBJECT_STATE_POSTRUN && - st != OBJECT_STATE_ABORTING && - st != OBJECT_STATE_REMOTE_CONTROL && - st != OBJECT_STATE_PRE_ARMING && - st != OBJECT_STATE_PRE_RUNNING) { + if (st != OBJECT_STATE_ARMED && st != OBJECT_STATE_RUNNING && st != OBJECT_STATE_POSTRUN && + st != OBJECT_STATE_ABORTING && st != OBJECT_STATE_REMOTE_CONTROL && st != OBJECT_STATE_PRE_ARMING && + st != OBJECT_STATE_PRE_RUNNING) { this->startOnTrigger = startOnTrigger; - } - else { + } else { std::stringstream errMsg; errMsg << "Attempted to change trigger configuration while in state "; errMsg << objectStateToASCII(st); @@ -107,7 +95,7 @@ void TestObject::setTriggerStart( } } -void TestObject::setLastReceivedPath(ROSChannels::Path::message_type::SharedPtr trajlet){ +void TestObject::setLastReceivedPath(ROSChannels::Path::message_type::SharedPtr trajlet) { // Warning: ensure thread safety here if other thread uses // lastReceivedPath. this->lastReceivedPath = trajlet; @@ -116,18 +104,17 @@ void TestObject::setLastReceivedPath(ROSChannels::Path::message_type::SharedPtr ObjectDataType TestObject::getAsObjectData() const { ObjectDataType retval; // TODO check on isValid for each - retval.origin.Latitude = this->conf.getOrigin().latitude_deg; + retval.origin.Latitude = this->conf.getOrigin().latitude_deg; retval.origin.Longitude = this->conf.getOrigin().longitude_deg; - retval.origin.Altitude = this->conf.getOrigin().altitude_m; + retval.origin.Altitude = this->conf.getOrigin().altitude_m; - retval.Enabled = OBJECT_ENABLED; // TODO maybe a setting for this + retval.Enabled = OBJECT_ENABLED; // TODO maybe a setting for this retval.ClientIP = this->comms.cmd.addr.sin_addr.s_addr; - retval.ClientID = this->getTransmitterID(); - auto diff = this->getTimeSinceLastMonitor(); + retval.ClientID = this->getTransmitterID(); + auto diff = this->getTimeSinceLastMonitor(); if (diff.count() == 0) { retval.lastPositionUpdate = {0, 0}; - } - else { + } else { struct timeval tvnow, tvdiff; TimeSetToCurrentSystemTime(&tvnow); tvdiff = to_timeval(diff); @@ -144,8 +131,7 @@ MonitorMessage TestObject::awaitNextMonitor() { if (this->comms.mntr.pendingMessageType(true) == MESSAGE_ID_MONR) { this->comms.mntr >> retval; return retval; - } - else { + } else { throw std::invalid_argument("Received non-MONR message on MONR channel"); } } @@ -153,49 +139,45 @@ MonitorMessage TestObject::awaitNextMonitor() { void TestObject::handleISOMessage(bool awaitNext) { auto message = this->comms.pendingMessageType(awaitNext); switch (message) { - case MESSAGE_ID_MONR: - { + case MESSAGE_ID_MONR: { struct timeval currentTime; auto prevObjState = this->getState(); - auto monr = this->readMonitorMessage(); + auto monr = this->readMonitorMessage(); TimeSetToCurrentSystemTime(¤tTime); this->publishMonitor(monr); if (this->getState() != prevObjState) { this->publishStateChange(prevObjState); } - } - break; - case MESSAGE_ID_TREO: - RCLCPP_WARN(get_logger(), "Unhandled TREO message"); - break; - case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_OPRO: - this->parseObjectPropertyMessage(); - break; - case MESSAGE_ID_GREM: - this->parseGremMessage(); - break; - default: - RCLCPP_WARN(get_logger(), "Received unknown message type"); - break; + } break; + case MESSAGE_ID_TREO: + RCLCPP_WARN(get_logger(), "Unhandled TREO message"); + break; + case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_OPRO: + this->parseObjectPropertyMessage(); + break; + case MESSAGE_ID_GREM: + this->parseGremMessage(); + break; + default: + RCLCPP_WARN(get_logger(), "Received unknown message type"); + break; } } void TestObject::updateMonitor(const MonitorMessage& data) { - if (data.first != this->getTransmitterID()) { - throw std::invalid_argument("Attempted to set monitor data with non-matching transmitter ID (" - + std::to_string(data.first) + " != " + std::to_string(this->getTransmitterID()) + ")"); + if (data.id != this->getTransmitterID()) { + throw std::invalid_argument("Attempted to set monitor data with non-matching transmitter ID (" + + std::to_string(data.id) + " != " + std::to_string(this->getTransmitterID()) + ")"); } - this->state = data.second.state; - this->lastMonitor = data.second; + this->state = data.object_monitor.state; + this->lastMonitor = data.object_monitor; } ObjectStateType TestObject::getState(const bool awaitUpdate) { return getState(awaitUpdate, maxAllowedMonitorPeriod); } -ObjectStateType TestObject::getState( - const bool awaitUpdate, - const std::chrono::milliseconds timeout) { +ObjectStateType TestObject::getState(const bool awaitUpdate, const std::chrono::milliseconds timeout) { if (awaitUpdate) { if (!nextMonitor.valid()) { nextMonitor = std::async(std::launch::async, &TestObject::awaitNextMonitor, this); @@ -203,37 +185,36 @@ ObjectStateType TestObject::getState( auto status = nextMonitor.wait_for(timeout); if (status == std::future_status::ready) { this->updateMonitor(nextMonitor.get()); - } - else { + } else { throw std::runtime_error("Timed out while waiting for monitor data"); } } return this->getState(); } -void TestObject::publishMonr(const ROSChannels::Monitor::message_type monr){ +void TestObject::publishMonr(const ROSChannels::Monitor::message_type monr) { monrPub->publish(monr); + monrPubAll->publish(monr); } -void TestObject::publishNavSatFix(const ROSChannels::NavSatFix::message_type navSatFix){ +void TestObject::publishNavSatFix(const ROSChannels::NavSatFix::message_type navSatFix) { navSatFixPub->publish(navSatFix); } std::string TestObject::toString() const { char ipAddr[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &this->comms.cmd.addr.sin_addr, ipAddr, sizeof (ipAddr)); + inet_ntop(AF_INET, &this->comms.cmd.addr.sin_addr, ipAddr, sizeof(ipAddr)); std::string retval = ""; retval += conf.toString(); // TODO add more return retval; } -void TestObject::parseConfigurationFile( - const fs::path &objectFile) { +void TestObject::parseConfigurationFile(const fs::path& objectFile) { struct sockaddr_in addr; this->conf.parseConfigurationFile(objectFile); addr.sin_addr.s_addr = conf.getIP(); - addr.sin_family = AF_INET; - addr.sin_port = htons(ISO_22133_DEFAULT_OBJECT_TCP_PORT); + addr.sin_family = AF_INET; + addr.sin_port = htons(ISO_22133_DEFAULT_OBJECT_TCP_PORT); this->setCommandAddress(addr); addr.sin_port = htons(ISO_22133_OBJECT_UDP_PORT); this->setMonitorAddress(addr); @@ -248,17 +229,14 @@ void TestObject::establishConnection(std::shared_future stopRequest) { if (this->isOsiCompatible()) { try { this->osiChannel.connect(stopRequest, TestObject::connRetryPeriod); - } - catch (std::runtime_error& e) { + } catch (std::runtime_error& e) { this->osiChannel.disconnect(); throw std::runtime_error(std::string("Failed to establish OSI connection. Reason: \n") + e.what()); } } } - -void TestObject::sendHeartbeat( - const ControlCenterStatusType ccStatus) { +void TestObject::sendHeartbeat(const ControlCenterStatusType ccStatus) { HeabMessageDataType heartbeat; TimeSetToCurrentSystemTime(&heartbeat.dataTimestamp); heartbeat.controlCenterStatus = ccStatus; @@ -270,30 +248,30 @@ void TestObject::sendSettings() { ObjectSettingsType objSettings; objSettings.testMode = TEST_MODE_PREPLANNED; - objSettings.desiredID.transmitter = conf.getTransmitterID(); - objSettings.desiredID.controlCentre = 0; + objSettings.desiredID.transmitter = conf.getTransmitterID(); + objSettings.desiredID.controlCentre = 0; objSettings.desiredID.subTransmitter = 0; - objSettings.coordinateSystemOrigin = conf.getOrigin(); - objSettings.coordinateSystemType = COORDINATE_SYSTEM_WGS84; + objSettings.coordinateSystemOrigin = conf.getOrigin(); + objSettings.coordinateSystemType = COORDINATE_SYSTEM_WGS84; objSettings.coordinateSystemRotation_rad = 0.0; TimeSetToCurrentSystemTime(&objSettings.currentTime); - + objSettings.heabTimeout.tv_usec = 20000; - objSettings.heabTimeout.tv_sec = 0; + objSettings.heabTimeout.tv_sec = 0; - objSettings.rate.heab = 10; - objSettings.rate.monr = 100; + objSettings.rate.heab = 10; + objSettings.rate.monr = 100; objSettings.rate.monr2 = 1; - objSettings.maxDeviation.lateral_m = 5.0; + objSettings.maxDeviation.lateral_m = 5.0; objSettings.maxDeviation.position_m = 5.0; - objSettings.maxDeviation.yaw_rad = 15.0 * M_PI/180.0; + objSettings.maxDeviation.yaw_rad = 15.0 * M_PI / 180.0; objSettings.minRequiredPositioningAccuracy_m = 1.0; - objSettings.timeServer.ip = 0; + objSettings.timeServer.ip = 0; objSettings.timeServer.port = 0; this->comms.cmd << objSettings; @@ -323,10 +301,9 @@ void TestObject::sendRemoteControl(bool on) { this->comms.cmd << (on ? OBJECT_COMMAND_REMOTE_CONTROL : OBJECT_COMMAND_DISARM); } -void TestObject::sendOsiData( - const OsiHandler::LocalObjectGroundTruth_t& osidata, - const std::string& projStr, - const std::chrono::system_clock::time_point& timestamp) { +void TestObject::sendOsiData(const OsiHandler::LocalObjectGroundTruth_t& osidata, + const std::string& projStr, + const std::chrono::system_clock::time_point& timestamp) { OsiHandler osi; auto rawData = osi.encodeSvGtMessage(osidata, timestamp, projStr, false); std::vector vec(rawData.length()); @@ -338,8 +315,9 @@ void TestObject::sendStart(std::chrono::system_clock::time_point startTime) { RCLCPP_INFO(get_logger(), "Starting object %d", conf.getTransmitterID()); StartMessageType strt; strt.startTime.tv_sec = std::chrono::duration_cast(startTime.time_since_epoch()).count(); - strt.startTime.tv_usec = std::chrono::duration_cast(startTime.time_since_epoch()).count() % 1000000; -// Temporary fix, TODO: allow input from user + strt.startTime.tv_usec = + std::chrono::duration_cast(startTime.time_since_epoch()).count() % 1000000; + // Temporary fix, TODO: allow input from user auto delay_us = 500000; // 0.5 s strt.startTime.tv_usec += delay_us; strt.isTimestampValid = true; @@ -350,27 +328,27 @@ void TestObject::sendControlSignal(const ControlSignalPercentage::SharedPtr csp) this->comms.mntr << csp; } -void TestObject::publishMonitor(MonitorMessage& monr){ +void TestObject::publishMonitor(MonitorMessage& monr) { // Publish to journal - auto objData = this->getAsObjectData(); - objData.MonrData = monr.second; + auto objData = this->getAsObjectData(); + objData.MonrData = monr.object_monitor; JournalRecordMonitorData(&objData); // Publish to ROS topic - auto rosMonr = ROSChannels::Monitor::fromISOMonr(monr.first,monr.second); + auto rosMonr = ROSChannels::Monitor::fromISOMonr(monr.id, monr.object_monitor, monr.raw_data); publishMonr(rosMonr); // TODO: Make a translator node that listens on Monitor topic and does this.. - auto origin = this->getOrigin(); - std::array llh_0 = {origin.latitude_deg, origin.longitude_deg, origin.altitude_m}; + auto origin = this->getOrigin(); + std::array llh_0 = {origin.latitude_deg, origin.longitude_deg, origin.altitude_m}; publishNavSatFix(ROSChannels::NavSatFix::fromROSMonr(llh_0, rosMonr)); } -void TestObject::publishStateChange(ObjectStateType &prevObjState){ +void TestObject::publishStateChange(ObjectStateType& prevObjState) { ROSChannels::ObjectStateChange::message_type msg; - msg.id = this->getTransmitterID(); + msg.id = this->getTransmitterID(); msg.prev_state.state = prevObjState; - msg.state.state = this->getState(); - + msg.state.state = this->getState(); + stateChangePub->publish(msg); -} \ No newline at end of file +} diff --git a/atos_interfaces b/atos_interfaces index 5956c8604..341eef356 160000 --- a/atos_interfaces +++ b/atos_interfaces @@ -1 +1 @@ -Subproject commit 5956c8604276c69c4db508cdd10e989c18e250ee +Subproject commit 341eef356f63eb7be7741fec2c27c92f63ccc0ce diff --git a/conf/conf/params.yaml b/conf/conf/params.yaml index 35a00a39b..97f5356e8 100644 --- a/conf/conf/params.yaml +++ b/conf/conf/params.yaml @@ -49,6 +49,10 @@ atos: qos: 1 ros: queue_size: 1 + monr_relay: + ros__parameters: + host: "127.0.0.1" + port: 51234 trajectorylet_streamer: ros__parameters: chunk_duration: 2.0