11 #include <mrpt/ros1bridge/pose.h> 12 #include <geometry_msgs/Pose.h> 13 #include <geometry_msgs/PoseWithCovariance.h> 14 #include <mrpt_msgs/NodeIDWithPose.h> 16 #include <mrpt/graphs/TNodeID.h> 20 using mrpt::graphs::TNodeID;
27 const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph,
28 mrpt_msgs::NetworkOfPoses& ros_graph)
32 using namespace mrpt::math;
33 using namespace mrpt::graphs;
37 typedef typename mrpt::graphs::CNetworkOfPoses2DInf::global_poses_t::
38 const_iterator poses_cit_t;
40 const CNetworkOfPoses2DInf::BASE::edges_map_t& constraints =
41 mrpt_graph.BASE::edges;
44 ros_graph.root = mrpt_graph.root;
47 for (poses_cit_t poses_cit = mrpt_graph.nodes.begin();
48 poses_cit != mrpt_graph.nodes.end(); ++poses_cit)
50 mrpt_msgs::NodeIDWithPose ros_node;
53 ros_node.node_id = poses_cit->first;
55 ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second);
58 ros_node.str_id.data =
"";
59 ros_node.node_id_loc = 0;
61 ros_graph.nodes.vec.push_back(ros_node);
65 for (
const auto& edge : constraints)
67 mrpt_msgs::GraphConstraint ros_constr;
70 ros_constr.node_id_from = edge.first.first;
71 ros_constr.node_id_to = edge.first.second;
74 if (mrpt_graph.edges_store_inverse_poses)
76 CPosePDFGaussianInf constr_inv;
77 edge.second.inverse(constr_inv);
78 ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv);
82 ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second);
85 ros_graph.constraints.push_back(ros_constr);
92 [[maybe_unused]]
const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph,
93 [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph)
95 THROW_EXCEPTION(
"Conversion not implemented yet");
101 const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph,
102 mrpt_msgs::NetworkOfPoses& ros_graph)
107 using namespace mrpt::math;
108 using namespace mrpt::graphs;
112 typedef typename mrpt::graphs::CNetworkOfPoses2DInf_NA::global_poses_t::
113 const_iterator poses_cit_t;
122 const CNetworkOfPoses2DInf_NA::BASE::edges_map_t& constraints =
123 mrpt_graph.BASE::edges;
126 ros_graph.root = mrpt_graph.root;
129 for (poses_cit_t poses_cit = mrpt_graph.nodes.begin();
130 poses_cit != mrpt_graph.nodes.end(); ++poses_cit)
132 mrpt_msgs::NodeIDWithPose ros_node;
135 ros_node.node_id = poses_cit->first;
137 ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second);
140 ros_node.str_id.data = poses_cit->second.agent_ID_str;
141 ros_node.node_id_loc = poses_cit->second.nodeID_loc;
143 ros_graph.nodes.vec.push_back(ros_node);
148 for (
const auto& edge : constraints)
150 mrpt_msgs::GraphConstraint ros_constr;
153 ros_constr.node_id_from = edge.first.first;
154 ros_constr.node_id_to = edge.first.second;
157 if (mrpt_graph.edges_store_inverse_poses)
159 CPosePDFGaussianInf constr_inv;
160 edge.second.inverse(constr_inv);
161 ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv);
165 ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second);
168 ros_graph.constraints.push_back(ros_constr);
175 [[maybe_unused]]
const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph,
176 [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph)
178 THROW_EXCEPTION(
"Conversion not implemented yet");
186 const mrpt_msgs::NetworkOfPoses& ros_graph,
187 mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph)
194 typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t;
195 typedef NetworkOfPoses::_constraints_type::const_iterator constraints_cit_t;
198 mrpt_graph.root = ros_graph.root;
201 for (nodes_cit_t nodes_cit = ros_graph.nodes.vec.begin();
202 nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit)
205 CPose2D mrpt_pose = CPose2D(mrpt::ros1bridge::fromROS(nodes_cit->pose));
207 mrpt_graph.nodes.insert(
208 make_pair(static_cast<TNodeID>(nodes_cit->node_id), mrpt_pose));
212 for (constraints_cit_t constr_cit = ros_graph.constraints.begin();
213 constr_cit != ros_graph.constraints.end(); ++constr_cit)
216 auto constr_ends(make_pair(
217 static_cast<TNodeID>(constr_cit->node_id_from),
218 static_cast<TNodeID>(constr_cit->node_id_to)));
221 const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf(
222 mrpt::ros1bridge::fromROS(constr_cit->constraint));
224 mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr));
227 mrpt_graph.edges_store_inverse_poses =
false;
233 [[maybe_unused]]
const mrpt_msgs::NetworkOfPoses& ros_graph,
234 [[maybe_unused]] mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph)
236 THROW_EXCEPTION(
"Conversion not implemented yet");
242 const mrpt_msgs::NetworkOfPoses& ros_graph,
243 mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph)
250 typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t;
251 typedef NetworkOfPoses::_constraints_type::const_iterator constraints_cit_t;
253 typedef mrpt::graphs::CNetworkOfPoses2DInf_NA::global_pose_t
257 mrpt_graph.root = ros_graph.root;
260 for (nodes_cit_t nodes_cit = ros_graph.nodes.vec.begin();
261 nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit)
264 mrpt_graph_pose_t mrpt_node =
265 mrpt::ros1bridge::fromROS(nodes_cit->pose);
268 mrpt_node.agent_ID_str = nodes_cit->str_id.data;
269 mrpt_node.nodeID_loc = nodes_cit->node_id_loc;
271 mrpt_graph.nodes.insert(
272 make_pair(static_cast<TNodeID>(nodes_cit->node_id), mrpt_node));
276 for (constraints_cit_t constr_cit = ros_graph.constraints.begin();
277 constr_cit != ros_graph.constraints.end(); ++constr_cit)
280 auto constr_ends(make_pair(
281 static_cast<TNodeID>(constr_cit->node_id_from),
282 static_cast<TNodeID>(constr_cit->node_id_to)));
285 const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf(
286 mrpt::ros1bridge::fromROS(constr_cit->constraint));
288 mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr));
291 mrpt_graph.edges_store_inverse_poses =
false;
297 [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph,
298 [[maybe_unused]]
const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph)
300 THROW_EXCEPTION(
"Conversion not implemented yet");
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)