Skip to content

Commit 045a69e

Browse files
macmacalmacmacal
committed
Merge branch 'humble-devel' into feature/real2sim_servo_calib
2 parents 69cf044 + 7967d9d commit 045a69e

File tree

9 files changed

+498
-159
lines changed

9 files changed

+498
-159
lines changed

‎aegis_grpc/CHANGELOG.md‎

Lines changed: 1 addition & 0 deletions
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-98](https://github.com/AGH-CEAI/aegis_ros/pull/98) - Added gRPC-based camera image reading.
1213
* [PR-97](https://github.com/AGH-CEAI/aegis_ros/pull/97) - Added gRPC func: MoveIt2 servo start & stop service.
1314
* [PR-96](https://github.com/AGH-CEAI/aegis_ros/pull/96) - Added gRPC func: ros2_control controller switching.
1415
* [PR-92](https://github.com/AGH-CEAI/aegis_ros/pull/92) - Added Python client installation script.

‎aegis_grpc/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,13 +13,15 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
1313
# ------------------------------
1414
# Dependencies
1515

16+
find_package(OpenCV REQUIRED)
1617
find_package(ament_cmake REQUIRED)
1718

1819
set(ROS_DEPENDS
1920
rclcpp
2021
rclcpp_action
2122
control_msgs
2223
controller_manager_msgs
24+
cv_bridge
2325
geometry_msgs
2426
moveit_msgs
2527
moveit_ros_planning_interface
@@ -177,6 +179,8 @@ target_link_libraries(
177179
gRPC::grpc++
178180
grpc++_reflection
179181
protobuf::libprotobuf
182+
opencv_core
183+
opencv_imgproc
180184
${PROJECT_NAME}_lib)
181185

182186
ament_target_dependencies(grpc_server ${ROS_DEPENDS})

‎aegis_grpc/README.md‎

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -71,15 +71,15 @@ grpcurl -plaintext 127.0.0.1:50051 list proto_aegis_grpc.v1.RobotControlService
7171
grpcurl -plaintext 127.0.0.1:50051 list proto_aegis_grpc.v1.RobotReadService
7272
grpcurl -plaintext 127.0.0.1:50051 describe proto_aegis_grpc.v1.RobotReadService.GetAll
7373
# or with tool from container
74-
podman run --network=host docker.io/fullstorydev/grpcurl -plaintext 127.0.0.1:50051 list
74+
podman run --rm --network=host docker.io/fullstorydev/grpcurl -plaintext 127.0.0.1:50051 list
7575
# map grpcurl container alias
76-
alias grpcurl="podman run --network=host docker.io/fullstorydev/grpcurl"
76+
alias grpcurl="podman run --rm --network=host docker.io/fullstorydev/grpcurl"
7777
```
7878

7979
Example call to the `GetAll` method with result as a plain json:
8080

8181
```bash
82-
grpcurl -plaintext -d '{}' 127.0.0.1:50051 proto_aegis_grpc.v1.RobotReadService.GetAll
82+
grpcurl -max-msg-sz 10485760 -plaintext -d '{}' 127.0.0.1:50051 proto_aegis_grpc.v1.RobotReadService.GetAll
8383
```
8484

8585
## Messages architecture
@@ -93,12 +93,18 @@ The server is split into 2 services defined in [`proto_aegis_grpc.v1.robot_srvs`
9393

9494
The "ROS-getters" are implemented in the [`RobotReadServiceImpl`](./include/aegis_grpc/robot_read_service.hpp) class as the following methods:
9595

96-
| Method name | Desc. | Impl. | gRPC Request | gRPC Response |
97-
|-------------------------------------------------------|-----------------------------|-------|-------------------------|-----------------------------------------------------------------------------------------|
98-
| `proto_aegis_grpc.v1.RobotReadService.GetAll` | Get the all available data. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.RobotState`](./proto_aegis_grpc/v1/robot_srvs.proto) |
99-
| `proto_aegis_grpc.v1.RobotReadService.GetJointStates` | Get the joints' states. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.sensor_msgs.JointState`](./proto_aegis_grpc/v1/sensor_msgs.proto) |
100-
| `proto_aegis_grpc.v1.RobotReadService.GetTCPPose` | Get the TCP pose. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Pose`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
101-
| `proto_aegis_grpc.v1.RobotReadService.GetWrench` | Read force/torque sensor. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Wrench`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
96+
| Method name | Desc. | Impl. | gRPC Request | gRPC Response |
97+
|------------------------------------------------------------|------------------------------------------------------|-------|-------------------------|---------------------------------------------------------------------------------------------|
98+
| `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) |
99+
| `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) |
100+
| `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) |
101+
| `proto_aegis_grpc.v1.RobotReadService.GetJointStates` | Get the joints' states. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.sensor_msgs.JointState`](./proto_aegis_grpc/v1/sensor_msgs.proto) |
102+
| `proto_aegis_grpc.v1.RobotReadService.GetTCPPose` | Get the TCP pose. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Pose`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
103+
| `proto_aegis_grpc.v1.RobotReadService.GetWrench` | Read force/torque sensor. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Wrench`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
104+
| `proto_aegis_grpc.v1.RobotReadService.GetCameraSceneImage` | Get scene camera image. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.sensor_msgs.Image`](./proto_aegis_grpc/v1/sensor_msgs.proto) |
105+
| `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) |
106+
| `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) |
107+
102108

103109
You can always list the methods with the `grpcurl` command:
104110
```bash

‎aegis_grpc/include/aegis_grpc/robot_read_service.hpp‎

Lines changed: 89 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,20 @@
11
#ifndef AEGIS_GRPC__ROBOT_READ_SERVICE_HPP_
22
#define AEGIS_GRPC__ROBOT_READ_SERVICE_HPP_
33
#include <mutex>
4+
#include <opencv2/opencv.hpp>
45

56
#include <grpcpp/grpcpp.h>
67
#include <google/protobuf/empty.pb.h>
78
#include "proto_aegis_grpc/v1/robot_srvs.grpc.pb.h"
89

910
#include <rclcpp/rclcpp.hpp>
11+
#include <cv_bridge/cv_bridge.h>
1012
#include <geometry_msgs/msg/pose.hpp>
1113
#include <geometry_msgs/msg/pose_stamped.hpp>
1214
#include <geometry_msgs/msg/wrench.hpp>
1315
#include <geometry_msgs/msg/wrench_stamped.hpp>
1416
#include <sensor_msgs/msg/joint_state.hpp>
17+
#include <sensor_msgs/msg/image.hpp>
1518

1619
namespace aegis_grpc {
1720

@@ -20,58 +23,121 @@ class RobotReadServiceImpl final
2023
public:
2124
explicit RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node);
2225

23-
grpc::Status GetTCPPose(grpc::ServerContext* context,
24-
const google::protobuf::Empty* request,
25-
proto_aegis_grpc::v1::Pose* response) override;
26-
27-
grpc::Status GetWrench(grpc::ServerContext* context,
28-
const google::protobuf::Empty* request,
29-
proto_aegis_grpc::v1::Wrench* response) override;
30-
31-
grpc::Status
32-
GetJointStates(grpc::ServerContext* context,
33-
const google::protobuf::Empty* request,
34-
proto_aegis_grpc::v1::JointState* response) override;
35-
36-
grpc::Status GetAll(grpc::ServerContext* context,
37-
const google::protobuf::Empty* request,
38-
proto_aegis_grpc::v1::RobotState* response) override;
26+
grpc::Status GetTCPPose(
27+
grpc::ServerContext* context,
28+
const google::protobuf::Empty* request,
29+
proto_aegis_grpc::v1::Pose* response
30+
) override;
31+
32+
grpc::Status GetWrench(
33+
grpc::ServerContext* context,
34+
const google::protobuf::Empty* request,
35+
proto_aegis_grpc::v1::Wrench* response
36+
) override;
37+
38+
grpc::Status GetJointStates(
39+
grpc::ServerContext* context,
40+
const google::protobuf::Empty* request,
41+
proto_aegis_grpc::v1::JointState* response
42+
) override;
43+
44+
grpc::Status GetCameraSceneImage(
45+
grpc::ServerContext* context,
46+
const google::protobuf::Empty* request,
47+
proto_aegis_grpc::v1::Image* response
48+
) override;
49+
50+
grpc::Status GetCameraRightImage(
51+
grpc::ServerContext* context,
52+
const google::protobuf::Empty* request,
53+
proto_aegis_grpc::v1::Image* response
54+
) override;
55+
56+
grpc::Status GetCameraLeftImage(
57+
grpc::ServerContext* context,
58+
const google::protobuf::Empty* request,
59+
proto_aegis_grpc::v1::Image* response
60+
) override;
61+
62+
grpc::Status GetRobotState(
63+
grpc::ServerContext* context,
64+
const google::protobuf::Empty* request,
65+
proto_aegis_grpc::v1::RobotState* response
66+
) override;
67+
68+
grpc::Status GetRobotVision(
69+
grpc::ServerContext* context,
70+
const google::protobuf::Empty* request,
71+
proto_aegis_grpc::v1::RobotVision* response
72+
) override;
73+
74+
grpc::Status GetAll(
75+
grpc::ServerContext* context,
76+
const google::protobuf::Empty* request,
77+
proto_aegis_grpc::v1::RobotObservation* response
78+
) override;
3979

4080
// TODO(issue#84) implement getters for images from cameras (RGB & RGBD)
4181
// TODO(issue#85) develop synchronization guard to monitor the freshness of the data
4282

4383
private:
44-
void DeclareROSParameter(const std::string& name, const std::string& default_val, const std::string& description);
84+
template <class T>
85+
void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description);
4586

4687
void PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
4788
void WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg);
4889
void JointStateSubCb(const sensor_msgs::msg::JointState::SharedPtr msg);
90+
void ImageSceneSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
91+
void ImageRightSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
92+
void ImageLeftSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
4993

5094
static void FillProtoPose(
51-
const geometry_msgs::msg::Pose& ros,
52-
proto_aegis_grpc::v1::Pose* out);
95+
const geometry_msgs::msg::Pose& ros,
96+
proto_aegis_grpc::v1::Pose* out);
5397

5498
static void FillProtoWrench(
55-
const geometry_msgs::msg::Wrench& ros,
56-
proto_aegis_grpc::v1::Wrench* out);
99+
const geometry_msgs::msg::Wrench& ros,
100+
proto_aegis_grpc::v1::Wrench* out);
57101

58102
static void FillProtoJointState(
59103
const sensor_msgs::msg::JointState& ros,
60104
proto_aegis_grpc::v1::JointState* out);
61105

106+
void FillProtoImage(
107+
const sensor_msgs::msg::Image& ros,
108+
proto_aegis_grpc::v1::Image* out,
109+
cv::Mat& resize_buffer);
110+
62111
std::shared_ptr<rclcpp::Node> node_;
63112
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
64113
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_sub_;
65-
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
66-
joint_state_sub_;
114+
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
115+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_scene_sub_;
116+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_right_sub_;
117+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_left_sub_;
67118

68119
geometry_msgs::msg::Pose pose_data_;
69120
geometry_msgs::msg::Wrench wrench_data_;
70121
sensor_msgs::msg::JointState joint_state_data_;
122+
sensor_msgs::msg::Image image_scene_data_;
123+
sensor_msgs::msg::Image image_right_data_;
124+
sensor_msgs::msg::Image image_left_data_;
71125

72126
std::mutex pose_mutex_;
73127
std::mutex wrench_mutex_;
74128
std::mutex joint_state_mutex_;
129+
std::mutex image_scene_mutex_;
130+
std::mutex image_right_mutex_;
131+
std::mutex image_left_mutex_;
132+
133+
uint32_t target_img_width_;
134+
uint32_t target_img_height_;
135+
136+
cv::Mat scene_resized_;
137+
cv::Mat right_resized_;
138+
cv::Mat left_resized_;
139+
140+
bool enable_image_resize_;
75141
};
76142

77143
} // namespace aegis_grpc

‎aegis_grpc/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919

2020
<depend>control_msgs</depend>
2121
<depend>controller_manager_msgs</depend>
22+
<depend>cv_bridge</depend>
2223
<depend>geometry_msgs</depend>
2324
<depend>libgrpc</depend>
2425
<depend>moveit_msgs</depend>

‎aegis_grpc/proto_aegis_grpc/v1/robot_srvs.proto‎

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,12 @@ service RobotReadService {
1111
rpc GetTCPPose(google.protobuf.Empty) returns (Pose);
1212
rpc GetWrench(google.protobuf.Empty) returns (Wrench);
1313
rpc GetJointStates(google.protobuf.Empty) returns (JointState);
14-
rpc GetAll(google.protobuf.Empty) returns (RobotState);
14+
rpc GetCameraSceneImage(google.protobuf.Empty) returns (Image);
15+
rpc GetCameraRightImage(google.protobuf.Empty) returns (Image);
16+
rpc GetCameraLeftImage(google.protobuf.Empty) returns (Image);
17+
rpc GetRobotState(google.protobuf.Empty) returns (RobotState);
18+
rpc GetRobotVision(google.protobuf.Empty) returns (RobotVision);
19+
rpc GetAll(google.protobuf.Empty) returns (RobotObservation);
1520
}
1621

1722
message RobotState {
@@ -20,6 +25,17 @@ message RobotState {
2025
JointState joint_state = 3;
2126
}
2227

28+
message RobotVision {
29+
Image image_scene = 1;
30+
Image image_right = 2;
31+
Image image_left = 3;
32+
}
33+
34+
message RobotObservation {
35+
RobotState robot_state = 1;
36+
RobotVision robot_vision = 2;
37+
}
38+
2339
service RobotControlService {
2440
rpc ServoEnable(google.protobuf.Empty) returns (TriggerResponse);
2541
rpc ServoDisable(google.protobuf.Empty) returns (TriggerResponse);

‎aegis_grpc/proto_aegis_grpc/v1/sensor_msgs.proto‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,3 +20,11 @@ message JointState {
2020
repeated double velocity = 3;
2121
repeated double effort = 4;
2222
}
23+
24+
// https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Image.msg
25+
message Image {
26+
uint32 height = 1;
27+
uint32 width = 2;
28+
uint32 step = 3;
29+
bytes data = 4;
30+
}

0 commit comments

Comments
 (0)