Feature to reset world via ros service #321
Conversation
| @@ -1140,6 +1140,33 @@ def update_bounds(self, entity, remove=False): | |||
| f"Updating bounds with unsupported entity type {type(entity)}" | |||
| ) | |||
|
|
|||
| def reset(self): | |||
| """Reset the world""" | |||
| from .yaml_utils import WorldYamlLoader | |||
There was a problem hiding this comment.
Is this to avoid circular imports?
| else: | ||
| new_world = WorldYamlLoader().from_yaml(self.source_yaml) | ||
|
|
||
| self.gui.set_world(new_world) |
There was a problem hiding this comment.
All this GUI code will fail if running pyrobosim headless. Put this all in a if self.has_gui: conditional.
Also, a good segue into suggesting adding a unit test for this function.
There was a problem hiding this comment.
Understood. Are you suggesting something like this ?
def reset(self):
"""Reset the world"""
from .yaml_utils import WorldYamlLoader
# Cancel all robot actions
for robot in self.robots:
robot.cancel_actions()
# Shut down all the old robots' ROS interfaces.
self.ros_node = self.ros_node if self.has_ros_node else None
if self.ros_node is not None:
for robot in self.robots:
self.ros_node.remove_robot_ros_interfaces(robot)
if self.has_gui:
if self.source_yaml_file is not None:
new_world = WorldYamlLoader().from_file(self.source_yaml_file)
else:
new_world = WorldYamlLoader().from_yaml(self.source_yaml)
self.gui.set_world(new_world)
# Start up the new robots' ROS interfaces.
if self.ros_node is not None:
self.ros_node.set_world(self)
for robot in self.robots:
self.ros_node.add_robot_ros_interfaces(robot)
self.gui.canvas.show()
self.gui.canvas.draw_signal.emit()
self.gui.on_robot_changed()There was a problem hiding this comment.
But I think if you don't have a GUI the world will not reset with this logic? So definitely check that with a test.
There was a problem hiding this comment.
Yes, you are right. I will verify this through a test. Now, my question is, do we want to reset the world in headless mode (without a GUI)? As per me in the previous reset implementation, we were only able to reset the world when the GUI was enabled. Please correct me if I am wrong.
There was a problem hiding this comment.
do we want to reset the world in headless mode (without a GUI)?
Absolutely! That is the main point of this associated issue, since resetting with a GUI was already possible.
There was a problem hiding this comment.
Now it makes sense to me. I need to find a way to reset all the important parameters of the World class without relying on the code from the gui.main module.
In the current codebase, the value of source_yaml is being set in the PyRoboSimMainWindow.set_world method. In my opinion, we need to implement similar functionality directly within the World class itself. Am I heading in the right direction? What do you think?
There was a problem hiding this comment.
Right, I think that should also go into the World class.
I think creating a unit test for loading a world and resetting it will give you confidence that the functionality will work headless, without the GUI. And from there on, you can work on ensuring it still works in the GUI. I bet it will make the code way less interconnected.
There was a problem hiding this comment.
I agree with you. Initially, it took me some time to figure out how the GUI code is used for resetting the world. I will consider your suggestion and start building upon it. Thanks.
|
|
||
| # Response | ||
| # bool success | ||
| # Indicates whether the world reset operation was successful: |
There was a problem hiding this comment.
You can probably just keep this line and not the other 2. It's pretty clear what this means.
| ResetWorld, | ||
| "reset_world", | ||
| self.reset_world_callback, | ||
| callback_group=ReentrantCallbackGroup(), |
There was a problem hiding this comment.
Since we won't be resetting worlds concurrently, MutuallyExclusiveCallbackGroup is okay.
| @@ -1140,6 +1140,33 @@ def update_bounds(self, entity, remove=False): | |||
| f"Updating bounds with unsupported entity type {type(entity)}" | |||
| ) | |||
|
|
|||
| def reset(self): | |||
There was a problem hiding this comment.
Can you also have this function cancel all robot actions before resetting? It was something missing and I had suggested in the issue.
There was a problem hiding this comment.
Okay, I included this in above mentioned reset function.
| """ | ||
| self.get_logger().info("Received reset world request.") | ||
|
|
||
| try: |
There was a problem hiding this comment.
Please try to avoid reacting to arbitrary errors with try/catch. I try to stay away from this in my code since it hides away unexpected bugs.
It's much better to e.g. have World.reset() return a bool for whether it succeeded, which can be used here.
There was a problem hiding this comment.
Okay, understood. I will think more about how to avoid using try/catch block and get back to you.
This PR adds a feature to reset the world via a ROS service in addition to the GUI interface.