first fully-working set_pose from python

Former-commit-id: fc8bf97f39c727755baabff803525dd275ab8741 [formerly afed3c5]
Former-commit-id: 23c9d78e6fe9f6619c8cdaa5aa1b3772c2140bd7

modules/comms/include/mvsim/Comms/Client.h

@@ -49,18 +49,13 @@ class Client : public mrpt::system::COutputLogger
    public:
 	Client();
 	Client(const std::string& nodeName);
-	// Overload for python wrapper
-	Client(const char* nodeName) : Client(std::string(nodeName)) {}
-
+
 	~Client();
 
 	/** @name Main mvsim client communication API
 	 * @{ */
 	void setName(const std::string& nodeName);
 
-	// Overload for python wrapper
-	void setName(const char* nodeName) { setName(std::string(nodeName)); }
-
 	/** Connects to the server in a parallel thread. */
 	void connect();
 
@@ -93,6 +88,10 @@ class Client : public mrpt::system::COutputLogger
 		const std::string& serviceName, const INPUT_MSG_T& input,
 		OUTPUT_MSG_T& output);
 
+	// Overload for python wrapper
+	std::string callService(
+		const std::string& serviceName, const std::string& inputSerializedMsg);
+
 	struct InfoPerNode
 	{
 		std::string name;
@@ -146,8 +145,10 @@ class Client : public mrpt::system::COutputLogger
 		const google::protobuf::Descriptor* descriptor,
 		const topic_callback_t& callback);
 	void doCallService(
-		const std::string& serviceName, const google::protobuf::Message& input,
-		google::protobuf::Message& output);
+		const std::string& serviceName, const std::string& inputSerializedMsg,
+		mrpt::optional_ref<google::protobuf::Message> outputMsg,
+		mrpt::optional_ref<std::string> outputSerializedMsg = std::nullopt,
+		mrpt::optional_ref<std::string> outputMsgTypeName = std::nullopt);
 
 	friend struct internal::InfoPerService;
 	friend struct internal::InfoPerSubscribedTopic;
@@ -192,7 +193,7 @@ void Client::callService(
 	const std::string& serviceName, const INPUT_MSG_T& input,
 	OUTPUT_MSG_T& output)
 {
-	doCallService(serviceName, input, output);
+	doCallService(serviceName, input.SerializeAsString(), output);
 }
 
 }  // namespace mvsim

modules/comms/python/generated-sources/mvsim/Comms/Client.cpp

@@ -21,15 +21,15 @@ void bind_mvsim_Comms_Client(std::function< pybind11::module &(std::string const
 {
 	{ // mvsim::Client file:mvsim/Comms/Client.h line:47
 		pybind11::class_<mvsim::Client, std::shared_ptr<mvsim::Client>> cl(M("mvsim"), "Client", "This is the connection of any user program with the MVSIM server, so\n it can advertise and subscribe to topics and use remote services.\n\n Users should instance a class mvsim::Client (C++) or mvsim.Client (Python) to\n communicate with the simulation runnin in mvsim::World or any other module.\n\n Usage:\n  - Instantiate a Client object.\n  - Call connect(). It will return immediately.\n  - The client will be working on the background as long as the object is not\n destroyed.\n\n Messages and topics are described as Protobuf messages, and communications\n are done via ZMQ sockets.\n\n See: https://mvsimulator.readthedocs.io/\n\n \n\n ");
-		{ // mvsim::Client::InfoPerNode file:mvsim/Comms/Client.h line:96
+		{ // mvsim::Client::InfoPerNode file:mvsim/Comms/Client.h line:95
 			auto & enclosing_class = cl;
 			pybind11::class_<mvsim::Client::InfoPerNode, std::shared_ptr<mvsim::Client::InfoPerNode>> cl(enclosing_class, "InfoPerNode", "");
 			cl.def( pybind11::init( [](){ return new mvsim::Client::InfoPerNode(); } ) );
 			cl.def( pybind11::init( [](mvsim::Client::InfoPerNode const &o){ return new mvsim::Client::InfoPerNode(o); } ) );
 			cl.def_readwrite("name", &mvsim::Client::InfoPerNode::name);
 		}
 
-		{ // mvsim::Client::InfoPerTopic file:mvsim/Comms/Client.h line:102
+		{ // mvsim::Client::InfoPerTopic file:mvsim/Comms/Client.h line:101
 			auto & enclosing_class = cl;
 			pybind11::class_<mvsim::Client::InfoPerTopic, std::shared_ptr<mvsim::Client::InfoPerTopic>> cl(enclosing_class, "InfoPerTopic", "");
 			cl.def( pybind11::init( [](){ return new mvsim::Client::InfoPerTopic(); } ) );
@@ -41,11 +41,12 @@ void bind_mvsim_Comms_Client(std::function< pybind11::module &(std::string const
 		}
 
 		cl.def( pybind11::init( [](){ return new mvsim::Client(); } ) );
-		cl.def( pybind11::init<const char *>(), pybind11::arg("nodeName") );
+		cl.def( pybind11::init<const std::string &>(), pybind11::arg("nodeName") );
 
-		cl.def("setName", (void (mvsim::Client::*)(const char *)) &mvsim::Client::setName, "C++: mvsim::Client::setName(const char *) --> void", pybind11::arg("nodeName"));
+		cl.def("setName", (void (mvsim::Client::*)(const std::string &)) &mvsim::Client::setName, "@{ \n\nC++: mvsim::Client::setName(const std::string &) --> void", pybind11::arg("nodeName"));
 		cl.def("connect", (void (mvsim::Client::*)()) &mvsim::Client::connect, "Connects to the server in a parallel thread. \n\nC++: mvsim::Client::connect() --> void");
 		cl.def("connected", (bool (mvsim::Client::*)() const) &mvsim::Client::connected, "Whether the client is correctly connected to the server. \n\nC++: mvsim::Client::connected() const --> bool");
 		cl.def("shutdown", (void (mvsim::Client::*)()) &mvsim::Client::shutdown, "Shutdowns the communication thread. Blocks until the thread is stopped.\n There is no need to manually call this method, it is called upon\n destruction. \n\nC++: mvsim::Client::shutdown() --> void");
+		cl.def("callService", (std::string (mvsim::Client::*)(const std::string &, const std::string &)) &mvsim::Client::callService, "C++: mvsim::Client::callService(const std::string &, const std::string &) --> std::string", pybind11::arg("serviceName"), pybind11::arg("inputSerializedMsg"));
 	}
 }

modules/comms/python/generated-sources/mvsim/Comms/common.cpp

@@ -1,5 +1,6 @@
 #include <mvsim/Comms/common.h>
 #include <sstream> // __str__
+#include <stdexcept>
 
 #include <pybind11/pybind11.h>
 #include <functional>
@@ -18,12 +19,25 @@
 struct PyCallBack_mvsim_UnexpectedMessageException : public mvsim::UnexpectedMessageException {
 	using mvsim::UnexpectedMessageException::UnexpectedMessageException;
 
+	const char * what() const noexcept override { 
+		pybind11::gil_scoped_acquire gil;
+		pybind11::function overload = pybind11::get_overload(static_cast<const mvsim::UnexpectedMessageException *>(this), "what");
+		if (overload) {
+			auto o = overload.operator()<pybind11::return_value_policy::reference>();
+			if (pybind11::detail::cast_is_temporary_value_reference<const char *>::value) {
+				static pybind11::detail::overload_caster_t<const char *> caster;
+				return pybind11::detail::cast_ref<const char *>(std::move(o), caster);
+			}
+			else return pybind11::detail::cast_safe<const char *>(std::move(o));
+		}
+		return runtime_error::what();
+	}
 };
 
 void bind_mvsim_Comms_common(std::function< pybind11::module &(std::string const &namespace_) > &M)
 {
 	{ // mvsim::UnexpectedMessageException file:mvsim/Comms/common.h line:49
-		pybind11::class_<mvsim::UnexpectedMessageException, std::shared_ptr<mvsim::UnexpectedMessageException>, PyCallBack_mvsim_UnexpectedMessageException> cl(M("mvsim"), "UnexpectedMessageException", "");
+		pybind11::class_<mvsim::UnexpectedMessageException, std::shared_ptr<mvsim::UnexpectedMessageException>, PyCallBack_mvsim_UnexpectedMessageException, std::runtime_error> cl(M("mvsim"), "UnexpectedMessageException", "");
 		cl.def( pybind11::init<const char *>(), pybind11::arg("reason") );
 
 		cl.def( pybind11::init( [](PyCallBack_mvsim_UnexpectedMessageException const &o){ return new PyCallBack_mvsim_UnexpectedMessageException(o); } ) );

modules/comms/python/generated-sources/pymvsim_comms.cpp

@@ -8,6 +8,8 @@
 
 typedef std::function< pybind11::module & (std::string const &) > ModuleGetter;
 
+void bind_std_exception(std::function< pybind11::module &(std::string const &namespace_) > &M);
+void bind_std_stdexcept(std::function< pybind11::module &(std::string const &namespace_) > &M);
 void bind_mvsim_Comms_common(std::function< pybind11::module &(std::string const &namespace_) > &M);
 void bind_mvsim_Comms_Client(std::function< pybind11::module &(std::string const &namespace_) > &M);
 
@@ -26,11 +28,14 @@ PYBIND11_MODULE(pymvsim_comms, root_module) {
 
 	std::vector< std::pair<std::string, std::string> > sub_modules {
 		{"", "mvsim"},
+		{"", "std"},
 	};
 	for(auto &p : sub_modules ) modules[p.first.size() ? p.first+"::"+p.second : p.second] = modules[p.first].def_submodule(p.second.c_str(), ("Bindings for " + p.first + "::" + p.second + " namespace").c_str() );
 
 	//pybind11::class_<std::shared_ptr<void>>(M(""), "_encapsulated_data_");
 
+	bind_std_exception(M);
+	bind_std_stdexcept(M);
 	bind_mvsim_Comms_common(M);
 	bind_mvsim_Comms_Client(M);
 

modules/comms/python/generated-sources/pymvsim_comms.modules

@@ -1 +1 @@
-mvsim 
+mvsim std 

modules/comms/python/generated-sources/pymvsim_comms.sources

@@ -1,3 +1,5 @@
 pymvsim_comms.cpp
+std/exception.cpp
+std/stdexcept.cpp
 mvsim/Comms/common.cpp
 mvsim/Comms/Client.cpp

modules/comms/python/generated-sources/std/exception.cpp

@@ -0,0 +1,46 @@
+#include <exception>
+#include <sstream> // __str__
+
+#include <pybind11/pybind11.h>
+#include <functional>
+#include <string>
+#include <pybind11/stl.h>
+
+
+#ifndef BINDER_PYBIND11_TYPE_CASTER
+	#define BINDER_PYBIND11_TYPE_CASTER
+	PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr<T>);
+	PYBIND11_DECLARE_HOLDER_TYPE(T, T*);
+	PYBIND11_MAKE_OPAQUE(std::shared_ptr<void>);
+#endif
+
+// std::exception file:bits/exception.h line:60
+struct PyCallBack_std_exception : public std::exception {
+	using std::exception::exception;
+
+	const char * what() const noexcept override { 
+		pybind11::gil_scoped_acquire gil;
+		pybind11::function overload = pybind11::get_overload(static_cast<const std::exception *>(this), "what");
+		if (overload) {
+			auto o = overload.operator()<pybind11::return_value_policy::reference>();
+			if (pybind11::detail::cast_is_temporary_value_reference<const char *>::value) {
+				static pybind11::detail::overload_caster_t<const char *> caster;
+				return pybind11::detail::cast_ref<const char *>(std::move(o), caster);
+			}
+			else return pybind11::detail::cast_safe<const char *>(std::move(o));
+		}
+		return exception::what();
+	}
+};
+
+void bind_std_exception(std::function< pybind11::module &(std::string const &namespace_) > &M)
+{
+	{ // std::exception file:bits/exception.h line:60
+		pybind11::class_<std::exception, std::shared_ptr<std::exception>, PyCallBack_std_exception> cl(M("std"), "exception", "");
+		cl.def( pybind11::init( [](){ return new std::exception(); }, [](){ return new PyCallBack_std_exception(); } ) );
+		cl.def( pybind11::init( [](PyCallBack_std_exception const &o){ return new PyCallBack_std_exception(o); } ) );
+		cl.def( pybind11::init( [](std::exception const &o){ return new std::exception(o); } ) );
+		cl.def("assign", (class std::exception & (std::exception::*)(const class std::exception &)) &std::exception::operator=, "C++: std::exception::operator=(const class std::exception &) --> class std::exception &", pybind11::return_value_policy::automatic, pybind11::arg(""));
+		cl.def("what", (const char * (std::exception::*)() const) &std::exception::what, "C++: std::exception::what() const --> const char *", pybind11::return_value_policy::automatic);
+	}
+}

modules/comms/python/generated-sources/std/stdexcept.cpp

@@ -0,0 +1,52 @@
+#include <iterator>
+#include <memory>
+#include <sstream> // __str__
+#include <stdexcept>
+#include <string>
+
+#include <pybind11/pybind11.h>
+#include <functional>
+#include <string>
+#include <pybind11/stl.h>
+
+
+#ifndef BINDER_PYBIND11_TYPE_CASTER
+	#define BINDER_PYBIND11_TYPE_CASTER
+	PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr<T>);
+	PYBIND11_DECLARE_HOLDER_TYPE(T, T*);
+	PYBIND11_MAKE_OPAQUE(std::shared_ptr<void>);
+#endif
+
+// std::runtime_error file:stdexcept line:219
+struct PyCallBack_std_runtime_error : public std::runtime_error {
+	using std::runtime_error::runtime_error;
+
+	const char * what() const noexcept override { 
+		pybind11::gil_scoped_acquire gil;
+		pybind11::function overload = pybind11::get_overload(static_cast<const std::runtime_error *>(this), "what");
+		if (overload) {
+			auto o = overload.operator()<pybind11::return_value_policy::reference>();
+			if (pybind11::detail::cast_is_temporary_value_reference<const char *>::value) {
+				static pybind11::detail::overload_caster_t<const char *> caster;
+				return pybind11::detail::cast_ref<const char *>(std::move(o), caster);
+			}
+			else return pybind11::detail::cast_safe<const char *>(std::move(o));
+		}
+		return runtime_error::what();
+	}
+};
+
+void bind_std_stdexcept(std::function< pybind11::module &(std::string const &namespace_) > &M)
+{
+	{ // std::runtime_error file:stdexcept line:219
+		pybind11::class_<std::runtime_error, std::shared_ptr<std::runtime_error>, PyCallBack_std_runtime_error, std::exception> cl(M("std"), "runtime_error", "");
+		cl.def( pybind11::init<const std::string &>(), pybind11::arg("__arg") );
+
+		cl.def( pybind11::init<const char *>(), pybind11::arg("") );
+
+		cl.def( pybind11::init( [](PyCallBack_std_runtime_error const &o){ return new PyCallBack_std_runtime_error(o); } ) );
+		cl.def( pybind11::init( [](std::runtime_error const &o){ return new std::runtime_error(o); } ) );
+		cl.def("assign", (class std::runtime_error & (std::runtime_error::*)(const class std::runtime_error &)) &std::runtime_error::operator=, "C++: std::runtime_error::operator=(const class std::runtime_error &) --> class std::runtime_error &", pybind11::return_value_policy::automatic, pybind11::arg(""));
+		cl.def("what", (const char * (std::runtime_error::*)() const) &std::runtime_error::what, "C++: std::runtime_error::what() const --> const char *", pybind11::return_value_policy::automatic);
+	}
+}

modules/comms/src/Comms/Client.cpp

@@ -687,9 +687,26 @@ void Client::internalTopicUpdatesThread()
 #endif
 }
 
+// Overload for python wrapper
+std::string Client::callService(
+	const std::string& serviceName, const std::string& inputSerializedMsg)
+{
+	MRPT_START
+#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
+
+	std::string outMsgData, outMsgType;
+	doCallService(
+		serviceName, inputSerializedMsg, std::nullopt, outMsgData, outMsgType);
+	return outMsgData;
+#endif
+	MRPT_END
+}
+
 void Client::doCallService(
-	const std::string& serviceName, const google::protobuf::Message& input,
-	google::protobuf::Message& output)
+	const std::string& serviceName, const std::string& inputSerializedMsg,
+	mrpt::optional_ref<google::protobuf::Message> outputMsg,
+	mrpt::optional_ref<std::string> outputSerializedMsg,
+	mrpt::optional_ref<std::string> outputMsgTypeName)
 {
 	MRPT_START
 #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
@@ -722,12 +739,23 @@ void Client::doCallService(
 
 	mvsim_msgs::CallService csMsg;
 	csMsg.set_servicename(serviceName);
-	csMsg.set_serializedinput(input.SerializeAsString());
+	csMsg.set_serializedinput(inputSerializedMsg);
 
 	mvsim::sendMessage(csMsg, srvReqSock);
 
 	const auto m = mvsim::receiveMessage(srvReqSock);
-	mvsim::parseMessage(m, output);
+	if (outputMsg)
+	{
+		mvsim::parseMessage(m, outputMsg.value().get());
+	}
+	if (outputSerializedMsg)
+	{
+		const auto [typeName, serializedData] =
+			internal::parseMessageToParts(m);
+
+		outputSerializedMsg.value().get() = serializedData;
+		if (outputMsgTypeName) outputMsgTypeName.value().get() = typeName;
+	}
 #endif
 	MRPT_END
 }

modules/python.conf

@@ -1,4 +1,3 @@
 +namespace mvsim
 -namespace mrpt
--namespace std
 +include <pybind11/stl.h>

mvsim_tutorial/python/publisher-example1.py

@@ -3,12 +3,26 @@
 # To test with a local build:
 #
 # export PYTHONPATH=$HOME/code/mvsim/build/lib/:$PYTHONPATH
-# export PYTHONPATH=/tmp/install-mvsim/include/mvsim/:$PYTHONPATH
 
-import pymvsim_comms
+from mvsim_comms import pymvsim_comms
 from mvsim_msgs import SrvSetPose_pb2
 import time
-from math import radians
+import math
+
+
+def setObjectPose(client, objectName, x, y, theta_rad):
+    # Send a set pose request:
+    req = SrvSetPose_pb2.SrvSetPose()
+    req.objectId = objectName  # vehicle/robot/object name in MVSIM
+    req.pose.x = x
+    req.pose.y = y
+    req.pose.z = 0
+    req.pose.yaw = theta_rad
+    req.pose.pitch = 0
+    req.pose.roll = 0 # math.radians(0.0)
+    #ret =
+    client.callService('set_pose', req.SerializeToString())
+
 
 if __name__ == "__main__":
     client = pymvsim_comms.mvsim.Client()
@@ -17,17 +31,8 @@
     client.connect()
     print("Connected successfully.")
 
-    # Send a set pose request:
-    req = SrvSetPose_pb2.SrvSetPose()
-    req.objectId = 'veh1'  # vehicle/robot/object name in MVSIM
-    req.pose.x = 1.0
-    req.pose.y = 1.0
-    req.pose.z = 1.0
-    req.pose.yaw = radians(0.0)
-    req.pose.pitch = radians(0.0)
-    req.pose.roll = radians(0.0)
-    print(req)
-    s = req.SerializeToString()
-    print(s)
-
-    time.sleep(2.0)
+    for i in range(1000):
+        th = i*0.02
+        R = 5
+        setObjectPose(client, 'r1', math.sin(th)*R, R*(1-math.cos(th)), th)
+        time.sleep(0.01)

mvsim_tutorial/python/subscriber-example1.py

@@ -3,9 +3,8 @@
 # To test with a local build:
 #
 # export PYTHONPATH=$HOME/code/mvsim/build/lib/:$PYTHONPATH
-# export PYTHONPATH=/tmp/install-mvsim/include/mvsim/:$PYTHONPATH
 
-import pymvsim_comms
+from mvsim_comms import pymvsim_comms
 import time
 
 if __name__ == "__main__":