robossembler/cnc-graver-assist
8
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

refactor(logging): print BT action ID

Browse source
This commit is contained in:
Ilya Uraev 2025-03-10 14:50:02 +03:00
parent 2a85dfce18
commit bb2599a006
4 changed files with 73 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

25
rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp
View file

@ -34,11 +34,12 @@ public:
}
bool setRequest(Request::SharedPtr &request) override {
RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
this->service_name_.c_str());
RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
name().c_str(), this->service_name_.c_str());
if (!getInput("action_name", request->action_name)) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Failed to get object_name from input port");
"[%s] Failed to get object_name from input port",
name().c_str());
return false;
}
return true;
@ -47,25 +48,25 @@ public:
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Response indicates failure.");
"[%s] Response indicates failure.", name().c_str());
return NodeStatus::FAILURE;
}
RCLCPP_INFO(this->node_.lock()->get_logger(),
"Response received successfully.");
"[%s] Response received successfully.", name().c_str());
auto logPose = [this](const std::string &pose_name,
const geometry_msgs::msg::Pose &pose) {
RCLCPP_INFO(this->node_.lock()->get_logger(),
"%s:\n"
"[%s] %s:\n"
" Position: x = %.4f, y = %.4f, z = %.4f\n"
" Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
pose_name.c_str(), pose.position.x, pose.position.y,
pose.position.z, pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w);
name().c_str(), pose_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 (!response->grasp.empty()) {
RCLCPP_INFO(this->logger(), "Got Pick Response");
RCLCPP_INFO(this->logger(), "[%s] Got Pick Response", name().c_str());
std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
"Postgrasp Pose"};
@ -87,7 +88,7 @@ public:
return NodeStatus::SUCCESS;
}
if (!response->place.empty()) {
RCLCPP_INFO(this->logger(), "Got Place Response");
RCLCPP_INFO(this->logger(), "[%s] Got Place Response", name().c_str());
std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
"Postplace Pose"};
@ -109,7 +110,7 @@ public:
return NodeStatus::SUCCESS;
}
RCLCPP_FATAL(this->logger(), "Could not response grasp pose");
RCLCPP_FATAL(this->logger(), "[%s] Could not response grasp pose", name().c_str());
return NodeStatus::FAILURE;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}

14
rbs_mill_assist/bt/plugins/get_named_pose.cpp
View file

@ -31,11 +31,12 @@ public:
}
bool setRequest(Request::SharedPtr &request) override {
RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
this->service_name_.c_str());
RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
name().c_str(), this->service_name_.c_str());
if (!getInput("pose_name", request->pose_name)) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Failed to get pose_name from input port");
"[%s] Failed to get pose_name from input port",
name().c_str());
return false;
}
return true;
@ -43,13 +44,14 @@ public:
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
RCLCPP_ERROR(this->logger(), "Response indicates failure.");
RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.",
name().c_str());
return NodeStatus::FAILURE;
}
RCLCPP_INFO(this->logger(),
"Response received successfully with pose name [%s]",
response->named_pose.name.c_str());
"[%s] Response received successfully with pose name [%s]",
name().c_str(), response->named_pose.name.c_str());
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
*pose = response->named_pose.pose;

48
rbs_mill_assist/bt/plugins/is_in_pose.cpp Normal file
View file

@ -0,0 +1,48 @@
#include "behaviortree_ros2/bt_topic_sub_node.hpp"
#include "behaviortree_ros2/plugins.hpp"
#include <behaviortree_cpp/basic_types.h>
// #include <geometry_msgs/msg/detail/point__struct.hpp>
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "rbs_utils_interfaces/srv/get_named_pose.hpp"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
using MSG = geometry_msgs::msg::Pose;
using namespace BT;
class IsInPose : public RosTopicSubNode<MSG> {
public:
IsInPose(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosTopicSubNode<MSG>(name, conf, params) {
RCLCPP_INFO(this->logger(), "Starting IsInPose");
}
//
// static PortsList providedPorts() {
// return {};
// }
NodeStatus onTick(const std::shared_ptr<MSG> &t_last_msg) override {
if (t_last_msg) {
RCLCPP_INFO(this->logger(), "[%s] Got message Pose", name().c_str());
m_last_msg = t_last_msg;
return NodeStatus::SUCCESS;
}
RCLCPP_INFO(this->logger(), "[%s] NOT message received", name().c_str());
return NodeStatus::SUCCESS;
}
bool latchLastMessage() const override {
return true;
}
private:
MSG::SharedPtr m_last_msg;
};
CreateRosNodePlugin(IsInPose, "IsInPose");

8
rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp
View file

@ -24,10 +24,10 @@ public:
}
bool setRequest(Request::SharedPtr &request) override {
RCLCPP_INFO(this->logger(), "Starting send request");
RCLCPP_INFO(this->logger(), "[%s] Starting send request", name().c_str());
if (!getInput("enable", request->data)) {
RCLCPP_ERROR(this->node_.lock()->get_logger(),
"Failed to get sending data from input port");
RCLCPP_ERROR(this->logger(),
"[%s] Failed to get sending data from input port", name().c_str());
return false;
}
return true;
@ -35,7 +35,7 @@ public:
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->success) {
RCLCPP_ERROR(this->logger(), "Response indicates failure.");
RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.", name().c_str());
return NodeStatus::FAILURE;
}