Connect the planner to Gazebo environment

[QuantumY-CHN]

Thank you for your excellent work! Could you please suggest how I can connect the complete planner to a gazebo simulation? Sorry, I'm new to this. I really appreciate any help you can provide.

[Schmluk]

Hi @QuantumY-CHN

The planner has also been tested with Gazebo. Just remove the in the namespace unreal in the launch file, and remap the input of voxblox to your sensor output from Gazebo. Notice that since this also removes the automated experiment manager you have to manually start the planner by calling the toggle_running service.

[QuantumY-CHN]

Thanks a lot!

[QuantumY-CHN]

Hi Lukas. Thank you for your work and your help here.

After remapping the input & calling the 'toggle_running'. I still got some problems and stuck here for a long time. The planner stuck at the first planning and then keeps rolling the TF_REPEATED_DATA warning:

[ INFO] [1677332842.071301456]: 
******************** Planner is now Running ********************

[INFO] [1677332842.326219, 1.630000]: Waiting for planner to be ready... done. Launch in 3 seconds.
[ INFO] [1677332843.061559147, 2.310000000]: Got an ESDF map from ROS topic!
[ INFO] [1677332843.064986037, 2.320000000]: Got an TSDF map from ROS topic!

[ INFO] [1677332845.422441632, 4.630000000]: Started planning.
[INFO] [1677332845.425233, 4.630000]: 
***************************************
* Succesfully started the simulation! *
***************************************
[ INFO] [1677332845.425299116, 4.630000000]: Replanning!
(0.00213s elapsed, 1 new, 1 total, 1 killed by root change, 0 killed while updating)
[ INFO] [1677332845.427804149, 4.630000000]: KF is updated for first time.
Warning: Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.240000 according to authority unknown_publisherTF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.240000 according to authority unknown_publisher
         at line 
278         at line  in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
[ WARN] [1677332846.027437605, 5.250000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.240000 according to authority unknown_publisher
Warning: Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.440000 according to authority unknown_publisherTF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.440000 according to authority unknown_publisher
         at line 
         at line 278278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
[ WARN] [1677332846.227424910, 5.440000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.440000 according to authority unknown_publisher
 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
Warning: Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.510000 according to authority unknown_publisher
         at line TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.510000 according to authority unknown_publisher278 in 
/tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp         at line 
278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
[ WARN] [1677332846.312312359, 5.510000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.510000 according to authority unknown_publisher
[ WARN] [1677332846.405524350, 5.610000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.600000 according to authority unknown_publisher
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.600000 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.600000 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.760000 according to authority unknown_publisher
Warning:          at line TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.760000 according to authority unknown_publisher278 in 
/tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp         at line 278
 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
[ WARN] [1677332846.550581679, 5.760000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.760000 according to authority unknown_publisher
Warning: Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.900000 according to authority unknown_publisherTF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.900000 according to authority unknown_publisher

         at line 278         at line  in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp278
 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
[ WARN] [1677332846.695592538, 5.900000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 5.900000 according to authority unknown_publisher
Warning: [ WARN] [1677332847.338887832, 6.550000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame firefly/current_reference at time 6.540000 according to authority unknown_publisher

I cannot find out the problem via rostopic info or rqt_tf_tree

ubuntu@4e8ecf15d6c5:~$ rostopic info /tf
Type: tf2_msgs/TFMessage

Publishers: 
 * /firefly/robot_state_publisher (http://4e8ecf15d6c5:42347/)
 * /firefly/tf_camera_to_link (http://4e8ecf15d6c5:44797/)
 * /firefly/mav_nonlinear_mpc (http://4e8ecf15d6c5:43177/)
 * /planner/voxblox_node (http://4e8ecf15d6c5:34147/)
 * /firefly/odometry_noise_model (http://4e8ecf15d6c5:33811/)
 * /gazebo (http://4e8ecf15d6c5:46559/)
 * /planner/planner_node (http://4e8ecf15d6c5:45921/)

Subscribers: 
 * /planner/voxblox_node (http://4e8ecf15d6c5:34147/)
 * /rviz_voxblox (http://4e8ecf15d6c5:40173/)
 * /planner/planner_node (http://4e8ecf15d6c5:45921/)

The mav cannot take off and the map in rviz looks like the screenshot below

Would it be possible to suggest where the problem could be? Great thanks in advance!

[QuantumY-CHN]

The current_reference frame is sticking at its initial position via the visualization of tf in rviz

[Schmluk]

Hi @QuantumY-CHN

The tf errors you are getting hints at issues with your simulation setup. The planner does not publish any tf's. The map looks like the sensing is running correctly. To properly start the planner make sure the drone is ready to receive commands (i.e. already taken off and in hover) and that the command outputs of the planner are subscribed to by your controller. Depending on the specific controller, you may have to add a converter for the current outputs.

Hope this helps!

[QuantumY-CHN]

Great thanks, Lukas. It runs correctly now.

[Badrivishal]

On running rosservice call /planner/planner_node/toggle_running "data: true", The planner is giving this, but its not commanding the drone, [ INFO] [1681120119.076997024, 405.156000000]: Started planning.

(0.00105s elapsed, 1 new, 1 total, 1 killed by root change, 0 killed while updating)

Has anyone faced this?

[Schmluk]

Hi @Badrivishal

Is the drone staying still or rotating in place? In the first case the simulator is probably not subscribing to the command output of the planner. In the second case the starting pose is likely intraversable, i.e. there's a surface close to the drone or the clearing radius is set too low to start.

Hope this helps!

[Badrivishal]

Hi, Thank you for your response @Schmluk , The drone is still, and the command outputs are remapped to "/mavros/setpoint_trajectory/local". I also tried increasing the distance to the obstacle but was not able to get a new point. When I tried to echo the topic, It was empty.

I am attaching some of the screenshots

[Schmluk]

Few things to consider:

• Does the set point include the orientation and is of the correct topic type (mav_active_3d_planning publishes trajectory messages by default)?
• Is the controller of your simulator running?
• Is the drone in hover and ready to fly?
• The planner will only send one trajectory and then wait till the end point is reached, this is why the may seem empty. Typically, the first trajectory is a pure rotation.

Hope this helps!

[Badrivishal]

Thanks a lot! I figured it out by writing a controller to change the mode to offboard. It was a PX4-SITL problem.

[Chai8226]

@QuantumY-CHN 你好，我也尝试在gazebo复现这个代码，但遇到一些问题没法解决，你可以分享一下代码吗

[QuantumY-CHN]

@Chai8226 Could you please describe what the problem is?

[Chai8226]

@Chai8226 Could you please describe what the problem is?

when I run roslaunch active_3d_planning_app_reconstruction example.launch, I met this error gzserver: symbol lookup error: /home/chai/catkin_ws/devel/lib/librotors_gazebo_multirotor_base_plugin.so: undefined symbol: _ZN14gz_sensor_msgs9ActuatorsC1Ev [gazebo-2] process has died [pid 25964, exit code 127, cmd /home/chai/catkin_ws/src/gazebo_ros_pkgs/gazebo_ros/scripts/gzserver -u -e ode /home/chai/PX4_Firmware/Tools/sitl_gazebo/worlds/empty.world name:=gazebo log:=/home/chai/.ros/log/d471cfbc-0b6e-11ee-ac66-70a6cc7ecc5f/gazebo-2.log]. log file: /home/chai/.ros/log/d471cfbc-0b6e-11ee-ac66-70a6cc7ecc5f/gazebo-2*.log

[QuantumY-CHN]

@Chai8226 You might get msgs definition conflict. I cannot remember how I solved this exact. But there are a lot of issues and blogs discussed in this. e.g., https://github.com/ethz-asl/rotors_simulator/issues/661 FYI.

[Chai8226]

@Chai8226 You might get msgs definition confliction. I cannot remember how I solved this exact. But there are a lot of issues and blogs discussed in this. e.g., ethz-asl/rotors_simulator#661 FYI.

Thank u! I keep trying