turtlebot3 initial_pose 수정 및 이동

package에서 nav2_params.yaml
amcl의 parameter을 다음과 같이 수정이 가능함.

근데 이걸 적용시키려면 package를 새로 구축해야하는데
/opt/ros/humble/share/nav2_bringup 의 파일들을 다 가져와서 사용하여도
tb3_simulation_launch가 계속 문제가 생겨서
그냥 패키지를 만들지 않고 python 파일 내에서 initial_pose를 수정하는 topic을 publish함

import rclpy as rp
from ament_index_python.packages import get_package_share_directory
from nav2_simple_commander.robot_navigator import BasicNavigator
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from rclpy.duration import Duration
import time


def main():
    rp.init()

    test_node = rp.create_node('client_test')

    nav = BasicNavigator()
    nav.waitUntilNav2Active()

    cli = test_node.create_publisher(PoseWithCovarianceStamped, '/initialpose', 10)
    msg = PoseWithCovarianceStamped()
    msg.header.frame_id = 'map'
    msg.header.stamp = nav.get_clock().now().to_msg()
    msg.pose.pose.position.x = -2.049304723739624
    msg.pose.pose.position.y = -0.5276116728782654
    msg.pose.pose.position.z = 0.0
    msg.pose.pose.orientation.z = -1.8466898869290566e-06
    msg.pose.pose.orientation.w = 0.9999999999982949

    cli.publish(msg)

    time.sleep(0.1)

    test_node.destroy_node()


    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.8231227397918701
    goal_pose.pose.position.y = 0.3753341734409332
    goal_pose.pose.position.z = -0.005340576171875
    goal_pose.pose.orientation.z = 0.10424110491274381
    goal_pose.pose.orientation.w = 0.9945520559762422

    nav.goToPose(goal_pose)

    i = 0
    while not nav.isTaskComplete():
        i = i + 1
        feedback = nav.getFeedback()
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=10.0):
            nav.cancelTask()
            time.sleep(0.5)
            nav.goToPose(goal_pose)


    goal_pose.header.stamp = nav.get_clock().now().to_msg()
    goal_pose.pose.position.x = 1.807801365852356
    goal_pose.pose.position.y = -0.666404664516449
    goal_pose.pose.position.z = -0.005340576171875
    goal_pose.pose.orientation.z = -0.44920840220179536
    goal_pose.pose.orientation.w = 0.8934270039523711

    nav.goToPose(goal_pose)

    i = 0
    while not nav.isTaskComplete():
        i = i + 1
        feedback = nav.getFeedback()
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=10.0):
            nav.cancelTask()
            time.sleep(0.5)
            nav.goToPose(goal_pose)

    goal_pose.header.stamp = nav.get_clock().now().to_msg()
    goal_pose.pose.position.x = -1.8231227397918701
    goal_pose.pose.position.y = 0.3753341734409332
    goal_pose.pose.position.z = -0.005340576171875

    nav.goToPose(goal_pose)

    i = 0
    while not nav.isTaskComplete():
        i = i + 1
        feedback = nav.getFeedback()
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=10.0):
            nav.cancelTask()
            time.sleep(0.5)
            nav.goToPose(goal_pose)

if __name__ == "__main__":
    main()

수행 영상 중 캡쳐본