From 9a2a6d8bc48cd930db25fb687a7331db2db73052 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Tue, 26 Nov 2024 10:32:07 -0500 Subject: [PATCH] fixed formatting --- lie/lie.hpp | 38 +++--- state_machine/state.hpp | 10 +- state_machine/state_machine.hpp | 148 +++++++++++------------ state_machine/state_publisher_server.hpp | 78 ++++++------ 4 files changed, 136 insertions(+), 138 deletions(-) diff --git a/lie/lie.hpp b/lie/lie.hpp index d1013a10..d6511978 100644 --- a/lie/lie.hpp +++ b/lie/lie.hpp @@ -43,28 +43,28 @@ namespace mrover { [[nodiscard]] static auto toTransformStamped(SE3d const& tf, std::string const& childFrame, std::string const& parentFrame, rclcpp::Time const& time) -> geometry_msgs::msg::TransformStamped; /** - * \brief Pull the most recent transform or pose between two frames from the TF tree. - * The second and third parameters are named for the transform interpretation. - * Consider them named "a" and "b" respectively: - * For a transform this is a rotation and translation, i.e. aToB. - * For a pose this is a position and orientation, i.e. aInB. - * \param buffer ROS TF Buffer, make sure a listener is attached - * \param fromFrame From (transform) or child (pose) frame - * \param toFrame To (transform) or parent (pose) frame - * \param time Time to query the transform at, default is the latest - * \return The transform or pose represented by an SE3 lie group element - */ + * \brief Pull the most recent transform or pose between two frames from the TF tree. + * The second and third parameters are named for the transform interpretation. + * Consider them named "a" and "b" respectively: + * For a transform this is a rotation and translation, i.e. aToB. + * For a pose this is a position and orientation, i.e. aInB. + * \param buffer ROS TF Buffer, make sure a listener is attached + * \param fromFrame From (transform) or child (pose) frame + * \param toFrame To (transform) or parent (pose) frame + * \param time Time to query the transform at, default is the latest + * \return The transform or pose represented by an SE3 lie group element + */ [[nodiscard]] static auto fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrame, std::string const& toFrame, rclcpp::Time const& time = rclcpp::Time{}) -> SE3d; /** - * \brief Push a transform to the TF tree between two frames - * \see fromTfTree for more explanation of the frames - * \param broadcaster ROS TF Broadcaster - * \param fromFrame From (transform) or child (pose) frame - * \param toFrame To (transform) or parent (pose) frame - * \param transform The transform or pose represented by an SE3 lie group element - * \param time - */ + * \brief Push a transform to the TF tree between two frames + * \see fromTfTree for more explanation of the frames + * \param broadcaster ROS TF Broadcaster + * \param fromFrame From (transform) or child (pose) frame + * \param toFrame To (transform) or parent (pose) frame + * \param transform The transform or pose represented by an SE3 lie group element + * \param time + */ static auto pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& fromFrame, std::string const& toFrame, SE3d const& transform, rclcpp::Time const& time) -> void; }; diff --git a/state_machine/state.hpp b/state_machine/state.hpp index 71a8bea7..30dd328b 100644 --- a/state_machine/state.hpp +++ b/state_machine/state.hpp @@ -1,19 +1,17 @@ #pragma once -#include -#include - /** +/** * \brief Virtual state class to describe how states in the state machine should act * \see state_machine.hpp for reference on how the states will be used by the state machine */ class State { public: - /** + /** * \brief Virtual destructor for the state class */ virtual ~State() = default; - /** + /** * \brief The function which will be called every loop in the state machine */ virtual auto onLoop() -> State* = 0; -}; \ No newline at end of file +}; diff --git a/state_machine/state_machine.hpp b/state_machine/state_machine.hpp index 09062e62..9bb5642d 100644 --- a/state_machine/state_machine.hpp +++ b/state_machine/state_machine.hpp @@ -11,7 +11,7 @@ #include #include - /** +/** * \brief State Machine class that facilitates transitioning between different states which inherit from the State class * \see state.hpp for reference on creating states */ @@ -25,11 +25,11 @@ class StateMachine{ std::unordered_map> mValidTransitions; std::unordered_map decoder; - /** - * \brief Ensures that transitioning from "from" to "to" is a valid transition - * \param from The runtime type of the from state - * \param to The runtime type of the to state - */ + /** + * \brief Ensures that transitioning from "from" to "to" is a valid transition + * \param from The runtime type of the from state + * \param to The runtime type of the to state + */ void assertValidTransition(std::type_info const& from, std::type_info const& to) const { auto it = mValidTransitions.find(from.hash_code()); @@ -43,11 +43,11 @@ class StateMachine{ } } - /** - * \brief Adds the demangled name to the map in the corresponding type hash slot - * \param hash The type hash for the runtime type - * \param name The demangled name of the runtime type - */ + /** + * \brief Adds the demangled name to the map in the corresponding type hash slot + * \param hash The type hash for the runtime type + * \param name The demangled name of the runtime type + */ void addNameToDecoder(TypeHash hash, std::string const& name){ constexpr static std::string prefix{"mrover::"}; TypeHash index = name.find(prefix); @@ -56,107 +56,107 @@ class StateMachine{ decoder[hash] = _name; } public: - /** - * \brief Constructor for the StateMachine Class - * \param name The name of the state machine useful for visualization - * \param initialState The initial state which the state machine will begin execution in - */ + /** + * \brief Constructor for the StateMachine Class + * \param name The name of the state machine useful for visualization + * \param initialState The initial state which the state machine will begin execution in + */ explicit StateMachine(std::string name, State* initialState) : mName{std::move(name)}, currState{initialState}{}; ~StateMachine(){ delete currState; } - - /** - * \brief Makes a state which the state machine will use for execution. + + /** + * \brief Makes a state which the state machine will use for execution. * DO NOT CALL THIS FUNCTION AND NOT PASS THE STATE TO THE STATE MACHINE - * \param args The arguments that will be passed to the constructor of the state - */ + * \param args The arguments that will be passed to the constructor of the state + */ template - static auto make_state(Args... args) -> T*{ - static_assert(std::derived_from, "State Must Be Derived From The State Class"); - return new T(args...); - } + static auto make_state(Args... args) -> T*{ + static_assert(std::derived_from, "State Must Be Derived From The State Class"); + return new T(args...); + } - /** - * \brief Returns the name of the state machine - * \return A constant reference to the name of the state machine - */ + /** + * \brief Returns the name of the state machine + * \return A constant reference to the name of the state machine + */ auto getName() const -> std::string const& { return mName; } - /** - * \brief Returns the demangled name of the state at runtime - * \param state A pointer to a state derived object which will have its runtime type analyzed - * \return A constant reference to the demangled state name at runtime - */ + /** + * \brief Returns the demangled name of the state at runtime + * \param state A pointer to a state derived object which will have its runtime type analyzed + * \return A constant reference to the demangled state name at runtime + */ auto getStateName(State const* state) const -> std::string const&{ return decoder.find(typeid(*state).hash_code())->second; } - /** - * \brief Returns the demangled name of the current state in the state machine - * \return A constant reference to the current state's demangled state name - */ + /** + * \brief Returns the demangled name of the current state in the state machine + * \return A constant reference to the current state's demangled state name + */ auto getCurrentState() const -> std::string const& { return getStateName(currState); } - /** - * \brief Returns a map of type hashes to a vector of each type hash - * \return A constant reference to the map describing all valid state transitions - */ + /** + * \brief Returns a map of type hashes to a vector of each type hash + * \return A constant reference to the map describing all valid state transitions + */ auto getTransitionTable() const -> std::unordered_map> const&{ return mValidTransitions; } - /** - * \brief Takes in a type hash and returns the demangled state name - * \return A constant reference to the demangled state name - */ + /** + * \brief Takes in a type hash and returns the demangled state name + * \return A constant reference to the demangled state name + */ auto decodeTypeHash(TypeHash hash) const -> std::string const&{ return decoder.find(hash)->second; } - /** - * \brief Enables the state transition from the first templated type to the subsequent templated types - */ + /** + * \brief Enables the state transition from the first templated type to the subsequent templated types + */ template - void enableTransitions(){ - static_assert(std::derived_from, "From State Must Be Derived From The State Class"); - static_assert((std::derived_from && ...), "All States Must Be Derived From The State Class"); - // Add From State To Decoder - int status = 0; - char* demangledName = abi::__cxa_demangle(typeid(From).name(), nullptr, nullptr, &status); - - if(status){ - throw std::runtime_error("C++ demangle failed!"); - } + void enableTransitions(){ + static_assert(std::derived_from, "From State Must Be Derived From The State Class"); + static_assert((std::derived_from && ...), "All States Must Be Derived From The State Class"); + // Add From State To Decoder + int status = 0; + char* demangledName = abi::__cxa_demangle(typeid(From).name(), nullptr, nullptr, &status); + + if(status){ + throw std::runtime_error("C++ demangle failed!"); + } + + addNameToDecoder(typeid(From).hash_code(), demangledName); + free(demangledName); - addNameToDecoder(typeid(From).hash_code(), demangledName); - free(demangledName); + mValidTransitions[typeid(From).hash_code()] = {typeid(To).hash_code()...}; - mValidTransitions[typeid(From).hash_code()] = {typeid(To).hash_code()...}; - - std::vector> types{std::ref(typeid(To))...}; - for(auto const& type : types){ - demangledName = abi::__cxa_demangle(type.get().name(), nullptr, nullptr, &status); - addNameToDecoder(type.get().hash_code(), demangledName); - free(demangledName); + std::vector> types{std::ref(typeid(To))...}; + for(auto const& type : types){ + demangledName = abi::__cxa_demangle(type.get().name(), nullptr, nullptr, &status); + addNameToDecoder(type.get().hash_code(), demangledName); + free(demangledName); + } } - } - /** - * \brief Runs the onLoop function for the state and then transitions to the state returned from that function - */ + /** + * \brief Runs the onLoop function for the state and then transitions to the state returned from that function + */ void update(){ State* newState = currState->onLoop(); - + assertValidTransition(typeid(*currState), typeid(*newState)); if(newState != currState){ delete currState; } currState = newState; } -}; \ No newline at end of file +}; diff --git a/state_machine/state_publisher_server.hpp b/state_machine/state_publisher_server.hpp index b29a7a9f..b22d9ecf 100644 --- a/state_machine/state_publisher_server.hpp +++ b/state_machine/state_publisher_server.hpp @@ -19,22 +19,22 @@ #include namespace mrover{ - class StatePublisher{ - private: - StateMachine const& mStateMachine; + class StatePublisher{ + private: + StateMachine const& mStateMachine; - rclcpp::Publisher::SharedPtr mStructurePub; - rclcpp::Publisher::SharedPtr mStatePub; + rclcpp::Publisher::SharedPtr mStructurePub; + rclcpp::Publisher::SharedPtr mStatePub; - rclcpp::TimerBase::SharedPtr mStructureTimer; - rclcpp::TimerBase::SharedPtr mStateTimer; - /** - * \brief Publishes the structure to be used by visualizer.py - * \see visualizer.py to see how these topic will be used - */ - void publishStructure(){ - auto structureMsg = mrover::msg::StateMachineStructure(); - structureMsg.machine_name = mStateMachine.getName(); + rclcpp::TimerBase::SharedPtr mStructureTimer; + rclcpp::TimerBase::SharedPtr mStateTimer; + /** + * \brief Publishes the structure to be used by visualizer.py + * \see visualizer.py to see how these topic will be used + */ + void publishStructure(){ + auto structureMsg = mrover::msg::StateMachineStructure(); + structureMsg.machine_name = mStateMachine.getName(); auto transitionTable = mStateMachine.getTransitionTable(); for(auto const&[from, tos] : transitionTable){ @@ -47,35 +47,35 @@ namespace mrover{ } mStructurePub->publish(structureMsg); - } + } - /** - * \brief Publishes the current state of the state machine - * \see visualizer.py to see how these topic will be used - */ - void publishState(){ + /** + * \brief Publishes the current state of the state machine + * \see visualizer.py to see how these topic will be used + */ + void publishState(){ auto stateMachineUpdate = mrover::msg::StateMachineStateUpdate(); stateMachineUpdate.state_machine_name = mStateMachine.getName(); stateMachineUpdate.state = mStateMachine.getCurrentState(); mStatePub->publish(stateMachineUpdate); - } + } - public: - /** - * \brief Creates a State Publisher to facilitate the communications between visualizer.py and the state machine - * \param node The node which owns the state publisher - * \param stateMachine The state machine which the publisher will describe - * \param structureTopicName The topic which will publish the state machine's structure - * \param structureTopicHz The rate at which the structure topic will publish - * \param stateTopicName The topic which will publish the state machine's state - * \param stateTopicHz The rate at which the state topic will publish - */ - StatePublisher(rclcpp::Node* node, StateMachine const& stateMachine, std::string const& structureTopicName, double structureTopicHz, std::string const& stateTopicName, double stateTopicHz) : mStateMachine{stateMachine} { - mStructurePub = node->create_publisher(structureTopicName, 1); - mStatePub = node->create_publisher(stateTopicName, 1); + public: + /** + * \brief Creates a State Publisher to facilitate the communications between visualizer.py and the state machine + * \param node The node which owns the state publisher + * \param stateMachine The state machine which the publisher will describe + * \param structureTopicName The topic which will publish the state machine's structure + * \param structureTopicHz The rate at which the structure topic will publish + * \param stateTopicName The topic which will publish the state machine's state + * \param stateTopicHz The rate at which the state topic will publish + */ + StatePublisher(rclcpp::Node* node, StateMachine const& stateMachine, std::string const& structureTopicName, double structureTopicHz, std::string const& stateTopicName, double stateTopicHz) : mStateMachine{stateMachine} { + mStructurePub = node->create_publisher(structureTopicName, 1); + mStatePub = node->create_publisher(stateTopicName, 1); - mStructureTimer = node->create_wall_timer(std::chrono::milliseconds(static_cast(1 / structureTopicHz)), [&](){publishStructure();}); - mStateTimer = node->create_wall_timer(std::chrono::milliseconds(static_cast(1 / stateTopicHz)), [&](){publishState();}); - } - }; -} \ No newline at end of file + mStructureTimer = node->create_wall_timer(std::chrono::milliseconds(static_cast(1 / structureTopicHz)), [&](){publishStructure();}); + mStateTimer = node->create_wall_timer(std::chrono::milliseconds(static_cast(1 / stateTopicHz)), [&](){publishState();}); + } + }; +}