From 36ed37febe916410a293d143c26c65fd1b869e99 Mon Sep 17 00:00:00 2001
From: dehann <dehann.fourie@gmail.com>
Date: Fri, 11 Oct 2019 16:58:37 -0400
Subject: [PATCH] initial ROS poc wip

---
 Project.toml                   |   2 +-
 examples/ros/ServiceExample.jl |  71 ++++++++++++++++++++
 examples/ros/cpp/example.cpp   | 114 +++++++++++++++++++++++++++++++++
 3 files changed, 186 insertions(+), 1 deletion(-)
 create mode 100644 examples/ros/ServiceExample.jl
 create mode 100644 examples/ros/cpp/example.cpp

diff --git a/Project.toml b/Project.toml
index 956ad799c..42a9fc884 100644
--- a/Project.toml
+++ b/Project.toml
@@ -49,7 +49,7 @@ CloudGraphs = "≥ 0.1.1"
 Combinatorics = "≥ 0.7"
 CoordinateTransformations = "≥ 0.5"
 DataStructures = "≥ 0.15"
-DistributedFactorGraphs = "0.2.2, 0.3"
+DistributedFactorGraphs = "0.3, 0.4"
 Distributions = "≥ 0.18"
 DocStringExtensions = "≥ 0.7"
 FFTW = "≥ 0.2.4"
diff --git a/examples/ros/ServiceExample.jl b/examples/ros/ServiceExample.jl
new file mode 100644
index 000000000..b4091b27d
--- /dev/null
+++ b/examples/ros/ServiceExample.jl
@@ -0,0 +1,71 @@
+# example of ros service in julia
+
+# $ export PYTHON=/usr/bin/python2.7
+@assert ENV["PYTHON"]=="/usr/bin/python2.7"
+# ]build PyCall
+
+using RobotOS
+
+# standard types
+@rosimport geometry_msgs.msg: Point, Pose2D
+# bespoke types
+@rosimport caesar_ros.srv: AddNode, AddFactor
+
+# generate the types and load
+rostypegen()
+using .geometry_msgs.msg
+using .caesar_ros.srv
+# service callbacks
+function add_node(req::AddNodeRequest)
+
+    reply = AddNodeResponse()
+    return reply
+end
+
+function add_factor(req::AddFactorRequest)
+
+    reply = AddFactorResponse()
+    return reply
+end
+
+function callback(msg::Pose2D, pub_obj::Publisher{Point})
+    pt_msg = Point(msg.x, msg.y, 0.0)
+    publish(pub_obj, pt_msg)
+end
+
+function loop(pub_obj)
+    loop_rate = Rate(5.0)
+    while ! is_shutdown()
+        npt = Point(rand(), rand(), 0.0)
+        publish(pub_obj, npt)
+        rossleep(loop_rate)
+    end
+end
+
+function main()
+    init_node("rosjl_example")
+    pub = Publisher{Point}("pts", queue_size=10)
+    sub = Subscriber{Pose2D}("pose", callback, (pub,), queue_size=10)
+    loop(pub)
+
+    add_node_srv = Service('AddNode',caesar_ros.srv.AddNode, addnode)
+
+end
+
+if ! isinteractive()
+    main()
+end
+
+
+
+
+
+
+
+
+
+
+
+
+
+#
diff --git a/examples/ros/cpp/example.cpp b/examples/ros/cpp/example.cpp
new file mode 100644
index 000000000..6b6e0de44
--- /dev/null
+++ b/examples/ros/cpp/example.cpp
@@ -0,0 +1,114 @@
+/*
+ * \file main.cpp
+ * \author Pedro Vaz Teixeira
+ * \brief Caesar service server
+ */
+
+#include <map>
+#include <mutex>
+
+#include <ros/ros.h>
+
+#include <caesar_ros/AddFactor.h>
+#include <caesar_ros/AddNode.h>
+#include <caesar_ros/GetEstimate.h>
+
+#include <graff/graff.hpp>
+
+namespace {
+class Server {
+  // private:
+  Server(){}; // can't touch this
+public:
+  Server(const std::string &address, const std::string &session)
+      : session_(session) {
+
+    // setup ROS side: advertise services
+    add_node_srv_ =
+        node_handle_.advertiseService("AddNode", &Server::addNode, this);
+    add_factor_srv_ =
+        node_handle_.advertiseService("AddFactor", &Server::addFactor, this);
+    get_estimate_srv_ = node_handle_.advertiseService(
+        "GetEstimate", &Server::getEstimate, this);
+
+    // setup Caesar side
+    endpoint_.Connect(address);
+
+    // TODO: register robot, session
+  };
+
+
+  // addFactor.srv
+  // REQUEST:
+  // string id0
+  // string id1
+  // geometry_msgs/PoseWithCovariance measurement
+  // ---
+  // REPLY:
+  // string status
+  bool addFactor(caesar_ros::AddFactor::Request &req,
+                 caesar_ros::AddFactor::Response &res) {
+    geometry_msgs::Point zp = req.measurement.pose.position;
+    geometry_msgs::Quaternion zo = req.measurement.pose.orientation;
+    std::vector<double> mu = {zp.x, zp.y, zp.z, zo.x, zo.y, zo.z, zo.w};
+    std::vector<double> cov(req.measurement.covariance.begin(),
+                            req.measurement.covariance.end());
+    graff::Normal *z = new graff::Normal(mu, cov);
+
+    std::vector<std::string> nodes{req.id0, req.id1};
+    graff::Factor factor("Pose3Pose3", nodes);
+    factor.push_back(z);
+    res.status = graff::AddFactor(endpoint_, session_, factor);
+    return (true);
+  }
+
+
+  bool addNode(caesar_ros::AddNode::Request &req,
+               caesar_ros::AddNode::Response &res) {
+    graff::Variable pose(req.id, "Pose3");
+    res.status = graff::AddVariable(endpoint_, session_, pose);
+    return (true);
+  }
+
+
+  bool getEstimate(caesar_ros::GetEstimate::Request &req,
+                   caesar_ros::GetEstimate::Response &res) {
+    // retrieve and send estimate from latest solution
+    auto rep = graff::GetVarMAPMean(endpoint_, session_, req.query);
+    // TODO: some parsing to fill out response
+    // (geometry_msgs::PoseWithCovariance)
+    return (true);
+  }
+
+  void solver(void) {
+    ros::Rate rate(1);
+    while (ros::ok()) {
+      // TODO fetch the latest estimates from the endpoint and cache them
+      // locally
+      rate.sleep();
+    }
+  }
+
+private:
+  // ros
+  ros::NodeHandle node_handle_;
+  ros::ServiceServer add_factor_srv_, add_node_srv_, get_estimate_srv_;
+
+  // caesar
+  graff::Endpoint endpoint_;
+  graff::Session session_;
+}; // class Server
+}; // namespace
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "caesar_server");
+
+  std::string ep_address("tcp://127.0.0.1:5555"), ep_session("ros-test");
+  Server caesar_server(ep_address, ep_session );
+
+  ros::Rate rate(10);
+  while (ros::ok()) {
+    ros::spinOnce();
+    rate.sleep();
+  }
+}