Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Switch to using the rclpy context manager everywhere. #389

Merged
merged 3 commits into from
Jul 24, 2024
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys

from action_msgs.msg import GoalStatus
from example_interfaces.action import Fibonacci

Expand Down Expand Up @@ -71,18 +69,15 @@ def send_goal(self):


def main(args=None):
rclpy.init(args=args)

try:
action_client = MinimalActionClient()
with rclpy.init(args=args):
action_client = MinimalActionClient()

action_client.send_goal()
action_client.send_goal()

rclpy.spin(action_client)
except KeyboardInterrupt:
rclpy.spin(action_client)
except (KeyboardInterrupt, ExternalShutdownException):
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

import rclpy
from rclpy.action import ActionClient
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node


Expand Down Expand Up @@ -69,48 +70,49 @@ async def spinning(node):

async def run(args, loop):

logger = rclpy.logging.get_logger('minimal_action_client')

# init ros2
clalancette marked this conversation as resolved.
Show resolved Hide resolved
rclpy.init(args=args)

# create node
action_client = MinimalActionClientAsyncIO()

# start spinning
spin_task = loop.create_task(spinning(action_client))

# Parallel example
# execute goal request and schedule in loop
my_task1 = loop.create_task(action_client.send_goal())
my_task2 = loop.create_task(action_client.send_goal())

# glue futures together and wait
wait_future = asyncio.wait([my_task1, my_task2])
# run event loop
finished, unfinished = await wait_future
logger.info(f'unfinished: {len(unfinished)}')
for task in finished:
logger.info('result {} and status flag {}'.format(*task.result()))

# Sequence
result, status = await loop.create_task(action_client.send_goal())
logger.info(f'A) result {result} and status flag {status}')
result, status = await loop.create_task(action_client.send_goal())
logger.info(f'B) result {result} and status flag {status}')

# cancel spinning task
spin_task.cancel()
try:
await spin_task
except asyncio.exceptions.CancelledError:
pass
rclpy.shutdown()
with rclpy.init(args=args):
logger = rclpy.logging.get_logger('minimal_action_client')

# create node
action_client = MinimalActionClientAsyncIO()

# start spinning
spin_task = loop.create_task(spinning(action_client))

# Parallel example
# execute goal request and schedule in loop
my_task1 = loop.create_task(action_client.send_goal())
my_task2 = loop.create_task(action_client.send_goal())

# glue futures together and wait
wait_future = asyncio.wait([my_task1, my_task2])
# run event loop
finished, unfinished = await wait_future
logger.info(f'unfinished: {len(unfinished)}')
for task in finished:
logger.info('result {} and status flag {}'.format(*task.result()))

# Sequence
result, status = await loop.create_task(action_client.send_goal())
logger.info(f'A) result {result} and status flag {status}')
result, status = await loop.create_task(action_client.send_goal())
logger.info(f'B) result {result} and status flag {status}')

# cancel spinning task
spin_task.cancel()
try:
await spin_task
except asyncio.exceptions.CancelledError:
pass


def main(args=None):
loop = asyncio.get_event_loop()
loop.run_until_complete(run(args, loop=loop))
try:
loop = asyncio.get_event_loop()
loop.run_until_complete(run(args, loop=loop))
except (KeyboardInterrupt, ExternalShutdownException):
pass


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys

from example_interfaces.action import Fibonacci

import rclpy
Expand Down Expand Up @@ -79,18 +77,15 @@ def send_goal(self):


def main(args=None):
rclpy.init(args=args)

try:
action_client = MinimalActionClient()
with rclpy.init(args=args):
action_client = MinimalActionClient()

action_client.send_goal()
action_client.send_goal()

rclpy.spin(action_client)
except KeyboardInterrupt:
rclpy.spin(action_client)
except (KeyboardInterrupt, ExternalShutdownException):
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys

from action_msgs.msg import GoalStatus

from example_interfaces.action import Fibonacci
Expand All @@ -28,53 +26,48 @@ def feedback_cb(logger, feedback):


def main(args=None):
rclpy.init(args=args)

try:
node = rclpy.create_node('minimal_action_client')
with rclpy.init(args=args):
node = rclpy.create_node('minimal_action_client')

action_client = ActionClient(node, Fibonacci, 'fibonacci')
action_client = ActionClient(node, Fibonacci, 'fibonacci')

node.get_logger().info('Waiting for action server...')
node.get_logger().info('Waiting for action server...')

action_client.wait_for_server()
action_client.wait_for_server()

goal_msg = Fibonacci.Goal()
goal_msg.order = 10
goal_msg = Fibonacci.Goal()
goal_msg.order = 10

node.get_logger().info('Sending goal request...')
node.get_logger().info('Sending goal request...')

send_goal_future = action_client.send_goal_async(
goal_msg, feedback_callback=lambda feedback: feedback_cb(node.get_logger(), feedback))
send_goal_future = action_client.send_goal_async(
goal_msg,
feedback_callback=lambda feedback: feedback_cb(node.get_logger(), feedback))

rclpy.spin_until_future_complete(node, send_goal_future)
rclpy.spin_until_future_complete(node, send_goal_future)

goal_handle = send_goal_future.result()
goal_handle = send_goal_future.result()

if not goal_handle.accepted:
node.get_logger().info('Goal rejected :(')
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
return
if not goal_handle.accepted:
node.get_logger().info('Goal rejected :(')
return

node.get_logger().info('Goal accepted :)')
node.get_logger().info('Goal accepted :)')

get_result_future = goal_handle.get_result_async()
get_result_future = goal_handle.get_result_async()

rclpy.spin_until_future_complete(node, get_result_future)
rclpy.spin_until_future_complete(node, get_result_future)

result = get_result_future.result().result
status = get_result_future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
node.get_logger().info(
'Goal succeeded! Result: {0}'.format(result.sequence))
else:
node.get_logger().info('Goal failed with status code: {0}'.format(status))
except KeyboardInterrupt:
result = get_result_future.result().result
status = get_result_future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
node.get_logger().info(
'Goal succeeded! Result: {0}'.format(result.sequence))
else:
node.get_logger().info('Goal failed with status code: {0}'.format(status))
except (KeyboardInterrupt, ExternalShutdownException):
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys
import time

from example_interfaces.action import Fibonacci
Expand Down Expand Up @@ -92,19 +91,16 @@ async def execute_callback(self, goal_handle):


def main(args=None):
rclpy.init(args=args)

try:
minimal_action_server = MinimalActionServer()
with rclpy.init(args=args):
minimal_action_server = MinimalActionServer()

# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()
# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()

rclpy.spin(minimal_action_server, executor=executor)
except KeyboardInterrupt:
rclpy.spin(minimal_action_server, executor=executor)
except (KeyboardInterrupt, ExternalShutdownException):
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys
import time

from example_interfaces.action import Fibonacci
Expand Down Expand Up @@ -106,19 +105,16 @@ async def execute_callback(self, goal_handle):


def main(args=None):
rclpy.init(args=args)

try:
minimal_action_server = MinimalActionServer()
with rclpy.init(args=args):
minimal_action_server = MinimalActionServer()

# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()
# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()

rclpy.spin(minimal_action_server, executor=executor)
except KeyboardInterrupt:
rclpy.spin(minimal_action_server, executor=executor)
except (KeyboardInterrupt, ExternalShutdownException):
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys
import time

from example_interfaces.action import Fibonacci
Expand Down Expand Up @@ -71,32 +70,30 @@ async def execute_callback(goal_handle):

def main(args=None):
global logger
rclpy.init(args=args)

try:
node = rclpy.create_node('minimal_action_server')
logger = node.get_logger()

# Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
# Default goal callback accepts all goals
# Default cancel callback rejects cancel requests
action_server = ActionServer(
node,
Fibonacci,
'fibonacci',
execute_callback=execute_callback,
cancel_callback=cancel_callback,
callback_group=ReentrantCallbackGroup())
action_server # Quiet flake8 warnings about unused variable

# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()

rclpy.spin(node, executor=executor)
except KeyboardInterrupt:
with rclpy.init(args=args):
node = rclpy.create_node('minimal_action_server')
logger = node.get_logger()

# Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
# Default goal callback accepts all goals
# Default cancel callback rejects cancel requests
action_server = ActionServer(
node,
Fibonacci,
'fibonacci',
execute_callback=execute_callback,
cancel_callback=cancel_callback,
callback_group=ReentrantCallbackGroup())
action_server # Quiet flake8 warnings about unused variable

# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()

rclpy.spin(node, executor=executor)
except (KeyboardInterrupt, ExternalShutdownException):
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Loading