[Unitree Go1] [RP Lidar] Odom and Lidar Integration Issues

I hope this message finds you well. I am writing to seek assistance with an issue we are currently facing regarding the integration of the odometer and LiDAR systems in our setup.

We are using the rplidar, which is publishing values with the frame ID set to ‘laser’ under the topic name ‘/scan’. We have encountered a couple of challenges, as detailed below:

Static Transformation Issue:
We initially attempted to connect the sensor using the static_transform_publisher. For this, we commented out the ouster.launch.xml part in map_navi.launch and added the following node:
<node pkg="tf" type="static_transform_publisher" name="lidar_to_robot" args="0.05 0 0.08 0 0 0 /base /laser 100" />
However, we observed that when the frame is set to ‘base’ or ‘odom’ in RViz, the /scan values are not visible. These values can only be checked when the frame is set to ‘laser’. The TF tree confirms that ‘odom’, ‘base’, and ‘laser’ are connected.

Xacro Connection Issue:
Next, we connected the laser to a joint under the trunk using xacro. In this configuration, the /scan value is visible when the frame is set to ‘laser’, ‘trunk’, or ‘base’ in RViz. Unfortunately, when the frame is set to ‘odom’, the /scan value disappears. The TF tree again shows that ‘odom’, ‘base’, ‘trunk’, and ‘laser’ are connected.

It appears that the issue of not receiving laser scan messages might be related to the connection not being properly established with ‘odom’. Could you please provide guidance on how we might resolve these issues to ensure proper linkage and visibility of scan values across all relevant frames?

Thank you very much for your time and assistance. We look forward to your expert advice on this matter.

Warm regards,
Hayden

Dear @Hayden,

It is difficult to debug without information.

Please provide the following information

OS: — (e.g. Ubuntu 20.04)
ROS Distro: —
Built from source or installed: —
Package/SDK version: — (if from the repository, give version)
Real hardware or simulation: —
Robot Type: —
Sensor Type: —

Expected behavior

A clear and concise description of what you expected to happen.

Actual behavior

A clear and concise description of what you encountered.

To Reproduce

Provide the steps to reproduce:

  • run something
  • launch something else
  • see the error
  • Other notes


On first glance:

This should work, please use rqt to first ensure that you can see that the base and laser frames are connected in the tf_trees.

It seems that the odom tf is not being published correctly. This seems to be a weird issue, please increase the publish_rate of the odom tf. Furthermore, this issue might be from the rp lidar driver, please post the launch file and its configuration

Ideally, post all the relevant code and pictures so that we can help debug!

Please provide the following information

OS: Ubuntu 20.04
ROS Distro: noetic
Built from source or installed: Establishment of qre_go1 workspace on ubuntu20.04 noetic PC.
Install and run the RPLiDAR package on Go1 raspberrypi (18.04 / melodic).
host: 192.168.12.1 and linked with Go1 using WiFi.
Package/SDK version: qre_go1 / noetic-robotic
Real hardware or simulation: real hardware
Robot Type: GO1
Sensor Type: RPLiDAR A1

Expected behavior
The tf tree connection is expected to be formed as follows.
map - odom - base - trunk - scan
And amcl and move base packages are expected to work.

Actual behavior

However, map nodes are not created and laser scan data is not connected to odom.
Even if the sensor is connected using static_transform_publisher, it appears to be connected to the tf tree of rqt, but data is not transmitted to odom.

To Reproduce
roslaunch go1_bringup bringup.launch
roslaunch go1_navigation map_navi.launch


@Hayden,

Please verify if there are name_spaces in the go1 launch files that you are running as that changes the tf prefix. Incase you are using the go1_om repository then simply add the same namespace to the rplidar as well.

I don’t understand what you mean by adding a name space.
On Go1 RaspberryPi :
roslaunch rplidar_a1 Run rplidar_a1.launch
rplidar_a1.launch is as follows:

<launch>
   <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
   <param name="serial_port" type="string" value="/dev/ttyUSB4"/>
   <param name="serial_baudrate" type="int" value="115200"/>
   <param name="frame_id" type="string" value="laser"/>
   <param name="inverted" type="bool" value="false"/>
   <param name="angle_compensate" type="bool" value="true"/>
   </node>
</launch>

On PC:
roslaunch go1_bringup bringup.launch
roslaunch go1_navigation map_navi.launch

Where and how do I add a name space?
And what is go1_om repository?

I meant go1-melodic-robotis branch. Please post your ros topic list

I ran rosnode kill -a in the go1 raspberrypi terminal.
Configure ubuntu20.04 and noetic docker on your PC and run the qre_go1 (branch: noetic-robotis) package in a container.

The rostopic list is as follows.
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
/battery_state
/camera5/point_cloud_rearDown
/clicked_point
/diagnostics
/go1_controller/cmd_vel
/go1_controller/imu/data
/go1_controller/odom
/go1_controller/state
/initialpose
/joint_states
/local_ekf/set_pose
/map
/map_metadata
/map_updates
/motor_states
/move_base/current_goal
/move_base/goal
/move_base/recovery_status
/move_base_quadruped/cmd_vel
/move_base_simple/goal
/odometry/filtered
/particlecloud
/rosout
/rosout_agg
/scan
/tf
/tf_static

Dear @Hayden,


Please disable the ekf_localization in the map_navi and check. It might be the case that there are 2 ekf localization modules running! Also please post the ros node list and the info of the ekf nodes.

The launch file code is below.

–rosnode list--------------------------------------
/amcl
/camera5
/go1_controller
/local_ekf/ekf_localization_with_odom
/map_server
/move_base
/robot_state_publisher
/rosout
/rplidarNode

–rosnode info /local_ekf/ekf_localization_with_odom-----------------------------------------
Node [/local_ekf/ekf_localization_with_odom]
Publications:

  • /diagnostics [diagnostic_msgs/DiagnosticArray]
  • /odometry/filtered [nav_msgs/Odometry]
  • /rosout [rosgraph_msgs/Log]
  • /tf [tf2_msgs/TFMessage]

Subscriptions:

  • /go1_controller/odom [nav_msgs/Odometry]
  • /local_ekf/set_pose [unknown type]
  • /tf [tf2_msgs/TFMessage]
  • /tf_static [tf2_msgs/TFMessage]

Services:

  • /local_ekf/ekf_localization_with_odom/enable
  • /local_ekf/ekf_localization_with_odom/get_loggers
  • /local_ekf/ekf_localization_with_odom/set_logger_level
  • /local_ekf/ekf_localization_with_odom/toggle
  • /local_ekf/set_pose

contacting node http://rga-dell:42035/
Pid: 898
Connections:

  • topic: /rosout
    • to: /rosout
    • direction: outbound (44929 - 192.168.12.1:46578) [11]
    • transport: TCPROS
  • topic: /tf
    • to: /local_ekf/ekf_localization_with_odom
    • direction: outbound
    • transport: INTRAPROCESS
  • topic: /tf
    • to: /amcl
    • direction: outbound (44929 - 127.0.0.1:59748) [14]
    • transport: TCPROS
  • topic: /tf
    • to: /move_base
    • direction: outbound (44929 - 127.0.0.1:33340) [16]
    • transport: TCPROS
  • topic: /tf
    • to: /local_ekf/ekf_localization_with_odom (http://rga-dell:42035/)
    • direction: inbound
    • transport: INTRAPROCESS
  • topic: /tf
    • to: /robot_state_publisher (http://rga-dell:40067/)
    • direction: inbound (40998 - rga-dell:44577) [12]
    • transport: TCPROS
  • topic: /tf
  • topic: /tf_static
    • to: /robot_state_publisher (http://rga-dell:40067/)
    • direction: inbound (41018 - rga-dell:44577) [10]
    • transport: TCPROS
  • topic: /go1_controller/odom
    • to: /go1_controller (http://rga-dell:44875/)
    • direction: inbound (58348 - rga-dell:57487) [13]
    • transport: TCPROS

–rostopic list---------------------------------------------
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
/battery_state
/camera5/point_cloud_rearDown
/diagnostics
/go1_controller/cmd_vel
/go1_controller/imu/data
/go1_controller/odom
/go1_controller/state
/initialpose
/joint_states
/local_ekf/set_pose
/map
/map_metadata
/motor_states
/move_base/current_goal
/move_base/goal
/move_base/recovery_status
/move_base_quadruped/cmd_vel
/move_base_simple/goal
/odometry/filtered
/particlecloud
/rosout
/rosout_agg
/scan
/tf
/tf_static

Please try disabling the ekf in map navi and then trying!

Do ‘bringup.launch’ and ‘map_navi.launch’ both disable ekf? So where is odom created?

It should be created from the bringup launch ekf

The same thing happens even if you change the execution location.

–rosnode list-------------------------------------------------------
/amcl
/camera5
/go1_controller
/local_ekf/ekf_localization_with_odom
/map_server
/move_base
/robot_state_publisher
/rosout
/rplidarNode

–rosnode info /local_ekf/ekf_localization_with_odom -----------------------------------------------------

Node [/local_ekf/ekf_localization_with_odom]
Publications:

  • /diagnostics [diagnostic_msgs/DiagnosticArray]
  • /odometry/filtered [nav_msgs/Odometry]
  • /rosout [rosgraph_msgs/Log]
  • /tf [tf2_msgs/TFMessage]

Subscriptions:

  • /go1_controller/odom [nav_msgs/Odometry]
  • /local_ekf/set_pose [unknown type]
  • /tf [tf2_msgs/TFMessage]
  • /tf_static [tf2_msgs/TFMessage]

Services:

  • /local_ekf/ekf_localization_with_odom/enable
  • /local_ekf/ekf_localization_with_odom/get_loggers
  • /local_ekf/ekf_localization_with_odom/set_logger_level
  • /local_ekf/ekf_localization_with_odom/toggle
  • /local_ekf/set_pose

contacting node http://rga-dell:38237/
Pid: 493
Connections:

  • topic: /tf
    • to: /local_ekf/ekf_localization_with_odom
    • direction: outbound
    • transport: INTRAPROCESS
  • topic: /tf
    • to: /move_base
    • direction: outbound (36519 - 127.0.0.1:35984) [15]
    • transport: TCPROS
  • topic: /tf
    • to: /amcl
    • direction: outbound (36519 - 127.0.0.1:35992) [16]
    • transport: TCPROS
  • topic: /tf
    • to: /local_ekf/ekf_localization_with_odom (http://rga-dell:38237/)
    • direction: inbound
    • transport: INTRAPROCESS
  • topic: /tf
    • to: /robot_state_publisher (http://rga-dell:40997/)
    • direction: inbound (58098 - rga-dell:55851) [12]
    • transport: TCPROS
  • topic: /tf
  • topic: /tf_static
    • to: /robot_state_publisher (http://rga-dell:40997/)
    • direction: inbound (58114 - rga-dell:55851) [10]
    • transport: TCPROS
  • topic: /go1_controller/odom
    • to: /go1_controller (http://rga-dell:46735/)
    • direction: inbound (49082 - rga-dell:44715) [13]
    • transport: TCPROS

– rostopic list ---------------------------------------------------
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
/battery_state
/camera5/point_cloud_rearDown
/diagnostics
/go1_controller/cmd_vel
/go1_controller/imu/data
/go1_controller/odom
/go1_controller/state
/initialpose
/joint_states
/local_ekf/set_pose
/map
/map_metadata
/motor_states
/move_base/current_goal
/move_base/goal
/move_base/recovery_status
/move_base_quadruped/cmd_vel
/move_base_simple/goal
/odometry/filtered
/particlecloud
/rosout
/rosout_agg
/scan
/tf
/tf_static

Try running only the go1 driver and rp lidar without map navi and see if laser is coming in odom. There is something wrong in the tf and/or the nodes!

go1 raspberrypi : roslaunch rplidar_ros rplidar_a1.launch
PC ubuntu docker : roslaunch go1_bringup bringup.launch
Screenshot from 2024-05-14 19-39-04

–rosnode list-------------------------------------------------------------
/camera5
/go1_controller
/local_ekf/ekf_localization_with_odom
/robot_state_publisher
/rosout
/rplidarNode
/rviz_1715682932397447738

–rosnode info /local_ekf/ekf_localization_with_odom -------------------------

Node [/local_ekf/ekf_localization_with_odom]
Publications:

  • /diagnostics [diagnostic_msgs/DiagnosticArray]
  • /odometry/filtered [nav_msgs/Odometry]
  • /rosout [rosgraph_msgs/Log]
  • /tf [tf2_msgs/TFMessage]

Subscriptions:

  • /go1_controller/odom [nav_msgs/Odometry]
  • /local_ekf/set_pose [unknown type]
  • /tf [tf2_msgs/TFMessage]
  • /tf_static [tf2_msgs/TFMessage]

Services:

  • /local_ekf/ekf_localization_with_odom/enable
  • /local_ekf/ekf_localization_with_odom/get_loggers
  • /local_ekf/ekf_localization_with_odom/set_logger_level
  • /local_ekf/ekf_localization_with_odom/toggle
  • /local_ekf/set_pose

contacting node http://rga-dell:41943/
Pid: 631
Connections:

  • topic: /rosout
    • to: /rosout
    • direction: outbound (33331 - 192.168.12.1:53896) [11]
    • transport: TCPROS
  • topic: /tf
    • to: /local_ekf/ekf_localization_with_odom
    • direction: outbound
    • transport: INTRAPROCESS
  • topic: /tf
    • to: /rviz_1715682932397447738
    • direction: outbound (33331 - 127.0.0.1:47002) [16]
    • transport: TCPROS
  • topic: /tf
    • to: /local_ekf/ekf_localization_with_odom (http://rga-dell:41943/)
    • direction: inbound
    • transport: INTRAPROCESS
  • topic: /tf
    • to: /robot_state_publisher (http://rga-dell:45721/)
    • direction: inbound (47126 - rga-dell:50295) [12]
    • transport: TCPROS
  • topic: /tf_static
    • to: /robot_state_publisher (http://rga-dell:45721/)
    • direction: inbound (47136 - rga-dell:50295) [10]
    • transport: TCPROS
  • topic: /go1_controller/odom
    • to: /go1_controller (http://rga-dell:34653/)
    • direction: inbound (47148 - rga-dell:51393) [13]
    • transport: TCPROS

– rostopic list-------------------------------------------------------
/battery_state
/camera5/point_cloud_rearDown
/clicked_point
/diagnostics
/go1_controller/cmd_vel
/go1_controller/imu/data
/go1_controller/odom
/go1_controller/state
/initialpose
/joint_states
/local_ekf/set_pose
/motor_states
/move_base_simple/goal
/odometry/filtered
/rosout
/rosout_agg
/scan
/tf
/tf_static

Odom and lidar are still not connected.

These 2 are on separate computers? have you correctly configured ros multi machine?

.bashrc and /etc/hosts

I can check the /scan data by connecting RPLiDAR to go1 raspberrypi and running rostopic echo /scan in the docker container on my PC.

Even in rviz, /scan data is confirmed in the base and trunk, but if I change to odom, /scan data is not visible.

Hmm

Please try putting this in both .bashrcs and configure them accordingly.

export ROS_MASTER_URI=http://192.168.12.1:11311
export ROS_HOSTNAME=192.168.12.1
export ROS_IP=192.168.12.1

for your pc

export ROS_MASTER_URI=http://192.168.12.1:11311
export ROS_HOSTNAME=192.168.12.<pc_ip>
export ROS_IP=192.168.12.<pc_ip>

.bashrc in my PC docker container
Screenshot from 2024-05-16 18-25-05

.bashrc in go1 raspberrypi
Screenshot from 2024-05-16 18-25-36

In go1 raspberrypi
roslaunch rplidar_ros rplidar_a1.launch
In my PC docker container
roslaunch go1_bringup bringup.launch

rviz results



– rosnode list ---------------------------------------
/camera5
/go1_controller
/local_ekf/ekf_localization_with_odom
/robot_state_publisher
/rosout
/rplidarNode
/rqt_gui_py_node_461

– rosnode info /local_ekf/ekf_localization_with_odom -------------------------------------------

Node [/local_ekf/ekf_localization_with_odom]
Publications:

  • /diagnostics [diagnostic_msgs/DiagnosticArray]
  • /odometry/filtered [nav_msgs/Odometry]
  • /rosout [rosgraph_msgs/Log]
  • /tf [tf2_msgs/TFMessage]

Subscriptions:

  • /go1_controller/odom [nav_msgs/Odometry]
  • /local_ekf/set_pose [unknown type]
  • /tf [tf2_msgs/TFMessage]
  • /tf_static [tf2_msgs/TFMessage]

Services:

  • /local_ekf/ekf_localization_with_odom/enable
  • /local_ekf/ekf_localization_with_odom/get_loggers
  • /local_ekf/ekf_localization_with_odom/set_logger_level
  • /local_ekf/ekf_localization_with_odom/toggle
  • /local_ekf/set_pose

contacting node http://192.168.12.235:40151/
Pid: 337
Connections:

  • topic: /rosout
    • to: /rosout
    • direction: outbound (34607 - 192.168.12.1:54676) [12]
    • transport: TCPROS
  • topic: /tf
    • to: /local_ekf/ekf_localization_with_odom
    • direction: outbound
    • transport: INTRAPROCESS
  • topic: /tf
  • topic: /tf
  • topic: /tf_static
  • topic: /go1_controller/odom

– rostopic list --------------------------------------------------------
/battery_state
/camera5/point_cloud_rearDown
/diagnostics
/go1_controller/cmd_vel
/go1_controller/imu/data
/go1_controller/odom
/go1_controller/state
/joint_states
/local_ekf/set_pose
/motor_states
/odometry/filtered
/rosout
/rosout_agg
/scan
/tf
/tf_static