Skip to content

Commit 242ec27

Browse files
macmacalmacmacal
authored
Merge pull request #103 from AGH-CEAI/feature/aegis_grpc/minor_improvments
`aegis_grpc`: Servoing idling & minor improvments
2 parents fa7f4e2 + 0758311 commit 242ec27

File tree

7 files changed

+56
-35
lines changed

7 files changed

+56
-35
lines changed

‎aegis_grpc/CHANGELOG.md‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
99

1010
### Added
1111

12+
* [PR-103](https://github.com/AGH-CEAI/aegis_ros/pull/103) - Server: Adds servo idling with zero commands.
1213
* [PR-98](https://github.com/AGH-CEAI/aegis_ros/pull/98) - Added gRPC-based camera image reading.
1314
* [PR-97](https://github.com/AGH-CEAI/aegis_ros/pull/97) - Added gRPC func: MoveIt2 servo start & stop service.
1415
* [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
1920

2021
### Changed
2122

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+
* [PR-103](https://github.com/AGH-CEAI/aegis_ros/pull/103) - Server: Changed `assert()` in `FillProtoImage()` to `if` which loggs an error message.
24+
* [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]`.
25+
* [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.
2326

2427
### Deprecated
2528
### Removed
2629
### Fixed
30+
2731
### Security

‎aegis_grpc/README.md‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,6 @@ The "ROS-getters" are implemented in the [`RobotReadServiceImpl`](./include/aegi
105105
| `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) |
106106
| `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) |
107107

108-
109108
You can always list the methods with the `grpcurl` command:
110109
```bash
111110
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
117116

118117
| Method name | Desc. | Impl. | gRPC Request | gRPC Response |
119118
| ------------------------------------------------------------ | --------------------------------------------------------------- | ----- | ---------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------ |
120-
| `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) |
121-
| `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) |
119+
| `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) |
120+
| `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) |
122121
| `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) |
123122
| `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) |
123+
| `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) |
124+
| `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) |
124125
| `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) |
125126
| `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` |
126127
| `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:
176177

177178
| Parameter | Type | Default | Units | Description |
178179
| --------------------- | -------- | ------------------------------- | ----- | ------------------------------------------------ |
179-
| `tcp_frame` | `string` | `"robotiq_hande_end"` | - | TF2 frame ID of the end-effector. |
180+
| `tcp_frame` | `string` | `"robotiq_hande_end"` | - | TF2 frame ID of the end-effector. |
180181
| `base_frame` | `string` | `"world"` | - | TF2 base frame ID for EE pose lookup. |
181182
| `topic_wrench` | `string` | `"/wrench"` | - | Topic providing force/torque (F/T) data. |
182183
| `topic_joints` | `string` | `"/joint_states"` | - | Topic providing joint state data. |

‎aegis_grpc/include/aegis_grpc/robot_control_service.hpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
#define AEGIS_GRPC__ROBOT_CONTROL_SERVICE_HPP_
33
#include <atomic>
44
#include <chrono>
5+
#include <memory>
56

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

3031
explicit RobotControlServiceImpl(std::shared_ptr<rclcpp::Node> node);
3132

32-
// TODO(issue#87) Create additional methods to enable and disable servo control.
3333
grpc::Status ServoEnable(grpc::ServerContext* context,
3434
const google::protobuf::Empty* request,
3535
proto_aegis_grpc::v1::TriggerResponse* response) override;
@@ -95,6 +95,8 @@ class RobotControlServiceImpl final : public proto_aegis_grpc::v1::RobotControlS
9595
std::string servo_tcp_link_;
9696
control_msgs::msg::JointJog servo_joint_msg_;
9797
geometry_msgs::msg::Twist servo_tcp_msg_;
98+
const control_msgs::msg::JointJog servo_joint_msg_zeros_;
99+
const geometry_msgs::msg::Twist servo_tcp_msg_zeros_;
98100
int servo_frequency_ratio_;
99101
int servo_msgs_left_;
100102

‎aegis_grpc/include/aegis_grpc/robot_read_service.hpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,6 @@ class RobotReadServiceImpl final : public proto_aegis_grpc::v1::RobotReadService
6161
const google::protobuf::Empty* request,
6262
proto_aegis_grpc::v1::RobotObservation* response) override;
6363

64-
// TODO(issue#84) implement getters for images from cameras (RGB & RGBD)
6564
// TODO(issue#85) develop synchronization guard to monitor the freshness of the data
6665

6766
private:

‎aegis_grpc/python_client/aegis_grpc_client/grpc_client.py‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,11 @@
1515
)
1616

1717

18+
class PrefixedLoggerAdapter(logging.LoggerAdapter):
19+
def process(self, msg, kwargs):
20+
return f"[aegis_robot_client] {msg}", kwargs
21+
22+
1823
class AegisRobotClient:
1924
"""
2025
Async gRPC client for Aegis Robot Control System.
@@ -30,7 +35,7 @@ def __init__(self, server_address: str = "127.0.0.1:50051"):
3035
Args:
3136
server_address: gRPC server address in format "host:port"
3237
"""
33-
self.logger = logging.getLogger("aegis_grpc_client")
38+
self.logger = PrefixedLoggerAdapter(logging.getLogger("aegis_grpc_client"), {})
3439
self.server_address = server_address
3540
self.channel: Optional[grpc.aio.Channel] = None
3641
self.read_stub: Optional[robot_srvs_pb2_grpc.RobotReadServiceStub] = None

‎aegis_grpc/src/robot_control_service.cpp‎

Lines changed: 33 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -164,7 +164,7 @@ grpc::Status RobotControlServiceImpl::ServoEnable(grpc::ServerContext*,
164164

165165
response->set_success(true);
166166
response->set_msg("");
167-
RCLCPP_INFO(get_logger(), "[RobotControlService][ServoDisable] Servo enabled");
167+
RCLCPP_INFO(get_logger(), "[RobotControlService][ServoEnable] Servo enabled");
168168
return grpc::Status::OK;
169169
}
170170

@@ -206,24 +206,31 @@ void RobotControlServiceImpl::ServoPublishLoop() {
206206

207207
{
208208
std::lock_guard<std::mutex> lock(servo_mutex_);
209-
if (servo_msgs_left_ == 0) {
210-
servo_mode_ = ServoMode::None;
211-
return;
212-
}
213-
214209
mode = servo_mode_;
215-
switch (mode) {
216-
case ServoMode::JointJog:
217-
jog_msg = servo_joint_msg_;
218-
break;
219-
case ServoMode::TCPTwist:
220-
twist_msg.twist = servo_tcp_msg_;
221-
break;
222-
default:
223-
break;
210+
if (servo_msgs_left_ == 0) {
211+
switch (mode) {
212+
case ServoMode::JointJog:
213+
jog_msg = servo_joint_msg_zeros_;
214+
break;
215+
case ServoMode::TCPTwist:
216+
twist_msg.twist = servo_tcp_msg_zeros_;
217+
break;
218+
default:
219+
break;
220+
}
221+
} else {
222+
switch (mode) {
223+
case ServoMode::JointJog:
224+
jog_msg = servo_joint_msg_;
225+
break;
226+
case ServoMode::TCPTwist:
227+
twist_msg.twist = servo_tcp_msg_;
228+
break;
229+
default:
230+
break;
231+
}
232+
servo_msgs_left_--;
224233
}
225-
226-
servo_msgs_left_--;
227234
}
228235

229236
switch (mode) {
@@ -252,12 +259,12 @@ grpc::Status RobotControlServiceImpl::ServoJoint(grpc::ServerContext* context,
252259
auto N = request->joint_names_size();
253260
if (N != request->displacements_size()) {
254261
std::string err = "The number of joints mismatches the number of displacmenets values!";
255-
RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str());
262+
RCLCPP_ERROR(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str());
256263
return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err);
257264
}
258265
if (N != request->velocities_size()) {
259266
std::string err = "The number of joints mismatches the number of velocities values!";
260-
RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str());
267+
RCLCPP_ERROR(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str());
261268
return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err);
262269
}
263270

@@ -288,11 +295,11 @@ grpc::Status RobotControlServiceImpl::ServoTCP(grpc::ServerContext* context,
288295
google::protobuf::Empty* response) {
289296
(void)context;
290297
(void)response;
298+
static auto ros_msg = geometry_msgs::msg::Twist();
291299

292300
const auto& lin = request->linear();
293301
const auto& ang = request->angular();
294302

295-
auto ros_msg = geometry_msgs::msg::Twist();
296303
ros_msg.linear.x = lin.x();
297304
ros_msg.linear.y = lin.y();
298305
ros_msg.linear.z = lin.z();
@@ -397,7 +404,7 @@ grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context,
397404

398405
if (!request->has_position() || !request->has_orientation()) {
399406
std::string err = "Missing position or orientation.";
400-
RCLCPP_WARN(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str());
407+
RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str());
401408
response->set_msg(err);
402409
return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err);
403410
}
@@ -417,15 +424,15 @@ grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context,
417424
auto result = move_group_->plan(plan);
418425
if (result != moveit::core::MoveItErrorCode::SUCCESS) {
419426
std::string err = "Planning failed.";
420-
RCLCPP_WARN(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str());
427+
RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoPose] %s", err.c_str());
421428
response->set_msg(err);
422429
return grpc::Status(grpc::StatusCode::INTERNAL, err);
423430
}
424431

425432
auto exec = move_group_->execute(plan);
426433
if (exec != moveit::core::MoveItErrorCode::SUCCESS) {
427434
std::string err = "Execution failed.";
428-
RCLCPP_WARN(get_logger(), "[RobotControlService][GoToPose] %s", err.c_str());
435+
RCLCPP_ERROR(get_logger(), "[RobotControlService][GoToPose] %s", err.c_str());
429436
response->set_msg(err);
430437
return grpc::Status(grpc::StatusCode::INTERNAL, err);
431438
}
@@ -444,7 +451,7 @@ grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context,
444451

445452
if (request->name_size() == 0 || request->name_size() != request->position_size()) {
446453
std::string err = "Joint names and positions mismatch or empty.";
447-
RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str());
454+
RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str());
448455
response->set_msg(err);
449456
return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err);
450457
}
@@ -460,15 +467,15 @@ grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context,
460467
auto result = move_group_->plan(plan);
461468
if (result != moveit::core::MoveItErrorCode::SUCCESS) {
462469
std::string err = "Planning failed.";
463-
RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str());
470+
RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str());
464471
response->set_msg(err);
465472
return grpc::Status(grpc::StatusCode::INTERNAL, err);
466473
}
467474

468475
auto exec = move_group_->execute(plan);
469476
if (exec != moveit::core::MoveItErrorCode::SUCCESS) {
470477
std::string err = "Execution failed.";
471-
RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str());
478+
RCLCPP_ERROR(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str());
472479
response->set_msg(err);
473480
return grpc::Status(grpc::StatusCode::INTERNAL, err);
474481
}

‎aegis_grpc/src/robot_read_service.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -288,7 +288,10 @@ void RobotReadServiceImpl::FillProtoJointState(const sensor_msgs::msg::JointStat
288288
void RobotReadServiceImpl::FillProtoImage(const sensor_msgs::msg::Image& ros,
289289
proto_aegis_grpc::v1::Image* out,
290290
cv::Mat& buffer) {
291-
assert((ros.encoding == "bgr8" && "Unsupported image encoding (expected BGR8)."));
291+
if (ros.encoding != "bgr8") {
292+
RCLCPP_ERROR(node_->get_logger(), "Unsupported image encoding (expected 'bgr8'), got: '%s'", ros.encoding.c_str());
293+
return;
294+
}
292295
if (!enable_image_resize_) {
293296
out->set_height(ros.height);
294297
out->set_width(ros.width);

0 commit comments

Comments
 (0)