diff --git a/aegis_grpc/CHANGELOG.md b/aegis_grpc/CHANGELOG.md index 664e10aa..ca6be381 100644 --- a/aegis_grpc/CHANGELOG.md +++ b/aegis_grpc/CHANGELOG.md @@ -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 diff --git a/aegis_grpc/README.md b/aegis_grpc/README.md index 52d0df50..8955939e 100644 --- a/aegis_grpc/README.md +++ b/aegis_grpc/README.md @@ -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) | @@ -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` | @@ -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: diff --git a/aegis_grpc/include/aegis_grpc/robot_read_service.hpp b/aegis_grpc/include/aegis_grpc/robot_read_service.hpp index 54c7004c..252f2348 100644 --- a/aegis_grpc/include/aegis_grpc/robot_read_service.hpp +++ b/aegis_grpc/include/aegis_grpc/robot_read_service.hpp @@ -9,12 +9,15 @@ #include #include -#include #include -#include +#include #include -#include +#include #include +#include +#include +#include +#include namespace aegis_grpc { @@ -65,14 +68,15 @@ class RobotReadServiceImpl final : public proto_aegis_grpc::v1::RobotReadService template 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); @@ -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 node_; - rclcpp::Subscription::SharedPtr pose_sub_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::string tcp_frame_; + std::string base_frame_; + rclcpp::Subscription::SharedPtr wrench_sub_; rclcpp::Subscription::SharedPtr joint_state_sub_; rclcpp::Subscription::SharedPtr image_scene_sub_; rclcpp::Subscription::SharedPtr image_right_sub_; rclcpp::Subscription::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_; diff --git a/aegis_grpc/launch/start_server.launch.py b/aegis_grpc/launch/start_server.launch.py index ec8e5533..74cc18e4 100644 --- a/aegis_grpc/launch/start_server.launch.py +++ b/aegis_grpc/launch/start_server.launch.py @@ -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", } ], diff --git a/aegis_grpc/package.xml b/aegis_grpc/package.xml index 7a7aa048..f5e40008 100644 --- a/aegis_grpc/package.xml +++ b/aegis_grpc/package.xml @@ -30,6 +30,8 @@ rclcpp_action sensor_msgs std_srvs + tf2_geometry_msgs + tf2_ros python3-grpc-tools python3-grpcio diff --git a/aegis_grpc/src/robot_read_service.cpp b/aegis_grpc/src/robot_read_service.cpp index 2e037a97..d27240a5 100644 --- a/aegis_grpc/src/robot_read_service.cpp +++ b/aegis_grpc/src/robot_read_service.cpp @@ -4,9 +4,10 @@ namespace aegis_grpc { RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr 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"), @@ -19,9 +20,9 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr 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( - node_->get_parameter("topic_pose").as_string(), 10, - std::bind(&RobotReadServiceImpl::PoseSubCb, this, std::placeholders::_1)); + tf_buffer_ = std::make_unique(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, node_, false); + wrench_sub_ = node_->create_subscription( node_->get_parameter("topic_wrench").as_string(), 10, std::bind(&RobotReadServiceImpl::WrenchSubCb, this, std::placeholders::_1)); @@ -37,6 +38,9 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr node) image_left_sub_ = node_->create_subscription( 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(); @@ -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 lock(pose_mutex_); - pose_data_ = msg->pose; -} - void RobotReadServiceImpl::WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { std::lock_guard lock(wrench_mutex_); wrench_data_ = msg->wrench; @@ -92,7 +91,8 @@ grpc::Status RobotReadServiceImpl::GetTCPPose(grpc::ServerContext* context, (void)request; { std::lock_guard lock(pose_mutex_); - FillProtoPose(pose_data_, response); + PoseTransformUpdate(); + FillProtoPose(pose_tf_data_, response); } return grpc::Status::OK; } @@ -165,7 +165,8 @@ grpc::Status RobotReadServiceImpl::GetRobotState(grpc::ServerContext* context, (void)request; { std::lock_guard lock(pose_mutex_); - FillProtoPose(pose_data_, response->mutable_pose()); + PoseTransformUpdate(); + FillProtoPose(pose_tf_data_, response->mutable_pose()); } { std::lock_guard lock(wrench_mutex_); @@ -206,7 +207,8 @@ grpc::Status RobotReadServiceImpl::GetAll(grpc::ServerContext* context, (void)request; { std::lock_guard 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 lock(wrench_mutex_); @@ -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) {