Added Matrix of Nodes
Change-Id: Ia7044f03713c85cbbd059e71a2324c30511fe469
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 2df9a33..fbdcfbb 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -115,6 +115,18 @@
(-point[3], -point[4], -point[5]))
cc_file.append(" }));")
cc_file.append("}")
+
+ # Matrix of nodes
+ h_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList();")
+
+ cc_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
+ cc_file.append(" ::std::vector<::Eigen::Matrix<double, 2, 1>> points;")
+ for index, point in enumerate(graph_generate.points):
+ cc_file.append(
+ " points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());" % (
+ numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+ cc_file.append(" return points;")
+ cc_file.append("}")
# Now create the MakeSearchGraph function.
h_file.append("")