Skip to content

aegis_grpc: Servoing idling & minor improvments #103

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
macmacal merged 8 commits into from
Feb 16, 2026
Merged

aegis_grpc: Servoing idling & minor improvments #103

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
9b5f1fa
added prefix to grpc_client logger
macmacal Feb 16, 2026
148f6b2
channged logs level and fixed typos
macmacal Feb 16, 2026
429ed37
servoing idle publishing
macmacal Feb 16, 2026
d660963
changed assert to if
macmacal Feb 16, 2026
6f50062
updated readme
macmacal Feb 16, 2026
0f83200
updated changelog
macmacal Feb 16, 2026
d881ef1
Merge branch 'humble-devel' into feature/aegis_grpc/minor_improvments
macmacal Feb 16, 2026
0758311
removed old todo
macmacal Feb 16, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion aegis_grpc/CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
9 changes: 5 additions & 4 deletions aegis_grpc/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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` |
Expand Down Expand Up @@ -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. |
Expand Down
4 changes: 3 additions & 1 deletion aegis_grpc/include/aegis_grpc/robot_control_service.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define AEGIS_GRPC__ROBOT_CONTROL_SERVICE_HPP_
#include <atomic>
#include <chrono>
#include <memory>

#include <grpcpp/grpcpp.h>
#include <google/protobuf/empty.pb.h>
Expand Down Expand Up @@ -29,7 +30,6 @@ class RobotControlServiceImpl final : public proto_aegis_grpc::v1::RobotControlS

explicit RobotControlServiceImpl(std::shared_ptr<rclcpp::Node> 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;
Expand Down Expand Up @@ -95,6 +95,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_;

Expand Down
1 change: 0 additions & 1 deletion aegis_grpc/include/aegis_grpc/robot_read_service.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
7 changes: 6 additions & 1 deletion aegis_grpc/python_client/aegis_grpc_client/grpc_client.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
Expand Down
59 changes: 33 additions & 26 deletions aegis_grpc/src/robot_control_service.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -206,24 +206,31 @@ void RobotControlServiceImpl::ServoPublishLoop() {

{
std::lock_guard<std::mutex> 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) {
Expand Down Expand Up @@ -252,12 +259,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);
}

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -397,7 +404,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);
}
Expand All @@ -417,15 +424,15 @@ 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);
}

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);
}
Expand All @@ -444,7 +451,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);
}
Expand All @@ -460,15 +467,15 @@ 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);
}

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);
}
Expand Down
5 changes: 4 additions & 1 deletion aegis_grpc/src/robot_read_service.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down