실내 경로 생성 파이썬으로 해보자 (Nav2를 이용한)

우선 가제보 실행 코드

ros2 launch minibot_gazebo bringup_gazebo.launch.py world_name:=simple_building.world

네비게이션 실행코드

 ros2 launch minibot_navigation2 map_building.launch.py use_sim_time:=true

시뮬레이션

ros2 launch minibot_navigation2 rviz_launch.py

로봇을 돌리기

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=base_controller/cmd_vel_unstamped

 

맵을 잘 만들었으니까 

ros2 run nav2_map_server map_saver_cli

이 맵을 Nav2 패키지의 maps에 넣는다.

 

 

 

일단 우리는 존재하는 맵으로 진행합니다. 근데, 만들어진 맵이 있으니까 일단 이걸 쓰자.

ros2 launch minibot_bringup bringup_robot_gazebo.launch.py world_name:=simple_building.world

ros2 launch minibot_navigation2 bringup_launch.py use_sim_time:=true map:=`ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/maps/simple_building.yaml

rviz2 -d `ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/rviz/nav2_view.rviz

 

 

문제는 충돌영역이 너무나도 크다는 문제가 있다. 이 충돌영역을 줄이는 방법을 찾기 위해서는 논문을 찾아보았다. 여튼 chatGPT에 물어봐서 어떤 파일을 수정해야하는지 알아보았다.

일단 쳇지피티가 건들라는 부분을 건들였더니 글로벌 패스의 충돌영역이 줄어들었다.

 

 

우선 minibot 폴더에 navigation2 패키지에 들어가서 params 폴더에 있는 nav2_params.yaml 파일을 건들여야합니다. 여기서 local_costmap은 inflaton_radius를 줄이고

global_costmap은 

cost_scaling_factor와 inflation_radius를 낮추면 줄어든다. 위의 이미지는 이를 줄인 이미지이다.

 

 

 

문제는 맵의 충돌 영역이 많이 줄었지만, 2D Pose Estimmate가 되지 않는 문제가 발생한다. 

 

 

 

 

쳇 GPT가 이야기한것

로봇이 벽에 붙어서 움직이는 것은 로봇의 충돌 체크 기능 때문입니다. 로봇의 충돌 체크 기능이 벽과의 거리를 어느 정도 띄우고 움직일지를 결정합니다.

이를 조절하기 위해서는 로봇의 크기와 충돌 체크 기능의 파라미터를 조정해야 합니다. 로봇의 크기를 늘리면 벽과의 거리가 늘어나므로 벽에 붙어서 움직이는 것을 막을 수 있습니다.

예를 들어, local_costmap의 footprint 파라미터를 로봇의 크기에 맞게 조정하면 됩니다. 이 값이 작으면 로봇이 벽에 붙어서 움직이는 경우가 더 자주 발생할 수 있습니다. 반면에 큰 값으로 설정하면 벽과의 거리를 좀 더 유지하면서 움직일 수 있습니다.

충돌 체크 기능의 파라미터도 조정할 수 있습니다. inflation_radius 값을 늘리면 로봇과 장애물 사이의 거리가 늘어나므로 벽과의 거리도 늘어납니다. local_costmap의 inflation_layer 파라미터에서 이 값을 조정할 수 있습니다.

충돌 체크 기능과 로봇 크기를 조절하면 벽과의 거리를 조절할 수 있습니다. 하지만 이 값을 지나치게 크게 설정하면 로봇이 길을 잃거나 벽을 통과할 수 있으므로 적절한 값을 찾는 것이 중요합니다.

 

결국 아까 조절한 값으로 진행을 해야한다는 것 같고, footprint는 로봇의 크기를 알려주는 것이라고 합니다.

 

 

이쯤하고 이제 파이썬 코드 상에서 경로로 보내는 코드를 작성해봅시다.
파이썬을 다루기 위해서는 다음을 설치해주어야합니다.

sudo apt install python3-pip
pip3 install --upgrade pip
pip3 install jupyter pyyaml bqplot
sudo apt install jupyter-core

여튼 원래 하던 사람이라면 설치가 되어있습니다.

 

여기서 minibot환경을 불러온 후에 (humble도됨)  jupyter notebook을 켭니다.

from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy

rclpy.init()
nav = BasicNavigator()

 

현재 로봇이 움직일 수 있는지 판단을 합니다.

nav.waitUntilNav2Active()

 

움직이고 싶은 좌표값을 찍고 

from geometry_msgs.msg import PoseStamped

goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = nav.get_clock().now().to_msg()
goal_pose.pose.position.x = 1.77
goal_pose.pose.position.x = 0.6
goal_pose.pose.position.x = 0.0
goal_pose.pose.orientation.x = 0.0
goal_pose.pose.orientation.y = 0.0
goal_pose.pose.orientation.z = 0.0
goal_pose.pose.orientation.w = 1.0

 

움직이라는 명령을 보냅니다.

nav.goToPose(goal_pose)

 

 

 

 

 

그리고 그러면 좌표값들을 받아야함으로 좌표값을 뽑는 방법을 알아봅시다

터미널을 하나 켠 후에 ros2 topic list에서 있는 clicked_point 을 구독합니다.

ros2 topic echo /clicked_point

그리고 rviz상에서 좌표를 찍으면 그 좌표를 알 수 있습니다.

publish point로 좌표를 찍으면 됩니다.

여기에 나오는 x, y, z를 오른쪽 지점으로 저장하고

 

총 3곳의 좌표점을 만들었다

# home으로 보내는 위치정보 
goal_home = PoseStamped()
goal_home.header.frame_id = 'map'
goal_home.header.stamp = nav.get_clock().now().to_msg()
goal_home.pose.position.x = -0.011291180737316608
goal_home.pose.position.y= -0.01612980104982853
goal_home.pose.position.z = 0.01239013671875
goal_home.pose.orientation.x = 0.0
goal_home.pose.orientation.y = 0.0
goal_home.pose.orientation.z = 0.0
goal_home.pose.orientation.w = 1.0

 

# 오른쪽으로 보내는 위치정보 
goal_right = PoseStamped()
goal_right.header.frame_id = 'map'
goal_right.header.stamp = nav.get_clock().now().to_msg()
goal_right.pose.position.x = 0.2140897661447525
goal_right.pose.position.y = -2.4429008960723877
goal_right.pose.position.z = -0.001434326171875
goal_right.pose.orientation.x = 0.0
goal_right.pose.orientation.y = 0.0
goal_right.pose.orientation.z = 0.0
goal_right.pose.orientation.w = 1.0

 

# 위로 보내는 위치정보 
goal_up = PoseStamped()
goal_up.header.frame_id = 'map'
goal_up.header.stamp = nav.get_clock().now().to_msg()
goal_up.pose.position.x = 3.3866727352142334
goal_up.pose.position.y = -0.03688139468431473
goal_up.pose.position.z = -0.001434326171875
goal_up.pose.orientation.x = 0.0
goal_up.pose.orientation.y = 0.0
goal_up.pose.orientation.z = 0.0
goal_up.pose.orientation.w = 1.0

 

그러면 위로 보내는 코드와 얼마까지 이행하지 못하면 취소시키는 코드이다.

nav.goToPose(goal_home)

i=0
while not nav.isTaskComplete():
    i = i+1
    feedback = nav.getFeedback()
    if feedback and i % 5 == 0:
        print("Distance remaining : " + str(feedback.distance_remaining) + " m")
    
    if Duration.from_msg(feedback.navigation_time) > Duration(seconds = 10.0):
        nav.cancelTask()

 

 

 

 

만약에 맵을 새로만든다면 가제보를 실행하고

 Edit에서 building Editor을 들어갑니다. 왼쪽에 벽 누르고 그리면 벽을 생성할 수 있습니다.

그 후 파일에서 나가기를 눌러나옵니다. 저장했다고 꺼버리면 다시 만들어야합니다.

가제보가 나오면 파일에서 월드 저장을 한 후에 gazebo에 넣으면 됩니다.

 

 

 

만들었던 맵 이를 활용해보자.

 

map을 새로만들었습니다.

ros2 launch minibot_bringup bringup_robot_gazebo.launch.py world_name:=test_room.world

rviz2 -d `ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/rviz/nav2_view.rviz

ros2 launch minibot_navigation2 bringup_launch.py use_sim_time:=true map:=`ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/maps/jemap.yaml

 

 

시작점은 우리 그.. SLAM을 완성한 후에 조정하는 것으로 확인됩니다. 우선 SLAM부터 완료합시다.

 

 

 

x. 1.0
y 0.0
z 0.0