1- // TODO: Copyright.
1+ // TODO(Yadunund) : Copyright.
22
33#include " ibpc_pose_estimator.hpp"
44
5+ #include < exception>
6+ #include < string>
7+ #include < utility>
8+
59#include " rclcpp/rclcpp.hpp"
610
7- #include < exception >
11+ #include " sensor_msgs/msg/camera_info.hpp "
812
9- namespace ibpc {
13+ namespace ibpc
14+ {
1015
1116// ==================================================================================================
1217PoseEstimator::PoseEstimator (const rclcpp::NodeOptions & options)
1318: Node(" ibpc_pose_estimator"
1419{
15- RCLCPP_INFO (
16- this ->get_logger (),
17- " Starting ibpc_pose_estimator..."
18- );
19- // Get the path to the model.
20- std::string path_str = this ->declare_parameter (" model_path" " "
21- if (path_str.empty ()) {
22- throw std::runtime_error (" ROS param model_path cannot be empty!"
23- }
24- RCLCPP_INFO (
25- this ->get_logger (),
26- " Loading model from path [ %s ]."
27- path_str.c_str ()
28- );
29- std::filesystem::path model_path{std::move (path_str)};
30- // Load the model.
31- if (!this ->load_model (model_path)) {
32- throw std::runtime_error (" Failed to load model."
33- } else {
34- RCLCPP_INFO (
35- this ->get_logger (),
36- " Model successfully loaded!"
37- );
38- }
39- std::string srv_name =
40- this ->declare_parameter (" service_name" " /get_pose_estimates"
41- RCLCPP_INFO (
42- this ->get_logger (),
43- " Pose estimates can be queried at %s."
44- srv_name.c_str ()
45- );
46- srv_ = this ->create_service <GetPoseEstimates>(
47- std::move (srv_name),
48- [this ](std::shared_ptr<const GetPoseEstimates::Request> request,
49- std::shared_ptr<GetPoseEstimates::Response> response)
50- {
51- response->pose_estimates = this ->get_pose_estimates (std::move (request));
52- }
53- );
20+ RCLCPP_INFO (
21+ this ->get_logger (),
22+ " Starting ibpc_pose_estimator..."
23+ );
24+ // Get the path to the model.
25+ std::string path_str = this ->declare_parameter (" model_dir" " "
26+ if (path_str.empty ()) {
27+ throw std::runtime_error (" ROS param model_dir cannot be empty!"
28+ }
29+ RCLCPP_INFO (
30+ this ->get_logger (),
31+ " Model directory set to [ %s ]."
32+ path_str.c_str ()
33+ );
34+ model_dir_ = std::filesystem::path (std::move (path_str));
35+
36+ std::string srv_name =
37+ this ->declare_parameter (" service_name" " /get_pose_estimates"
38+ RCLCPP_INFO (
39+ this ->get_logger (),
40+ " Pose estimates can be queried over srv %s."
41+ srv_name.c_str ()
42+ );
43+ srv_ = this ->create_service <GetPoseEstimates>(
44+ std::move (srv_name),
45+ [this ](std::shared_ptr<const GetPoseEstimates::Request> request,
46+ std::shared_ptr<GetPoseEstimates::Response> response)
47+ {
48+ cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare (request->rgb , request);
49+ this ->rgb_camera_model_ .fromCameraInfo (request->rgb_info );
50+ cv_bridge::CvImageConstPtr depth = cv_bridge::toCvShare (request->depth , request);
51+ this ->depth_camera_model_ .fromCameraInfo (request->depth_info );
52+ cv_bridge::CvImageConstPtr polarized = cv_bridge::toCvShare (request->polarized , request);
53+ this ->polarized_camera_model_ .fromCameraInfo (request->polarized_info );
54+ response->pose_estimates = this ->get_pose_estimates (
55+ request->object_ids ,
56+ rgb->image ,
57+ this ->rgb_camera_model_ ,
58+ depth->image ,
59+ this ->depth_camera_model_ ,
60+ polarized->image ,
61+ this ->polarized_camera_model_ );
62+ }
63+ );
5464}
5565
5666// ==================================================================================================
57- bool PoseEstimator::load_model ( const std::filesystem::path & model_path)
67+ const std::filesystem::path & PoseEstimator::model_dir () const
5868{
59- // Fill.
60- return true ;
69+ return model_dir_;
6170}
6271
6372// ==================================================================================================
6473auto PoseEstimator::get_pose_estimates (
65- std::shared_ptr<
const GetPoseEstimates::Request> request) -> std::vector<PoseEstimate>
74+ const std::vector<uint64_t > & object_ids,
75+ const cv::Mat & rgb,
76+ const image_geometry::PinholeCameraModel & rgb_camera_model,
77+ const cv::Mat & depth,
78+ const image_geometry::PinholeCameraModel & depth_camera_model,
79+ const cv::Mat & polarized,
80+ const image_geometry::PinholeCameraModel & polarized_camera_model) -> std::vector<PoseEstimate>
6681{
67- std::vector<PoseEstimate> pose_estimates = {};
82+ std::vector<PoseEstimate> pose_estimates = {};
6883
69- // Fill.
84+ // Fill.
7085
71- return pose_estimates;
86+ return pose_estimates;
7287}
7388
7489} // namespace ibpc
7590
7691// ==================================================================================================
7792#include < rclcpp_components/register_node_macro.hpp>
78- RCLCPP_COMPONENTS_REGISTER_NODE (ibpc::PoseEstimator)
93+ RCLCPP_COMPONENTS_REGISTER_NODE (ibpc::PoseEstimator)
0 commit comments