... | ... | @@ -218,7 +218,6 @@ tst_registry->loadDirectory( |
|
|
```
|
|
|
|
|
|
When you want to load a tree for execution from a file, for instance, from within your service callback, you can do the following:
|
|
|
|
|
|
```c++
|
|
|
TstML::TSTNode* tst_node = TstML::TSTNode::load(
|
|
|
QUrl::fromLocalFile(QString::fromStdString(filename)),
|
... | ... | @@ -361,6 +360,16 @@ from irobot_create_msgs.action import Undock |
|
|
|
|
|
import ament_index_python
|
|
|
|
|
|
# Work around for ROS not showing error with a MultiThreadedExecutor
|
|
|
def display_exceptions(func):
|
|
|
def wrapper(*args):
|
|
|
try:
|
|
|
func(*args)
|
|
|
except Exception as ex:
|
|
|
traceback.print_exception(*sys.exc_info())
|
|
|
print(f"Unhandled exception was caught: '{ex}'")
|
|
|
return wrapper
|
|
|
|
|
|
# Ugly hack to get a new name for each node
|
|
|
ros_name_counter = 0
|
|
|
def gen_name(name):
|
... | ... | @@ -388,8 +397,11 @@ class UndockExecutor(TstML.Executor.AbstractNodeExecutor): |
|
|
self._send_goal_future = self._action_client.send_goal_async(goal_msg, self.feedback_callback)
|
|
|
self._send_goal_future.add_done_callback(self.goal_response_callback)
|
|
|
return TstML.Executor.ExecutionStatus.Started()
|
|
|
|
|
|
def feedback_callback(self, feedback_msg):
|
|
|
print(feedback_msg.feedback)
|
|
|
|
|
|
@display_exceptions
|
|
|
def goal_response_callback(self, future):
|
|
|
self._goal_handle = future.result()
|
|
|
if not self._goal_handle.accepted:
|
... | ... | @@ -400,6 +412,8 @@ class UndockExecutor(TstML.Executor.AbstractNodeExecutor): |
|
|
|
|
|
self._get_result_future = self._goal_handle.get_result_async()
|
|
|
self._get_result_future.add_done_callback(self.handle_result_callback)
|
|
|
|
|
|
@display_exceptions
|
|
|
def handle_result_callback(self, future):
|
|
|
print("Finished!")
|
|
|
self.executionFinished(TstML.Executor.ExecutionStatus.Finished())
|
... | ... | @@ -608,6 +622,31 @@ service = self.create_service(..., callback_group=self.group) |
|
|
|
|
|
Note that the ```execute_tst``` call is not reentrant, and should not be added to the group.
|
|
|
|
|
|
Also, due to a bug in `rclpy` for the version of ROS installed in the labs computers, python exceptions are not displayed when using a `MultiThreadedExecutor`. To work around that problem, it is possible to use the `display_exception` decorator, at the top of your file for the ROS node, add the following:
|
|
|
|
|
|
```python
|
|
|
import traceback
|
|
|
import sys
|
|
|
|
|
|
def display_exception(func):
|
|
|
def wrapper(*args):
|
|
|
try:
|
|
|
func(*args)
|
|
|
except Exception as ex:
|
|
|
traceback.print_exception(*sys.exc_info())
|
|
|
print(f"Unhandled exception was caught: '{ex}'")
|
|
|
return wrapper
|
|
|
```
|
|
|
|
|
|
Then just before the function used to define your service callback, add `@display_exceptions`, so that it looks like this:
|
|
|
|
|
|
```python
|
|
|
@display_exceptions
|
|
|
def execute_tst_callback(self, request, response):
|
|
|
``
|
|
|
|
|
|
You can also check the `UndockExecutor` for an example of use of `@display_exceptions`.
|
|
|
|
|
|
### C++
|
|
|
|
|
|
Modify your main function to look like this, instead of using `rclcpp::spin(some_node)`:
|
... | ... | |