-
-
Notifications
You must be signed in to change notification settings - Fork 162
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add Python plugin support to
webots_ros2_driver (#265)
* Add Python <--> C++ test * Prepare * Initial prototype * Add WebotsNode object * Add properties * Import the Python lib * Improve error handling * Cleaner * Add node example * Integrate the new libController * Delete the test * Fix copyright * Explain * Copyright fix * Constructor can be private * Print Python errors * Fix Windows usage * Update webots_ros2_driver/src/PythonPlugin.cpp Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/src/WebotsNode.cpp Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_turtlebot/webots_ros2_turtlebot/plugin_example.py Co-authored-by: Olivier Michel <[email protected]> * Use const Co-authored-by: Olivier Michel <[email protected]>
- Loading branch information
1 parent
c1a1f5c
commit b2e7635
Showing
12 changed files
with
235 additions
and
16 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
42 changes: 42 additions & 0 deletions
42
webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,42 @@ | ||
| // Copyright 1996-2021 Cyberbotics Ltd. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef PYTHON_PLUGIN_HPP | ||
| #define PYTHON_PLUGIN_HPP | ||
|
|
||
| #include <unordered_map> | ||
| #include <Python.h> | ||
|
|
||
| #include <webots_ros2_driver/PluginInterface.hpp> | ||
| #include <webots_ros2_driver/WebotsNode.hpp> | ||
|
|
||
| namespace webots_ros2_driver | ||
| { | ||
| class PythonPlugin : public PluginInterface | ||
| { | ||
| public: | ||
| void init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> ¶meters) override; | ||
| void step() override; | ||
|
|
||
| static std::shared_ptr<PythonPlugin> createFromType(const std::string &type); | ||
|
|
||
| private: | ||
| PythonPlugin(PyObject *pyPlugin); | ||
|
|
||
| PyObject *mPyPlugin; | ||
| PyObject *getPyWebotsNodeInstance(); | ||
| }; | ||
| } | ||
|
|
||
| #endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,76 @@ | ||
| #include "webots_ros2_driver/PythonPlugin.hpp" | ||
|
|
||
| static PyObject *gPyWebotsNode = NULL; | ||
|
|
||
| namespace webots_ros2_driver | ||
| { | ||
| PythonPlugin::PythonPlugin(PyObject *pyPlugin) : mPyPlugin(pyPlugin){}; | ||
|
|
||
| void PythonPlugin::init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> ¶meters) | ||
| { | ||
| PyObject *pyParameters = PyDict_New(); | ||
| for (const std::pair<std::string, std::string> ¶meter : parameters) | ||
| PyDict_SetItem(pyParameters, PyUnicode_FromString(parameter.first.c_str()), PyUnicode_FromString(parameter.second.c_str())); | ||
|
|
||
| PyObject_CallMethod(mPyPlugin, "init", "OO", getPyWebotsNodeInstance(), pyParameters); | ||
| PyErr_Print(); | ||
| } | ||
|
|
||
| void PythonPlugin::step() | ||
| { | ||
| PyObject_CallMethod(mPyPlugin, "step", ""); | ||
| PyErr_Print(); | ||
| } | ||
|
|
||
| PyObject *PythonPlugin::getPyWebotsNodeInstance() | ||
| { | ||
| if (gPyWebotsNode) | ||
| return gPyWebotsNode; | ||
|
|
||
| PyObject *pyWebotsExtraModuleSource = Py_CompileString( | ||
| R"EOT( | ||
| from webots_ros2_driver.webots.controller import Supervisor | ||
| class WebotsNode: | ||
| def __init__(self): | ||
| self.robot = Supervisor.internalGetInstance() | ||
| )EOT", | ||
| "webots_extra", Py_file_input); | ||
| if (!pyWebotsExtraModuleSource) | ||
| throw std::runtime_error("Error: The Python module with the WebotsNode class cannot be compiled."); | ||
|
|
||
| PyObject *pyWebotsExtraModule = PyImport_ExecCodeModule("webots_extra", pyWebotsExtraModuleSource); | ||
| if (!pyWebotsExtraModule) | ||
| throw std::runtime_error("Error: The Python module with the WebotsNode class cannot be executed."); | ||
|
|
||
| PyObject *pyDict = PyModule_GetDict(pyWebotsExtraModule); | ||
| PyObject *pyClass = PyDict_GetItemString(pyDict, "WebotsNode"); | ||
| gPyWebotsNode = PyObject_CallObject(pyClass, nullptr); | ||
| return gPyWebotsNode; | ||
| } | ||
|
|
||
| std::shared_ptr<PythonPlugin> PythonPlugin::createFromType(const std::string &type) | ||
| { | ||
| const std::string moduleName = type.substr(0, type.find_last_of(".")); | ||
| const std::string className = type.substr(type.find_last_of(".") + 1); | ||
|
|
||
| Py_Initialize(); | ||
|
|
||
| PyObject *pyName = PyUnicode_FromString(moduleName.c_str()); | ||
| PyObject *pyModule = PyImport_Import(pyName); | ||
| PyErr_Print(); | ||
|
|
||
| // If the module cannot be found the error should be handled in the upper level (e.g. try loading C++ plugin) | ||
| if (!pyModule) | ||
| return NULL; | ||
|
|
||
| PyObject *pyDict = PyModule_GetDict(pyModule); | ||
| PyObject *pyClass = PyDict_GetItemString(pyDict, className.c_str()); | ||
| PyErr_Print(); | ||
| if (!pyClass) | ||
| throw std::runtime_error("Error in plugin " + type + ": The class " + className + " cannot be found."); | ||
|
|
||
| PyObject *pyPlugin = PyObject_CallObject(pyClass, nullptr); | ||
| return std::shared_ptr<PythonPlugin>(new PythonPlugin(pyPlugin)); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
43 changes: 43 additions & 0 deletions
43
webots_ros2_turtlebot/webots_ros2_turtlebot/plugin_example.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,43 @@ | ||
| # Copyright 1996-2021 Cyberbotics Ltd. | ||
| # | ||
| # Licensed under the Apache License, Version 2.0 (the "License"); | ||
| # you may not use this file except in compliance with the License. | ||
| # You may obtain a copy of the License at | ||
| # | ||
| # http://www.apache.org/licenses/LICENSE-2.0 | ||
| # | ||
| # Unless required by applicable law or agreed to in writing, software | ||
| # distributed under the License is distributed on an "AS IS" BASIS, | ||
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| # See the License for the specific language governing permissions and | ||
| # limitations under the License. | ||
|
|
||
| """A simple dummy plugin that demonstrates the usage of Python plugins.""" | ||
|
|
||
| from webots_ros2_driver.webots.controller import Node | ||
| from std_msgs.msg import Float32 | ||
| import rclpy | ||
| import rclpy.node | ||
|
|
||
|
|
||
| class PluginExample: | ||
| def init(self, webots_node, properties): | ||
| print('PluginExample: The init() method is called') | ||
| print(' - properties:', properties) | ||
|
|
||
| print(' - basic timestep:', int(webots_node.robot.getBasicTimeStep())) | ||
| print(' - robot name:', webots_node.robot.getName()) | ||
| print(' - is robot?', webots_node.robot.getType() == Node.ROBOT) | ||
|
|
||
| self.__robot = webots_node.robot | ||
|
|
||
| # Unfortunately, we cannot get an instance of the parent ROS node. | ||
| # However, we can create a new one. | ||
| rclpy.init(args=None) | ||
| self.__node = rclpy.node.Node('plugin_node_example') | ||
| print('PluginExample: Node created') | ||
| self.__publisher = self.__node.create_publisher(Float32, 'custom_time', 1) | ||
| print('PluginExample: Publisher created') | ||
|
|
||
| def step(self): | ||
| self.__publisher.publish(Float32(data=self.__robot.getTime())) |