아래 명령어로 실행
# terminal 1
ros2 launch neo_simulation2 simulation.launch.py
# terminal 2
ros2 launch neo_simulation2 navigation.launch.py use_amcl:=True
# terminal 3
ros2 launch neo_nav2_bringup rviz_launch.py
nav2_simple_commander패키지의 API를 사용하기 위해서는 AMCL을 사용하고 있어야 합니다.
nav2_ws/src 디렉터리에 nav2_programming 새 패키지를 만들고 토픽 작업에 필요한 종속성을 추가
cd ~/nav2_ws/src
ros2 pkg create --build-type ament_python nav2_programming --dependencies rclpy geometry_msgs nav2_simple_commander
방금 생성한 패키지의 nav2_programming 디렉터리에 navigate_to_pose.py라는 ****새 파일 생성
navigate_to_pose.py에 방금 만든 파일에 다음 코드 복사
#! /usr/bin/env python3
from geometry_msgs.msg import PoseStamped
from rclpy.duration import Duration
import rclpy
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
# Shelf positions for picking
shelf_positions = {
"shelf_A": [11.538, 0.121],
"shelf_B": [9.528, 0.086],
"shelf_C": [7.822, 0.138],
"shelf_D": [5.901, 0.104]}
# Shipping destination for picked products
shipping_destinations = {
"recycling": [-5.405, -8.781],
"pallet_jack7": [12.360, -3.459],
"conveyer_432": [-2.941, -7.078],
"frieght_bay_3": [1.912, 2.069]}
'''
Basic item picking demo. In this demonstration, the expectation
is that a person is waiting at the item shelf to put the item on the robot
and at the pallet jack to remove it
(probably with a button for 'got item, robot go do next task').
'''
def main():
# Recieved virtual request for picking item at Shelf A and bringing to
# worker at the pallet jack 7 for shipping. This request would
# contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7")
####################
request_item_location = 'shelf_B'
request_destination = 'pallet_jack7'
####################
rclpy.init()
navigator = BasicNavigator()
# Set your demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = -4.0
initial_pose.pose.orientation.x = 0.0
initial_pose.pose.orientation.y = 0.0
initial_pose.pose.orientation.z = 0.7070
initial_pose.pose.orientation.w = 0.7070
navigator.setInitialPose(initial_pose)
# Wait for navigation to activate fully
navigator.waitUntilNav2Active()
shelf_item_pose = PoseStamped()
shelf_item_pose.header.frame_id = 'map'
shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg()
shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0]
shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1]
shelf_item_pose.pose.orientation.z = 1.0
shelf_item_pose.pose.orientation.w = 0.0
print('Received request for item picking at ' + request_item_location + '.')
navigator.goToPose(shelf_item_pose)
# Do something during your route
# (e.x. queue up future tasks or detect person for fine-tuned positioning)
# Print information for workers on the robot's ETA for the demonstration
i = 0
while not navigator.isTaskComplete():
i = i + 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Estimated time of arrival at ' + request_item_location +
' for worker: ' + '{0:.0f}'.format(
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
+ ' seconds.')
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Got product from ' + request_item_location +
'! Bringing product to shipping destination (' + request_destination + ')...')
shipping_destination = PoseStamped()
shipping_destination.header.frame_id = 'map'
shipping_destination.header.stamp = navigator.get_clock().now().to_msg()
shipping_destination.pose.position.x = shipping_destinations[request_destination][0]
shipping_destination.pose.position.y = shipping_destinations[request_destination][1]
shipping_destination.pose.orientation.z = 1.0
shipping_destination.pose.orientation.w = 0.0
navigator.goToPose(shipping_destination)
elif result == TaskResult.CANCELED:
print('Task at ' + request_item_location +
' was canceled. Returning to staging point...')
navigator.goToPose(initial_pose)
elif result == TaskResult.FAILED:
print('Task at ' + request_item_location + ' failed!')
exit(-1)
while not navigator.isTaskComplete():
pass
exit(0)
if __name__ == '__main__':
main()
-
nav2_simple_commander API를 선언하고, 인스턴스화하는 부분
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult ... navigator = BasicNavigator() -
로봇의 초기 포즈를 설정하는
setInitialPose()메서드를 사용하고 있음 (메시지는 PoseStamped 이어야 합니다.)navigator.setInitialPose(initial_pose) -
waitUntilNav2Active()메서드는 Nav2가 완전히 온라인 상태가 되고 라이프사이클 노드가 활성 상태가 될 때까지 프로세스 대기navigator.waitUntilNav2Active() -
로봇이 지정된 포즈로 이동하도록 요청하는
goToPose()메서드를 사용 (메시지는 PoseStamped 이어야 합니다.)navigator.goToPose(shelf_item_pose) -
로봇이 목표에 도달한 후에만
isNavComplete()메서드가 True를 반환하고, 아직 진행 중일 때는 False를 반환while not navigator.isTaskComplete(): ... -
getFeedback()메서드는 NavigateToPose 액션 서버의 피드백 반환feedback = navigator.getFeedback() -
getResult()메서드는 NavigateToPose 액션 서버의 결과 반환result = navigator.getResult()
setup.py를 수정하여 navigate_to_pose.py 스크립트의 실행 파일을 entry_points에 추가
from setuptools import find_packages, setup
package_name = 'nav2_programming'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='spoons',
maintainer_email='[email protected]',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
**'navigate_to_pose = nav2_programming.navigate_to_pose:main',**
],
},
)
패키지 빌드
cd ~/nav2_ws
colcon build --symlink-install --packages-select nav2_programming
source ~/nav2_ws/install/local_setup.bash
아래 명령어를 통해 노드를 실행시켜 시나리오 결과 확인
ros2 run nav2_programming navigate_to_pose
-
shelf_B 위치에서 물건을 픽업해 pallet_jack7 위치에 물건을 두는 것 확인 가능
... request_item_location = 'shelf_B' request_destination = 'pallet_jack7' ...