Skip to content

Commit 8a77a27

Browse files
sivralsivral
committed
Integrate camera image reading via gRPC
1 parent 857ddc3 commit 8a77a27

File tree

2 files changed

+196
-22
lines changed

2 files changed

+196
-22
lines changed

‎aegis_grpc/include/aegis_grpc/robot_read_service.hpp‎

Lines changed: 69 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
#include <geometry_msgs/msg/wrench.hpp>
1313
#include <geometry_msgs/msg/wrench_stamped.hpp>
1414
#include <sensor_msgs/msg/joint_state.hpp>
15+
#include <sensor_msgs/msg/image.hpp>
1516

1617
namespace aegis_grpc {
1718

@@ -20,22 +21,53 @@ class RobotReadServiceImpl final
2021
public:
2122
explicit RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node);
2223

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;
24+
grpc::Status GetTCPPose(
25+
grpc::ServerContext* context,
26+
const google::protobuf::Empty* request,
27+
proto_aegis_grpc::v1::Pose* response
28+
) override;
29+
30+
grpc::Status GetWrench(
31+
grpc::ServerContext* context,
32+
const google::protobuf::Empty* request,
33+
proto_aegis_grpc::v1::Wrench* response
34+
) override;
35+
36+
grpc::Status GetJointStates(
37+
grpc::ServerContext* context,
38+
const google::protobuf::Empty* request,
39+
proto_aegis_grpc::v1::JointState* response
40+
) override;
41+
42+
grpc::Status GetCameraSceneImage(
43+
grpc::ServerContext* context,
44+
const google::protobuf::Empty* request,
45+
proto_aegis_grpc::v1::Image* response
46+
) override;
47+
48+
grpc::Status GetCameraRightImage(
49+
grpc::ServerContext* context,
50+
const google::protobuf::Empty* request,
51+
proto_aegis_grpc::v1::Image* response
52+
) override;
53+
54+
grpc::Status GetCameraLeftImage(
55+
grpc::ServerContext* context,
56+
const google::protobuf::Empty* request,
57+
proto_aegis_grpc::v1::Image* response
58+
) override;
59+
60+
grpc::Status GetAll(
61+
grpc::ServerContext* context,
62+
const google::protobuf::Empty* request,
63+
proto_aegis_grpc::v1::RobotState* response
64+
) override;
65+
66+
grpc::Status GetAllVis(
67+
grpc::ServerContext* context,
68+
const google::protobuf::Empty* request,
69+
proto_aegis_grpc::v1::RobotStateVis* response
70+
) override;
3971

4072
// TODO(issue#84) implement getters for images from cameras (RGB & RGBD)
4173
// TODO(issue#85) develop synchronization guard to monitor the freshness of the data
@@ -46,32 +78,47 @@ class RobotReadServiceImpl final
4678
void PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
4779
void WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg);
4880
void JointStateSubCb(const sensor_msgs::msg::JointState::SharedPtr msg);
81+
void ImageSceneSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
82+
void ImageRightSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
83+
void ImageLeftSubCb(const sensor_msgs::msg::Image::SharedPtr msg);
4984

5085
static void FillProtoPose(
51-
const geometry_msgs::msg::Pose& ros,
52-
proto_aegis_grpc::v1::Pose* out);
86+
const geometry_msgs::msg::Pose& ros,
87+
proto_aegis_grpc::v1::Pose* out);
5388

5489
static void FillProtoWrench(
55-
const geometry_msgs::msg::Wrench& ros,
56-
proto_aegis_grpc::v1::Wrench* out);
90+
const geometry_msgs::msg::Wrench& ros,
91+
proto_aegis_grpc::v1::Wrench* out);
5792

5893
static void FillProtoJointState(
5994
const sensor_msgs::msg::JointState& ros,
6095
proto_aegis_grpc::v1::JointState* out);
6196

97+
static void FillProtoImage(
98+
const sensor_msgs::msg::Image& ros,
99+
proto_aegis_grpc::v1::Image* out);
100+
62101
std::shared_ptr<rclcpp::Node> node_;
63102
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
64103
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_sub_;
65-
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
66-
joint_state_sub_;
104+
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
105+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_scene_sub_;
106+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_right_sub_;
107+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_left_sub_;
67108

68109
geometry_msgs::msg::Pose pose_data_;
69110
geometry_msgs::msg::Wrench wrench_data_;
70111
sensor_msgs::msg::JointState joint_state_data_;
112+
sensor_msgs::msg::Image image_scene_data_;
113+
sensor_msgs::msg::Image image_right_data_;
114+
sensor_msgs::msg::Image image_left_data_;
71115

72116
std::mutex pose_mutex_;
73117
std::mutex wrench_mutex_;
74118
std::mutex joint_state_mutex_;
119+
std::mutex image_scene_mutex_;
120+
std::mutex image_right_mutex_;
121+
std::mutex image_left_mutex_;
75122
};
76123

77124
} // namespace aegis_grpc

‎aegis_grpc/src/robot_read_service.cpp‎

Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,10 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
99
DeclareROSParameter("topic_pose", "/tcp_pose", "[str] Init; Sub: topic with the TCP pose data.");
1010
DeclareROSParameter("topic_wrench", "/wrench", "[str] Init; Sub: topic with the F/T data.");
1111
DeclareROSParameter("topic_joints", "/joint_states", "[str] Init; Sub: topic with the joint states.");
12+
DeclareROSParameter("topic_camera_scene", "/cam_scene_rgb/image_rect", "[str] Camera scene image topic");
13+
DeclareROSParameter("topic_camera_right", "/cam_tool_right/image_rect", "[str] Camera scene image topic");
14+
DeclareROSParameter("topic_camera_left", "/cam_tool_left/image_rect", "[str] Camera scene image topic");
15+
1216

1317
pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
1418
node_->get_parameter("topic_pose").as_string(), 10,
@@ -22,6 +26,18 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
2226
node_->get_parameter("topic_joints").as_string(), 10,
2327
std::bind(&RobotReadServiceImpl::JointStateSubCb, this,
2428
std::placeholders::_1));
29+
image_scene_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
30+
node_->get_parameter("topic_camera_scene").as_string(), 10,
31+
std::bind(&RobotReadServiceImpl::ImageSceneSubCb, this,
32+
std::placeholders::_1));
33+
image_right_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
34+
node_->get_parameter("topic_camera_right").as_string(), 10,
35+
std::bind(&RobotReadServiceImpl::ImageRightSubCb, this,
36+
std::placeholders::_1));
37+
image_left_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
38+
node_->get_parameter("topic_camera_left").as_string(), 10,
39+
std::bind(&RobotReadServiceImpl::ImageLeftSubCb, this,
40+
std::placeholders::_1));
2541
}
2642

2743
void RobotReadServiceImpl::DeclareROSParameter(const std::string& name, const std::string& default_val, const std::string& description) {
@@ -53,6 +69,24 @@ void RobotReadServiceImpl::JointStateSubCb(
5369
joint_state_data_ = *msg;
5470
}
5571

72+
void RobotReadServiceImpl::ImageSceneSubCb(
73+
const sensor_msgs::msg::Image::SharedPtr msg) {
74+
std::lock_guard<std::mutex> lock(image_scene_mutex_);
75+
image_scene_data_ = *msg;
76+
}
77+
78+
void RobotReadServiceImpl::ImageRightSubCb(
79+
const sensor_msgs::msg::Image::SharedPtr msg) {
80+
std::lock_guard<std::mutex> lock(image_right_mutex_);
81+
image_right_data_ = *msg;
82+
}
83+
84+
void RobotReadServiceImpl::ImageLeftSubCb(
85+
const sensor_msgs::msg::Image::SharedPtr msg) {
86+
std::lock_guard<std::mutex> lock(image_left_mutex_);
87+
image_left_data_ = *msg;
88+
}
89+
5690
grpc::Status
5791
RobotReadServiceImpl::GetTCPPose(grpc::ServerContext *context,
5892
const google::protobuf::Empty *request,
@@ -91,6 +125,45 @@ grpc::Status RobotReadServiceImpl::GetJointStates(
91125
return grpc::Status::OK;
92126
}
93127

128+
grpc::Status RobotReadServiceImpl::GetCameraSceneImage(
129+
grpc::ServerContext* context,
130+
const google::protobuf::Empty* request,
131+
proto_aegis_grpc::v1::Image* response) {
132+
(void)context;
133+
(void)request;
134+
{
135+
std::lock_guard<std::mutex> lock(image_scene_mutex_);
136+
FillProtoImage(image_scene_data_, response);
137+
}
138+
return grpc::Status::OK;
139+
}
140+
141+
grpc::Status RobotReadServiceImpl::GetCameraRightImage(
142+
grpc::ServerContext* context,
143+
const google::protobuf::Empty* request,
144+
proto_aegis_grpc::v1::Image* response) {
145+
(void)context;
146+
(void)request;
147+
{
148+
std::lock_guard<std::mutex> lock(image_right_mutex_);
149+
FillProtoImage(image_right_data_, response);
150+
}
151+
return grpc::Status::OK;
152+
}
153+
154+
grpc::Status RobotReadServiceImpl::GetCameraLeftImage(
155+
grpc::ServerContext* context,
156+
const google::protobuf::Empty* request,
157+
proto_aegis_grpc::v1::Image* response) {
158+
(void)context;
159+
(void)request;
160+
{
161+
std::lock_guard<std::mutex> lock(image_left_mutex_);
162+
FillProtoImage(image_left_data_, response);
163+
}
164+
return grpc::Status::OK;
165+
}
166+
94167
grpc::Status
95168
RobotReadServiceImpl::GetAll(grpc::ServerContext *context,
96169
const google::protobuf::Empty *request,
@@ -112,6 +185,42 @@ RobotReadServiceImpl::GetAll(grpc::ServerContext *context,
112185
return grpc::Status::OK;
113186
}
114187

188+
grpc::Status RobotReadServiceImpl::GetAllVis(
189+
grpc::ServerContext* context,
190+
const google::protobuf::Empty* request,
191+
proto_aegis_grpc::v1::RobotStateVis* response) {
192+
193+
(void)context;
194+
(void)request;
195+
196+
{
197+
std::lock_guard<std::mutex> lock(pose_mutex_);
198+
FillProtoPose(pose_data_, response->mutable_pose());
199+
}
200+
{
201+
std::lock_guard<std::mutex> lock(wrench_mutex_);
202+
FillProtoWrench(wrench_data_, response->mutable_wrench());
203+
}
204+
{
205+
std::lock_guard<std::mutex> lock(joint_state_mutex_);
206+
FillProtoJointState(joint_state_data_, response->mutable_joint_state());
207+
}
208+
{
209+
std::lock_guard<std::mutex> lock(image_scene_mutex_);
210+
FillProtoImage(image_scene_data_, response->mutable_image_scene());
211+
}
212+
{
213+
std::lock_guard<std::mutex> lock(image_right_mutex_);
214+
FillProtoImage(image_right_data_, response->mutable_image_right());
215+
}
216+
{
217+
std::lock_guard<std::mutex> lock(image_left_mutex_);
218+
FillProtoImage(image_left_data_, response->mutable_image_left());
219+
}
220+
221+
return grpc::Status::OK;
222+
}
223+
115224
void RobotReadServiceImpl::FillProtoPose(
116225
const geometry_msgs::msg::Pose& ros,
117226
proto_aegis_grpc::v1::Pose* out) {
@@ -155,5 +264,23 @@ void RobotReadServiceImpl::FillProtoJointState(
155264
for (const auto& v : ros.effort) out->add_effort(v);
156265
}
157266

267+
void RobotReadServiceImpl::FillProtoImage(
268+
const sensor_msgs::msg::Image& ros,
269+
proto_aegis_grpc::v1::Image* out) {
270+
out->set_height(ros.height);
271+
out->set_width(ros.width);
272+
273+
if (ros.encoding == "bgr8") {
274+
out->set_encoding(proto_aegis_grpc::v1::Image::BGR8);
275+
} else if (ros.encoding == "bayer_rggb8") {
276+
out->set_encoding(proto_aegis_grpc::v1::Image::BAYER_RGGB8);
277+
} else {
278+
out->set_encoding(proto_aegis_grpc::v1::Image::UNKNOWN);
279+
}
280+
281+
out->set_step(ros.step);
282+
out->set_data(ros.data.data(), ros.data.size());
283+
}
284+
158285

159286
} // namespace aegis_grpc

0 commit comments

Comments
 (0)