diff --git a/CMakeLists.txt b/CMakeLists.txt index bb9f4a1ba..9eafa91aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ set(WITH_OSI_ADAPTER ON CACHE BOOL "Enable OSIAdapter module") set(WITH_ESMINI_ADAPTER ON CACHE BOOL "Enable EsminiAdapter module") set(WITH_MQTT_BRIDGE ON CACHE BOOL "Enable MQTTBridge module") set(WITH_POINTCLOUD_PUBLISHER ON CACHE BOOL "Enable PointcloudPublisher module") +set(WITH_DRONE_CONTROL ON CACHE BOOL "Enable DroneControl module") set(ENABLE_TESTS ON CACHE BOOL "Enable testing on build") @@ -71,6 +72,9 @@ endif() if(WITH_POINTCLOUD_PUBLISHER) list(APPEND ENABLED_MODULES PointcloudPublisher) endif() +if(WITH_DRONE_CONTROL) + list(APPEND ENABLED_MODULES DroneControl) +endif() # Add corresponding subprojects add_subdirectory(iso22133) diff --git a/atos_interfaces b/atos_interfaces index f2c94cf35..20e6b227d 160000 --- a/atos_interfaces +++ b/atos_interfaces @@ -1 +1 @@ -Subproject commit f2c94cf3522b3b10b4a8f4de4de01a656f548edf +Subproject commit 20e6b227d1efeb18da7ce9a0b7896935e68033e7 diff --git a/common/roschannels/monitorchannel.hpp b/common/roschannels/monitorchannel.hpp index 426889cab..23624fcdc 100644 --- a/common/roschannels/monitorchannel.hpp +++ b/common/roschannels/monitorchannel.hpp @@ -7,6 +7,9 @@ #include "roschannel.hpp" #include "atos_interfaces/msg/monitor.hpp" +#include "positioning.h" +#include +#include namespace ROSChannels { namespace Monitor { @@ -29,5 +32,49 @@ namespace ROSChannels { objectId(id), BaseSub(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; + } } } \ No newline at end of file diff --git a/common/roschannels/navsatfixchannel.hpp b/common/roschannels/navsatfixchannel.hpp index 9c69f56fa..8994aebf6 100644 --- a/common/roschannels/navsatfixchannel.hpp +++ b/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(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos) {} }; + // TODO: Remove below.. + static message_type fromMonr(std::array 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; + } } } \ No newline at end of file diff --git a/common/roschannels/objstatechangechannel.hpp b/common/roschannels/objstatechangechannel.hpp new file mode 100644 index 000000000..1b63aa42c --- /dev/null +++ b/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 { + public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} + }; + + class Sub : public BaseSub { + public: + Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} + }; + } +} \ No newline at end of file diff --git a/launch/launch_utils/launch_base.py b/launch/launch_utils/launch_base.py index abaaf2ed8..056f19c8c 100644 --- a/launch/launch_utils/launch_base.py +++ b/launch/launch_utils/launch_base.py @@ -71,11 +71,18 @@ def get_base_nodes(): name='osi_adapter', parameters=[files["params"]] ), + #Node( + # package='atos', + # namespace='atos', + # executable='esmini_adapter', + # name='esmini_adapter', + # parameters=[files["params"]] + #), Node( package='atos', namespace='atos', - executable='esmini_adapter', - name='esmini_adapter', + executable='drone_control', + name='drone_control', parameters=[files["params"]] ), Node( diff --git a/modules/ATOSBase/inc/ATOSbase.hpp b/modules/ATOSBase/inc/ATOSbase.hpp index 07a8ef696..a1872d7c4 100644 --- a/modules/ATOSBase/inc/ATOSbase.hpp +++ b/modules/ATOSBase/inc/ATOSbase.hpp @@ -10,6 +10,7 @@ #include #include "atos_interfaces/srv/get_object_ids.hpp" #include "atos_interfaces/srv/get_test_origin.hpp" +#include "atos_interfaces/srv/get_object_ip.hpp" #include "module.hpp" class ATOSBase : public Module { @@ -21,6 +22,7 @@ class ATOSBase : public Module { static inline std::string const moduleName = "atos_base"; rclcpp::Service::SharedPtr initDataDictionaryService; rclcpp::Service::SharedPtr getObjectIdsService; + rclcpp::Service::SharedPtr getObjectIpService; rclcpp::Service::SharedPtr getTestOriginService; void onExitMessage(const ROSChannels::Exit::message_type::SharedPtr) override; @@ -31,10 +33,13 @@ class ATOSBase : public Module { std::shared_ptr); void onRequestObjectIDs(const std::shared_ptr, std::shared_ptr); + void onRequestObjectIP(const std::shared_ptr, + std::shared_ptr); void onRequestTestOrigin(const std::shared_ptr, std::shared_ptr); bool isInitialized = false; ROSChannels::Exit::Sub exitSub; -}; + std::map getObjectsInfo(); + }; #endif \ No newline at end of file diff --git a/modules/ATOSBase/src/ATOSbase.cpp b/modules/ATOSBase/src/ATOSbase.cpp index ddd047464..115ce1a85 100644 --- a/modules/ATOSBase/src/ATOSbase.cpp +++ b/modules/ATOSBase/src/ATOSbase.cpp @@ -7,6 +7,7 @@ #include "datadictionary.h" #include "objectconfig.hpp" #include +#include #include @@ -31,6 +32,8 @@ ATOSBase::ATOSBase() std::bind(&ATOSBase::onInitDataDictionary, this, _1, _2)); getObjectIdsService = create_service(ServiceNames::getObjectIds, std::bind(&ATOSBase::onRequestObjectIDs, this, _1, _2)); + getObjectIpService = create_service(ServiceNames::getObjectIp, + std::bind(&ATOSBase::onRequestObjectIP, this, _1, _2)); getTestOriginService = create_service(ServiceNames::getTestOrigin, std::bind(&ATOSBase::onRequestTestOrigin, this, _1, _2)); } @@ -80,9 +83,7 @@ void ATOSBase::onExitMessage(const Exit::message_type::SharedPtr) rclcpp::shutdown(); } -void ATOSBase::onRequestObjectIDs( - const std::shared_ptr req, - std::shared_ptr res) +std::map ATOSBase::getObjectsInfo() { char path[PATH_MAX]; std::vector errors; @@ -94,7 +95,7 @@ void ATOSBase::onRequestObjectIDs( throw std::ios_base::failure("Object directory does not exist"); } - std::vector objectIDs; + std::map objectIps; for (const auto& entry : fs::directory_iterator(objectDir)) { if (!fs::is_regular_file(entry.status())) { continue; @@ -105,9 +106,10 @@ void ATOSBase::onRequestObjectIDs( RCLCPP_DEBUG(get_logger(), "Loaded configuration: %s", conf.toString().c_str()); // Check preexisting - auto foundID = std::find(objectIDs.begin(), objectIDs.end(), conf.getTransmitterID()); - if (foundID == objectIDs.end()) { - objectIDs.push_back(conf.getTransmitterID()); + + auto foundID = objectIps.find(conf.getTransmitterID()); + if (foundID == objectIps.end()) { + objectIps.emplace(conf.getTransmitterID(), conf.getIP()); } else { std::string errMsg = "Duplicate object ID " + std::to_string(conf.getTransmitterID()) @@ -115,8 +117,43 @@ void ATOSBase::onRequestObjectIDs( throw std::invalid_argument(errMsg); } } + return objectIps; +} - res->ids = objectIDs; +void ATOSBase::onRequestObjectIDs( + const std::shared_ptr req, + std::shared_ptr res) +{ + std::vector objectIDs; + try { + for(auto const& objs: getObjectsInfo()) + objectIDs.push_back(objs.first); + res->ids = objectIDs; + } + catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to get object IDs: %s", e.what()); + res->success = false; + return; + } +} + +void ATOSBase::onRequestObjectIP( + const std::shared_ptr req, + std::shared_ptr res) +{ + uint32_t objectIp; + try { + auto objinfo = getObjectsInfo(); + if (objinfo.find(req->id) == objinfo.end()) { + throw std::invalid_argument("Object ID not found"); + } + res->ip = std::string(inet_ntoa(in_addr{objinfo.at(req->id)})); + res->success = true; + } + catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to get object IPs: %s", e.what()); + res->success = false; + } } void ATOSBase::onRequestTestOrigin( diff --git a/modules/DroneControl/CMakeLists.txt b/modules/DroneControl/CMakeLists.txt new file mode 100644 index 000000000..248d15cb9 --- /dev/null +++ b/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} +) diff --git a/modules/DroneControl/README.md b/modules/DroneControl/README.md new file mode 100644 index 000000000..2700208f3 --- /dev/null +++ b/modules/DroneControl/README.md @@ -0,0 +1,40 @@ +# Sample module +This is a sample module to be used as template for creating new modules. +## Features +The sample module is a ros2 node that features some basic publishers and subscribers to various topics. +It also features a TCPServer running in a separate thread. + +## Usage +In order to compile and launch this module (or any other module created from the template) you need to go to the outer-most CMakeLists.txt file in the root of the repository and add the following line: +``` +set(WITH_MODULE_X ON CACHE BOOL "Enable ModuleX module") +``` + + +Followed by: +``` +if(WITH_MODULE_X) + list(APPEND ENABLED_MODULES ModuleX) +endif() +``` + +Note: When switching ON/OFF certain modules, it might be nessesscary to remove the CMakeCache.txt file in ~/atos_ws/install/atos/. + +It is also necessary to add the module to a launch file, located in the launch directory. This is done by adding the following line to the list of nodes in the appropriate launch file: +``` + Node( + package='atos', + namespace='atos', + executable='module_x', + name='module_x', + ) +``` + +Then you can compile and launch the module by running the following commands: +``` +MAKEFLAGS=-j5 colcon build --packages-up-to atos +``` +(tune -j5 to an approperiate number depending on your availiable RAM memory and CPU cores) +``` +ros2 launch atos launch_basic.py +``` \ No newline at end of file diff --git a/modules/DroneControl/inc/dronecontrol.hpp b/modules/DroneControl/inc/dronecontrol.hpp new file mode 100644 index 000000000..fde61bade --- /dev/null +++ b/modules/DroneControl/inc/dronecontrol.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include "trajectory.hpp" +#include "module.hpp" +#include "path.hpp" +#include "atos_interfaces/srv/get_object_trajectory.hpp" + +/*! + * \brief The DroneControl module + */ +class DroneControl : public Module{ +public: + static inline std::string const moduleName = "drone_control"; + DroneControl(); + ~DroneControl(); + +private: + // Member variables + std::map droneTrajectories; // map from drone object-id to trajectory + + // Provided services + std::shared_ptr> objectTrajectoryService; + + // Service callbacks + void onRequestObjectTrajectory(const atos_interfaces::srv::GetObjectTrajectory::Request::SharedPtr, + const atos_interfaces::srv::GetObjectTrajectory::Response::SharedPtr); + + // Business logic + ABD::Path createPath(const std::string&); + ATOS::Trajectory createDroneTrajectory(ABD::Path&, uint32_t); + + + +}; diff --git a/modules/DroneControl/inc/path.hpp b/modules/DroneControl/inc/path.hpp new file mode 100644 index 000000000..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..1856d01f2 --- /dev/null +++ b/modules/DroneControl/src/dronecontrol.cpp @@ -0,0 +1,82 @@ +#include "dronecontrol.hpp" +#include "path.hpp" + +using namespace ROSChannels; +using namespace std::placeholders; +using ObjectTrajectorySrv = atos_interfaces::srv::GetObjectTrajectory; + +DroneControl::DroneControl() : + Module(moduleName) +{ + // Entry-point to the module + + // Initialize services that this module provides to other modules + objectTrajectoryService = create_service(ServiceNames::getObjectTrajectory, + std::bind(&DroneControl::onRequestObjectTrajectory, this, _1, _2)); +} + +// Some module has requested the object trajectories +void DroneControl::onRequestObjectTrajectory(const ObjectTrajectorySrv::Request::SharedPtr req, + const ObjectTrajectorySrv::Response::SharedPtr res) +{ + + res->id = req->id; + try { + // TODO: Parse .path file + auto path = createPath("path/path.path"); + + // TODO: Create trajectory object for drone with id req->id + auto traj = createDroneTrajectory(path,req->id); + RCLCPP_INFO(get_logger(), "Esmini trajectory service called, returning trajectory for object %s", traj.toString().c_str()); + res->trajectory = traj.toCartesianTrajectory(); + res->success = true; + } + catch (std::out_of_range& e){ + RCLCPP_ERROR(get_logger(), "Esmini trajectory service called, no trajectory found for object %d", req->id); + res->success = false; + } +} + +// Creates Path objects from .path file +ABD::Path DroneControl::createPath(const std::string& path) +{ + return ABD::Path(path); +} +ATOS::Trajectory DroneControl::createDroneTrajectory(ABD::Path& path, uint32_t id) +{ + // TODO: Automate creation of multiple points + auto traj = ATOS::Trajectory(get_logger()); + + // The starting point + auto point1 = ATOS::Trajectory::TrajectoryPoint(get_logger()); + point1.setXCoord(0); + point1.setYCoord(0); + point1.setZCoord(0); + point1.setHeading(0); + point1.setLongitudinalVelocity(3); + point1.setLateralVelocity(3); + point1.setLongitudinalAcceleration(3); + point1.setLateralAcceleration(3); + point1.setTime(0); + traj.points.push_back(point1); + + // The ending point + auto point2 = ATOS::Trajectory::TrajectoryPoint(get_logger()); + point2.setXCoord(1); + point2.setYCoord(1); + point2.setZCoord(1); + point2.setHeading(0); + point2.setLongitudinalVelocity(0); + point2.setLateralVelocity(0); + point2.setLongitudinalAcceleration(0); + point2.setLateralAcceleration(0); + point2.setTime(100); + traj.points.push_back(point2); + + return traj; +} + + +DroneControl::~DroneControl() +{ +} diff --git a/modules/DroneControl/src/main.cpp b/modules/DroneControl/src/main.cpp new file mode 100644 index 000000000..20004445b --- /dev/null +++ b/modules/DroneControl/src/main.cpp @@ -0,0 +1,11 @@ +#include "rclcpp/rclcpp.hpp" +#include "dronecontrol.hpp" + + +int main(int argc, char** argv) { + rclcpp::init(argc,argv); + auto dc = std::make_shared(); + rclcpp::spin(dc); + rclcpp::shutdown(); + return 0; +} diff --git a/modules/DroneControl/src/path.cpp b/modules/DroneControl/src/path.cpp new file mode 100644 index 000000000..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/ObjectControl/CMakeLists.txt b/modules/ObjectControl/CMakeLists.txt index ac5f4552f..8e7a951a4 100644 --- a/modules/ObjectControl/CMakeLists.txt +++ b/modules/ObjectControl/CMakeLists.txt @@ -42,7 +42,10 @@ include(GNUInstallDirs) add_executable(${OBJECT_CONTROL_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/testobject.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/relativetestobject.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/objectlistener.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/objectconnection.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/channel.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/states/state.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/states/idle.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/states/initialized.cpp diff --git a/modules/ObjectControl/inc/channel.hpp b/modules/ObjectControl/inc/channel.hpp new file mode 100644 index 000000000..e73452513 --- /dev/null +++ b/modules/ObjectControl/inc/channel.hpp @@ -0,0 +1,53 @@ +/* + * 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 +#include +#include "loggable.hpp" +#include "iso22133.h" +#include "trajectory.hpp" +#include "roschannels/controlsignalchannel.hpp" + +struct MonitorMessage : std::pair {}; + +class Channel : public Loggable +{ +public: + Channel(const size_t bufferLength, const int type, rclcpp::Logger log) + : channelType(type), + transmitBuffer(bufferLength, 0), + receiveBuffer(bufferLength, 0), + Loggable(log) + {} + Channel(int type, rclcpp::Logger log) : Channel(1024, type, log) {} + struct sockaddr_in addr = {}; + int socket = -1; + int channelType = 0; //!< SOCK_STREAM or SOCK_DGRAM + 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); + 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&,MonitorMessage&); + friend Channel& operator>>(Channel&,ObjectPropertiesType&); + +}; \ No newline at end of file diff --git a/modules/ObjectControl/inc/objectconnection.hpp b/modules/ObjectControl/inc/objectconnection.hpp new file mode 100644 index 000000000..460a6515c --- /dev/null +++ b/modules/ObjectControl/inc/objectconnection.hpp @@ -0,0 +1,45 @@ +/* + * 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 +#include "iso22133.h" +#include "loggable.hpp" +#include "channel.hpp" + +/*! + * \brief The ObjectConnection class holds network connection data for + * a single object, i.e. the two channels for command and + * safety data. + */ +class ObjectConnection : public Loggable { +public: + Channel cmd; + Channel mntr; + + ObjectConnection(rclcpp::Logger log) + : cmd(SOCK_STREAM, log), + mntr(SOCK_DGRAM, log), + Loggable(log) { + pipe(interruptionPipeFds); + } + + bool isValid() const; + bool isConnected() const; + void connect(std::shared_future stopRequest, + const std::chrono::milliseconds retryPeriod); + void disconnect(); + ISOMessageID pendingMessageType(bool awaitNext = false); + void interruptSocket() { + int i = 1; + write(interruptionPipeFds[1], &i, sizeof(i)); + close(interruptionPipeFds[1]); + } +private: + int interruptionPipeFds[2]; +}; \ No newline at end of file diff --git a/modules/ObjectControl/inc/objectcontrol.hpp b/modules/ObjectControl/inc/objectcontrol.hpp index f3a796f64..3c7cb5806 100644 --- a/modules/ObjectControl/inc/objectcontrol.hpp +++ b/modules/ObjectControl/inc/objectcontrol.hpp @@ -20,6 +20,7 @@ #include "roschannels/remotecontrolchannels.hpp" #include "roschannels/pathchannel.hpp" #include "roschannels/controlsignalchannel.hpp" +#include "roschannels/objstatechangechannel.hpp" #include "atos_interfaces/srv/get_object_ids.hpp" #include "atos_interfaces/srv/get_object_trajectory.hpp" #include "atos_interfaces/srv/get_object_ip.hpp" @@ -116,7 +117,7 @@ class ObjectControl : public Module public: int initialize(); - ObjectControl(); + ObjectControl(std::shared_ptr); typedef enum { RELATIVE_KINEMATICS, //!< Scenario executed relative to immobile VUT ABSOLUTE_KINEMATICS //!< Scenario executed relative to earth-fixed point @@ -208,6 +209,7 @@ class ObjectControl : public Module private: std::mutex stateMutex; + std::shared_ptr exec; static inline std::string const moduleName = "object_control"; void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; void onConnectMessage(const ROSChannels::Connect::message_type::SharedPtr) override; @@ -221,6 +223,7 @@ class ObjectControl : public Module void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr) override; void onRemoteControlEnableMessage(const ROSChannels::RemoteControlEnable::message_type::SharedPtr); void onRemoteControlDisableMessage(const ROSChannels::RemoteControlDisable::message_type::SharedPtr); + void onObjectStateChangeMessage(const ROSChannels::ObjectStateChange::message_type::SharedPtr); void onControlSignalMessage(const ROSChannels::ControlSignal::message_type::SharedPtr); void onPathMessage(const ROSChannels::Path::message_type::SharedPtr,const uint32_t); void onRequestState(const std::shared_ptr, @@ -254,6 +257,7 @@ class ObjectControl : public Module ROSChannels::RemoteControlEnable::Sub scnRemoteControlEnableSub; //!< Subscriber to remote control enable requests ROSChannels::RemoteControlDisable::Sub scnRemoteControlDisableSub; //!< Subscriber to remote control disable requests ROSChannels::GetStatus::Sub getStatusSub; //!< Subscriber to scenario get status requests + ROSChannels::ObjectStateChange::Sub objectStateChangeSub; //!< Subscriber to scenario path requests std::shared_ptr controlSignalSub; //!< Pointer to subscriber to receive control signal messages with percentage rclcpp::TimerBase::SharedPtr objectsConnectedTimer; //!< Timer to periodically publish connected objects diff --git a/modules/ObjectControl/inc/objectlistener.hpp b/modules/ObjectControl/inc/objectlistener.hpp index 1f3ba37a2..c73f34955 100644 --- a/modules/ObjectControl/inc/objectlistener.hpp +++ b/modules/ObjectControl/inc/objectlistener.hpp @@ -4,7 +4,7 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #pragma once -#include "positioning.h" + #include "objectcontrol.hpp" #include "testobject.hpp" #include "loggable.hpp" diff --git a/modules/ObjectControl/inc/relativetestobject.hpp b/modules/ObjectControl/inc/relativetestobject.hpp new file mode 100644 index 000000000..90f6c3538 --- /dev/null +++ b/modules/ObjectControl/inc/relativetestobject.hpp @@ -0,0 +1,19 @@ +#include "testobject.hpp" + +class RelativeTestObject : public TestObject { +public: + RelativeTestObject(uint32_t id, uint32_t anchorID); + RelativeTestObject(const RelativeTestObject&) = delete; + //RelativeTestObject(RelativeTestObject&&); + + RelativeTestObject& operator=(const RelativeTestObject&) = delete; + RelativeTestObject& operator=(RelativeTestObject&&) = default; + +private: + uint32_t anchor; + bool isAnchor; + ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, + const ObjectMonitorType& anchor, + const bool debug); + MonitorMessage readMonitorMessage() override; +}; diff --git a/modules/ObjectControl/inc/testobject.hpp b/modules/ObjectControl/inc/testobject.hpp index d5f86286c..01918b642 100644 --- a/modules/ObjectControl/inc/testobject.hpp +++ b/modules/ObjectControl/inc/testobject.hpp @@ -5,6 +5,8 @@ */ #pragma once +#include "objectconnection.hpp" + #include #include #include @@ -15,6 +17,7 @@ #include "roschannels/navsatfixchannel.hpp" #include "roschannels/pathchannel.hpp" #include "roschannels/monitorchannel.hpp" +#include "roschannels/objstatechangechannel.hpp" #include "loggable.hpp" @@ -29,150 +32,71 @@ namespace fs = std::filesystem; namespace fs = std::experimental::filesystem; #endif -struct MonitorMessage : std::pair {}; - #define OSI_DEFAULT_OBJECT_TCP_PORT 53250 -/*! - * \brief The Channel class represents any socket based connection - * and allows transmission / reception of ISO messages - */ -class Channel : public Loggable -{ +class TestObject : public rclcpp::Node { public: - Channel(const size_t bufferLength, const int type, rclcpp::Logger log) - : channelType(type), - transmitBuffer(bufferLength, 0), - receiveBuffer(bufferLength, 0), - Loggable(log) - {} - Channel(int type, rclcpp::Logger log) : Channel(1024, type, log) {} - struct sockaddr_in addr = {}; - int socket = -1; - int channelType = 0; //!< SOCK_STREAM or SOCK_DGRAM - 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); - 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 ControlSignalPercentage::SharedPtr csp); - - friend Channel& operator>>(Channel&,MonitorMessage&); - friend Channel& operator>>(Channel&,ObjectPropertiesType&); - -}; - -/*! - * \brief The ObjectConnection class holds network connection data for - * a single object, i.e. the two channels for command and - * safety data. - */ -class ObjectConnection : public Loggable { -public: - Channel cmd; - Channel mntr; - - ObjectConnection(rclcpp::Logger log) - : cmd(SOCK_STREAM, log), - mntr(SOCK_DGRAM, log), - Loggable(log) { - pipe(interruptionPipeFds); - } - - bool isValid() const; - bool isConnected() const; - void connect(std::shared_future stopRequest, - const std::chrono::milliseconds retryPeriod); - void disconnect(); - ISOMessageID pendingMessageType(bool awaitNext = false); - void interruptSocket() { - int i = 1; - write(interruptionPipeFds[1], &i, sizeof(i)); - close(interruptionPipeFds[1]); - } -private: - int interruptionPipeFds[2]; -}; - -class TestObject : public Loggable { - using clock = std::chrono::steady_clock; -public: - TestObject(rclcpp::Logger, - std::shared_ptr, - std::shared_ptr, - std::shared_ptr - ); + TestObject(uint32_t id); TestObject(const TestObject&) = delete; TestObject(TestObject&&); TestObject& operator=(const TestObject&) = delete; TestObject& operator=(TestObject&&) = default; - void parseConfigurationFile(const fs::path& file); - - uint32_t getTransmitterID() const { return conf.getTransmitterID(); } - std::string getTrajectoryFileName() const { return conf.getTrajectoryFileName(); } - ATOS::Trajectory getTrajectory() const { return conf.getTrajectory(); } - GeographicPositionType getOrigin() const { return conf.getOrigin(); } - ObjectStateType getState(const bool awaitUpdate); - ObjectStateType getState(const bool awaitUpdate, const std::chrono::milliseconds timeout); - ObjectStateType getState() const { return isConnected() ? state : OBJECT_STATE_UNKNOWN; } - ObjectMonitorType getLastMonitorData() const { return lastMonitor; } - ObjectConfig getObjectConfig() const { return conf; } - void setTrajectory(const ATOS::Trajectory& newTrajectory) { conf.setTrajectory(newTrajectory); } - void setTransmitterID(const uint32_t newID) { conf.setTransmitterID(newID); } - void setLastReceivedPath(ROSChannels::Path::message_type::SharedPtr); - void setObjectIP(const in_addr_t newIP); - void setCommandAddress(const sockaddr_in& newAddr); - void setMonitorAddress(const sockaddr_in& newAddr); - void setOsiAddress(const sockaddr_in& newAddr); - void setObjectConfig(ObjectConfig& newObjectConfig); - void setTriggerStart(const bool startOnTrigger = true); - void setOrigin(const GeographicPositionType&); - void interruptSocket() { comms.interruptSocket();} + 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 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 void setLastReceivedPath(ROSChannels::Path::message_type::SharedPtr); + virtual void setObjectIP(const in_addr_t newIP); + virtual void setCommandAddress(const sockaddr_in& newAddr); + virtual void setMonitorAddress(const sockaddr_in& newAddr); + virtual void setOsiAddress(const sockaddr_in& newAddr); + virtual void setObjectConfig(ObjectConfig& newObjectConfig); + virtual void setTriggerStart(const bool startOnTrigger = true); + virtual void setOrigin(const GeographicPositionType&); + virtual void interruptSocket() { comms.interruptSocket();} - bool isAnchor() const { return conf.isAnchor(); } - bool isOsiCompatible() const { return conf.isOSI(); } - bool isStartingOnTrigger() const { return startOnTrigger; } - std::string toString() const; - std::string getProjString() const { return conf.getProjString(); } - ObjectDataType getAsObjectData() const; - - bool isConnected() const { return comms.isConnected(); } - void establishConnection(std::shared_future stopRequest); - void disconnect() { + 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 ObjectDataType getAsObjectData() const; + + 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()); this->comms.disconnect(); } - void sendSettings(); - void sendHeartbeat(const ControlCenterStatusType ccStatus); - void sendArm(); - void sendDisarm(); - void sendRemoteControl(bool on); - void sendStart(std::chrono::system_clock::time_point timestamp); - void sendAllClear(); - void sendOsiData(const OsiHandler::LocalObjectGroundTruth_t& osidata, + virtual void sendSettings(); + virtual void sendHeartbeat(const ControlCenterStatusType ccStatus); + virtual void sendArm(); + virtual void sendDisarm(); + virtual void sendRemoteControl(bool on); + 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); - void sendControlSignal(const ControlSignalPercentage::SharedPtr csp); - void publishMonr(const ROSChannels::Monitor::message_type); - void publishNavSatFix(const ROSChannels::NavSatFix::message_type); + virtual void sendControlSignal(const ControlSignalPercentage::SharedPtr csp); + virtual void publishMonr(const ROSChannels::Monitor::message_type); + virtual void publishNavSatFix(const ROSChannels::NavSatFix::message_type); - std::chrono::milliseconds getTimeSinceLastMonitor() const { + virtual std::chrono::milliseconds getTimeSinceLastMonitor() const { if (lastMonitorTime.time_since_epoch().count() == 0) { return std::chrono::milliseconds(0); } @@ -180,43 +104,45 @@ class TestObject : public Loggable { clock::now() - lastMonitorTime); } - std::chrono::milliseconds getMaxAllowedMonitorPeriod() const { + virtual std::chrono::milliseconds getMaxAllowedMonitorPeriod() const { return this->maxAllowedMonitorPeriod; } - MonitorMessage readMonitorMessage() { + virtual MonitorMessage readMonitorMessage() { MonitorMessage retval; this->comms.mntr >> retval; lastMonitorTime = clock::now(); updateMonitor(retval); return retval; } - ObjectPropertiesType parseObjectPropertyMessage() { + virtual ObjectPropertiesType parseObjectPropertyMessage() { ObjectPropertiesType retval; this->comms.cmd >> retval; // TODO make use of this RCLCPP_DEBUG(get_logger(), "Ignoring object properties message"); return retval; } + virtual void handleISOMessage(bool awaitNext = false); - ISOMessageID pendingMessageType(bool awaitNext = false) { - return this->comms.pendingMessageType(awaitNext); - } -private: +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 ObjectStateType state = OBJECT_STATE_UNKNOWN; std::shared_ptr monrPub; std::shared_ptr navSatFixPub; std::shared_ptr pathSub; + std::shared_ptr stateChangePub; std::shared_ptr lastReceivedPath; - void onPathMessage(const ROSChannels::Path::message_type::SharedPtr); + virtual void onPathMessage(const ROSChannels::Path::message_type::SharedPtr msg, int id); + virtual void publishMonitor(MonitorMessage& monr); + virtual void publishStateChange(ObjectStateType &prevObjState); ObjectConfig conf; bool startOnTrigger = false; - void updateMonitor(const MonitorMessage&); - MonitorMessage awaitNextMonitor(); + virtual void updateMonitor(const MonitorMessage&); + virtual MonitorMessage awaitNextMonitor(); std::future nextMonitor; ObjectMonitorType lastMonitor; // TODO change this into a more usable format clock::time_point lastMonitorTime; diff --git a/modules/ObjectControl/src/channel.cpp b/modules/ObjectControl/src/channel.cpp new file mode 100644 index 000000000..2f6e500cb --- /dev/null +++ b/modules/ObjectControl/src/channel.cpp @@ -0,0 +1,271 @@ +/* + * 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/. + */ +#include "channel.hpp" +#include "iso22133.h" +#include +#include "atosTime.h" + +using namespace ROSChannels; + +Channel& operator<<(Channel& chnl, const HeabMessageDataType& heartbeat) { + auto nBytes = encodeHEABMessage(&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)); + } + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send HEAB: ") + strerror(errno)); + } + return chnl; +} + +Channel& operator<<(Channel& chnl, const ObjectSettingsType& settings) { + auto nBytes = encodeOSEMMessage(&settings, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + if (nBytes < 0) { + throw std::invalid_argument(std::string("Failed to encode OSEM message: ") + strerror(errno)); + } + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send OSEM: ") + strerror(errno)); + } + return chnl; +} + +Channel& operator<<(Channel& chnl, const ATOS::Trajectory& traj) { + ssize_t nBytes; + + // TRAJ header + nBytes = encodeTRAJMessageHeader( + 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)); + } + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send TRAJ message header: ") + strerror(errno)); + } + + // TRAJ points + for (const auto& pt : traj.points) { + struct timeval relTime; + CartesianPosition pos = pt.getISOPosition(); + 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); + if (nBytes < 0) { + // TODO what to do here? + throw std::invalid_argument(std::string("Failed to encode TRAJ message point: ") + strerror(errno)); + } + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + + if (nBytes < 0) { + // TODO what to do here? + throw std::runtime_error(std::string("Failed to send TRAJ message point: ") + strerror(errno)); + } + } + + // TRAJ footer + nBytes = encodeTRAJMessageFooter(chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + if (nBytes < 0) { + throw std::invalid_argument(std::string("Failed to encode TRAJ message footer: ") + strerror(errno)); + } + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send TRAJ message footer: ") + strerror(errno)); + } + return chnl; +} + +Channel& operator<<(Channel& chnl, const ObjectCommandType& cmd) { + auto nBytes = encodeOSTMMessage(cmd, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + if (nBytes < 0) { + throw std::invalid_argument(std::string("Failed to encode OSTM message: ") + strerror(errno)); + } + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send OSTM: ") + strerror(errno)); + } + return chnl; +} + +Channel& operator<<(Channel& chnl, const StartMessageType& strt) { + auto nBytes = encodeSTRTMessage(&strt, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + if (nBytes < 0) { + throw std::invalid_argument(std::string("Failed to encode STRT message: ") + strerror(errno)); + } + + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send STRT: ") + strerror(errno)); + } + return chnl; +} + +Channel& operator>>(Channel& chnl, MonitorMessage& monitor) { + if (chnl.pendingMessageType() == MESSAGE_ID_MONR) { + struct timeval tv; + TimeSetToCurrentSystemTime(&tv); + auto nBytes = decodeMONRMessage(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), tv, + &monitor.first, &monitor.second, false); + if (nBytes < 0) { + throw std::invalid_argument("Failed to decode MONR message"); + } + 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"); + } + } + } + return chnl; +} + +Channel& operator>>(Channel& chnl, ObjectPropertiesType& prop) { + if (chnl.pendingMessageType() == MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_OPRO) { + auto nBytes = decodeOPROMessage(&prop, chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), false); + if (nBytes < 0) { + throw std::invalid_argument(strerror(errno)); + } + 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"); + } + } + } + return chnl; +} + +Channel& operator<<(Channel& chnl, const ControlSignal::message_type::SharedPtr csp) { + RemoteControlManoeuvreMessageType rcmm; + rcmm.command = MANOEUVRE_NONE; + rcmm.isThrottleManoeuvreValid = 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; + auto nBytes = encodeRCMMMessage(&rcmm, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); + if (nBytes < 0) { + throw std::invalid_argument(std::string("Failed to encode RCM message: ") + strerror(errno)); + } + nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send RCM: ") + strerror(errno)); + } + return chnl; +} + +Channel& operator<<(Channel& chnl, const std::vector& data) { + auto nBytes = send(chnl.socket, data.data(), data.size(), 0); + if (nBytes < 0) { + throw std::runtime_error(std::string("Failed to send raw data: ") + strerror(errno)); + } + return chnl; +} + +std::string Channel::remoteIP() const { + char ipString[INET_ADDRSTRLEN]; + 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); + 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) { + throw std::runtime_error("Connection reset by peer"); + } + 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()); + } + return retval; + } +} + +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) { + errMsg << "inet_ntop: " << strerror(errno); + throw std::invalid_argument(errMsg.str()); + } + + if (this->addr.sin_addr.s_addr == 0) { + errMsg << "Invalid connection IP specified: " << ipString; + throw std::invalid_argument(errMsg.str()); + } + + std::string type = "???"; + if (this->channelType == SOCK_STREAM) { + type = "TCP"; + } + else if (this->channelType == SOCK_DGRAM) { + type = "UDP"; + } + + this->socket = ::socket(AF_INET, this->channelType, 0); + if (this->socket < 0) { + errMsg << "Failed to open " << type << " socket: " << strerror(errno); + this->disconnect(); + throw std::runtime_error(errMsg.str()); + } + + // Begin connection attempt + 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) { + 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) { + errMsg << "Connection attempt interrupted"; + throw std::runtime_error(errMsg.str()); + } + } + } +} + +void Channel::disconnect() { + if (this->socket != -1) { + if (shutdown(this->socket, SHUT_RDWR) == -1) { + RCLCPP_ERROR(get_logger(), "Socket shutdown: %s", strerror(errno)); + } + if (close(this->socket) == -1) { + RCLCPP_ERROR(get_logger(), "Socket close: %s", strerror(errno)); + } + this->socket = -1; + } +} \ No newline at end of file diff --git a/modules/ObjectControl/src/main.cpp b/modules/ObjectControl/src/main.cpp index a9a036373..f5e34dff8 100644 --- a/modules/ObjectControl/src/main.cpp +++ b/modules/ObjectControl/src/main.cpp @@ -7,8 +7,10 @@ int main(int argc, char **argv) { rclcpp::init(argc,argv); - auto obc = std::make_shared(); - rclcpp::spin(obc); + auto executor = std::make_shared(); + auto obc = std::make_shared(executor); + executor->add_node(obc); + executor->spin(); rclcpp::shutdown(); return 0; diff --git a/modules/ObjectControl/src/objectconnection.cpp b/modules/ObjectControl/src/objectconnection.cpp new file mode 100644 index 000000000..311eed5c6 --- /dev/null +++ b/modules/ObjectControl/src/objectconnection.cpp @@ -0,0 +1,79 @@ +/* + * 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/. + */ +#include "objectconnection.hpp" + +void ObjectConnection::connect( + std::shared_future stopRequest, + const std::chrono::milliseconds retryPeriod) { + try { + this->cmd.connect(stopRequest, retryPeriod); + this->mntr.connect(stopRequest, retryPeriod); + } catch (std::runtime_error& e) { + this->disconnect(); + throw std::runtime_error(std::string("Failed to establish ISO 22133 connection. Reason: \n") + e.what()); + } + + return; +} + +bool ObjectConnection::isConnected() const { + if (!isValid()) { + return false; + } + pollfd fds[2]; + fds[0].fd = mntr.socket; + fds[0].events = POLLIN | POLLOUT; + fds[1].fd = cmd.socket; + fds[1].events = POLLIN | POLLOUT; + return poll(fds, 2, 0) >= 0; +} + +bool ObjectConnection::isValid() const { + return this->cmd.isValid() && this->mntr.isValid(); +} + +void ObjectConnection::disconnect() { + this->cmd.disconnect(); + this->mntr.disconnect(); + close(this->interruptionPipeFds[0]); + close(this->interruptionPipeFds[1]); +} + +ISOMessageID ObjectConnection::pendingMessageType(bool awaitNext) { + if (awaitNext) { + if (!isValid()) { + throw std::invalid_argument("Attempted to check pending message type for unconnected object"); + } + fd_set fds; + FD_ZERO(&fds); + FD_SET(interruptionPipeFds[0], &fds); + FD_SET(mntr.socket, &fds); + FD_SET(cmd.socket, &fds); + auto result = select(std::max({mntr.socket,cmd.socket,interruptionPipeFds[0]})+1, + &fds, nullptr, nullptr, nullptr); + if (result < 0) { + throw std::runtime_error(std::string("Failed socket operation (select: ") + strerror(errno) + ")"); // TODO clearer + } + else if (!isValid()) { + throw std::invalid_argument("Connection invalidated during select call"); + } + else if (FD_ISSET(interruptionPipeFds[0], &fds)){ + close(interruptionPipeFds[0]); + throw std::range_error("Select call was interrupted"); + } + else if (FD_ISSET(mntr.socket, &fds)) { + return this->mntr.pendingMessageType(); + } + else if (FD_ISSET(cmd.socket, &fds)) { + return this->cmd.pendingMessageType(); + } + throw std::logic_error("Call to select returned unexpectedly: " + std::to_string(result)); + } + else { + auto retval = this->mntr.pendingMessageType(); + return retval != MESSAGE_ID_INVALID ? retval : this->cmd.pendingMessageType(); + } +} \ No newline at end of file diff --git a/modules/ObjectControl/src/objectcontrol.cpp b/modules/ObjectControl/src/objectcontrol.cpp index 2fe949b1f..c46fb988b 100644 --- a/modules/ObjectControl/src/objectcontrol.cpp +++ b/modules/ObjectControl/src/objectcontrol.cpp @@ -27,7 +27,7 @@ using namespace ROSChannels; using namespace std::chrono_literals; using namespace ATOS; -ObjectControl::ObjectControl() +ObjectControl::ObjectControl(std::shared_ptr exec) : Module(ObjectControl::moduleName), scnInitSub(*this, std::bind(&ObjectControl::onInitMessage, this, _1)), scnStartSub(*this, std::bind(&ObjectControl::onStartMessage, this, _1)), @@ -42,10 +42,12 @@ ObjectControl::ObjectControl() getStatusSub(*this, std::bind(&ObjectControl::onGetStatusMessage, this, _1)), scnRemoteControlEnableSub(*this, std::bind(&ObjectControl::onRemoteControlEnableMessage, this, _1)), scnRemoteControlDisableSub(*this, std::bind(&ObjectControl::onRemoteControlDisableMessage, this, _1)), + objectStateChangeSub(*this, std::bind(&ObjectControl::onObjectStateChangeMessage, this, _1)), failurePub(*this), scnAbortPub(*this), objectsConnectedPub(*this), - connectedObjectIdsPub(*this) + connectedObjectIdsPub(*this), + exec(exec) { int queueSize=0; objectsConnectedTimer = create_wall_timer(1000ms, std::bind(&ObjectControl::publishObjectIds, this)); @@ -251,13 +253,11 @@ void ObjectControl::loadScenario() { RCLCPP_INFO(get_logger(), "Received %d configured object ids", idResponse->ids.size()); for (const auto id : idResponse->ids) { - auto trajletSub = std::make_shared(*this, id, std::bind(&ObjectControl::onPathMessage, this, _1, id)); - auto monrPub = std::make_shared(*this, id); - auto navSatFixPub = std::make_shared(*this, id); - auto object = std::make_shared(this->get_logger(), trajletSub, monrPub, navSatFixPub); + auto object = std::make_shared(id); 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 +273,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) { @@ -306,7 +307,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) { @@ -320,7 +321,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) { @@ -376,12 +377,7 @@ void ObjectControl::loadObjectFiles() { // Check preexisting auto foundObject = objects.find(id); if (foundObject == objects.end()) { - // Create sub and pub as unique ptrs, when TestObject is destroyed, these get destroyed too. - - auto trajletSub = std::make_shared(*this, id, std::bind(&ObjectControl::onPathMessage, this, _1, id)); - auto monrPub = std::make_shared(*this, id); - auto navSatFixPub = std::make_shared(*this, id); - std::shared_ptr object = std::make_shared(get_logger(),trajletSub,monrPub,navSatFixPub); + std::shared_ptr object = std::make_shared(id); object->parseConfigurationFile(inputFile); objects.emplace(id, object); } @@ -578,6 +574,7 @@ OsiHandler::LocalObjectGroundTruth_t ObjectControl::buildOSILocalGroundTruth( return gt; } +// 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()) { std::chrono::system_clock::time_point ts; @@ -750,6 +747,36 @@ void ObjectControl::startObject( } } +void ObjectControl::onObjectStateChangeMessage(const ObjectStateChange::message_type::SharedPtr stateChangeMsg) { + ObjectStateType prevState = static_cast(stateChangeMsg->prev_state.state); + ObjectStateType state = static_cast(stateChangeMsg->state.state); + uint32_t id = stateChangeMsg->id; + switch (state) { + case OBJECT_STATE_DISARMED: + if (prevState == OBJECT_STATE_ABORTING) { + this->state->objectAbortDisarmed(*this, id); + } + else { + this->state->objectDisarmed(*this, id); + } + break; + case OBJECT_STATE_ARMED: + this->state->objectArmed(*this, id); + break; + case OBJECT_STATE_ABORTING: + this->state->objectAborting(*this, id); + break; + case OBJECT_STATE_INIT: + case OBJECT_STATE_RUNNING: + case OBJECT_STATE_POSTRUN: + case OBJECT_STATE_REMOTE_CONTROL: + break; + + default: + RCLCPP_WARN(get_logger(), "Received unknown object state change message for object %u: %d", id, state); + } +} + void ObjectControl::allClearObjects() { // Send allClear to all connected objects for (auto& id : getVehicleIDs()) { diff --git a/modules/ObjectControl/src/objectlistener.cpp b/modules/ObjectControl/src/objectlistener.cpp index 3dbc515b6..eb446dd50 100644 --- a/modules/ObjectControl/src/objectlistener.cpp +++ b/modules/ObjectControl/src/objectlistener.cpp @@ -5,19 +5,8 @@ */ #include "objectlistener.hpp" #include "objectcontrol.hpp" -#include "atosTime.h" -#include "iso22133.h" #include "state.hpp" -#include "journal.hpp" -#include -#include -#include -#include -#include #include -#include "roschannels/monitorchannel.hpp" -#include "roschannels/navsatfixchannel.hpp" -#include "util/coordinateutils.hpp"// xyz2llh #if ROS_FOXY #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -25,10 +14,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -static ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, const ObjectMonitorType& anchor, const bool debug = false); -static atos_interfaces::msg::Monitor createROSMessage(const MonitorMessage& data); // TODO move to somewhere central -static sensor_msgs::msg::NavSatFix createNavSatFixMessage(const struct timeval& tv, std::array origin, const atos_interfaces::msg::Monitor &monr); // TODO move to somewhere central - ObjectListener::ObjectListener( ObjectControl* sh, std::shared_ptr ob, @@ -56,70 +41,8 @@ ObjectListener::~ObjectListener() { void ObjectListener::listen() { try { while (!this->quit) { - switch (obj->pendingMessageType(true)) { - case MESSAGE_ID_MONR: { - struct timeval currentTime; - auto prevObjState = obj->getState(); - auto monr = obj->readMonitorMessage(); - TimeSetToCurrentSystemTime(¤tTime); - if (handler->controlMode == ObjectControl::RELATIVE_KINEMATICS && !obj->isAnchor()) { - monr.second = transformCoordinate(monr.second, handler->getLastAnchorData(), true); - } - - // Publish monitor data to journal - auto objData = obj->getAsObjectData(); - objData.MonrData = monr.second; - JournalRecordMonitorData(&objData); - - // Publish a ROS monr message - auto rosMonr = createROSMessage(monr); - obj->publishMonr(rosMonr); - - // Publish a NavSatFix message - auto origin = obj->getOrigin(); - std::array llh_0 = {origin.latitude_deg, origin.longitude_deg, origin.altitude_m}; - obj->publishNavSatFix(createNavSatFixMessage(monr.second.timestamp, llh_0, rosMonr)); - - // Check if state has changed - if (obj->getState() != prevObjState) { - switch (obj->getState()) { - case OBJECT_STATE_DISARMED: - if (prevObjState == OBJECT_STATE_ABORTING) { - RCLCPP_INFO(get_logger(), "Object %u abort cleared", obj->getTransmitterID()); - handler->state->objectAbortDisarmed(*handler, obj->getTransmitterID()); - } - else { - RCLCPP_INFO(get_logger(), "Object %u disarmed", obj->getTransmitterID()); - handler->state->objectDisarmed(*handler, obj->getTransmitterID()); - } - break; - case OBJECT_STATE_POSTRUN: - break; - case OBJECT_STATE_ARMED: - RCLCPP_INFO(get_logger(), "Object %u armed", obj->getTransmitterID()); - handler->state->objectArmed(*handler, obj->getTransmitterID()); - break; - case OBJECT_STATE_ABORTING: - RCLCPP_INFO(get_logger(), "Object %u aborting", obj->getTransmitterID()); - handler->state->objectAborting(*handler, obj->getTransmitterID()); - break; - } - } - - handler->injectObjectData(monr); - - break; - } - case MESSAGE_ID_TREO: - RCLCPP_WARN(get_logger(), "Unhandled TREO message"); - break; - case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_OPRO: - obj->parseObjectPropertyMessage(); - break; - default: - RCLCPP_WARN(get_logger(), "Received unknown message type"); - break; - } + //handle incoming iso22133 messages + obj->handleISOMessage(true); } } catch (std::invalid_argument& e) { RCLCPP_ERROR(get_logger(), e.what()); @@ -131,148 +54,4 @@ void ObjectListener::listen() { handler->state->disconnectedFromObject(*handler, obj->getTransmitterID()); } RCLCPP_INFO(get_logger(), "Listener thread for object %u exiting", obj->getTransmitterID()); -} - -/*! - * \brief transformCoordinate 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 - */ -ObjectMonitorType transformCoordinate( - const ObjectMonitorType& point, - const ObjectMonitorType& anchor, - const bool debug) { - using namespace Eigen; - using namespace std::chrono; - static auto nextPrintTime = steady_clock::now(); - static constexpr auto printInterval = seconds(5); - bool print = false; - if (steady_clock::now() > nextPrintTime) { - 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}; - std::chrono::milliseconds diff; - if (point.isTimestampValid && anchor.isTimestampValid) { - timersub(&point.timestamp, &anchor.timestamp, &tvdiff); - } - from_timeval(tvdiff, diff); - - AngleAxis anchorToGlobal(anchor.position.heading_rad, Vector3d::UnitZ()); - AngleAxis pointToGlobal(point.position.heading_rad, Vector3d::UnitZ()); - Vector3d pointPos(point.position.xCoord_m, point.position.yCoord_m, point.position.zCoord_m); - Vector3d anchorPos(anchor.position.xCoord_m, anchor.position.yCoord_m, anchor.position.zCoord_m); - Vector3d pointVel(point.speed.longitudinal_m_s, point.speed.lateral_m_s, 0.0); - Vector3d anchorVel(anchor.speed.longitudinal_m_s, anchor.speed.lateral_m_s, 0.0); - Vector3d pointAcc(point.acceleration.longitudinal_m_s2, point.acceleration.lateral_m_s2, 0.0); - Vector3d anchorAcc(anchor.acceleration.longitudinal_m_s2, anchor.acceleration.lateral_m_s2, 0.0); - - 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; - } - - // 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 - - 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.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.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; - if (print) { - dbg << "res: " << pointInAnchorFrame << ", " << retval.position.heading_rad*180.0/M_PI << "deg"; - std::cerr << dbg.str(); - } - return retval; -} - - -atos_interfaces::msg::Monitor createROSMessage(const MonitorMessage& monrMessage) { - atos_interfaces::msg::Monitor msg; - auto txid = monrMessage.first; - auto indata = monrMessage.second; - 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; -} - -sensor_msgs::msg::NavSatFix createNavSatFixMessage(const struct timeval& tv, std::array origin, const atos_interfaces::msg::Monitor &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; } \ No newline at end of file diff --git a/modules/ObjectControl/src/relativetestobject.cpp b/modules/ObjectControl/src/relativetestobject.cpp new file mode 100644 index 000000000..fb617b335 --- /dev/null +++ b/modules/ObjectControl/src/relativetestobject.cpp @@ -0,0 +1,112 @@ +#include "relativetestobject.hpp" +#include "testobject.hpp" +#include "atosTime.h" +#include + +RelativeTestObject::RelativeTestObject(uint32_t id, uint32_t anchorID) : + TestObject(id), + anchor(anchorID) +{ + isAnchor = id == anchorID; +} + +/*! + * \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 + */ +MonitorMessage RelativeTestObject::readMonitorMessage() { + MonitorMessage retval; + this->comms.mntr >> retval; + lastMonitorTime = clock::now(); + updateMonitor(retval); + if (!isAnchor){ // If we are not the anchor, transform the monitor data relative to anchor + transformCoordinate(retval.second, + retval.second, // TODO: replace this with last anchor monr from OBC + true); + } + return retval; +} + + +/*! + * \brief transformCoordinate 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 + */ +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 constexpr auto printInterval = seconds(5); + bool print = false; + if (steady_clock::now() > nextPrintTime) { + 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}; + std::chrono::milliseconds diff; + if (point.isTimestampValid && anchor.isTimestampValid) { + timersub(&point.timestamp, &anchor.timestamp, &tvdiff); + } + from_timeval(tvdiff, diff); + + AngleAxis anchorToGlobal(anchor.position.heading_rad, Vector3d::UnitZ()); + AngleAxis pointToGlobal(point.position.heading_rad, Vector3d::UnitZ()); + Vector3d pointPos(point.position.xCoord_m, point.position.yCoord_m, point.position.zCoord_m); + Vector3d anchorPos(anchor.position.xCoord_m, anchor.position.yCoord_m, anchor.position.zCoord_m); + Vector3d pointVel(point.speed.longitudinal_m_s, point.speed.lateral_m_s, 0.0); + Vector3d anchorVel(anchor.speed.longitudinal_m_s, anchor.speed.lateral_m_s, 0.0); + Vector3d pointAcc(point.acceleration.longitudinal_m_s2, point.acceleration.lateral_m_s2, 0.0); + Vector3d anchorAcc(anchor.acceleration.longitudinal_m_s2, anchor.acceleration.lateral_m_s2, 0.0); + + 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; + } + + // 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 + + 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.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.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; + if (print) { + dbg << "res: " << pointInAnchorFrame << ", " << retval.position.heading_rad*180.0/M_PI << "deg"; + std::cerr << dbg.str(); + } + return retval; +} diff --git a/modules/ObjectControl/src/states/idle.cpp b/modules/ObjectControl/src/states/idle.cpp index 42c4d8c02..0c47ce2c7 100644 --- a/modules/ObjectControl/src/states/idle.cpp +++ b/modules/ObjectControl/src/states/idle.cpp @@ -19,7 +19,7 @@ void AbstractKinematics::Idle::initializeRequest( ObjectControl& handler) { RCLCPP_INFO(handler.get_logger(), "Handling initialization request"); JournalRecordData(JOURNAL_RECORD_EVENT, "INIT received"); - handler.loadScenario(); + handler.loadScenario(); // Reload objects on each initialize request. try { auto anchorID = handler.getAnchorObjectID(); handler.transformScenarioRelativeTo(anchorID); diff --git a/modules/ObjectControl/src/testobject.cpp b/modules/ObjectControl/src/testobject.cpp index aa5ce10fa..6087ddb6f 100644 --- a/modules/ObjectControl/src/testobject.cpp +++ b/modules/ObjectControl/src/testobject.cpp @@ -7,22 +7,24 @@ #include "util.h" #include "atosTime.h" #include "osi_handler.hpp" +#include "journal.hpp" using namespace ATOS; +using std::placeholders::_1; -TestObject::TestObject(rclcpp::Logger log, - std::shared_ptr pathSub, - std::shared_ptr monrPub, - std::shared_ptr navSatFixPub - ) : - osiChannel(SOCK_STREAM, log), - comms(log), - Loggable(log), - pathSub(pathSub), - monrPub(monrPub), - navSatFixPub(navSatFixPub), - conf(log) +TestObject::TestObject(uint32_t id) : + Node("object_" + std::to_string(id)), + osiChannel(SOCK_STREAM, get_logger()), + comms(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 msg, int id){ + ; } TestObject::TestObject(TestObject&& other) : @@ -32,7 +34,7 @@ TestObject::TestObject(TestObject&& other) : conf(other.conf), lastMonitor(other.lastMonitor), maxAllowedMonitorPeriod(other.maxAllowedMonitorPeriod), - Loggable(other.get_logger()) + rclcpp::Node(other.get_name()) { this->comms = other.comms; this->osiChannel = other.osiChannel; @@ -148,6 +150,33 @@ MonitorMessage TestObject::awaitNextMonitor() { } } +void TestObject::handleISOMessage(bool awaitNext) { + auto message = this->comms.pendingMessageType(awaitNext); + switch (message) { + case MESSAGE_ID_MONR: + { + struct timeval currentTime; + auto prevObjState = this->getState(); + 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; + 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 (" @@ -224,109 +253,6 @@ void TestObject::establishConnection(std::shared_future stopRequest) { } } -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) { - errMsg << "inet_ntop: " << strerror(errno); - throw std::invalid_argument(errMsg.str()); - } - - if (this->addr.sin_addr.s_addr == 0) { - errMsg << "Invalid connection IP specified: " << ipString; - throw std::invalid_argument(errMsg.str()); - } - - std::string type = "???"; - if (this->channelType == SOCK_STREAM) { - type = "TCP"; - } - else if (this->channelType == SOCK_DGRAM) { - type = "UDP"; - } - - this->socket = ::socket(AF_INET, this->channelType, 0); - if (this->socket < 0) { - errMsg << "Failed to open " << type << " socket: " << strerror(errno); - this->disconnect(); - throw std::runtime_error(errMsg.str()); - } - - // Begin connection attempt - 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) { - 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) { - errMsg << "Connection attempt interrupted"; - throw std::runtime_error(errMsg.str()); - } - } - } -} - -void Channel::disconnect() { - if (this->socket != -1) { - if (shutdown(this->socket, SHUT_RDWR) == -1) { - RCLCPP_ERROR(get_logger(), "Socket shutdown: %s", strerror(errno)); - } - if (close(this->socket) == -1) { - RCLCPP_ERROR(get_logger(), "Socket close: %s", strerror(errno)); - } - this->socket = -1; - } -} - -void ObjectConnection::connect( - std::shared_future stopRequest, - const std::chrono::milliseconds retryPeriod) { - try { - this->cmd.connect(stopRequest, retryPeriod); - this->mntr.connect(stopRequest, retryPeriod); - } catch (std::runtime_error& e) { - this->disconnect(); - throw std::runtime_error(std::string("Failed to establish ISO 22133 connection. Reason: \n") + e.what()); - } - - return; -} - - -bool ObjectConnection::isConnected() const { - if (!isValid()) { - return false; - } - pollfd fds[2]; - fds[0].fd = mntr.socket; - fds[0].events = POLLIN | POLLOUT; - fds[1].fd = cmd.socket; - fds[1].events = POLLIN | POLLOUT; - return poll(fds, 2, 0) >= 0; -} - -bool ObjectConnection::isValid() const { - return this->cmd.isValid() && this->mntr.isValid(); -} - -void ObjectConnection::disconnect() { - this->cmd.disconnect(); - this->mntr.disconnect(); - close(this->interruptionPipeFds[0]); - close(this->interruptionPipeFds[1]); -} void TestObject::sendHeartbeat( const ControlCenterStatusType ccStatus) { @@ -389,11 +315,6 @@ void TestObject::sendRemoteControl(bool on) { this->comms.cmd << (on ? OBJECT_COMMAND_REMOTE_CONTROL : OBJECT_COMMAND_DISARM); } -//void TestObject::sendOsiGlobalObjectGroundTruth( -// GlobalObjectGroundTruth_t gogt) { - //this->comms.osi << llls; -//} - void TestObject::sendOsiData( const OsiHandler::LocalObjectGroundTruth_t& osidata, const std::string& projStr, @@ -418,232 +339,27 @@ void TestObject::sendControlSignal(const ControlSignalPercentage::SharedPtr csp) this->comms.mntr << csp; } -ISOMessageID Channel::pendingMessageType(bool awaitNext) { - 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) { - throw std::runtime_error("Connection reset by peer"); - } - 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()); - } - return retval; - } -} +void TestObject::publishMonitor(MonitorMessage& monr){ + // Publish to journal + auto objData = this->getAsObjectData(); + objData.MonrData = monr.second; + JournalRecordMonitorData(&objData); -ISOMessageID ObjectConnection::pendingMessageType(bool awaitNext) { - if (awaitNext) { - if (!isValid()) { - throw std::invalid_argument("Attempted to check pending message type for unconnected object"); - } - fd_set fds; - FD_ZERO(&fds); - FD_SET(interruptionPipeFds[0], &fds); - FD_SET(mntr.socket, &fds); - FD_SET(cmd.socket, &fds); - auto result = select(std::max({mntr.socket,cmd.socket,interruptionPipeFds[0]})+1, - &fds, nullptr, nullptr, nullptr); - if (result < 0) { - throw std::runtime_error(std::string("Failed socket operation (select: ") + strerror(errno) + ")"); // TODO clearer - } - else if (!isValid()) { - throw std::invalid_argument("Connection invalidated during select call"); - } - else if (FD_ISSET(interruptionPipeFds[0], &fds)){ - close(interruptionPipeFds[0]); - throw std::range_error("Select call was interrupted"); - } - else if (FD_ISSET(mntr.socket, &fds)) { - return this->mntr.pendingMessageType(); - } - else if (FD_ISSET(cmd.socket, &fds)) { - return this->cmd.pendingMessageType(); - } - throw std::logic_error("Call to select returned unexpectedly: " + std::to_string(result)); - } - else { - auto retval = this->mntr.pendingMessageType(); - return retval != MESSAGE_ID_INVALID ? retval : this->cmd.pendingMessageType(); - } -} - -Channel& operator<<(Channel& chnl, const HeabMessageDataType& heartbeat) { - auto nBytes = encodeHEABMessage(&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)); - } - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send HEAB: ") + strerror(errno)); - } - return chnl; -} - -Channel& operator<<(Channel& chnl, const ObjectSettingsType& settings) { - auto nBytes = encodeOSEMMessage(&settings, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); - if (nBytes < 0) { - throw std::invalid_argument(std::string("Failed to encode OSEM message: ") + strerror(errno)); - } - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send OSEM: ") + strerror(errno)); - } - return chnl; -} - -Channel& operator<<(Channel& chnl, const Trajectory& traj) { - ssize_t nBytes; + // Publish to ROS topic + auto rosMonr = ROSChannels::Monitor::fromMonr(monr.first,monr.second); + publishMonr(rosMonr); - // TRAJ header - nBytes = encodeTRAJMessageHeader( - 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)); - } - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send TRAJ message header: ") + strerror(errno)); - } - - // TRAJ points - for (const auto& pt : traj.points) { - struct timeval relTime; - CartesianPosition pos = pt.getISOPosition(); - 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); - if (nBytes < 0) { - // TODO what to do here? - throw std::invalid_argument(std::string("Failed to encode TRAJ message point: ") + strerror(errno)); - } - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - - if (nBytes < 0) { - // TODO what to do here? - throw std::runtime_error(std::string("Failed to send TRAJ message point: ") + strerror(errno)); - } - } - - // TRAJ footer - nBytes = encodeTRAJMessageFooter(chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); - if (nBytes < 0) { - throw std::invalid_argument(std::string("Failed to encode TRAJ message footer: ") + strerror(errno)); - } - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send TRAJ message footer: ") + strerror(errno)); - } - return chnl; + // 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}; + publishNavSatFix(ROSChannels::NavSatFix::fromMonr(llh_0, rosMonr)); } -Channel& operator<<(Channel& chnl, const ObjectCommandType& cmd) { - auto nBytes = encodeOSTMMessage(cmd, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); - if (nBytes < 0) { - throw std::invalid_argument(std::string("Failed to encode OSTM message: ") + strerror(errno)); - } - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send OSTM: ") + strerror(errno)); - } - return chnl; -} - -Channel& operator<<(Channel& chnl, const StartMessageType& strt) { - auto nBytes = encodeSTRTMessage(&strt, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); - if (nBytes < 0) { - throw std::invalid_argument(std::string("Failed to encode STRT message: ") + strerror(errno)); - } - - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send STRT: ") + strerror(errno)); - } - return chnl; -} - -Channel& operator>>(Channel& chnl, MonitorMessage& monitor) { - if (chnl.pendingMessageType() == MESSAGE_ID_MONR) { - struct timeval tv; - TimeSetToCurrentSystemTime(&tv); - auto nBytes = decodeMONRMessage(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), tv, - &monitor.first, &monitor.second, false); - if (nBytes < 0) { - throw std::invalid_argument("Failed to decode MONR message"); - } - 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"); - } - } - } - return chnl; -} - -Channel& operator>>(Channel& chnl, ObjectPropertiesType& prop) { - if (chnl.pendingMessageType() == MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_OPRO) { - auto nBytes = decodeOPROMessage(&prop, chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), false); - if (nBytes < 0) { - throw std::invalid_argument(strerror(errno)); - } - 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"); - } - } - } - return chnl; -} - -Channel& operator<<(Channel& chnl, const ControlSignalPercentage::SharedPtr csp) { - RemoteControlManoeuvreMessageType rcmm; - rcmm.command = MANOEUVRE_NONE; - rcmm.isThrottleManoeuvreValid = 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; - auto nBytes = encodeRCMMMessage(&rcmm, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false); - if (nBytes < 0) { - throw std::invalid_argument(std::string("Failed to encode RCM message: ") + strerror(errno)); - } - nBytes = send(chnl.socket, chnl.transmitBuffer.data(), static_cast(nBytes), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send RCM: ") + strerror(errno)); - } - return chnl; -} - -Channel& operator<<(Channel& chnl, const std::vector& data) { - auto nBytes = send(chnl.socket, data.data(), data.size(), 0); - if (nBytes < 0) { - throw std::runtime_error(std::string("Failed to send raw data: ") + strerror(errno)); - } - return chnl; -} - -std::string Channel::remoteIP() const { - char ipString[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &this->addr.sin_addr, ipString, sizeof (ipString)); - return std::string(ipString); +void TestObject::publishStateChange(ObjectStateType &prevObjState){ + ROSChannels::ObjectStateChange::message_type msg; + msg.id = this->getTransmitterID(); + msg.prev_state.state = prevObjState; + msg.state.state = this->getState(); + + stateChangePub->publish(msg); } \ No newline at end of file