官方其实提供了自动导航方案了，https://www.originbot.org/application/navigation/#4。

但是这个方案里最不好的地方在于需要通过rviz手动指定具体的目标点和姿势才能驱动OriginBot进行导航，也就是手动的自动导航，这个就限制了后续基于这个功能做拓展的可能，因为总不能每次需要导航的时候都手动在rviz上做吧。

所以我就想用代码来实现这一块功能，以下是目前探索出来的部分，欢迎交流。

要用代码实现自动导航，有以下2个步骤：

1. 要知道导航到哪里，也就是目标点
2. 要有一个办法把目标点发送给小车

如何获取目标点

有两个办法获取目标点，一个是在小车运行的时候打印出目标点，还有一个就是通过initialpose这个topic来获取。

1. 在小车运行的时候打印目标点
这个方法我是早网上找资料的时候看到的，但我并没有实际使用，所以代码不能保证完全正确，但是逻辑上来说应该可以工作的。

import rclpy
from tf2_ros import LookupException, ConnectivityException, ExtrapolationException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('pose_listener')
    tf_buffer = Buffer()
    tf_listener = TransformListener(tf_buffer, node)

    try:
        # The second argument is the target frame.
        # The first argument is the source frame.
        trans = tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time())
    except (LookupException, ConnectivityException, ExtrapolationException):
        print("Error occurred while getting TF transform.")
    else:
        print(f"Position: {trans.transform.translation}")
        print(f"Rotation: {trans.transform.rotation}")

    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

2. 通过initialpose订阅

之所以不用上面那个方法来获取目标点，是因为一定要小车运行到特定的位置才能知道这个位置的坐标和姿势，比较麻烦。

initialpose这个topic其实是用于设置机器人的初始位置的，但是也可以用来获取目标点。

使用这个方法有一个前提，就是已经绘制好了SLAM地图。

步骤如下：

# 启动底盘
ros2 launch originbot_bringup originbot.launch.py use_lidar:=true
# 在originbot_desktop上启动rviz
ros2 launch originbot_viz display_navigation.launch.py
# 在小车上启动自动导航功能
ros2 launch originbot_navigation nav_bringup.launch.py
# 在originbot_desktop上另开一个shell打印initialpose内容
ros2 topic echo initialpose

接下来通过rviz上的“2D Estimate”功能在地图上选中一个目标点和方向，然后在ros2 topic echo initialpose 这个shell窗口就会输出相应的坐标点和姿势。

把目标点发送给小车

按照上面的方法，可以事先获取几个目标点，接下来就是要把这些目标点发送给小车了。

小车通过ros2 launch originbot_navigation nav_bringup.launch.py启动了自动导航功能，实际上是在小车上启动了一个ActionServer，我们可以通过一个ActionClient来发送目标点和姿势给小车。

在小车上创建一个文件，名为send_goal.py，具体代码如下：

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.action.server import GoalStatus
from rclpy.callback_groups import ReentrantCallbackGroup
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose


class GoalCoordinate(Node):
    def __init__(self):
        super().__init__('goal_coordinate')

        self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')

        self.send_goal()

    def send_goal(self):
        """
        #TODO: 应该把目标点和姿势改成以参数的形式接收，这边方便外部调用
        """
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.pose.position.x = 2.742
        goal_msg.pose.pose.position.y = -0.173
        goal_msg.pose.pose.orientation.w = 0.035

        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 goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('Goal request rejected by server!')
        else:
            self.get_logger().info('Goal accepted by server, waiting for result')
            self._get_result_future = goal_handle.get_result_async()
            self._get_result_future.add_done_callback(self.result_callback)

    def feedback_callback(self, feedback_message):
        feedback = feedback_message.feedback
        self.get_logger().info(
            f'Remaining Distance from Destination: {feedback.distance_remaining}'
        )

    def result_callback(self, future):
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Goal succeeded')
        else:
            self.get_logger().error(f'Failed with status code: {status}')


def main(args=None):
    rclpy.init(args=args)

    client = GoalCoordinate()

    rclpy.spin(client)


if __name__ == '__main__':
    main()

上面这个代码不需要编译，直接在命令行中运行python3 send_goal.py即可。

目前代码还比较简单，只是在逻辑上实现了自动发送目标点，还有很多可以优化的地方，比如代码中的目标点是hard-coding的，可以改成监听一个topic或者api的方式来获取，