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 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. | diff --git a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp index d371d610..b0dd9047 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 @@ -29,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; @@ -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_; 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/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 diff --git a/aegis_grpc/src/robot_control_service.cpp b/aegis_grpc/src/robot_control_service.cpp index f56089b8..d260a72f 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; } @@ -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) { @@ -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); } @@ -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(); @@ -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); } @@ -417,7 +424,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 +432,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 +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); } @@ -460,7 +467,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 +475,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); } 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);