body.cpp
Go to the documentation of this file.
1 // Copyright 2022 PAL Robotics S.L.
2 //
3 // Redistribution and use in source and binary forms, with or without
4 // modification, are permitted provided that the following conditions are met:
5 //
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 //
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 //
13 // * Neither the name of the PAL Robotics S.L. nor the names of its
14 // contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 
29 #include "hri/body.h"
30 
31 #include <cv_bridge/cv_bridge.h>
32 
33 using namespace std;
34 using namespace hri;
35 
36 Body::Body(ID id, ros::NodeHandle& nh, tf2_ros::Buffer* tf_buffer_ptr,
37  const std::string& reference_frame)
38  : FeatureTracker{ id, nh }, _tf_buffer_ptr(tf_buffer_ptr), _reference_frame(reference_frame)
39 {
40 }
41 
42 
44 {
45  ROS_DEBUG_STREAM("Deleting body " << id_);
47 }
48 
49 void Body::init()
50 {
51  ns_ = "/humans/bodies/" + id_;
52  ROS_DEBUG_STREAM("New body detected: " << ns_);
53 
54  roi_subscriber_ = node_.subscribe<sensor_msgs::RegionOfInterest>(
55  ns_ + "/roi", 1, bind(&Body::onRoI, this, _1));
56 
57  cropped_subscriber_ = node_.subscribe<sensor_msgs::Image>(
58  ns_ + "/cropped", 1, bind(&Body::onCropped, this, _1));
59 
60  skeleton_subscriber_ = node_.subscribe<hri_msgs::Skeleton2D>(
61  ns_ + "/skeleton2d", 1, bind(&Body::onSkeleton, this, _1));
62 }
63 
64 void Body::onRoI(sensor_msgs::RegionOfInterestConstPtr roi)
65 {
66  roi_ = cv::Rect(roi->x_offset, roi->y_offset, roi->width, roi->height);
67 }
68 
69 cv::Rect Body::roi() const
70 {
71  return roi_;
72 }
73 
74 void Body::onCropped(sensor_msgs::ImageConstPtr msg)
75 {
76  cropped_ = cv_bridge::toCvShare(msg)->image;
77 }
78 
79 cv::Mat Body::cropped() const
80 {
81  return cropped_;
82 }
83 
84 void Body::onSkeleton(hri_msgs::Skeleton2DConstPtr msg)
85 {
86  skeleton_ = msg->skeleton;
87 }
88 
89 std::vector<SkeletonPoint> Body::skeleton() const
90 {
91  return skeleton_;
92 }
93 
94 boost::optional<geometry_msgs::TransformStamped> Body::transform() const
95 {
96  try
97  {
100 
101  return transform;
102  }
103  catch (tf2::LookupException)
104  {
105  ROS_WARN_STREAM("failed to transform the body frame " << frame() << " to " << _reference_frame
106  << ". Are the frames published?");
107  return boost::optional<geometry_msgs::TransformStamped>();
108  }
109 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::string ns_
Definition: base.h:119
std::vector< SkeletonPoint > skeleton_
Definition: body.h:129
ros::NodeHandle node_
Definition: base.h:121
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void onRoI(sensor_msgs::RegionOfInterestConstPtr roi)
Definition: body.cpp:64
cv::Rect roi_
Definition: body.h:121
std::vector< SkeletonPoint > skeleton() const
Returns the 2D skeleton keypoints.
Definition: body.cpp:89
cv::Mat cropped_
Definition: body.h:125
void onCropped(sensor_msgs::ImageConstPtr roi)
Definition: body.cpp:74
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
static const ros::Duration BODY_TF_TIMEOUT(0.01)
void init() override
Definition: body.cpp:49
ros::Subscriber roi_subscriber_
Definition: body.h:119
tf2_ros::Buffer * _tf_buffer_ptr
Definition: body.h:132
std::string _reference_frame
Definition: body.h:131
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
virtual ~Body()
Definition: body.cpp:43
cv::Rect roi() const
If available, returns the 2D region of interest (RoI) of the body.
Definition: body.cpp:69
void onSkeleton(hri_msgs::Skeleton2DConstPtr skeleton)
Definition: body.cpp:84
Definition: base.h:38
ros::Subscriber skeleton_subscriber_
Definition: body.h:127
ID id() const
Definition: base.h:88
std::string frame() const
the name of the tf frame that correspond to this body
Definition: body.h:66
boost::optional< geometry_msgs::TransformStamped > transform() const
Returns the (stamped) 3D transform of the body (if available).
Definition: body.cpp:94
ros::Subscriber cropped_subscriber_
Definition: body.h:123
std::string ID
Definition: base.h:40
cv::Mat cropped() const
Returns the body image, cropped from the source image.
Definition: body.cpp:79


libhri
Author(s): Séverin Lemaignan
autogenerated on Wed Apr 19 2023 02:30:41