Enable gRPC-based camera image reading

aegis_grpc/README.md

@@ -71,15 +71,15 @@ grpcurl -plaintext 127.0.0.1:50051 list proto_aegis_grpc.v1.RobotControlService
 grpcurl -plaintext 127.0.0.1:50051 list proto_aegis_grpc.v1.RobotReadService
 grpcurl -plaintext 127.0.0.1:50051 describe proto_aegis_grpc.v1.RobotReadService.GetAll
 # or with tool from container
-podman run --network=host docker.io/fullstorydev/grpcurl -plaintext 127.0.0.1:50051 list
+podman run --rm --network=host docker.io/fullstorydev/grpcurl -plaintext 127.0.0.1:50051 list
 # map grpcurl container alias
 alias grpcurl="podman run --network=host docker.io/fullstorydev/grpcurl"
 ```
 
 Example call to the `GetAll` method with result as a plain json:
 
 ```bash
-grpcurl -plaintext -d '{}' 127.0.0.1:50051 proto_aegis_grpc.v1.RobotReadService.GetAll
+grpcurl -max-msg-sz 20000000 -plaintext -d '{}' 127.0.0.1:50051 proto_aegis_grpc.v1.RobotReadService.GetAll
 ```
 
 ## Messages architecture

aegis_grpc/include/aegis_grpc/robot_read_service.hpp

@@ -12,6 +12,7 @@
 #include <geometry_msgs/msg/wrench.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <sensor_msgs/msg/image.hpp>
 
 namespace aegis_grpc {
 
@@ -20,22 +21,59 @@ class RobotReadServiceImpl final
   public:
     explicit RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node);
 
-    grpc::Status GetTCPPose(grpc::ServerContext* context,
-                            const google::protobuf::Empty* request,
-                            proto_aegis_grpc::v1::Pose* response) override;
-
-    grpc::Status GetWrench(grpc::ServerContext* context,
-                          const google::protobuf::Empty* request,
-                          proto_aegis_grpc::v1::Wrench* response) override;
-
-    grpc::Status
-    GetJointStates(grpc::ServerContext* context,
-                  const google::protobuf::Empty* request,
-                  proto_aegis_grpc::v1::JointState* response) override;
-
-    grpc::Status GetAll(grpc::ServerContext* context,
-                        const google::protobuf::Empty* request,
-                        proto_aegis_grpc::v1::RobotState* response) override;
+    grpc::Status GetTCPPose(
+      grpc::ServerContext* context,
+      const google::protobuf::Empty* request,
+      proto_aegis_grpc::v1::Pose* response
+    ) override;
+
+    grpc::Status GetWrench(
+      grpc::ServerContext* context,
+      const google::protobuf::Empty* request,
+      proto_aegis_grpc::v1::Wrench* response
+    ) override;
+
+    grpc::Status GetJointStates(
+      grpc::ServerContext* context,
+      const google::protobuf::Empty* request,
+      proto_aegis_grpc::v1::JointState* response
+    ) override;
+
+    grpc::Status GetCameraSceneImage(
+      grpc::ServerContext* context,
+      const google::protobuf::Empty* request,
+      proto_aegis_grpc::v1::Image* response
+    ) override;
+
+    grpc::Status GetCameraRightImage(
+      grpc::ServerContext* context,
+      const google::protobuf::Empty* request,
+      proto_aegis_grpc::v1::Image* response
+    ) override;
+
+    grpc::Status GetCameraLeftImage(
+      grpc::ServerContext* context,
+      const google::protobuf::Empty* request,
+      proto_aegis_grpc::v1::Image* response
+    ) override;
+
+    grpc::Status GetRobotState(
+        grpc::ServerContext* context,
+        const google::protobuf::Empty* request,
+        proto_aegis_grpc::v1::RobotState* response
+    ) override;
+
+    grpc::Status GetRobotVision(
+        grpc::ServerContext* context,
+        const google::protobuf::Empty* request,
+        proto_aegis_grpc::v1::RobotVision* response
+    ) override;
+
+    grpc::Status GetAll(
+        grpc::ServerContext* context,
+        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
@@ -46,32 +84,47 @@ class RobotReadServiceImpl final
     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);
+    const geometry_msgs::msg::Pose& ros,
+    proto_aegis_grpc::v1::Pose* out);
 
   static void FillProtoWrench(
-      const geometry_msgs::msg::Wrench& ros,
-      proto_aegis_grpc::v1::Wrench* out);
+    const geometry_msgs::msg::Wrench& ros,
+    proto_aegis_grpc::v1::Wrench* out);
 
   static void FillProtoJointState(
       const sensor_msgs::msg::JointState& ros,
       proto_aegis_grpc::v1::JointState* out);
 
+  static void FillProtoImage(
+    const sensor_msgs::msg::Image& ros,
+    proto_aegis_grpc::v1::Image* out);
+
     std::shared_ptr<rclcpp::Node> node_;
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
     rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_sub_;
-    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
-        joint_state_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_scene_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_right_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_left_sub_;
 
     geometry_msgs::msg::Pose pose_data_;
     geometry_msgs::msg::Wrench wrench_data_;
     sensor_msgs::msg::JointState joint_state_data_;
+    sensor_msgs::msg::Image image_scene_data_;
+    sensor_msgs::msg::Image image_right_data_;
+    sensor_msgs::msg::Image image_left_data_;
 
     std::mutex pose_mutex_;
     std::mutex wrench_mutex_;
     std::mutex joint_state_mutex_;
+    std::mutex image_scene_mutex_;
+    std::mutex image_right_mutex_;
+    std::mutex image_left_mutex_;
 };
 
 } // namespace aegis_grpc

aegis_grpc/proto_aegis_grpc/v1/robot_srvs.proto

@@ -11,7 +11,12 @@ service RobotReadService {
   rpc GetTCPPose(google.protobuf.Empty) returns (Pose);
   rpc GetWrench(google.protobuf.Empty) returns (Wrench);
   rpc GetJointStates(google.protobuf.Empty) returns (JointState);
-  rpc GetAll(google.protobuf.Empty) returns (RobotState);
+  rpc GetCameraSceneImage(google.protobuf.Empty) returns (Image);
+  rpc GetCameraRightImage(google.protobuf.Empty) returns (Image);
+  rpc GetCameraLeftImage(google.protobuf.Empty) returns (Image);
+  rpc GetRobotState(google.protobuf.Empty) returns (RobotState);
+  rpc GetRobotVision(google.protobuf.Empty) returns (RobotVision);
+  rpc GetAll(google.protobuf.Empty) returns (RobotObservation);
 }
 
 message RobotState {
@@ -20,6 +25,17 @@ message RobotState {
   JointState joint_state = 3;
 }
 
+message RobotVision {
+  Image image_scene = 1;
+  Image image_right = 2;
+  Image image_left = 3;
+}
+
+message RobotObservation {
+  RobotState robot_state = 1;
+  RobotVision robot_vision = 2;
+}
+
 service RobotControlService {
   rpc ServoEnable(google.protobuf.Empty) returns (TriggerResponse);
   rpc ServoDisable(google.protobuf.Empty) returns (TriggerResponse);

aegis_grpc/proto_aegis_grpc/v1/sensor_msgs.proto

@@ -20,3 +20,17 @@ message JointState {
   repeated double velocity = 3;
   repeated double effort = 4;
 }
+
+// https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Image.msg
+message Image {
+  uint32 height = 1;
+  uint32 width = 2;
+  enum Encoding {
+    UNKNOWN = 0;
+    BGR8 = 1;
+    BAYER_RGGB8 = 2;
+  }
+  Encoding encoding = 3;
+  uint32 step = 4;
+  bytes data = 5;
+}