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 
11 #include <mrpt/ros1bridge/pose.h>
12 #include <geometry_msgs/Pose.h>
13 #include <geometry_msgs/PoseWithCovariance.h>
14 #include <mrpt_msgs/NodeIDWithPose.h>
15 #include <iostream> // for debugging reasons
16 #include <mrpt/graphs/TNodeID.h>
17 
18 namespace mrpt_bridge
19 {
20 using mrpt::graphs::TNodeID;
21 
23 // MRPT => ROS
25 
26 void convert(
27  const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph,
28  mrpt_msgs::NetworkOfPoses& ros_graph)
29 {
30  MRPT_START
31  using namespace geometry_msgs;
32  using namespace mrpt::math;
33  using namespace mrpt::graphs;
34  using namespace mrpt::poses;
35  using namespace std;
36 
37  typedef typename mrpt::graphs::CNetworkOfPoses2DInf::global_poses_t::
38  const_iterator poses_cit_t;
39 
40  const CNetworkOfPoses2DInf::BASE::edges_map_t& constraints =
41  mrpt_graph.BASE::edges;
42 
43  // fill root node
44  ros_graph.root = mrpt_graph.root;
45 
46  // fill nodeIDs, positions
47  for (poses_cit_t poses_cit = mrpt_graph.nodes.begin();
48  poses_cit != mrpt_graph.nodes.end(); ++poses_cit)
49  {
50  mrpt_msgs::NodeIDWithPose ros_node;
51 
52  // nodeID
53  ros_node.node_id = poses_cit->first;
54  // pose
55  ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second);
56 
57  // zero the optional fields
58  ros_node.str_id.data = "";
59  ros_node.node_id_loc = 0;
60 
61  ros_graph.nodes.vec.push_back(ros_node);
62  }
63 
64  // fill the constraints
65  for (const auto& edge : constraints)
66  {
67  mrpt_msgs::GraphConstraint ros_constr;
68 
69  // constraint ends
70  ros_constr.node_id_from = edge.first.first;
71  ros_constr.node_id_to = edge.first.second;
72 
73  // constraint mean and covariance
74  if (mrpt_graph.edges_store_inverse_poses)
75  {
76  CPosePDFGaussianInf constr_inv;
77  edge.second.inverse(constr_inv);
78  ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv);
79  }
80  else
81  {
82  ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second);
83  }
84 
85  ros_graph.constraints.push_back(ros_constr);
86  }
87 
88  MRPT_END
89 }
90 
91 void convert(
92  [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph,
93  [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph)
94 {
95  THROW_EXCEPTION("Conversion not implemented yet");
96  // TODO: Implement CNetworkOfPoses3DInf => mrpt_msgs::NetworkOfPoses
97  // conversion
98 }
99 
100 void convert(
101  const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph,
102  mrpt_msgs::NetworkOfPoses& ros_graph)
103 {
104  MRPT_START
105 
106  using namespace geometry_msgs;
107  using namespace mrpt::math;
108  using namespace mrpt::graphs;
109  using namespace mrpt::poses;
110  using namespace std;
111 
112  typedef typename mrpt::graphs::CNetworkOfPoses2DInf_NA::global_poses_t::
113  const_iterator poses_cit_t;
114 
116  // for (poses_cit_t it = mrpt_graph.nodes.begin();
117  // it != mrpt_graph.nodes.end();
118  //++it) {
119  // cout << it->first << " | " << it->second << endl;
120  //}
121 
122  const CNetworkOfPoses2DInf_NA::BASE::edges_map_t& constraints =
123  mrpt_graph.BASE::edges;
124 
125  // fill root node
126  ros_graph.root = mrpt_graph.root;
127 
128  // fill nodeIDs, positions
129  for (poses_cit_t poses_cit = mrpt_graph.nodes.begin();
130  poses_cit != mrpt_graph.nodes.end(); ++poses_cit)
131  {
132  mrpt_msgs::NodeIDWithPose ros_node;
133 
134  // nodeID
135  ros_node.node_id = poses_cit->first;
136  // pose
137  ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second);
138 
139  // optional fields for the MR-SLAM case
140  ros_node.str_id.data = poses_cit->second.agent_ID_str;
141  ros_node.node_id_loc = poses_cit->second.nodeID_loc;
142 
143  ros_graph.nodes.vec.push_back(ros_node);
144  }
145 
146  // fill the constraints -- same as in the conversion from
147  // CNetworkOfPoses2DInf
148  for (const auto& edge : constraints)
149  {
150  mrpt_msgs::GraphConstraint ros_constr;
151 
152  // constraint ends
153  ros_constr.node_id_from = edge.first.first;
154  ros_constr.node_id_to = edge.first.second;
155 
156  // constraint mean and covariance
157  if (mrpt_graph.edges_store_inverse_poses)
158  {
159  CPosePDFGaussianInf constr_inv;
160  edge.second.inverse(constr_inv);
161  ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv);
162  }
163  else
164  {
165  ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second);
166  }
167 
168  ros_graph.constraints.push_back(ros_constr);
169  }
170 
171  MRPT_END
172 }
173 
174 void convert(
175  [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph,
176  [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph)
177 {
178  THROW_EXCEPTION("Conversion not implemented yet");
179 }
180 
182 // ROS => MRPT
184 
185 void convert(
186  const mrpt_msgs::NetworkOfPoses& ros_graph,
187  mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph)
188 {
189  MRPT_START
190  using namespace mrpt::poses;
191  using namespace mrpt_msgs;
192  using namespace std;
193 
194  typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t;
195  typedef NetworkOfPoses::_constraints_type::const_iterator constraints_cit_t;
196 
197  // fill root node
198  mrpt_graph.root = ros_graph.root;
199 
200  // fill nodeIDs, positions
201  for (nodes_cit_t nodes_cit = ros_graph.nodes.vec.begin();
202  nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit)
203  {
204  // get the pose
205  CPose2D mrpt_pose = CPose2D(mrpt::ros1bridge::fromROS(nodes_cit->pose));
206 
207  mrpt_graph.nodes.insert(
208  make_pair(static_cast<TNodeID>(nodes_cit->node_id), mrpt_pose));
209  }
210 
211  // fill the constraints
212  for (constraints_cit_t constr_cit = ros_graph.constraints.begin();
213  constr_cit != ros_graph.constraints.end(); ++constr_cit)
214  {
215  // constraint ends
216  auto constr_ends(make_pair(
217  static_cast<TNodeID>(constr_cit->node_id_from),
218  static_cast<TNodeID>(constr_cit->node_id_to)));
219 
220  // constraint value
221  const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf(
222  mrpt::ros1bridge::fromROS(constr_cit->constraint));
223 
224  mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr));
225  }
226 
227  mrpt_graph.edges_store_inverse_poses = false;
228 
229  MRPT_END
230 }
231 
232 void convert(
233  [[maybe_unused]] const mrpt_msgs::NetworkOfPoses& ros_graph,
234  [[maybe_unused]] mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph)
235 {
236  THROW_EXCEPTION("Conversion not implemented yet");
237  // TODO: Implement mrpt_msgs::NetworkOfPoses => CNetworkOfPoses3DInf
238  // conversion.
239 }
240 
241 void convert(
242  const mrpt_msgs::NetworkOfPoses& ros_graph,
243  mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph)
244 {
245  MRPT_START
246  using namespace mrpt::poses;
247  using namespace mrpt_msgs;
248  using namespace std;
249 
250  typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t;
251  typedef NetworkOfPoses::_constraints_type::const_iterator constraints_cit_t;
252 
253  typedef mrpt::graphs::CNetworkOfPoses2DInf_NA::global_pose_t
254  mrpt_graph_pose_t;
255 
256  // fill root node
257  mrpt_graph.root = ros_graph.root;
258 
259  // fill nodeIDs, positions
260  for (nodes_cit_t nodes_cit = ros_graph.nodes.vec.begin();
261  nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit)
262  {
263  // set the nodeID/pose
264  mrpt_graph_pose_t mrpt_node =
265  mrpt::ros1bridge::fromROS(nodes_cit->pose);
266 
267  // set the MR-SLAM fields
268  mrpt_node.agent_ID_str = nodes_cit->str_id.data;
269  mrpt_node.nodeID_loc = nodes_cit->node_id_loc;
270 
271  mrpt_graph.nodes.insert(
272  make_pair(static_cast<TNodeID>(nodes_cit->node_id), mrpt_node));
273  }
274 
275  // fill the constraints
276  for (constraints_cit_t constr_cit = ros_graph.constraints.begin();
277  constr_cit != ros_graph.constraints.end(); ++constr_cit)
278  {
279  // constraint ends
280  auto constr_ends(make_pair(
281  static_cast<TNodeID>(constr_cit->node_id_from),
282  static_cast<TNodeID>(constr_cit->node_id_to)));
283 
284  // constraint value
285  const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf(
286  mrpt::ros1bridge::fromROS(constr_cit->constraint));
287 
288  mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr));
289  }
290 
291  mrpt_graph.edges_store_inverse_poses = false;
292 
293  MRPT_END
294 }
295 
296 void convert(
297  [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph,
298  [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph)
299 {
300  THROW_EXCEPTION("Conversion not implemented yet");
301 }
302 
303 } // namespace mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types...
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Thu May 12 2022 02:26:47