ros学习（7）拓扑导航

基本概念：参考课程ppt

workshop:

1. 基本拓扑导航建立

[

xxx.py

No module name topological_navigation_msgs

sudo apt install ros-melodic-topological-navigation-msgs

]

sudo apt-get install \
    ros-melodic-topological-utils \
    ros-melodic-topological-navigation \
    ros-melodic-topological-navigation-msgs \
    ros-melodic-strands-navigation

Create a folder (named mongodb) in your user home directory(#home 目录：/home/xumin)

• roslaunch bacchus_gazebo vineyard_demo.launch
• roslaunch uol_cmp9767m_tutorial topo_nav.launch
• You will see some warnings in the terminal where you launched topo_nav.launch saying the pointset is not found in the message_store. This is because we haven't loaded the topological map to the mongodb yet. Once you do the next step, that warning should stop.
• rosrun topological_utils load_yaml_map.py $(rospack find uol_cmp9767m_tutorial)/maps/test.yaml #This step is required only once.
• rviz -d $(rospack find uol_cmp9767m_tutorial)/config/topo_nav.rviz
• click the green arrows at the nodes seen in RVIZ to send topological_navigation goals to the robot.

2. Map Modifications

直接打开test.yaml，添加自己的定义的拓扑点

#node: WayPoint6

#WayPoint0_WayPoint6（ending point=WayPoint0）：

- meta:
    map: test
    node: WayPoint6
    pointset: test
  node:
    edges:
    - action: move_base
      edge_id: WayPoint0_WayPoint6
      inflation_radius: 0.0
      map_2d: cropped
      node: WayPoint0
      recovery_behaviours_config: ''
      top_vel: 0.55

    localise_by_topic: ''
    map: test
    name: WayPoint6
    pointset: test
    pose:
      orientation:
        w: 1.0
        x: 0.0
        y: 0.0
        z: 0.0
      position:
        x: -2.0
        y: -2.0
        z: 0.0
    verts:
    - x: 0.689999997616
      y: 0.287000000477
    - x: 0.287000000477
      y: 0.689999997616
    - x: -0.287000000477
      y: 0.689999997616
    - x: -0.689999997616
      y: 0.287000000477
    - x: -0.689999997616
      y: -0.287000000477
    - x: -0.287000000477
      y: -0.689999997616
    - x: 0.287000000477
      y: -0.689999997616
    - x: 0.689999997616
      y: -0.287000000477
    xy_goal_tolerance: 0.3
    yaw_goal_tolerance: 0.2

如果需要双向，需要在node: WayPoint0中添加：

#WayPoint0_WayPoint6（ending point=WayPoint6）：

- action: move_base
      edge_id: WayPoint0_WayPoint6
      inflation_radius: 0.0
      map_2d: cropped
      node: WayPoint6
      recovery_behaviours_config: ''
      top_vel: 0.55

之后删除home目录下的mongodb文件夹，同时ctrl+c终止roslaunch uol_cmp9767m_tutorial topo_nav.launch，

之后重新执行：

• rosrun topological_utils load_yaml_map.py $(rospack find uol_cmp9767m_tutorial)/maps/test.yaml #This step is required only once.
• rviz -d $(rospack find uol_cmp9767m_tutorial)/config/topo_nav.rviz

就会看到自己定义的拓扑点并可执行导航。

3. Action Client

In this task, you will create an action client that can send goals to the robot's topological navigation action.

#! /usr/bin/env python
# ----------------------------------
# @author: gpdas
# @email: pdasgautham@gmail.com
# @date:
# ----------------------------------

import rospy
import actionlib

from topological_navigation.msg import GotoNodeAction, GotoNodeGoal

if __name__ == '__main__':
    rospy.init_node('topological_navigation_client')
    client = actionlib.SimpleActionClient('/thorvald_001/topological_navigation', GotoNodeAction)
    client.wait_for_server()

    # send first goal
    goal = GotoNodeGoal()
    goal.target = "WayPoint1"
    client.send_goal(goal)
    status = client.wait_for_result() # wait until the action is complete
    result = client.get_result()
    rospy.loginfo("status is %s", status)
    rospy.loginfo("result is %s", result)

    # send second goal
    goal.target = "WayPoint5"
    # Fill in the goal here
    client.send_goal(goal)
    status = client.wait_for_result() # wait until the action is complete
    result = client.get_result()
    rospy.loginfo("status is %s", status)
    rospy.loginfo("result is %s", result)