Skip to content

aegis_grpc: tcp pose from tf transform #102

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 2 commits into from
Feb 16, 2026
Merged

aegis_grpc: tcp pose from tf transform #102

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
05a0922
CHG: EE pose taken from tf2 transform
macmacal Feb 16, 2026
4cf5d0e
Merge branch 'humble-devel' into feature/aegis_grpc/tcp_pose_from_tf_…
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
3 changes: 3 additions & 0 deletions aegis_grpc/CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
* [PR-82](https://github.com/AGH-CEAI/aegis_ros/pull/82) - MVP of the gRPC-ROS server.

### 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.

### Deprecated
### Removed
### Fixed
Expand Down
26 changes: 18 additions & 8 deletions aegis_grpc/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ The server is split into 2 services defined in [`proto_aegis_grpc.v1.robot_srvs`
The "ROS-getters" are implemented in the [`RobotReadServiceImpl`](./include/aegis_grpc/robot_read_service.hpp) class as the following methods:

| Method name | Desc. | Impl. | gRPC Request | gRPC Response |
|------------------------------------------------------------|------------------------------------------------------|-------|-------------------------|---------------------------------------------------------------------------------------------|
| ---------------------------------------------------------- | ---------------------------------------------------- | ----- | ----------------------- | ------------------------------------------------------------------------------------------- |
| `proto_aegis_grpc.v1.RobotReadService.GetAll` | Get all available robot data. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.RobotObservation`](./proto_aegis_grpc/v1/robot_srvs.proto) |
| `proto_aegis_grpc.v1.RobotReadService.GetRobotState` | Get consolidated robot state (pose, joints, wrench). | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.RobotState`](./proto_aegis_grpc/v1/robot_srvs.proto) |
| `proto_aegis_grpc.v1.RobotReadService.GetRobotVision` | Get all camera images (scene, left, right). | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.RobotVision`](./proto_aegis_grpc/v1/robot_srvs.proto) |
Expand All @@ -119,8 +119,8 @@ The control bridge for MoveIt2 servo and planners are implemented in the [`Robot
| ------------------------------------------------------------ | --------------------------------------------------------------- | ----- | ---------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------ |
| `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.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.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 @@ -174,11 +174,21 @@ There is no hardcoded parameters, everything can be modified via ROS:

**Initialization parameters**

| Parameter | Type | Default | Units | Description |
| -------------- | -------- | ----------------- | ----- | -------------------------------------------- |
| `topic_pose` | `string` | `"/tcp_pose"` | - | Topic providing the TCP pose. |
| `topic_wrench` | `string` | `"/wrench"` | - | Topic providing the force/torque (F/T) data. |
| `topic_joints` | `string` | `"/joint_states"` | - | Topic providing joint state data. |
| Parameter | Type | Default | Units | Description |
| --------------------- | -------- | ------------------------------- | ----- | ------------------------------------------------ |
| `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. |
| `topic_camera_scene` | `string` | `"/cam_scene/rgb/image_rect"` | - | Camera scene image topic. |
| `topic_camera_right` | `string` | `"/cam_tool_right/image_color"` | - | Right tool camera image topic. |
| `topic_camera_left` | `string` | `"/cam_tool_left/image_color"` | - | Left tool camera image topic. |
| `target_image_width` | `int` | `64` | px | Target output image width in pixels. |
| `target_image_height` | `int` | `64` | px | Target output image height in pixels. |
| `enable_image_resize` | `bool` | `true` | - | Enable resizing images before sending over gRPC. |

>[!TIP]
> The end-effector's TCP pose is obtained from the tf2 transform (using `tcp_frame` and `base_frame`).

### Run and set examples
The initialization params can be set via ROS 2 run or launch:
Expand Down
23 changes: 16 additions & 7 deletions 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 @@ -9,12 +9,15 @@

#include <rclcpp/rclcpp.hpp>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/wrench.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <geometry_msgs/msg/wrench.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

namespace aegis_grpc {

Expand Down Expand Up @@ -65,14 +68,15 @@ class RobotReadServiceImpl final : public proto_aegis_grpc::v1::RobotReadService
template <class T>
void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description);

void PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
void WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg);
void JointStateSubCb(const sensor_msgs::msg::JointState::SharedPtr msg);
void ImageSceneSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
void ImageRightSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
void ImageLeftSubCb(const sensor_msgs::msg::Image::SharedPtr msg);

static void FillProtoPose(const geometry_msgs::msg::Pose& ros, proto_aegis_grpc::v1::Pose* out);
void PoseTransformUpdate();

static void FillProtoPose(const geometry_msgs::msg::TransformStamped& ros, proto_aegis_grpc::v1::Pose* out);

static void FillProtoWrench(const geometry_msgs::msg::Wrench& ros, proto_aegis_grpc::v1::Wrench* out);

Expand All @@ -81,14 +85,19 @@ class RobotReadServiceImpl final : public proto_aegis_grpc::v1::RobotReadService
void FillProtoImage(const sensor_msgs::msg::Image& ros, proto_aegis_grpc::v1::Image* out, cv::Mat& resize_buffer);

std::shared_ptr<rclcpp::Node> node_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;

std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::string tcp_frame_;
std::string base_frame_;

rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_sub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_scene_sub_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_right_sub_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_left_sub_;

geometry_msgs::msg::Pose pose_data_;
geometry_msgs::msg::TransformStamped pose_tf_data_;
geometry_msgs::msg::Wrench wrench_data_;
sensor_msgs::msg::JointState joint_state_data_;
sensor_msgs::msg::Image image_scene_data_;
Expand Down
3 changes: 2 additions & 1 deletion aegis_grpc/launch/start_server.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,8 @@ def generate_launch_description():
output="both",
parameters=[
{
"topic_pose": "/tcp_pose_broadcaster/pose",
"tcp_frame": "robotiq_hande_end",
"base_frame": "world",
"topic_wrench": "/net_ft_sensor_broadcaster/wrench",
}
],
Expand Down
2 changes: 2 additions & 0 deletions aegis_grpc/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
<depend>rclcpp_action</depend>
<depend>sensor_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>

<exec_depend>python3-grpc-tools</exec_depend>
<exec_depend>python3-grpcio</exec_depend>
Expand Down
54 changes: 33 additions & 21 deletions aegis_grpc/src/robot_read_service.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,10 @@
namespace aegis_grpc {

RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
: node_(node), pose_data_(), wrench_data_(), joint_state_data_() {
: node_(node), pose_tf_data_(), wrench_data_(), joint_state_data_() {
// Initialization parameters
DeclareROSParameter("topic_pose", std::string("/tcp_pose"), "[str] Init; Sub: topic with the TCP pose data.");
DeclareROSParameter("tcp_frame", std::string("robotiq_hande_end"), "[str] Init; TF2 frame ID of the end-effector.");
DeclareROSParameter("base_frame", std::string("world"), "[str] Init; TF2 base frame ID for EE pose lookup.");
DeclareROSParameter("topic_wrench", std::string("/wrench"), "[str] Init; Sub: topic with the F/T data.");
DeclareROSParameter("topic_joints", std::string("/joint_states"), "[str] Init; Sub: topic with the joint states.");
DeclareROSParameter("topic_camera_scene", std::string("/cam_scene/rgb/image_rect"),
Expand All @@ -19,9 +20,9 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
DeclareROSParameter("target_image_height", 64, "[int] Init; Target output image height in pixels.");
DeclareROSParameter("enable_image_resize", true, "[bool] Init; Enable resizing images before sending over gRPC.");

pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
node_->get_parameter("topic_pose").as_string(), 10,
std::bind(&RobotReadServiceImpl::PoseSubCb, this, std::placeholders::_1));
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, node_, false);

wrench_sub_ = node_->create_subscription<geometry_msgs::msg::WrenchStamped>(
node_->get_parameter("topic_wrench").as_string(), 10,
std::bind(&RobotReadServiceImpl::WrenchSubCb, this, std::placeholders::_1));
Expand All @@ -37,6 +38,9 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
image_left_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
node_->get_parameter("topic_camera_left").as_string(), 10,
std::bind(&RobotReadServiceImpl::ImageLeftSubCb, this, std::placeholders::_1));

tcp_frame_ = node_->get_parameter("tcp_frame").as_string();
base_frame_ = node_->get_parameter("base_frame").as_string();
target_img_width_ = node_->get_parameter("target_image_width").as_int();
target_img_height_ = node_->get_parameter("target_image_height").as_int();
enable_image_resize_ = node_->get_parameter("enable_image_resize").as_bool();
Expand All @@ -55,11 +59,6 @@ void RobotReadServiceImpl::DeclareROSParameter(const std::string& name,
RCLCPP_INFO(node_->get_logger(), "> %s := %s", name.c_str(), p.value_to_string().c_str());
}

void RobotReadServiceImpl::PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
std::lock_guard<std::mutex> lock(pose_mutex_);
pose_data_ = msg->pose;
}

void RobotReadServiceImpl::WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) {
std::lock_guard<std::mutex> lock(wrench_mutex_);
wrench_data_ = msg->wrench;
Expand Down Expand Up @@ -92,7 +91,8 @@ grpc::Status RobotReadServiceImpl::GetTCPPose(grpc::ServerContext* context,
(void)request;
{
std::lock_guard<std::mutex> lock(pose_mutex_);
FillProtoPose(pose_data_, response);
PoseTransformUpdate();
FillProtoPose(pose_tf_data_, response);
}
return grpc::Status::OK;
}
Expand Down Expand Up @@ -165,7 +165,8 @@ grpc::Status RobotReadServiceImpl::GetRobotState(grpc::ServerContext* context,
(void)request;
{
std::lock_guard<std::mutex> lock(pose_mutex_);
FillProtoPose(pose_data_, response->mutable_pose());
PoseTransformUpdate();
FillProtoPose(pose_tf_data_, response->mutable_pose());
}
{
std::lock_guard<std::mutex> lock(wrench_mutex_);
Expand Down Expand Up @@ -206,7 +207,8 @@ grpc::Status RobotReadServiceImpl::GetAll(grpc::ServerContext* context,
(void)request;
{
std::lock_guard<std::mutex> lock(pose_mutex_);
FillProtoPose(pose_data_, response->mutable_robot_state()->mutable_pose());
PoseTransformUpdate();
FillProtoPose(pose_tf_data_, response->mutable_robot_state()->mutable_pose());
}
{
std::lock_guard<std::mutex> lock(wrench_mutex_);
Expand All @@ -231,17 +233,27 @@ grpc::Status RobotReadServiceImpl::GetAll(grpc::ServerContext* context,
return grpc::Status::OK;
}

void RobotReadServiceImpl::FillProtoPose(const geometry_msgs::msg::Pose& ros, proto_aegis_grpc::v1::Pose* out) {
void RobotReadServiceImpl::PoseTransformUpdate() {
try {
pose_tf_data_ = tf_buffer_->lookupTransform(base_frame_, tcp_frame_, tf2::TimePointZero);
} catch (const tf2::TransformException& ex) {
RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Could not lookup transform from %s to %s: %s",
base_frame_.c_str(), tcp_frame_.c_str(), ex.what());
}
}

void RobotReadServiceImpl::FillProtoPose(const geometry_msgs::msg::TransformStamped& ros,
proto_aegis_grpc::v1::Pose* out) {
auto* pos = out->mutable_position();
pos->set_x(ros.position.x);
pos->set_y(ros.position.y);
pos->set_z(ros.position.z);
pos->set_x(ros.transform.translation.x);
pos->set_y(ros.transform.translation.y);
pos->set_z(ros.transform.translation.z);

auto* ori = out->mutable_orientation();
ori->set_x(ros.orientation.x);
ori->set_y(ros.orientation.y);
ori->set_z(ros.orientation.z);
ori->set_w(ros.orientation.w);
ori->set_x(ros.transform.rotation.x);
ori->set_y(ros.transform.rotation.y);
ori->set_z(ros.transform.rotation.z);
ori->set_w(ros.transform.rotation.w);
}

void RobotReadServiceImpl::FillProtoWrench(const geometry_msgs::msg::Wrench& ros, proto_aegis_grpc::v1::Wrench* out) {
Expand Down