From 9b5f1fa75727039297e82123dff33d251acd0e8b Mon Sep 17 00:00:00 2001 From: macmacal Date: Mon, 16 Feb 2026 15:05:01 +0100 Subject: [PATCH 1/7] added prefix to grpc_client logger --- aegis_grpc/python_client/aegis_grpc_client/grpc_client.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/aegis_grpc/python_client/aegis_grpc_client/grpc_client.py b/aegis_grpc/python_client/aegis_grpc_client/grpc_client.py index 66f55189..34b5d70e 100644 --- a/aegis_grpc/python_client/aegis_grpc_client/grpc_client.py +++ b/aegis_grpc/python_client/aegis_grpc_client/grpc_client.py @@ -15,6 +15,11 @@ ) +class PrefixedLoggerAdapter(logging.LoggerAdapter): + def process(self, msg, kwargs): + return f"[aegis_robot_client] {msg}", kwargs + + class AegisRobotClient: """ Async gRPC client for Aegis Robot Control System. @@ -30,7 +35,7 @@ def __init__(self, server_address: str = "127.0.0.1:50051"): Args: server_address: gRPC server address in format "host:port" """ - self.logger = logging.getLogger("aegis_grpc_client") + self.logger = PrefixedLoggerAdapter(logging.getLogger("aegis_grpc_client"), {}) self.server_address = server_address self.channel: Optional[grpc.aio.Channel] = None self.read_stub: Optional[robot_srvs_pb2_grpc.RobotReadServiceStub] = None From 148f6b2c7cbace7f9d72a2d6c7a3cecbdab1ef0e Mon Sep 17 00:00:00 2001 From: macmacal Date: Mon, 16 Feb 2026 15:06:10 +0100 Subject: [PATCH 2/7] channged logs level and fixed typos --- .../aegis_grpc/robot_control_service.hpp | 1 + .../include/aegis_grpc/robot_read_service.hpp | 1 - aegis_grpc/src/robot_control_service.cpp | 18 +++++++++--------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp index d371d610..713a9c38 100644 --- a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp +++ b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp @@ -2,6 +2,7 @@ #define AEGIS_GRPC__ROBOT_CONTROL_SERVICE_HPP_ #include #include +#include #include #include diff --git a/aegis_grpc/include/aegis_grpc/robot_read_service.hpp b/aegis_grpc/include/aegis_grpc/robot_read_service.hpp index 252f2348..aed53def 100644 --- a/aegis_grpc/include/aegis_grpc/robot_read_service.hpp +++ b/aegis_grpc/include/aegis_grpc/robot_read_service.hpp @@ -61,7 +61,6 @@ class RobotReadServiceImpl final : public proto_aegis_grpc::v1::RobotReadService const google::protobuf::Empty* request, proto_aegis_grpc::v1::RobotObservation* response) override; - // TODO(issue#84) implement getters for images from cameras (RGB & RGBD) // TODO(issue#85) develop synchronization guard to monitor the freshness of the data private: diff --git a/aegis_grpc/src/robot_control_service.cpp b/aegis_grpc/src/robot_control_service.cpp index f56089b8..c262c0d0 100644 --- a/aegis_grpc/src/robot_control_service.cpp +++ b/aegis_grpc/src/robot_control_service.cpp @@ -164,7 +164,7 @@ grpc::Status RobotControlServiceImpl::ServoEnable(grpc::ServerContext*, response->set_success(true); response->set_msg(""); - RCLCPP_INFO(get_logger(), "[RobotControlService][ServoDisable] Servo enabled"); + RCLCPP_INFO(get_logger(), "[RobotControlService][ServoEnable] Servo enabled"); return grpc::Status::OK; } @@ -252,12 +252,12 @@ grpc::Status RobotControlServiceImpl::ServoJoint(grpc::ServerContext* context, auto N = request->joint_names_size(); if (N != request->displacements_size()) { std::string err = "The number of joints mismatches the number of displacmenets values!"; - RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str()); return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err); } if (N != request->velocities_size()) { std::string err = "The number of joints mismatches the number of velocities values!"; - RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str()); return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err); } @@ -397,7 +397,7 @@ grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context, if (!request->has_position() || !request->has_orientation()) { std::string err = "Missing position or orientation."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str()); response->set_msg(err); return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err); } @@ -417,7 +417,7 @@ grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context, auto result = move_group_->plan(plan); if (result != moveit::core::MoveItErrorCode::SUCCESS) { std::string err = "Planning failed."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str()); response->set_msg(err); return grpc::Status(grpc::StatusCode::INTERNAL, err); } @@ -425,7 +425,7 @@ grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context, auto exec = move_group_->execute(plan); if (exec != moveit::core::MoveItErrorCode::SUCCESS) { std::string err = "Execution failed."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GoToPose] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][GoToPose] %s", err.c_str()); response->set_msg(err); return grpc::Status(grpc::StatusCode::INTERNAL, err); } @@ -444,7 +444,7 @@ grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context, if (request->name_size() == 0 || request->name_size() != request->position_size()) { std::string err = "Joint names and positions mismatch or empty."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); response->set_msg(err); return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err); } @@ -460,7 +460,7 @@ grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context, auto result = move_group_->plan(plan); if (result != moveit::core::MoveItErrorCode::SUCCESS) { std::string err = "Planning failed."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); response->set_msg(err); return grpc::Status(grpc::StatusCode::INTERNAL, err); } @@ -468,7 +468,7 @@ grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context, auto exec = move_group_->execute(plan); if (exec != moveit::core::MoveItErrorCode::SUCCESS) { std::string err = "Execution failed."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); + RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); response->set_msg(err); return grpc::Status(grpc::StatusCode::INTERNAL, err); } From 429ed37db1d45fd0953e173f3839e13912026aa6 Mon Sep 17 00:00:00 2001 From: macmacal Date: Mon, 16 Feb 2026 15:08:51 +0100 Subject: [PATCH 3/7] servoing idle publishing --- .../aegis_grpc/robot_control_service.hpp | 2 + aegis_grpc/src/robot_control_service.cpp | 41 +++++++++++-------- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp index 713a9c38..051621d6 100644 --- a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp +++ b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp @@ -96,6 +96,8 @@ class RobotControlServiceImpl final : public proto_aegis_grpc::v1::RobotControlS std::string servo_tcp_link_; control_msgs::msg::JointJog servo_joint_msg_; geometry_msgs::msg::Twist servo_tcp_msg_; + const control_msgs::msg::JointJog servo_joint_msg_zeros_; + const geometry_msgs::msg::Twist servo_tcp_msg_zeros_; int servo_frequency_ratio_; int servo_msgs_left_; diff --git a/aegis_grpc/src/robot_control_service.cpp b/aegis_grpc/src/robot_control_service.cpp index c262c0d0..d260a72f 100644 --- a/aegis_grpc/src/robot_control_service.cpp +++ b/aegis_grpc/src/robot_control_service.cpp @@ -206,24 +206,31 @@ void RobotControlServiceImpl::ServoPublishLoop() { { std::lock_guard lock(servo_mutex_); - if (servo_msgs_left_ == 0) { - servo_mode_ = ServoMode::None; - return; - } - mode = servo_mode_; - switch (mode) { - case ServoMode::JointJog: - jog_msg = servo_joint_msg_; - break; - case ServoMode::TCPTwist: - twist_msg.twist = servo_tcp_msg_; - break; - default: - break; + if (servo_msgs_left_ == 0) { + switch (mode) { + case ServoMode::JointJog: + jog_msg = servo_joint_msg_zeros_; + break; + case ServoMode::TCPTwist: + twist_msg.twist = servo_tcp_msg_zeros_; + break; + default: + break; + } + } else { + switch (mode) { + case ServoMode::JointJog: + jog_msg = servo_joint_msg_; + break; + case ServoMode::TCPTwist: + twist_msg.twist = servo_tcp_msg_; + break; + default: + break; + } + servo_msgs_left_--; } - - servo_msgs_left_--; } switch (mode) { @@ -288,11 +295,11 @@ grpc::Status RobotControlServiceImpl::ServoTCP(grpc::ServerContext* context, google::protobuf::Empty* response) { (void)context; (void)response; + static auto ros_msg = geometry_msgs::msg::Twist(); const auto& lin = request->linear(); const auto& ang = request->angular(); - auto ros_msg = geometry_msgs::msg::Twist(); ros_msg.linear.x = lin.x(); ros_msg.linear.y = lin.y(); ros_msg.linear.z = lin.z(); From d660963503c748416a72b991a3d555bb2948cf7f Mon Sep 17 00:00:00 2001 From: macmacal Date: Mon, 16 Feb 2026 15:09:17 +0100 Subject: [PATCH 4/7] changed assert to if --- aegis_grpc/src/robot_read_service.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/aegis_grpc/src/robot_read_service.cpp b/aegis_grpc/src/robot_read_service.cpp index d27240a5..03ec446c 100644 --- a/aegis_grpc/src/robot_read_service.cpp +++ b/aegis_grpc/src/robot_read_service.cpp @@ -288,7 +288,10 @@ void RobotReadServiceImpl::FillProtoJointState(const sensor_msgs::msg::JointStat void RobotReadServiceImpl::FillProtoImage(const sensor_msgs::msg::Image& ros, proto_aegis_grpc::v1::Image* out, cv::Mat& buffer) { - assert((ros.encoding == "bgr8" && "Unsupported image encoding (expected BGR8).")); + if (ros.encoding != "bgr8") { + RCLCPP_ERROR(node_->get_logger(), "Unsupported image encoding (expected 'bgr8'), got: '%s'", ros.encoding.c_str()); + return; + } if (!enable_image_resize_) { out->set_height(ros.height); out->set_width(ros.width); From 6f500625af81f1f3fa967faa68b9ee7f43974875 Mon Sep 17 00:00:00 2001 From: macmacal Date: Mon, 16 Feb 2026 15:09:27 +0100 Subject: [PATCH 5/7] updated readme --- aegis_grpc/README.md | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/aegis_grpc/README.md b/aegis_grpc/README.md index 8955939e..d99522fe 100644 --- a/aegis_grpc/README.md +++ b/aegis_grpc/README.md @@ -105,7 +105,6 @@ The "ROS-getters" are implemented in the [`RobotReadServiceImpl`](./include/aegi | `proto_aegis_grpc.v1.RobotReadService.GetCameraRightImage` | Get right tool camera image. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.sensor_msgs.Image`](./proto_aegis_grpc/v1/sensor_msgs.proto) | | `proto_aegis_grpc.v1.RobotReadService.GetCameraLeftImage` | Get left tool camera image. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.sensor_msgs.Image`](./proto_aegis_grpc/v1/sensor_msgs.proto) | - You can always list the methods with the `grpcurl` command: ```bash grpcurl -plaintext 127.0.0.1:50051 list proto_aegis_grpc.v1.RobotReadService @@ -117,10 +116,12 @@ The control bridge for MoveIt2 servo and planners are implemented in the [`Robot | Method name | Desc. | Impl. | gRPC Request | gRPC Response | | ------------------------------------------------------------ | --------------------------------------------------------------- | ----- | ---------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------ | -| `proto_aegis_grpc.v1.RobotControlService.GotoJoints` | Plan & execute the MoveIt2 trajectory to a given joints target. | ❌ | [`proto_aegis_grpc.v1.sensor_msgs.JointState`](./proto_aegis_grpc/v1/sensor_msgs.proto) | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | -| `proto_aegis_grpc.v1.RobotControlService.GotoPose` | Plan & execute the MoveIt2 trajectory to a given pose target. | ❌ | [`proto_aegis_grpc.v1.geometry_msgs.Pose`](./proto_aegis_grpc/v1/geometry_msgs.proto) | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | +| `proto_aegis_grpc.v1.RobotControlService.GotoJoints` | Plan & execute the MoveIt2 trajectory to a given joints target. | ✅ | [`proto_aegis_grpc.v1.sensor_msgs.JointState`](./proto_aegis_grpc/v1/sensor_msgs.proto) | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | +| `proto_aegis_grpc.v1.RobotControlService.GotoPose` | Plan & execute the MoveIt2 trajectory to a given pose target. | ✅ | [`proto_aegis_grpc.v1.geometry_msgs.Pose`](./proto_aegis_grpc/v1/geometry_msgs.proto) | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | | `proto_aegis_grpc.v1.RobotControlService.GripperClose` | Closes the gripper. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | | `proto_aegis_grpc.v1.RobotControlService.GripperOpen` | Opens the gripper. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | +| `proto_aegis_grpc.v1.RobotControlService.ServoEnable` | Enable servo control. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | +| `proto_aegis_grpc.v1.RobotControlService.ServoDisable` | Disable servo control. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | | `proto_aegis_grpc.v1.RobotControlService.GripperSetPosition` | Set the gripper width. | ✅ | [`proto_aegis_grpc.v1.robot_srvs.GripperSetPositionRequest`](./proto_aegis_grpc/v1/robot_srvs.proto) | [`proto_aegis_grpc.v1.robot_srvs.TriggerResponse`](./proto_aegis_grpc/v1/robot_srvs.proto) | | `proto_aegis_grpc.v1.RobotControlService.ServoJoint` | Servoing (w.r.t. joints). | ✅ | [`proto_aegis_grpc.v1.control_msgs.JointJog`](./proto_aegis_grpc/v1/control_msgs.proto) | `google.protobuf.Empty` | | `proto_aegis_grpc.v1.RobotControlService.ServoTCP` | Servoing (w.r.t. TCP). | ✅ | [`proto_aegis_grpc.v1.geometry_msgs.Twist`](./proto_aegis_grpc/v1/geometry_msgs.proto) | `google.protobuf.Empty` | @@ -176,7 +177,7 @@ There is no hardcoded parameters, everything can be modified via ROS: | Parameter | Type | Default | Units | Description | | --------------------- | -------- | ------------------------------- | ----- | ------------------------------------------------ | -| `tcp_frame` | `string` | `"robotiq_hande_end"` | - | TF2 frame ID of the end-effector. | +| `tcp_frame` | `string` | `"robotiq_hande_end"` | - | TF2 frame ID of the end-effector. | | `base_frame` | `string` | `"world"` | - | TF2 base frame ID for EE pose lookup. | | `topic_wrench` | `string` | `"/wrench"` | - | Topic providing force/torque (F/T) data. | | `topic_joints` | `string` | `"/joint_states"` | - | Topic providing joint state data. | From 0f8320070234960ae92bc7a72b8e5cc957e19a36 Mon Sep 17 00:00:00 2001 From: macmacal Date: Mon, 16 Feb 2026 15:21:18 +0100 Subject: [PATCH 6/7] updated changelog --- aegis_grpc/CHANGELOG.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/aegis_grpc/CHANGELOG.md b/aegis_grpc/CHANGELOG.md index ca6be381..95bd379c 100644 --- a/aegis_grpc/CHANGELOG.md +++ b/aegis_grpc/CHANGELOG.md @@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* [PR-103](https://github.com/AGH-CEAI/aegis_ros/pull/103) - Server: Adds servo idling with zero commands. * [PR-98](https://github.com/AGH-CEAI/aegis_ros/pull/98) - Added gRPC-based camera image reading. * [PR-97](https://github.com/AGH-CEAI/aegis_ros/pull/97) - Added gRPC func: MoveIt2 servo start & stop service. * [PR-96](https://github.com/AGH-CEAI/aegis_ros/pull/96) - Added gRPC func: ros2_control controller switching. @@ -19,9 +20,12 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed -* [PR-102](https://github.com/AGH-CEAI/aegis_ros/pull/102) - The end effector's TCP pose is now taken from the tf2 transform instead of a given topic. +* [PR-103](https://github.com/AGH-CEAI/aegis_ros/pull/103) - Server: Changed `assert()` in `FillProtoImage()` to `if` which loggs an error message. +* [PR-103](https://github.com/AGH-CEAI/aegis_ros/pull/103) - Client: The Python client `aegis_grpc.py` will now log every message with the prefix `[aegis_robot_client]`. +* [PR-102](https://github.com/AGH-CEAI/aegis_ros/pull/102) - Server: The end effector's TCP pose is now taken from the tf2 transform instead of a given topic. ### Deprecated ### Removed ### Fixed + ### Security From 075831198ccbcd5fbb918ed8f01e50533e6b78de Mon Sep 17 00:00:00 2001 From: macmacal Date: Mon, 16 Feb 2026 16:57:18 +0100 Subject: [PATCH 7/7] removed old todo --- aegis_grpc/include/aegis_grpc/robot_control_service.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp index 051621d6..b0dd9047 100644 --- a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp +++ b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp @@ -30,7 +30,6 @@ class RobotControlServiceImpl final : public proto_aegis_grpc::v1::RobotControlS explicit RobotControlServiceImpl(std::shared_ptr node); - // TODO(issue#87) Create additional methods to enable and disable servo control. grpc::Status ServoEnable(grpc::ServerContext* context, const google::protobuf::Empty* request, proto_aegis_grpc::v1::TriggerResponse* response) override;