Open PhilippPolterauer opened 1 year ago
This seems to be an issue with Ros2Supervisor
main loop. Apparently, python plugins are forced to be stopped without proper exit handling. Similarly in #655 , when the Ros2Supervisor.__supervisor_step_callback
enter the condition of wb_robot_step() == -1
here, the only return triggered to end the execution happens on WebotsNode C++ side only, leaving the Python side without proper stop and cleaning procedure.
In the side of ROS2 client, at first sight, can be a resource leak when the simulation is resetted because Nodes associated results are never removed and always reinstanciated at the Python side.
Any thoughts, @ygoumaz ?
I was tempting a workaround with a topic publish from Ros2Supervisor
to notify the wb_robot_step() == -1
condition without success. Then I add the wb_robot_step() == -1
verification in the Python plugins, calling sys.exit(0)
and only works inside step()
finishing the driver process cleanly.
Other thing to try maybe can be add a shutdown()
method to the plugins.
At the ROS2 side also have, lifecycle which can also be a good option to create a management for node states.
Adding the shutdown()
methods seems feasible.
In PythonPlugin.cpp
void PythonPlugin::stop() {
PyObject_CallMethod(mPyPlugin, "stop", "");
std::cout << "PythonPlugin::stop" << std::endl;
// PyErr_Print();
std::cout << "PythonPlugin::stop::print" << std::endl;
Py_Finalize();
std::cout << "PythonPlugin::stop::finalize" << std::endl;
}
the Python method
def stop(self):
self.__node.get_logger().info('stopping')
self.__node.destroy_node()
sys.exit(0)
In WebotsNode.cpp
int WebotsNode::step() {
if (gShutdownSignalReceived && !mWaitingForUrdfRobotToBeRemoved) {
mRemoveUrdfRobotPublisher->publish(mRemoveUrdfRobotMessage);
mWaitingForUrdfRobotToBeRemoved = true;
}
const int result = wb_robot_step(mStep);
if (result == -1) {
// Check if the plugin is actually an instance of PythonPlugin
for (std::shared_ptr<PluginInterface> plugin : mPlugins) {
std::shared_ptr<webots_ros2_driver::PythonPlugin> pythonPlugin = std::dynamic_pointer_cast<webots_ros2_driver::PythonPlugin>(plugin);
if(pythonPlugin) {
std::cout << "stopssss" << std::endl;
pythonPlugin->stop(); // stop only for python plugins
}
}
return result;
}
for (std::shared_ptr<PluginInterface> plugin : mPlugins)
plugin->step();
return result;
}
Using this code in Driver.cpp
std::shared_ptr<webots_ros2_driver::WebotsNode> node = std::make_shared<webots_ros2_driver::WebotsNode>(robotName);
node->init();
RCLCPP_INFO(node->get_logger(), "Controller successfully connected to robot in Webots simulation.");
while (true) {
if (node->step() == -1)
break;
rclcpp::spin_some(node);
}
std::cout << "outlook" << std::endl;
// rclcpp::spin_some(node);
// std::cout << "last spin" << std::endl;
RCLCPP_INFO(node->get_logger(), "Controller successfully disconnected from robot in Webots simulation.");
std::cout << "shutting down" << std::endl;
rclcpp::shutdown();
std::cout << "clean" << std::endl;
wb_robot_cleanup();
std::cout << "return" << std::endl;
return 0;
However, the print trace reach until std::cout << "clean" << std::endl;
before the process exit with code 1, with both Python plugins.
Apparently there is an unhandled event when wb_robot_cleanup();
is called.
UPDATED
Apparently, seems to be a twice call of the wb_robot_cleanup()
function, called once at Robot.__del__
Python method, and at the end of Driver.cpp
Regarding issue #655, I will implement a shutdown handler in the launch file that will clean all other started nodes, excluding Webots, and then respawn them. However, before proceeding with this implementation in Iron, I am awaiting the merge of #760.
Regarding this issue, I believe your approach is correct. While I haven't examined it in depth yet, you could potentially invoke wb_robot_cleanup
in the driver only if the plugin is not a Python plugin. Otherwise, we can adjust the Python API similarly to what was done for the constructor and wb_robot_init
, thus avoiding a redundant call in Robot.del.
I will test the wb_robot_cleanup
condition, initial idea is itering again the WebotsNode.plugin
elements to correctly choose a shutdown method.
In this regard, the Python API seems to require a more in depth examination.
I developed a Gym environment using webots_ros2 and encountered that the info gathered at the environment side is delayed. Initial findings show the lack of spin_some()
method at the side of rlcpy
(the mantainers already know this), since the C++ ROS nodes appears to be faster than Python ROS nodes in using the DDS layer due at each step the driver call spin_some()
in the C++ side and only spin_once(timeout_sec=0)
in the Python side, this last requiring more time to finish all the ROS callbacks in one step.
Still related to #788.
Bug Description when trying to stop a running launch file via CTRL+C the ros driver does not terminate cleanly. We encountered this issue when writing a C++ plugin which utilizes RAII and found out that objects which are created inside the C++ plugin are not properly destructed.
Steps to Reproduce
mkdir -p ~/dev_ws/src/; cd ~/dev_ws/src; git clone --recursive https://github.com/cyberbotics/webots_ros2.git; cd ..
colcon build --symlink-install
source install/local_setup.bash; ros2 launch webots_ros2_tesla robot_launch.py
Error: [ERROR] [ros2_supervisor.py-2]: process has died [pid 413862, exit code -2, cmd '/home/philpolt/ros/webots_shutdown_issue/install/webots_ros2_driver/lib/webots_ros2_driver/ros2_supervisor.py --ros-args'].
Expected behavior The signal SIGINT should be properly handled inside the driver node. The launch file process
ros2_supervisor.py-2
should finish cleanly!System