convert from geodesic to map coords

seerep_srv/seerep_core/src/core_project.cpp

@@ -95,7 +95,36 @@ seerep_core_msgs::QueryResultProject CoreProject::getDataset(seerep_core_msgs::Q
   // is the query not in map frame?
   if (!query.inMapFrame)
   {
+    seerep_core_msgs::GeodeticCoordinates g = m_geodeticCoordinates.value();
+
+    // https://proj.org/en/9.2/operations/conversions/topocentric.html
+    // the proj pipeline has two steps, convert geodesic to cartesian coordinates
+    // then project to topocentric coordinates
+    std::string proj_pipeline = "step +proj=cartesian +ellps=" + g.coordinateSystem +
+                                " \nstep +proj=topocentric +ellps" + g.coordinateSystem +
+                                "+lon_0=" + std::to_string(g.longitude) + "lat_0=" + std::to_string(g.latitude) +
+                                "h_0=" + std::to_string(g.altitude);
+
+    // PJ_FWD: forward, PJ_IDENT: identity, PJ_INV: inverse
+    PJ_DIRECTION direction = PJ_FWD;
+
+    seerep_core_msgs::Polygon2D transformed_polygon;
+
     // perform an affine transform to transpose the query polygon to map frame
+    // the first argument of 0 is the thread context, the second is the pipeline string created above
+    auto to_topographic = proj_create(0, proj_pipeline.c_str());
+
+    // we traverse the polygon and apply the transform
+    for (seerep_core_msgs::Point2D p : query.polygon.value().vertices)
+    {
+      PJ_COORD c = proj_coord(p.get<0>(), p.get<1>(), NULL, NULL);
+      PJ_COORD t_coord = proj_trans(to_topographic, direction, c);
+
+      seerep_core_msgs::Point2D transformed_p;
+      p.set<0>(t_coord.xy.x);
+      p.set<1>(t_coord.xy.y);
+      transformed_polygon.vertices.push_back(transformed_p);
+    }
   }
 
   result.dataOrInstanceUuids = m_coreDatasets->getData(m_coreTfs->transformQuery(query, m_frameId));