network_of_poses.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "mrpt_bridge/network_of_poses.h"
11 #include "mrpt_bridge/pose.h"
12 
13 #include <geometry_msgs/Pose.h>
14 #include <geometry_msgs/PoseWithCovariance.h>
15 #include <mrpt_msgs/NodeIDWithPose.h>
16 
17 #include <iostream> // for debugging reasons
18 
19 #include <mrpt/version.h>
20 #if MRPT_VERSION < 0x199
21 using namespace mrpt::utils;
22 #else
23 #include <mrpt/graphs/TNodeID.h>
24 using mrpt::graphs::TNodeID;
25 #endif
26 
27 namespace mrpt_bridge
28 {
30 // MRPT => ROS
32 
33 void convert(
34  const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph,
35  mrpt_msgs::NetworkOfPoses& ros_graph)
36 {
37  MRPT_START;
38  using namespace geometry_msgs;
39  using namespace mrpt::math;
40  using namespace mrpt::graphs;
41  using namespace mrpt::poses;
42  using namespace std;
43 
44  typedef typename mrpt::graphs::CNetworkOfPoses2DInf::global_poses_t::
45  const_iterator poses_cit_t;
46 
47  const CNetworkOfPoses2DInf::BASE::edges_map_t& constraints =
48  mrpt_graph.BASE::edges;
49 
50  // fill root node
51  ros_graph.root = mrpt_graph.root;
52 
53  // fill nodeIDs, positions
54  for (poses_cit_t poses_cit = mrpt_graph.nodes.begin();
55  poses_cit != mrpt_graph.nodes.end(); ++poses_cit)
56  {
57  mrpt_msgs::NodeIDWithPose ros_node;
58 
59  // nodeID
60  ros_node.nodeID = poses_cit->first;
61  // pose
62  convert(poses_cit->second, ros_node.pose);
63 
64  // zero the optional fields
65  ros_node.str_ID.data = "";
66  ros_node.nodeID_loc = 0;
67 
68  ros_graph.nodes.vec.push_back(ros_node);
69  }
70 
71  // fill the constraints
72  for (CNetworkOfPoses2DInf::const_iterator constr_it = constraints.begin();
73  constr_it != constraints.end(); ++constr_it)
74  {
75  mrpt_msgs::GraphConstraint ros_constr;
76 
77  // constraint ends
78  ros_constr.nodeID_from = constr_it->first.first;
79  ros_constr.nodeID_to = constr_it->first.second;
80 
81  // constraint mean and covariance
82  if (mrpt_graph.edges_store_inverse_poses)
83  {
84  CPosePDFGaussianInf constr_inv;
85  constr_it->second.inverse(constr_inv);
86  convert(constr_inv, ros_constr.constraint);
87  }
88  else
89  {
90  convert(constr_it->second, ros_constr.constraint);
91  }
92 
93  ros_graph.constraints.push_back(ros_constr);
94  }
95 
96  MRPT_END;
97 }
98 
99 void convert(
100  const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph,
101  mrpt_msgs::NetworkOfPoses& ros_graph)
102 {
103  THROW_EXCEPTION("Conversion not implemented yet");
104  MRPT_TODO(
105  "Implement CNetworkOfPoses3DInf => mrpt_msgs::NetworkOfPoses "
106  "conversion.");
107 }
108 
109 void convert(
110  const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph,
111  mrpt_msgs::NetworkOfPoses& ros_graph)
112 {
113  MRPT_START;
114 
115  using namespace geometry_msgs;
116  using namespace mrpt::math;
117  using namespace mrpt::graphs;
118  using namespace mrpt::poses;
119  using namespace std;
120 
121  typedef typename mrpt::graphs::CNetworkOfPoses2DInf_NA::global_poses_t::
122  const_iterator poses_cit_t;
123 
125  // for (poses_cit_t it = mrpt_graph.nodes.begin();
126  // it != mrpt_graph.nodes.end();
127  //++it) {
128  // cout << it->first << " | " << it->second << endl;
129  //}
130 
131  const CNetworkOfPoses2DInf_NA::BASE::edges_map_t& constraints =
132  mrpt_graph.BASE::edges;
133 
134  // fill root node
135  ros_graph.root = mrpt_graph.root;
136 
137  // fill nodeIDs, positions
138  for (poses_cit_t poses_cit = mrpt_graph.nodes.begin();
139  poses_cit != mrpt_graph.nodes.end(); ++poses_cit)
140  {
141  mrpt_msgs::NodeIDWithPose ros_node;
142 
143  // nodeID
144  ros_node.nodeID = poses_cit->first;
145  // pose
146  convert(poses_cit->second, ros_node.pose);
147 
148  // optional fields for the MR-SLAM case
149  ros_node.str_ID.data = poses_cit->second.agent_ID_str;
150  ros_node.nodeID_loc = poses_cit->second.nodeID_loc;
151 
152  ros_graph.nodes.vec.push_back(ros_node);
153  }
154 
155  // fill the constraints -- same as in the conversion from
156  // CNetworkOfPoses2DInf
157  for (CNetworkOfPoses2DInf::const_iterator constr_it = constraints.begin();
158  constr_it != constraints.end(); ++constr_it)
159  {
160  mrpt_msgs::GraphConstraint ros_constr;
161 
162  // constraint ends
163  ros_constr.nodeID_from = constr_it->first.first;
164  ros_constr.nodeID_to = constr_it->first.second;
165 
166  // constraint mean and covariance
167  if (mrpt_graph.edges_store_inverse_poses)
168  {
169  CPosePDFGaussianInf constr_inv;
170  constr_it->second.inverse(constr_inv);
171  convert(constr_inv, ros_constr.constraint);
172  }
173  else
174  {
175  convert(constr_it->second, ros_constr.constraint);
176  }
177 
178  ros_graph.constraints.push_back(ros_constr);
179  }
180 
181  MRPT_END;
182 }
183 
184 void convert(
185  const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph,
186  mrpt_msgs::NetworkOfPoses& ros_graph)
187 {
188  THROW_EXCEPTION("Conversion not implemented yet");
189 }
190 
192 // ROS => MRPT
194 
195 void convert(
196  const mrpt_msgs::NetworkOfPoses& ros_graph,
197  mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph)
198 {
199  MRPT_START;
200  using namespace mrpt::poses;
201  using namespace mrpt_msgs;
202  using namespace std;
203 
204  typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t;
205  typedef NetworkOfPoses::_constraints_type::const_iterator constraints_cit_t;
206 
207  // fill root node
208  mrpt_graph.root = ros_graph.root;
209 
210  // fill nodeIDs, positions
211  for (nodes_cit_t nodes_cit = ros_graph.nodes.vec.begin();
212  nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit)
213  {
214  // get the pose
215  CPose2D mrpt_pose;
216  convert(nodes_cit->pose, mrpt_pose);
217 
218  mrpt_graph.nodes.insert(
219  make_pair(static_cast<TNodeID>(nodes_cit->nodeID), mrpt_pose));
220  }
221 
222  // fill the constraints
223  for (constraints_cit_t constr_cit = ros_graph.constraints.begin();
224  constr_cit != ros_graph.constraints.end(); ++constr_cit)
225  {
226  // constraint ends
227  auto constr_ends(make_pair(
228  static_cast<TNodeID>(constr_cit->nodeID_from),
229  static_cast<TNodeID>(constr_cit->nodeID_to)));
230 
231  // constraint value
232  mrpt::poses::CPosePDFGaussianInf mrpt_constr;
233  convert(constr_cit->constraint, mrpt_constr);
234 
235  mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr));
236  }
237 
238  mrpt_graph.edges_store_inverse_poses = false;
239 
240  MRPT_END;
241 }
242 
243 void convert(
244  const mrpt_msgs::NetworkOfPoses& ros_graph,
245  mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph)
246 {
247  THROW_EXCEPTION("Conversion not implemented yet");
248  MRPT_TODO(
249  "Implement mrpt_msgs::NetworkOfPoses => CNetworkOfPoses3DInf "
250  "conversion.");
251 }
252 
253 void convert(
254  const mrpt_msgs::NetworkOfPoses& ros_graph,
255  mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph)
256 {
257  MRPT_START;
258  using namespace mrpt::poses;
259  using namespace mrpt_msgs;
260  using namespace std;
261 
262  typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t;
263  typedef NetworkOfPoses::_constraints_type::const_iterator constraints_cit_t;
264 
265  typedef mrpt::graphs::CNetworkOfPoses2DInf_NA::global_pose_t
266  mrpt_graph_pose_t;
267 
268  // fill root node
269  mrpt_graph.root = ros_graph.root;
270 
271  // fill nodeIDs, positions
272  for (nodes_cit_t nodes_cit = ros_graph.nodes.vec.begin();
273  nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit)
274  {
275  mrpt_graph_pose_t mrpt_node;
276 
277  // set the nodeID/pose
278  convert(nodes_cit->pose, mrpt_node);
279 
280  // set the MR-SLAM fields
281  mrpt_node.agent_ID_str = nodes_cit->str_ID.data;
282  mrpt_node.nodeID_loc = nodes_cit->nodeID_loc;
283 
284  mrpt_graph.nodes.insert(
285  make_pair(static_cast<TNodeID>(nodes_cit->nodeID), mrpt_node));
286  }
287 
288  // fill the constraints
289  for (constraints_cit_t constr_cit = ros_graph.constraints.begin();
290  constr_cit != ros_graph.constraints.end(); ++constr_cit)
291  {
292  // constraint ends
293  auto constr_ends(make_pair(
294  static_cast<TNodeID>(constr_cit->nodeID_from),
295  static_cast<TNodeID>(constr_cit->nodeID_to)));
296 
297  // constraint value
298  mrpt::poses::CPosePDFGaussianInf mrpt_constr;
299  convert(constr_cit->constraint, mrpt_constr);
300 
301  mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr));
302  }
303 
304  mrpt_graph.edges_store_inverse_poses = false;
305 
306  MRPT_END;
307 }
308 
309 void convert(
310  mrpt_msgs::NetworkOfPoses& ros_graph,
311  const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph)
312 {
313  THROW_EXCEPTION("Conversion not implemented yet");
314 }
315 
316 } // namespace mrpt_bridge
CDirectedGraph< CPOSE, EDGE_ANNOTATIONS >::edges_map_t
mrpt::aligned_containers< TPairNodeIDs, edge_t >::multimap_t edges_map_t
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
mrpt_msgs
Definition: include/mrpt_bridge/beacon.h:28
network_of_poses.h
mrpt_bridge::convert
void convert(mrpt_msgs::NetworkOfPoses &ros_graph, const mrpt::graphs::CNetworkOfPoses3DInf_NA &mrpt_graph)
Definition: network_of_poses.cpp:309
MRPT_TODO
MRPT_TODO("Modify ping to run on Windows + Test this")
MRPT_END
#define MRPT_END
mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types...
Definition: include/mrpt_bridge/beacon.h:52
mrpt::poses::CPosePDFGaussianInf::inverse
void inverse(CPosePDF &o) const MRPT_OVERRIDE
TNodeID
uint64_t TNodeID
CDirectedGraph< CPOSE, EDGE_ANNOTATIONS >::const_iterator
edges_map_t::const_iterator const_iterator
MRPT_START
#define MRPT_START
geometry_msgs
mrpt::graphs::CNetworkOfPoses::global_pose_t
mrpt::graphs::CNetworkOfPoses::root
mrpt::utils::TNodeID root

mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Feb 28 2020 03:22:14