how to deploy Control algorithm and Verify it in gazebo environment

Hello,

I am trying to deploy my controller algorithm in px4 software and verify it in gazebo environment.

I know that PX4_SITL communicates via UDP with jMAVSIM or Gazebo, so after I deploy my control algorithm(using build button), I execute

make px4_sitl_nolockstep gazebo

And I can verify that I can deploy it. Below is my test deploy simulink model and I can verify it is properly operate when execute it in gazebo environment.

However, When I try to execute my control algorithm, It doesn't work.

The reason I am suspecting is that uORB read part in simulink file. It seems reading topic is not done appropriately and cannot generate control signal.

The Question is, is there a Debug method to check whether my controller algorithm is working appropriately or not in gazebo environment?

Below are my trials

1. Use Monitor and Tune to see signal information where the problem occurs. However, It seems monitor and tune doesn't support gazebo simulation. (By the way, even in jmavsim, time pollout error occured)
2. Use Ros Toolbox to connect Gazebo and Simulink to see the joint velocity(motor). However, this is very limited because I can see only joint velocity value and cannot see any signal information ahead of it.

I already verify my controller algorithm in simulink environment by making simulink plant model and simulink controller.

So what I am trying to do now is verify it in gazebo environment.

In short, Is there any method I can debug my controller algorithm in gazebo environment?

Thank you.

[Manoj Velmurugan]

You could try using the listener command (in SITL command window) to know if the topic is published in the first place.

listener <uorb_topic name>

[종원 이]

Thank you. I added my custom uorb message and able to check the signal.

Answer 1

[Arun Mathamkode]

I am glad to see that you used the custom uORB to do the debugging of your controller model. That is a good way.

Another option is to use the 'Simulink' as the Simulation option instead of 'jMAVSim'. This is just to makesure that then when you build or run Monitor and Tune simulation jMAVSim is not launched. Once the SITL is launched and the pop-up asking you to run the Simulink plant comes up , instead of Simulink plant you can run the Gazebo. The Gazebo Simulator should get connected with SITL. If you are running Gazebo inside a VM ensure that you formward the Simulator port accordingly.