HiveCoreRD/hivecore_robot_brain
6
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

refactor(smacc2): replace RCLCPP logging macros with hivecore logger SDK

Browse Source 
- Replace all RCLCPP_DEBUG/INFO/WARN/ERROR/FATAL macros across 43 files
  in smacc2 headers and sources
- Printf-style format strings converted to fmt/spdlog {} style
- Stream variants (RCLCPP_*_STREAM) converted to lambda + ostringstream:
    LOG_X("{}", [&]{ std::ostringstream _oss; _oss << expr; ... }())
- Throttle variants converted to LOG_*_THROTTLE(ms, ...)
- Add hivecore_logger_cpp dependency to smacc2/CMakeLists.txt
This commit is contained in:
david
2026-03-04 14:22:05 +08:00
parent a39f6a6d2e
commit 80217bbae7
44 changed files with 395 additions and 590 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/thirdparty/SMACC2-humble/smacc2/CMakeLists.txt vendored
View File

@@ -19,6 +19,10 @@ find_package(Boost COMPONENTS thread REQUIRED)
find_package(tracetools)
find_package(LTTngUST REQUIRED)
# hivecore logger SDK
set(hivecore_logger_cpp_DIR "/opt/hivecore/logger-sdk/1.0.0/lib/cmake/hivecore_logger_cpp")
find_package(hivecore_logger_cpp REQUIRED)
set(dependencies
rclcpp
smacc2_msgs
@@ -26,7 +30,9 @@ set(dependencies
tracetools
)
include_directories(include ${LTTNGUST_INCLUDE_DIRS})
include_directories(include ${LTTNGUST_INCLUDE_DIRS}
/opt/hivecore/logger-sdk/1.0.0/include
)
file(GLOB_RECURSE SRC_FILES src *.cpp)
add_library(smacc2 SHARED ${SRC_FILES})
@@ -42,6 +48,7 @@ ament_target_dependencies(smacc2 ${dependencies})
target_link_libraries(smacc2
${Boost_LIBRARIES}
${LTTNGUST_LIBRARIES}
hivecore_logger_cpp::hivecore_logger_cpp
)
ament_export_include_directories(include)

61
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp vendored
View File

@@ -21,11 +21,13 @@
#pragma once
#include <smacc2/client_bases/smacc_action_client.hpp>
#include <sstream>
#include <smacc2/smacc_default_events.hpp>
#include <smacc2/smacc_signal.hpp>
#include <optional>
#include <rclcpp_action/rclcpp_action.hpp>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -86,9 +88,7 @@ public:
void waitForServer()
{
RCLCPP_INFO_STREAM(
this->getLogger(),
"Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>(); return _oss.str(); }());
client_->wait_for_action_server();
}
@@ -125,8 +125,7 @@ public:
auto * ev = new EvType();
// ev->client = this;
// ev->resultMessage = *result;
RCLCPP_INFO(
getLogger(), "Action client Posting EVENT %s", demangleSymbol(typeid(ev).name()).c_str());
LOG_INFO("Action client Posting EVENT {}", demangleSymbol(typeid(ev).name()).c_str());
this->postEvent(ev);
}
@@ -148,7 +147,7 @@ public:
actionFeedbackEvent->client = this;
actionFeedbackEvent->feedbackMessage = msg;
this->postEvent(actionFeedbackEvent);
RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
LOG_DEBUG("[{}] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
};
feedback_cb = [this](auto client, auto feedback) { this->onFeedback(client, feedback); };
@@ -275,13 +274,11 @@ public:
// if goal ids are not matched, the older goa call this callback so ignore the result
// if matched, it must be processed (including aborted)
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] Action result callback, getting shared future");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Action result callback, getting shared future"; return _oss.str(); }());
// auto goalHandle = result->get();
// goalHandle_ = lastRequest_->get();
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] Action client Result goal id: "
<< rclcpp_action::to_string(result.goal_id));
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Action client Result goal id: "
<< rclcpp_action::to_string(result.goal_id); return _oss.str(); }());
// if (goalHandle_->get_goal_id() == result.goal_id)
// {
@@ -293,15 +290,13 @@ public:
if (resultCallbackPtr != nullptr)
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] Result CB calling user callback:"
<< demangleSymbol(typeid(*resultCallbackPtr).name()));
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Result CB calling user callback:"
<< demangleSymbol(typeid(*resultCallbackPtr).name()); return _oss.str(); }());
(*resultCallbackPtr)(result);
}
else
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] Result CB calling default callback");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Result CB calling default callback"; return _oss.str(); }());
this->onResult(result);
}
@@ -323,23 +318,18 @@ public:
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": no previous request.");
// }
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] client ready clients: "
<< this->client_->get_number_of_ready_clients());
RCLCPP_INFO_STREAM(
getLogger(),
"[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready());
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] client ready clients: "
<< this->client_->get_number_of_ready_clients(); return _oss.str(); }());
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready(); return _oss.str(); }());
RCLCPP_INFO_STREAM(getLogger(), getName() << ": async send goal.");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << getName() << ": async send goal."; return _oss.str(); }());
auto lastRequest = this->client_->async_send_goal(goal, options);
this->lastRequest_ = lastRequest;
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] Action request "
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Action request "; return _oss.str(); }());
// << rclcpp_action::to_string(this->goalHandle_->get_goal_id()) <<". Goal sent to " << this->action_endpoint_
// << "\": " << std::endl
// << goal
);
// if (client_->isServerConnected())
// {
@@ -395,46 +385,43 @@ protected:
// const auto &resultType = this->getState();
const auto & resultType = result_msg.code;
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] response result ["
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response result ["
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType; return _oss.str(); }());
if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
{
RCLCPP_INFO(getLogger(), "[%s] request result: Success", this->getName().c_str());
LOG_INFO("[{}] request result: Success", this->getName().c_str());
onSucceeded_(result_msg);
postSuccessEvent(result_msg);
}
else if (resultType == rclcpp_action::ResultCode::ABORTED)
{
RCLCPP_INFO(getLogger(), "[%s] request result: Aborted", this->getName().c_str());
LOG_INFO("[{}] request result: Aborted", this->getName().c_str());
onAborted_(result_msg);
postAbortedEvent(result_msg);
}
else if (resultType == rclcpp_action::ResultCode::CANCELED)
{
RCLCPP_INFO(getLogger(), "[%s] request result: Cancelled", this->getName().c_str());
LOG_INFO("[{}] request result: Cancelled", this->getName().c_str());
onCancelled_(result_msg);
postCancelledEvent(result_msg);
}
/*
else if (resultType == actionlib::SimpleClientGoalState::REJECTED)
{
RCLCPP_INFO(getLogger(),"[%s] request result: Rejected", this->getName().c_str());
LOG_INFO("[{}] request result: Rejected", this->getName().c_str());
onRejected_(result_msg);
postRejectedEvent(result_msg);
}
else if (resultType == actionlib::SimpleClientGoalState::PREEMPTED)
{
RCLCPP_INFO(getLogger(),"[%s] request result: Preempted", this->getName().c_str());
LOG_INFO("[{}] request result: Preempted", this->getName().c_str());
onPreempted_(result_msg);
postPreemptedEvent(result_msg);
}*/
else
{
RCLCPP_INFO(
getLogger(), "[%s] request result: NOT HANDLED TYPE: %d", this->getName().c_str(),
(int)resultType);
LOG_INFO("[{}] request result: NOT HANDLED TYPE: {}", this->getName().c_str(), (int)resultType);
}
}
};

7
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_publisher_client.hpp vendored
View File

@@ -21,6 +21,8 @@
#pragma once
#include <optional>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client.hpp>
namespace smacc2
@@ -46,7 +48,7 @@ public:
{
if (!this->topicName)
{
RCLCPP_ERROR(getLogger(), "topic publisher with no topic name set. Skipping advertising.");
LOG_ERROR("topic publisher with no topic name set. Skipping advertising.");
return;
}
@@ -58,8 +60,7 @@ public:
qos.durability(*durability);
qos.reliability(*reliability);
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Client Publisher to topic: " << topicName);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Publisher to topic: " << topicName; return _oss.str(); }());
pub_ = getNode()->create_publisher<MessageType>(*(this->topicName), qos);
this->initialized_ = true;

7
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_client.hpp vendored
View File

@@ -21,6 +21,8 @@
#pragma once
#include <optional>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client.hpp>
namespace smacc2
@@ -43,12 +45,11 @@ public:
{
if (!serviceName_)
{
RCLCPP_ERROR(getLogger(), "service client with no service name set. Skipping.");
LOG_ERROR("service client with no service name set. Skipping.");
}
else
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Service: " << *serviceName_; return _oss.str(); }());
this->initialized_ = true;
client_ = getNode()->create_client<ServiceType>(*serviceName_);
}

9
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_server_client.hpp vendored
View File

@@ -21,6 +21,8 @@
#pragma once
#include <optional>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client.hpp>
#include <smacc2/smacc_signal.hpp>
@@ -67,14 +69,11 @@ public:
{
if (!serviceName_)
{
RCLCPP_ERROR_STREAM(
getLogger(),
"[" << this->getName() << "] service server with no service name set. Skipping.");
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] service server with no service name set. Skipping."; return _oss.str(); }());
}
else
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Service: " << *serviceName_; return _oss.str(); }());
server_ = getNode()->create_service<TService>(
*serviceName_, std::bind(

7
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_subscriber_client.hpp vendored
View File

@@ -21,6 +21,8 @@
#pragma once
#include <optional>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client.hpp>
#include <smacc2/smacc_signal.hpp>
#include <smacc2/smacc_state_machine.hpp>
@@ -101,12 +103,11 @@ protected:
if (!topicName)
{
RCLCPP_ERROR(getLogger(), "topic client with no topic name set. Skipping subscribing");
LOG_ERROR("topic client with no topic name set. Skipping subscribing");
}
else
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Subscribing to topic: " << *topicName);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Subscribing to topic: " << *topicName; return _oss.str(); }());
rclcpp::SensorDataQoS qos;
if (queueSize) qos.keep_last(*queueSize);

23
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_call_service.hpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#pragma once
#include <smacc2/impl/smacc_asynchronous_client_behavior_impl.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client_behavior.hpp>
namespace smacc2
@@ -45,41 +47,36 @@ public:
void onEntry() override
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] creating ros service client: " << serviceName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] creating ros service client: " << serviceName_; return _oss.str(); }());
client_ = getNode()->create_client<ServiceType>(serviceName_);
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] making service request to " << serviceName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] making service request to " << serviceName_; return _oss.str(); }());
resultFuture_ = client_->async_send_request(request_).future.share();
std::future_status status = resultFuture_.wait_for(0s);
RCLCPP_INFO_STREAM(
getLogger(), "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
<< this->isShutdownRequested() << "");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
<< this->isShutdownRequested() << ""; return _oss.str(); }());
while (status != std::future_status::ready && rclcpp::ok() && !this->isShutdownRequested())
{
RCLCPP_INFO_STREAM_THROTTLE(
getLogger(), *getNode()->get_clock(), 1000,
"[" << this->getName() << "] waiting response ");
LOG_INFO_THROTTLE(1000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] waiting response "; return _oss.str(); }());
rclcpp::sleep_for(pollRate_);
status = resultFuture_.wait_for(0s);
}
if (status == std::future_status::ready)
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response received "; return _oss.str(); }());
result_ = resultFuture_.get();
onServiceResponse(result_);
this->postSuccessEvent();
}
else
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response not received ");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response not received "; return _oss.str(); }());
this->postFailureEvent();
}
}
@@ -97,7 +94,7 @@ protected:
virtual void onServiceResponse(std::shared_ptr<typename ServiceType::Response> /*result*/)
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response received "; return _oss.str(); }());
}
};

5
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_sequence.hpp vendored
View File

@@ -22,6 +22,7 @@
#include <functional>
#include <rclcpp/rclcpp.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace smacc2
@@ -60,9 +61,7 @@ public:
std::function<std::shared_ptr<smacc2::SmaccAsyncClientBehavior>()> delayedCBFactoryFn =
[this, args...]()
{
RCLCPP_INFO(
getLogger(), "[CbSequence] then creating new sub behavior %s ",
demangleSymbol<TBehavior>().c_str());
LOG_INFO("[CbSequence] then creating new sub behavior {} ", demangleSymbol<TBehavior>().c_str());
auto createdBh = std::shared_ptr<TBehavior>(new TBehavior(args...));
this->getCurrentState()->getOrthogonal<TOrthogonal>()->addClientBehavior(createdBh);

9
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_topic_message.hpp vendored
View File

@@ -21,7 +21,9 @@
#pragma once
#include <functional>
#include <sstream>
#include <rclcpp/rclcpp.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
namespace smacc2
@@ -50,9 +52,8 @@ public:
rclcpp::SensorDataQoS qos;
// qos.reliable();
// rclcpp::SubscriptionOptions sub_option;
RCLCPP_INFO_STREAM(
getLogger(), "[CbWaitTopicMessage] waiting message from topic: "
<< topicname_ << "[" << demangledTypeName<TMessage>() << "]");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbWaitTopicMessage] waiting message from topic: "
<< topicname_ << "[" << demangledTypeName<TMessage>() << "]"; return _oss.str(); }());
// sub_ = getNode()->create_subscription<TMessage>(
// topicname_, qos,
@@ -70,7 +71,7 @@ public:
{
if (guardFn_ == nullptr || guardFn_(*msg))
{
RCLCPP_INFO(getLogger(), "[CbWaitTopicMessage] message received.");
LOG_INFO("[CbWaitTopicMessage] message received.");
success = true;
this->postSuccessEvent();
}

49
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_action_client.hpp vendored
View File

@@ -15,6 +15,7 @@
#pragma once
#include <smacc2/component.hpp>
#include <sstream>
#include <smacc2/smacc_default_events.hpp>
#include <smacc2/smacc_signal.hpp>
#include <smacc2/smacc_state_machine.hpp>
@@ -25,6 +26,7 @@
#include <mutex>
#include <optional>
#include <rclcpp_action/rclcpp_action.hpp>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -91,25 +93,23 @@ public:
{
std::lock_guard<std::mutex> lock(actionMutex_);
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Action result callback, goal id: "
<< rclcpp_action::to_string(result.goal_id));
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action result callback, goal id: "
<< rclcpp_action::to_string(result.goal_id); return _oss.str(); }());
auto resultCallbackPtr = resultCallback.lock();
if (resultCallbackPtr != nullptr)
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Calling user result callback");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Calling user result callback"; return _oss.str(); }());
(*resultCallbackPtr)(result);
}
else
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Using default result handling");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Using default result handling"; return _oss.str(); }());
this->onResult(result);
}
};
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Sending goal to action server");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Sending goal to action server"; return _oss.str(); }());
auto goalFuture = client_->async_send_goal(goal, options);
lastRequest_ = goalFuture;
@@ -123,7 +123,7 @@ public:
if (lastRequest_ && lastRequest_->valid())
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling current goal");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Cancelling current goal"; return _oss.str(); }());
auto cancelFuture = client_->async_cancel_all_goals();
lastCancelResponse_ = cancelFuture;
@@ -131,7 +131,7 @@ public:
}
else
{
RCLCPP_WARN_STREAM(getLogger(), "[" << this->getName() << "] No active goal to cancel");
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] No active goal to cancel"; return _oss.str(); }());
return false;
}
}
@@ -142,9 +142,7 @@ public:
{
if (client_)
{
RCLCPP_INFO_STREAM(
getLogger(),
"[" << this->getName() << "] Waiting for action server: " << *actionServerName);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Waiting for action server: " << *actionServerName; return _oss.str(); }());
client_->wait_for_action_server();
}
}
@@ -154,13 +152,11 @@ public:
{
if (!actionServerName)
{
RCLCPP_ERROR_STREAM(getLogger(), "[" << this->getName() << "] Action server name not set!");
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action server name not set!"; return _oss.str(); }());
return;
}
RCLCPP_INFO_STREAM(
getLogger(),
"[" << this->getName() << "] Initializing action client for: " << *actionServerName);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Initializing action client for: " << *actionServerName; return _oss.str(); }());
client_ = rclcpp_action::create_client<ActionType>(getNode(), *actionServerName);
@@ -188,7 +184,7 @@ public:
actionFeedbackEvent->client = this;
actionFeedbackEvent->feedbackMessage = feedback;
this->postEvent(actionFeedbackEvent);
RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", this->getName().c_str());
LOG_DEBUG("[{}] FEEDBACK EVENT", this->getName().c_str());
};
}
@@ -241,33 +237,30 @@ private:
{
const auto & resultType = result_msg.code;
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Action result ["
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action result ["
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType; return _oss.str(); }());
if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
{
RCLCPP_INFO(getLogger(), "[%s] Action result: Success", this->getName().c_str());
LOG_INFO("[{}] Action result: Success", this->getName().c_str());
onActionSucceeded_(result_msg);
postSuccessEvent(result_msg);
}
else if (resultType == rclcpp_action::ResultCode::ABORTED)
{
RCLCPP_INFO(getLogger(), "[%s] Action result: Aborted", this->getName().c_str());
LOG_INFO("[{}] Action result: Aborted", this->getName().c_str());
onActionAborted_(result_msg);
postAbortedEvent(result_msg);
}
else if (resultType == rclcpp_action::ResultCode::CANCELED)
{
RCLCPP_INFO(getLogger(), "[%s] Action result: Cancelled", this->getName().c_str());
LOG_INFO("[{}] Action result: Cancelled", this->getName().c_str());
onActionCancelled_(result_msg);
postCancelledEvent(result_msg);
}
else
{
RCLCPP_WARN(
getLogger(), "[%s] Action result: Unhandled type: %d", this->getName().c_str(),
(int)resultType);
LOG_WARN("[{}] Action result: Unhandled type: {}", this->getName().c_str(), (int)resultType);
}
}
@@ -275,9 +268,7 @@ private:
void postResultEvent(const WrappedResult & /* result */)
{
auto * ev = new EvType();
RCLCPP_INFO(
getLogger(), "[%s] Posting event: %s", this->getName().c_str(),
smacc2::demangleSymbol(typeid(ev).name()).c_str());
LOG_INFO("[{}] Posting event: {}", this->getName().c_str(), smacc2::demangleSymbol(typeid(ev).name()).c_str());
this->postEvent(ev);
}
};

18
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_ros2_timer.hpp vendored
View File

@@ -15,8 +15,10 @@
#pragma once
#include <chrono>
#include <sstream>
#include <optional>
#include <rclcpp/rclcpp.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/common.hpp>
#include <smacc2/component.hpp>
#include <smacc2/smacc_signal.hpp>
@@ -47,9 +49,8 @@ public:
{
if (!initialized_)
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Initializing ROS2 Timer with duration: "
<< duration_.seconds() << "s, oneshot: " << (oneshot_ ? "true" : "false"));
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Initializing ROS2 Timer with duration: "
<< duration_.seconds() << "s, oneshot: " << (oneshot_ ? "true" : "false"); return _oss.str(); }());
auto clock = this->getNode()->get_clock();
timer_ = rclcpp::create_timer(
@@ -65,7 +66,7 @@ public:
{
if (timer_ && timer_->is_canceled())
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Starting timer");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Starting timer"; return _oss.str(); }());
timer_->reset();
}
}
@@ -74,7 +75,7 @@ public:
{
if (timer_)
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Stopping timer");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Stopping timer"; return _oss.str(); }());
timer_->cancel();
}
}
@@ -83,7 +84,7 @@ public:
{
if (timer_)
{
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling timer");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Cancelling timer"; return _oss.str(); }());
timer_->cancel();
}
}
@@ -97,7 +98,7 @@ public:
private:
void timerCallback()
{
RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] Timer tick");
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Timer tick"; return _oss.str(); }());
if (!onTimerTick_.empty())
{
@@ -106,8 +107,7 @@ private:
if (oneshot_)
{
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Oneshot timer completed, cancelling");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Oneshot timer completed, cancelling"; return _oss.str(); }());
timer_->cancel();
}
}

5
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_publisher.hpp vendored
View File

@@ -20,7 +20,9 @@
#pragma once
#include <optional>
#include <sstream>
#include <rclcpp/rclcpp.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/common.hpp>
#include <smacc2/component.hpp>
@@ -73,8 +75,7 @@ void CpTopicPublisher<T>::onInitialize()
qos.durability(*durability);
qos.reliability(*reliability);
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Publisher to topic: " << topicName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Publisher to topic: " << topicName_; return _oss.str(); }());
auto nh = this->getNode();
pub_ = nh->template create_publisher<T>(this->topicName_, qos);

5
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_subscriber.hpp vendored
View File

@@ -20,6 +20,8 @@
#pragma once
#include <optional>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/client_bases/smacc_subscriber_client.hpp>
#include <smacc2/component.hpp>
#include <smacc2/smacc_signal.hpp>
@@ -93,8 +95,7 @@ public:
if (!queueSize) queueSize = 1;
RCLCPP_INFO_STREAM(
getLogger(), "[" << this->getName() << "] Subscribing to topic: " << topicName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Subscribing to topic: " << topicName_; return _oss.str(); }());
rclcpp::SensorDataQoS qos;
if (queueSize) qos.keep_last(*queueSize);

5
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_asynchronous_client_behavior_impl.hpp vendored
View File

@@ -20,6 +20,7 @@
#pragma once
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_state_machine.hpp>
namespace smacc2
@@ -35,9 +36,7 @@ void SmaccAsyncClientBehavior::onOrthogonalAllocation()
{
if (postFinishEventFn_ || postSuccessEventFn_ || postFailureEventFn_)
{
RCLCPP_WARN(
getLogger(),
"SmaccAsyncClientBehavior already has event posting functions assigned. Skipping "
LOG_WARN("SmaccAsyncClientBehavior already has event posting functions assigned. Skipping "
"re-assignment. This could be a problem if you are using the same behavior in multiple "
"states. This may be related with the deprecation of onOrthogonalAllocation in favor of "
"onStateOrthogonalAllocation.");

13
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_client_behavior_impl.hpp vendored
View File

@@ -21,6 +21,7 @@
#pragma once
#include <smacc2/smacc_client_behavior.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_state_machine.hpp>
namespace smacc2
@@ -30,9 +31,7 @@ void ISmaccClientBehavior::postEvent(const EventType & ev)
{
if (stateMachine_ == nullptr)
{
RCLCPP_ERROR(
getLogger(),
"The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
LOG_ERROR("The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
"post event call.");
}
else
@@ -46,9 +45,7 @@ void ISmaccClientBehavior::postEvent()
{
if (stateMachine_ == nullptr)
{
RCLCPP_ERROR(
getLogger(),
"The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
LOG_ERROR("The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
"post event call.");
}
else
@@ -75,9 +72,7 @@ void ISmaccClientBehavior::requiresComponent(
{
if (stateMachine_ == nullptr)
{
RCLCPP_ERROR(
getLogger(),
"Cannot use the requiresComponent funcionality before assigning the client behavior to an "
LOG_ERROR("Cannot use the requiresComponent funcionality before assigning the client behavior to an "
"orthogonal. Try using the OnEntry method to capture required components.");
}
else

14
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_client_impl.hpp vendored
View File

@@ -21,6 +21,7 @@
#pragma once
#include <smacc2/impl/smacc_state_machine_impl.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client.hpp>
namespace smacc2
@@ -99,11 +100,8 @@ SmaccComponentType * ISmaccClient::createNamedComponent(std::string name, TArgs.
if (it == this->components_.end())
{
auto tname = demangledTypeName<SmaccComponentType>();
RCLCPP_INFO(
getLogger(),
"Creating a new component of type: %s smacc component is required by client '%s'. Creating a "
"new instance %s",
this->getName().c_str(), demangledTypeName<SmaccComponentType>().c_str(), tname.c_str());
LOG_INFO("Creating a new component of type: {} smacc component is required by client '{}'. Creating a "
"new instance {}", this->getName().c_str(), demangledTypeName<SmaccComponentType>().c_str(), tname.c_str());
ret = std::shared_ptr<SmaccComponentType>(new SmaccComponentType(targs...));
ret->setStateMachine(this->getStateMachine());
@@ -114,13 +112,11 @@ SmaccComponentType * ISmaccClient::createNamedComponent(std::string name, TArgs.
this->components_[componentkey] =
ret; //std::dynamic_pointer_cast<smacc2::ISmaccComponent>(ret);
RCLCPP_DEBUG(getLogger(), "%s resource is required. Done.", tname.c_str());
LOG_DEBUG("{} resource is required. Done.", tname.c_str());
}
else
{
RCLCPP_INFO(
getLogger(), "%s resource is required. Found resource in cache.",
demangledTypeName<SmaccComponentType>().c_str());
LOG_INFO("{} resource is required. Found resource in cache.", demangledTypeName<SmaccComponentType>().c_str());
ret = dynamic_pointer_cast<SmaccComponentType>(it->second);
}

16
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_component_impl.hpp vendored
View File

@@ -21,6 +21,8 @@
#pragma once
#include <string>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/component.hpp>
#include <smacc2/impl/smacc_state_machine_impl.hpp>
@@ -48,17 +50,16 @@ void ISmaccComponent::requiresComponent(
if (requiredComponentStorage == nullptr && throwExceptionIfNotExist)
{
RCLCPP_DEBUG_STREAM(
this->getLogger(), std::string("Required component ") +
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << std::string("Required component ") +
demangleSymbol(typeid(TComponent).name()) +
" not found. Available components:");
" not found. Available components:"; return _oss.str(); }());
std::vector<std::shared_ptr<ISmaccComponent>> components;
this->owner_->getComponents(components);
for (auto c : components)
{
RCLCPP_DEBUG(this->getLogger(), "- Component %s", c->getName().c_str());
LOG_DEBUG("- Component {}", c->getName().c_str());
}
throw std::runtime_error(
@@ -74,17 +75,16 @@ void ISmaccComponent::requiresComponent(
if (requiredComponentStorage == nullptr && throwExceptionIfNotExist)
{
RCLCPP_DEBUG_STREAM(
this->getLogger(), std::string("Required component with name: '") + name + "'" +
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << std::string("Required component with name: '") + name + "'" +
demangleSymbol(typeid(TComponent).name()) +
" not found. Available components:");
" not found. Available components:"; return _oss.str(); }());
std::vector<std::shared_ptr<ISmaccComponent>> components;
this->owner_->getComponents(components);
for (auto c : components)
{
RCLCPP_DEBUG(this->getLogger(), " - Component %s", c->getName().c_str());
LOG_DEBUG(" - Component {}", c->getName().c_str());
}
throw std::runtime_error(

25
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_orthogonal_impl.hpp vendored
View File

@@ -20,6 +20,8 @@
#pragma once
#include <cassert>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client.hpp>
#include <smacc2/smacc_orthogonal.hpp>
@@ -35,10 +37,9 @@ bool ISmaccOrthogonal::requiresClient(SmaccClientType *& storage)
}
auto requiredClientName = demangledTypeName<SmaccClientType>();
RCLCPP_WARN_STREAM(
getLogger(), "Required client ["
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Required client ["
<< requiredClientName
<< "] not found in current orthogonal. Searching in other orthogonals.");
<< "] not found in current orthogonal. Searching in other orthogonals."; return _oss.str(); }());
for (auto & orthoentry : this->getStateMachine()->getOrthogonals())
{
@@ -47,19 +48,16 @@ bool ISmaccOrthogonal::requiresClient(SmaccClientType *& storage)
storage = dynamic_cast<SmaccClientType *>(client.get());
if (storage != nullptr)
{
RCLCPP_WARN_STREAM(
getLogger(),
"Required client [" << requiredClientName << "] found in other orthogonal.");
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Required client [" << requiredClientName << "] found in other orthogonal."; return _oss.str(); }());
return true;
}
}
}
RCLCPP_ERROR_STREAM(
getLogger(), "Required client ["
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Required client ["
<< requiredClientName
<< "] not found even in other orthogonals. Returning null pointer. If the "
"requested client is used may result in a segmentation fault.");
"requested client is used may result in a segmentation fault."; return _oss.str(); }());
return false;
}
@@ -68,9 +66,7 @@ void ISmaccOrthogonal::requiresComponent(SmaccComponentType *& storage)
{
if (stateMachine_ == nullptr)
{
RCLCPP_ERROR(
getLogger(),
"Cannot use the requiresComponent funcionality from an orthogonal before onInitialize");
LOG_ERROR("Cannot use the requiresComponent funcionality from an orthogonal before onInitialize");
}
else
{
@@ -177,10 +173,7 @@ public:
// return nullptr;
// }
RCLCPP_INFO(
getLogger(), "[%s] Creating client object, type:'%s' object tag: '%s'",
demangleType(typeid(*this)).c_str(), demangledTypeName<TClient>().c_str(),
demangledTypeName<TOrthogonal>().c_str());
LOG_INFO("[{}] Creating client object, type:'{}' object tag: '{}'", demangleType(typeid(*this)).c_str(), demangledTypeName<TClient>().c_str(), demangledTypeName<TOrthogonal>().c_str());
auto client = std::make_shared<ClientHandler<TOrthogonal, TClient>>(args...);
this->template assignClientToOrthogonal<TOrthogonal, TClient>(client.get());

15
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_state_impl.hpp vendored
View File

@@ -20,6 +20,7 @@
#pragma once
#include <smacc2/introspection/introspection.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client_behavior.hpp>
#include <smacc2/smacc_orthogonal.hpp>
#include <smacc2/smacc_state.hpp>
@@ -37,8 +38,7 @@ template <typename TOrthogonal, typename TBehavior, typename... Args>
std::shared_ptr<TBehavior> ISmaccState::configure(Args &&... args)
{
std::string orthogonalkey = demangledTypeName<TOrthogonal>();
RCLCPP_INFO(
getLogger(), "[%s] Configuring orthogonal: %s", THIS_STATE_NAME, orthogonalkey.c_str());
LOG_INFO("[{}] Configuring orthogonal: {}", THIS_STATE_NAME, orthogonalkey.c_str());
TOrthogonal * orthogonal = this->getOrthogonal<TOrthogonal>();
if (orthogonal != nullptr)
@@ -53,9 +53,7 @@ std::shared_ptr<TBehavior> ISmaccState::configure(Args &&... args)
}
else
{
RCLCPP_ERROR(
getLogger(), "[%s] Skipping client behavior creation in orthogonal [%s]. It does not exist.",
THIS_STATE_NAME, orthogonalkey.c_str());
LOG_ERROR("[{}] Skipping client behavior creation in orthogonal [{}]. It does not exist.", THIS_STATE_NAME, orthogonalkey.c_str());
return nullptr;
}
}
@@ -80,11 +78,8 @@ void ISmaccState::requiresClient(SmaccClientType *& storage)
if (storage != nullptr) return;
}
RCLCPP_ERROR(
getLogger(),
"[%s] Client of type '%s' not found in any orthogonal of the current state machine. This may "
"produce a segmentation fault if the returned reference is used.",
sname, demangleSymbol<SmaccClientType>().c_str());
LOG_ERROR("[{}] Client of type '{}' not found in any orthogonal of the current state machine. This may "
"produce a segmentation fault if the returned reference is used.", sname, demangleSymbol<SmaccClientType>().c_str());
}
//-------------------------------------------------------------------------------------------------------

156
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp vendored
View File

@@ -21,6 +21,7 @@
#pragma once
#include <memory>
#include "hivecore_logger/logger.hpp"
#include <sstream>
#include <string>
@@ -63,12 +64,9 @@ TOrthogonal * ISmaccStateMachine::getOrthogonal()
if (it != orthogonals_.end())
{
RCLCPP_DEBUG(
getLogger(),
"Orthogonal %s resource is being required from some state, client or component. Found "
LOG_DEBUG("Orthogonal {} resource is being required from some state, client or component. Found "
"resource in "
"cache.",
orthogonalkey.c_str());
"cache.", orthogonalkey.c_str());
ret = dynamic_cast<TOrthogonal *>(it->second.get());
return ret;
}
@@ -82,7 +80,7 @@ TOrthogonal * ISmaccStateMachine::getOrthogonal()
ss << " - " << orthogonal.first << std::endl;
}
RCLCPP_WARN_STREAM(getLogger(), ss.str());
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << ss.str(); return _oss.str(); }());
return nullptr;
}
@@ -104,20 +102,19 @@ void ISmaccStateMachine::createOrthogonal()
ret->setStateMachine(this);
RCLCPP_INFO(getLogger(), "%s Orthogonal is created", orthogonalkey.c_str());
LOG_INFO("{} Orthogonal is created", orthogonalkey.c_str());
}
else
{
RCLCPP_WARN_STREAM(
getLogger(), "There were already one existing orthogonal of type "
<< orthogonalkey.c_str() << ". Skipping creation orthogonal request. ");
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "There were already one existing orthogonal of type "
<< orthogonalkey.c_str() << ". Skipping creation orthogonal request. "; return _oss.str(); }());
std::stringstream ss;
ss << "The existing orthogonals are the following: " << std::endl;
for (auto & orthogonal : orthogonals_)
{
ss << " - " << orthogonal.first << std::endl;
}
RCLCPP_WARN_STREAM(getLogger(), ss.str());
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << ss.str(); return _oss.str(); }());
}
//this->unlockStateMachine("create orthogonal");
}
@@ -126,9 +123,7 @@ void ISmaccStateMachine::createOrthogonal()
template <typename SmaccComponentType>
void ISmaccStateMachine::requiresComponent(SmaccComponentType *& storage, bool throwsException)
{
RCLCPP_DEBUG(
getLogger(), "component %s is required",
demangleSymbol(typeid(SmaccComponentType).name()).c_str());
LOG_DEBUG("component {} is required", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
for (auto ortho : this->orthogonals_)
@@ -143,9 +138,7 @@ void ISmaccStateMachine::requiresComponent(SmaccComponentType *& storage, bool t
}
}
RCLCPP_WARN(
getLogger(), "component %s is required but it was not found in any orthogonal",
demangleSymbol(typeid(SmaccComponentType).name()).c_str());
LOG_WARN("component {} is required but it was not found in any orthogonal", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
if (throwsException)
throw std::runtime_error("component is required but it was not found in any orthogonal");
@@ -188,10 +181,8 @@ void ISmaccStateMachine::postEvent(EventType * ev, EventLifeTime evlifetime)
(stateMachineCurrentAction == StateMachineInternalAction::STATE_EXITING ||
stateMachineCurrentAction == StateMachineInternalAction::TRANSITIONING))
{
RCLCPP_WARN_STREAM(
getLogger(),
"[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
<< eventtypename);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
<< eventtypename; return _oss.str(); }());
return;
// in this case we may lose/skip events, if this is not right for some cases we should create a
// queue to lock the events during the transitions. This issues appeared when a client
@@ -203,7 +194,7 @@ void ISmaccStateMachine::postEvent(EventType * ev, EventLifeTime evlifetime)
// we reach this place. Now, we propagate the events to all the state state reactors to generate
// some more events
RCLCPP_DEBUG_STREAM(getLogger(), "[PostEvent entry point] " << eventtypename);
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[PostEvent entry point] " << eventtypename; return _oss.str(); }());
for (auto currentState : currentState_)
{
propagateEventToStateReactors(currentState, ev);
@@ -216,7 +207,7 @@ template <typename EventType>
void ISmaccStateMachine::postEvent(EventLifeTime evlifetime)
{
auto evname = smacc2::introspection::demangleSymbol<EventType>();
RCLCPP_INFO_STREAM(getLogger(), "Event: " << evname);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event: " << evname; return _oss.str(); }());
auto * ev = new EventType();
this->postEvent(ev, evlifetime);
}
@@ -247,7 +238,7 @@ bool ISmaccStateMachine::getGlobalSMData(std::string name, T & ret)
}
catch (boost::bad_any_cast & ex)
{
RCLCPP_ERROR(getLogger(), "bad any cast: %s", ex.what());
LOG_ERROR("bad any cast: {}", ex.what());
success = false;
}
}
@@ -282,9 +273,7 @@ void ISmaccStateMachine::mapBehavior()
{
std::string stateFieldName = demangleSymbol(typeid(StateField).name());
std::string behaviorType = demangleSymbol(typeid(BehaviorType).name());
RCLCPP_INFO(
getLogger(), "Mapping state field '%s' to stateReactor '%s'", stateFieldName.c_str(),
behaviorType.c_str());
LOG_INFO("Mapping state field '{}' to stateReactor '{}'", stateFieldName.c_str(), behaviorType.c_str());
SmaccClientBehavior * globalreference;
if (!this->getGlobalSMData(stateFieldName, globalreference))
{
@@ -433,12 +422,8 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
std::is_base_of<ISmaccOrthogonal, TSmaccObjectType>::value ||
std::is_base_of<ISmaccStateMachine, TSmaccObjectType>::value)
{
RCLCPP_INFO(
getLogger(),
"[StateMachine] Long life-time SMACC signal subscription created. Subscriber is %s. Callback "
"is: %s",
demangledTypeName<TSmaccObjectType>().c_str(),
demangleSymbol(typeid(callback).name()).c_str());
LOG_INFO("[StateMachine] Long life-time SMACC signal subscription created. Subscriber is {}. Callback "
"is: {}", demangledTypeName<TSmaccObjectType>().c_str(), demangleSymbol(typeid(callback).name()).c_str());
connection = binder.bindaux(signal, callback, object, nullptr);
}
@@ -447,10 +432,7 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
std::is_base_of<StateReactor, TSmaccObjectType>::value ||
std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value)
{
RCLCPP_INFO(
getLogger(),
"[StateMachine] Life-time constrained SMACC signal subscription created. Subscriber is %s",
demangledTypeName<TSmaccObjectType>().c_str());
LOG_INFO("[StateMachine] Life-time constrained SMACC signal subscription created. Subscriber is {}", demangledTypeName<TSmaccObjectType>().c_str());
std::shared_ptr<CallbackCounterSemaphore> callbackCounterSemaphore;
if (stateCallbackConnections.count(object))
@@ -469,9 +451,7 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
}
else // state life-time objects
{
RCLCPP_WARN(
getLogger(),
"[StateMachine] Connecting signal to an unknown object with unknown lifetime "
LOG_WARN("[StateMachine] Connecting signal to an unknown object with unknown lifetime "
"behavior. An exception may occur if the object is destroyed during the execution.");
connection = binder.bindaux(signal, callback, object, nullptr);
@@ -485,11 +465,8 @@ void ISmaccStateMachine::notifyOnStateEntryStart(StateType * state)
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
RCLCPP_DEBUG(
getLogger(),
"[State Machine] Initializing a new state '%s' and updating current state. Getting state "
"meta-information. number of orthogonals: %ld",
demangleSymbol(typeid(StateType).name()).c_str(), this->orthogonals_.size());
LOG_DEBUG("[State Machine] Initializing a new state '{}' and updating current state. Getting state "
"meta-information. number of orthogonals: {}", demangleSymbol(typeid(StateType).name()).c_str(), this->orthogonals_.size());
stateSeqCounter_++;
currentState_.push_back(state);
@@ -499,14 +476,12 @@ void ISmaccStateMachine::notifyOnStateEntryStart(StateType * state)
template <typename StateType>
void ISmaccStateMachine::notifyOnStateEntryEnd(StateType *)
{
RCLCPP_INFO(
getLogger(), "[%s] State OnEntry code finished.",
demangleSymbol(typeid(StateType).name()).c_str());
LOG_INFO("[{}] State OnEntry code finished.", demangleSymbol(typeid(StateType).name()).c_str());
auto currentState = this->currentState_.back();
for (auto pair : this->orthogonals_)
{
RCLCPP_DEBUG(getLogger(), "Orthogonal onEntry: %s.", pair.second->getName().c_str());
LOG_DEBUG("Orthogonal onEntry: {}.", pair.second->getName().c_str());
auto & orthogonal = pair.second;
try
{
@@ -514,46 +489,37 @@ void ISmaccStateMachine::notifyOnStateEntryEnd(StateType *)
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Orthogonal %s] Exception on Entry - continuing with next orthogonal. Exception info: %s",
pair.second->getName().c_str(), e.what());
LOG_ERROR("[Orthogonal {}] Exception on Entry - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
}
}
for (auto & sr : currentState->getStateReactors())
{
auto srname = smacc2::demangleSymbol(typeid(*sr).name());
RCLCPP_INFO_STREAM(getLogger(), "State reactor onEntry: " << srname);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "State reactor onEntry: " << srname; return _oss.str(); }());
try
{
sr->onEntry();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on Entry - continuing with next state reactor. Exception "
"info: %s",
srname.c_str(), e.what());
LOG_ERROR("[State Reactor {}] Exception on Entry - continuing with next state reactor. Exception "
"info: {}", srname.c_str(), e.what());
}
}
for (auto & eg : currentState->getEventGenerators())
{
auto egname = smacc2::demangleSymbol(typeid(*eg).name());
RCLCPP_INFO_STREAM(getLogger(), "Event generator onEntry: " << egname);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event generator onEntry: " << egname; return _oss.str(); }());
try
{
eg->onEntry();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Event generator %s] Exception on Entry - continuing with next state reactor. Exception "
"info: %s",
egname.c_str(), e.what());
LOG_ERROR("[Event generator {}] Exception on Entry - continuing with next state reactor. Exception "
"info: {}", egname.c_str(), e.what());
}
}
@@ -594,10 +560,10 @@ void ISmaccStateMachine::notifyOnStateExitting(StateType * state)
{
stateMachineCurrentAction = StateMachineInternalAction::STATE_EXITING;
auto fullname = demangleSymbol(typeid(StateType).name());
RCLCPP_WARN_STREAM(getLogger(), "Exiting state: " << fullname);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Exiting state: " << fullname; return _oss.str(); }());
// this->set_parameter("destroyed", true);
RCLCPP_INFO_STREAM(getLogger(), "Notification state exit: leaving state " << state);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Notification state exit: leaving state " << state; return _oss.str(); }());
for (auto pair : this->orthogonals_)
{
auto & orthogonal = pair.second;
@@ -607,46 +573,37 @@ void ISmaccStateMachine::notifyOnStateExitting(StateType * state)
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Orthogonal %s] Exception onExit - continuing with next orthogonal. Exception info: %s",
pair.second->getName().c_str(), e.what());
LOG_ERROR("[Orthogonal {}] Exception onExit - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
}
}
for (auto & sr : state->getStateReactors())
{
auto srname = smacc2::demangleSymbol(typeid(*sr).name());
RCLCPP_INFO_STREAM(getLogger(), "State reactor OnExit: " << srname);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "State reactor OnExit: " << srname; return _oss.str(); }());
try
{
sr->onExit();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
"info: %s",
srname.c_str(), e.what());
LOG_ERROR("[State Reactor {}] Exception on OnExit - continuing with next state reactor. Exception "
"info: {}", srname.c_str(), e.what());
}
}
for (auto & eg : state->getEventGenerators())
{
auto egname = smacc2::demangleSymbol(typeid(*eg).name());
RCLCPP_INFO_STREAM(getLogger(), "Event generator OnExit: " << egname);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event generator OnExit: " << egname; return _oss.str(); }());
try
{
eg->onExit();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
"info: %s",
egname.c_str(), e.what());
LOG_ERROR("[State Reactor {}] Exception on OnExit - continuing with next state reactor. Exception "
"info: {}", egname.c_str(), e.what());
}
}
}
@@ -659,9 +616,9 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
signalDetector_->notifyStateExited(state);
auto fullname = demangleSymbol(typeid(StateType).name());
RCLCPP_WARN_STREAM(getLogger(), "Exiting state: " << fullname);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Exiting state: " << fullname; return _oss.str(); }());
RCLCPP_INFO_STREAM(getLogger(), "Notification state disposing: leaving state" << state);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Notification state disposing: leaving state" << state; return _oss.str(); }());
for (auto pair : this->orthogonals_)
{
auto & orthogonal = pair.second;
@@ -671,46 +628,37 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Orthogonal %s] Exception onDispose - continuing with next orthogonal. Exception info: %s",
pair.second->getName().c_str(), e.what());
LOG_ERROR("[Orthogonal {}] Exception onDispose - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
}
}
for (auto & sr : state->getStateReactors())
{
auto srname = smacc2::demangleSymbol(typeid(*sr).name()).c_str();
RCLCPP_INFO(getLogger(), "State reactor disposing: %s", srname);
LOG_INFO("State reactor disposing: {}", srname);
try
{
this->disconnectSmaccSignalObject(sr.get());
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
"info: %s",
srname, e.what());
LOG_ERROR("[State Reactor {}] Exception on OnDispose - continuing with next state reactor. Exception "
"info: {}", srname, e.what());
}
}
for (auto & eg : state->getEventGenerators())
{
auto egname = smacc2::demangleSymbol(typeid(*eg).name()).c_str();
RCLCPP_INFO(getLogger(), "Event generator disposing: %s", egname);
LOG_INFO("Event generator disposing: {}", egname);
try
{
this->disconnectSmaccSignalObject(eg.get());
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
"info: %s",
egname, e.what());
LOG_ERROR("[State Reactor {}] Exception on OnDispose - continuing with next state reactor. Exception "
"info: {}", egname, e.what());
}
}
@@ -718,7 +666,7 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
currentState_.pop_back();
// then call exit state
RCLCPP_WARN_STREAM(getLogger(), "State exit: " << fullname);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "State exit: " << fullname; return _oss.str(); }());
stateMachineCurrentAction = StateMachineInternalAction::TRANSITIONING;
this->unlockStateMachine("state exit");
@@ -727,9 +675,7 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
template <typename EventType>
void ISmaccStateMachine::propagateEventToStateReactors(ISmaccState * st, EventType * ev)
{
RCLCPP_DEBUG(
getLogger(), "PROPAGATING EVENT [%s] TO SRs [%s]: ", demangleSymbol<EventType>().c_str(),
st->getClassName().c_str());
LOG_DEBUG("PROPAGATING EVENT [{}] TO SRs [{}]: ", demangleSymbol<EventType>().c_str(), st->getClassName().c_str());
for (auto & sb : st->getStateReactors())
{
sb->notifyEvent(ev);

13
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_state_reactor_impl.hpp vendored
View File

@@ -20,6 +20,8 @@
#pragma once
#include <smacc2/introspection/introspection.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_state_reactor.hpp>
namespace smacc2
@@ -41,8 +43,7 @@ void StateReactor::setOutputEvent()
{
this->postEventFn = [this]()
{
RCLCPP_INFO_STREAM(
this->getLogger(), "[State Reactor Base] postingfn posting event: " << demangleSymbol<TEv>());
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[State Reactor Base] postingfn posting event: " << demangleSymbol<TEv>(); return _oss.str(); }());
auto * ev = new TEv();
this->ownerState->getStateMachine().postEvent(ev);
};
@@ -84,9 +85,7 @@ void StateReactorHandler::addInputEvent()
StateReactorCallbackFunctor functor;
functor.fn = [this](std::shared_ptr<smacc2::StateReactor> sr)
{
RCLCPP_INFO(
nh_->get_logger(), "[%s] State Reactor adding input event: %s",
srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
LOG_INFO("[{}] State Reactor adding input event: {}", srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
sr->addInputEvent<TEv>();
};
@@ -105,9 +104,7 @@ void StateReactorHandler::setOutputEvent()
StateReactorCallbackFunctor functor;
functor.fn = [this](std::shared_ptr<smacc2::StateReactor> sr)
{
RCLCPP_INFO(
nh_->get_logger(), "[%s] State Reactor setting output event: %s",
srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
LOG_INFO("[{}] State Reactor setting output event: {}", srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
sr->setOutputEvent<TEv>();
};

34
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/introspection/smacc_state_machine_info.hpp vendored
View File

@@ -21,6 +21,8 @@
#pragma once
#include <map>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <memory>
#include <string>
#include <vector>
@@ -160,9 +162,7 @@ template <typename T>
typename enable_if<boost::mpl::is_sequence<T>>::type processTransitions(
std::shared_ptr<SmaccStateInfo> & sourceState)
{
RCLCPP_INFO(
globalNh_->get_logger(), "State %s Walker has transition list",
sourceState->fullStateName.c_str());
LOG_INFO("State {} Walker has transition list", sourceState->fullStateName.c_str());
using boost::mpl::_1;
using wrappedList = typename boost::mpl::transform<T, add_type_wrapper<_1>>::type;
boost::mpl::for_each<wrappedList>(AddTransition(sourceState));
@@ -184,9 +184,7 @@ void processTransition(
smacc2::Transition<Ev, Dst, Tag> * t, std::shared_ptr<SmaccStateInfo> & sourceState)
{
auto transitionTypeInfo = TypeInfo::getTypeInfoFromType<smacc2::Transition<Ev, Dst, Tag>>();
RCLCPP_INFO(
globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
demangleSymbol(typeid(Ev).name()).c_str());
LOG_INFO("State {} Walker transition: {}", sourceState->toShortName().c_str(), demangleSymbol(typeid(Ev).name()).c_str());
processTransitionAux(t, sourceState, false, transitionTypeInfo);
}
@@ -195,9 +193,7 @@ void processTransitionAux(
smacc2::Transition<Ev, Dst, Tag> *, std::shared_ptr<SmaccStateInfo> & sourceState, bool history,
TypeInfo::Ptr & transitionTypeInfo)
{
RCLCPP_INFO(
globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
demangleSymbol(typeid(Ev).name()).c_str());
LOG_INFO("State {} Walker transition: {}", sourceState->toShortName().c_str(), demangleSymbol(typeid(Ev).name()).c_str());
std::string transitionTag;
std::string transitionType;
@@ -205,7 +201,7 @@ void processTransitionAux(
{
transitionTag = demangleSymbol<Tag>();
transitionType = getTransitionType<Tag>();
RCLCPP_DEBUG_STREAM(globalNh_->get_logger(), "TRANSITION TYPE:" << transitionType);
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "TRANSITION TYPE:" << transitionType; return _oss.str(); }());
}
else
{
@@ -214,7 +210,7 @@ void processTransitionAux(
automaticTransitionType<Ev>(transitionType);
}
RCLCPP_INFO_STREAM(globalNh_->get_logger(), "Transition tag: " << transitionTag);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Transition tag: " << transitionTag; return _oss.str(); }());
if (!sourceState->stateMachine_->containsState<Dst>())
{
@@ -350,17 +346,14 @@ template <typename T>
typename std::enable_if<HasOnDefinition<T>::value, void>::type CallOnDefinition()
{
/* something when T has toString ... */
RCLCPP_INFO_STREAM(
globalNh_->get_logger(), "EXECUTING ONDEFINITION: " << demangleSymbol(typeid(T).name()));
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "EXECUTING ONDEFINITION: " << demangleSymbol(typeid(T).name()); return _oss.str(); }());
T::staticConfigure();
}
template <typename T>
typename std::enable_if<!HasOnDefinition<T>::value, void>::type CallOnDefinition()
{
RCLCPP_INFO_STREAM(
globalNh_->get_logger(),
"static OnDefinition: dont exist for " << demangleSymbol(typeid(T).name()));
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "static OnDefinition: dont exist for " << demangleSymbol(typeid(T).name()); return _oss.str(); }());
/* something when T has toString ... */
}
@@ -371,7 +364,7 @@ template <typename> typename EvType, typename Tag, typename DestinyState >
typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState, Tag>>>::type
processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
{
RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
LOG_INFO("DETECTED COMPLEX TRANSITION **************");
// RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
<< " of type: " << demangledTypeName<T>());
TTransition<EvType<TevSource>,DestinyState, Tag> *dummy;
@@ -383,7 +376,7 @@ template <typename> typename EvType, typename DestinyState >
typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState>>>::type
processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
{
RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
LOG_INFO("DETECTED COMPLEX TRANSITION **************");
// RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
<< " of type: " << demangledTypeName<T>());
TTransition<EvType<TevSource>,DestinyState> *dummy;
@@ -480,7 +473,7 @@ std::shared_ptr<SmaccStateInfo> SmaccStateMachineInfo::createState(
auto * statetid = &(typeid(StateType));
auto demangledName = demangledTypeName<StateType>();
RCLCPP_INFO_STREAM(getLogger(), "Creating State Info: " << demangledName);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Creating State Info: " << demangledName; return _oss.str(); }());
auto state = std::make_shared<SmaccStateInfo>(statetid, parent, thisptr);
state->demangledStateName = demangledName;
@@ -510,8 +503,7 @@ std::shared_ptr<SmaccStateInfo> SmaccStateInfo::createChildState()
auto childState = this->stateMachine_->createState<StateType>(realparentState);
RCLCPP_WARN_STREAM(
getLogger(), "Real parent state> " << demangleSymbol<typename StateType::TContext>());
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Real parent state> " << demangleSymbol<typename StateType::TContext>(); return _oss.str(); }());
/*auto contextInfo = TypeInfo::getTypeInfoFromType<InitialStateType>();
auto parentState2= getState<InitialStateType::TContext>();

55
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/smacc_state_base.hpp vendored
View File

@@ -20,6 +20,8 @@
#pragma once
#include <smacc2/introspection/state_traits.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_event_generator.hpp>
#include <smacc2/smacc_state.hpp>
#include <smacc2/smacc_state_machine.hpp>
@@ -76,7 +78,7 @@ public:
logger_.reset(new rclcpp::Logger(
rclcpp::get_logger(smacc2::utils::cleanShortTypeName(typeid(MostDerived)))));
RCLCPP_INFO(getLogger(), "[%s] Creating state ", STATE_NAME);
LOG_INFO("[{}] Creating state ", STATE_NAME);
this->set_context(ctx.pContext_);
node_ = this->getStateMachine().getNode();
@@ -345,7 +347,7 @@ public:
{
this->postEvent<EvLoopEnd<MostDerived>>();
}
RCLCPP_INFO(getLogger(), "[%s] POST THROW CONDITION", STATE_NAME);
LOG_INFO("[{}] POST THROW CONDITION", STATE_NAME);
}
void throwSequenceFinishedEvent() { this->postEvent<EvSequenceFinished<MostDerived>>(); }
@@ -406,38 +408,32 @@ private:
SmaccStateInfo::staticBehaviorInfo[tindex] = std::vector<ClientBehaviorInfoEntry>();
SmaccStateInfo::staticBehaviorInfo[tindex].push_back(bhinfo);
RCLCPP_INFO_STREAM(
rclcpp::get_logger("static"), "[states walking] State "
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[states walking] State "
<< smacc2::utils::cleanShortTypeName(*tindex)
<< "client behavior count: "
<< SmaccStateInfo::staticBehaviorInfo[tindex].size());
<< SmaccStateInfo::staticBehaviorInfo[tindex].size(); return _oss.str(); }());
}
void entryStateInternal()
{
// finally we go to the derived state onEntry Function
RCLCPP_DEBUG(getLogger(), "[%s] State object created. Initializating...", STATE_NAME);
LOG_DEBUG("[{}] State object created. Initializating...", STATE_NAME);
this->getStateMachine().notifyOnStateEntryStart(static_cast<MostDerived *>(this));
RCLCPP_DEBUG_STREAM(
getLogger(), "[" << smacc2::utils::cleanShortTypeName(typeid(MostDerived)).c_str()
<< "] creating ros subnode");
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << smacc2::utils::cleanShortTypeName(typeid(MostDerived)).c_str()
<< "] creating ros subnode"; return _oss.str(); }());
// before dynamic runtimeConfigure, we execute the staticConfigure behavior configurations
{
RCLCPP_DEBUG(getLogger(), "[%s] -- STATIC STATE DESCRIPTION --", STATE_NAME);
LOG_DEBUG("[{}] -- STATIC STATE DESCRIPTION --", STATE_NAME);
for (const auto & clientBehavior : SmaccStateInfo::staticBehaviorInfo)
{
RCLCPP_DEBUG(
getLogger(), "[%s] client behavior info: %s", STATE_NAME,
demangleSymbol(clientBehavior.first->name()).c_str());
LOG_DEBUG("[{}] client behavior info: {}", STATE_NAME, demangleSymbol(clientBehavior.first->name()).c_str());
for (auto & cbinfo : clientBehavior.second)
{
RCLCPP_DEBUG(
getLogger(), "[%s] client behavior: %s", STATE_NAME,
demangleSymbol(cbinfo.behaviorType->name()).c_str());
LOG_DEBUG("[{}] client behavior: {}", STATE_NAME, demangleSymbol(cbinfo.behaviorType->name()).c_str());
}
}
@@ -448,45 +444,36 @@ private:
for (auto & ortho : this->getStateMachine().getOrthogonals())
{
RCLCPP_INFO(
getLogger(), "[%s] Initializing orthogonal: %s", STATE_NAME,
demangleSymbol(typeid(*ortho.second).name()).c_str());
LOG_INFO("[{}] Initializing orthogonal: {}", STATE_NAME, demangleSymbol(typeid(*ortho.second).name()).c_str());
ortho.second->initState(this);
}
RCLCPP_DEBUG_STREAM(
getLogger(), "Finding static client behaviors. State Database: "
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Finding static client behaviors. State Database: "
<< SmaccStateInfo::staticBehaviorInfo.size() << ". Current state "
<< cleanShortTypeName(*tindex)
<< " cbs: " << SmaccStateInfo::staticBehaviorInfo[tindex].size());
<< " cbs: " << SmaccStateInfo::staticBehaviorInfo[tindex].size(); return _oss.str(); }());
for (auto & bhinfo : staticDefinedBehaviors)
{
RCLCPP_DEBUG(
getLogger(), "[%s] Creating static client behavior: %s", STATE_NAME,
demangleSymbol(bhinfo.behaviorType->name()).c_str());
LOG_DEBUG("[{}] Creating static client behavior: {}", STATE_NAME, demangleSymbol(bhinfo.behaviorType->name()).c_str());
bhinfo.factoryFunction(this);
}
for (auto & sr : staticDefinedStateReactors)
{
RCLCPP_DEBUG(
getLogger(), "[%s] Creating static state reactor: %s", STATE_NAME,
sr->stateReactorType->getFullName().c_str());
LOG_DEBUG("[{}] Creating static state reactor: {}", STATE_NAME, sr->stateReactorType->getFullName().c_str());
sr->factoryFunction(this);
}
for (auto & eg : staticDefinedEventGenerators)
{
RCLCPP_DEBUG(
getLogger(), "[%s] Creating static event generator: %s", STATE_NAME,
eg->eventGeneratorType->getFullName().c_str());
LOG_DEBUG("[{}] Creating static event generator: {}", STATE_NAME, eg->eventGeneratorType->getFullName().c_str());
eg->factoryFunction(this);
}
RCLCPP_DEBUG(getLogger(), "[%s] ---- END STATIC DESCRIPTION", STATE_NAME);
LOG_DEBUG("[{}] ---- END STATIC DESCRIPTION", STATE_NAME);
}
RCLCPP_DEBUG(getLogger(), "[%s] State runtime configuration", STATE_NAME);
LOG_DEBUG("[{}] State runtime configuration", STATE_NAME);
auto * derivedthis = static_cast<MostDerived *>(this);
@@ -500,7 +487,7 @@ private:
this->getStateMachine().notifyOnRuntimeConfigurationFinished(derivedthis);
RCLCPP_DEBUG(getLogger(), "[%s] State OnEntry", STATE_NAME);
LOG_DEBUG("[{}] State OnEntry", STATE_NAME);
static_cast<MostDerived *>(this)->onEntry();

9
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/smacc_state_machine_base.hpp vendored
View File

@@ -19,6 +19,7 @@
******************************************************************************************************************/
#pragma once
#include <smacc2/common.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_state_base.hpp>
#include <smacc2/smacc_state_machine.hpp>
@@ -74,15 +75,15 @@ public:
// this is before because this creates orthogonals
this->onInitialize();
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Introspecting state machine via typeWalker");
LOG_INFO("[SmaccStateMachine] Introspecting state machine via typeWalker");
this->buildStateMachineInfo<InitialStateType>();
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] initiate_impl");
LOG_INFO("[SmaccStateMachine] initiate_impl");
auto shortname = smacc2::utils::cleanShortTypeName(typeid(DerivedStateMachine));
this->initializeROS(shortname);
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Initializing ROS communication mechanisms");
LOG_INFO("[SmaccStateMachine] Initializing ROS communication mechanisms");
this->onInitialized();
// publish startup state machine transition info
@@ -90,7 +91,7 @@ public:
transitionInfo->destinyState = this->stateMachineInfo_->getState<InitialStateType>();
this->publishTransition(*transitionInfo);
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Initializing state machine");
LOG_INFO("[SmaccStateMachine] Initializing state machine");
sc::state_machine<DerivedStateMachine, InitialStateType, SmaccAllocator>::initiate();
}
};

5
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/smacc_transition.hpp vendored
View File

@@ -21,6 +21,7 @@
#pragma once
#include <smacc2/introspection/introspection.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/introspection/state_traits.hpp>
namespace smacc2
{
@@ -40,7 +41,7 @@ private:
{
static boost::statechart::result react_without_action(State & stt)
{
RCLCPP_DEBUG(stt.getLogger(), "[Smacc Transition] REACT WITHOUT ACTION");
LOG_DEBUG("[Smacc Transition] REACT WITHOUT ACTION");
typedef smacc2::Transition<Event, Destination, Tag, TransitionContext, pTransitionAction>
Transtype;
TRANSITION_TAG mock;
@@ -52,7 +53,7 @@ private:
static boost::statechart::result react_with_action(State & stt, const Event & evt)
{
RCLCPP_DEBUG(stt.getLogger(), "[Smacc Transition] REACT WITH ACTION AND EVENT");
LOG_DEBUG("[Smacc Transition] REACT WITH ACTION AND EVENT");
typedef smacc2::Transition<Event, Destination, Tag, TransitionContext, pTransitionAction>
Transtype;
TRANSITION_TAG mock;

26
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/callback_counter_semaphore.cpp vendored
View File

@@ -22,6 +22,7 @@
#include <iostream>
#include <mutex>
#include <rclcpp/rclcpp.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/callback_counter_semaphore.hpp>
#include <thread>
@@ -35,24 +36,18 @@ CallbackCounterSemaphore::CallbackCounterSemaphore(std::string name, int count)
bool CallbackCounterSemaphore::acquire()
{
std::unique_lock<std::mutex> lock(mutex_);
RCLCPP_DEBUG(
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] acquire callback %s %ld", name_.c_str(),
(long)this);
LOG_DEBUG("[CallbackCounterSemaphore] acquire callback {} {}", name_.c_str(), (long)this);
if (finalized)
{
RCLCPP_DEBUG(
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback rejected %s %ld",
name_.c_str(), (long)this);
LOG_DEBUG("[CallbackCounterSemaphore] callback rejected {} {}", name_.c_str(), (long)this);
return false;
}
++count_;
cv_.notify_one();
RCLCPP_DEBUG(
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback accepted %s %ld", name_.c_str(),
(long)this);
LOG_DEBUG("[CallbackCounterSemaphore] callback accepted {} {}", name_.c_str(), (long)this);
return true;
}
@@ -62,9 +57,7 @@ void CallbackCounterSemaphore::release()
--count_;
cv_.notify_one();
RCLCPP_DEBUG(
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback finished %s %ld", name_.c_str(),
(long)this);
LOG_DEBUG("[CallbackCounterSemaphore] callback finished {} {}", name_.c_str(), (long)this);
}
void CallbackCounterSemaphore::finalize()
@@ -83,9 +76,7 @@ void CallbackCounterSemaphore::finalize()
}
connections_.clear();
RCLCPP_DEBUG(
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callbacks finalized %s %ld",
name_.c_str(), (long)this);
LOG_DEBUG("[CallbackCounterSemaphore] callbacks finalized {} {}", name_.c_str(), (long)this);
}
void CallbackCounterSemaphore::addConnection(boost::signals2::connection conn)
@@ -94,10 +85,7 @@ void CallbackCounterSemaphore::addConnection(boost::signals2::connection conn)
if (finalized)
{
RCLCPP_DEBUG(
rclcpp::get_logger(name_),
"[CallbackCounterSemaphore] ignoring adding callback, already finalized %s %ld",
name_.c_str(), (long)this);
LOG_DEBUG("[CallbackCounterSemaphore] ignoring adding callback, already finalized {} {}", name_.c_str(), (long)this);
return;
}

11
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_bases/smacc_ros_launch_client.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <smacc2/client_bases/smacc_ros_launch_client.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -46,8 +48,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
std::launch::async,
[=]()
{
RCLCPP_WARN_STREAM(
rclcpp::get_logger("smacc2"), "[ClRosLaunch static] starting ros launch thread ");
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static] starting ros launch thread "; return _oss.str(); }());
std::stringstream cmd;
cmd << "ros2 launch " << packageName << " " << launchFileName;
@@ -70,8 +71,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
ss << buffer.data();
if (cancelled) // break_pipe
{
RCLCPP_WARN_STREAM(
rclcpp::get_logger("smacc2"), "[ClRosLaunch static] cancelling ros launch thread ");
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static] cancelling ros launch thread "; return _oss.str(); }());
// break pipe
pclose(pipe.release());
@@ -86,8 +86,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
}
result = ss.str();
RCLCPP_WARN_STREAM(
rclcpp::get_logger("smacc2"), "[ClRosLaunch static]] RESULT = \n " << ss.str());
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static]] RESULT = \n " << ss.str(); return _oss.str(); }());
return result;
});
}

18
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_bases/smacc_ros_launch_client_2.cpp vendored
View File

@@ -18,6 +18,8 @@
*
******************************************************************************************************************/
#include <errno.h> // Agrega esta inclusión
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <fcntl.h> // Agrega esta inclusión
#include <poll.h>
#include <signal.h>
@@ -75,7 +77,7 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
std::launch::async,
[packageName, launchFileName, cancelCondition, client]()
{
RCLCPP_WARN_STREAM(rclcpp::get_logger("smacc2"), "[ClRosLaunch2] Starting ros launch thread");
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch2] Starting ros launch thread"; return _oss.str(); }());
std::stringstream cmd;
cmd << "ros2 launch " << packageName << " " << launchFileName;
@@ -119,7 +121,7 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
else
{
// Error de lectura
RCLCPP_ERROR(rclcpp::get_logger("smacc2"), "Error de lectura en pipe");
LOG_ERROR("Error de lectura en pipe");
break;
}
}
@@ -138,25 +140,23 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
if (WIFEXITED(status))
{
int exit_status = WEXITSTATUS(status);
RCLCPP_INFO(
rclcpp::get_logger("smacc2"), "Child process exited with status: %d", exit_status);
LOG_INFO("Child process exited with status: {}", exit_status);
}
else if (WIFSIGNALED(status))
{
int term_signal = WTERMSIG(status);
RCLCPP_WARN(
rclcpp::get_logger("smacc2"), "Child process terminated by signal: %d", term_signal);
LOG_WARN("Child process terminated by signal: {}", term_signal);
}
}
else
{
RCLCPP_ERROR(rclcpp::get_logger("smacc2"), "Error waiting for child process.");
LOG_ERROR("Error waiting for child process.");
}
pclose(child.pipe);
close(child.pipe->_fileno); // Close pipe file descriptor but not processes
RCLCPP_WARN_STREAM(rclcpp::get_logger("smacc2"), "[ClRosLaunch2] RESULT:\n" << result);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch2] RESULT:\n" << result; return _oss.str(); }());
// Devuelve una std::future con el resultado
// return std::async(std::launch::async, [result]() { return result; });
@@ -251,7 +251,7 @@ void killProcessesRecursive(pid_t pid)
int res = kill(pid, SIGTERM);
if (res == 0)
{
RCLCPP_FATAL(rclcpp::get_logger("smacc2"), "Killed process %d", pid);
LOG_FATAL("Killed process {}", pid);
}
}

20
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_ros_launch.cpp vendored
View File

@@ -18,6 +18,8 @@
*
******************************************************************************************************************/
#include <smacc2/client_behaviors/cb_ros_launch.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -47,7 +49,7 @@ void onOrthogonalAllocation()
void CbRosLaunch::onEntry()
{
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch] OnEntry");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] OnEntry"; return _oss.str(); }());
std::string packageName, launchFileName;
if (launchFileName_ && packageName_)
@@ -63,9 +65,8 @@ void CbRosLaunch::onEntry()
breakfunction = []() -> bool { return false; };
}
RCLCPP_INFO_STREAM(
getLogger(), "[CbRosLaunch] launching: " << *packageName_ << " , " << *launchFileName_
<< "LaunchMode: " << (int)launchMode_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] launching: " << *packageName_ << " , " << *launchFileName_
<< "LaunchMode: " << (int)launchMode_; return _oss.str(); }());
auto fut = smacc2::client_bases::ClRosLaunch::executeRosLaunch(
*packageName_, *launchFileName_, breakfunction);
@@ -77,22 +78,19 @@ void CbRosLaunch::onEntry()
}
else
{
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch] finding Ros Launch client");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] finding Ros Launch client"; return _oss.str(); }());
this->requiresClient(client_);
if (client_ != nullptr)
{
RCLCPP_INFO_STREAM(
getLogger(), "[CbRosLaunch] launching from client: " << client_->packageName_ << " , "
<< client_->launchFileName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] launching from client: " << client_->packageName_ << " , "
<< client_->launchFileName_; return _oss.str(); }());
client_->launch();
}
else
{
RCLCPP_ERROR(
getLogger(),
"[CbRosLaunch] Inccorrect ros launch operation. No Ros Launch client specified neither "
LOG_ERROR("[CbRosLaunch] Inccorrect ros launch operation. No Ros Launch client specified neither "
"package/roslaunch file path.");
}
}

24
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_ros_launch_2.cpp vendored
View File

@@ -18,6 +18,8 @@
*
******************************************************************************************************************/
#include <smacc2/client_behaviors/cb_ros_launch_2.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -48,7 +50,7 @@ void onOrthogonalAllocation()
void CbRosLaunch2::onEntry()
{
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] OnEntry");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] OnEntry"; return _oss.str(); }());
std::string packageName, launchFileName;
if (
@@ -59,9 +61,8 @@ void CbRosLaunch2::onEntry()
breakfunction = std::bind(&CbRosLaunch2::isShutdownRequested, this);
RCLCPP_INFO_STREAM(
getLogger(), "[CbRosLaunch2] launching: " << *packageName_ << " , " << *launchFileName_
<< "LaunchMode: " << (int)launchMode_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] launching: " << *packageName_ << " , " << *launchFileName_
<< "LaunchMode: " << (int)launchMode_; return _oss.str(); }());
auto result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
// this->result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
@@ -76,30 +77,25 @@ void CbRosLaunch2::onEntry()
client_->launchFileName_ = *launchFileName_;
}
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] finding Ros Launch client");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] finding Ros Launch client"; return _oss.str(); }());
// this->requiresClient(client_);
if (client_ != nullptr)
{
RCLCPP_INFO_STREAM(
getLogger(), "[CbRosLaunch2] launching from client: " << client_->packageName_ << " , "
<< client_->launchFileName_);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] launching from client: " << client_->packageName_ << " , "
<< client_->launchFileName_; return _oss.str(); }());
client_->launch();
}
else
{
RCLCPP_ERROR(
getLogger(),
"[CbRosLaunch2] Inccorrect ros launch operation. No Ros Launch client specified neither "
LOG_ERROR("[CbRosLaunch2] Inccorrect ros launch operation. No Ros Launch client specified neither "
"package/roslaunch file path.");
}
}
else
{
RCLCPP_ERROR(
getLogger(),
"[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
LOG_ERROR("[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
}
}

37
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_sequence.cpp vendored
View File

@@ -20,6 +20,7 @@
#include <functional>
#include <rclcpp/rclcpp.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/client_behaviors/cb_sequence.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
@@ -31,30 +32,24 @@ CbSequence::CbSequence() {}
void CbSequence::recursiveConsumeNext()
{
RCLCPP_INFO(
getLogger(), "[SequenceNode %ld] next onEntry: %ld", (long)this, sequenceNodes_.size());
LOG_INFO("[SequenceNode {}] next onEntry: {}", (long)this, sequenceNodes_.size());
auto first = sequenceNodes_.front();
auto onDelayedConfigureFn = first;
RCLCPP_INFO(getLogger(), "[SequenceNode %ld] Behavior on delayed sequence configure", (long)this);
LOG_INFO("[SequenceNode {}] Behavior on delayed sequence configure", (long)this);
bh_ = onDelayedConfigureFn();
std::string currentNodeName = bh_->getName();
RCLCPP_INFO(getLogger(), "[SequenceNode %ld] Subscribing OnSuccess", (long)this);
LOG_INFO("[SequenceNode {}] Subscribing OnSuccess", (long)this);
this->conn_ = bh_->onSuccess(&CbSequence::onSubNodeSuccess, this);
this->conn2_ = bh_->onFailure(&CbSequence::onSubNodeAbort, this);
RCLCPP_INFO(
getLogger(), "[SequenceNode %ld] subnode %s on entry", (long)this, currentNodeName.c_str());
LOG_INFO("[SequenceNode {}] subnode {} on entry", (long)this, currentNodeName.c_str());
bh_->executeOnEntry();
RCLCPP_INFO(
getLogger(), "[SequenceNode %ld] subnode %s on entry finished", (long)this,
currentNodeName.c_str());
LOG_INFO("[SequenceNode {}] subnode {} on entry finished", (long)this, currentNodeName.c_str());
bh_->waitOnEntryThread(false); // we do not request to finish to keep on subscriptions
RCLCPP_INFO(
getLogger(), "[SequenceNode %ld] subnode %s thread finished", (long)this,
currentNodeName.c_str());
LOG_INFO("[SequenceNode {}] subnode {} thread finished", (long)this, currentNodeName.c_str());
}
void CbSequence::onEntry()
@@ -69,10 +64,7 @@ void CbSequence::onEntry()
}
rclcpp::sleep_for(std::chrono::milliseconds(200));
RCLCPP_INFO_THROTTLE(
getLogger(), *(getNode()->get_clock()), 1000,
"[CbSequence %ld] Waiting for subnodes to finish %ld. Current head Behavior: %s ", (long)this,
sequenceNodes_.size(), demangleType(&typeid(*bh_)).c_str());
LOG_INFO_THROTTLE(1000, "[CbSequence {}] Waiting for subnodes to finish {}. Current head Behavior: {} ", (long)this, sequenceNodes_.size(), demangleType(&typeid(*bh_)).c_str());
if (consume_)
{
@@ -92,34 +84,31 @@ void CbSequence::onEntry()
if (sequenceNodes_.empty())
{
RCLCPP_INFO(getLogger(), "[CbSequence %ld] All subnodes finished", (long)this);
LOG_INFO("[CbSequence {}] All subnodes finished", (long)this);
this->postSuccessEvent();
}
else
{
RCLCPP_INFO(getLogger(), "[CbSequence %ld] Aborting", (long)this);
LOG_INFO("[CbSequence {}] Aborting", (long)this);
this->postFailureEvent();
}
}
void CbSequence::onSubNodeSuccess()
{
RCLCPP_INFO(
getLogger(), "[CbSequence %ld] Success NextCbSequence %ld", (long)this, sequenceNodes_.size());
LOG_INFO("[CbSequence {}] Success NextCbSequence {}", (long)this, sequenceNodes_.size());
consume_++;
}
void CbSequence::onSubNodeAbort()
{
RCLCPP_INFO(
getLogger(), "[CbSequence %ld] Abort NextCbSequence %ld", (long)this, sequenceNodes_.size());
LOG_INFO("[CbSequence {}] Abort NextCbSequence {}", (long)this, sequenceNodes_.size());
// bh_->waitOnEntryThread();
this->conn_.disconnect();
this->conn2_.disconnect();
this->postFailureEvent();
RCLCPP_INFO(
getLogger(), "[CbSequence %ld] Abort NextCbSequence requesting for force finish", (long)this);
LOG_INFO("[CbSequence {}] Abort NextCbSequence requesting for force finish", (long)this);
this->requestForceFinish();
consume_++;
}

9
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_wait_action_server.cpp vendored
View File

@@ -19,6 +19,7 @@
******************************************************************************************************************/
#include <smacc2/client_behaviors/cb_wait_action_server.hpp>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -37,24 +38,24 @@ void CbWaitActionServer::onEntry()
while (!this->isShutdownRequested() && !found && (getNode()->now() - starttime) < timeout_)
{
std::shared_ptr<rclcpp_action::ClientBase> client_base = client_->getClientBase();
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] waiting action server..");
LOG_INFO("[CbWaitActionServer] waiting action server..");
found = client_base->wait_for_action_server(std::chrono::milliseconds(1000));
}
if (found)
{
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] action server already available");
LOG_INFO("[CbWaitActionServer] action server already available");
this->postSuccessEvent();
}
else
{
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] action server not found, timeout");
LOG_INFO("[CbWaitActionServer] action server not found, timeout");
this->postFailureEvent();
}
}
else
{
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] there is no action client in this orthogonal");
LOG_INFO("[CbWaitActionServer] there is no action client in this orthogonal");
this->postFailureEvent();
}
}

7
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_wait_node.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <smacc2/client_behaviors/cb_wait_node.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -42,10 +44,9 @@ void CbWaitNode::onEntry()
}
auto totalstr = ss.str();
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] on entry, listing nodes (" << nodenames.size() << ")"
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] on entry, listing nodes (" << nodenames.size() << ")"
<< std::endl
<< totalstr);
<< totalstr; return _oss.str(); }());
rate_.sleep();
}

12
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_wait_topic.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <smacc2/client_behaviors/cb_wait_topic.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -33,9 +35,7 @@ void CbWaitTopic::onEntry()
bool found = false;
while (!this->isShutdownRequested() && !found)
{
RCLCPP_INFO_STREAM_THROTTLE(
getLogger(), *(getNode()->get_clock()), 1000,
"[" << getName() << "] waiting topic: " << topicName_);
LOG_INFO_THROTTLE(1000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] waiting topic: " << topicName_; return _oss.str(); }());
std::stringstream ss;
auto topicnames = getNode()->get_topic_names_and_types();
@@ -49,11 +49,9 @@ void CbWaitTopic::onEntry()
}
auto totalstr = ss.str();
RCLCPP_INFO_STREAM_THROTTLE(
getLogger(), *(getNode()->get_clock()), 5000,
"[" << getName() << "] still waiting topic " << topicName_ << ", listing topics ("
LOG_INFO_THROTTLE(5000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] still waiting topic " << topicName_ << ", listing topics ("
<< topicnames.size() << ")" << std::endl
<< totalstr);
<< totalstr; return _oss.str(); }());
rate_.sleep();
}

8
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/introspection/string_type_walker.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <smacc2/introspection/smacc_type_info.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <algorithm>
#include <boost/algorithm/string.hpp>
@@ -243,7 +245,7 @@ TypeInfo::Ptr TypeInfo::getTypeInfoFromString(std::string inputtext)
<< " - " << originalinputtext << std::endl;
auto strmsg = ss.str();
RCLCPP_WARN_STREAM(globalNh_->get_logger(), strmsg);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << strmsg; return _oss.str(); }());
}
// std::sort(types.begin(), types.end(),[](auto& a, auto& b)
@@ -333,7 +335,7 @@ TypeInfo::Ptr TypeInfo::getTypeInfoFromString(std::string inputtext)
//RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Current Database");
for (auto & en : typeInfoDatabase)
{
if (globalNh_ != nullptr) RCLCPP_DEBUG_STREAM(globalNh_->get_logger(), "- " << en.first);
if (globalNh_ != nullptr) LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "- " << en.first; return _oss.str(); }());
}
typeInfoDatabase[originalinputtext] = roottype;
@@ -369,7 +371,7 @@ TypeInfo::Ptr TypeInfo::getTypeInfoFromString(std::string inputtext)
}
auto strmsg = ss.str();
RCLCPP_WARN_STREAM(globalNh_->get_logger(), strmsg);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << strmsg; return _oss.str(); }());
}
return roottype;

42
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/orthogonal.cpp vendored
View File

@@ -19,6 +19,7 @@
******************************************************************************************************************/
#include <smacc2/impl/smacc_state_machine_impl.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_client_behavior.hpp>
#include <smacc2/smacc_orthogonal.hpp>
#include <smacc2/smacc_tracing/smacc_tracing.hpp>
@@ -52,9 +53,7 @@ void ISmaccOrthogonal::addClientBehavior(std::shared_ptr<smacc2::ISmaccClientBeh
if (clBehavior != nullptr)
{
RCLCPP_INFO(
getLogger(), "[Orthogonal: %s] Adding client behavior: %s.", this->getName().c_str(),
clBehavior->getName().c_str());
LOG_INFO("[Orthogonal: {}] Adding client behavior: {}.", this->getName().c_str(), clBehavior->getName().c_str());
clBehavior->stateMachine_ = this->getStateMachine();
clBehavior->currentOrthogonal = this;
clBehavior->currentState = clBehavior->stateMachine_->getCurrentState();
@@ -63,8 +62,7 @@ void ISmaccOrthogonal::addClientBehavior(std::shared_ptr<smacc2::ISmaccClientBeh
}
else
{
RCLCPP_INFO(
getLogger(), "[Orthogonal: %s] No client behaviors in this state.", this->getName().c_str());
LOG_INFO("[Orthogonal: {}] No client behaviors in this state.", this->getName().c_str());
}
}
@@ -72,9 +70,7 @@ void ISmaccOrthogonal::onInitialize() {}
void ISmaccOrthogonal::initState(ISmaccState * state)
{
RCLCPP_INFO(
getLogger(), "[Orthogonal: %s] InitState: %s.", this->getName().c_str(),
state->getClassName().c_str());
LOG_INFO("[Orthogonal: {}] InitState: {}.", this->getName().c_str(), state->getClassName().c_str());
clientBehaviors_.push_back(std::vector<std::shared_ptr<smacc2::ISmaccClientBehavior>>());
}
@@ -86,9 +82,7 @@ void ISmaccOrthogonal::runtimeConfigure()
for (auto & clBehavior : clientBehaviors_.back())
{
RCLCPP_INFO(
getLogger(), "[Orthogonal: %s] runtimeConfigure(), current behavior: %s.",
this->getName().c_str(), clBehavior->getName().c_str());
LOG_INFO("[Orthogonal: {}] runtimeConfigure(), current behavior: {}.", this->getName().c_str(), clBehavior->getName().c_str());
clBehavior->runtimeConfigure();
}
@@ -102,8 +96,7 @@ void ISmaccOrthogonal::onEntry()
{
for (auto & clBehavior : clientBehaviors_.back())
{
RCLCPP_INFO(
getLogger(), "[Orthogonal: %s] OnEntry, current Behavior: %s", orthogonalName, cbName);
LOG_INFO("[Orthogonal: {}] OnEntry, current Behavior: {}", orthogonalName, cbName);
try
{
@@ -112,21 +105,16 @@ void ISmaccOrthogonal::onEntry()
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[ClientBehavior: %s] Exception on Entry - continuing with next client behavior. "
LOG_ERROR("[ClientBehavior: {}] Exception on Entry - continuing with next client behavior. "
"Exception info: "
"%s",
cbName, e.what());
"{}", cbName, e.what());
}
TRACEPOINT(smacc2_client_behavior_on_entry_end, statename, orthogonalName, cbName);
}
}
else
{
RCLCPP_INFO(
getLogger(), "[Orthogonal: %s] OnEntry -> empty orthogonal (no client behavior).",
orthogonalName);
LOG_INFO("[Orthogonal: {}] OnEntry -> empty orthogonal (no client behavior).", orthogonalName);
}
}
@@ -138,8 +126,7 @@ void ISmaccOrthogonal::onExit()
{
for (auto & clBehavior : clientBehaviors_.back())
{
RCLCPP_INFO(
getLogger(), "[Orthogonal: %s] OnExit, current Behavior: %s", orthogonalName, cbName);
LOG_INFO("[Orthogonal: {}] OnExit, current Behavior: {}", orthogonalName, cbName);
try
{
TRACEPOINT(smacc2_client_behavior_on_exit_start, statename, orthogonalName, cbName);
@@ -147,18 +134,15 @@ void ISmaccOrthogonal::onExit()
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[ClientBehavior: %s] Exception onExit - continuing with next client behavior. Exception "
"info: %s",
cbName, e.what());
LOG_ERROR("[ClientBehavior: {}] Exception onExit - continuing with next client behavior. Exception "
"info: {}", cbName, e.what());
}
TRACEPOINT(smacc2_client_behavior_on_exit_end, statename, orthogonalName, cbName);
}
}
else
{
RCLCPP_INFO(getLogger(), "[Orthogonal %s] OnExit().", orthogonalName);
LOG_INFO("[Orthogonal {}] OnExit().", orthogonalName);
}
}

63
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/signal_detector.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <signal.h>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <limits>
#include <memory>
#include <thread>
@@ -87,8 +89,7 @@ void SignalDetector::findUpdatableClientsAndComponents()
if (updatableClient != nullptr)
{
RCLCPP_DEBUG_STREAM(
getLogger(), "Adding updatable client: " << demangleType(typeid(updatableClient)));
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable client: " << demangleType(typeid(updatableClient)); return _oss.str(); }());
this->updatableClients_.push_back(updatableClient);
}
@@ -100,9 +101,7 @@ void SignalDetector::findUpdatableClientsAndComponents()
auto updatableComponent = dynamic_cast<ISmaccUpdatable *>(componententry.get());
if (updatableComponent != nullptr)
{
RCLCPP_DEBUG_STREAM(
getLogger(),
"Adding updatable component: " << demangleType(typeid(*updatableComponent)));
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable component: " << demangleType(typeid(*updatableComponent)); return _oss.str(); }());
this->updatableClients_.push_back(updatableComponent);
}
}
@@ -133,9 +132,7 @@ void SignalDetector::findUpdatableStateElements(ISmaccState * currentState)
if (updatableClientBehavior != nullptr)
{
RCLCPP_DEBUG_STREAM(
getLogger(),
"Adding updatable behavior: " << demangleType(typeid(updatableClientBehavior)));
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable behavior: " << demangleType(typeid(updatableClientBehavior)); return _oss.str(); }());
updatableElements.push_back(updatableClientBehavior);
}
}
@@ -153,9 +150,7 @@ void SignalDetector::findUpdatableStateElements(ISmaccState * currentState)
ISmaccUpdatable * updatableStateReactor = dynamic_cast<ISmaccUpdatable *>(sr.get());
if (updatableStateReactor != nullptr)
{
RCLCPP_DEBUG_STREAM(
getLogger(),
"Adding updatable stateReactorr: " << demangleType(typeid(updatableStateReactor)));
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable stateReactorr: " << demangleType(typeid(updatableStateReactor)); return _oss.str(); }());
updatableElements.push_back(updatableStateReactor);
}
}
@@ -166,9 +161,7 @@ void SignalDetector::findUpdatableStateElements(ISmaccState * currentState)
ISmaccUpdatable * updatableEventGenerator = dynamic_cast<ISmaccUpdatable *>(eg.get());
if (updatableEventGenerator != nullptr)
{
RCLCPP_DEBUG_STREAM(
getLogger(),
"Adding updatable eventGenerator: " << demangleType(typeid(updatableEventGenerator)));
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable eventGenerator: " << demangleType(typeid(updatableEventGenerator)); return _oss.str(); }());
updatableElements.push_back(updatableEventGenerator);
}
}
@@ -236,7 +229,7 @@ void SignalDetector::pollOnce()
//smaccStateMachine_->lockStateMachine("update behaviors");
this->findUpdatableClientsAndComponents();
RCLCPP_DEBUG_STREAM(getLogger(), "Updatable clients: " << this->updatableClients_.size());
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Updatable clients: " << this->updatableClients_.size(); return _oss.str(); }());
if (this->updatableClients_.size())
{
@@ -246,9 +239,7 @@ void SignalDetector::pollOnce()
auto updatableElementName = demangleType(typeid(*updatableClient)).c_str();
try
{
RCLCPP_DEBUG_STREAM(
node->get_logger(),
"[PollOnce] update client call: " << demangleType(typeid(*updatableClient)));
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[PollOnce] update client call: " << demangleType(typeid(*updatableClient)); return _oss.str(); }());
TRACEPOINT(smacc2_state_update_start, updatableElementName);
updatableClient->executeUpdate(smaccStateMachine_->getNode());
@@ -256,9 +247,7 @@ void SignalDetector::pollOnce()
}
catch (const std::exception & e)
{
RCLCPP_ERROR_STREAM(
node->get_logger(),
"Error in updatable elemnent " << updatableElementName << ": " << e.what() << '\n');
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Error in updatable elemnent " << updatableElementName << ": " << e.what() << '\n'; return _oss.str(); }());
}
}
}
@@ -274,8 +263,7 @@ void SignalDetector::pollOnce()
this->smaccStateMachine_->stateMachineCurrentAction !=
StateMachineInternalAction::STATE_EXITING)
{
RCLCPP_DEBUG_STREAM(
getLogger(), "Updatable states: " << this->updatableStateElements_.size());
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Updatable states: " << this->updatableStateElements_.size(); return _oss.str(); }());
for (auto stateElement : this->updatableStateElements_)
{
@@ -284,9 +272,8 @@ void SignalDetector::pollOnce()
std::string updatableElementName = demangleType(typeid(*udpatableStateElement));
auto updatableElementNameCstr = updatableElementName.c_str();
RCLCPP_DEBUG_STREAM(
getLogger(), "pollOnce update client behavior call: "
<< demangleType(typeid(*udpatableStateElement)));
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "pollOnce update client behavior call: "
<< demangleType(typeid(*udpatableStateElement)); return _oss.str(); }());
TRACEPOINT(smacc2_state_update_start, updatableElementNameCstr);
udpatableStateElement->executeUpdate(smaccStateMachine_->getNode());
@@ -297,7 +284,7 @@ void SignalDetector::pollOnce()
}
catch (std::exception & ex)
{
RCLCPP_ERROR(getLogger(), "Exception during Signal Detector update loop. %s.", ex.what());
LOG_ERROR("Exception during Signal Detector update loop. {}.", ex.what());
}
auto nh = this->getNode();
@@ -325,33 +312,27 @@ void SignalDetector::pollingLoop()
if (!nh->get_parameter("signal_detector_loop_freq", this->loop_rate_hz))
{
RCLCPP_WARN(
getLogger(),
"Signal Detector frequency (ros param signal_detector_loop_freq) was not set, using default "
LOG_WARN("Signal Detector frequency (ros param signal_detector_loop_freq) was not set, using default "
"frequency: "
"%lf",
this->loop_rate_hz);
"{}", this->loop_rate_hz);
}
else
{
RCLCPP_WARN(
getLogger(), "Signal Detector frequency (ros param signal_detector_loop_freq): %lf",
this->loop_rate_hz);
LOG_WARN("Signal Detector frequency (ros param signal_detector_loop_freq): {}", this->loop_rate_hz);
}
nh->set_parameter(rclcpp::Parameter("signal_detector_loop_freq", this->loop_rate_hz));
RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Loop rate hz:" << loop_rate_hz);
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Loop rate hz:" << loop_rate_hz; return _oss.str(); }());
if (this->executionModel_ == ExecutionModel::SINGLE_THREAD_SPINNER)
{
RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Running in single-threaded mode.");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Running in single-threaded mode."; return _oss.str(); }());
rclcpp::Rate r(loop_rate_hz);
while (rclcpp::ok() && !end_)
{
RCLCPP_INFO_STREAM_THROTTLE(
getLogger(), *getNode()->get_clock(), 10000, "[SignalDetector] Heartbeat");
LOG_INFO_THROTTLE(10000, "{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Heartbeat"; return _oss.str(); }());
pollOnce();
rclcpp::spin_some(nh);
r.sleep();
@@ -359,7 +340,7 @@ void SignalDetector::pollingLoop()
}
else
{
RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Running in multi-threaded mode.");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Running in multi-threaded mode."; return _oss.str(); }());
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(nh);
@@ -369,7 +350,7 @@ void SignalDetector::pollingLoop()
void onSigQuit(int)
{
RCLCPP_INFO(rclcpp::get_logger("SMACC"), "SignalDetector: SIGQUIT received.");
LOG_INFO("SignalDetector: SIGQUIT received.");
exit(0);
}

54
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/smacc_client_async_behavior.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
using namespace std::chrono_literals;
@@ -26,14 +28,14 @@ namespace smacc2
{
void SmaccAsyncClientBehavior::executeOnEntry()
{
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] asynchronous onEntry thread started");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] asynchronous onEntry thread started"; return _oss.str(); }());
this->onEntryThread_ = std::async(
std::launch::async,
[=]
{
this->onEntry();
this->postFinishEventFn_();
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] asynchronous onEntry thread finished");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] asynchronous onEntry thread finished"; return _oss.str(); }());
return 0;
});
}
@@ -61,39 +63,30 @@ void SmaccAsyncClientBehavior::waitFutureIfNotFinished(
else
{
// in progress
RCLCPP_INFO_STREAM(
getLogger(),
"[" << getName()
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
<< "] fut valid but waiting for asynchronous onEntry thread to finish: state: "
<< (int)status);
<< (int)status; return _oss.str(); }());
}
}
else
{
RCLCPP_INFO_STREAM(
getLogger(),
"[" << getName()
<< "] waiting future onEntryThread. It was not even created. Skipping wait.");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
<< "] waiting future onEntryThread. It was not even created. Skipping wait."; return _oss.str(); }());
break;
}
// r.sleep();
rclcpp::sleep_for(100ms);
// rclcpp::spin_some(getNode());
RCLCPP_WARN_THROTTLE(
getLogger(), *(getNode()->get_clock()), 1000,
"[%s] waiting for finishing client behavior, before leaving the state. Is the client "
"behavior stuck? requesting force finish",
this->getName().c_str());
LOG_WARN_THROTTLE(1000, "[{}] waiting for finishing client behavior, before leaving the state. Is the client "
"behavior stuck? requesting force finish", this->getName().c_str());
if (requestFinish) requestForceFinish();
}
}
catch (const std::exception & e)
{
RCLCPP_DEBUG(
getLogger(),
"[SmaccAsyncClientBehavior] trying to join function, but it was already finished.");
LOG_DEBUG("[SmaccAsyncClientBehavior] trying to join function, but it was already finished.");
}
}
@@ -104,27 +97,25 @@ void SmaccAsyncClientBehavior::waitOnEntryThread(bool requestFinish)
void SmaccAsyncClientBehavior::executeOnExit()
{
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] onExit - join async onEntry thread");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] onExit - join async onEntry thread"; return _oss.str(); }());
this->waitOnEntryThread(true);
RCLCPP_INFO_STREAM(
getLogger(), "[" << getName() << "] onExit - Creating asynchronous onExit thread");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] onExit - Creating asynchronous onExit thread"; return _oss.str(); }());
this->onExitThread_ = std::async(
std::launch::async,
[=]
{
this->onExit();
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] asynchronous onExit done.");
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] asynchronous onExit done."; return _oss.str(); }());
return 0;
});
}
void SmaccAsyncClientBehavior::dispose()
{
RCLCPP_DEBUG_STREAM(
getLogger(), "[" << getName()
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
<< "] Destroying client behavior- Waiting finishing of asynchronous onExit "
"thread");
"thread"; return _oss.str(); }());
try
{
if (this->onExitThread_)
@@ -136,16 +127,13 @@ void SmaccAsyncClientBehavior::dispose()
}
catch (...)
{
RCLCPP_DEBUG(
getLogger(),
"[SmaccAsyncClientBehavior] trying to Join onExit function, but it was already "
LOG_DEBUG("[SmaccAsyncClientBehavior] trying to Join onExit function, but it was already "
"finished.");
}
RCLCPP_DEBUG_STREAM(
getLogger(), "[" << getName()
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
<< "] Destroying client behavior- onExit thread finished. Proccedding "
"destruction.");
"destruction."; return _oss.str(); }());
}
SmaccAsyncClientBehavior::~SmaccAsyncClientBehavior() {}
@@ -156,9 +144,7 @@ void SmaccAsyncClientBehavior::postFailureEvent() { postFailureEventFn_(); }
void SmaccAsyncClientBehavior::requestForceFinish()
{
RCLCPP_FATAL_STREAM_THROTTLE(
getLogger(), *(getNode()->get_clock()), 1000,
"[" << getName() << "] " << ((uint64_t)this) << " requestForceFinish");
LOG_WARN_THROTTLE(1000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] " << ((uint64_t)this) << " requestForceFinish"; return _oss.str(); }());
isShutdownRequested_ = true;
}

7
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/smacc_client_behavior.cpp vendored
View File

@@ -19,18 +19,17 @@
******************************************************************************************************************/
#include <smacc2/smacc_client_behavior.hpp>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
void SmaccClientBehavior::onEntry()
{
RCLCPP_DEBUG(
getLogger(), "[%s] Default empty SmaccClientBehavior onEntry", this->getName().c_str());
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onEntry", this->getName().c_str());
}
void SmaccClientBehavior::onExit()
{
RCLCPP_DEBUG(
getLogger(), "[%s] Default empty SmaccClientBehavior onExit", this->getName().c_str());
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onExit", this->getName().c_str());
}
} // namespace smacc2

17
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/smacc_client_behavior_base.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <smacc2/smacc_client_behavior.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -30,9 +32,8 @@ ISmaccClientBehavior::ISmaccClientBehavior()
ISmaccClientBehavior::~ISmaccClientBehavior()
{
RCLCPP_WARN_STREAM(
getLogger(), "[" << getName() << "]"
<< " Client behavior deallocated");
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "]"
<< " Client behavior deallocated"; return _oss.str(); }());
}
std::string ISmaccClientBehavior::getName() const { return demangleSymbol(typeid(*this).name()); }
@@ -57,22 +58,18 @@ rclcpp::Logger ISmaccClientBehavior::getLogger() const
void ISmaccClientBehavior::runtimeConfigure()
{
RCLCPP_DEBUG(
getLogger(), "[%s] Default empty SmaccClientBehavior runtimeConfigure()",
this->getName().c_str());
LOG_DEBUG("[{}] Default empty SmaccClientBehavior runtimeConfigure()", this->getName().c_str());
}
void ISmaccClientBehavior::executeOnEntry()
{
RCLCPP_DEBUG(
getLogger(), "[%s] Default empty SmaccClientBehavior onEntry()", this->getName().c_str());
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onEntry()", this->getName().c_str());
this->onEntry();
}
void ISmaccClientBehavior::executeOnExit()
{
RCLCPP_DEBUG(
getLogger(), "[%s] Default empty SmaccClientBehavior onExit()", this->getName().c_str());
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onExit()", this->getName().c_str());
this->onExit();
}

23
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/smacc_state.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <smacc2/smacc_state.hpp>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_state_machine.hpp>
namespace smacc2
@@ -27,7 +29,7 @@ std::string ISmaccState::getClassName() { return demangleSymbol(typeid(*this).na
void ISmaccState::notifyTransitionFromTransitionTypeInfo(TypeInfo::Ptr & transitionType)
{
RCLCPP_INFO_STREAM(getLogger(), "TRANSITION RULE TRIGGERED: " << transitionType->getFullName());
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "TRANSITION RULE TRIGGERED: " << transitionType->getFullName(); return _oss.str(); }());
//auto currstateinfo = this->getStateMachine().getCurrentStateInfo();
auto currstateinfo = this->stateInfo_;
@@ -48,12 +50,10 @@ void ISmaccState::notifyTransitionFromTransitionTypeInfo(TypeInfo::Ptr & transit
}
// debug information if not found
RCLCPP_ERROR_STREAM(
getLogger(),
"Transition happened, from current state "
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Transition happened, from current state "
<< currstateinfo->getDemangledFullName()
<< " but there is not any transitioninfo match available to publish transition: "
<< transitionType->getFullName());
<< transitionType->getFullName(); return _oss.str(); }());
std::stringstream ss;
auto stateinfo = currstateinfo;
@@ -61,32 +61,31 @@ void ISmaccState::notifyTransitionFromTransitionTypeInfo(TypeInfo::Ptr & transit
for (auto & transition : currstateinfo->transitions_)
{
std::string transitionCandidateName = transition.transitionTypeInfo->getFullName();
RCLCPP_ERROR_STREAM(getLogger(), "- candidate transition: " << transitionCandidateName);
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "- candidate transition: " << transitionCandidateName; return _oss.str(); }());
}
RCLCPP_ERROR(getLogger(), "Ancestors candidates: ");
LOG_ERROR("Ancestors candidates: ");
std::list<const SmaccStateInfo *> ancestors;
stateinfo->getAncestors(ancestors);
for (auto & ancestor : ancestors)
{
RCLCPP_ERROR_STREAM(getLogger(), " * Ancestor " << ancestor->getDemangledFullName() << ":");
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << " * Ancestor " << ancestor->getDemangledFullName() << ":"; return _oss.str(); }());
for (auto & transition : ancestor->transitions_)
{
std::string transitionCandidateName = transition.transitionTypeInfo->getFullName();
RCLCPP_ERROR_STREAM(getLogger(), "- candidate transition: " << transitionCandidateName);
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "- candidate transition: " << transitionCandidateName; return _oss.str(); }());
if (transitionType->getFullName() == transitionCandidateName)
{
RCLCPP_ERROR(getLogger(), "GOTCHA");
LOG_ERROR("GOTCHA");
}
}
}
}
else
{
RCLCPP_ERROR_STREAM(
getLogger(), "Transition happened, but current state was not set. Transition candidates:");
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Transition happened, but current state was not set. Transition candidates:"; return _oss.str(); }());
}
}

27
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/smacc_state_machine.cpp vendored
View File

@@ -19,6 +19,8 @@
******************************************************************************************************************/
#include <rcl/time.h>
#include <sstream>
#include "hivecore_logger/logger.hpp"
#include <chrono>
#include <functional>
#include <smacc2/client_bases/smacc_action_client.hpp>
@@ -43,8 +45,7 @@ ISmaccStateMachine::ISmaccStateMachine(
// node_options.automatically_declare_parameters_from_overrides(true);
nh_ = rclcpp::Node::make_shared(stateMachineName, nodeOptions); //
RCLCPP_INFO_STREAM(
nh_->get_logger(), "Creating state machine base: " << nh_->get_fully_qualified_name());
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Creating state machine base: " << nh_->get_fully_qualified_name(); return _oss.str(); }());
signalDetector_ = signalDetector;
signalDetector_->initialize(this);
@@ -62,7 +63,7 @@ ISmaccStateMachine::ISmaccStateMachine(
}
else
{
RCLCPP_ERROR(nh_->get_logger(), "Incorrect run_mode value: %s", runMode.c_str());
LOG_ERROR("Incorrect run_mode value: {}", runMode.c_str());
}
}
else
@@ -73,12 +74,12 @@ ISmaccStateMachine::ISmaccStateMachine(
ISmaccStateMachine::~ISmaccStateMachine()
{
RCLCPP_INFO(nh_->get_logger(), "Finishing State Machine");
LOG_INFO("Finishing State Machine");
}
void ISmaccStateMachine::disconnectSmaccSignalObject(void * object_ptr)
{
RCLCPP_INFO(nh_->get_logger(), "[SmaccSignal] Object signal disconnecting %ld", (long)object_ptr);
LOG_INFO("[SmaccSignal] Object signal disconnecting {}", (long)object_ptr);
if (stateCallbackConnections.count(object_ptr) > 0)
{
auto callbackSemaphore = stateCallbackConnections[object_ptr];
@@ -87,7 +88,7 @@ void ISmaccStateMachine::disconnectSmaccSignalObject(void * object_ptr)
}
else
{
RCLCPP_INFO(nh_->get_logger(), "[SmaccSignal] No signals found %ld", (long)object_ptr);
LOG_INFO("[SmaccSignal] No signals found {}", (long)object_ptr);
}
}
@@ -111,9 +112,7 @@ void ISmaccStateMachine::updateStatusMessage()
if (currentStateInfo_ != nullptr)
{
RCLCPP_WARN_STREAM(
nh_->get_logger(),
"[StateMachine] Setting state active: " << currentStateInfo_->getFullPath());
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[StateMachine] Setting state active: " << currentStateInfo_->getFullPath(); return _oss.str(); }());
if (runMode_ == SMRunMode::DEBUG)
{
@@ -163,7 +162,7 @@ void ISmaccStateMachine::onInitialized()
void ISmaccStateMachine::initializeROS(std::string shortname)
{
RCLCPP_WARN_STREAM(nh_->get_logger(), "State machine base creation: " << shortname);
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "State machine base creation: " << shortname; return _oss.str(); }());
// STATE MACHINE TOPICS
stateMachinePub_ = nh_->create_publisher<smacc2_msgs::msg::SmaccStateMachine>(
shortname + "/smacc/state_machine_description", rclcpp::QoS(1));
@@ -185,9 +184,7 @@ void ISmaccStateMachine::getTransitionLogHistory(
const std::shared_ptr<smacc2_msgs::srv::SmaccGetTransitionHistory::Request> /*req*/,
std::shared_ptr<smacc2_msgs::srv::SmaccGetTransitionHistory::Response> res)
{
RCLCPP_WARN(
nh_->get_logger(), "Requesting transition log history, current size: %ld",
this->transitionLogHistory_.size());
LOG_WARN("Requesting transition log history, current size: {}", this->transitionLogHistory_.size());
res->history = this->transitionLogHistory_;
}
@@ -209,13 +206,13 @@ void ISmaccStateMachine::state_machine_visualization()
void ISmaccStateMachine::lockStateMachine(std::string msg)
{
RCLCPP_DEBUG(nh_->get_logger(), "-- locking SM: %s", msg.c_str());
LOG_DEBUG("-- locking SM: {}", msg.c_str());
m_mutex_.lock();
}
void ISmaccStateMachine::unlockStateMachine(std::string msg)
{
RCLCPP_DEBUG(nh_->get_logger(), "-- unlocking SM: %s", msg.c_str());
LOG_DEBUG("-- unlocking SM: {}", msg.c_str());
m_mutex_.unlock();
}

3
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/smacc_state_machine_info.cpp vendored
View File

@@ -19,6 +19,7 @@
******************************************************************************************************************/
#include <smacc2/introspection/introspection.hpp>
#include "hivecore_logger/logger.hpp"
#include <smacc2/smacc_state_machine.hpp>
namespace smacc2
@@ -212,7 +213,7 @@ void SmaccStateMachineInfo::assembleSMStructureMessage(ISmaccStateMachine * sm)
ss << "----------------------------------------------------------" << std::endl;
auto resumeMsg = ss.str();
RCLCPP_DEBUG(getLogger(), "%s", resumeMsg.c_str());
LOG_DEBUG("{}", resumeMsg.c_str());
stateMsgs.push_back(stateMsg);
}
}

3
src/thirdparty/SMACC2-humble/smacc2/src/smacc2/state_reactor.cpp vendored
View File

@@ -23,6 +23,7 @@
#include <smacc2/smacc_state.hpp>
#include <smacc2/smacc_state_reactor.hpp>
#include "rclcpp/rclcpp.hpp"
#include "hivecore_logger/logger.hpp"
namespace smacc2
{
@@ -48,7 +49,7 @@ void StateReactor::update()
{
if (this->triggers())
{
RCLCPP_INFO(getLogger(), "State reactor base REALLY TRIGGERS!!");
LOG_INFO("State reactor base REALLY TRIGGERS!!");
this->postEventFn();
}
}