[Unitree GO2] SLAM(Creating Map) not working

Software
21

unitree@go2-unit-41097:/opt/mybotshop/src/mybotshop/go2_navigation/param$ ROS_DOMAIN_ID=10 ros2 run tf2_tools view_frames.py --force-discover --ros-args --remap tf:=/$GO2_NS/tf --ros-args --remap tf_static:=/$GO2_NS/tf_static
[INFO] [0000015788.629641856] [view_frames]: Listening to tf data during 5 seconds…
[INFO] [0000015793.634290848] [view_frames]: Generating graph in frames.pdf file…
[INFO] [0000015793.659543936] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml=“base_link: \n parent: ‘odom’\n broadcaster: ‘default_authority’\n rate: 99.905\n most_recent_transform: 15793.631282\n oldest_transform: 15788.556453\n buffer_length: 5.075\nodom: \n parent: ‘map’\n broadcaster: ‘default_authority’\n rate: 62.714\n most_recent_transform: 15794.625143\n oldest_transform: 15789.522634\n buffer_length: 5.103\nFL_foot: \n parent: ‘FL_calf’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nFL_calf: \n parent: ‘FL_thigh’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nFR_foot: \n parent: ‘FR_calf’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nFR_calf: \n parent: ‘FR_thigh’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nRL_foot: \n parent: ‘RL_calf’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nRL_calf: \n parent: ‘RL_thigh’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nRR_foot: \n parent: ‘RR_calf’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nRR_calf: \n parent: ‘RR_thigh’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\ncamera_link: \n parent: ‘base_link’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\ncomputer_dock_link: \n parent: ‘base_link’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nend_effector_link: \n parent: ‘link5’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nlink5: \n parent: ‘link4’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\ntrunk: \n parent: ‘base_link’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nfront_top_mount: \n parent: ‘utlidar_lidar’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nutlidar_lidar: \n parent: ‘base_link’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nimu_link: \n parent: ‘trunk’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nlidar_link: \n parent: ‘base_link’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nopen_manipulator_base_bracket: \n parent: ‘front_top_mount’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nopen_manipulator_bracket: \n parent: ‘front_top_mount’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nlink1: \n parent: ‘front_top_mount’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nsteamdeck_bracket_link: \n parent: ‘computer_dock_link’\n broadcaster: ‘default_authority’\n rate: 10000.000\n most_recent_transform: 0.000000\n oldest_transform: 0.000000\n buffer_length: 0.000\nFL_thigh: \n parent: ‘FL_hip’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nFL_hip: \n parent: ‘trunk’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nFR_thigh: \n parent: ‘FR_hip’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nFR_hip: \n parent: ‘trunk’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nRL_thigh: \n parent: ‘RL_hip’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nRL_hip: \n parent: ‘trunk’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nRR_thigh: \n parent: ‘RR_hip’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nRR_hip: \n parent: ‘trunk’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\ngripper_link: \n parent: ‘link5’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\ngripper_link_sub: \n parent: ‘link5’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nlink2: \n parent: ‘link1’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nlink3: \n parent: ‘link2’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\nlink4: \n parent: ‘link3’\n broadcaster: ‘default_authority’\n rate: 25.197\n most_recent_transform: 15793.594306\n oldest_transform: 15788.553933\n buffer_length: 5.040\n”)

22

After a restart is /tf no more there, but the situation with map is same here. You may check the following

unitree@go2-unit-41097:/opt/mybotshop/src/mybotshop/go2_navigation/param$ ros2 topic info /map -v
Type: nav_msgs/msg/OccupancyGrid

Publisher count: 1

Node name: map_server
Node namespace: /
Topic type: nav_msgs/msg/OccupancyGrid
Endpoint type: PUBLISHER
GID: 01.10.a2.19.3c.b9.2b.bb.54.cf.ae.48.00.00.22.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Subscription count: 2

Node name: global_costmap
Node namespace: /global_costmap
Topic type: nav_msgs/msg/OccupancyGrid
Endpoint type: SUBSCRIPTION
GID: 01.10.5e.d3.a0.7b.bd.66.58.da.0f.09.00.00.6b.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Node name: amcl
Node namespace: /
Topic type: nav_msgs/msg/OccupancyGrid
Endpoint type: SUBSCRIPTION
GID: 01.10.c8.54.39.e1.4c.32.23.88.77.bd.00.00.39.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

unitree@go2-unit-41097:/opt/mybotshop/src/mybotshop/go2_navigation/param$ ros2 topic info /go2_unit_41097/map -v
Type: nav_msgs/msg/OccupancyGrid

Publisher count: 0

Subscription count: 1

Node name: go2_webserver
Node namespace: /go2_unit_41097
Topic type: nav_msgs/msg/OccupancyGrid
Endpoint type: SUBSCRIPTION
GID: 01.10.8a.d3.47.d1.8e.d3.21.c3.e1.55.00.00.16.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds
23

I post what I can see here

unitree@go2-unit-41097:/opt/mybotshop/src/mybotshop/go2_navigation/param$ ros2 topic info /go2_unit_41097/tf_static -v
Type: tf2_msgs/msg/TFMessage

Publisher count: 1

Node name: robot_state_publisher
Node namespace: /go2_unit_41097
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: PUBLISHER
GID: 01.10.f3.a8.0b.dd.e1.0f.1d.04.78.16.00.00.16.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Subscription count: 7

Node name: transform_listener_impl_ffff8c01d5b0
Node namespace: /
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: SUBSCRIPTION
GID: 01.10.05.3d.41.1e.63.1e.d1.08.a6.80.00.00.32.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Node name: transform_listener_impl_aaaaf8ba0778
Node namespace: /go2_unit_41097
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: SUBSCRIPTION
GID: 01.10.29.13.9d.b9.e1.37.0f.f4.bf.f5.00.00.18.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Node name: transform_listener_impl_aaab10f89140
Node namespace: /
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: SUBSCRIPTION
GID: 01.10.5e.d3.a0.7b.bd.66.58.da.0f.09.00.00.6a.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Node name: bt_navigator
Node namespace: /
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: SUBSCRIPTION
GID: 01.10.6c.3f.71.f4.c5.07.46.e0.6d.a6.00.00.38.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Node name: go2_webserver
Node namespace: /go2_unit_41097
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: SUBSCRIPTION
GID: 01.10.8a.d3.47.d1.8e.d3.21.c3.e1.55.00.00.1b.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Node name: transform_listener_impl_aaaae9dc0950
Node namespace: /
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: SUBSCRIPTION
GID: 01.10.8d.34.ef.da.f9.2c.b6.70.9a.f3.00.00.6a.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Node name: transform_listener_impl_ffff9802af70
Node namespace: /
Topic type: tf2_msgs/msg/TFMessage
Endpoint type: SUBSCRIPTION
GID: 01.10.c8.54.39.e1.4c.32.23.88.77.bd.00.00.32.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  Durability: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

And an example part of echo from the topic:

- header:
    stamp:
      sec: 16341
      nanosec: 404077792
    frame_id: base_link
  child_frame_id: camera_link
  transform:
    translation:
      x: 0.29
      y: 0.0
      z: -0.08
    rotation:
      x: 0.0
      y: 0.706825181105366
      z: 0.0
      w: 0.7073882691671998
- header:
    stamp:
      sec: 16341
      nanosec: 404080096
    frame_id: base_link
  child_frame_id: computer_dock_link
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: -0.057
    rotation:
      x: 0.0
      y: 0.0
      z: 0.706825181105366
      w: 0.7073882691671998
- header:
    stamp:
      sec: 16341
      nanosec: 404083328
    frame_id: link5
  child_frame_id: end_effector_link
  transform:
    translation:
      x: 0.126
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 16341
      nanosec: 404084736
    frame_id: base_link
  child_frame_id: trunk
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 16341
      nanosec: 404086176
    frame_id: utlidar_lidar
  child_frame_id: front_top_mount
  transform:
    translation:
      x: 0.15
      y: 0.0
      z: -0.15
    rotation:
      x: 0.0007963264582434141
      y: 0.9999993658637698
      z: 0.0007963264582434141
      w: 6.341362302272584e-07
- header:
    stamp:
      sec: 16341
      nanosec: 404095712
    frame_id: trunk
  child_frame_id: imu_link
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 16341
      nanosec: 404096864
    frame_id: base_link
  child_frame_id: lidar_link
  transform:
    translation:
      x: 0.29
      y: 0.0
      z: -0.08
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 16341
      nanosec: 404097696
    frame_id: front_top_mount
  child_frame_id: open_manipulator_base_bracket
  transform:
    translation:
      x: 0.032
      y: 0.0
      z: 0.007
    rotation:
      x: 0.0
      y: 0.0
      z: 0.7071080798594737
      w: 0.7071054825112364
- header:
    stamp:
      sec: 16341
      nanosec: 404099360
    frame_id: front_top_mount
  child_frame_id: open_manipulator_bracket
  transform:
    translation:
      x: 0.042
      y: 0.0
      z: 0.021
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 16341
      nanosec: 404100736
    frame_id: front_top_mount
  child_frame_id: link1
  transform:
    translation:
      x: 0.0025
      y: 0.0
      z: 0.01
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 16341
      nanosec: 404101568
    frame_id: computer_dock_link
25

After scanning the map, saving to the map directory and colcon build again. I started the navigation here ROS_DOMAIN_ID=10 GO2_NS=go2_unit_41097 ros2 launch go2_navigation map_navi.launch.py map:=/opt/mybotshop/src/mybotshop/go2_navigation/maps/ais_test.yaml with this specified map name. and now I can see in rviz the scanned map with rviz2 --ros-args -r /tf:=/go2_unit_41097/tf -r /tf_static:=/go2_unit_41097/tf_static and set the durability policy in map as “transient local”. However the robot model is not available and navigation(using 2D Pose Estimate to localize and 2D Goal Pose to set goal) is also not working there(The description Topic is set to /go2_unit_41097/robot_description)

image
image1753×1157 385 KB

26

Please post the image of the rqt graph it should be some sort of tree, the commands for it are written in the repository.