SaveObjectsExample.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
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  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
29 #include <tf/transform_listener.h>
30 #include <find_object_2d/ObjectsStamped.h>
31 #include <rtabmap/utilite/UConversion.h>
32 #include <opencv2/core/core.hpp>
33 #include <rtabmap_ros/UserData.h>
34 #include <rtabmap_ros/MsgConversion.h>
35 #include <rtabmap/core/Compression.h>
36 #include <visualization_msgs/MarkerArray.h>
37 
38 class SaveObjectsExample
39 {
40 public:
41  SaveObjectsExample() :
42  objFramePrefix_("object"),
43  frameId_("base_link")
44  {
45  ros::NodeHandle pnh("~");
46  pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
47  pnh.param("frame_id", frameId_, frameId_);
48 
49  ros::NodeHandle nh;
50  subObjects_ = nh.subscribe("objectsStamped", 1, &SaveObjectsExample::objectsDetectedCallback, this);
51  subsMapData_ = nh.subscribe("mapData", 1, &SaveObjectsExample::mapDataCallback, this);
52  pub_ = nh.advertise<rtabmap_ros::UserData>("objectsData", 1);
53  pubMarkers_ = nh.advertise<visualization_msgs::MarkerArray>("objectsMarkers", 1);
54  }
55 
56  // from find_object_2d to rtabmap
57  void objectsDetectedCallback(const find_object_2d::ObjectsStampedConstPtr & msg)
58  {
59  if(msg->objects.data.size())
60  {
61  cv::Mat data(msg->objects.data.size()/12, 9, CV_64FC1);
62  for(unsigned int i=0; i<msg->objects.data.size(); i+=12)
63  {
64  // get data
65  int id = (int)msg->objects.data[i];
66  std::string objectFrameId = uFormat("%s_%d", objFramePrefix_.c_str(),id); // "object_1", "object_2"
67 
68  // get pose of the object in base frame
69  tf::StampedTransform pose;
70  try
71  {
72  tfListener_.lookupTransform(frameId_, objectFrameId, msg->header.stamp, pose);
73  }
74  catch(tf::TransformException & ex)
75  {
76  ROS_WARN("%s",ex.what());
77  return;
78  }
79 
80  data.at<double>(i, 0) = double(id);
81  data.at<double>(i, 1) = ros::Time(msg->header.stamp).toSec();
82  data.at<double>(i, 2) = pose.getOrigin().x();
83  data.at<double>(i, 3) = pose.getOrigin().y();
84  data.at<double>(i, 4) = pose.getOrigin().z();
85  data.at<double>(i, 5) = pose.getRotation().x();
86  data.at<double>(i, 6) = pose.getRotation().y();
87  data.at<double>(i, 7) = pose.getRotation().z();
88  data.at<double>(i, 8) = pose.getRotation().w();
89  }
90 
91  rtabmap_ros::UserData dataMsg;
92  dataMsg.header.frame_id = msg->header.frame_id;
93  dataMsg.header.stamp = msg->header.stamp;
94  rtabmap_ros::userDataToROS(data, dataMsg, false);
95  pub_.publish(dataMsg);
96  }
97  }
98 
99  // from rtabmap to rviz visualization
100  void mapDataCallback(const rtabmap_ros::MapDataConstPtr & msg)
101  {
102  std::map<int, rtabmap::Signature> signatures;
103  std::map<int, rtabmap::Transform> poses;
104  std::multimap<int, rtabmap::Link> links;
105  rtabmap::Transform mapToOdom;
106  rtabmap_ros::mapDataFromROS(*msg, poses, links, signatures, mapToOdom);
107 
108  // handle the case where we can receive only latest data, or if all data are published
109  for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
110  {
111  int id = iter->first;
112  rtabmap::Signature & node = iter->second;
113 
114  nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
115 
116  if(!node.sensorData().userDataCompressed().empty() && nodeToObjects_.find(id)==nodeToObjects_.end())
117  {
118  cv::Mat data = rtabmap::uncompressData(node.sensorData().userDataCompressed());
119  ROS_ASSERT(data.cols == 9 && data.type() == CV_64FC1);
120  ROS_INFO("Node %d has %d object(s)", id, data.rows);
121  nodeToObjects_.insert(std::make_pair(id, data));
122  }
123  }
124 
125  // for the logic below, we should keep only stamps for
126  // nodes still in the graph (in case nodes are ignored when not moving)
127  std::map<double, int> nodeStamps;
128  for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
129  {
130  std::map<int, rtabmap::Transform>::const_iterator jter = poses.find(iter->second);
131  if(jter != poses.end())
132  {
133  nodeStamps.insert(*iter);
134  }
135  }
136 
137  // Publish markers accordingly to current optimized graph
138  std::map<int, float> objectsAdded;
139  visualization_msgs::MarkerArray markers;
140 
141  if(nodeToObjects_.size())
142  {
143  for(std::map<int, rtabmap::Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
144  {
145  if(nodeToObjects_.find(iter->first) != nodeToObjects_.end())
146  {
147  cv::Mat data = nodeToObjects_.at(iter->first);
148  for(int i=0; i<data.rows; ++i)
149  {
150  int objId = int(data.at<double>(i,0));
151  double stamp = data.at<double>(i,1);
152 
153  // The object detection may have been taken between two nodes, interpolate its position.
154  std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stamp); // lower bound of the stamp
155  if(previousNode!=nodeStamps.end() && previousNode->first > stamp && previousNode != nodeStamps.begin())
156  {
157  --previousNode;
158  }
159  std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stamp); // upper bound of the stamp
160 
161  if(previousNode != nodeStamps.end() &&
162  nextNode != nodeStamps.end() &&
163  previousNode->second != nextNode->second &&
164  poses.find(previousNode->second)!=poses.end() && poses.find(nextNode->second)!=poses.end())
165  {
166  rtabmap::Transform poseA = poses.at(previousNode->second);
167  rtabmap::Transform poseB = poses.at(nextNode->second);
168  double stampA = previousNode->first;
169  double stampB = nextNode->first;
170  UASSERT(stamp>=stampA && stamp <=stampB);
171 
172  double ratio = (stamp-stampA)/(stampB-stampA);
173  rtabmap::Transform robotPose = poseA.interpolate(ratio, poseB);
174 
175  // transform object pose in map frame
176  rtabmap::Transform objPose(
177  data.at<double>(i,2), data.at<double>(i,3), data.at<double>(i,4),
178  data.at<double>(i,5), data.at<double>(i,6), data.at<double>(i,7), data.at<double>(i,8));
179  float distanceFromNode = objPose.getNorm();
180  objPose = robotPose * objPose;
181 
182  if(objectsAdded.find(objId) == objectsAdded.end())
183  {
184  visualization_msgs::Marker marker;
185  marker.header.frame_id = msg->header.frame_id;
186  marker.header.stamp = msg->header.stamp;
187  marker.ns = "objects";
188  marker.id = objId;
189  marker.action = visualization_msgs::Marker::ADD;
190  marker.pose.position.x = objPose.x();
191  marker.pose.position.y = objPose.y();
192  marker.pose.position.z = objPose.z();
193  Eigen::Quaterniond q = objPose.getQuaterniond();
194  marker.pose.orientation.x = q.x();
195  marker.pose.orientation.y = q.y();
196  marker.pose.orientation.z = q.z();
197  marker.pose.orientation.w = q.w();
198  marker.scale.x = 0.3;
199  marker.scale.y = 0.3;
200  marker.scale.z = 0.3;
201  marker.color.a = 0.8;
202  marker.color.r = 0.0;
203  marker.color.g = 1.0;
204  marker.color.b = 0.0;
205 
206  // cube
207  marker.type = visualization_msgs::Marker::CUBE;
208  markers.markers.push_back(marker);
209 
210  // text
211  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
212  marker.text = uFormat("%s_%d", objFramePrefix_.c_str(), objId);
213  marker.id = -objId;
214  marker.color.a = 1.0;
215  marker.pose.position.z += 0.3; // text over the cube
216  markers.markers.push_back(marker);
217 
218  objectsAdded.insert(std::make_pair(objId, distanceFromNode));
219  }
220  else
221  {
222  // update pose of the marker only if current observation is closer to robot
223  if(distanceFromNode < objectsAdded.at(objId))
224  {
225  for(unsigned int j=0; j<markers.markers.size(); ++j)
226  {
227  if(markers.markers[j].id == objId || markers.markers[j].id == -objId)
228  {
229  markers.markers[j].pose.position.x = objPose.x();
230  markers.markers[j].pose.position.y = objPose.y();
231  markers.markers[j].pose.position.z = objPose.z() + (markers.markers[j].id<0?0.3:0);
232  Eigen::Quaterniond q = objPose.getQuaterniond();
233  markers.markers[j].pose.orientation.x = q.x();
234  markers.markers[j].pose.orientation.y = q.y();
235  markers.markers[j].pose.orientation.z = q.z();
236  markers.markers[j].pose.orientation.w = q.w();
237  }
238  }
239  objectsAdded.at(objId) = distanceFromNode;
240  }
241  }
242  }
243  }
244  }
245  }
246  if(!markers.markers.empty())
247  {
248  pubMarkers_.publish(markers);
249  }
250  }
251  }
252 
253 private:
254  std::string objFramePrefix_;
255  ros::Subscriber subObjects_;
256  ros::Subscriber subsMapData_;
257  tf::TransformListener tfListener_;
258  ros::Publisher pub_;
259  ros::Publisher pubMarkers_;
260  std::map<int, cv::Mat> nodeToObjects_;
261  std::map<double, int> nodeStamps_; // <stamp, id>
262  std::string frameId_;
263 };
264 
265 int main(int argc, char * argv[])
266 {
267  ros::init(argc, argv, "save_objects_example");
268 
269  SaveObjectsExample sync;
270  ros::spin();
271 }
rtabmap::uncompressData
cv::Mat RTABMAP_EXP uncompressData(const cv::Mat &bytes)
uFormat
std::string uFormat(const char *fmt,...)
transform_listener.h
ros::NodeHandle
tf::Transform::getRotation
Quaternion getRotation() const
SaveObjectsExample::pubMarkers_
ros::Publisher pubMarkers_
Definition: SaveObjectsExample.cpp:259
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
data
data
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
rtabmap::Transform::interpolate
Transform interpolate(float t, const Transform &other) const
rtabmap::Transform::getNorm
float getNorm() const
ROS_WARN
#define ROS_WARN(...)
SaveObjectsExample::subsMapData_
ros::Subscriber subsMapData_
Definition: SaveObjectsExample.cpp:256
SaveObjectsExample::nodeStamps_
std::map< double, int > nodeStamps_
Definition: SaveObjectsExample.cpp:261
UASSERT
#define UASSERT(condition)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap_ros::mapDataFromROS
void mapDataFromROS(const rtabmap_ros::MapData &msg, std::map< int, rtabmap::Transform > &poses, std::multimap< int, rtabmap::Link > &links, std::map< int, rtabmap::Signature > &signatures, rtabmap::Transform &mapToOdom)
Definition: MsgConversion.cpp:977
main
int main(int argc, char *argv[])
Definition: SaveObjectsExample.cpp:265
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::spin
ROSCPP_DECL void spin()
gt_tf_broadcaster.stamp
float stamp
Definition: gt_tf_broadcaster.py:28
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
cartographer_sync_markers_gt.MarkerArray
MarkerArray
Definition: cartographer_sync_markers_gt.py:57
rtabmap::Signature::sensorData
SensorData & sensorData()
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
rtabmap::SensorData::userDataCompressed
const cv::Mat & userDataCompressed() const
rtabmap::Signature::getStamp
double getStamp() const
SaveObjectsExample::nodeToObjects_
std::map< int, cv::Mat > nodeToObjects_
Definition: SaveObjectsExample.cpp:260
rtabmap_ros::userDataToROS
void userDataToROS(const cv::Mat &data, rtabmap_ros::UserData &dataMsg, bool compress)
Definition: MsgConversion.cpp:1643
ROS_ASSERT
#define ROS_ASSERT(cond)
SaveObjectsExample::tfListener_
tf::TransformListener tfListener_
Definition: SaveObjectsExample.cpp:257
SaveObjectsExample::subObjects_
ros::Subscriber subObjects_
Definition: SaveObjectsExample.cpp:255
SaveObjectsExample::objectsDetectedCallback
void objectsDetectedCallback(const find_object_2d::ObjectsStampedConstPtr &msg)
Definition: SaveObjectsExample.cpp:57
q
q
SaveObjectsExample::mapDataCallback
void mapDataCallback(const rtabmap_ros::MapDataConstPtr &msg)
Definition: SaveObjectsExample.cpp:100

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40