Skip to content

Commit 05a0922

Browse files
macmacalmacmacal
committed
CHG: EE pose taken from tf2 transform
1 parent 76b0e91 commit 05a0922

File tree

6 files changed

+74
-37
lines changed

6 files changed

+74
-37
lines changed

‎aegis_grpc/CHANGELOG.md‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
1818
* [PR-82](https://github.com/AGH-CEAI/aegis_ros/pull/82) - MVP of the gRPC-ROS server.
1919

2020
### Changed
21+
22+
* [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.
23+
2124
### Deprecated
2225
### Removed
2326
### Fixed

‎aegis_grpc/README.md‎

Lines changed: 18 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,7 @@ The server is split into 2 services defined in [`proto_aegis_grpc.v1.robot_srvs`
9494
The "ROS-getters" are implemented in the [`RobotReadServiceImpl`](./include/aegis_grpc/robot_read_service.hpp) class as the following methods:
9595

9696
| Method name | Desc. | Impl. | gRPC Request | gRPC Response |
97-
|------------------------------------------------------------|------------------------------------------------------|-------|-------------------------|---------------------------------------------------------------------------------------------|
97+
| ---------------------------------------------------------- | ---------------------------------------------------- | ----- | ----------------------- | ------------------------------------------------------------------------------------------- |
9898
| `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) |
9999
| `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) |
100100
| `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) |
@@ -119,8 +119,8 @@ The control bridge for MoveIt2 servo and planners are implemented in the [`Robot
119119
| ------------------------------------------------------------ | --------------------------------------------------------------- | ----- | ---------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------ |
120120
| `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) |
121121
| `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) |
122-
| `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) |
123-
| `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) |
122+
| `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) |
123+
| `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) |
124124
| `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) |
125125
| `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` |
126126
| `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` |
@@ -174,11 +174,21 @@ There is no hardcoded parameters, everything can be modified via ROS:
174174

175175
**Initialization parameters**
176176

177-
| Parameter | Type | Default | Units | Description |
178-
| -------------- | -------- | ----------------- | ----- | -------------------------------------------- |
179-
| `topic_pose` | `string` | `"/tcp_pose"` | - | Topic providing the TCP pose. |
180-
| `topic_wrench` | `string` | `"/wrench"` | - | Topic providing the force/torque (F/T) data. |
181-
| `topic_joints` | `string` | `"/joint_states"` | - | Topic providing joint state data. |
177+
| Parameter | Type | Default | Units | Description |
178+
| --------------------- | -------- | ------------------------------- | ----- | ------------------------------------------------ |
179+
| `tcp_frame` | `string` | `"robotiq_hande_end"` | - | TF2 frame ID of the end-effector. |
180+
| `base_frame` | `string` | `"world"` | - | TF2 base frame ID for EE pose lookup. |
181+
| `topic_wrench` | `string` | `"/wrench"` | - | Topic providing force/torque (F/T) data. |
182+
| `topic_joints` | `string` | `"/joint_states"` | - | Topic providing joint state data. |
183+
| `topic_camera_scene` | `string` | `"/cam_scene/rgb/image_rect"` | - | Camera scene image topic. |
184+
| `topic_camera_right` | `string` | `"/cam_tool_right/image_color"` | - | Right tool camera image topic. |
185+
| `topic_camera_left` | `string` | `"/cam_tool_left/image_color"` | - | Left tool camera image topic. |
186+
| `target_image_width` | `int` | `64` | px | Target output image width in pixels. |
187+
| `target_image_height` | `int` | `64` | px | Target output image height in pixels. |
188+
| `enable_image_resize` | `bool` | `true` | - | Enable resizing images before sending over gRPC. |
189+
190+
>[!TIP]
191+
> The end-effector's TCP pose is obtained from the tf2 transform (using `tcp_frame` and `base_frame`).
182192
183193
### Run and set examples
184194
The initialization params can be set via ROS 2 run or launch:

‎aegis_grpc/include/aegis_grpc/robot_read_service.hpp‎

Lines changed: 16 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,15 @@
99

1010
#include <rclcpp/rclcpp.hpp>
1111
#include <cv_bridge/cv_bridge.h>
12-
#include <geometry_msgs/msg/pose.hpp>
1312
#include <geometry_msgs/msg/pose_stamped.hpp>
14-
#include <geometry_msgs/msg/wrench.hpp>
13+
#include <geometry_msgs/msg/pose.hpp>
1514
#include <geometry_msgs/msg/wrench_stamped.hpp>
16-
#include <sensor_msgs/msg/joint_state.hpp>
15+
#include <geometry_msgs/msg/wrench.hpp>
1716
#include <sensor_msgs/msg/image.hpp>
17+
#include <sensor_msgs/msg/joint_state.hpp>
18+
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
19+
#include <tf2_ros/buffer.h>
20+
#include <tf2_ros/transform_listener.h>
1821

1922
namespace aegis_grpc {
2023

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

68-
void PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
6971
void WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg);
7072
void JointStateSubCb(const sensor_msgs::msg::JointState::SharedPtr msg);
7173
void ImageSceneSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
7274
void ImageRightSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
7375
void ImageLeftSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
7476

75-
static void FillProtoPose(const geometry_msgs::msg::Pose& ros, proto_aegis_grpc::v1::Pose* out);
77+
void PoseTransformUpdate();
78+
79+
static void FillProtoPose(const geometry_msgs::msg::TransformStamped& ros, proto_aegis_grpc::v1::Pose* out);
7680

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

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

8387
std::shared_ptr<rclcpp::Node> node_;
84-
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
88+
89+
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
90+
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
91+
std::string tcp_frame_;
92+
std::string base_frame_;
93+
8594
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_sub_;
8695
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
8796
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_scene_sub_;
8897
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_right_sub_;
8998
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_left_sub_;
9099

91-
geometry_msgs::msg::Pose pose_data_;
100+
geometry_msgs::msg::TransformStamped pose_tf_data_;
92101
geometry_msgs::msg::Wrench wrench_data_;
93102
sensor_msgs::msg::JointState joint_state_data_;
94103
sensor_msgs::msg::Image image_scene_data_;

‎aegis_grpc/launch/start_server.launch.py‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,8 @@ def generate_launch_description():
1111
output="both",
1212
parameters=[
1313
{
14-
"topic_pose": "/tcp_pose_broadcaster/pose",
14+
"tcp_frame": "robotiq_hande_end",
15+
"base_frame": "world",
1516
"topic_wrench": "/net_ft_sensor_broadcaster/wrench",
1617
}
1718
],

‎aegis_grpc/package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,8 @@
3030
<depend>rclcpp_action</depend>
3131
<depend>sensor_msgs</depend>
3232
<depend>std_srvs</depend>
33+
<depend>tf2_geometry_msgs</depend>
34+
<depend>tf2_ros</depend>
3335

3436
<exec_depend>python3-grpc-tools</exec_depend>
3537
<exec_depend>python3-grpcio</exec_depend>

‎aegis_grpc/src/robot_read_service.cpp‎

Lines changed: 33 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,10 @@
44
namespace aegis_grpc {
55

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

22-
pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
23-
node_->get_parameter("topic_pose").as_string(), 10,
24-
std::bind(&RobotReadServiceImpl::PoseSubCb, this, std::placeholders::_1));
23+
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
24+
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, node_, false);
25+
2526
wrench_sub_ = node_->create_subscription<geometry_msgs::msg::WrenchStamped>(
2627
node_->get_parameter("topic_wrench").as_string(), 10,
2728
std::bind(&RobotReadServiceImpl::WrenchSubCb, this, std::placeholders::_1));
@@ -37,6 +38,9 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
3738
image_left_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
3839
node_->get_parameter("topic_camera_left").as_string(), 10,
3940
std::bind(&RobotReadServiceImpl::ImageLeftSubCb, this, std::placeholders::_1));
41+
42+
tcp_frame_ = node_->get_parameter("tcp_frame").as_string();
43+
base_frame_ = node_->get_parameter("base_frame").as_string();
4044
target_img_width_ = node_->get_parameter("target_image_width").as_int();
4145
target_img_height_ = node_->get_parameter("target_image_height").as_int();
4246
enable_image_resize_ = node_->get_parameter("enable_image_resize").as_bool();
@@ -55,11 +59,6 @@ void RobotReadServiceImpl::DeclareROSParameter(const std::string& name,
5559
RCLCPP_INFO(node_->get_logger(), "> %s := %s", name.c_str(), p.value_to_string().c_str());
5660
}
5761

58-
void RobotReadServiceImpl::PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
59-
std::lock_guard<std::mutex> lock(pose_mutex_);
60-
pose_data_ = msg->pose;
61-
}
62-
6362
void RobotReadServiceImpl::WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) {
6463
std::lock_guard<std::mutex> lock(wrench_mutex_);
6564
wrench_data_ = msg->wrench;
@@ -92,7 +91,8 @@ grpc::Status RobotReadServiceImpl::GetTCPPose(grpc::ServerContext* context,
9291
(void)request;
9392
{
9493
std::lock_guard<std::mutex> lock(pose_mutex_);
95-
FillProtoPose(pose_data_, response);
94+
PoseTransformUpdate();
95+
FillProtoPose(pose_tf_data_, response);
9696
}
9797
return grpc::Status::OK;
9898
}
@@ -165,7 +165,8 @@ grpc::Status RobotReadServiceImpl::GetRobotState(grpc::ServerContext* context,
165165
(void)request;
166166
{
167167
std::lock_guard<std::mutex> lock(pose_mutex_);
168-
FillProtoPose(pose_data_, response->mutable_pose());
168+
PoseTransformUpdate();
169+
FillProtoPose(pose_tf_data_, response->mutable_pose());
169170
}
170171
{
171172
std::lock_guard<std::mutex> lock(wrench_mutex_);
@@ -206,7 +207,8 @@ grpc::Status RobotReadServiceImpl::GetAll(grpc::ServerContext* context,
206207
(void)request;
207208
{
208209
std::lock_guard<std::mutex> lock(pose_mutex_);
209-
FillProtoPose(pose_data_, response->mutable_robot_state()->mutable_pose());
210+
PoseTransformUpdate();
211+
FillProtoPose(pose_tf_data_, response->mutable_robot_state()->mutable_pose());
210212
}
211213
{
212214
std::lock_guard<std::mutex> lock(wrench_mutex_);
@@ -231,17 +233,27 @@ grpc::Status RobotReadServiceImpl::GetAll(grpc::ServerContext* context,
231233
return grpc::Status::OK;
232234
}
233235

234-
void RobotReadServiceImpl::FillProtoPose(const geometry_msgs::msg::Pose& ros, proto_aegis_grpc::v1::Pose* out) {
236+
void RobotReadServiceImpl::PoseTransformUpdate() {
237+
try {
238+
pose_tf_data_ = tf_buffer_->lookupTransform(base_frame_, tcp_frame_, tf2::TimePointZero);
239+
} catch (const tf2::TransformException& ex) {
240+
RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Could not lookup transform from %s to %s: %s",
241+
base_frame_.c_str(), tcp_frame_.c_str(), ex.what());
242+
}
243+
}
244+
245+
void RobotReadServiceImpl::FillProtoPose(const geometry_msgs::msg::TransformStamped& ros,
246+
proto_aegis_grpc::v1::Pose* out) {
235247
auto* pos = out->mutable_position();
236-
pos->set_x(ros.position.x);
237-
pos->set_y(ros.position.y);
238-
pos->set_z(ros.position.z);
248+
pos->set_x(ros.transform.translation.x);
249+
pos->set_y(ros.transform.translation.y);
250+
pos->set_z(ros.transform.translation.z);
239251

240252
auto* ori = out->mutable_orientation();
241-
ori->set_x(ros.orientation.x);
242-
ori->set_y(ros.orientation.y);
243-
ori->set_z(ros.orientation.z);
244-
ori->set_w(ros.orientation.w);
253+
ori->set_x(ros.transform.rotation.x);
254+
ori->set_y(ros.transform.rotation.y);
255+
ori->set_z(ros.transform.rotation.z);
256+
ori->set_w(ros.transform.rotation.w);
245257
}
246258

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

0 commit comments

Comments
 (0)