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>
32 #include <opencv2/core/core.hpp>
33 #include <rtabmap_ros/UserData.h>
36 #include <visualization_msgs/MarkerArray.h>
37 
39 {
40 public:
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);
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
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  {
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;
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_;
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 }
cv::Mat RTABMAP_EXP uncompressData(const cv::Mat &bytes)
std::string uFormat(const char *fmt,...)
Quaternion getRotation() const
ros::Publisher pubMarkers_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
data
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Transform interpolate(float t, const Transform &other) const
float getNorm() const
#define ROS_WARN(...)
ros::Subscriber subsMapData_
std::map< double, int > nodeStamps_
#define UASSERT(condition)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
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)
int main(int argc, char *argv[])
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
SensorData & sensorData()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
const cv::Mat & userDataCompressed() const
double getStamp() const
std::map< int, cv::Mat > nodeToObjects_
void userDataToROS(const cv::Mat &data, rtabmap_ros::UserData &dataMsg, bool compress)
#define ROS_ASSERT(cond)
tf::TransformListener tfListener_
ros::Subscriber subObjects_
void objectsDetectedCallback(const find_object_2d::ObjectsStampedConstPtr &msg)
q
void mapDataCallback(const rtabmap_ros::MapDataConstPtr &msg)


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