diff --git a/state_machine/state_machine.hpp b/state_machine/state_machine.hpp index 9bb5642d..df364847 100644 --- a/state_machine/state_machine.hpp +++ b/state_machine/state_machine.hpp @@ -99,7 +99,7 @@ class StateMachine{ * \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& { + auto getCurrentStateName() const -> std::string const& { return getStateName(currState); } @@ -142,6 +142,9 @@ class StateMachine{ std::vector> types{std::ref(typeid(To))...}; for(auto const& type : types){ demangledName = abi::__cxa_demangle(type.get().name(), nullptr, nullptr, &status); + if(status){ + throw std::runtime_error("C++ demangle failed!"); + } addNameToDecoder(type.get().hash_code(), demangledName); free(demangledName); } diff --git a/state_machine/state_publisher_server.hpp b/state_machine/state_publisher_server.hpp index b22d9ecf..1d8c21b6 100644 --- a/state_machine/state_publisher_server.hpp +++ b/state_machine/state_publisher_server.hpp @@ -56,7 +56,7 @@ namespace mrover{ void publishState(){ auto stateMachineUpdate = mrover::msg::StateMachineStateUpdate(); stateMachineUpdate.state_machine_name = mStateMachine.getName(); - stateMachineUpdate.state = mStateMachine.getCurrentState(); + stateMachineUpdate.state = mStateMachine.getCurrentStateName(); mStatePub->publish(stateMachineUpdate); } @@ -74,8 +74,8 @@ namespace mrover{ 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();}); + mStructureTimer = node->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / structureTopicHz)), [&](){publishStructure();}); + mStateTimer = node->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / stateTopicHz)), [&](){publishState();}); } }; }