[Unitree Go1] high_state topic receives only zeros

Hi!

I am making progress with the control of the robot via ROS.
I’m currently able to send high-level commands and the robot moves accordingly.

My next step would be to explore the status messages the robot sends back.
Unfortunately, the messages I receive on the /high_state topic are filled with nothing but zeros.

I am a bit clueless, why that happens, since the UDP connection in one direction works obviously fine.

Am I missing something important?

Thanks in advance!

Jonas

Hi @Tahir,

Perhaps an update on this!

So, I’ve digged a little bit deeper into the issues in the unitree github repositories and found this issue #42 with a similar problem.

I am also connected via wifi but activated the forwarding on the pi, as shown in this video by unitree. I also added a route on my computer to the 192.168.123.0/24 network via 192.168.12.1.

With these network settings, I’m able to ping the devices in the 192.168.123.0/24 network without any problems.
And since sending commands works, I did not change the IP in the code to keep it valid for WiFi and ethernet.

I will now try changing it, but that wouldn’t be my preferred solution.

Update:

Changing the IP address in the ros_udp.cpp file to the Pi’s IP worked.
But this unfortunately means, that I always have to change and recompile the code if I want to switch between Ethernet and WiFi.

I’d guess the problem occurs because the forwarding on the Pi isn’t working in both directions, as I thought it would.

Edit
I’ve tested the connection in both ways by connecting to the Nvidia boards via ssh and pinging my host machine from there (which was obviously working since the ssh connection worked).
But this makes me even more clueless.

Question

Since configuring the forwarding seems to be the more elegant way, than changing the address in the code, every time you switch the connection type, I’d rather do that.

Does somebody know, how to achieve this?

Edit
Does somebody have a clue, why I don’t get status messages via udp?

@jgrube can you share your ports and ip address for UDP setup?

Also can you please share how are you connected to the robot?

@jgrube. The issue seems to be that connecting via LAN and via WiFi have different behaviors. With LAN I am sure that you will get all the state information when running the driver, it is also possible to do it on WiFi but has a different IP requirement, we will update this information soon in the Quadruped Docs in the GO1 section.

Until then you can share this information so that we can inform you on the way how to do it or you can perhaps wait for the docs to eventually get updated!

This might help clarify the issue, select the hyperlink GO1 Unitree Legged SDK Tutorials in the Quadruped Docs

Network setup

My host computer is connected via WiFi.
IP address: 192.168.12.156
Static route: 192.168.123.0/24 via 192.168.12.1 dev wlp146s0 proto static metric 100

The Pi has two IP addresses:
eth0: 192.168.123.161
wlan1: 192.168.12.1

Routes on the PI:

default via 192.168.123.1 dev eth0 src 192.168.123.161 metric 202 
default via 192.168.12.1 dev wlan1 src 192.168.12.1 metric 305 
192.168.12.0/24 dev wlan1 proto dhcp scope link src 192.168.12.1 metric 305 
192.168.123.0/24 dev eth0 proto dhcp scope link src 192.168.123.161 metric 202 
224.0.0.0/4 dev lo scope link 

I ran the following commands on the Pi to enable the forwarding of IP packets:

sudo sysctl -p
sudo iptables -F
sudo iptables -t nat -F
sudo iptables -t nat -A POSTROUTING -o wlan1 -j MASQUERADE
sudo iptables -t nat -A POSTROUTING -o eth0 -j MASQUERADE
sudo iptables -A FORWARD -i wlan1 -o eth0 -j ACCEPT
sudo iptables -A FORWARD -i eth0 -o wlan1 -j ACCEPT

And I uncommented the line net.ipv4.ip_forward=1 in the file /etc/sysctl.conf.

Therefore I’m able to reach the following devices by running nmap on my host computer:

Nmap scan report for 192.168.123.10
Host is up (0.0039s latency).
Nmap scan report for 192.168.123.13
Host is up (0.0015s latency).
Nmap scan report for 192.168.123.14
Host is up (0.0018s latency).
Nmap scan report for 192.168.123.15
Host is up (0.0023s latency).
Nmap scan report for 192.168.123.161
Host is up (0.0017s latency).
Nmap done: 256 IP addresses (5 hosts up) scanned in 15.51 seconds

What I can and can’t do

I am further able to control the robot without changing the IP address in the program (set to 192.168.123.161). I just don’t get status messages.

When I change the IP in the program to 192.168.12.1, I can control the robot and I do get status messages.

I am also able to connect to the Nvidia computers via ssh over the WiFi connection without any problem, which means the forwarding through the PI is working in both directions.

What I don’t understand

Since both of the addresses I use in the program belong to the PI there should not be a difference, which one I’m using to connect to it.
And because every inbound package at 192.168.12.1 addressed to the 192.168.123.0/24 network gets forwarded into the internal network, they should also be able to reach the “other side of the PI” at 192.168.123.161.

Why I don’t like the solution of changing the IP in the code

  1. It shouldn’t be necessary because I can connect to the internal network
  2. Changing the code every time I switch between a wired and wireless connection is a possible error source
  3. After changing the IP in the code I’d have to recompile it
  4. In the future we are going to have a laboratory where we will probably be developing in a wired configuration and then be testing with the wireless connection. Therefore the code should stay the same to achieve equal conditions and enable a quick change between the configurations.
1 Like

Hi @jgrube,

We also agree that IP switching shouldn’t be required, but we suspect it is due to addition of the Nvidia boards for the vision system.

An option is to perhaps open an issue in the unitree_legged_sdk and suggest changes for the next release.

From our side, we are currently looking into a method for our driver in which we automatically detect whether the robot is connected to WiFi or LAN and switch to the correct IP configuration.

Hi @Sohail!

I am glad we agree on this!

As a quick solution, i changed the ros_udp.cpp file to read a ROS parameter instead of the fixed IP and added a default value in case it shouldn’t be set.
I also added the parameter as an argument to the launch file real.launch to change it easily.

This way at least I don’t have to recompile the code.

I do not think I am currently able to spot the change, which has to be made to send the state.
Probably because the SDK only provides the header files and does the rest in the precompiled libraries.
Therefore I will only point out the problem in an issue.

Thank you for your time!

1 Like

Dear Jgrube,

Glad to hear that it’s working. Also, you seem to have found a nice workaround for their IP issue :+1:t3:

1 Like

Solution / Workaround

My version of the code can be found here.

I’ve added lines 117 following:

std::string ip_string;
nh.param<std::string>("/UDP_IP", ip_string, "192.168.123.161");
strcpy(udp_ip, ip_string.c_str());

custom = Custom();

printf("Parameter /UDP_IP: %s%s\n", udp_ip, nh.hasParam("/UDP_IP") ? "" : " (as default value)");

And modified the constructor in line 32:

Custom()
   : low_udp(LOWLEVEL),
     high_udp(8090, const_cast<const char*>(udp_ip), 8082, sizeof(HighCmd), sizeof(HighState))

I also modified the launch file to add the IP as an argument.

<launch>
    <arg name="ctrl_level" default="highlevel"/>
    <arg name="udp_ip" default="192.168.123.161"/>

    <node pkg="unitree_legged_real" type="ros_udp" name="node_ros_udp" output="screen" args="$(arg ctrl_level)"/>

    <param name="control_level" value="$(arg ctrl_level)"/>
    <param name="/UDP_IP" value="$(arg udp_ip)"/>
</launch>

There might be room for improvement, since I’m not really a C++ guy, but it works.
Feel free to suggest changes.

1 Like

BUGFIX

The above stated solution contains a bug!
Since the default constructor of the Custom class was already called by declaring the variable, the UDP ports where blocked and therefore a connection wasn’t possible.

I changed the custom-variable to a Pointer to call it’s constructor, when the IP address is read.
Therefore all references to that object had to be changed to work with pointers instead of references (see this PR for details).

1 Like

Hi @jgrube our pre-release driver for Go1 is ready. In case you are interested in the ROS wrapper please send an email to support@mybotshop.de.

1 Like