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

mrpt_msgs_bridge
Author(s): José Luis Blanco-Claraco
autogenerated on Thu Jun 1 2023 03:07:04