Integrating the planner with different UAV platfrom (not available in rotors_gazebo)

[reem90]

I am using a different UAV platform in GAZEBO that is equipped only with a Lidar and a depth camera and uses px4. My system provides the following topics 1) odometry (localization) 2) point cloud (from Lidar) 3)images. When I run my system, the drone takes off and waits for a goal position. I have my own controller that is responsible for moving the drone from Point A to Point B.

I was wondering 1) how can I integrate the planner with my system?

I tried to modify the rmf_sim.launch file to make it work with my system as shown. Kindly let me know 2)what to change in the rmf_sim.launch file to make it work?

3) Is there any documentation to guide us through the process of adding a new drone?

Also, 4) what is the topic that is used to send a goal position to the drone or path? Should I change it from the code or from the launch file?

<?xml version="1.0"?>
<launch>
  <!-- All settings -->
  <arg name="robot_name" default="uav1"/>
  <arg name="gazebo_gui_en" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="rviz_en" default="true" />
  <arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->

  <param name="use_sim_time" value="$(arg use_sim_time)"/>

<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <arg name="odometry_topic" default="/uav1/aloam/odom"/>
<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <!-- Config files -->
  <arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
  <arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
  <arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/>
  <arg name="map_config_file" default="$(arg voxblox_config_file)"/>
  <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_subt_final_circuit.world"/>

  <group ns="$(arg robot_name)">
    <node name="img_throttler" type="throttle" pkg="topic_tools" args="messages vi_sensor/camera_depth/depth/points 5 vi_sensor/camera_depth/depth/points_throttled" />
    <node name="odo_throttler" type="throttle" pkg="topic_tools" args="messages ground_truth/odometry 100 ground_truth/odometry_throttled" />

    <!-- Position controller  -->
    <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
      <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg robot_name).yaml" />
      <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg robot_name).yaml" />
      <remap from="odometry" to="$(arg odometry_topic)" />
    </node>
  </group>

  <node name="pose_throttler" type="throttle" pkg="topic_tools" args="messages $(arg robot_name)/ground_truth/pose_with_covariance 10 /msf_core/pose" />
  <!-- Graph based planning -->
  <node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
    <remap from="odometry" to="$(arg odometry_topic)" />
    <remap from="/pointcloud" to="/uav1/os_cloud_nodelet/points_processed" />
    <rosparam command="load" file="$(arg gbplanner_config_file)" />
    <rosparam command="load" file="$(arg map_config_file)" />
  </node>

  <!-- Planner and Control Interface -->
  <node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">

    <remap from="planner_server" to="gbplanner" />
    <remap from="planner_homing_server" to="gbplanner/homing" />
    <remap from="odometry" to="$(arg odometry_topic)"/>
    <rosparam command="load" file="$(arg pci_file)" />
  </node>

  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/rmf_obelix.rviz"/>

</launch>

[MihirDharmadhikari]

Hi @reem90 ,

Thank you for contacting us. Assuming that you have a separate launch file from which you launch your simulator and controller, you will have to do the following modifications to the above launch file:

• You don't need the lee_position_controller_node as you have your own controller. So you can remove that part.
• The img_throttler, odo_throttler, and pose_throttler are also unnecessary.

The remaining looks fine.

Next, the final path published by the planner will be on the topic command/trajectory, which is published by the pci_general_ros_node. This is of the type trajectory_msgs/MultiDOFJointTrajectory. You will have to adapt your controller to follow this path. You don't need to change anything in the GBPlanner code.

Regarding your question number 3, do you intend to add a second (and possibly more) UAV in the simulation or are you asking about using a different UAV than that from the demo?

Let me know if this helps.

Best, Mihir

[reem90]

@MihirDharmadhikari Thank you for your reply. I am going to try your suggestions and get back to you soon.

For question number 3, yes I am using a different model than the one from the demo and it is not available in the list of the drones you have in your files.

[reem90]

@MihirDharmadhikari So I need to create a node that subscribes for the command/trajectoryof type trajectory_msgs/MultiDOFJointTrajectory and publish it to my controller with its own type?

[reem90]

@MihirDharmadhikari @tungdanganh @fmascarich @nkhedekar

I modified the launch file as follows, however, my drone is not of model rmf_obelix. Also, when I run my system which has a drone that takes off and waits for control commands, then I run the following modified launch file, I tried to press initialization from gazebo GUI then RVIZ crashed. I tried to echo the command/trajectory but no output is available. Kindly advise.

<?xml version="1.0"?>
<launch>
  <!-- All settings -->
  <arg name="robot_name" default="rmf_obelix"/>
  <arg name="gazebo_gui_en" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="rviz_en" default="true" />
  <arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->

  <param name="use_sim_time" value="$(arg use_sim_time)"/>

<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <arg name="odometry_topic" default="/uav1/aloam/odom"/>
<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <!-- Config files -->
  <arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
  <arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
  <arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/>
  <arg name="map_config_file" default="$(arg voxblox_config_file)"/>
  <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_subt_final_circuit.world"/>

  <group ns="$(arg robot_name)">

  <!-- Graph based planning -->
  <node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
    <remap from="odometry" to="$(arg odometry_topic)" />
    <remap from="/pointcloud" to="/uav1/os_cloud_nodelet/points_processed" />
    <rosparam command="load" file="$(arg gbplanner_config_file)" />
    <rosparam command="load" file="$(arg map_config_file)" />
  </node>
  </group>
  <!-- Planner and Control Interface -->
  <node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">

    <remap from="planner_server" to="gbplanner" />
    <remap from="planner_homing_server" to="gbplanner/homing" />
    <remap from="odometry" to="$(arg odometry_topic)"/>
    <rosparam command="load" file="$(arg pci_file)" />
  </node>

  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/rmf_obelix.rviz"/>

</launch>

[MihirDharmadhikari]

Hi,

Sorry for the late reply. Even if you have a different UAV model, the parameters for the planner (gbplanner_config_file, pci_file, voxblox_config_file) will still work. Regarding your other question, which gazebo UI are you referring to? I believe you meant the Rviz UI. If that crashed, can you check what message it prints out on the terminal? Also, in your current launch file, you should not launch gbplanner inside the namespace $(arg robot_name). What you had done in the previous launch file was correct.

Best, Mihir

[reem90]

@MihirDharmadhikari Thank you for your reply.

Yes, I meant RVIZ. It doesn't crash it just freezes. When I click initialization, the /command/trajectory topic is not published.

Here is the modified version of the launch file after your suggestions.

<?xml version="1.0"?>
<launch>
  <!-- All settings -->
  <arg name="robot_name" default="rmf_obelix"/>
  <arg name="gazebo_gui_en" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="rviz_en" default="true" />
  <arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->

  <param name="use_sim_time" value="$(arg use_sim_time)"/>
  <node pkg="tf" type="static_transform_publisher" name="world_to_uav1" args="0 0 0 0 0 0 1 /uav1/stable_origin world 100" />
<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <arg name="odometry_topic" default="/uav1/aloam/odom"/>
<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <!-- Config files -->
  <arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
  <arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
  <arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/>
  <arg name="map_config_file" default="$(arg voxblox_config_file)"/>
  <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_subt_final_circuit.world"/>

  <!-- Graph based planning -->
  <node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
    <remap from="odometry" to="$(arg odometry_topic)" />
    <remap from="/pointcloud" to="/uav1/os_cloud_nodelet/points_processed" />
    <rosparam command="load" file="$(arg gbplanner_config_file)" />
    <rosparam command="load" file="$(arg map_config_file)" />
  </node>
  <!-- Planner and Control Interface -->
  <node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">
    <remap from="planner_server" to="gbplanner" />
    <remap from="planner_homing_server" to="gbplanner/homing" />
    <remap from="odometry" to="$(arg odometry_topic)"/>
    <rosparam command="load" file="$(arg pci_file)" />
  </node>

  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/rmf_obelix.rviz"/>

</launch>

Please Advise.

[MihirDharmadhikari]

Closing the issue since it has been resolved. The issue was caused due to incorrect topic configuration.