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

Update the shutdown handling in all of the Python examples. #379

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
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,11 +12,14 @@
# 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

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


Expand Down Expand Up @@ -70,11 +73,16 @@ def send_goal(self):
def main(args=None):
rclpy.init(args=args)

action_client = MinimalActionClient()
try:
action_client = MinimalActionClient()

action_client.send_goal()
action_client.send_goal()

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


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

import sys

from example_interfaces.action import Fibonacci

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


Expand Down Expand Up @@ -78,11 +81,16 @@ def send_goal(self):
def main(args=None):
rclpy.init(args=args)

action_client = MinimalActionClient()
try:
action_client = MinimalActionClient()

action_client.send_goal()
action_client.send_goal()

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


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,15 @@
# 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

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


def feedback_cb(logger, feedback):
Expand All @@ -27,50 +30,51 @@ def feedback_cb(logger, feedback):
def main(args=None):
rclpy.init(args=args)

node = rclpy.create_node('minimal_action_client')

action_client = ActionClient(node, Fibonacci, 'fibonacci')
try:
node = rclpy.create_node('minimal_action_client')

node.get_logger().info('Waiting for action server...')
action_client = ActionClient(node, Fibonacci, 'fibonacci')

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

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

node.get_logger().info('Sending goal request...')
goal_msg = Fibonacci.Goal()
goal_msg.order = 10

send_goal_future = action_client.send_goal_async(
goal_msg, feedback_callback=lambda feedback: feedback_cb(node.get_logger(), feedback))
node.get_logger().info('Sending goal request...')

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

goal_handle = send_goal_future.result()
rclpy.spin_until_future_complete(node, send_goal_future)

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

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

get_result_future = goal_handle.get_result_async()
node.get_logger().info('Goal accepted :)')

rclpy.spin_until_future_complete(node, get_result_future)
get_result_future = goal_handle.get_result_async()

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))
rclpy.spin_until_future_complete(node, get_result_future)

action_client.destroy()
node.destroy_node()
rclpy.shutdown()
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:
pass
except ExternalShutdownException:
sys.exit(1)


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

import sys
import time

from example_interfaces.action import Fibonacci

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import ExternalShutdownException
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

Expand Down Expand Up @@ -92,15 +94,17 @@ async def execute_callback(self, goal_handle):
def main(args=None):
rclpy.init(args=args)

minimal_action_server = MinimalActionServer()
try:
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)

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


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

import sys
import time

from example_interfaces.action import Fibonacci

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import ExternalShutdownException
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

Expand Down Expand Up @@ -106,15 +108,17 @@ async def execute_callback(self, goal_handle):
def main(args=None):
rclpy.init(args=args)

minimal_action_server = MinimalActionServer()
try:
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)

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


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

import sys
import time

from example_interfaces.action import Fibonacci

import rclpy
from rclpy.action import ActionServer, CancelResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import ExternalShutdownException
from rclpy.executors import MultiThreadedExecutor


Expand Down Expand Up @@ -71,28 +73,30 @@ def main(args=None):
global logger
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())

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

rclpy.spin(node, executor=executor)

action_server.destroy()
node.destroy_node()
rclpy.shutdown()
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:
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
# limitations under the License.

import collections
import sys
import threading
import time

Expand All @@ -21,6 +22,7 @@
import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import ExternalShutdownException
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

Expand Down Expand Up @@ -123,14 +125,16 @@ def execute_callback(self, goal_handle):
def main(args=None):
rclpy.init(args=args)

minimal_action_server = MinimalActionServer()
try:
minimal_action_server = MinimalActionServer()

executor = MultiThreadedExecutor()
executor = MultiThreadedExecutor()

rclpy.spin(minimal_action_server, executor=executor)

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


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

import sys
import threading
import time

Expand All @@ -20,6 +21,7 @@
import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import ExternalShutdownException
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

Expand Down Expand Up @@ -112,14 +114,16 @@ def execute_callback(self, goal_handle):
def main(args=None):
rclpy.init(args=args)

action_server = MinimalActionServer()
try:
action_server = MinimalActionServer()

# We use a MultiThreadedExecutor to handle incoming goal requests concurrently
executor = MultiThreadedExecutor()
rclpy.spin(action_server, executor=executor)

action_server.destroy()
rclpy.shutdown()
# We use a MultiThreadedExecutor to handle incoming goal requests concurrently
executor = MultiThreadedExecutor()
rclpy.spin(action_server, executor=executor)
except KeyboardInterrupt:
pass
except ExternalShutdownException:
sys.exit(1)


if __name__ == '__main__':
Expand Down
Loading