AS: adding first phase of orientation

This commit is contained in:
Alexander Schaefer
2025-01-29 09:58:44 +00:00
parent 79001dc331
commit 45650caa1b
5106 changed files with 582827 additions and 0 deletions

View File

@@ -0,0 +1,81 @@
# Copyright 2018 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from action_msgs.msg import GoalStatus
from example_interfaces.action import Fibonacci
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
class MinimalActionClient(Node):
def __init__(self):
super().__init__('minimal_action_client')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def feedback_callback(self, feedback):
self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.sequence))
def get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence))
else:
self.get_logger().info('Goal failed with status: {0}'.format(status))
# Shutdown after receiving a result
rclpy.shutdown()
def send_goal(self):
self.get_logger().info('Waiting for action server...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self.get_logger().info('Sending goal request...')
self._send_goal_future = self._action_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def main(args=None):
rclpy.init(args=args)
action_client = MinimalActionClient()
action_client.send_goal()
rclpy.spin(action_client)
if __name__ == '__main__':
main()

View File

@@ -0,0 +1,117 @@
# Copyright 2021 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import asyncio
from action_msgs.msg import GoalStatus
from example_interfaces.action import Fibonacci
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
class MinimalActionClientAsyncIO(Node):
def __init__(self):
super().__init__('minimal_action_client_asyncio')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
def feedback_callback(self, feedback):
self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.sequence))
async def send_goal(self):
self.get_logger().info('Waiting for action server...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self.get_logger().info('Sending goal request...')
goal_handle = await self._action_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_callback
)
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
res = await goal_handle.get_result_async()
result = res.result
status = res.status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence))
else:
self.get_logger().info('Goal failed with status: {0}'.format(status))
return result, status
async def spinning(node):
while rclpy.ok():
rclpy.spin_once(node, timeout_sec=0.01)
await asyncio.sleep(0.001)
async def run(args, loop):
logger = rclpy.logging.get_logger('minimal_action_client')
# init ros2
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()
def main(args=None):
loop = asyncio.get_event_loop()
loop.run_until_complete(run(args, loop=loop))
if __name__ == '__main__':
main()

View File

@@ -0,0 +1,89 @@
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from example_interfaces.action import Fibonacci
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
class MinimalActionClient(Node):
def __init__(self):
super().__init__('minimal_action_client')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
def cancel_done(self, future):
cancel_response = future.result()
if len(cancel_response.goals_canceling) > 0:
self.get_logger().info('Goal successfully canceled')
else:
self.get_logger().info('Goal failed to cancel')
rclpy.shutdown()
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self._goal_handle = goal_handle
self.get_logger().info('Goal accepted :)')
# Start a 2 second timer
self._timer = self.create_timer(2.0, self.timer_callback)
def feedback_callback(self, feedback):
self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.sequence))
def timer_callback(self):
self.get_logger().info('Canceling goal')
# Cancel the goal
future = self._goal_handle.cancel_goal_async()
future.add_done_callback(self.cancel_done)
# Cancel the timer
self._timer.cancel()
def send_goal(self):
self.get_logger().info('Waiting for action server...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self.get_logger().info('Sending goal request...')
self._send_goal_future = self._action_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def main(args=None):
rclpy.init(args=args)
action_client = MinimalActionClient()
action_client.send_goal()
rclpy.spin(action_client)
if __name__ == '__main__':
main()

View File

@@ -0,0 +1,77 @@
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from action_msgs.msg import GoalStatus
from example_interfaces.action import Fibonacci
import rclpy
from rclpy.action import ActionClient
def feedback_cb(logger, feedback):
logger.info('Received feedback: {0}'.format(feedback.feedback.sequence))
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_action_client')
action_client = ActionClient(node, Fibonacci, 'fibonacci')
node.get_logger().info('Waiting for action server...')
action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
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))
rclpy.spin_until_future_complete(node, send_goal_future)
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
node.get_logger().info('Goal accepted :)')
get_result_future = goal_handle.get_result_async()
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))
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

View File

@@ -0,0 +1 @@
# generated from colcon_core/shell/template/command_prefix.sh.em

View File

@@ -0,0 +1,20 @@
AMENT_PREFIX_PATH=/opt/ros/humble
COLCON=1
HOME=/root
HOSTNAME=a41c1d4d4b48
LANG=C.UTF-8
LC_ALL=C.UTF-8
LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
OLDPWD=/root/ros2_ws/src
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
PWD=/root/ros2_ws/build/examples_rclpy_minimal_action_client
PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
ROS_DISTRO=humble
ROS_LOCALHOST_ONLY=0
ROS_PYTHON_VERSION=3
ROS_VERSION=2
SHLVL=1
TERM=xterm
_=/usr/bin/colcon
_colcon_cd_root=/opt/ros/humble/

View File

@@ -0,0 +1 @@
/root/ros2_ws/src/examples/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client

View File

@@ -0,0 +1,19 @@
Metadata-Version: 2.1
Name: examples-rclpy-minimal-action-client
Version: 0.15.3
Summary: Examples of action clients using rclpy.
Home-page: UNKNOWN
Author: Jacob Perron
Author-email: jacob@openrobotics.org
Maintainer: Aditya Pande, Shane Loretz
Maintainer-email: aditya.pande@openrobotics.org, shane@openrobotics.org
License: Apache License, Version 2.0
Keywords: ROS
Platform: UNKNOWN
Classifier: Intended Audience :: Developers
Classifier: License :: OSI Approved :: Apache Software License
Classifier: Programming Language :: Python
Classifier: Topic :: Software Development
UNKNOWN

View File

@@ -0,0 +1,19 @@
package.xml
setup.cfg
setup.py
../../../../../build/examples_rclpy_minimal_action_client/examples_rclpy_minimal_action_client.egg-info/PKG-INFO
../../../../../build/examples_rclpy_minimal_action_client/examples_rclpy_minimal_action_client.egg-info/SOURCES.txt
../../../../../build/examples_rclpy_minimal_action_client/examples_rclpy_minimal_action_client.egg-info/dependency_links.txt
../../../../../build/examples_rclpy_minimal_action_client/examples_rclpy_minimal_action_client.egg-info/entry_points.txt
../../../../../build/examples_rclpy_minimal_action_client/examples_rclpy_minimal_action_client.egg-info/requires.txt
../../../../../build/examples_rclpy_minimal_action_client/examples_rclpy_minimal_action_client.egg-info/top_level.txt
../../../../../build/examples_rclpy_minimal_action_client/examples_rclpy_minimal_action_client.egg-info/zip-safe
examples_rclpy_minimal_action_client/__init__.py
examples_rclpy_minimal_action_client/client.py
examples_rclpy_minimal_action_client/client_asyncio.py
examples_rclpy_minimal_action_client/client_cancel.py
examples_rclpy_minimal_action_client/client_not_composable.py
resource/examples_rclpy_minimal_action_client
test/test_copyright.py
test/test_flake8.py
test/test_pep257.py

View File

@@ -0,0 +1,6 @@
[console_scripts]
client = examples_rclpy_minimal_action_client.client:main
client_asyncio = examples_rclpy_minimal_action_client.client_asyncio:main
client_cancel = examples_rclpy_minimal_action_client.client_cancel:main
client_not_composable = examples_rclpy_minimal_action_client.client_not_composable:main

View File

@@ -0,0 +1 @@
examples_rclpy_minimal_action_client

View File

@@ -0,0 +1,23 @@
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/__init__.py
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/client.py
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/client_asyncio.py
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/client_cancel.py
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/client_not_composable.py
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/__pycache__/__init__.cpython-310.pyc
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/__pycache__/client.cpython-310.pyc
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/__pycache__/client_asyncio.cpython-310.pyc
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/__pycache__/client_cancel.cpython-310.pyc
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client/__pycache__/client_not_composable.cpython-310.pyc
/root/ros2_ws/install/examples_rclpy_minimal_action_client/share/ament_index/resource_index/packages/examples_rclpy_minimal_action_client
/root/ros2_ws/install/examples_rclpy_minimal_action_client/share/examples_rclpy_minimal_action_client/package.xml
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client-0.15.3-py3.10.egg-info/zip-safe
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client-0.15.3-py3.10.egg-info/dependency_links.txt
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client-0.15.3-py3.10.egg-info/top_level.txt
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client-0.15.3-py3.10.egg-info/requires.txt
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client-0.15.3-py3.10.egg-info/entry_points.txt
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client-0.15.3-py3.10.egg-info/PKG-INFO
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/python3.10/site-packages/examples_rclpy_minimal_action_client-0.15.3-py3.10.egg-info/SOURCES.txt
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/examples_rclpy_minimal_action_client/client
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/examples_rclpy_minimal_action_client/client_asyncio
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/examples_rclpy_minimal_action_client/client_cancel
/root/ros2_ws/install/examples_rclpy_minimal_action_client/lib/examples_rclpy_minimal_action_client/client_not_composable

View File

@@ -0,0 +1 @@
/root/ros2_ws/src/examples/rclpy/actions/minimal_action_client/package.xml

View File

@@ -0,0 +1,4 @@
import sys
if sys.prefix == '/usr':
sys.real_prefix = sys.prefix
sys.prefix = sys.exec_prefix = '/root/ros2_ws/install/examples_rclpy_minimal_action_client'

View File

@@ -0,0 +1 @@
/root/ros2_ws/src/examples/rclpy/actions/minimal_action_client/resource/examples_rclpy_minimal_action_client

View File

@@ -0,0 +1 @@
/root/ros2_ws/src/examples/rclpy/actions/minimal_action_client/setup.cfg

View File

@@ -0,0 +1 @@
prepend-non-duplicate;PYTHONPATH;/root/ros2_ws/build/examples_rclpy_minimal_action_client

View File

@@ -0,0 +1,3 @@
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\/root/ros2_ws/build/examples_rclpy_minimal_action_client"

View File

@@ -0,0 +1,3 @@
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
_colcon_prepend_unique_value PYTHONPATH "/root/ros2_ws/build/examples_rclpy_minimal_action_client"