From 225b2a19dc70976a514e397538fc41612e36eba0 Mon Sep 17 00:00:00 2001 From: Mats Nilsson Date: Wed, 26 Feb 2025 10:19:31 +0100 Subject: [PATCH 1/2] Format state_machine related files --- .../ObjectControl/inc/objectcontrol.hpp | 150 ++-- .../ObjectControl/inc/statemachine.hpp | 247 +++--- .../ObjectControl/src/objectcontrol.cpp | 743 +++++++++--------- 3 files changed, 551 insertions(+), 589 deletions(-) diff --git a/atos/modules/ObjectControl/inc/objectcontrol.hpp b/atos/modules/ObjectControl/inc/objectcontrol.hpp index 6abbe7344..d61858de4 100644 --- a/atos/modules/ObjectControl/inc/objectcontrol.hpp +++ b/atos/modules/ObjectControl/inc/objectcontrol.hpp @@ -4,38 +4,38 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #pragma once -#include -#include -#include #include -#include +#include +#include #include +#include +#include #include #include "sml.hpp" #include "geographic_msgs/msg/geo_point.hpp" -#include "module.hpp" #include "atosTime.h" -#include "testobject.hpp" +#include "atos_interfaces/srv/get_object_control_state.hpp" +#include "atos_interfaces/srv/get_object_ids.hpp" +#include "atos_interfaces/srv/get_object_ip.hpp" +#include "atos_interfaces/srv/get_object_return_trajectory.hpp" +#include "atos_interfaces/srv/get_object_trajectory.hpp" +#include "atos_interfaces/srv/get_object_trigger_start.hpp" +#include "atos_interfaces/srv/get_test_origin.hpp" +#include "module.hpp" #include "objectlistener.hpp" -#include "statemachine.hpp" #include "roschannels/commandchannels.hpp" -#include "roschannels/monitorchannel.hpp" -#include "roschannels/remotecontrolchannels.hpp" -#include "roschannels/pathchannel.hpp" -#include "roschannels/gnsspathchannel.hpp" #include "roschannels/controlsignalchannel.hpp" +#include "roschannels/gnsspathchannel.hpp" +#include "roschannels/monitorchannel.hpp" #include "roschannels/objstatechangechannel.hpp" +#include "roschannels/pathchannel.hpp" +#include "roschannels/remotecontrolchannels.hpp" #include "roschannels/statechange.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" -#include "atos_interfaces/srv/get_object_trigger_start.hpp" -#include "atos_interfaces/srv/get_test_origin.hpp" -#include "atos_interfaces/srv/get_object_control_state.hpp" -#include "atos_interfaces/srv/get_object_return_trajectory.hpp" +#include "statemachine.hpp" +#include "testobject.hpp" // Forward declarations class ObjectControlState; @@ -46,15 +46,13 @@ enum class ControlMode : int { RelativeKinematics, }; - /*! * \brief The ObjectControl class is intended as an overarching device * used to control a scenario. No behaviour is implemented in it * (this is left up to the State to determine), only functionality * which can be called. */ -class ObjectControl : public Module -{ +class ObjectControl : public Module { friend class ObjectControlState; friend class ObjectListener; @@ -63,7 +61,7 @@ class ObjectControl : public Module typedef struct { unsigned int numberOfTargets; - uint32_t *targetIDs; + uint32_t* targetIDs; bool isActive; } DataInjectionMap; @@ -106,7 +104,7 @@ class ObjectControl : public Module //! \brief Get transmitter IDs of all test participants. std::vector getVehicleIDs() const { std::vector retval; - for (auto it = objects.begin(); it != objects.end(); ++it) { + for (auto it = objects.begin(); it != objects.end(); ++it) { retval.push_back(it->first); } return retval; @@ -114,14 +112,15 @@ class ObjectControl : public Module [[deprecated("Avoid referring to objects by IP")]] uint32_t getVehicleIDByIP(const in_addr_t& ip) { - auto res = std::find_if(objects.begin(), objects.end(), [&](const std::pair>& elem){ - return elem.second->getObjectConfig().getIP() == ip; - }); + auto res = std::find_if( + objects.begin(), objects.end(), [&](const std::pair>& elem) { + return elem.second->getObjectConfig().getIP() == ip; + }); return res->first; } //! \brief Get last known ISO state of test participants. - std::map getObjectStates() const; + std::map getObjectStates() const; //! \brief Chneck if any object fulfill a predicate. bool isAnyObject(std::function)> predicate) const; @@ -144,7 +143,9 @@ class ObjectControl : public Module void startControlSignalSubscriber(); void stopControlSignalSubscriber(); - void setControlMode(ControlMode cm) { controlMode = cm; } + void setControlMode(ControlMode cm) { + controlMode = cm; + } private: bool isResetting; @@ -168,72 +169,78 @@ class ObjectControl : public Module 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 onPathMessage(const ROSChannels::Path::message_type::SharedPtr, const uint32_t); void onRequestState(const std::shared_ptr, - std::shared_ptr); + std::shared_ptr); using clock = std::chrono::steady_clock; ControlMode controlMode{ControlMode::AbsoluteKinematics}; - - std::map> objects; //!< List of configured test participants - std::map objectListeners; - std::map> storedActions; + std::map> objects; //!< List of configured test participants + std::map objectListeners; + std::map> storedActions; std::mutex monitorTimeMutex; static constexpr auto heartbeatPeriod = std::chrono::milliseconds(1000 / HEAB_FREQUENCY_HZ); std::thread safetyThread; std::promise stopHeartbeatSignal; - std::shared_future connStopReqFuture; //!< Request to stop a connection attempt + std::shared_future connStopReqFuture; //!< Request to stop a connection attempt std::promise connStopReqPromise; //!< Promise that the above value will be emitted atos_interfaces::srv::GetObjectTrajectory::Response::SharedPtr trajResponse; atos_interfaces::srv::GetObjectReturnTrajectory::Response::SharedPtr returnTrajResponse; - ROSChannels::Init::Sub scnInitSub; //!< Subscriber to scenario initialization requests - ROSChannels::Start::Sub scnStartSub; //!< Subscriber to scenario start requests - ROSChannels::StartObject::Sub objectStartSub; //!< Subscriber to scenario start requests - ROSChannels::Arm::Sub scnArmSub; //!< Subscriber to scenario arm requests - ROSChannels::Disarm::Sub scnDisarmSub; //!< Subscriber to scenario arm requests - ROSChannels::Stop::Sub scnStopSub; //!< Subscriber to scenario stop requests - ROSChannels::Abort::Sub scnAbortSub; //!< Subscriber to scenario abort requests - ROSChannels::AllClear::Sub scnAllClearSub; //!< Subscriber to scenario all clear requests - ROSChannels::Connect::Sub scnConnectSub; //!< Subscriber to scenario connect requests - ROSChannels::Disconnect::Sub scnDisconnectSub; //!< Subscriber to scenario disconnect requests - 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 object state changes - std::shared_ptr controlSignalSub; //!< Pointer to subscriber to receive control signal messages with percentage - ROSChannels::ResetTestObjects::Sub scnResetTestObjectsSub; //!< Subscriber to scenario reset test requests - ROSChannels::ReloadObjectSettings::Sub scnReloadObjectSettingsSub; //!< Subscriber to scenario reset test requests - - rclcpp::TimerBase::SharedPtr objectsConnectedTimer; //!< Timer to periodically publish connected objects + ROSChannels::Init::Sub scnInitSub; //!< Subscriber to scenario initialization requests + ROSChannels::Start::Sub scnStartSub; //!< Subscriber to scenario start requests + ROSChannels::StartObject::Sub objectStartSub; //!< Subscriber to scenario start requests + ROSChannels::Arm::Sub scnArmSub; //!< Subscriber to scenario arm requests + ROSChannels::Disarm::Sub scnDisarmSub; //!< Subscriber to scenario arm requests + ROSChannels::Stop::Sub scnStopSub; //!< Subscriber to scenario stop requests + ROSChannels::Abort::Sub scnAbortSub; //!< Subscriber to scenario abort requests + ROSChannels::AllClear::Sub scnAllClearSub; //!< Subscriber to scenario all clear requests + ROSChannels::Connect::Sub scnConnectSub; //!< Subscriber to scenario connect requests + ROSChannels::Disconnect::Sub scnDisconnectSub; //!< Subscriber to scenario disconnect requests + 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 object state changes + std::shared_ptr + controlSignalSub; //!< Pointer to subscriber to receive control signal messages with percentage + ROSChannels::ResetTestObjects::Sub scnResetTestObjectsSub; //!< Subscriber to scenario reset test requests + ROSChannels::ReloadObjectSettings::Sub scnReloadObjectSettingsSub; //!< Subscriber to scenario reset test requests + + rclcpp::TimerBase::SharedPtr objectsConnectedTimer; //!< Timer to periodically publish connected objects ROSChannels::Failure::Pub failurePub; //!< Publisher to scenario failure reports ROSChannels::Abort::Pub scnAbortPub; //!< Publisher to scenario abort reports - ROSChannels::ObjectsConnected::Pub objectsConnectedPub; //!< Publisher to report that objects have been connected - ROSChannels::ConnectedObjectIds::Pub connectedObjectIdsPub; //!< Publisher to periodically report connected object ids - ROSChannels::StateChange::Pub stateChangePub; //!< Publisher to report state changes - std::unordered_map pathPublishers; - std::unordered_map gnssPathPublishers; + ROSChannels::ObjectsConnected::Pub objectsConnectedPub; //!< Publisher to report that objects have been connected + ROSChannels::ConnectedObjectIds::Pub + connectedObjectIdsPub; //!< Publisher to periodically report connected object ids + ROSChannels::StateChange::Pub stateChangePub; //!< Publisher to report state changes + std::unordered_map pathPublishers; + std::unordered_map gnssPathPublishers; rclcpp::CallbackGroup::SharedPtr id_client_cb_group_; rclcpp::CallbackGroup::SharedPtr traj_client_cb_group_; rclcpp::CallbackGroup::SharedPtr ip_client_cb_group_; rclcpp::CallbackGroup::SharedPtr origin_client_cb_group_; - rclcpp::Client::SharedPtr idClient; //!< Client to request object ids - rclcpp::Client::SharedPtr originClient; //!< Client to request object status - rclcpp::Client::SharedPtr trajectoryClient; //!< Client to request object trajectories - rclcpp::Client::SharedPtr ipClient; //!< Client to request object IPs - rclcpp::Client::SharedPtr returnTrajectoryClient; //!< Client to request object return trajectory - rclcpp::Service::SharedPtr stateService; //!< Service to request object control state + rclcpp::Client::SharedPtr idClient; //!< Client to request object ids + rclcpp::Client::SharedPtr originClient; //!< Client to request object status + rclcpp::Client::SharedPtr + trajectoryClient; //!< Client to request object trajectories + rclcpp::Client::SharedPtr ipClient; //!< Client to request object IPs + rclcpp::Client::SharedPtr + returnTrajectoryClient; //!< Client to request object return trajectory + rclcpp::Service::SharedPtr + stateService; //!< Service to request object control state state_machine::Logger sm_logger{get_logger(), stateChangePub, failurePub}; boost::sml::sm, - boost::sml::thread_safe> sm{this, sm_logger}; + boost::sml::logger, + boost::sml::thread_safe> + sm{this, sm_logger}; public: //! Connection methods @@ -270,7 +277,8 @@ class ObjectControl : public Module void transformScenarioRelativeTo(const uint32_t objectID); //! \brief Upload the configuration in the ScenarioHandler to all connected objects. void uploadAllConfigurations(); - //! \brief Upload the configuration in the ScenarioHandler to the connected obj std::unique_ptr scenarioHandler; + //! \brief Upload the configuration in the ScenarioHandler to the connected obj std::unique_ptr + //! scenarioHandler; void uploadObjectConfiguration(const uint32_t id); //! \brief Clear loaded data and object list. void clearScenario(); @@ -281,7 +289,8 @@ class ObjectControl : public Module void disarmObjects(); //! \brief void startScenario(); - //! \brief Resets the test by offering a back to start trajectory. Still needs arm and start commands to execute the reset. + //! \brief Resets the test by offering a back to start trajectory. Still needs arm and start commands to execute the + //! reset. void resetTestObjects(); //! \brief Reloads the scenario trajectories for each object. void reloadScenarioTrajectories(); @@ -290,7 +299,8 @@ class ObjectControl : public Module //! \brief Callback for the trajectory request. Sends the new trajectory to the object. void trajectoryCallback(const rclcpp::Client::SharedFuture future); //! \brief Callback for the return trajectory request. Sends the new trajectory to the object. - void returnTrajectoryCallback(const rclcpp::Client::SharedFuture future); + void returnTrajectoryCallback( + const rclcpp::Client::SharedFuture future); //! \brief Requests a new trajectory and sends it to the object. void setObjectTrajectory(uint32_t id); //! \brief diff --git a/atos/modules/ObjectControl/inc/statemachine.hpp b/atos/modules/ObjectControl/inc/statemachine.hpp index 0281cf0e7..8a17d5eae 100644 --- a/atos/modules/ObjectControl/inc/statemachine.hpp +++ b/atos/modules/ObjectControl/inc/statemachine.hpp @@ -6,41 +6,43 @@ #pragma once -#include "sml.hpp" #include #include -#include "roschannels/objstatechangechannel.hpp" + #include "roschannels/commandchannels.hpp" +#include "roschannels/objstatechangechannel.hpp" #include "roschannels/statechange.hpp" +#include "sml.hpp" #include "util.h" class ObjectControl; namespace state_machine { - namespace events { - struct Initialize{}; - struct Connect{}; - struct Disconnect{}; - struct DisconnectedFromObject{ - uint32_t id{}; - }; - struct Ready{}; - struct Reset{}; - struct Reload{}; - struct Arm{}; - struct Disarm{}; - struct TestLive{}; - struct RemoteControl{}; - struct Clear{}; - struct Abort{}; - struct Start{}; - struct StartObject { - uint32_t id{}; - std::chrono::system_clock::time_point startTime{}; - }; - struct Done{}; -} + +struct Initialize {}; +struct Connect {}; +struct Disconnect {}; +struct DisconnectedFromObject { + uint32_t id{}; +}; +struct Ready {}; +struct Reset {}; +struct Reload {}; +struct Arm {}; +struct Disarm {}; +struct TestLive {}; +struct RemoteControl {}; +struct Clear {}; +struct Abort {}; +struct Start {}; +struct StartObject { + uint32_t id{}; + std::chrono::system_clock::time_point startTime{}; +}; +struct Done {}; + +} // namespace events static constexpr auto Idle = boost::sml::state; static constexpr auto Initialized = boost::sml::state; @@ -55,6 +57,7 @@ static constexpr auto Clearing = boost::sml::state; static constexpr auto Done = boost::sml::state; static constexpr auto RemoteControl = boost::sml::state; +// clang-format off struct idle_on_entry { void operator()(ObjectControl* oc) const; }; struct idle_to_init_guard { bool operator()(ObjectControl* oc) const; }; struct idle_to_init_action { void operator()(ObjectControl* oc) const; }; @@ -75,154 +78,138 @@ struct armed_on_enter { void operator()(ObjectControl* oc) const; }; struct armed_to_testlive_guard { bool operator()(ObjectControl* oc) const; }; struct test_live_start_object { void operator()(const events::StartObject& event, ObjectControl* oc) const; }; struct disconnected_from_object { void operator()(const events::DisconnectedFromObject& event, ObjectControl* oc) const; }; +// clang-format on template OBCState_t asNumber() { - if constexpr(std::is_same_v){ + if constexpr (std::is_same_v) { return OBC_STATE_IDLE; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_INITIALIZED; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_CONNECTED; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_CONNECTED; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_ARMED; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_DISARMING; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_RUNNING; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_REMOTECTRL; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_ABORTING; - } - else if constexpr(std::is_same_v){ + } else if constexpr (std::is_same_v) { return OBC_STATE_CLEARING; - } - else { + } else { return OBC_STATE_UNDEFINED; } } class Logger { public: - Logger(rclcpp::Logger l, - ROSChannels::StateChange::Pub sc, - ROSChannels::Failure::Pub f) : - logger_{l}, - stateChangePub_{sc}, - failurePub_{f} - {} - - template + Logger(rclcpp::Logger l, ROSChannels::StateChange::Pub sc, ROSChannels::Failure::Pub f) : + logger_{l}, + stateChangePub_{sc}, + failurePub_{f} {} + + template void log_process_event(const TEvent&) { RCLCPP_DEBUG(logger_, - "[%s][process_event] %s", - boost::sml::aux::get_type_name(), - boost::sml::aux::get_type_name()); + "[%s][process_event] %s", + boost::sml::aux::get_type_name(), + boost::sml::aux::get_type_name()); } - template + template void log_guard(const TGuard&, const TEvent&, bool result) { RCLCPP_DEBUG(logger_, - "[%s][guard] %s %s %s", - boost::sml::aux::get_type_name(), - boost::sml::aux::get_type_name(), - boost::sml::aux::get_type_name(), - (result ? "[OK]" : "[Reject]")); + "[%s][guard] %s %s %s", + boost::sml::aux::get_type_name(), + boost::sml::aux::get_type_name(), + boost::sml::aux::get_type_name(), + (result ? "[OK]" : "[Reject]")); } - template + template void log_action(const TAction&, const TEvent&) { RCLCPP_DEBUG(logger_, - "[%s][action] %s %s", - boost::sml::aux::get_type_name(), - boost::sml::aux::get_type_name(), - boost::sml::aux::get_type_name()); + "[%s][action] %s %s", + boost::sml::aux::get_type_name(), + boost::sml::aux::get_type_name(), + boost::sml::aux::get_type_name()); } - template + template void log_state_change(const TSrcState& src, const TDstState& dst) { - RCLCPP_DEBUG(logger_, - "[%s][transition] %s -> %s", - boost::sml::aux::get_type_name(), - src.c_str(), - dst.c_str()); - auto stateChangeMsg = atos_interfaces::msg::StateChange(); - stateChangeMsg.prev_state = asNumber(); + RCLCPP_DEBUG( + logger_, "[%s][transition] %s -> %s", boost::sml::aux::get_type_name(), src.c_str(), dst.c_str()); + auto stateChangeMsg = atos_interfaces::msg::StateChange(); + stateChangeMsg.prev_state = asNumber(); stateChangeMsg.current_state = asNumber(); stateChangePub_.publish(stateChangeMsg); } - private: +private: rclcpp::Logger logger_; ROSChannels::StateChange::Pub stateChangePub_; ROSChannels::Failure::Pub failurePub_; }; -class StateMachine -{ - public: - auto operator()() const noexcept - { +class StateMachine { +public: + auto operator()() const noexcept { namespace sml = boost::sml; return sml::make_transition_table( - *Idle + sml::on_entry / idle_on_entry(), - Idle + sml::event[idle_to_init_guard()] / idle_to_init_action() = Initialized, - - Initialized + sml::event / init_to_idle_action() = Idle, - Initialized + sml::event[init_to_connecting_guard()] / init_to_connecting_action() = Connecting, - - Connecting + sml::on_entry / connecting_on_entry(), - Connecting + sml::event = Aborting, - Connecting + sml::event = Disarming, - Connecting + sml::event / connecting_to_idle_action() = Idle, - Connecting + sml::event = Ready, - - Ready + sml::on_entry / ready_on_entry(), - Ready + sml::event = Aborting, - Ready + sml::event = Armed, - Ready + sml::event = Idle, - Ready + sml::event / disconnected_from_object() = Connecting, - Ready + sml::event = RemoteControl, - Ready + sml::event / ready_reset(), - Ready + sml::event / ready_reload(), - - Armed + sml::on_entry / armed_on_enter(), - Armed + sml::event[armed_to_testlive_guard()] = TestLive, - Armed + sml::event = Disarming, - - Disarming + sml::on_entry / disarming_on_entry(), - Disarming + sml::event = Aborting, - Disarming + sml::event = Connecting, - Disarming + sml::event = Idle, - Disarming + sml::event = Ready, - - TestLive + sml::event = Aborting, - TestLive + sml::event = Done, - TestLive + sml::event / test_live_start_object(), - - Done + sml::on_entry / done_on_entry(), - Done + sml::event = Ready, - - Aborting + sml::event = Clearing, - - Clearing + sml::on_entry / clearing_on_entry(), - Clearing + sml::event = Ready, - - RemoteControl + sml::on_entry / remote_control_on_entry(), - RemoteControl + sml::on_exit / remote_control_on_exit(), - RemoteControl + sml::event = Ready); + *Idle + sml::on_entry / idle_on_entry(), + Idle + sml::event[idle_to_init_guard()] / idle_to_init_action() = Initialized, + + Initialized + sml::event / init_to_idle_action() = Idle, + Initialized + sml::event[init_to_connecting_guard()] / init_to_connecting_action() = + Connecting, + + Connecting + sml::on_entry / connecting_on_entry(), + Connecting + sml::event = Aborting, + Connecting + sml::event = Disarming, + Connecting + sml::event / connecting_to_idle_action() = Idle, + Connecting + sml::event = Ready, + + Ready + sml::on_entry / ready_on_entry(), + Ready + sml::event = Aborting, + Ready + sml::event = Armed, + Ready + sml::event = Idle, + Ready + sml::event / disconnected_from_object() = Connecting, + Ready + sml::event = RemoteControl, + Ready + sml::event / ready_reset(), + Ready + sml::event / ready_reload(), + + Armed + sml::on_entry / armed_on_enter(), + Armed + sml::event[armed_to_testlive_guard()] = TestLive, + Armed + sml::event = Disarming, + + Disarming + sml::on_entry / disarming_on_entry(), + Disarming + sml::event = Aborting, + Disarming + sml::event = Connecting, + Disarming + sml::event = Idle, + Disarming + sml::event = Ready, + + TestLive + sml::event = Aborting, + TestLive + sml::event = Done, + TestLive + sml::event / test_live_start_object(), + + Done + sml::on_entry / done_on_entry(), + Done + sml::event = Ready, + + Aborting + sml::event = Clearing, + + Clearing + sml::on_entry / clearing_on_entry(), + Clearing + sml::event = Ready, + + RemoteControl + sml::on_entry / remote_control_on_entry(), + RemoteControl + sml::on_exit / remote_control_on_exit(), + RemoteControl + sml::event = Ready); } }; -} +} // namespace state_machine diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp index 181a6a4dd..a747776a2 100644 --- a/atos/modules/ObjectControl/src/objectcontrol.cpp +++ b/atos/modules/ObjectControl/src/objectcontrol.cpp @@ -3,19 +3,19 @@ * 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 -#include -#include #include -#include +#include +#include #include #include +#include +#include +#include #include -#include -#include +#include -#include "util.h" #include "journal.hpp" +#include "util.h" #include "objectcontrol.hpp" @@ -25,44 +25,48 @@ using namespace ROSChannels; using namespace std::chrono_literals; using namespace ATOS; -ObjectControl::ObjectControl(std::shared_ptr exec) - : Module(ObjectControl::moduleName), - exec(exec), - scnInitSub(*this, std::bind(&ObjectControl::onInitMessage, this, _1)), - scnStartSub(*this, std::bind(&ObjectControl::onStartMessage, this, _1)), - objectStartSub(*this, std::bind(&ObjectControl::onStartObjectMessage, this, _1)), - scnArmSub(*this, std::bind(&ObjectControl::onArmMessage, this, _1)), - scnDisarmSub(*this, std::bind(&ObjectControl::onDisarmMessage, this, _1)), - scnStopSub(*this, std::bind(&ObjectControl::onStopMessage, this, _1)), - scnAbortSub(*this, std::bind(&ObjectControl::onAbortMessage, this, _1)), - scnAllClearSub(*this, std::bind(&ObjectControl::onAllClearMessage, this, _1)), - scnConnectSub(*this, std::bind(&ObjectControl::onConnectMessage, this, _1)), - scnDisconnectSub(*this, std::bind(&ObjectControl::onDisconnectMessage, this, _1)), - scnRemoteControlEnableSub(*this, std::bind(&ObjectControl::onRemoteControlEnableMessage, this, _1)), - scnRemoteControlDisableSub(*this, std::bind(&ObjectControl::onRemoteControlDisableMessage, this, _1)), - getStatusSub(*this, std::bind(&ObjectControl::onGetStatusMessage, this, _1)), - objectStateChangeSub(*this, std::bind(&ObjectControl::onObjectStateChangeMessage, this, _1)), - scnResetTestObjectsSub(*this, std::bind(&ObjectControl::onResetTestObjectsMessage, this, _1)), - scnReloadObjectSettingsSub(*this, std::bind(&ObjectControl::onReloadObjectSettingsMessage, this, _1)), - failurePub(*this), - scnAbortPub(*this), - objectsConnectedPub(*this), - connectedObjectIdsPub(*this), - stateChangePub(*this) -{ - id_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - traj_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);; - ip_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); +ObjectControl::ObjectControl(std::shared_ptr exec) : + Module(ObjectControl::moduleName), + exec(exec), + scnInitSub(*this, std::bind(&ObjectControl::onInitMessage, this, _1)), + scnStartSub(*this, std::bind(&ObjectControl::onStartMessage, this, _1)), + objectStartSub(*this, std::bind(&ObjectControl::onStartObjectMessage, this, _1)), + scnArmSub(*this, std::bind(&ObjectControl::onArmMessage, this, _1)), + scnDisarmSub(*this, std::bind(&ObjectControl::onDisarmMessage, this, _1)), + scnStopSub(*this, std::bind(&ObjectControl::onStopMessage, this, _1)), + scnAbortSub(*this, std::bind(&ObjectControl::onAbortMessage, this, _1)), + scnAllClearSub(*this, std::bind(&ObjectControl::onAllClearMessage, this, _1)), + scnConnectSub(*this, std::bind(&ObjectControl::onConnectMessage, this, _1)), + scnDisconnectSub(*this, std::bind(&ObjectControl::onDisconnectMessage, this, _1)), + scnRemoteControlEnableSub(*this, std::bind(&ObjectControl::onRemoteControlEnableMessage, this, _1)), + scnRemoteControlDisableSub(*this, std::bind(&ObjectControl::onRemoteControlDisableMessage, this, _1)), + getStatusSub(*this, std::bind(&ObjectControl::onGetStatusMessage, this, _1)), + objectStateChangeSub(*this, std::bind(&ObjectControl::onObjectStateChangeMessage, this, _1)), + scnResetTestObjectsSub(*this, std::bind(&ObjectControl::onResetTestObjectsMessage, this, _1)), + scnReloadObjectSettingsSub(*this, std::bind(&ObjectControl::onReloadObjectSettingsMessage, this, _1)), + failurePub(*this), + scnAbortPub(*this), + objectsConnectedPub(*this), + connectedObjectIdsPub(*this), + stateChangePub(*this) { + id_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + traj_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + ip_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); origin_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); this->declare_parameter("max_missing_heartbeats", 100); objectsConnectedTimer = create_wall_timer(1000ms, std::bind(&ObjectControl::publishObjectIds, this)); - idClient = create_client(ServiceNames::getObjectIds, rmw_qos_profile_services_default, id_client_cb_group_); - originClient = create_client(ServiceNames::getTestOrigin, rmw_qos_profile_services_default, origin_client_cb_group_); - trajectoryClient = create_client(ServiceNames::getObjectTrajectory, rmw_qos_profile_services_default, traj_client_cb_group_); - ipClient = create_client(ServiceNames::getObjectIp, rmw_qos_profile_services_default, ip_client_cb_group_); - returnTrajectoryClient = create_client(ServiceNames::getObjectReturnTrajectory); - stateService = create_service(ServiceNames::getObjectControlState, - std::bind(&ObjectControl::onRequestState, this, _1, _2)); + idClient = create_client( + ServiceNames::getObjectIds, rmw_qos_profile_services_default, id_client_cb_group_); + originClient = create_client( + ServiceNames::getTestOrigin, rmw_qos_profile_services_default, origin_client_cb_group_); + trajectoryClient = create_client( + ServiceNames::getObjectTrajectory, rmw_qos_profile_services_default, traj_client_cb_group_); + ipClient = create_client( + ServiceNames::getObjectIp, rmw_qos_profile_services_default, ip_client_cb_group_); + returnTrajectoryClient = + create_client(ServiceNames::getObjectReturnTrajectory); + stateService = create_service( + ServiceNames::getObjectControlState, std::bind(&ObjectControl::onRequestState, this, _1, _2)); // Create test journal if (JournalInit(get_name(), get_logger()) == -1) { @@ -72,32 +76,30 @@ ObjectControl::ObjectControl(std::shared_ptr, - std::shared_ptr res) { +void ObjectControl::onRequestState(const std::shared_ptr, + std::shared_ptr res) { try { - res->state = stateAsNumber(); + res->state = stateAsNumber(); res->success = true; - } - catch (std::exception& e) { + } catch (std::exception& e) { RCLCPP_ERROR(get_logger(), "Failed to get state: %s", e.what()); - res->state = -1; + res->state = -1; res->success = false; } } -void ObjectControl::handleActionConfigurationCommand( - const TestScenarioCommandAction& /* action */) { +void ObjectControl::handleActionConfigurationCommand(const TestScenarioCommandAction& /* action */) { // TODO: Implement } -void ObjectControl::handleExecuteActionCommand( - const uint16_t &actionID, - const std::chrono::system_clock::time_point &when) { - auto delayedExecutor = [&](){ +void ObjectControl::handleExecuteActionCommand(const uint16_t& actionID, + const std::chrono::system_clock::time_point& when) { + auto delayedExecutor = [&]() { using namespace std::chrono; - RCLCPP_DEBUG(get_logger(), "Executing action %u in %ld ms", actionID, - duration_cast(when - system_clock::now()).count()); + RCLCPP_DEBUG(get_logger(), + "Executing action %u in %ld ms", + actionID, + duration_cast(when - system_clock::now()).count()); std::this_thread::sleep_until(when); RCLCPP_INFO(get_logger(), "Executing action %u", actionID); this->storedActions[actionID](); @@ -106,44 +108,44 @@ void ObjectControl::handleExecuteActionCommand( thd.detach(); } -void ObjectControl::onInitMessage(const Init::message_type::SharedPtr){ +void ObjectControl::onInitMessage(const Init::message_type::SharedPtr) { sm.process_event(state_machine::events::Initialize{}); } -void ObjectControl::onConnectMessage(const Connect::message_type::SharedPtr){ +void ObjectControl::onConnectMessage(const Connect::message_type::SharedPtr) { sm.process_event(state_machine::events::Connect{}); } -void ObjectControl::onArmMessage(const Arm::message_type::SharedPtr){ +void ObjectControl::onArmMessage(const Arm::message_type::SharedPtr) { sm.process_event(state_machine::events::Arm{}); } -void ObjectControl::onDisarmMessage(const Disarm::message_type::SharedPtr){ +void ObjectControl::onDisarmMessage(const Disarm::message_type::SharedPtr) { sm.process_event(state_machine::events::Disarm{}); } -void ObjectControl::onStartMessage(const Start::message_type::SharedPtr){ +void ObjectControl::onStartMessage(const Start::message_type::SharedPtr) { sm.process_event(state_machine::events::Start{}); } -void ObjectControl::onStartObjectMessage(const StartObject::message_type::SharedPtr strtObj){ +void ObjectControl::onStartObjectMessage(const StartObject::message_type::SharedPtr strtObj) { sm.process_event(state_machine::events::StartObject{ - strtObj->id, - std::chrono::system_clock::time_point{ - std::chrono::seconds{strtObj->stamp.sec} + std::chrono::nanoseconds{strtObj->stamp.nanosec}}}); + strtObj->id, + std::chrono::system_clock::time_point{std::chrono::seconds{strtObj->stamp.sec} + + std::chrono::nanoseconds{strtObj->stamp.nanosec}}}); } -void ObjectControl::onDisconnectMessage(const Disconnect::message_type::SharedPtr){ +void ObjectControl::onDisconnectMessage(const Disconnect::message_type::SharedPtr) { sm.process_event(state_machine::events::Disconnect{}); } -void ObjectControl::onStopMessage(const Stop::message_type::SharedPtr){} +void ObjectControl::onStopMessage(const Stop::message_type::SharedPtr) {} /** * @brief Resets the test objects to the scenario starting positions * * @param msg Trigger message for resetting test objects (std_msgs/msg/Empty) -*/ + */ void ObjectControl::onResetTestObjectsMessage(const ResetTestObjects::message_type::SharedPtr) { sm.process_event(state_machine::events::Reset{}); } @@ -152,38 +154,37 @@ void ObjectControl::onResetTestObjectsMessage(const ResetTestObjects::message_ty * @brief Reloads the object settings from the configuration file * * @param msg Trigger message for reloading object settings (std_msgs/msg/Empty) -*/ + */ void ObjectControl::onReloadObjectSettingsMessage(const ReloadObjectSettings::message_type::SharedPtr) { sm.process_event(state_machine::events::Reload{}); } -void ObjectControl::onAbortMessage(const Abort::message_type::SharedPtr){ +void ObjectControl::onAbortMessage(const Abort::message_type::SharedPtr) { publishScenarioInfoToJournal(); // TODO: This should be moved to a state that occurs right after a test is finished sm.process_event(state_machine::events::Abort{}); } -void ObjectControl::onAllClearMessage(const AllClear::message_type::SharedPtr){ +void ObjectControl::onAllClearMessage(const AllClear::message_type::SharedPtr) { sm.process_event(state_machine::events::Clear{}); } -void ObjectControl::onRemoteControlEnableMessage(const RemoteControlEnable::message_type::SharedPtr){ +void ObjectControl::onRemoteControlEnableMessage(const RemoteControlEnable::message_type::SharedPtr) { sm.process_event(state_machine::events::RemoteControl{}); } -void ObjectControl::onRemoteControlDisableMessage(const RemoteControlDisable::message_type::SharedPtr){ +void ObjectControl::onRemoteControlDisableMessage(const RemoteControlDisable::message_type::SharedPtr) { sm.process_event(state_machine::events::Ready{}); } -void ObjectControl::onControlSignalMessage(const ControlSignal::message_type::SharedPtr csp){ - try{ +void ObjectControl::onControlSignalMessage(const ControlSignal::message_type::SharedPtr csp) { + try { objects.at(csp->atos_header.object_id)->sendControlSignal(csp); - } - catch(const std::exception& e){ + } catch (const std::exception& e) { RCLCPP_ERROR(get_logger(), "Failed to translate/send Control Signal Percentage: %s", e.what()); } } -void ObjectControl::onPathMessage(const Path::message_type::SharedPtr trajlet,uint32_t id){ +void ObjectControl::onPathMessage(const Path::message_type::SharedPtr trajlet, uint32_t id) { objects.at(id)->setLastReceivedPath(trajlet); } @@ -204,20 +205,21 @@ bool ObjectControl::loadScenario() { objects.at(id)->setTransmitterID(id); std::promise trajLoaded; - auto trajectoryCallback = [&](const rclcpp::Client::SharedFuture future) { - if (!future.get()->success) { - RCLCPP_ERROR(get_logger(), "Get trajectory service call failed for object ID %u", id); - trajLoaded.set_value(false); - return; - } - ATOS::Trajectory traj(get_logger()); - traj.initializeFromCartesianTrajectory(future.get()->trajectory); - objects.at(id)->setTrajectory(traj); - trajLoaded.set_value(true); - RCLCPP_INFO(get_logger(), "Loaded trajectory for object ID %u with %lu points", id, traj.size()); - }; + auto trajectoryCallback = + [&](const rclcpp::Client::SharedFuture future) { + if (!future.get()->success) { + RCLCPP_ERROR(get_logger(), "Get trajectory service call failed for object ID %u", id); + trajLoaded.set_value(false); + return; + } + ATOS::Trajectory traj(get_logger()); + traj.initializeFromCartesianTrajectory(future.get()->trajectory); + objects.at(id)->setTrajectory(traj); + trajLoaded.set_value(true); + RCLCPP_INFO(get_logger(), "Loaded trajectory for object ID %u with %lu points", id, traj.size()); + }; auto trajRequest = std::make_shared(); - trajRequest->id = id; + trajRequest->id = id; trajectoryClient->async_send_request(trajRequest, trajectoryCallback); std::promise ipLoaded; @@ -229,13 +231,14 @@ bool ObjectControl::loadScenario() { } // Resolve the hostname or numerical IP address to an in_addr_t value addrinfo* result; - addrinfo hints = {}; - hints.ai_family = AF_INET; // Use AF_INET6 for IPv6 + addrinfo hints = {}; + hints.ai_family = AF_INET; // Use AF_INET6 for IPv6 hints.ai_socktype = SOCK_STREAM; int status = getaddrinfo(future.get()->ip.c_str(), nullptr, &hints, &result); if (status != 0) { - RCLCPP_ERROR(get_logger(), "Failed to resolve address for object ID %u: %s", id, gai_strerror(status)); + RCLCPP_ERROR( + get_logger(), "Failed to resolve address for object ID %u: %s", id, gai_strerror(status)); ipLoaded.set_value(false); return; } @@ -253,10 +256,9 @@ bool ObjectControl::loadScenario() { }; auto ipRequest = std::make_shared(); - ipRequest->id = id; + ipRequest->id = id; ipClient->async_send_request(ipRequest, ipCallback); - // Get test origin std::promise originLoaded; auto originCallback = [&](const rclcpp::Client::SharedFuture future) { @@ -266,33 +268,39 @@ bool ObjectControl::loadScenario() { return; } auto origin = future.get(); - objects.at(id)->setOrigin({ - origin->origin.position.latitude, - origin->origin.position.longitude, - origin->origin.position.altitude, - true,true,true}); - RCLCPP_INFO(get_logger(), "Got origin for object ID %u: (%.6f, %.6f, %.3f)", id, - origin->origin.position.latitude, - origin->origin.position.longitude, - origin->origin.position.altitude); - originLoaded.set_value(true); + objects.at(id)->setOrigin({origin->origin.position.latitude, + origin->origin.position.longitude, + origin->origin.position.altitude, + true, + true, + true}); + RCLCPP_INFO(get_logger(), + "Got origin for object ID %u: (%.6f, %.6f, %.3f)", + id, + origin->origin.position.latitude, + origin->origin.position.longitude, + origin->origin.position.altitude); + originLoaded.set_value(true); }; auto requestOrigin = std::make_shared(); originClient->async_send_request(requestOrigin, originCallback); // Wait for all services to finish - std::future trajLoadedFuture = trajLoaded.get_future(); - std::future ipLoadedFuture = ipLoaded.get_future(); + std::future trajLoadedFuture = trajLoaded.get_future(); + std::future ipLoadedFuture = ipLoaded.get_future(); std::future originLoadedFuture = originLoaded.get_future(); - if (auto status = trajLoadedFuture.wait_for(5s); status != std::future_status::ready || !trajLoadedFuture.get()) { + if (auto status = trajLoadedFuture.wait_for(5s); + status != std::future_status::ready || !trajLoadedFuture.get()) { RCLCPP_ERROR(get_logger(), "Trajectory loading failed for object ID %u", id); successful = false; } - if (auto status = ipLoadedFuture.wait_for(250ms); status != std::future_status::ready || !ipLoadedFuture.get()) { + if (auto status = ipLoadedFuture.wait_for(250ms); + status != std::future_status::ready || !ipLoadedFuture.get()) { RCLCPP_ERROR(get_logger(), "IP loading failed for object ID %u", id); successful = false; } - if (auto status = originLoadedFuture.wait_for(250ms); status != std::future_status::ready || !originLoadedFuture.get()) { + if (auto status = originLoadedFuture.wait_for(250ms); + status != std::future_status::ready || !originLoadedFuture.get()) { RCLCPP_ERROR(get_logger(), "Origin loading failed for object ID %u", id); successful = false; } @@ -303,18 +311,17 @@ bool ObjectControl::loadScenario() { originClient->prune_pending_requests(); return; } - } scenarioLoaded.set_value(successful); }; auto request = std::make_shared(); - auto future = idClient->async_send_request(request, idsCallback); + auto future = idClient->async_send_request(request, idsCallback); // Wait for all objects to load std::future scenarioLoadedFuture = scenarioLoaded.get_future(); if (auto status = scenarioLoadedFuture.wait_for(10s); status == std::future_status::ready) { - return scenarioLoadedFuture.get(); // Get the status value + return scenarioLoadedFuture.get(); // Get the status value } else if (status == std::future_status::timeout) { RCLCPP_ERROR(get_logger(), "Scenario loading timed out"); idClient->prune_pending_requests(); @@ -332,7 +339,7 @@ void ObjectControl::loadObjectFiles() { char path[MAX_FILE_PATH]; std::vector errors; - UtilGetObjectDirectoryPath(path, sizeof (path)); + UtilGetObjectDirectoryPath(path, sizeof(path)); fs::path objectDir(path); if (!fs::exists(objectDir)) { throw std::ios_base::failure("Object directory does not exist"); @@ -352,16 +359,14 @@ void ObjectControl::loadObjectFiles() { std::shared_ptr object = std::make_shared(id); object->parseConfigurationFile(inputFile); objects.emplace(id, object); - } - else { - auto badID = conf.getTransmitterID(); - std::string errMsg = "Duplicate object ID " + std::to_string(badID) - + " detected in files " + objects.at(badID)->getTrajectoryFileName() - + " and " + conf.getTrajectoryFileName(); + } else { + auto badID = conf.getTransmitterID(); + std::string errMsg = "Duplicate object ID " + std::to_string(badID) + " detected in files " + + objects.at(badID)->getTrajectoryFileName() + " and " + + conf.getTrajectoryFileName(); throw std::invalid_argument(errMsg); } - } - catch (std::invalid_argument& e) { + } catch (std::invalid_argument& e) { RCLCPP_ERROR(get_logger(), e.what()); errors.push_back(e); } @@ -381,9 +386,7 @@ void ObjectControl::loadObjectFiles() { if (!errors.empty()) { objects.clear(); std::ostringstream ostr; - auto append = [&ostr](const std::invalid_argument& e){ - ostr << e.what() << std::endl; - }; + auto append = [&ostr](const std::invalid_argument& e) { ostr << e.what() << std::endl; }; std::for_each(errors.begin(), errors.end(), append); throw std::invalid_argument("Failed to parse object file(s):\n" + ostr.str()); } @@ -403,7 +406,7 @@ ObjectMonitorType ObjectControl::getLastAnchorData() const { return objects.at(anchorID)->getLastMonitorData(); } -std::map ObjectControl::getObjectStates() const { +std::map ObjectControl::getObjectStates() const { std::map retval; for (const auto& elem : objects) { retval[elem.first] = elem.second->getState(); @@ -411,14 +414,13 @@ std::map ObjectControl::getObjectStates() const { return retval; } -void ObjectControl::transformScenarioRelativeTo( - const uint32_t objectID) { +void ObjectControl::transformScenarioRelativeTo(const uint32_t objectID) { for (auto& id : getVehicleIDs()) { if (id == objectID) { // Skip for now TODO also here - maybe? continue; } - auto traj = objects.at(id)->getTrajectory(); + auto traj = objects.at(id)->getTrajectory(); auto relTraj = traj.relativeTo(objects.at(objectID)->getTrajectory()); objects.at(id)->setTrajectory(relTraj); @@ -430,16 +432,14 @@ void ObjectControl::clearScenario() { storedActions.clear(); } - void ObjectControl::beginConnectionAttempt() { connStopReqPromise = std::promise(); - connStopReqFuture = connStopReqPromise.get_future(); + connStopReqFuture = connStopReqPromise.get_future(); RCLCPP_DEBUG(get_logger(), "Initiating connection attempt"); for (const auto id : getVehicleIDs()) { - auto t = std::thread(&ObjectControl::connectToObject, this, - std::ref(objects.at(id)), - std::ref(connStopReqFuture)); + auto t = + std::thread(&ObjectControl::connectToObject, this, std::ref(objects.at(id)), std::ref(connStopReqFuture)); t.detach(); } } @@ -447,8 +447,7 @@ void ObjectControl::beginConnectionAttempt() { void ObjectControl::abortConnectionAttempt() { try { connStopReqPromise.set_value(); - } - catch (std::future_error&) { + } catch (std::future_error&) { // Attempted to stop when none in progress } } @@ -457,8 +456,7 @@ void ObjectControl::disconnectObjects() { abortConnectionAttempt(); try { stopHeartbeatSignal.set_value(); - } - catch (std::future_error&) { + } catch (std::future_error&) { // Attempted to stop when none in progress } objectListeners.clear(); @@ -467,8 +465,7 @@ void ObjectControl::disconnectObjects() { } } -void ObjectControl::disconnectObject( - const uint32_t id) { +void ObjectControl::disconnectObject(const uint32_t id) { objects.at(id)->disconnect(); objectListeners.erase(id); } @@ -479,8 +476,7 @@ void ObjectControl::uploadAllConfigurations() { } } -void ObjectControl::uploadObjectConfiguration( - const uint32_t id) { +void ObjectControl::uploadObjectConfiguration(const uint32_t id) { objects.at(id)->sendSettings(); } @@ -493,7 +489,7 @@ void ObjectControl::startSafetyThread() { } void ObjectControl::heartbeat() { - auto stopRequest = stopHeartbeatSignal.get_future(); + auto stopRequest = stopHeartbeatSignal.get_future(); clock::time_point nextHeartbeat = clock::now(); RCLCPP_DEBUG(get_logger(), "Starting heartbeat thread"); @@ -505,8 +501,11 @@ void ObjectControl::heartbeat() { if (objects.at(id)->isConnected()) { auto diff = objects.at(id)->getTimeSinceLastMonitor(); if (diff > objects.at(id)->getMaxAllowedMonitorPeriod()) { - RCLCPP_WARN(get_logger(), "MONR timeout for object %u: %ld ms > %ld ms", id, - diff.count(), objects.at(id)->getMaxAllowedMonitorPeriod().count()); + RCLCPP_WARN(get_logger(), + "MONR timeout for object %u: %ld ms > %ld ms", + id, + diff.count(), + objects.at(id)->getMaxAllowedMonitorPeriod().count()); objects.at(id)->disconnect(); sm.process_event(state_machine::events::DisconnectedFromObject{id}); } @@ -519,8 +518,7 @@ void ObjectControl::heartbeat() { if (objects.at(id)->isConnected()) { objects.at(id)->sendHeartbeat(controlCenterStatus()); } - } - catch (std::exception& e) { + } catch (std::exception& e) { RCLCPP_WARN(get_logger(), e.what()); objects.at(id)->disconnect(); sm.process_event(state_machine::events::DisconnectedFromObject{id}); @@ -530,33 +528,32 @@ void ObjectControl::heartbeat() { RCLCPP_INFO(get_logger(), "Heartbeat thread exiting"); } -OsiHandler::LocalObjectGroundTruth_t ObjectControl::buildOSILocalGroundTruth( - const MonitorMessage& monr) const { +OsiHandler::LocalObjectGroundTruth_t ObjectControl::buildOSILocalGroundTruth(const MonitorMessage& monr) const { 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.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.orientation_rad.roll = 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.orientation_rad.roll = 0.0; gt.orientation_rad.pitch = 0.0; return gt; } // TODO (NOT WORKING ATM!): Replace this with a subscriber that listens for monr messages. -void ObjectControl::injectObjectData(const MonitorMessage &monr) { +void ObjectControl::injectObjectData(const MonitorMessage& monr) { if (!objects.at(monr.first)->getObjectConfig().getInjectionMap().targetIDs.empty()) { std::chrono::system_clock::time_point ts; - auto secs = std::chrono::seconds(monr.second.timestamp.tv_sec); + auto secs = std::chrono::seconds(monr.second.timestamp.tv_sec); auto usecs = std::chrono::microseconds(monr.second.timestamp.tv_usec); ts += secs + usecs; auto osiGtData = buildOSILocalGroundTruth(monr); @@ -568,7 +565,6 @@ void ObjectControl::injectObjectData(const MonitorMessage &monr) { } } - void ObjectControl::startListeners() { RCLCPP_DEBUG(get_logger(), "Starting listeners"); objectListeners.clear(); @@ -587,31 +583,28 @@ void ObjectControl::notifyObjectsConnected() { objectsConnectedPub.publish(ROSChannels::ObjectsConnected::message_type()); } -void ObjectControl::connectToObject( - std::shared_ptr obj, - std::shared_future &connStopReq) { - const int maxConnHeabs = this->get_parameter("max_missing_heartbeats").as_int(); +void ObjectControl::connectToObject(std::shared_ptr obj, std::shared_future& connStopReq) { + const int maxConnHeabs = this->get_parameter("max_missing_heartbeats").as_int(); constexpr int maxConnMonrs = 100; try { if (!obj->isConnected()) { try { obj->establishConnection(connStopReq); obj->sendSettings(); - } - catch (std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), "Connection attempt for object %u failed: %s", - obj->getTransmitterID(), e.what()); + } catch (std::runtime_error& e) { + RCLCPP_ERROR( + get_logger(), "Connection attempt for object %u failed: %s", obj->getTransmitterID(), e.what()); obj->disconnect(); return; // TODO connection failed event? } try { - int initializingMonrs = maxConnMonrs; + int initializingMonrs = maxConnMonrs; int connectionHeartbeats = maxConnHeabs; while (true) { ObjectStateType objState = OBJECT_STATE_UNKNOWN; - auto nextSendTime = std::chrono::system_clock::now(); + auto nextSendTime = std::chrono::system_clock::now(); try { obj->sendHeartbeat(controlCenterStatus()); nextSendTime += heartbeatPeriod; @@ -620,66 +613,65 @@ void ObjectControl::connectToObject( if (connectionHeartbeats-- > 0) { std::this_thread::sleep_until(nextSendTime); continue; - } - else { - throw std::runtime_error("No monitor reply after " + std::to_string(maxConnHeabs) + " heartbeats. Details:\n" + e.what()); + } else { + throw std::runtime_error("No monitor reply after " + std::to_string(maxConnHeabs) + + " heartbeats. Details:\n" + e.what()); } } switch (objState) { - case OBJECT_STATE_ARMED: - case OBJECT_STATE_REMOTE_CONTROL: - RCLCPP_INFO(get_logger(), "Connected to armed object ID %u", obj->getTransmitterID()); - sm.process_event(state_machine::events::Disarm{}); - break; - case OBJECT_STATE_ABORTING: - case OBJECT_STATE_POSTRUN: - case OBJECT_STATE_RUNNING: - RCLCPP_INFO(get_logger(), "Connected to running object ID %u", obj->getTransmitterID()); - // Object became armed while clearing abort, disarm all objects - // RCLCPP_WARN(get_logger(), "Connected to live object %d while CC in clearing state!", id); - sm.process_event(state_machine::events::Abort{}); - break; - case OBJECT_STATE_INIT: - if (initializingMonrs-- > 0) { - continue; - } - else { - RCLCPP_INFO(get_logger(), "Connected object %u in initializing state after connection", obj->getTransmitterID()); - sm.process_event(state_machine::events::Abort{}); - } - break; - case OBJECT_STATE_DISARMED: - RCLCPP_INFO(get_logger(), "Connected to disarmed object ID %u", obj->getTransmitterID()); - if (areAllObjectsIn(OBJECT_STATE_DISARMED)){ - startListeners(); - notifyObjectsConnected(); - sm.process_event(state_machine::events::Ready{}); - } - else if (isAnyObjectIn(OBJECT_STATE_ARMED)) { + case OBJECT_STATE_ARMED: + case OBJECT_STATE_REMOTE_CONTROL: + RCLCPP_INFO(get_logger(), "Connected to armed object ID %u", obj->getTransmitterID()); sm.process_event(state_machine::events::Disarm{}); - } - else if (isAnyObjectIn(OBJECT_STATE_RUNNING)) { + break; + case OBJECT_STATE_ABORTING: + case OBJECT_STATE_POSTRUN: + case OBJECT_STATE_RUNNING: + RCLCPP_INFO(get_logger(), "Connected to running object ID %u", obj->getTransmitterID()); + // Object became armed while clearing abort, disarm all objects + // RCLCPP_WARN(get_logger(), "Connected to live object %d while CC in clearing state!", id); + sm.process_event(state_machine::events::Abort{}); + break; + case OBJECT_STATE_INIT: + if (initializingMonrs-- > 0) { + continue; + } else { + RCLCPP_INFO(get_logger(), + "Connected object %u in initializing state after connection", + obj->getTransmitterID()); + sm.process_event(state_machine::events::Abort{}); + } + break; + case OBJECT_STATE_DISARMED: + RCLCPP_INFO(get_logger(), "Connected to disarmed object ID %u", obj->getTransmitterID()); + if (areAllObjectsIn(OBJECT_STATE_DISARMED)) { + startListeners(); + notifyObjectsConnected(); + sm.process_event(state_machine::events::Ready{}); + } else if (isAnyObjectIn(OBJECT_STATE_ARMED)) { + sm.process_event(state_machine::events::Disarm{}); + } else if (isAnyObjectIn(OBJECT_STATE_RUNNING)) { + sm.process_event(state_machine::events::Abort{}); + } + break; + default: + RCLCPP_INFO( + get_logger(), "Connected to object %u in unknown state", obj->getTransmitterID()); sm.process_event(state_machine::events::Abort{}); - } - break; - default: - RCLCPP_INFO(get_logger(), "Connected to object %u in unknown state", obj->getTransmitterID()); - sm.process_event(state_machine::events::Abort{}); } break; } - } - catch (std::runtime_error &e) { - RCLCPP_ERROR(get_logger(), "Connection startup procedure failed for object %u: %s", - obj->getTransmitterID(), e.what()); + } catch (std::runtime_error& e) { + RCLCPP_ERROR(get_logger(), + "Connection startup procedure failed for object %u: %s", + obj->getTransmitterID(), + e.what()); obj->disconnect(); // TODO: connection failed event? } } - } - catch (std::invalid_argument &e) { - RCLCPP_ERROR(get_logger(), "Bad connection attempt for object %u: %s", - obj->getTransmitterID(), e.what()); + } catch (std::invalid_argument& e) { + RCLCPP_ERROR(get_logger(), "Bad connection attempt for object %u: %s", obj->getTransmitterID(), e.what()); obj->disconnect(); // TODO: connection failed event? } @@ -701,11 +693,9 @@ void ObjectControl::disarmObjects() { for (auto& id : getVehicleIDs()) { try { objects.at(id)->sendDisarm(); - } - catch (std::invalid_argument& e) { + } catch (std::invalid_argument& e) { RCLCPP_ERROR(get_logger(), "Unable to disarm object %u: %s", id, e.what()); - } - catch (std::runtime_error& e) { + } catch (std::runtime_error& e) { RCLCPP_ERROR(get_logger(), "Unable to disarm object %u: %s", id, e.what()); objects.at(id)->disconnect(); } @@ -716,7 +706,7 @@ void ObjectControl::disarmObjects() { * @brief Resets the scenario by offering return trajectories for all objects * * @param isResetting state-flag to indicate if the reset procedure is currently running -*/ + */ void ObjectControl::resetTestObjects() { // Check if we are already resetting if (this->isResetting) { @@ -754,8 +744,8 @@ void ObjectControl::reloadScenarioTrajectories() { * @brief Republishes the trajectory paths for a given object * * @param id the object id -*/ -void ObjectControl::republishTrajectoryPaths(uint32_t id){ + */ +void ObjectControl::republishTrajectoryPaths(uint32_t id) { // Update the GUI with the new trajectory in local coordinates ATOS::Trajectory traj = objects.at(id)->getTrajectory(); this->pathPublishers.emplace(id, ROSChannels::Path::Pub(*this, id)); @@ -763,19 +753,21 @@ void ObjectControl::republishTrajectoryPaths(uint32_t id){ // Update the GUI with the new trajectory in global coordinates GeographicPositionType origin = objects.at(id)->getOrigin(); - std::array llh_0 = {origin.latitude_deg, origin.longitude_deg, origin.altitude_m}; + std::array llh_0 = {origin.latitude_deg, origin.longitude_deg, origin.altitude_m}; this->gnssPathPublishers.emplace(id, ROSChannels::GNSSPath::Pub(*this, id)); this->gnssPathPublishers.at(id).publish(traj.toGeoJSON(llh_0)); } /** - * @brief Callback function for the trajectory service call. Sets the new trajectory for the object in ATOS and sends it to the object + * @brief Callback function for the trajectory service call. Sets the new trajectory for the object in ATOS and sends it + * to the object * * @param future the future object containing the response from the trajectory service call -*/ -void ObjectControl::trajectoryCallback(const rclcpp::Client::SharedFuture future) { + */ +void ObjectControl::trajectoryCallback( + const rclcpp::Client::SharedFuture future) { this->trajResponse = future.get(); - auto id = returnTrajResponse->id; + auto id = returnTrajResponse->id; // Check if the return trajectory service call was successful if (!trajResponse->success) { RCLCPP_ERROR(get_logger(), "Get trajectory service call failed for object %u", id); @@ -793,13 +785,15 @@ void ObjectControl::trajectoryCallback(const rclcpp::Client::SharedFuture future) { + */ +void ObjectControl::returnTrajectoryCallback( + const rclcpp::Client::SharedFuture future) { this->returnTrajResponse = future.get(); - auto id = returnTrajResponse->id; + auto id = returnTrajResponse->id; // Check if the return trajectory service call was successful if (!returnTrajResponse->success) { RCLCPP_ERROR(get_logger(), "Get return trajectory service call failed for object %u", id); @@ -817,60 +811,58 @@ void ObjectControl::returnTrajectoryCallback(const rclcpp::ClientisResetting) { using namespace std::chrono_literals; - if (!this->returnTrajectoryClient->wait_for_service(1s)) // TODO: This is blocking any other callbacks for 1s if the service is not available. Consider new thread. + if (!this->returnTrajectoryClient->wait_for_service(1s)) // TODO: This is blocking any other callbacks for 1s if + // the service is not available. Consider new thread. { RCLCPP_ERROR(get_logger(), "The return trajectory service is not available. Try again later."); return; } // Get the current trajectory and create a request for the return trajectory service auto returnTrajectoryRequest = std::make_shared(); - returnTrajectoryRequest->id = id; + returnTrajectoryRequest->id = id; returnTrajectoryRequest->trajectory = objects.at(id)->getTrajectory().toCartesianTrajectory(); - auto pos = objects.at(id)->getLastMonitorData().position; + auto pos = objects.at(id)->getLastMonitorData().position; returnTrajectoryRequest->position.x = pos.xCoord_m; returnTrajectoryRequest->position.y = pos.yCoord_m; returnTrajectoryRequest->position.z = pos.zCoord_m; - // Send the request to the return trajectory service asynchronously and bind the callback function for when the response is received - returnTrajectoryClient->async_send_request(returnTrajectoryRequest, std::bind(&ObjectControl::returnTrajectoryCallback, this, _1)); - } - else { + // Send the request to the return trajectory service asynchronously and bind the callback function for when the + // response is received + returnTrajectoryClient->async_send_request(returnTrajectoryRequest, + std::bind(&ObjectControl::returnTrajectoryCallback, this, _1)); + } else { // Request the scenario trajectory from the esminiAdapter auto trajRequest = std::make_shared(); - trajRequest->id = id; + trajRequest->id = id; trajectoryClient->async_send_request(trajRequest, std::bind(&ObjectControl::trajectoryCallback, this, _1)); } } -void ObjectControl::startObject( - uint32_t id, - std::chrono::system_clock::time_point startTime) { +void ObjectControl::startObject(uint32_t id, std::chrono::system_clock::time_point startTime) { try { - objects.at(id)->sendStart(startTime); - } - catch (std::out_of_range& e) { + objects.at(id)->sendStart(startTime); + } catch (std::out_of_range& e) { std::stringstream ss; for (auto& id : getVehicleIDs()) { ss << " " << id << ","; } auto str = ss.str(); str.pop_back(); - RCLCPP_WARN(get_logger(), "Attempted to start nonexistent object %u - configured objects are%s", id, str.c_str()); + RCLCPP_WARN( + get_logger(), "Attempted to start nonexistent object %u - configured objects are%s", id, str.c_str()); } } - auto readyOrDisconnected = [](std::shared_ptr obj) { - return obj->getState() == OBJECT_STATE_DISARMED || - obj->getState() == OBJECT_STATE_ARMED || - !obj->isConnected(); + return obj->getState() == OBJECT_STATE_DISARMED || obj->getState() == OBJECT_STATE_ARMED || !obj->isConnected(); }; auto disarmedOrDisconnected = [](const std::shared_ptr obj) { @@ -879,44 +871,39 @@ auto disarmedOrDisconnected = [](const std::shared_ptr obj) { 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; + ObjectStateType state = static_cast(stateChangeMsg->state.state); + uint32_t id = stateChangeMsg->id; switch (state) { - case OBJECT_STATE_DISARMED: - if (prevState == OBJECT_STATE_ABORTING) { - if (areAllObjects(disarmedOrDisconnected)) { + case OBJECT_STATE_DISARMED: + if (prevState == OBJECT_STATE_ABORTING) { + if (areAllObjects(disarmedOrDisconnected)) { + sm.process_event(state_machine::events::Ready{}); + } else if (areAllObjects(readyOrDisconnected)) { + sm.process_event(state_machine::events::Connect{}); + } + } else if (areAllObjectsIn(OBJECT_STATE_DISARMED)) { + sm.process_event(state_machine::events::Done{}); sm.process_event(state_machine::events::Ready{}); } - else if (areAllObjects(readyOrDisconnected)) { - sm.process_event(state_machine::events::Connect{}); + break; + case OBJECT_STATE_ARMED: + if (sm.is(state_machine::Clearing)) { + sm.process_event(state_machine::events::Disarm{}); + } else if (sm.is(state_machine::Aborting)) { + disconnectObject(id); // TODO just stop sending HEAB to keep tracking available } - } - else if (areAllObjectsIn(OBJECT_STATE_DISARMED)) { - sm.process_event(state_machine::events::Done{}); - sm.process_event(state_machine::events::Ready{}); - } - break; - case OBJECT_STATE_ARMED: - if (sm.is(state_machine::Clearing)) - { - sm.process_event(state_machine::events::Disarm{}); - } - else if (sm.is(state_machine::Aborting)) - { - disconnectObject(id); // TODO just stop sending HEAB to keep tracking available - } - break; - case OBJECT_STATE_ABORTING: - sm.process_event(state_machine::events::Abort{}); - 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); + break; + case OBJECT_STATE_ABORTING: + sm.process_event(state_machine::events::Abort{}); + 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); } } @@ -930,53 +917,56 @@ void ObjectControl::allClearObjects() { } bool ObjectControl::areAllObjects(std::function)> predicate) const { - return std::all_of(objects.cbegin(), objects.cend(), [predicate](const std::pair>& obj) { - return predicate(obj.second); - }); + return std::all_of( + objects.cbegin(), objects.cend(), [predicate](const std::pair>& obj) { + return predicate(obj.second); + }); } bool ObjectControl::isAnyObject(std::function)> predicate) const { - return std::any_of(objects.cbegin(), objects.cend(), [predicate](const std::pair>& obj) { - return predicate(obj.second); - }); + return std::any_of( + objects.cbegin(), objects.cend(), [predicate](const std::pair>& obj) { + return predicate(obj.second); + }); } -bool ObjectControl::isAnyObjectIn( - const ObjectStateType state) { - return std::any_of(objects.cbegin(), objects.cend(), [state](const std::pair>& obj) { - return obj.second->getState() == state; - }); +bool ObjectControl::isAnyObjectIn(const ObjectStateType state) { + return std::any_of( + objects.cbegin(), objects.cend(), [state](const std::pair>& obj) { + return obj.second->getState() == state; + }); } -bool ObjectControl::areAllObjectsIn( - const ObjectStateType state) { - return std::all_of(objects.cbegin(), objects.cend(), [state](const std::pair>& obj) { - return obj.second->getState() == state; - }); +bool ObjectControl::areAllObjectsIn(const ObjectStateType state) { + return std::all_of( + objects.cbegin(), objects.cend(), [state](const std::pair>& obj) { + return obj.second->getState() == state; + }); } -bool ObjectControl::isAnyObjectIn( - const std::set& states) { - return std::any_of(objects.cbegin(), objects.cend(), [states](const std::pair>& obj) { - return states.find(obj.second->getState()) != states.end(); - }); +bool ObjectControl::isAnyObjectIn(const std::set& states) { + return std::any_of( + objects.cbegin(), objects.cend(), [states](const std::pair>& obj) { + return states.find(obj.second->getState()) != states.end(); + }); } -bool ObjectControl::areAllObjectsIn( - const std::set& states) { - return std::all_of(objects.cbegin(), objects.cend(), [states](const std::pair>& obj) { - return states.find(obj.second->getState()) != states.end(); - }); +bool ObjectControl::areAllObjectsIn(const std::set& states) { + return std::all_of( + objects.cbegin(), objects.cend(), [states](const std::pair>& obj) { + return states.find(obj.second->getState()) != states.end(); + }); } -void ObjectControl::startControlSignalSubscriber(){ - controlSignalSub = std::make_shared(*this, std::bind(&ObjectControl::onControlSignalMessage, this, _1)); +void ObjectControl::startControlSignalSubscriber() { + controlSignalSub = + std::make_shared(*this, std::bind(&ObjectControl::onControlSignalMessage, this, _1)); } -void ObjectControl::stopControlSignalSubscriber(){ +void ObjectControl::stopControlSignalSubscriber() { this->controlSignalSub.reset(); } -void ObjectControl::sendAbortNotification(){ +void ObjectControl::sendAbortNotification() { this->scnAbortPub.publish(ROSChannels::Abort::message_type()); } @@ -990,7 +980,7 @@ void ObjectControl::publishScenarioInfoToJournal() { int index = 1; for (const auto& object : objects) { auto testObject = object.second; - auto traj = testObject->getTrajectory(); + auto traj = testObject->getTrajectory(); // Convert binary IP to string for logging auto ip = testObject->getAsObjectData().ClientIP; @@ -998,17 +988,17 @@ void ObjectControl::publishScenarioInfoToJournal() { inet_ntop(AF_INET, &ip, ip_str, INET_ADDRSTRLEN); ss << "\n> Object " << index << ": \n" - << "\t - ID: " << object.first << "\n" - << "\t - IP: " << ip_str << "\n" - << "\t - Origin: (" << testObject->getOrigin().latitude_deg << ", " << testObject->getOrigin().longitude_deg << ", " << testObject->getOrigin().altitude_m << ")\n" - << "\t - Trajectory size: " << testObject->getTrajectory().size() << "\n" - << "\t - Trajectory: \n"; + << "\t - ID: " << object.first << "\n" + << "\t - IP: " << ip_str << "\n" + << "\t - Origin: (" << testObject->getOrigin().latitude_deg << ", " << testObject->getOrigin().longitude_deg + << ", " << testObject->getOrigin().altitude_m << ")\n" + << "\t - Trajectory size: " << testObject->getTrajectory().size() << "\n" + << "\t - Trajectory: \n"; for (const auto& point : traj.points) { ss << "\t\t" << point.toString() << "\n"; } ++index; - } JournalRecordData(JOURNAL_RECORD_STRING, ss.str().c_str()); @@ -1024,35 +1014,25 @@ void ObjectControl::publishScenarioInfoToJournal() { OBCState_t ObjectControl::stateAsNumber() { if (sm.is(state_machine::Idle)) { return OBC_STATE_IDLE; - } - else if (sm.is(state_machine::Initialized)) { + } else if (sm.is(state_machine::Initialized)) { return OBC_STATE_INITIALIZED; - } - else if (sm.is(state_machine::Connecting)) { + } else if (sm.is(state_machine::Connecting)) { return OBC_STATE_CONNECTED; - } - else if (sm.is(state_machine::Ready)) { + } else if (sm.is(state_machine::Ready)) { return OBC_STATE_CONNECTED; - } - else if (sm.is(state_machine::Armed)) { + } else if (sm.is(state_machine::Armed)) { return OBC_STATE_ARMED; - } - else if (sm.is(state_machine::Disarming)) { + } else if (sm.is(state_machine::Disarming)) { return OBC_STATE_DISARMING; - } - else if (sm.is(state_machine::TestLive)) { + } else if (sm.is(state_machine::TestLive)) { return OBC_STATE_RUNNING; - } - else if (sm.is(state_machine::RemoteControl)) { + } else if (sm.is(state_machine::RemoteControl)) { return OBC_STATE_REMOTECTRL; - } - else if (sm.is(state_machine::Aborting)) { + } else if (sm.is(state_machine::Aborting)) { return OBC_STATE_ABORTING; - } - else if (sm.is(state_machine::Clearing)) { + } else if (sm.is(state_machine::Clearing)) { return OBC_STATE_CLEARING; - } - else { + } else { return OBC_STATE_UNDEFINED; } } @@ -1060,38 +1040,27 @@ OBCState_t ObjectControl::stateAsNumber() { ControlCenterStatusType ObjectControl::controlCenterStatus() { if (sm.is(state_machine::Idle)) { return CONTROL_CENTER_STATUS_INIT; - } - else if (sm.is(state_machine::Initialized)) { + } else if (sm.is(state_machine::Initialized)) { return CONTROL_CENTER_STATUS_INIT; - } - else if (sm.is(state_machine::Connecting)) { + } else if (sm.is(state_machine::Connecting)) { return CONTROL_CENTER_STATUS_INIT; - } - else if (sm.is(state_machine::Ready)) { + } else if (sm.is(state_machine::Ready)) { return CONTROL_CENTER_STATUS_READY; - } - else if (sm.is(state_machine::Armed)) { + } else if (sm.is(state_machine::Armed)) { return CONTROL_CENTER_STATUS_RUNNING; // TODO - } - else if (sm.is(state_machine::Disarming)) { + } else if (sm.is(state_machine::Disarming)) { return CONTROL_CENTER_STATUS_RUNNING; // TODO - } - else if (sm.is(state_machine::TestLive)) { + } else if (sm.is(state_machine::TestLive)) { return CONTROL_CENTER_STATUS_RUNNING; - } - else if (sm.is(state_machine::RemoteControl)) { + } else if (sm.is(state_machine::RemoteControl)) { return CONTROL_CENTER_STATUS_READY; - } - else if (sm.is(state_machine::Aborting)) { + } else if (sm.is(state_machine::Aborting)) { return CONTROL_CENTER_STATUS_ABORT; - } - else if (sm.is(state_machine::Clearing)) { + } else if (sm.is(state_machine::Clearing)) { return CONTROL_CENTER_STATUS_READY; - } - else if (sm.is(state_machine::Done)){ + } else if (sm.is(state_machine::Done)) { return CONTROL_CENTER_STATUS_TEST_DONE; - } - else { + } else { return CONTROL_CENTER_STATUS_ABORT; } } @@ -1135,7 +1104,7 @@ void disarming_on_entry::operator()(ObjectControl* oc) const { // Done void done_on_entry::operator()(ObjectControl* oc) const { RCLCPP_WARN(oc->get_logger(), "Nothing to be done for postprocessing"); // TODO - oc->sendAbortNotification(); // TODO temporary to trigger logging etc. + oc->sendAbortNotification(); // TODO temporary to trigger logging etc. } // Idle @@ -1146,7 +1115,7 @@ void idle_on_entry::operator()(ObjectControl* oc) const { } bool idle_to_init_guard::operator()(ObjectControl* oc) const { - // Reload objects on each initialize request. + // Reload objects on each initialize request. bool const successful{oc->loadScenario()}; if (!successful) { RCLCPP_ERROR(oc->get_logger(), "Failed to load scenario"); @@ -1169,25 +1138,21 @@ void idle_to_init_action::operator()(ObjectControl* oc) const { // Initialized void init_to_connecting_action::operator()(ObjectControl* oc) const { - RCLCPP_INFO(oc->get_logger(), "Handling connect request"); - JournalRecordData(JOURNAL_RECORD_EVENT, "CONNECT received"); + RCLCPP_INFO(oc->get_logger(), "Handling connect request"); + JournalRecordData(JOURNAL_RECORD_EVENT, "CONNECT received"); } bool init_to_connecting_guard::operator()(ObjectControl* oc) const { - if (oc->getVehicleIDs().empty()) - { - RCLCPP_WARN(oc->get_logger(), - "No objects are configured! Canceling connect request..."); - return false; - } - else - { - return true; - } + if (oc->getVehicleIDs().empty()) { + RCLCPP_WARN(oc->get_logger(), "No objects are configured! Canceling connect request..."); + return false; + } else { + return true; + } } void init_to_idle_action::operator()(ObjectControl* oc) const { - oc->clearScenario(); + oc->clearScenario(); } // Ready @@ -1220,4 +1185,4 @@ void test_live_start_object::operator()(const events::StartObject& event, Object oc->startObject(event.id, event.startTime); } -} +} // namespace state_machine From 5700420449903f4552418825e1e75fbc8fbb0417 Mon Sep 17 00:00:00 2001 From: Mats Nilsson Date: Wed, 26 Feb 2025 10:31:09 +0100 Subject: [PATCH 2/2] Switch from pointer to reference for state machine Reference should always be preferred if possible. --- .../ObjectControl/inc/objectcontrol.hpp | 2 +- .../ObjectControl/inc/statemachine.hpp | 40 +++---- .../ObjectControl/src/objectcontrol.cpp | 106 +++++++++--------- 3 files changed, 74 insertions(+), 74 deletions(-) diff --git a/atos/modules/ObjectControl/inc/objectcontrol.hpp b/atos/modules/ObjectControl/inc/objectcontrol.hpp index d61858de4..72fe68f07 100644 --- a/atos/modules/ObjectControl/inc/objectcontrol.hpp +++ b/atos/modules/ObjectControl/inc/objectcontrol.hpp @@ -240,7 +240,7 @@ class ObjectControl : public Module { boost::sml::sm, boost::sml::thread_safe> - sm{this, sm_logger}; + sm{*this, sm_logger}; public: //! Connection methods diff --git a/atos/modules/ObjectControl/inc/statemachine.hpp b/atos/modules/ObjectControl/inc/statemachine.hpp index 8a17d5eae..ddce6c8d7 100644 --- a/atos/modules/ObjectControl/inc/statemachine.hpp +++ b/atos/modules/ObjectControl/inc/statemachine.hpp @@ -58,26 +58,26 @@ static constexpr auto Done = boost::sml::state; static constexpr auto RemoteControl = boost::sml::state; // clang-format off -struct idle_on_entry { void operator()(ObjectControl* oc) const; }; -struct idle_to_init_guard { bool operator()(ObjectControl* oc) const; }; -struct idle_to_init_action { void operator()(ObjectControl* oc) const; }; -struct init_to_connecting_action { void operator()(ObjectControl* oc) const; }; -struct init_to_connecting_guard { bool operator()(ObjectControl* oc) const; }; -struct init_to_idle_action { void operator()(ObjectControl* oc) const; }; -struct connecting_on_entry { void operator()(ObjectControl* oc) const; }; -struct connecting_to_idle_action { void operator()(ObjectControl* oc) const; }; -struct clearing_on_entry { void operator()(ObjectControl* oc) const; }; -struct ready_on_entry { void operator()(ObjectControl* oc) const; }; -struct ready_reset { void operator()(ObjectControl* oc) const; }; -struct ready_reload { void operator()(ObjectControl* oc) const; }; -struct remote_control_on_entry { void operator()(ObjectControl* oc) const; }; -struct remote_control_on_exit { void operator()(ObjectControl* oc) const; }; -struct disarming_on_entry { void operator()(ObjectControl* oc) const; }; -struct done_on_entry { void operator()(ObjectControl* oc) const; }; -struct armed_on_enter { void operator()(ObjectControl* oc) const; }; -struct armed_to_testlive_guard { bool operator()(ObjectControl* oc) const; }; -struct test_live_start_object { void operator()(const events::StartObject& event, ObjectControl* oc) const; }; -struct disconnected_from_object { void operator()(const events::DisconnectedFromObject& event, ObjectControl* oc) const; }; +struct idle_on_entry { void operator()(ObjectControl& oc) const; }; +struct idle_to_init_guard { bool operator()(ObjectControl& oc) const; }; +struct idle_to_init_action { void operator()(ObjectControl& oc) const; }; +struct init_to_connecting_action { void operator()(ObjectControl& oc) const; }; +struct init_to_connecting_guard { bool operator()(ObjectControl& oc) const; }; +struct init_to_idle_action { void operator()(ObjectControl& oc) const; }; +struct connecting_on_entry { void operator()(ObjectControl& oc) const; }; +struct connecting_to_idle_action { void operator()(ObjectControl& oc) const; }; +struct clearing_on_entry { void operator()(ObjectControl& oc) const; }; +struct ready_on_entry { void operator()(ObjectControl& oc) const; }; +struct ready_reset { void operator()(ObjectControl& oc) const; }; +struct ready_reload { void operator()(ObjectControl& oc) const; }; +struct remote_control_on_entry { void operator()(ObjectControl& oc) const; }; +struct remote_control_on_exit { void operator()(ObjectControl& oc) const; }; +struct disarming_on_entry { void operator()(ObjectControl& oc) const; }; +struct done_on_entry { void operator()(ObjectControl& oc) const; }; +struct armed_on_enter { void operator()(ObjectControl& oc) const; }; +struct armed_to_testlive_guard { bool operator()(ObjectControl& oc) const; }; +struct test_live_start_object { void operator()(const events::StartObject& event, ObjectControl& oc) const; }; +struct disconnected_from_object { void operator()(const events::DisconnectedFromObject& event, ObjectControl& oc) const; }; // clang-format on template diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp index a747776a2..31026e83f 100644 --- a/atos/modules/ObjectControl/src/objectcontrol.cpp +++ b/atos/modules/ObjectControl/src/objectcontrol.cpp @@ -1068,121 +1068,121 @@ ControlCenterStatusType ObjectControl::controlCenterStatus() { namespace state_machine { // Aborting -void disconnected_from_object::operator()(const events::DisconnectedFromObject& event, ObjectControl* oc) const { - RCLCPP_WARN(oc->get_logger(), "Object %d disconnected while CC in aborting state!", event.id); +void disconnected_from_object::operator()(const events::DisconnectedFromObject& event, ObjectControl& oc) const { + RCLCPP_WARN(oc.get_logger(), "Object %d disconnected while CC in aborting state!", event.id); } // Armed -void armed_on_enter::operator()(ObjectControl* oc) const { - oc->armObjects(); +void armed_on_enter::operator()(ObjectControl& oc) const { + oc.armObjects(); } -bool armed_to_testlive_guard::operator()(ObjectControl* oc) const { - return oc->areAllObjectsIn(OBJECT_STATE_ARMED); +bool armed_to_testlive_guard::operator()(ObjectControl& oc) const { + return oc.areAllObjectsIn(OBJECT_STATE_ARMED); } // Clearing -void clearing_on_entry::operator()(ObjectControl* oc) const { - oc->allClearObjects(); +void clearing_on_entry::operator()(ObjectControl& oc) const { + oc.allClearObjects(); } // Connecting -void connecting_on_entry::operator()(ObjectControl* oc) const { - oc->beginConnectionAttempt(); +void connecting_on_entry::operator()(ObjectControl& oc) const { + oc.beginConnectionAttempt(); } -void connecting_to_idle_action::operator()(ObjectControl* oc) const { - oc->abortConnectionAttempt(); - oc->disconnectObjects(); +void connecting_to_idle_action::operator()(ObjectControl& oc) const { + oc.abortConnectionAttempt(); + oc.disconnectObjects(); } // Disarming -void disarming_on_entry::operator()(ObjectControl* oc) const { - oc->disarmObjects(); +void disarming_on_entry::operator()(ObjectControl& oc) const { + oc.disarmObjects(); } // Done -void done_on_entry::operator()(ObjectControl* oc) const { - RCLCPP_WARN(oc->get_logger(), "Nothing to be done for postprocessing"); // TODO - oc->sendAbortNotification(); // TODO temporary to trigger logging etc. +void done_on_entry::operator()(ObjectControl& oc) const { + RCLCPP_WARN(oc.get_logger(), "Nothing to be done for postprocessing"); // TODO + oc.sendAbortNotification(); // TODO temporary to trigger logging etc. } // Idle -void idle_on_entry::operator()(ObjectControl* oc) const { - RCLCPP_INFO(oc->get_logger(), "Handling initialization request"); +void idle_on_entry::operator()(ObjectControl& oc) const { + RCLCPP_INFO(oc.get_logger(), "Handling initialization request"); JournalRecordData(JOURNAL_RECORD_EVENT, "INIT received"); - oc->clearScenario(); + oc.clearScenario(); } -bool idle_to_init_guard::operator()(ObjectControl* oc) const { +bool idle_to_init_guard::operator()(ObjectControl& oc) const { // Reload objects on each initialize request. - bool const successful{oc->loadScenario()}; + bool const successful{oc.loadScenario()}; if (!successful) { - RCLCPP_ERROR(oc->get_logger(), "Failed to load scenario"); + RCLCPP_ERROR(oc.get_logger(), "Failed to load scenario"); JournalRecordData(JOURNAL_RECORD_EVENT, "INIT failed"); } return successful; } -void idle_to_init_action::operator()(ObjectControl* oc) const { +void idle_to_init_action::operator()(ObjectControl& oc) const { try { - auto anchorID = oc->getAnchorObjectID(); - oc->transformScenarioRelativeTo(anchorID); - oc->setControlMode(ControlMode::RelativeKinematics); - RCLCPP_INFO(oc->get_logger(), "Relative control mode enabled"); + auto anchorID = oc.getAnchorObjectID(); + oc.transformScenarioRelativeTo(anchorID); + oc.setControlMode(ControlMode::RelativeKinematics); + RCLCPP_INFO(oc.get_logger(), "Relative control mode enabled"); } catch (std::invalid_argument&) { - oc->setControlMode(ControlMode::AbsoluteKinematics); - RCLCPP_INFO(oc->get_logger(), "Absolute control mode enabled"); + oc.setControlMode(ControlMode::AbsoluteKinematics); + RCLCPP_INFO(oc.get_logger(), "Absolute control mode enabled"); } } // Initialized -void init_to_connecting_action::operator()(ObjectControl* oc) const { - RCLCPP_INFO(oc->get_logger(), "Handling connect request"); +void init_to_connecting_action::operator()(ObjectControl& oc) const { + RCLCPP_INFO(oc.get_logger(), "Handling connect request"); JournalRecordData(JOURNAL_RECORD_EVENT, "CONNECT received"); } -bool init_to_connecting_guard::operator()(ObjectControl* oc) const { - if (oc->getVehicleIDs().empty()) { - RCLCPP_WARN(oc->get_logger(), "No objects are configured! Canceling connect request..."); +bool init_to_connecting_guard::operator()(ObjectControl& oc) const { + if (oc.getVehicleIDs().empty()) { + RCLCPP_WARN(oc.get_logger(), "No objects are configured! Canceling connect request..."); return false; } else { return true; } } -void init_to_idle_action::operator()(ObjectControl* oc) const { - oc->clearScenario(); +void init_to_idle_action::operator()(ObjectControl& oc) const { + oc.clearScenario(); } // Ready -void ready_on_entry::operator()(ObjectControl* oc) const { - oc->startSafetyThread(); +void ready_on_entry::operator()(ObjectControl& oc) const { + oc.startSafetyThread(); } -void ready_reset::operator()(ObjectControl* oc) const { - oc->resetTestObjects(); +void ready_reset::operator()(ObjectControl& oc) const { + oc.resetTestObjects(); } -void ready_reload::operator()(ObjectControl* oc) const { - oc->reloadScenarioTrajectories(); - oc->uploadAllConfigurations(); +void ready_reload::operator()(ObjectControl& oc) const { + oc.reloadScenarioTrajectories(); + oc.uploadAllConfigurations(); } // RemoteControl -void remote_control_on_entry::operator()(ObjectControl* oc) const { - oc->remoteControlObjects(true); - oc->startControlSignalSubscriber(); +void remote_control_on_entry::operator()(ObjectControl& oc) const { + oc.remoteControlObjects(true); + oc.startControlSignalSubscriber(); } -void remote_control_on_exit::operator()(ObjectControl* oc) const { - oc->stopControlSignalSubscriber(); - oc->remoteControlObjects(false); +void remote_control_on_exit::operator()(ObjectControl& oc) const { + oc.stopControlSignalSubscriber(); + oc.remoteControlObjects(false); } // TestLive -void test_live_start_object::operator()(const events::StartObject& event, ObjectControl* oc) const { - oc->startObject(event.id, event.startTime); +void test_live_start_object::operator()(const events::StartObject& event, ObjectControl& oc) const { + oc.startObject(event.id, event.startTime); } } // namespace state_machine