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

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

Browse Source 
- Replace all RCLCPP_* logging macros with LOG_* hivecore macros across
  brain_node, cerebrum_node, cerebellum_node, skill_manager, and related files
- Add hivecore_logger_cpp dependency to CMakeLists.txt
- Initialize Logger with explicit LoggerOptions in brain_node.cpp
  (level, log_dir, fallback_dir, file size, queue, worker threads,
  console output, level sync, flush interval)
This commit is contained in:
david
2026-03-04 14:10:17 +08:00
parent 6709fd510c
commit a39f6a6d2e
12 changed files with 360 additions and 360 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/brain/CMakeLists.txt
View File

@@ -8,6 +8,10 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp 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)
find_package(rclcpp_components REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
@@ -44,6 +48,7 @@ target_include_directories(brain_node PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
interfaces/include
/opt/hivecore/logger-sdk/1.0.0/include
)
target_compile_features(brain_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
@@ -70,6 +75,7 @@ ament_target_dependencies(brain_node
target_link_libraries(brain_node
Boost::thread
yaml-cpp
hivecore_logger_cpp::hivecore_logger_cpp
)
install(TARGETS brain_node

88
src/brain/include/brain/action_registry.hpp
View File

@@ -11,6 +11,7 @@
#include <optional>
#include <rclcpp/rclcpp.hpp>
#include <hivecore_logger/logger.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
/**
@@ -144,11 +145,11 @@ public:
{
virtual ~EntryBase() = default;
/** @brief Send a goal using the entry's callbacks. */
virtual void send(rclcpp::Logger logger) = 0;
virtual void send() = 0;
/** @brief Check if server is available within timeout. */
virtual bool available(std::chrono::nanoseconds timeout) const = 0;
/** @brief Attempt to cancel last goal if present. */
virtual void cancel(rclcpp::Logger logger) = 0;
virtual void cancel() = 0;
};
template<typename ActionT>
@@ -178,16 +179,16 @@ public:
client = rclcpp_action::create_client<ActionT>(node, name);
}
void send(rclcpp::Logger logger) override
void send() override
{
if (!client) {return;}
RCLCPP_INFO(logger, "[ActionClientRegistry] waiting for server '%s'", name.c_str());
LOG_INFO("[ActionClientRegistry] waiting for server '{}'", name.c_str());
if (!client->wait_for_action_server(std::chrono::seconds(10))) {
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
LOG_ERROR("Action server '{}' not available", name.c_str());
return;
}
auto goal_msg = make_goal ? make_goal() : Goal{};
RCLCPP_INFO(logger, "[ActionClientRegistry] sending goal on '%s'", name.c_str());
LOG_INFO("[ActionClientRegistry] sending goal on '{}'", name.c_str());
auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
if (on_goal_response) {opts.goal_response_callback = on_goal_response;}
@@ -195,7 +196,7 @@ public:
if (on_result) {opts.result_callback = on_result;}
auto future_handle = client->async_send_goal(goal_msg, opts);
std::thread(
[future_handle, this, logger]() mutable {
[future_handle, this]() mutable {
if (future_handle.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
future_handle.wait();
}
@@ -203,11 +204,10 @@ public:
auto gh = future_handle.get();
if (gh) {
last_goal_handle = gh;
RCLCPP_INFO(logger, "Stored goal handle for '%s'", name.c_str());
LOG_INFO("Stored goal handle for '{}'", name.c_str());
}
} catch (const std::exception & e) {
RCLCPP_WARN(
logger, "Failed to obtain goal handle for '%s': %s", name.c_str(),
LOG_WARN("Failed to obtain goal handle for '{}': {}", name.c_str(),
e.what());
}
}).detach();
@@ -219,14 +219,14 @@ public:
return client->wait_for_action_server(timeout);
}
void cancel(rclcpp::Logger logger) override
void cancel() override
{
auto gh = last_goal_handle.lock();
if (!gh) {
RCLCPP_WARN(logger, "No active goal handle to cancel for '%s'", name.c_str());
LOG_WARN("No active goal handle to cancel for '{}'", name.c_str());
return;
}
RCLCPP_INFO(logger, "Cancelling goal on '%s'", name.c_str());
LOG_INFO("Cancelling goal on '{}'", name.c_str());
client->async_cancel_goal(gh);
}
};
@@ -250,7 +250,7 @@ public:
std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> on_goal_response = nullptr)
{
if (entries_.find(name) != entries_.end()) {
RCLCPP_WARN(node_->get_logger(), "[ActionClientRegistry] Overwriting existing client for '%s'", name.c_str());
LOG_WARN("[ActionClientRegistry] Overwriting existing client for '{}'", name.c_str());
}
entries_[name] =
std::make_unique<Entry<ActionT>>(
@@ -263,15 +263,15 @@ public:
* @param name Action client key.
* @param logger Logger instance.
*/
void send(const std::string & name, rclcpp::Logger logger)
void send(const std::string & name)
{
auto it = entries_.find(name);
if (it == entries_.end()) {
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
LOG_ERROR("No action client registered for '{}'", name.c_str());
return;
}
RCLCPP_INFO(logger, "Sending action '%s'", name.c_str());
it->second->send(logger);
LOG_INFO("Sending action '{}'", name.c_str());
it->second->send();
}
/**
@@ -279,14 +279,14 @@ public:
* @param name Action client key.
* @param logger Logger instance.
*/
void cancel(const std::string & name, rclcpp::Logger logger)
void cancel(const std::string & name)
{
auto it = entries_.find(name);
if (it == entries_.end()) {
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
LOG_ERROR("No action client registered for '{}'", name.c_str());
return;
}
it->second->cancel(logger);
it->second->cancel();
}
/**
@@ -333,22 +333,21 @@ public:
template<typename ActionT>
std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait(
const std::string & name,
rclcpp::Logger logger,
std::chrono::nanoseconds timeout = std::chrono::seconds(30))
{
auto it = entries_.find(name);
if (it == entries_.end()) {
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
LOG_ERROR("No action client registered for '{}'", name.c_str());
return std::nullopt;
}
auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
if (!e) {
RCLCPP_ERROR(logger, "Action client type mismatch for '%s'", name.c_str());
LOG_ERROR("Action client type mismatch for '{}'", name.c_str());
return std::nullopt;
}
if (!e->client) {return std::nullopt;}
if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
LOG_ERROR("Action server '{}' not available", name.c_str());
return std::nullopt;
}
auto goal = e->make_goal ? e->make_goal() : typename ActionT::Goal{};
@@ -357,7 +356,7 @@ public:
auto goal_future = e->client->async_send_goal(goal, opts);
auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", name.c_str());
LOG_ERROR("Timed out waiting for goal response on '{}'", name.c_str());
return std::nullopt;
}
@@ -365,11 +364,11 @@ public:
try {
goal_handle = goal_future.get();
} catch (const std::exception & ex) {
RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
LOG_ERROR("Failed to get goal handle for '{}': {}", name.c_str(), ex.what());
return std::nullopt;
}
if (!goal_handle) {
RCLCPP_WARN(logger, "Goal rejected for '%s'", name.c_str());
LOG_WARN("Goal rejected for '{}'", name.c_str());
return std::nullopt;
}
e->last_goal_handle = goal_handle;
@@ -377,18 +376,18 @@ public:
try {
e->on_goal_response(goal_handle);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", name.c_str(), ex.what());
LOG_WARN("Goal response callback threw for '{}': {}", name.c_str(), ex.what());
}
}
auto result_future = e->client->async_get_result(goal_handle);
auto deadline = std::chrono::steady_clock::now() + timeout;
if (result_future.wait_until(deadline) != std::future_status::ready) {
RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
LOG_ERROR("Timed out waiting for result on '{}', sending cancel", name.c_str());
try {
e->client->async_cancel_goal(goal_handle);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
LOG_WARN("Failed cancelling goal for '{}': {}", name.c_str(), ex.what());
}
return std::nullopt;
}
@@ -396,14 +395,14 @@ public:
try {
wrapped_result = result_future.get();
} catch (const std::exception & ex) {
RCLCPP_ERROR(logger, "Failed to get result for '%s': %s", name.c_str(), ex.what());
LOG_ERROR("Failed to get result for '{}': {}", name.c_str(), ex.what());
return std::nullopt;
}
if (e->on_result) {
try {
e->on_result(wrapped_result);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Result callback threw for '%s': %s", name.c_str(), ex.what());
LOG_WARN("Result callback threw for '{}': {}", name.c_str(), ex.what());
}
}
return wrapped_result;
@@ -412,23 +411,22 @@ public:
template<typename ActionT>
std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait_with_goal(
const std::string & name,
rclcpp::Logger logger,
const typename ActionT::Goal & goal,
std::chrono::nanoseconds timeout = std::chrono::seconds(30))
{
auto it = entries_.find(name);
if (it == entries_.end()) {
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
LOG_ERROR("No action client registered for '{}'", name.c_str());
return std::nullopt;
}
auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
if (!e) {
RCLCPP_ERROR(logger, "Action client type mismatch for '%s'", name.c_str());
LOG_ERROR("Action client type mismatch for '{}'", name.c_str());
return std::nullopt;
}
if (!e->client) {return std::nullopt;}
if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
LOG_ERROR("Action server '{}' not available", name.c_str());
return std::nullopt;
}
auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
@@ -438,7 +436,7 @@ public:
auto goal_future = e->client->async_send_goal(goal, opts);
auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", name.c_str());
LOG_ERROR("Timed out waiting for goal response on '{}'", name.c_str());
return std::nullopt;
}
@@ -446,11 +444,11 @@ public:
try {
goal_handle = goal_future.get();
} catch (const std::exception & ex) {
RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
LOG_ERROR("Failed to get goal handle for '{}': {}", name.c_str(), ex.what());
return std::nullopt;
}
if (!goal_handle) {
RCLCPP_WARN(logger, "Goal rejected for '%s'", name.c_str());
LOG_WARN("Goal rejected for '{}'", name.c_str());
return std::nullopt;
}
e->last_goal_handle = goal_handle;
@@ -458,18 +456,18 @@ public:
try {
e->on_goal_response(goal_handle);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", name.c_str(), ex.what());
LOG_WARN("Goal response callback threw for '{}': {}", name.c_str(), ex.what());
}
}
auto result_future = e->client->async_get_result(goal_handle);
auto deadline = std::chrono::steady_clock::now() + timeout;
if (result_future.wait_until(deadline) != std::future_status::ready) {
RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
LOG_ERROR("Timed out waiting for result on '{}', sending cancel", name.c_str());
try {
e->client->async_cancel_goal(goal_handle);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
LOG_WARN("Failed cancelling goal for '{}': {}", name.c_str(), ex.what());
}
return std::nullopt;
}
@@ -477,14 +475,14 @@ public:
try {
wrapped_result = result_future.get();
} catch (const std::exception & ex) {
RCLCPP_ERROR(logger, "Failed to get result for '%s': %s", name.c_str(), ex.what());
LOG_ERROR("Failed to get result for '{}': {}", name.c_str(), ex.what());
return std::nullopt;
}
if (e->on_result) {
try {
e->on_result(wrapped_result);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Result callback threw for '%s': %s", name.c_str(), ex.what());
LOG_WARN("Result callback threw for '{}': {}", name.c_str(), ex.what());
}
}
return wrapped_result;

13
src/brain/include/brain/bt_registry.hpp
View File

@@ -25,6 +25,7 @@
#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <rclcpp/rclcpp.hpp>
#include <hivecore_logger/logger.hpp>
#include <functional>
#include <map>
#include <string>
@@ -203,12 +204,12 @@ public:
auto it = bt_sequences_.find(seq_name);
if (it == bt_sequences_.end()) {
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "BT sequence '%s' not found", seq_name.c_str());
LOG_ERROR("BT sequence '{}' not found", seq_name.c_str());
}
return false;
}
const std::string xml_text = build_xml_from_sequence(it->second);
RCLCPP_INFO(node_->get_logger(), "Building BT sequence '%s'", xml_text.c_str());
LOG_INFO("Building BT sequence '{}'", xml_text.c_str());
out_tree = factory_.createTreeFromText(xml_text);
return true;
}
@@ -226,7 +227,7 @@ public:
return true;
} catch (const std::exception & e) {
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "Failed to build BT from XML text: %s", e.what());
LOG_ERROR("Failed to build BT from XML text: {}", e.what());
}
return false;
}
@@ -248,7 +249,7 @@ public:
std::ifstream ifs(file_path);
if (!ifs.is_open()) {
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "Could not open BT XML file: %s", file_path.c_str());
LOG_ERROR("Could not open BT XML file: {}", file_path.c_str());
}
return false;
}
@@ -256,12 +257,12 @@ public:
buffer << ifs.rdbuf();
if (buffer.str().empty()) {
if (node_) {
RCLCPP_ERROR(node_->get_logger(), "BT XML file empty: %s", file_path.c_str());
LOG_ERROR("BT XML file empty: {}", file_path.c_str());
}
return false;
}
if (node_) {
RCLCPP_INFO(node_->get_logger(), "Building BT from XML file: %s \n%s", file_path.c_str(), buffer.str().c_str());
LOG_INFO("Building BT from XML file: {} \n{}", file_path.c_str(), buffer.str().c_str());
}
return build_tree_from_xml_text(buffer.str(), out_tree);
}

5
src/brain/include/brain/cerebellum_node.hpp
View File

@@ -7,6 +7,7 @@
#include <vector>
#include <unordered_map>
#include <rclcpp/rclcpp.hpp>
#include <hivecore_logger/logger.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/string.hpp>
@@ -213,10 +214,10 @@ private:
try {
auto node = YAML::Load(c.payload_yaml);
out = brain::from_yaml<T>(node);
RCLCPP_INFO(rclcpp::Node::get_logger(), "[%s] payload_yaml parse success", skill_name.c_str());
LOG_INFO("[{}] payload_yaml parse success", skill_name.c_str());
return true;
} catch (const std::exception & e) {
RCLCPP_ERROR(rclcpp::Node::get_logger(), "[%s] payload_yaml parse failed: %s",
LOG_ERROR("[{}] payload_yaml parse failed: {}",
skill_name.c_str(), e.what());
}
break;

16
src/brain/include/brain/skill_manager.hpp
View File

@@ -1,6 +1,7 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <hivecore_logger/logger.hpp>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <functional>
@@ -296,7 +297,7 @@ private:
if (it != action_override_registrars_.end()) {
it->second(topic, internal_skill);
RCLCPP_INFO(node_->get_logger(), "Registered action client '%s' via Hook on '%s'",
LOG_INFO("Registered action client '{}' via Hook on '{}'",
internal_skill.c_str(), it->first.c_str());
return;
}
@@ -332,9 +333,9 @@ private:
result_cb = [this, internal_skill](const typename GoalHandle::WrappedResult & res) {
if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
SkillActionTrait<ActionT>::success(*res.result, "")) {
RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
LOG_INFO("action succeeded: {}", internal_skill.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
LOG_WARN("action cancelled: {}", internal_skill.c_str());
} else {
std::string detail;
if (res.result) {
@@ -342,8 +343,7 @@ private:
} else {
detail = "result unavailable";
}
RCLCPP_ERROR(
node_->get_logger(), "action failed: %s (%s)", internal_skill.c_str(), detail.c_str());
LOG_ERROR("action failed: {} ({})", internal_skill.c_str(), detail.c_str());
}
};
}
@@ -372,14 +372,14 @@ private:
template<size_t I = 0, typename TupleT, typename RegistryT>
bool dispatch_skill_action(
const std::string & skill, const std::string & topic, RegistryT * reg,
rclcpp::Logger logger, std::chrono::nanoseconds timeout, const std::string & instance_name, std::string & detail)
std::chrono::nanoseconds timeout, const std::string & instance_name, std::string & detail)
{
if constexpr (I == std::tuple_size<TupleT>::value) {
return false;
} else {
using ActionT = std::tuple_element_t<I, TupleT>;
if (skill == SkillActionTrait<ActionT>::skill_name) {
auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
auto opt = reg->template send_and_wait<ActionT>(topic, timeout);
if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
bool ok = SkillActionTrait<ActionT>::success(*opt->result, instance_name);
detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
@@ -389,7 +389,7 @@ bool dispatch_skill_action(
detail = "action failed";
return false;
}
return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, instance_name, detail);
return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, timeout, instance_name, detail);
}
}

25
src/brain/include/brain/sm_cerebellum.hpp
View File

@@ -3,6 +3,7 @@
#include <smacc2/smacc.hpp>
#include <boost/mpl/list.hpp>
#include <rclcpp/rclcpp.hpp>
#include <hivecore_logger/logger.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <atomic>
#include <string>
@@ -49,7 +50,7 @@ struct CerebellumData
* @brief Bound by the action execute callback: performs the whole skill
* sequence synchronously when invoked inside StExecuting supervision thread.
*/
std::function<ExecResult(rclcpp::Logger)> execute_fn; // installed by action execute
std::function<ExecResult()> execute_fn; // installed by action execute
/**
* @brief Completion callback invoked by terminal states (Completed/Failed/Emergency)
* to produce the actual action result (succeed/abort) back to the client.
@@ -117,7 +118,7 @@ struct StIdle : smacc2::SmaccState<StIdle, SmCerebellum>
*/
sc::result react(const EvGoalReceived &)
{
RCLCPP_INFO(this->getLogger(), "[SM] Transition Idle -> Executing (goal received)");
LOG_INFO("[SM] Transition Idle -> Executing (goal received)");
return transit<StExecuting>();
}
};
@@ -140,7 +141,7 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
/** @brief State entry: spawns supervision thread and invokes execute_fn when ready. */
void onEntry()
{
RCLCPP_INFO(this->getLogger(), "[SM] Enter Executing");
LOG_INFO("[SM] Enter Executing");
// Supervising thread waits until execute_fn is set, then runs it.
std::thread{[this]() {
auto & d = runtime();
@@ -150,12 +151,12 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
std::this_thread::sleep_for(5ms);
auto now = std::chrono::steady_clock::now();
if (now - last_warn > 10s) {
RCLCPP_WARN(this->getLogger(), "[SM] Still waiting for execute_fn to be set...");
LOG_WARN("[SM] Still waiting for execute_fn to be set...");
last_warn = now;
}
}
try {
auto res = d.execute_fn(this->getLogger());
auto res = d.execute_fn();
d.preempt.store(!res.success, std::memory_order_release);
d.preempt_reason = res.message;
d.last_success.store(res.success, std::memory_order_release);
@@ -180,19 +181,19 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
* @return Transition to Completed state.
*/
sc::result react(const EvExecutionFinished &)
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Executing -> Completed"); return transit<StCompleted>(); }
{ LOG_INFO("[SM] Transition Executing -> Completed"); return transit<StCompleted>(); }
/**
* @brief Reaction: functional failure -> StFailed.
* @return Transition to Failed state.
*/
sc::result react(const EvExecutionFailed &)
{ RCLCPP_WARN(this->getLogger(), "[SM] Transition Executing -> Failed"); return transit<StFailed>(); }
{ LOG_WARN("[SM] Transition Executing -> Failed"); return transit<StFailed>(); }
/**
* @brief Reaction: exception / emergency -> StEmergency.
* @return Transition to Emergency state.
*/
sc::result react(const EvEmergency &)
{ RCLCPP_ERROR(this->getLogger(), "[SM] Transition Executing -> Emergency"); return transit<StEmergency>(); }
{ LOG_ERROR("[SM] Transition Executing -> Emergency"); return transit<StEmergency>(); }
};
// ----------------------------------------------------------------------------
@@ -223,7 +224,7 @@ struct StCompleted : smacc2::SmaccState<StCompleted, SmCerebellum>
* @return Transition to StExecuting.
*/
sc::result react(const EvGoalReceived &)
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Completed -> Executing (new goal)"); return transit<StExecuting>(); }
{ LOG_INFO("[SM] Transition Completed -> Executing (new goal)"); return transit<StExecuting>(); }
};
// ----------------------------------------------------------------------------
@@ -249,7 +250,7 @@ struct StFailed : smacc2::SmaccState<StFailed, SmCerebellum>
* @return Transition to StExecuting.
*/
sc::result react(const EvGoalReceived &)
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Failed -> Executing (new goal)"); return transit<StExecuting>(); }
{ LOG_INFO("[SM] Transition Failed -> Executing (new goal)"); return transit<StExecuting>(); }
};
// ----------------------------------------------------------------------------
@@ -268,7 +269,7 @@ struct StEmergency : smacc2::SmaccState<StEmergency, SmCerebellum>
{
auto & d = runtime();
if (d.complete_cb) d.complete_cb(false, std::string("EMERGENCY: ")+ d.preempt_reason);
RCLCPP_ERROR(this->getLogger(), "[SM] Emergency: %s", d.preempt_reason.c_str());
LOG_ERROR("[SM] Emergency: {}", d.preempt_reason.c_str());
d.execute_fn = nullptr; d.complete_cb = nullptr; d.preempt.store(false);
}
/**
@@ -276,7 +277,7 @@ struct StEmergency : smacc2::SmaccState<StEmergency, SmCerebellum>
* @return Transition to StExecuting.
*/
sc::result react(const EvGoalReceived &)
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Emergency -> Executing (new goal)"); return transit<StExecuting>(); }
{ LOG_INFO("[SM] Transition Emergency -> Executing (new goal)"); return transit<StExecuting>(); }
};
} // namespace brain

19
src/brain/src/brain_node.cpp
View File

@@ -1,5 +1,5 @@
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logger.hpp>
#include <hivecore_logger/logger.hpp>
#include "../include/brain/cerebellum_node.hpp"
#include "../include/brain/cerebrum_node.hpp"
@@ -22,8 +22,20 @@ int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::Logger logger = rclcpp::get_logger("brain_node");
RCLCPP_INFO(logger, "Starting brain node...");
hivecore::log::LoggerOptions log_opts;
log_opts.default_level = hivecore::log::Level::INFO; // 默认日志等级
log_opts.log_dir = "/var/log/robot"; // 主日志目录
log_opts.fallback_log_dir = "/tmp/robot_logs"; // 备用目录（主目录不可写时使用）
log_opts.max_file_size_mb = 50; // 单个日志文件最大 50 MB
log_opts.max_files = 10; // 最多保留 10 个轮转文件
log_opts.queue_size = 8192; // 异步队列容量
log_opts.worker_threads = 1; // 后台写日志线程数
log_opts.enable_console = true; // 同时输出到控制台
log_opts.enable_level_sync = true; // 允许运行时动态调整日志等级
log_opts.level_sync_interval_ms = 100; // 动态等级同步检查间隔 (ms)
hivecore::log::Logger::init("brain_node", log_opts);
LOG_INFO("Starting brain node...");
// Create node options with parameter overrides if provided
rclcpp::NodeOptions options;
@@ -39,6 +51,7 @@ int main(int argc, char ** argv)
executor->spin();
hivecore::log::Logger::shutdown();
rclcpp::shutdown();
return 0;
}

2
src/brain/src/calc_grasp_pose.cpp
View File

@@ -6,7 +6,7 @@
#include <utility>
#include <string.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logger.hpp>
#include <hivecore_logger/logger.hpp>
namespace brain {

228
src/brain/src/cerebellum_hooks.cpp
View File

@@ -5,6 +5,7 @@
#include "interfaces/action/dual_arm.hpp"
#include "interfaces/msg/arm_motion_params.hpp"
#include <rclcpp/rclcpp.hpp>
#include <hivecore_logger/logger.hpp>
#include <sstream>
#include <iomanip>
#include <mutex>
@@ -15,7 +16,7 @@ namespace brain
void CerebellumHooks::ConfigureActionHooks(CerebellumNode * node)
{
if (!node->skill_manager_) {
RCLCPP_WARN(node->get_logger(), "SkillManager unavailable, skipping action hook configuration");
LOG_WARN("SkillManager unavailable, skipping action hook configuration");
return;
}
ConfigureArmHooks(node);
@@ -43,11 +44,11 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::Arm::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s]Received Parse Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld",
LOG_INFO("[{}]Received Parse Arm goal: body_id={}, data_type={}, data_length={}, command_id={}, frame_time_stamp={}",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
goal.body_id = (CerebellumNode::tls_arm_body_id == interfaces::msg::ArmMotionParams::ARM_LEFT ||
@@ -97,7 +98,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
} else if (goal.data_type == node->arm_cmd_type_take_object_) { //take object
action = "take_object";
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
LOG_ERROR("ARM make goal error: invalid data_type");
return goal;
}
@@ -127,21 +128,21 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
goal.data_length = 6;
goal.data_array = {angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]};
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid angle size: %ld/%d", angle.size(), 6);
LOG_ERROR("ARM make goal error: invalid angle size: {}/{}", angle.size(), 6);
}
#endif
}
else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
LOG_ERROR("ARM make goal error: data_type[{}] not support", goal.data_type);
return goal;
}
if (goal.data_array.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
LOG_INFO("[{}] Control params: body_id={}, data_type={}, data_length={}, command_id={}, frame_time_stamp={}, data_array=[{:.6f}, {:.6f}, {:.6f}, {:.6f}, {:.6f}, {:.6f}]",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_array size[%ld]", goal.data_array.size());
LOG_ERROR("ARM make goal error: invalid data_array size[{}]", goal.data_array.size());
}
return goal;
@@ -151,7 +152,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
ActionClientRegistry::GoalHandle<interfaces::action::Arm>::SharedPtr,
const std::shared_ptr<const interfaces::action::Arm::Feedback> & feedback) {
if (!feedback) {
RCLCPP_WARN(node->get_logger(), "[%s] feedback is empty", skill_name.c_str());
LOG_WARN("[{}] feedback is empty", skill_name.c_str());
return;
}
(void)feedback;
@@ -172,16 +173,16 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
res_arm_name = "Right Arm";
}
RCLCPP_WARN(node->get_logger(), "[%s] %s Final position: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f",
LOG_WARN("[{}] {} Final position: {:.6f}, {:.6f}, {:.6f}, {:.6f}, {:.6f}, {:.6f}, {:.6f}",
skill_name.c_str(), res_arm_name.c_str(), pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] %s command_id %ld completed: %s",
LOG_INFO("[{}] {} command_id {} completed: {}",
skill_name.c_str(), res_arm_name.c_str(), res.result->command_id, message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -189,9 +190,9 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::Arm>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(hooks));
@@ -204,11 +205,11 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::DualArm::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::DualArm::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s] DualArm goal: arm_motion_params=%zu, velocity_scaling=%d",
LOG_INFO("[{}] DualArm goal: arm_motion_params={}, velocity_scaling={}",
skill_name.c_str(), goal.arm_motion_params.size(), goal.velocity_scaling);
const auto load_cached_pose_angle = [node](
@@ -237,7 +238,7 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
} else if (p.motion_type == node->arm_cmd_type_take_object_) {
action = "take_object";
}
RCLCPP_INFO(node->get_logger(), "[%s] param[%zu]: action=%s, motion_type=%u",
LOG_INFO("[{}] param[{}]: action={}, motion_type={}",
skill_name.c_str(), i, action, p.motion_type);
if (action) {
@@ -256,8 +257,7 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
: interfaces::msg::ArmMotionParams::MOVEJ;
}
RCLCPP_INFO(node->get_logger(),
"[%s] param[%zu]: arm_id=%u motion_type=%u blend_radius=%d pose=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f) joint_size=%zu joints=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f)",
LOG_INFO("[{}] param[{}]: arm_id={} motion_type={} blend_radius={} pose=({:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}) joint_size={} joints=({:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f})",
skill_name.c_str(), i, p.arm_id, p.motion_type, p.blend_radius,
p.target_pose.position.x, p.target_pose.position.y, p.target_pose.position.z,
p.target_pose.orientation.x, p.target_pose.orientation.y, p.target_pose.orientation.z, p.target_pose.orientation.w, p.target_joints.size(),
@@ -271,7 +271,7 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
ActionClientRegistry::GoalHandle<interfaces::action::DualArm>::SharedPtr,
const std::shared_ptr<const interfaces::action::DualArm::Feedback> & feedback) {
if (!feedback) {return;}
RCLCPP_INFO(node->get_logger(), "[%s] progress=%.2f status=%u", skill_name.c_str(),
LOG_INFO("[{}] progress={:.2f} status={}", skill_name.c_str(),
feedback->progress, feedback->status);
};
hooks.on_result = [node](
@@ -280,12 +280,12 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s (progress=%.2f)",
LOG_INFO("[{}] Success: {} (progress={:.2f})",
skill_name.c_str(), message.c_str(), res.result->final_progress);
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -293,9 +293,9 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::DualArm>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::DualArm>("DualArm", std::move(hooks));
@@ -309,7 +309,7 @@ void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
interfaces::action::HandControl::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::HandControl::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
const std::string param = "hand_control.target_pose";
@@ -317,7 +317,7 @@ void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
node->declare_parameter<std::vector<double>>(param, {0.0, 0.0, 0.0});
}
const auto values = node->get_parameter(param).as_double_array();
RCLCPP_INFO(node->get_logger(), "[%s] Target parameters size=%zu", skill_name.c_str(), values.size());
LOG_INFO("[{}] Target parameters size={}", skill_name.c_str(), values.size());
(void)values;
goal.mode = node->hand_control_default_mode_;
return goal;
@@ -328,7 +328,7 @@ void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
const std::shared_ptr<const interfaces::action::HandControl::Feedback> & feedback) {
if (!feedback) {return;}
(void)feedback;
RCLCPP_INFO(node->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
LOG_INFO("[{}] Current progress: {:.2f}", skill_name.c_str(), feedback->progress);
};
hooks.on_result = [node](
const std::string & skill_name,
@@ -336,11 +336,11 @@ void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
LOG_INFO("[{}] Success: {}", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -348,9 +348,9 @@ void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::HandControl>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::HandControl>("HandControl", std::move(hooks));
@@ -363,7 +363,7 @@ void CerebellumHooks::ConfigureCameraTakePhotoHooks(CerebellumNode * node)
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::CameraTakePhoto::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::CameraTakePhoto::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
return goal;
@@ -374,11 +374,11 @@ void CerebellumHooks::ConfigureCameraTakePhotoHooks(CerebellumNode * node)
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
LOG_INFO("[{}] Success: {}", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -386,9 +386,9 @@ void CerebellumHooks::ConfigureCameraTakePhotoHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::CameraTakePhoto>("CameraTakePhoto", std::move(hooks));
@@ -422,11 +422,11 @@ void CerebellumHooks::ConfigureMoveWaistHooks(CerebellumNode * node)
interfaces::action::MoveWaist::Goal goal{};
// Plan A: load per-call YAML payload into goal if present
if (!node->TryParseCallPayload<interfaces::action::MoveWaist::Goal>(skill_name, goal)) {
RCLCPP_INFO(node->get_logger(), "Loaded MoveWaist goal from YAML payload");
LOG_INFO("Loaded MoveWaist goal from YAML payload");
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s] move_pitch_degree: %f, move_yaw_degree: %f",
LOG_INFO("[{}] move_pitch_degree: {}, move_yaw_degree: {}",
skill_name.c_str(), goal.move_pitch_degree, goal.move_yaw_degree);
return goal;
@@ -444,11 +444,11 @@ void CerebellumHooks::ConfigureMoveWaistHooks(CerebellumNode * node)
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
LOG_INFO("[{}] Success: {}", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -456,9 +456,9 @@ void CerebellumHooks::ConfigureMoveWaistHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveWaist>("MoveWaist", std::move(hooks));
@@ -471,7 +471,7 @@ void CerebellumHooks::ConfigureMoveLegHooks(CerebellumNode * node)
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::MoveLeg::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::MoveLeg::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
return goal;
@@ -489,11 +489,11 @@ void CerebellumHooks::ConfigureMoveLegHooks(CerebellumNode * node)
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
LOG_INFO("[{}] Success: {}", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -501,9 +501,9 @@ void CerebellumHooks::ConfigureMoveLegHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveLeg>("MoveLeg", std::move(hooks));
@@ -517,11 +517,11 @@ void CerebellumHooks::ConfigureMoveWheelHooks(CerebellumNode * node)
interfaces::action::MoveWheel::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::MoveWheel::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s] move_distance=%f, move_angle=%f",
LOG_INFO("[{}] move_distance={}, move_angle={}",
skill_name.c_str(), goal.move_distance, goal.move_angle);
return goal;
@@ -539,11 +539,11 @@ void CerebellumHooks::ConfigureMoveWheelHooks(CerebellumNode * node)
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
LOG_INFO("[{}] Success: {}", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -551,9 +551,9 @@ void CerebellumHooks::ConfigureMoveWheelHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveWheel>("MoveWheel", std::move(hooks));
@@ -567,7 +567,7 @@ void CerebellumHooks::ConfigureMoveHomeHooks(CerebellumNode * node)
interfaces::action::MoveHome::Goal goal{};
// Plan A: per-call YAML overrides (even though goal is empty)
if (!node->TryParseCallPayload<interfaces::action::MoveHome::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
@@ -586,11 +586,11 @@ void CerebellumHooks::ConfigureMoveHomeHooks(CerebellumNode * node)
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
LOG_INFO("[{}] Success: {}", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
LOG_ERROR("[{}] failed (code={}): {}",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
@@ -598,9 +598,9 @@ void CerebellumHooks::ConfigureMoveHomeHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveHome>("MoveHome", std::move(hooks));
@@ -615,13 +615,13 @@ void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
interfaces::action::GripperCmd::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::GripperCmd::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill_name.c_str());
return goal;
}
// goal.loc = 30; //right_hand_grab_width_; //TODO transfer
RCLCPP_INFO(node->get_logger(), "[%s] action make_goal params, loc: %d, speed: %d, torque: %d, mode: %d",
LOG_INFO("[{}] action make_goal params, loc: {}, speed: {}, torque: {}, mode: {}",
skill_name.c_str(), goal.loc, goal.speed, goal.torque, goal.mode);
return goal;
@@ -632,7 +632,7 @@ void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
const std::shared_ptr<const interfaces::action::GripperCmd::Feedback> & feedback) {
if (!feedback) {return;}
RCLCPP_INFO(node->get_logger(), "[%s] action feedback, loc: %d, speed: %d, torque: %d",
LOG_INFO("[{}] action feedback, loc: {}, speed: {}, torque: {}",
skill_name.c_str(), feedback->loc, feedback->speed, feedback->torque);
};
@@ -641,12 +641,12 @@ void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
const ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::WrappedResult & res) {
if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
LOG_WARN("[{}] was cancelled", skill_name.c_str());
} else if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(node->get_logger(), "[%s] action on_result, state: %s",
LOG_INFO("[{}] action on_result, state: {}",
skill_name.c_str(), res.result->state.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] was failed(code=%d)", skill_name.c_str(), static_cast<int>(res.code));
LOG_ERROR("[{}] was failed(code={})", skill_name.c_str(), static_cast<int>(res.code));
}
};
@@ -654,9 +654,9 @@ void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
LOG_INFO("[{}] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
LOG_WARN("[{}] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::GripperCmd>(target_skill, std::move(hooks));
@@ -672,7 +672,7 @@ void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
void CerebellumHooks::ConfigureServiceHooks(CerebellumNode * node)
{
if (!node->skill_manager_) {
RCLCPP_WARN(node->get_logger(), "SkillManager unavailable, skipping service hook configuration");
LOG_WARN("SkillManager unavailable, skipping service hook configuration");
return;
}
ConfigureVisionObjectRecognitionHooks(node);
@@ -688,7 +688,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
auto normalize_camera_position = [node](std::string & camera_position, const std::string & skill_name) {
if (camera_position != "left" && camera_position != "right" && camera_position != "top") {
RCLCPP_ERROR(node->get_logger(), "[%s] Invalid camera_position: %s", skill_name.c_str(), camera_position.c_str());
LOG_ERROR("[{}] Invalid camera_position: {}", skill_name.c_str(), camera_position.c_str());
camera_position = "top";
}
};
@@ -706,7 +706,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
node->right_hand_grab_width_.reserve(widths.size());
for (const auto w : widths) {
node->right_hand_grab_width_.push_back(static_cast<float>(w));
RCLCPP_INFO(node->get_logger(), "grab_width : %.4f", w);
LOG_INFO("grab_width : {:.4f}", w);
}
};
@@ -715,7 +715,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
const std_msgs::msg::Header & header) -> bool {
for (auto & tf : node->target_frames_) {
if (tf != obj.class_name) {
RCLCPP_WARN(node->get_logger(), "object class_name: %s, target_frame: %s not match", obj.class_name.c_str(), tf.c_str());
LOG_WARN("object class_name: {}, target_frame: {} not match", obj.class_name.c_str(), tf.c_str());
continue;
}
node->target_pose_[tf].header = header;
@@ -723,12 +723,12 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
node->target_pose_[tf].pose = obj.pose;
update_grab_width(obj.grab_width);
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, right_hand_grab_width_size: %zu, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
LOG_INFO("target_frame: {}, right_hand_grab_width_size: {}, target_pose: {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}",
tf.c_str(), node->right_hand_grab_width_.size(),
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
return true;
}
RCLCPP_WARN(node->get_logger(), "object class_name not match target frames");
LOG_WARN("object class_name not match target frames");
return false;
};
@@ -744,10 +744,10 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
auto log_joint_angles = [node](const std::string & label, const std::vector<double> & joint_angles) {
if (joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "%s joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
LOG_INFO("{} joint angles: [{:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}]",
label.c_str(), joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "%s joint angles size invalid: %ld", label.c_str(), joint_angles.size());
LOG_ERROR("{} joint angles size invalid: {}", label.c_str(), joint_angles.size());
}
};
@@ -806,7 +806,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
geometry_msgs::msg::PoseStamped left_arm_pose;
const bool transformed = node->TransformPoseToArmFrame(target_pose, "base_link_leftarm", left_arm_pose);
if (!transformed) {
RCLCPP_WARN(node->get_logger(), "[%s] coordinate transformation failed, continuing with %s frame data",
LOG_WARN("[{}] coordinate transformation failed, continuing with {} frame data",
skill_name.c_str(), left_arm_pose.header.frame_id.c_str());
return false;
}
@@ -818,13 +818,13 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
auto pose = target_pose.pose;
RCLCPP_INFO(node->get_logger(), "cam take photo pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
LOG_INFO("cam take photo pose: {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}",
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
pose.orientation.y, pose.orientation.z, pose.orientation.w);
// if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.1 ||
// pose.position.x > 0.8 || pose.position.y > 0.8 || pose.position.z > 1.0) {
// RCLCPP_ERROR(node->get_logger(), "invalid %s pose value, cal grasp pose failed", target_arm.c_str());
// LOG_ERROR("invalid {} pose value, cal grasp pose failed", target_arm.c_str());
// return false;
// }
@@ -833,7 +833,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
int arm_id = (target_arm == "left_arm") ? 0 : 1;
if (node->CallInverseKinematics(target_pos, target_quat, joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d take_photo_pose rm arm inverse kinematics failed.", arm_id);
LOG_ERROR("arm_id {} take_photo_pose rm arm inverse kinematics failed.", arm_id);
return false;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
@@ -855,13 +855,13 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
geometry_msgs::msg::PoseStamped pose_in_arm;
const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], node->arm_target_frame_, pose_in_arm);
if (!transformed) {
RCLCPP_WARN(node->get_logger(), "[%s] coordinate transformation failed, continuing with %s frame data",
LOG_WARN("[{}] coordinate transformation failed, continuing with {} frame data",
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
return false;
}
RCLCPP_INFO(node->get_logger(), "[%s] target pose transformed to %s frame", skill_name.c_str(), node->arm_target_frame_.c_str());
RCLCPP_INFO(node->get_logger(), "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
LOG_INFO("[{}] target pose transformed to {} frame", skill_name.c_str(), node->arm_target_frame_.c_str());
LOG_INFO("{:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}",
pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z,
pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
@@ -870,7 +870,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
// if (best_angles.empty()) {
// RCLCPP_WARN(node->get_logger(), "[%s]no valid grasp angles found", skill_name.c_str());
// LOG_WARN("[{}]no valid grasp angles found", skill_name.c_str());
// return false;
// }
@@ -885,12 +885,12 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
target_quat, node->grasp_type_, angle, palm, node->grasp_safety_dist_, node->grasp_finger_length_, node->arm_target_frame_);
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
LOG_INFO("[{}] angle {}, palm {}, pre_grasp_pose[{:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}]",
skill_name.c_str(), angle, palm.c_str(),
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
LOG_INFO("[{}] angle {}, palm {}, grasp_pose[{:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}, {:.4f}]",
skill_name.c_str(), angle, palm.c_str(),
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
@@ -902,7 +902,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
std::vector<double> pre_grasp_joint_angles(6, 360.0);
if (node->CallInverseKinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
LOG_ERROR("arm_id {}, [{}] angle {}, palm {} pre_grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
@@ -910,7 +910,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
std::vector<double> grasp_joint_angles(6, 360.0);
if (node->CallInverseKinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
LOG_ERROR("arm_id {}, [{}] angle {}, palm {} grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
@@ -921,7 +921,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
std::vector<double> take_object_joint_angles(6, 360.0);
if (node->CallInverseKinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",
LOG_ERROR("arm_id {}, [{}] angle {}, palm {} take_object joint_angles rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
@@ -952,7 +952,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
node->arm_angles_["left_arm_take_object"].push_back(take_object_joint_angles);
}
RCLCPP_INFO(node->get_logger(), "Generated Pose Success (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
LOG_INFO("Generated Pose Success (grasp_type: {}, Angle: {:.1f}, Palm: {})", result.name.c_str(), angle, palm.c_str());
}
}
return true;
@@ -984,11 +984,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
auto validate_generated_results = [node](const std::string & skill_name) -> bool {
if (node->camera_position_ == "top") {
if (node->arm_poses_["right_arm_take_photo"].empty() && node->arm_poses_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo poses generated", skill_name.c_str());
LOG_WARN("[{}] no valid hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->arm_angles_["right_arm_take_photo"].empty() && node->arm_angles_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo angles generated", skill_name.c_str());
LOG_WARN("[{}] no valid hand take photo angles generated", skill_name.c_str());
return false;
}
return true;
@@ -1004,23 +1004,23 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
const auto & take_object_angle = node->arm_angles_[pose_prefix + "take_object"];
if (pre_grasp_pose.empty() || grasp_pose.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
LOG_WARN("[{}] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (pre_grasp_pose.size() != grasp_pose.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] %s_pre_grasp size %zu not match %s_grasp size %zu",
LOG_WARN("[{}] {}_pre_grasp size {} not match {}_grasp size {}",
skill_name.c_str(), pose_prefix.c_str(), pre_grasp_pose.size(), pose_prefix.c_str(), grasp_pose.size());
return false;
}
if (pre_grasp_angle.empty() || grasp_angle.empty() || take_object_angle.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
LOG_WARN("[{}] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (pre_grasp_angle.size() != grasp_angle.size() ||
grasp_angle.size() != take_object_angle.size() ||
pre_grasp_angle.size() != take_object_angle.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
LOG_WARN("[{}] angles size not match [{} {} {}]",
skill_name.c_str(), pre_grasp_angle.size(), grasp_angle.size(), take_object_angle.size());
return false;
}
@@ -1041,7 +1041,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
auto req = std::make_shared<VisionSrv::Request>();
// Prefer payload_yaml from calls
if (!node->TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
RCLCPP_ERROR(node->get_logger(), "[%s] Payload Parse failed", skill_name.c_str());
LOG_ERROR("[{}] Payload Parse failed", skill_name.c_str());
return req;
}
// fallback to parameter
@@ -1054,12 +1054,12 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
std::find(node->target_frames_.begin(), node->target_frames_.end(), req->classes[0]) != node->target_frames_.end());
if (classes_empty || !classes_in_targets) {
if (node->target_frames_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] classes empty or not in target_frames_, and target_frames_ is empty",
LOG_WARN("[{}] classes empty or not in target_frames_, and target_frames_ is empty",
skill_name.c_str());
req->classes.clear();
} else {
req->classes = node->target_frames_;
RCLCPP_INFO(node->get_logger(), "[%s] classes reset from target_frames_, size=%zu",
LOG_INFO("[{}] classes reset from target_frames_, size={}",
skill_name.c_str(), req->classes.size());
}
}
@@ -1069,10 +1069,10 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
if (node->target_frame_.empty() && !node->target_frames_.empty()) {
node->target_frame_ = node->target_frames_.front();
RCLCPP_WARN(node->get_logger(), "[%s] target_frame empty, fallback to first target_frames_: %s",
LOG_WARN("[{}] target_frame empty, fallback to first target_frames_: {}",
skill_name.c_str(), node->target_frame_.c_str());
}
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s, target_frame: %s",
LOG_INFO("[{}] camera_position: {}, target_frame: {}",
skill_name.c_str(), req->camera_position.c_str(), node->target_frame_.c_str());
return req;
};
@@ -1084,21 +1084,21 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
std::string & detail) {
if (!resp->success) {
detail = resp->info.empty() ? std::string("Not success") : resp->info;
RCLCPP_WARN(node->get_logger(), "[%s] VisionObjectRecognition failed: %s",
LOG_WARN("[{}] VisionObjectRecognition failed: {}",
skill_name.c_str(), detail.c_str());
return false;
}
detail = resp->info.empty() ? std::string("success pose") : resp->info;
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
RCLCPP_INFO(node->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
LOG_INFO("[{}] success={} classes={} frame_id={} stamp={:.3f} info={}",
skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
clear_cached_grasp_data();
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
// RCLCPP_INFO(node->get_logger(), " [%zu] class='%s' id=%d", i, obj.class_name.c_str(), obj.class_id);
// LOG_INFO(" [{}] class='{}' id={}", i, obj.class_name.c_str(), obj.class_id);
if (!UpdateTargetPose(obj, resp->header)) {
continue;
}
@@ -1144,9 +1144,9 @@ void CerebellumHooks::ConfigureMapLoadHooks(CerebellumNode * node)
// MapLoad response schema may not include a message field; rely on success only
detail = resp->success ? std::string("ok") : std::string("failed");
if (!resp->success) {
RCLCPP_WARN(node->get_logger(), "[%s] MapLoad failed: %s", skill_name.c_str(), detail.c_str());
LOG_WARN("[{}] MapLoad failed: {}", skill_name.c_str(), detail.c_str());
} else {
RCLCPP_INFO(node->get_logger(), "[%s] MapLoad: %s", skill_name.c_str(), detail.c_str());
LOG_INFO("[{}] MapLoad: {}", skill_name.c_str(), detail.c_str());
}
return resp->success;
};
@@ -1177,9 +1177,9 @@ void CerebellumHooks::ConfigureMapSaveHooks(CerebellumNode * node)
// MapSave response schema may not include a message field; rely on success only
detail = resp->success ? std::string("ok") : std::string("failed");
if (!resp->success) {
RCLCPP_WARN(node->get_logger(), "[%s] MapSave failed: %s", skill_name.c_str(), detail.c_str());
LOG_WARN("[{}] MapSave failed: {}", skill_name.c_str(), detail.c_str());
} else {
RCLCPP_INFO(node->get_logger(), "[%s] MapSave: %s", skill_name.c_str(), detail.c_str());
LOG_INFO("[{}] MapSave: {}", skill_name.c_str(), detail.c_str());
}
return resp->success;
};

135
src/brain/src/cerebellum_node.cpp
View File

@@ -25,8 +25,8 @@
// Third-party / external libraries (ROS2, ament).
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/rclcpp.hpp>
#include <hivecore_logger/logger.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
// Project headers.
@@ -73,7 +73,7 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
: Node("cerebellum_node", options),
share_directory_(ament_index_cpp::get_package_share_directory("brain"))
{
RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
LOG_INFO("Cerebellum node started");
ik_client_ = this->create_client<interfaces::srv::InverseKinematics>("inverse_kinematics");
@@ -85,7 +85,7 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
robot_config_ = std::make_unique<brain::robot_config::RobotConfig>(config_path);
auto skill_file = robot_config_->SkillFile("brain");
if (skill_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
LOG_ERROR("No skill_file entry found for 'brain' in robot_config.yaml");
return;
}
@@ -94,7 +94,7 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
? share_directory_ + *skill_file
: robot_skill_file_path_;
RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
LOG_WARN("skill file {}", robot_skill_file_path_.c_str());
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_buffer_->setUsingDedicatedThread(true);
@@ -119,11 +119,10 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
#ifndef BRAIN_DISABLE_SM
if (!sm_exec_) {
sm_exec_.reset(smacc2::run_async<brain::SmCerebellum>());
RCLCPP_INFO(this->get_logger(), "SMACC2 SmCerebellum started");
LOG_INFO("SMACC2 SmCerebellum started");
}
#else
RCLCPP_INFO(
this->get_logger(), "SMACC2 disabled via BRAIN_DISABLE_SM for lightweight test context");
LOG_INFO("SMACC2 disabled via BRAIN_DISABLE_SM for lightweight test context");
#endif
}
@@ -269,8 +268,7 @@ void CerebellumNode::DeclareAndLoadParameters()
nav_goal_distance_tolerance_ = std::max(0.0, nav_goal_distance_tolerance_);
if (dual_arm_motion_type_ != "MOVEJ" && dual_arm_motion_type_ != "MOVEL") {
RCLCPP_WARN(
this->get_logger(), "Invalid dual_arm_motion_type '%s', defaulting to MOVEJ",
LOG_WARN("Invalid dual_arm_motion_type '{}', defaulting to MOVEJ",
dual_arm_motion_type_.c_str());
dual_arm_motion_type_ = "MOVEJ";
}
@@ -289,8 +287,7 @@ void CerebellumNode::DeclareAndLoadParameters()
try {
skill_timeouts_[key] = std::stod(vstr);
} catch (...) {
RCLCPP_WARN(
this->get_logger(), "Bad timeout value for %s: %s", key.c_str(),
LOG_WARN("Bad timeout value for {}: {}", key.c_str(),
vstr.c_str());
}
}
@@ -305,13 +302,11 @@ void CerebellumNode::DeclareAndLoadParameters()
}
flush();
if (!skill_timeouts_.empty()) {
RCLCPP_INFO(
this->get_logger(), "[cerebellum_node] Loaded %zu skill-specific timeouts",
LOG_INFO("[cerebellum_node] Loaded {} skill-specific timeouts",
skill_timeouts_.size());
}
}
RCLCPP_INFO(
this->get_logger(), "[cerebellum_node] abort_on_failure=%s default_action_timeout=%.1f vision_grasp_object_timeout=%.1f",
LOG_INFO("[cerebellum_node] abort_on_failure={} default_action_timeout={:.1f} vision_grasp_object_timeout={:.1f}",
abort_on_failure_ ? "true" : "false", default_action_timeout_sec_,
vision_grasp_object_timeout_sec_);
}
@@ -326,17 +321,15 @@ void CerebellumNode::DeclareAndLoadParameters()
void CerebellumNode::LoadSkillsFile()
{
if (robot_skill_file_path_.empty()) {
RCLCPP_ERROR(
this->get_logger(), "[cerebellum_node] No skill file path provided, skipping skill load");
LOG_ERROR("[cerebellum_node] No skill file path provided, skipping skill load");
return;
}
if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
RCLCPP_ERROR(
this->get_logger(), "[cerebellum_node] Failed to load skills from %s", robot_skill_file_path_.c_str());
LOG_ERROR("[cerebellum_node] Failed to load skills from {}", robot_skill_file_path_.c_str());
return;
}
for (const auto & kv : skill_manager_->skills()) {
RCLCPP_INFO(this->get_logger(), "[cerebellum_node] Loaded skill '%s'", kv.first.c_str());
LOG_INFO("[cerebellum_node] Loaded skill '{}'", kv.first.c_str());
}
}
@@ -373,30 +366,29 @@ void CerebellumNode::SetupExecuteBtServer()
if (!goal) {return rclcpp_action::GoalResponse::REJECT;}
#ifdef ENABLE_EPOCH_CHECK
if (goal->epoch < current_epoch_) {
RCLCPP_WARN(
this->get_logger(), "Reject stale ExecuteBtAction goal epoch=%u < current=%u", goal->epoch,
LOG_WARN("Reject stale ExecuteBtAction goal epoch={} < current={}", goal->epoch,
current_epoch_.load());
return rclcpp_action::GoalResponse::REJECT;
}
#endif
if (goal->epoch > current_epoch_) {
current_epoch_ = goal->epoch;
RCLCPP_INFO(this->get_logger(), "Switch to new epoch %u", current_epoch_.load());
LOG_INFO("Switch to new epoch {}", current_epoch_.load());
}
if (sm_exec_ && sm_exec_->signalDetector) {
sm_exec_->signalDetector->postEvent(new brain::EvGoalReceived());
}
// Log detailed goal contents to help debugging call propagation from Cerebrum
RCLCPP_INFO(this->get_logger(), "ExecuteBtAction GoalReceived epoch=%u action_name=%s calls_count=%zu",
LOG_INFO("ExecuteBtAction GoalReceived epoch={} action_name={} calls_count={}",
goal->epoch, goal->action_name.c_str(), goal->calls.size());
for (size_t i = 0; i < goal->calls.size(); ++i) {
const auto & c = goal->calls[i];
RCLCPP_INFO(this->get_logger(), "call[%zu]: name=%s instance=%s interface=%s kind=%s topic=%s timeout_ms=%u payload_len=%zu",
LOG_INFO("call[{}]: name={} instance={} interface={} kind={} topic={} timeout_ms={} payload_len={}",
i, c.name.c_str(), c.instance_name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.timeout_ms, c.payload_yaml.size());
if (!c.payload_yaml.empty()) {
// Log payload content (trim if very long)
const std::string payload = c.payload_yaml.size() > 512 ? c.payload_yaml.substr(0, 512) + "..." : c.payload_yaml;
// RCLCPP_INFO(this->get_logger(), "payload:{%s}", payload.c_str());
// LOG_INFO("payload:{{}}", payload.c_str());
}
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@@ -409,7 +401,7 @@ void CerebellumNode::SetupExecuteBtServer()
if (it != goal_ctxs_.end() && it->second) {
it->second->cancel.store(true, std::memory_order_release);
}
RCLCPP_WARN(this->get_logger(), "[ExecuteBtAction] cancellation requested (epoch=%u)", gh->get_goal()->epoch);
LOG_WARN("[ExecuteBtAction] cancellation requested (epoch={})", gh->get_goal()->epoch);
return rclcpp_action::CancelResponse::ACCEPT;
};
// Each goal executes in its own detached thread (provided by ActionServerRegistry)
@@ -427,15 +419,15 @@ bool CerebellumNode::TransformPoseToArmFrame(
geometry_msgs::msg::PoseStamped & transformed) const
{
if (!tf_buffer_) {
RCLCPP_ERROR(this->get_logger(), "TF buffer not initialized, unable to transform target pose");
LOG_ERROR("TF buffer not initialized, unable to transform target pose");
return false;
}
if (source.header.frame_id.empty()) {
RCLCPP_WARN(this->get_logger(), "Target pose missing source frame, skipping transformation");
LOG_WARN("Target pose missing source frame, skipping transformation");
return false;
}
if (source.header.frame_id == arm_target_frame) {
RCLCPP_INFO(this->get_logger(), "Target pose already in %s frame, skipping transformation", arm_target_frame.c_str());
LOG_INFO("Target pose already in {} frame, skipping transformation", arm_target_frame.c_str());
transformed = source;
return true;
}
@@ -445,28 +437,24 @@ bool CerebellumNode::TransformPoseToArmFrame(
// ===================
#ifdef TF_DEBUG_LOGGING
RCLCPP_INFO(this->get_logger(),
"TF Debug: Source [%s] pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f) t=%.4f",
LOG_INFO("TF Debug: Source [{}] pos({:.4f}, {:.4f}, {:.4f}) quat({:.4f}, {:.4f}, {:.4f}, {:.4f}) t={:.4f}",
source.header.frame_id.c_str(),
source.pose.position.x, source.pose.position.y, source.pose.position.z,
source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w,
rclcpp::Time(source.header.stamp).seconds());
RCLCPP_INFO(this->get_logger(),
"TF Debug: Result [%s] pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
LOG_INFO("TF Debug: Result [{}] pos({:.4f}, {:.4f}, {:.4f}) quat({:.4f}, {:.4f}, {:.4f}, {:.4f})",
transformed.header.frame_id.c_str(),
transformed.pose.position.x, transformed.pose.position.y, transformed.pose.position.z,
transformed.pose.orientation.x, transformed.pose.orientation.y, transformed.pose.orientation.z, transformed.pose.orientation.w);
#endif
// ===================
RCLCPP_INFO(
this->get_logger(), "Target pose transformed from %s to %s",
LOG_INFO("Target pose transformed from {} to {}",
source.header.frame_id.c_str(), arm_target_frame.c_str());
return true;
} catch (const tf2::TransformException & ex) {
RCLCPP_ERROR(
this->get_logger(), "Failed to transform target pose: %s -> %s, reason: %s",
LOG_ERROR("Failed to transform target pose: {} -> {}, reason: {}",
source.header.frame_id.c_str(), arm_target_frame.c_str(), ex.what());
}
return false;
@@ -707,7 +695,7 @@ bool CerebellumNode::ExecuteActionSkill(
(void)spec; // Currently unused beyond interface kind selection.
if (!action_clients_->has_client(topic)) {
detail = "Client not registered";
RCLCPP_ERROR(this->get_logger(), "[%s] %s, TOPIC: %s", skill.c_str(), detail.c_str(), topic.c_str());
LOG_ERROR("[{}] {}, TOPIC: {}", skill.c_str(), detail.c_str(), topic.c_str());
return false;
}
@@ -721,7 +709,7 @@ bool CerebellumNode::ExecuteActionSkill(
interface_base = skill;
}
RCLCPP_INFO(this->get_logger(), "SKILL: [%s] (Base: %s), TOPIC:[%s]",
LOG_INFO("SKILL: [{}] (Base: {}), TOPIC:[{}]",
skill.c_str(), interface_base.c_str(), topic.c_str());
const float base_progress =
@@ -752,7 +740,7 @@ bool CerebellumNode::ExecuteActionSkill(
if (interface_base == "Arm") {
interfaces::action::Arm::Goal goal{};
if (!TryParseCallPayload<interfaces::action::Arm::Goal>(skill, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill.c_str());
LOG_ERROR("Failed to parse call payload for skill {}", skill.c_str());
return false;
}
@@ -762,7 +750,7 @@ bool CerebellumNode::ExecuteActionSkill(
} else if (goal.body_id == 0 || goal.body_id == 1) { //left or right
tls_arm_body_id = goal.body_id;
} else {
RCLCPP_ERROR(this->get_logger(), "Invalid body_id: %d for skill %s", goal.body_id, skill.c_str());
LOG_ERROR("Invalid body_id: {} for skill {}", goal.body_id, skill.c_str());
return false;
}
}
@@ -801,16 +789,15 @@ bool CerebellumNode::RunActionSkillWithProgress(
{
std::atomic<bool> finished{false};
bool ok = false;
auto logger = this->get_logger();
std::string local_detail;
(void)timeout_ms_override; // currently encoded into timeout_ns already
std::thread worker([&]() {
const auto * prev_calls = CerebellumNode::tls_current_calls_;
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
RCLCPP_INFO(this->get_logger(), "[%s] instance='%s' timeout=%.3fs dispatch='%s' topic='%s'",
LOG_INFO("[{}] instance='{}' timeout={:.3f}s dispatch='{}' topic='{}'",
skill.c_str(), instance_name.c_str(), timeout_ns.count()/1e9, interface_name.c_str(), topic.c_str());
ok = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
interface_name, topic, action_clients_.get(), logger, timeout_ns, instance_name, local_detail);
interface_name, topic, action_clients_.get(), timeout_ns, instance_name, local_detail);
CerebellumNode::tls_current_calls_ = prev_calls;
finished.store(true, std::memory_order_release);
});
@@ -829,7 +816,7 @@ bool CerebellumNode::RunActionSkillWithProgress(
try {
worker.join();
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "Exception in worker thread join: %s", e.what());
LOG_ERROR("Exception in worker thread join: {}", e.what());
}
}
detail = local_detail;
@@ -863,8 +850,8 @@ bool CerebellumNode::ExecuteBilateralArmAction(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
std::string & detail)
{
RCLCPP_INFO(this->get_logger(), "[Arm] send two goals: body_id=1 and body_id=0, index=%d, total_skills=%d", index, total_skills);
RCLCPP_INFO(this->get_logger(), "[Arm] instance='%s' timeout=%ld ms", instance_name.c_str(), static_cast<long>(timeout_ms_override));
LOG_INFO("[Arm] send two goals: body_id=1 and body_id=0, index={}, total_skills={}", index, total_skills);
LOG_INFO("[Arm] instance='{}' timeout={} ms", instance_name.c_str(), static_cast<long>(timeout_ms_override));
std::atomic<bool> right_arm_finished{false}, left_arm_finished{false};
bool right_arm_ok = false, left_arm_ok = false;
@@ -878,11 +865,11 @@ bool CerebellumNode::ExecuteBilateralArmAction(
const auto * prev_calls = CerebellumNode::tls_current_calls_;
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
interface_name, topic, action_clients_.get(), this->get_logger(), timeout, instance_name, d_out);
interface_name, topic, action_clients_.get(), timeout, instance_name, d_out);
CerebellumNode::tls_current_calls_ = prev_calls;
done_flag.store(true, std::memory_order_release);
time_steady = std::chrono::steady_clock::now();
RCLCPP_WARN(this->get_logger(), "left_arm_time_steady2");
LOG_WARN("left_arm_time_steady2");
};
std::thread left_arm_thread(left_arm_worker, interfaces::msg::ArmMotionParams::ARM_LEFT); // ARM_LEFT=0
@@ -892,11 +879,11 @@ bool CerebellumNode::ExecuteBilateralArmAction(
const auto * prev_calls = CerebellumNode::tls_current_calls_;
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
interface_name, topic, action_clients_.get(), this->get_logger(), timeout, instance_name, d_out);
interface_name, topic, action_clients_.get(), timeout, instance_name, d_out);
CerebellumNode::tls_current_calls_ = prev_calls;
done_flag.store(true, std::memory_order_release);
time_steady = std::chrono::steady_clock::now();
RCLCPP_WARN(this->get_logger(), "right_arm_time_steady2");
LOG_WARN("right_arm_time_steady2");
};
std::thread right_arm_thread(right_arm_worker, interfaces::msg::ArmMotionParams::ARM_RIGHT); // ARM_RIGHT=1
@@ -909,26 +896,26 @@ bool CerebellumNode::ExecuteBilateralArmAction(
}
double arm_exe_bias = std::chrono::duration_cast<std::chrono::milliseconds>(left_arm_time_steady - right_arm_time_steady).count();
RCLCPP_WARN(this->get_logger(), "arm_exe_bias: %f", arm_exe_bias);
LOG_WARN("arm_exe_bias: {}", arm_exe_bias);
if (timeout_ms_override > 0 && std::abs(arm_exe_bias) > timeout_ms_override) {
right_arm_ok = false;
left_arm_ok = false;
RCLCPP_ERROR(this->get_logger(), "Fail in Arm arm_exe_bias: %f, timeout_ms_override=%ld", arm_exe_bias, static_cast<long>(timeout_ms_override));
LOG_ERROR("Fail in Arm arm_exe_bias: {}, timeout_ms_override={}", arm_exe_bias, static_cast<long>(timeout_ms_override));
}
if (right_arm_thread.joinable()) {
try {
right_arm_thread.join();
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 1 join: %s", e.what());
LOG_ERROR("Exception in Arm thread 1 join: {}", e.what());
}
}
if (left_arm_thread.joinable()) {
try {
left_arm_thread.join();
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 2 join: %s", e.what());
LOG_ERROR("Exception in Arm thread 2 join: {}", e.what());
}
}
@@ -1017,13 +1004,11 @@ void CerebellumNode::RecordSkillResult(const std::string & skill, bool success)
void CerebellumNode::LogStatsPeriodic()
{
std::lock_guard<std::mutex> lk(stats_mutex_);
RCLCPP_INFO(
this->get_logger(), "[stats] sequences success=%zu failure=%zu",
LOG_INFO("[stats] sequences success={} failure={}",
total_sequence_success_, total_sequence_failure_);
for (const auto & kv : skill_success_counts_) {
size_t fails = skill_failure_counts_[kv.first];
RCLCPP_INFO(
this->get_logger(), "skill %s: OK=%zu FAIL=%zu", kv.first.c_str(), kv.second,
LOG_INFO("skill {}: OK={} FAIL={}", kv.first.c_str(), kv.second,
fails);
}
if (stats_pub_) {
@@ -1127,7 +1112,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
}
const auto & skill = skills[seq_index];
const std::string topic = ResolveTopicForSkill(skill);
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s, total_skills:%d",
LOG_INFO("[ExecuteBtAction] Dispatch skill={} topic={}, total_skills:{}",
skill.c_str(), topic.c_str(), static_cast<int>(skills.size()));
bool ok = false; std::string detail;
auto it = skill_manager_->skills().find(skill);
@@ -1154,7 +1139,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
if (loop_index + 1 < skills.size()) summary << ", ";
++loop_index;
if (!ok) {
RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s, detail: %s", skill.c_str(),
LOG_ERROR("Skill {} failed{}, detail: {}", skill.c_str(),
abort_on_failure_ ? ", abort sequence" : " (continue)", detail.c_str());
if (abort_on_failure_) break;
}
@@ -1183,7 +1168,6 @@ bool CerebellumNode::ValidateAndParseGoal(
std::vector<std::string> & skills_out) const
{
using EBA = interfaces::action::ExecuteBtAction;
auto logger = this->get_logger();
const auto goal = goal_handle->get_goal();
if (!goal) {
auto res = std::make_shared<EBA::Result>();
@@ -1205,10 +1189,10 @@ bool CerebellumNode::ValidateAndParseGoal(
oss << goal->calls[i].name;
}
oss << "] (count=" << goal->calls.size() << ")";
RCLCPP_INFO(logger, "%s", oss.str().c_str());
LOG_INFO("{}", oss.str().c_str());
} else {
const std::string & raw = goal->action_name;
RCLCPP_INFO(logger, "[ExecuteBtAction] Received goal action_name=%s", raw.c_str());
LOG_INFO("[ExecuteBtAction] Received goal action_name={}", raw.c_str());
skills_out = ParseSkillSequence(raw);
}
if (skills_out.empty()) {
@@ -1286,13 +1270,12 @@ void CerebellumNode::FinalizeExecuteBtResult(
res->message = summary_stream.str();
res->total_skills = total_skills;
res->succeeded_skills = succeeded_skills;
auto logger = this->get_logger();
if (overall_ok) {
goal_handle->succeed(res);
RCLCPP_INFO(logger, "%s", res->message.c_str());
LOG_INFO("{}", res->message.c_str());
} else {
goal_handle->abort(res);
RCLCPP_ERROR(logger, "%s", res->message.c_str());
LOG_ERROR("{}", res->message.c_str());
}
}
@@ -1377,7 +1360,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequenceConcurrent(
}
const auto & skill = skills[seq_index];
const std::string topic = ResolveTopicForSkillFromCalls(calls, skill);
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s, total_skills:%d",
LOG_INFO("[ExecuteBtAction] Dispatch skill={} topic={}, total_skills:{}",
skill.c_str(), topic.c_str(), static_cast<int>(skills.size()));
bool ok = false; std::string detail;
auto it = skill_manager_->skills().find(skill);
@@ -1404,7 +1387,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequenceConcurrent(
if (loop_index + 1 < skills.size()) summary << ", ";
++loop_index;
if (!ok) {
RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s, detail: %s", skill.c_str(),
LOG_ERROR("Skill {} failed{}, detail: {}", skill.c_str(),
abort_on_failure_ ? ", abort sequence" : " (continue)", detail.c_str());
if (abort_on_failure_) break;
}
@@ -1435,10 +1418,10 @@ void CerebellumNode::JointStateCallback(const sensor_msgs::msg::JointState::Shar
right_arm_joint_angles_.push_back(angle_deg);
}
#ifdef DEBUG_JOINT_STATE
RCLCPP_INFO(this->get_logger(), "Received left arm joint angle[%f, %f, %f, %f, %f, %f] ",
LOG_INFO("Received left arm joint angle[{}, {}, {}, {}, {}, {}] ",
left_arm_joint_angles_[0], left_arm_joint_angles_[1], left_arm_joint_angles_[2],
left_arm_joint_angles_[3], left_arm_joint_angles_[4], left_arm_joint_angles_[5]);
RCLCPP_INFO(this->get_logger(), "Received right arm joint angle[%f, %f, %f, %f, %f, %f] ",
LOG_INFO("Received right arm joint angle[{}, {}, {}, {}, {}, {}] ",
right_arm_joint_angles_[0], right_arm_joint_angles_[1], right_arm_joint_angles_[2],
right_arm_joint_angles_[3], right_arm_joint_angles_[4], right_arm_joint_angles_[5]);
#endif
@@ -1448,7 +1431,7 @@ void CerebellumNode::JointStateCallback(const sensor_msgs::msg::JointState::Shar
int CerebellumNode::CallInverseKinematics(const tf2::Vector3& pos, const tf2::Quaternion& quat, std::vector<double>& angles, int arm_id)
{
if (!ik_client_->wait_for_service(std::chrono::seconds(1))) {
RCLCPP_ERROR(this->get_logger(), "Service inverse_kinematics not available");
LOG_ERROR("Service inverse_kinematics not available");
return -1;
}
@@ -1474,15 +1457,15 @@ int CerebellumNode::CallInverseKinematics(const tf2::Vector3& pos, const tf2::Qu
for (size_t i = 0; i < result->joint_angles.size(); ++i) {
angles[i] = result->joint_angles[i];
}
RCLCPP_INFO(this->get_logger(), "arm_id %d Inverse kinematics service SUCCESS returned angles: [%f, %f, %f, %f, %f, %f]", arm_id,
LOG_INFO("arm_id {} Inverse kinematics service SUCCESS returned angles: [{}, {}, {}, {}, {}, {}]", arm_id,
angles[0], angles[1], angles[2], angles[3], angles[4], angles[5]);
return 0;
} else {
RCLCPP_ERROR(this->get_logger(), "arm_id %d Inverse kinematics service returned failure: %d", arm_id, result->result);
LOG_ERROR("arm_id {} Inverse kinematics service returned failure: {}", arm_id, result->result);
return -1;
}
} else {
RCLCPP_ERROR(this->get_logger(), "arm_id %d Inverse kinematics service call timed out", arm_id);
LOG_ERROR("arm_id {} Inverse kinematics service call timed out", arm_id);
return -1;
}
}

160
src/brain/src/cerebrum_node.cpp
View File

@@ -24,6 +24,7 @@
#include <behaviortree_cpp/decorator_node.h>
#include <behaviortree_cpp/controls/parallel_node.h>
#include <rclcpp_action/rclcpp_action.hpp>
#include <hivecore_logger/logger.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
// Project headers
@@ -245,10 +246,10 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("cerebrum_node", options),
share_directory_(ament_index_cpp::get_package_share_directory("brain"))
{
RCLCPP_INFO(this->get_logger(), "Cerebrum node started");
LOG_INFO("Cerebrum node started");
if (!LoadRobotConfiguration()) {
RCLCPP_ERROR(this->get_logger(), "Failed to load robot configuration");
LOG_ERROR("Failed to load robot configuration");
return;
}
@@ -257,7 +258,7 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", robot_skill_file_path_.c_str());
LOG_WARN("Failed to load skills from {}", robot_skill_file_path_.c_str());
}
DeclareParameters();
@@ -269,18 +270,18 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
if (!bt_timer_) {
RCLCPP_ERROR(this->get_logger(), "Failed to create BT timer");
LOG_ERROR("Failed to create BT timer");
}
// task_timer_ = this->create_wall_timer(10000ms, [this]() {CerebrumTask();});
// if (!task_timer_) {
// RCLCPP_ERROR(this->get_logger(), "Failed to create task timer");
// LOG_ERROR("Failed to create task timer");
// }
CreateServices();
RobotWorkInfoPublish();
RCLCPP_INFO(this->get_logger(), "CerebrumNode initialized (timeout=%.2f random_pick=%zu)",
LOG_INFO("CerebrumNode initialized (timeout={:.2f} random_pick={})",
node_timeout_sec_, random_skill_pick_count_);
}
@@ -306,10 +307,10 @@ void CerebrumNode::RegisterBtAction(
void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
{
if (!action_registry_) {
RCLCPP_WARN(this->get_logger(), "Action registry not initialized; cannot send %s", bt_action_name.c_str());
LOG_WARN("Action registry not initialized; cannot send {}", bt_action_name.c_str());
return;
}
action_registry_->send(bt_action_name, this->get_logger());
action_registry_->send(bt_action_name);
}
/**
@@ -355,13 +356,13 @@ void CerebrumNode::RegisterActionClient()
}
robot_work_info_.bt_node_status = "RUNNING";
// Debug: log outgoing calls being sent to Cerebellum
// RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
// LOG_INFO("[ExecuteBtAction] Sending goal with calls_count={}", g.calls.size());
for (size_t i = 0; i < g.calls.size(); ++i) {
const auto & c = g.calls[i];
RCLCPP_INFO(this->get_logger(), " out_call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
LOG_INFO(" out_call[{}]: name={} interface={} kind={} topic={} payload_len={}",
i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
}
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal epoch=%u skills=%s", g.epoch, g.action_name.c_str());
LOG_INFO("[ExecuteBtAction] Sending goal epoch={} skills={}", g.epoch, g.action_name.c_str());
return g;
},
// Feedback
@@ -373,8 +374,7 @@ void CerebrumNode::RegisterActionClient()
return;
}
// For parallel per-instance leaves, we only log feedback; mapping to instance is ambiguous.
RCLCPP_INFO(this->get_logger(),
"[ExecuteBtAction][fb][epoch=%u] stage=%s skill=%s progress=%.2f detail=%s",
LOG_INFO("[ExecuteBtAction][fb][epoch={}] stage={} skill={} progress={:.2f} detail={}",
feedback->epoch, feedback->stage.c_str(), feedback->current_skill.c_str(),
feedback->progress, feedback->detail.c_str());
},
@@ -382,10 +382,10 @@ void CerebrumNode::RegisterActionClient()
[this](const ActionClientRegistry::GoalHandle<EBA>::WrappedResult & res) {
const bool ok = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success);
if (ok) {
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] OK: %s",
LOG_INFO("[ExecuteBtAction][result] OK: {}",
res.result ? res.result->message.c_str() : "<null>");
} else {
RCLCPP_ERROR(this->get_logger(), "[ExecuteBtAction][result] FAIL code=%d msg=%s",
LOG_ERROR("[ExecuteBtAction][result] FAIL code={} msg={}",
static_cast<int>(res.code), res.result ? res.result->message.c_str() : "<null>");
}
// Per-instance worker threads handle updating node status; avoid mutating shared state here.
@@ -402,7 +402,7 @@ void CerebrumNode::SendAction(const std::string & name)
if (!action_registry_) {
return;
}
action_registry_->send(name, this->get_logger());
action_registry_->send(name);
}
/**
@@ -443,7 +443,7 @@ void CerebrumNode::CerebrumTask()
robot_work_info_.expt_completion_time = GetFutureTime(300);
robot_work_time_.start = rclcpp::Clock().now();
RCLCPP_WARN(this->get_logger(), "CerebrumTask Switching to BT config file path: %s", path_param.config.c_str());
LOG_WARN("CerebrumTask Switching to BT config file path: {}", path_param.config.c_str());
return;
}
}
@@ -486,7 +486,7 @@ void CerebrumNode::ExecuteBehaviorTree()
robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
auto duration = (this->now() - bt_execution_start_time_).seconds();
RCLCPP_INFO(this->get_logger(), "BT finished status=%s, scheduled=%s, duration=%.2f s",
LOG_INFO("BT finished status={}, scheduled={}, duration={:.2f} s",
BT::toStr(status, true).c_str(), scheduled.c_str(), duration);
}
}
@@ -503,13 +503,13 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
{
BT::Tree new_tree;
if (!bt_registry_->build_tree_from_xml_file(xml_file, new_tree)) {
RCLCPP_ERROR(this->get_logger(), "Failed building BT from file %s", xml_file.c_str());
LOG_ERROR("Failed building BT from file {}", xml_file.c_str());
return;
}
try {
tree_.haltTree();
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
LOG_ERROR("Exception halting previous BT");
}
tree_ = std::move(new_tree);
{
@@ -541,7 +541,7 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
}
bt_execution_start_time_ = this->now();
bt_pending_run_ = true;
RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
LOG_INFO("BT built from file epoch={}", static_cast<unsigned long long>(new_epoch));
}
/**
@@ -560,14 +560,14 @@ void CerebrumNode::UpdateBehaviorTree()
UpdatingFlagGuard guard(bt_updating_);
BT::Tree new_tree;
if (!bt_registry_->build_tree_from_sequence(active_sequence_, new_tree)) {
RCLCPP_ERROR(this->get_logger(), "Failed building BT for sequence %s", active_sequence_.c_str());
LOG_ERROR("Failed building BT for sequence {}", active_sequence_.c_str());
return;
}
try {
tree_.haltTree();
} catch (...) {
// Swallow halt exceptions.
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
LOG_ERROR("Exception halting previous BT");
}
tree_ = std::move(new_tree);
uint64_t new_epoch = epoch_filter_.epoch() + 1;
@@ -594,7 +594,7 @@ void CerebrumNode::UpdateBehaviorTree()
}
bt_execution_start_time_ = this->now();
bt_pending_run_ = true;
RCLCPP_INFO(this->get_logger(), "BT updated epoch=%llu sequence=%s",
LOG_INFO("BT updated epoch={} sequence={}",
static_cast<unsigned long long>(new_epoch), active_sequence_.c_str());
}
@@ -625,7 +625,7 @@ void CerebrumNode::RegisterSkillBtActions()
{
std::lock_guard<std::mutex> lk(exec_mutex_);
if (done_instances_epoch_.find(key) != done_instances_epoch_.end()) {
// RCLCPP_INFO(this->get_logger(), "[%s/%s] skipped: already SUCCESS in current epoch", bt_type.c_str(), instance_name.c_str());
// LOG_INFO("[{}/{}] skipped: already SUCCESS in current epoch", bt_type.c_str(), instance_name.c_str());
return BT::NodeStatus::SUCCESS;
}
}
@@ -644,7 +644,7 @@ void CerebrumNode::RegisterSkillBtActions()
std::string instance_params;
if (!DeclareBtActionParamsForSkillInstance(skill.name, instance_name, &instance_params) ||
!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
LOG_ERROR("ExecuteBtAction server unavailable");
inst.in_flight = false;
inst.finished = true;
inst.success = false;
@@ -665,11 +665,10 @@ void CerebrumNode::RegisterSkillBtActions()
UpdateLeafParallelismStatusLocked();
}
auto logger = this->get_logger();
std::thread([this, key, goal, logger]() {
std::thread([this, key, goal]() {
using EBA = interfaces::action::ExecuteBtAction;
auto res = action_registry_->send_and_wait_with_goal<EBA>(
brain::kExecuteBtActionName, logger, goal,
brain::kExecuteBtActionName, goal,
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(node_timeout_sec_)));
auto it = per_instance_exec_.find(key);
if (it == per_instance_exec_.end()) { return; }
@@ -687,7 +686,7 @@ void CerebrumNode::RegisterSkillBtActions()
inst.finished = true;
}).detach();
RCLCPP_INFO(this->get_logger(), "[%s/%s] launched per-instance ExecuteBtAction goal", bt_type.c_str(), instance_name.c_str());
LOG_INFO("[{}/{}] launched per-instance ExecuteBtAction goal", bt_type.c_str(), instance_name.c_str());
robot_work_info_.bt_node_status = "RUNNING";
return BT::NodeStatus::RUNNING;
};
@@ -723,10 +722,10 @@ void CerebrumNode::RegisterSkillBtActions()
UpdateActionInfoStateLocked(key, "HALTED");
UpdateLeafParallelismStatusLocked();
}
RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_halted", bt_type.c_str());
LOG_INFO("[{}] BTAction on_halted", bt_type.c_str());
};
try {RegisterBtAction(bt_type, handlers);} catch (const BT::BehaviorTreeException & e) {
RCLCPP_WARN(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
LOG_WARN("BT type exists {}: {}", bt_type.c_str(), e.what());
}
}
@@ -744,7 +743,7 @@ void CerebrumNode::RegisterSkillBtActions()
if (!has_match) {
done_instances_epoch_.clear();
per_node_exec_.clear();
RCLCPP_INFO(this->get_logger(), "Cleared done_instances_epoch_ and per_node_exec_ for retry");
LOG_INFO("Cleared done_instances_epoch_ and per_node_exec_ for retry");
} else {
std::size_t removed_count = 0;
for (auto it = done_instances_epoch_.begin(); it != done_instances_epoch_.end(); ) {
@@ -762,14 +761,14 @@ void CerebrumNode::RegisterSkillBtActions()
++it;
}
}
RCLCPP_INFO(this->get_logger(), "Cleared %zu instances matching '%s'", removed_count, match_pattern.c_str());
LOG_INFO("Cleared {} instances matching '{}'", removed_count, match_pattern.c_str());
}
return BT::NodeStatus::SUCCESS;
};
try {
RegisterBtAction("ClearDoneInstances_H", clear_handlers);
} catch (const BT::BehaviorTreeException & e) {
RCLCPP_WARN(this->get_logger(), "BT type exists ClearDoneInstances_H: %s", e.what());
LOG_WARN("BT type exists ClearDoneInstances_H: {}", e.what());
}
}
@@ -786,7 +785,7 @@ bool CerebrumNode::UpdateBtActionParamsForSkillInstance(
const std::string & instance_params)
{
if (skill_name.empty() || instance_name.empty()) {
RCLCPP_ERROR(this->get_logger(), "Update BtAction Params ForSkillInstance called with empty skill_name, instance_name");
LOG_ERROR("Update BtAction Params ForSkillInstance called with empty skill_name, instance_name");
return false;
}
// Per-instance namespace: cerebrum.bt.<Skill>.<Instance>.*
@@ -799,12 +798,12 @@ bool CerebrumNode::UpdateBtActionParamsForSkillInstance(
if (!this->has_parameter(p_payload)) {
this->declare_parameter<std::string>(p_payload, std::string(""));
}
RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill instance %s", instance_name.c_str());
LOG_INFO("Declaring BT action parameters for skill instance {}", instance_name.c_str());
try {
this->set_parameter(rclcpp::Parameter(p_payload, instance_params));
// RCLCPP_INFO(this->get_logger(), "Seeded sample payload for %s instance %s", skill_name.c_str(), instance_name.c_str());
// LOG_INFO("Seeded sample payload for {} instance {}", skill_name.c_str(), instance_name.c_str());
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
LOG_ERROR("[{}] seeding sample payload failed: {}", skill_name.c_str(), e.what());
return false;
}
return true;
@@ -822,16 +821,16 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
std::string * out_instance_params)
{
if (skill_name.empty() || instance_name.empty()) {
RCLCPP_ERROR(this->get_logger(), "Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
LOG_ERROR("Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
return false;
}
if (current_bt_config_params_path_.param.empty() && active_sequence_param_overrides_.empty()) {
RCLCPP_ERROR(this->get_logger(), "BT params file path is empty");
LOG_ERROR("BT params file path is empty");
// return false; //move home no params
}
RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill_name=%s, instance_name=%s",
LOG_INFO("Declaring BT action parameters for skill_name={}, instance_name={}",
skill_name.c_str(), instance_name.c_str());
std::string instance_params;
@@ -852,18 +851,18 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
if (active_sequence_param_overrides_.count(idx)) {
instance_params = active_sequence_param_overrides_[idx];
found_override = true;
RCLCPP_INFO(this->get_logger(), "Found override param at index %zu, instance_params: %s", idx, instance_params.c_str());
LOG_INFO("Found override param at index {}, instance_params: {}", idx, instance_params.c_str());
}
} catch (...) {
// Failed to parse index, ignore
RCLCPP_ERROR(this->get_logger(), "Failed to parse index %s", idx_str.c_str());
LOG_ERROR("Failed to parse index {}", idx_str.c_str());
}
}
}
}
if (current_path_copy.param.empty() && !found_override) {
RCLCPP_ERROR(this->get_logger(), "BT params file path is empty");
LOG_ERROR("BT params file path is empty");
// return false; //move home no params
}
@@ -871,23 +870,23 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
// Use override
} else if (current_path_copy.config == skill_name) {
instance_params = current_path_copy.param;
RCLCPP_INFO(this->get_logger(), "Remote params: %s", instance_params.c_str());
LOG_INFO("Remote params: {}", instance_params.c_str());
} else {
//READ PARAMS FROM ROBOT CONFIG FILE
try {
robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(current_path_copy.param);
auto params = robot_config_params_->GetValue(instance_name, "params");
if (params == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "BT params file %s does not contain params for %s",
LOG_ERROR("BT params file {} does not contain params for {}",
current_path_copy.param.c_str(), instance_name.c_str());
return false;
} else {
instance_params = *params;
// RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s",
// LOG_INFO("Loaded BT params for {}, instance_params: {}",
// instance_name.c_str(), instance_params.c_str());
}
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
LOG_ERROR("[{}] read params failed: {}", skill_name.c_str(), e.what());
return false;
}
}
@@ -953,7 +952,7 @@ void CerebrumNode::SetupDynamicParameterHandling()
break;
}
// No further validation here; downstream will interpret.
RCLCPP_INFO(this->get_logger(), "[params] updated %s", name.c_str());
LOG_INFO("[params] updated {}", name.c_str());
}
return result;
};
@@ -1003,11 +1002,11 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::strin
long parsed = std::stol(v);
if (parsed > 0) call.timeout_ms = static_cast<uint32_t>(parsed);
} catch (...) {
RCLCPP_WARN(this->get_logger(), "[%s] invalid timeout_ms value '%s'", skill_name.c_str(), v.c_str());
LOG_WARN("[{}] invalid timeout_ms value '{}'", skill_name.c_str(), v.c_str());
}
}
} catch (const std::exception & e) {
RCLCPP_WARN(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
LOG_WARN("[{}] read params failed: {}", skill_name.c_str(), e.what());
}
return call;
}
@@ -1050,7 +1049,7 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
long parsed = std::stol(v);
if (parsed > 0) base.timeout_ms = static_cast<uint32_t>(parsed);
} catch (...) {
RCLCPP_WARN(this->get_logger(), "[%s/%s] invalid timeout_ms value '%s'", skill_name.c_str(), instance_name->c_str(), v.c_str());
LOG_WARN("[{}/{}] invalid timeout_ms value '{}'", skill_name.c_str(), instance_name->c_str(), v.c_str());
}
}
@@ -1059,7 +1058,7 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
base.timeout_ms = 500;
}
} catch (const std::exception & e) {
RCLCPP_WARN(this->get_logger(), "[%s/%s] read instance params failed: %s", skill_name.c_str(), instance_name->c_str(), e.what());
LOG_WARN("[{}/{}] read instance params failed: {}", skill_name.c_str(), instance_name->c_str(), e.what());
}
return base;
}
@@ -1176,9 +1175,7 @@ void CerebrumNode::RunVlmModel()
} else {
auto picked = PickRandomActionSkills(random_skill_pick_count_);
if (!register_and_activate("VLMSequence", picked)) {
RCLCPP_WARN(
this->get_logger(),
"No action-capable skills available");
LOG_WARN("No action-capable skills available");
}
}
}
@@ -1317,8 +1314,7 @@ void CerebrumNode::CancelActiveExecuteBtGoal()
{
if (action_registry_) {
action_registry_->cancel(
brain::kExecuteBtActionName,
this->get_logger());
brain::kExecuteBtActionName);
}
}
@@ -1329,7 +1325,7 @@ void CerebrumNode::StopBehaviorTree(bool cancel_goals)
try {
CancelActiveExecuteBtGoal();
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "Exception while cancelling ExecuteBtAction goals");
LOG_ERROR("Exception while cancelling ExecuteBtAction goals");
}
}
@@ -1341,7 +1337,7 @@ void CerebrumNode::StopBehaviorTree(bool cancel_goals)
try {
tree_.haltTree();
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "Exception halting Behavior Tree");
LOG_ERROR("Exception halting Behavior Tree");
}
// 4) Stop periodic ticking and clear cooldown gate
@@ -1374,7 +1370,7 @@ void CerebrumNode::StopBehaviorTree(bool cancel_goals)
robot_work_info_.bt_node_status = "CANCELLED";
robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
RCLCPP_INFO(this->get_logger(), "Behavior Tree stopped (epoch=%llu)", static_cast<unsigned long long>(new_epoch));
LOG_INFO("Behavior Tree stopped (epoch={})", static_cast<unsigned long long>(new_epoch));
}
/**
@@ -1472,7 +1468,7 @@ void CerebrumNode::LoadParameters()
this->get_parameter("allowed_action_skills", allowed_raw);
if (!allowed_raw.empty()) {
allowed_action_skills_ = ParseListString(allowed_raw);
RCLCPP_INFO(this->get_logger(), "Allowed action skills count=%zu", allowed_action_skills_.size());
LOG_INFO("Allowed action skills count={}", allowed_action_skills_.size());
}
std::string fixed_seq_raw;
@@ -1482,7 +1478,7 @@ void CerebrumNode::LoadParameters()
if (!tmp.empty()) {fixed_skill_sequence_ = tmp;}
}
if (fixed_skill_sequence_) {
RCLCPP_INFO(this->get_logger(), "Using fixed skill sequence size=%zu", fixed_skill_sequence_->size());
LOG_INFO("Using fixed skill sequence size={}", fixed_skill_sequence_->size());
}
}
@@ -1507,7 +1503,7 @@ void CerebrumNode::CreateServices()
StopBehaviorTree(true);
resp->success = true;
resp->message = "No active BT (cleanup applied)";
RCLCPP_WARN(this->get_logger(), "No active BT to cancel (cleanup applied)");
LOG_WARN("No active BT to cancel (cleanup applied)");
return;
}
StopBehaviorTree(true);
@@ -1531,11 +1527,11 @@ void CerebrumNode::CreateServices()
// TODO: Implement local rebuild
resp->success = false;
resp->message = "Local rebuild not implemented";
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Local rebuild not implemented");
LOG_ERROR("cerebrum/rebuild_now Service Local rebuild not implemented");
} else {
resp->success = false;
resp->message = "Unknown rebuild type";
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Unknown rebuild type: %s", req->type.c_str());
LOG_ERROR("cerebrum/rebuild_now Service Unknown rebuild type: {}", req->type.c_str());
}
// Update work info for successful rebuilds
@@ -1563,10 +1559,10 @@ void CerebrumNode::HandleTriggerRebuild(
std::lock_guard<std::mutex> lk(exec_mutex_);
active_sequence_param_overrides_.clear(); // Clear overrides on cancel
}
RCLCPP_INFO(this->get_logger(), "halting previous BT ok, CancelBTTask");
LOG_INFO("halting previous BT ok, CancelBTTask");
} catch (...) {
// Swallow halt exceptions.
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT CancelBTTask");
LOG_ERROR("Exception halting previous BT CancelBTTask");
}
resp->success = true;
resp->message = "Rebuild triggered";
@@ -1582,7 +1578,7 @@ void CerebrumNode::HandleTriggerRebuild(
std::string sch_filename = ExtractFilenameWithoutExtension(path_param.config);
if (sch_filename != req->config) {
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service INVALID config: %s, local: %s", req->config.c_str(), sch_filename.c_str());
LOG_WARN("cerebrum/rebuild_now Service INVALID config: {}, local: {}", req->config.c_str(), sch_filename.c_str());
continue;
}
@@ -1600,7 +1596,7 @@ void CerebrumNode::HandleTriggerRebuild(
robot_work_info_.task[2] = sch_filename; // current
}
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
LOG_WARN("cerebrum/rebuild_now Service Switching to BT config file path: {}", path_param.config.c_str());
match_found = true;
break;
}
@@ -1611,7 +1607,7 @@ void CerebrumNode::HandleTriggerRebuild(
} else {
resp->success = false;
resp->message = "Rebuild config not found";
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to find config: %s", req->config.c_str());
LOG_ERROR("cerebrum/rebuild_now Service Failed to find config: {}", req->config.c_str());
}
}
@@ -1631,7 +1627,7 @@ void CerebrumNode::HandleGenericRebuild(
if (!skills.empty()) {
// Check if params match skills count
if (!params.empty() && params.size() != skills.size()) {
RCLCPP_WARN(this->get_logger(), "HandleGenericRebuild: Skills count (%zu) != Params count (%zu).", skills.size(), params.size());
LOG_WARN("HandleGenericRebuild: Skills count ({}) != Params count ({}).", skills.size(), params.size());
}
// Clear previous override params and update config safely
@@ -1662,11 +1658,11 @@ void CerebrumNode::HandleGenericRebuild(
resp->success = true;
resp->message = req->type + " rebuild triggered";
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service %s rebuild triggered", req->type.c_str());
LOG_WARN("cerebrum/rebuild_now Service {} rebuild triggered", req->type.c_str());
} else {
resp->success = false;
resp->message = req->type + " rebuild failed";
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to parse %s rebuild config", req->type.c_str());
LOG_ERROR("cerebrum/rebuild_now Service Failed to parse {} rebuild config", req->type.c_str());
}
}
@@ -1704,7 +1700,7 @@ bool CerebrumNode::LoadRobotConfiguration()
robot_config_ = std::make_unique<brain::robot_config::RobotConfig>(config_path);
auto skill_file = robot_config_->SkillFile("brain");
if (skill_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
LOG_ERROR("No skill_file entry found for 'brain' in robot_config.yaml");
return false;
}
@@ -1713,13 +1709,13 @@ bool CerebrumNode::LoadRobotConfiguration()
? share_directory_ + *skill_file
: skill_file_path_;
RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
LOG_WARN("skill file {}", robot_skill_file_path_.c_str());
auto path = robot_config_->ConfigParamsPath("cerebrum_node");
if (path != std::nullopt) {
for (const auto & p : *path) {
if (!p.config.empty() && !p.param.empty()) {
bt_config_params_paths_.push_back({share_directory_ + p.config, share_directory_ + p.param});
RCLCPP_WARN(this->get_logger(), "bt config param %s %s", p.config.c_str(), p.param.c_str());
LOG_WARN("bt config param {} {}", p.config.c_str(), p.param.c_str());
}
}
}
@@ -1744,7 +1740,7 @@ int CerebrumNode::SimulatedBatteryCapacity()
if (elapsed >= kBatteryDecrementIntervalSec) {
if (simulated_battery_capacity > 0) {
--simulated_battery_capacity;
RCLCPP_INFO(this->get_logger(), "Battery simulator: decreased to %d%%", simulated_battery_capacity);
LOG_INFO("Battery simulator: decreased to {}%%", simulated_battery_capacity);
}
last_battery_update = this->now();
}
@@ -1791,7 +1787,7 @@ void CerebrumNode::RobotWorkInfoPublish()
interfaces::msg::RobotWorkInfo>("/robot_work_info", 10);
if (!robot_work_info_pub_) {
RCLCPP_ERROR(this->get_logger(), "Failed to create robot work info publisher");
LOG_ERROR("Failed to create robot work info publisher");
return;
}
@@ -1847,12 +1843,12 @@ void CerebrumNode::RobotWorkInfoPublish()
if (robot_work_info_pub_) {
robot_work_info_pub_->publish(msg);
// RCLCPP_WARN(this->get_logger(), "Publishing robot work info id: %ld", msg.msg_id);
// LOG_WARN("Publishing robot work info id: {}", msg.msg_id);
}
});
if (!pub_robot_work_info_timer_) {
RCLCPP_ERROR(this->get_logger(), "Failed to create robot work info timer");
LOG_ERROR("Failed to create robot work info timer");
}
}

23
src/brain/src/skill_manager.cpp
View File

@@ -14,6 +14,7 @@
#include <cmath>
#include <utility>
#include <rclcpp_action/rclcpp_action.hpp>
#include <hivecore_logger/logger.hpp>
namespace brain
{
@@ -87,7 +88,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
{
YAML::Node root = YAML::LoadFile(yaml_path);
if (!root || !root.IsSequence()) {
RCLCPP_ERROR(node_->get_logger(), "Invalid skills YAML: expected sequence at root");
LOG_ERROR("Invalid skills YAML: expected sequence at root");
return false;
}
@@ -112,7 +113,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
// Extended form: { name: BaseName.kind, default_topic: <str> }
const auto name_node = v["name"];
if (!name_node || !name_node.IsScalar()) {
RCLCPP_ERROR(node_->get_logger(), "Invalid interface entry (missing 'name'): %s", YAML::Dump(v).c_str());
LOG_ERROR("Invalid interface entry (missing 'name'): {}", YAML::Dump(v).c_str());
continue;
}
const std::string token = name_node.as<std::string>("");
@@ -126,7 +127,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
}
}
} else {
RCLCPP_ERROR(node_->get_logger(), "Unsupported interface entry type: %s", YAML::Dump(v).c_str());
LOG_ERROR("Unsupported interface entry type: {}", YAML::Dump(v).c_str());
}
}
}
@@ -135,7 +136,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
register_interfaces_(s);
}
RCLCPP_INFO(node_->get_logger(), "Loaded %zu skills from %s", skills_.size(), yaml_path.c_str());
LOG_INFO("Loaded {} skills from {}", skills_.size(), yaml_path.c_str());
return true;
}
@@ -275,15 +276,15 @@ static std::string join_keys(const std::unordered_map<std::string, T> & m)
}
template<size_t I = 0>
static void validate_action_registrars_complete(rclcpp::Logger logger)
static void validate_action_registrars_complete()
{
if constexpr (I < std::tuple_size<SkillActionTypes>::value) {
using ActionT = std::tuple_element_t<I, SkillActionTypes>;
const char * name = SkillActionTrait<ActionT>::skill_name;
if (get_action_registrars().find(name) == get_action_registrars().end()) {
RCLCPP_ERROR(logger, "Missing action registrar for '%s'", name);
LOG_ERROR("Missing action registrar for '{}'", name);
}
validate_action_registrars_complete<I + 1>(logger);
validate_action_registrars_complete<I + 1>();
}
}
@@ -300,7 +301,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
// One-time runtime validation to ensure all actions in SkillActionTypes have a registrar.
static bool s_validated = false;
if (!s_validated) {
validate_action_registrars_complete(node_->get_logger());
validate_action_registrars_complete();
s_validated = true;
}
@@ -316,7 +317,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
for (const auto & iface : s.interfaces) {
auto parsed = parse_interface_entry(iface);
if (!parsed) {
RCLCPP_ERROR(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
LOG_ERROR("Skip invalid interface entry: {}", iface.c_str());
continue;
}
@@ -366,7 +367,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
if (it != action_regs.end()) {
it->second(this, topic_name, s.name);
} else {
RCLCPP_ERROR(node_->get_logger(), "No action registrar for interface base '%s'. Available: [%s]",
LOG_ERROR("No action registrar for interface base '{}'. Available: [{}]",
parsed->base.c_str(), join_keys(action_regs).c_str());
}
} else if (parsed->kind == "srv") {
@@ -376,7 +377,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
if (it != service_regs.end()) {
it->second(this, topic_name, s.name);
} else {
RCLCPP_ERROR(node_->get_logger(), "No service registrar for interface base '%s'. Available: [%s]",
LOG_ERROR("No service registrar for interface base '{}'. Available: [{}]",
parsed->base.c_str(), join_keys(service_regs).c_str());
}
}