Skip to content

Commit

Permalink
Implement Behaviors to Interact with ROS Services (#215)
Browse files Browse the repository at this point in the history
  • Loading branch information
amalnanavati authored Aug 31, 2024
1 parent ba572a7 commit 1729e28
Show file tree
Hide file tree
Showing 11 changed files with 775 additions and 4 deletions.
1 change: 1 addition & 0 deletions docs/sources/conf.py
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,7 @@
'ros2topic', 'ros2topic.api',
'sensor_msgs', 'sensor_msgs.msg',
'std_msgs', 'std_msgs.msg',
'std_srvs', 'std_srvs.srv',
'tf2_ros',
'unique_identifier_msgs', 'unique_identifier_msgs.msg'
]
Expand Down
1 change: 1 addition & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
<depend>rclpy</depend>
<depend>ros2topic</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>unique_identifier_msgs</depend>

<!-- behaviour dependencies (subscribers, transforms, ...) -->
Expand Down
1 change: 1 addition & 0 deletions py_trees_ros/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
from . import mock
from . import programs
from . import publishers
from . import service_clients
from . import subscribers
from . import transforms
from . import trees
Expand Down
4 changes: 3 additions & 1 deletion py_trees_ros/action_clients.py
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ class FromBlackboard(py_trees.behaviour.Behaviour):
name="WaitForGoal",
variable_name="/my_goal"
)
action_client = py_trees_ros.aciton_clients.FromBlackboard(
action_client = py_trees_ros.action_clients.FromBlackboard(
action_type=py_trees_actions.Dock,
action_name="dock",
name="ActionClient"
Expand Down Expand Up @@ -153,6 +153,8 @@ def setup(self, **kwargs):
result = None
if self.wait_for_server_timeout_sec > 0.0:
result = self.action_client.wait_for_server(timeout_sec=self.wait_for_server_timeout_sec)
elif self.wait_for_server_timeout_sec == 0.0:
result = True # don't wait and don't check if the server is ready
else:
iterations = 0
period_sec = -1.0*self.wait_for_server_timeout_sec
Expand Down
2 changes: 2 additions & 0 deletions py_trees_ros/mock/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,3 +15,5 @@

from . import actions
from . import dock
from . import services
from . import set_bool
78 changes: 78 additions & 0 deletions py_trees_ros/mock/services.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,78 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# License: BSD
# https://raw.githubusercontent.com/splintered-reality/py_trees_ros_tutorials/devel/LICENSE
#
##############################################################################
# Documentation
##############################################################################

"""
Service server templates.
"""

##############################################################################
# Imports
##############################################################################

import time

import rclpy

##############################################################################
# Service Server
##############################################################################
#
# References:
#
# service client : https://github.com/ros2/rclpy/blob/rolling/rclpy/rclpy/client.py
# service server : https://github.com/ros2/rclpy/blob/rolling/rclpy/rclpy/service.py
# service examples : https://github.com/ros2/examples/tree/rolling/rclpy/services

class GenericServer(object):
"""
Generic service server that can be used for testing.
Args:
node_name (:obj:`str`): name of the node
service_name (:obj:`str`): name of the service
service_type (:obj:`type`): type of the service
sleep_time (:obj:`float`): time to sleep before returning a response
callback (:obj:`Optional[callable]`): callback to execute when the service is called
"""
def __init__(self,
node_name,
service_name,
service_type,
sleep_time=1.0,
callback=None):
self.node = rclpy.create_node(node_name)

self.sleep_time = sleep_time
self.callback = callback

# Create the service
self.server = self.node.create_service(
service_type,
service_name,
self.execute_callback
)

def execute_callback(self, request, response):
"""
Execute the callback and populate the response.
"""
if self.callback is not None:
return self.callback(request, response)

# Do nothing
time.sleep(self.sleep_time)
return response

def shutdown(self):
"""
Cleanup
"""
self.server.destroy()
self.node.destroy_node()
55 changes: 55 additions & 0 deletions py_trees_ros/mock/set_bool.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# License: BSD
# https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE
#
##############################################################################
# Documentation
##############################################################################

"""
Mocks a SetBool service
"""


##############################################################################
# Imports
##############################################################################

import time

from std_srvs.srv import SetBool

from . import services

##############################################################################
# Class
##############################################################################


class SetBoolServer(services.GenericServer):
"""
Simple server that docks if the goal is true, undocks otherwise.
"""
SUCCESS_MESSAGE = "Succeeded in setting bool to True"
FAILURE_MESSAGE = "Failed to set bool to False"

def __init__(self, sleep_time=1.0):
super().__init__(
node_name="set_bool_server",
service_name="set_bool",
service_type=SetBool,
sleep_time=sleep_time,
callback=self.callback,
)

def callback(self, request, response):
time.sleep(self.sleep_time)
if request.data:
response.success = True
response.message = SetBoolServer.SUCCESS_MESSAGE
else:
response.success = False
response.message = SetBoolServer.FAILURE_MESSAGE
return response
Loading

0 comments on commit 1729e28

Please sign in to comment.