Skip to content

Commit ea4d790

Browse files
pbustospbustos
committed
added anoptional parameter to the inner_eigen subapi to interpolate timestamped queries
1 parent 5630877 commit ea4d790

11 files changed

Lines changed: 395 additions & 103 deletions

File tree

‎README.md‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
* [Tutorials to start creating agents and using existing configurations](#tutorials-to-start-creating-agents-and-using-existing-configurations)
1313
* [Installing existing agents from the RoboComp repository](#installing-existing-agents-from-the-robocomp-repository)
1414
- [Developer Documentation](#developer-documentation)
15+
* [Combined C++/Python API Guide](API_CPP_PYTHON_GUIDE.md)
1516
* [DSR-API (C++)](#dsr-api--c---)
1617
* [Common examples](#common-examples)
1718
* [Predefined names and types](#predefined-names-and-types)
@@ -198,6 +199,9 @@ If you want to install and try some existing agents, you can clone the [dsr-grap
198199

199200

200201
# Developer Documentation
202+
203+
[Combined C++/Python API Guide](API_CPP_PYTHON_GUIDE.md)
204+
201205
## DSR-API (C++)
202206
G-API is the user-level access layer to G. It comprises a set of core methods that access the underlying CRDT and RTPS APIs, and an extendable set of auxiliary methods added to simplify the user coding tasks.
203207

@@ -663,6 +667,8 @@ These structures are compiled into C++ code that is included in the agent, formi
663667
664668
# Python API
665669
670+
[Combined C++/Python API Guide](API_CPP_PYTHON_GUIDE.md)
671+
666672
[python-API](python_api_documentation.md)
667673
668674

‎api/dsr_inner_eigen_api.cpp‎

Lines changed: 27 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ InnerEigenAPI::InnerEigenAPI(DSR::DSRGraph *G_)
1010
rt = G->get_rt_api();
1111
//update signals
1212
connect(G, &DSR::DSRGraph::update_edge_signal, this, &InnerEigenAPI::add_or_assign_edge_slot, Qt::QueuedConnection);
13+
connect(G, &DSR::DSRGraph::update_edge_attr_signal, this, &InnerEigenAPI::add_or_assign_edge_attr_slot, Qt::QueuedConnection);
1314
connect(G, &DSR::DSRGraph::del_edge_signal, this, &InnerEigenAPI::del_edge_slot, Qt::QueuedConnection);
1415
connect(G, &DSR::DSRGraph::del_node_signal, this, &InnerEigenAPI::del_node_slot, Qt::QueuedConnection);
1516
}
@@ -18,7 +19,7 @@ InnerEigenAPI::InnerEigenAPI(DSR::DSRGraph *G_)
1819
////// TRANSFORMATION MATRIX
1920
////////////////////////////////////////////////////////////////////////////////////////
2021

21-
std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
22+
std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
2223
{
2324
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
2425
return {};
@@ -57,7 +58,7 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
5758
qWarning() << __FUNCTION__ << ":"<<__LINE__<< " Cannot find " << QString::fromStdString(edge_type) << " edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
5859
return {};
5960
}
60-
if( auto rtmat = rt->get_edge_RT_as_rtmat(edge_rt.value(), timestamp); rtmat.has_value())
61+
if( auto rtmat = rt->get_edge_RT_as_rtmat(edge_rt.value(), timestamp, time_query); rtmat.has_value())
6162
{
6263
atotal = rtmat.value() * atotal;
6364
if(use_cache)
@@ -78,7 +79,7 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
7879
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find " << QString::fromStdString(edge_type) << " edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
7980
return {};
8081
}
81-
if( auto rtmat = rt->get_edge_RT_as_rtmat(edge_rt.value(), timestamp); rtmat.has_value())
82+
if( auto rtmat = rt->get_edge_RT_as_rtmat(edge_rt.value(), timestamp, time_query); rtmat.has_value())
8283
{
8384
btotal = rtmat.value() * btotal;
8485
if(use_cache)
@@ -108,8 +109,8 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
108109
qWarning() << __FUNCTION__ << ":"<<__LINE__ << " Cannot find " << QString::fromStdString(edge_type) << " edge between Parent (" << QString::fromStdString(p_node->name()) << ", " << p_node->id() <<") and son (" << QString::fromStdString(a.name()) << ", " << a.id() <<") nodes going from: " << QString::fromStdString(orig) << " to: " << QString::fromStdString(dest);
109110
return {};
110111
}
111-
auto a_rtmat = rt->get_edge_RT_as_rtmat(a_edge_rt.value(), timestamp);
112-
auto b_rtmat = rt->get_edge_RT_as_rtmat(b_edge_rt.value(), timestamp);
112+
auto a_rtmat = rt->get_edge_RT_as_rtmat(a_edge_rt.value(), timestamp, time_query);
113+
auto b_rtmat = rt->get_edge_RT_as_rtmat(b_edge_rt.value(), timestamp, time_query);
113114
if(a_rtmat.has_value() and b_rtmat.has_value())
114115
{
115116
atotal = a_rtmat.value() * atotal;
@@ -144,11 +145,11 @@ std::optional<Mat::RTMat> InnerEigenAPI::get_transformation_matrix(const std::st
144145
}
145146
}
146147

147-
std::optional<Mat::Rot3D> InnerEigenAPI::get_rotation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
148+
std::optional<Mat::Rot3D> InnerEigenAPI::get_rotation_matrix(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
148149
{
149150
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
150151
return {};
151-
if( auto r = get_transformation_matrix(dest, orig, timestamp, edge_type); r.has_value())
152+
if( auto r = get_transformation_matrix(dest, orig, timestamp, edge_type, time_query); r.has_value())
152153
return r.value().rotation();
153154
else
154155
{
@@ -157,11 +158,11 @@ std::optional<Mat::Rot3D> InnerEigenAPI::get_rotation_matrix(const std::string &
157158
}
158159
}
159160

160-
std::optional<Mat::Vector3d> InnerEigenAPI::get_translation_vector(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
161+
std::optional<Mat::Vector3d> InnerEigenAPI::get_translation_vector(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
161162
{
162163
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
163164
return {};
164-
if (auto r = get_transformation_matrix(dest, orig, timestamp, edge_type); r.has_value())
165+
if (auto r = get_transformation_matrix(dest, orig, timestamp, edge_type, time_query); r.has_value())
165166
return r.value().translation();
166167
else
167168
{
@@ -170,11 +171,11 @@ std::optional<Mat::Vector3d> InnerEigenAPI::get_translation_vector(const std::st
170171
return {};
171172
}
172173
}
173-
std::optional<Mat::Vector3d> InnerEigenAPI::get_euler_xyz_angles(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
174+
std::optional<Mat::Vector3d> InnerEigenAPI::get_euler_xyz_angles(const std::string &dest, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
174175
{
175176
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
176177
return {};
177-
if( auto r = get_transformation_matrix(dest, orig, timestamp, edge_type); r.has_value())
178+
if( auto r = get_transformation_matrix(dest, orig, timestamp, edge_type, time_query); r.has_value())
178179
return r.value().rotation().eulerAngles(0,1,2); //X Y Z order
179180
else
180181
{
@@ -186,12 +187,12 @@ std::optional<Mat::Vector3d> InnerEigenAPI::get_euler_xyz_angles(const std::stri
186187
////////////////////////////////////////////////////////////////////////////////////////
187188
////// TRANSFORM
188189
////////////////////////////////////////////////////////////////////////////////////////
189-
std::optional<Mat::Vector3d> InnerEigenAPI::transform(const std::string &dest, const Mat::Vector3d &vector, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
190+
std::optional<Mat::Vector3d> InnerEigenAPI::transform(const std::string &dest, const Mat::Vector3d &vector, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
190191
{
191192
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
192193
return {};
193194
//std::cout <<__FUNCTION__ << " " << initVec << std::endl;
194-
auto tm = get_transformation_matrix(dest, orig, timestamp, edge_type);
195+
auto tm = get_transformation_matrix(dest, orig, timestamp, edge_type, time_query);
195196
//std::cout << __FUNCTION__ << " " << tm.value().matrix().format(CleanFmt) << std::endl;
196197
if(tm.has_value())
197198
{
@@ -202,18 +203,18 @@ std::optional<Mat::Vector3d> InnerEigenAPI::transform(const std::string &dest, c
202203
return {};
203204
}
204205

205-
std::optional<Mat::Vector3d> InnerEigenAPI::transform( const std::string &dest, const std::string & orig, std::uint64_t timestamp, const std::string &edge_type)
206+
std::optional<Mat::Vector3d> InnerEigenAPI::transform( const std::string &dest, const std::string & orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
206207
{
207208
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
208209
return {};
209-
return transform(dest, Mat::Vector3d(0.,0.,0.), orig, timestamp, edge_type);
210+
return transform(dest, Mat::Vector3d(0.,0.,0.), orig, timestamp, edge_type, time_query);
210211
}
211212

212-
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis(const std::string &dest, const Mat::Vector6d &vector, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type)
213+
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis(const std::string &dest, const Mat::Vector6d &vector, const std::string &orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
213214
{
214215
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
215216
return {};
216-
auto tm = get_transformation_matrix(dest, orig, timestamp, edge_type);
217+
auto tm = get_transformation_matrix(dest, orig, timestamp, edge_type, time_query);
217218
if(tm.has_value())
218219
{
219220
const Mat::RTMat rtmat = tm.value();
@@ -231,12 +232,12 @@ std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis(const std::string &de
231232
return {};
232233
}
233234

234-
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis( const std::string &dest, const std::string & orig, std::uint64_t timestamp, const std::string &edge_type)
235+
std::optional<Mat::Vector6d> InnerEigenAPI::transform_axis( const std::string &dest, const std::string & orig, std::uint64_t timestamp, const std::string &edge_type, RT_API::TimeQuery time_query)
235236
{
236237
if( not DSR::DSRGraph::is_valid_edge_type(edge_type))
237238
return {};
238239
Mat::Vector6d v;
239-
return transform_axis(dest, Mat::Vector6d::Zero(), orig, timestamp, edge_type);
240+
return transform_axis(dest, Mat::Vector6d::Zero(), orig, timestamp, edge_type, time_query);
240241
}
241242

242243
////////////////////////////////////////////////////////////////////////
@@ -250,6 +251,13 @@ void InnerEigenAPI::add_or_assign_edge_slot(uint64_t from, uint64_t to, const st
250251
remove_cache_entry(to);
251252
}
252253
}
254+
void InnerEigenAPI::add_or_assign_edge_attr_slot(uint64_t from,
255+
uint64_t to,
256+
const std::string& edge_type,
257+
const std::vector<std::string>& /*att_names*/)
258+
{
259+
add_or_assign_edge_slot(from, to, edge_type);
260+
}
253261
void InnerEigenAPI::del_node_slot(uint64_t id)
254262
{
255263
remove_cache_entry(id);

0 commit comments

Comments
 (0)