ROS2 네비게이션 패키지 생성
네비게이션 패키지 생성
cd ~/robot_ws/src 에서
ros2 pkg create nav_pkg --build-type ament_python --dependencies rclpy
cd nav_pkg/nav_pkg 에서 follow_waypoints.py
import rclpy, sys,os
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from rclpy.action import ActionClient
from action_msgs.msg import GoalStatus
from nav2_msgs.action import FollowWaypoints
class ClientFollowPoints(Node):
def __init__(self):
super().__init__('client_follow_points')
self._client = ActionClient(self, FollowWaypoints, '/FollowWaypoints')
def send_points(self, points):
msg = FollowWaypoints.Goal()
msg.poses = points
self._client.wait_for_server()
self._send_goal_future = self._client.send_goal_async(msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
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 get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
print("-----------------------------")
print("arrived at goal position!!!!!")
print("-----------------------------")
#os.system("ros2 param set /reg_params go2wp2 go")
sys.exit(1)
self.get_logger().info('Result: {0}'.format(result.missed_waypoints))
def feedback_callback(self, feedback_msg):
feedback = feedback_msg.feedback
self.get_logger().info('Received feedback: {0}'.format(feedback.current_waypoint))
def main(args=None):
rclpy.init(args=args)
node = ClientFollowPoints()
print('client inited')
rgoal = PoseStamped()
rgoal.header.frame_id = "map"
rgoal.header.stamp.sec = 0
rgoal.header.stamp.nanosec = 0
rgoal.pose.position.z = 0.0
rgoal.pose.position.x = 2.85
rgoal.pose.position.y = 2.64
rgoal.pose.orientation.w = 1.0
print(rgoal)
mgoal = [rgoal]
node.send_points(mgoal)
rclpy.spin(node)
cd .. 해서 setup.py에서 ‘follow_waypoints = nav_pkg.follow_waypoints:main’,
entry_points={
'console_scripts': [
'follow_waypoints = nav_pkg.follow_waypoints:main',
],
},
당연히 빌드 하고, 소스 하고,
확인해보자
gazebo 실행
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
navigation 실행
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=$HOME/map.yaml
특정 포인트로 가도록 짠 코드 실행
ros2 run nav_pkg follow_waypoints
좌표 알아내기
RViz 에 Publish Point 클릭 후 좌표를 갖다 대면 좌측 하단 숫자 배열을 볼 수 있다.
follow_waypoints.py를 복사해서 follow_waypoints1.py, follow_waypoints2.py, follow_waypoints3.py, follow_waypoints4.py 로 만들어봤다.
그리고나서 약 61, 62째 줄에
rgoal.pose.position.x = 2.94
rgoal.pose.position.y = -0.966
에 숫자를 바꾼다.
그리고 setup.py 에도 해야 할 일을 한다.
entry_points={
'console_scripts': [
'follow_waypoints1 = nav_pkg.follow_waypoints1:main',
'follow_waypoints2 = nav_pkg.follow_waypoints2:main',
'follow_waypoints3 = nav_pkg.follow_waypoints3:main',
'follow_waypoints4 = nav_pkg.follow_waypoints4:main',
],
},
빌드 잘 하고 위에서 터미널에서 했던 거 gazebo 실행, navigation 잘 하고,
follow_waypoints1, follow_waypoints2, follow_waypoints3, follow_waypoints4 중에 멋대로 하면 됨
ros2 run nav_pkg follow_waypoints1
숫자 입력해서 String msg의 data 글자 바꾸는 코드
cd nav_pkg 에 pub_nav_msg.py
(getchar.py 도 같은 폴더에 넣어둬야 import Getchar 가능)
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from .getchar import Getchar
class PubNavMsg(Node):
def __init__(self):
super().__init__('pub_nav_msg')
def main(args=None):
rclpy.init(args=args)
node = PubNavMsg()
str_msg = String()
kb = Getchar()
pub = node.create_publisher(String, '/nav_msg', 10)
key = ' '
try:
while rclpy.ok():
key = kb.getch()
if key == '1':
str_msg.data = "point1"
elif key == '2':
str_msg.data = "point2"
elif key == '3':
str_msg.data = "point3"
elif key == '4':
str_msg.data = "point4"
else:
pass
pub.publish(str_msg)
print(str_msg.data)
except KeyboardInterrupt:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
cd .. 해서 setup.py에 ‘pub_nav_msg = nav_pkg.pub_nav_msg:main’,
entry_points={
'console_scripts': [
'pub_nav_msg = nav_pkg.pub_nav_msg:main',
'follow_waypoints1 = nav_pkg.follow_waypoints1:main',
'follow_waypoints2 = nav_pkg.follow_waypoints2:main',
'follow_waypoints3 = nav_pkg.follow_waypoints3:main',
'follow_waypoints4 = nav_pkg.follow_waypoints4:main',
],
},
당연히 빌드 하고 소스 하고
ros2 run nav_pkg pub_nav_msg
새로운 터미널에서
ros2 topic list
ros2 topic echo /nav_msg
기존 터미널에서 키보드로 1,2,3,4 중 숫자를 치면
String msg 의 data 글자가 변하는 것을 볼 수 있다.
숫자 입력해서 특정 위치에 바로 가도록 하는 subscription 코드
cd nav_pkg 에서 sub_n_go.py
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from rclpy.action import ActionClient
from action_msgs.msg import GoalStatus
from nav2_msgs.action import FollowWaypoints
# from rclpy.duration import Duration # Handles time for ROS 2
from rclpy.qos import QoSProfile
from std_msgs.msg import String
class Sub_n_Follow(Node):
def __init__(self):
super().__init__('sub_n_follow')
self._client = ActionClient(self, FollowWaypoints, '/FollowWaypoints')
qos_profile = QoSProfile(depth=10)
self.create_subscription( String, '/nav_msg', self.get_nav_msg, qos_profile )
self.nav_msg = String()
def send_points(self, points):
msg = FollowWaypoints.Goal()
msg.poses = points
self._client.wait_for_server()
self._send_goal_future = self._client.send_goal_async(msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
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 get_result_callback(self, future):
result = future.result().result
self.get_logger().info('Result: {0}'.format(result.missed_waypoints))
def feedback_callback(self, feedback_msg):
feedback = feedback_msg.feedback
self.get_logger().info('Received feedback: {0}'.format(feedback.current_waypoint))
def get_nav_msg(self, msg):
self.nav_msg = msg
def main(args=None):
rclpy.init(args=args)
node = Sub_n_Follow()
rgoal = PoseStamped()
try:
while rclpy.ok():
rclpy.spin_once(node, timeout_sec = 0.1)
if node.nav_msg.data == "point1":
rgoal.header.frame_id = "map"
rgoal.header.stamp.sec = 0
rgoal.header.stamp.nanosec = 0
rgoal.pose.position.z = 0.0
rgoal.pose.position.x = 2.85
rgoal.pose.position.y = 2.64
rgoal.pose.orientation.w = 1.0
elif node.nav_msg.data == "point2":
rgoal.header.frame_id = "map"
rgoal.header.stamp.sec = 0
rgoal.header.stamp.nanosec = 0
rgoal.pose.position.z = 0.0
rgoal.pose.position.x = 1.7
rgoal.pose.position.y = 2.66
rgoal.pose.orientation.w = 1.0
elif node.nav_msg.data == "point3":
rgoal.header.frame_id = "map"
rgoal.header.stamp.sec = 0
rgoal.header.stamp.nanosec = 0
rgoal.pose.position.z = 0.0
rgoal.pose.position.x = 1.71
rgoal.pose.position.y = -0.966
rgoal.pose.orientation.w = 1.0
elif node.nav_msg.data == "point4":
rgoal.header.frame_id = "map"
rgoal.header.stamp.sec = 0
rgoal.header.stamp.nanosec = 0
rgoal.pose.position.z = 0.0
rgoal.pose.position.x = 2.94
rgoal.pose.position.y = -0.966
rgoal.pose.orientation.w = 1.0
else:
pass
print(rgoal)
mgoal = [rgoal]
node.send_points(mgoal)
except KeyboardInterrupt:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
cd .. 해서 setup.py에 ‘sub_n_go = nav_pkg.sub_n_go:main’,
entry_points={
'console_scripts': [
'pub_nav_msg = nav_pkg.pub_nav_msg:main',
'sub_n_go = nav_pkg.sub_n_go:main',
'follow_waypoints1 = nav_pkg.follow_waypoints1:main',
'follow_waypoints2 = nav_pkg.follow_waypoints2:main',
'follow_waypoints3 = nav_pkg.follow_waypoints3:main',
'follow_waypoints4 = nav_pkg.follow_waypoints4:main',
],
},
당연히 빌드하고 소스하고
1번 터미널
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
2번 터미널
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=$HOME/map.yaml
3번 터미널
ros2 run nav_pkg pub_nav_msg
4번 터미널
ros2 run nav_pkg sub_n_go
3번 터미널에서 숫자 1, 2, 3, 4 중에 키보드 눌러서 navigation RViz 에서 로봇이 움직이는지 확인
Comments