Integrating the gbplanner2 with another agv platform?

[hhhhgggg80]

Hi, I am trying to using an agv with a lidar on it. So I tried to modified the smb_sim.launch like below. It turn out that, Voxblox had results on RViz, but planner did not work when I click the "start planner" button.

And, when I tried to use original module(smb.urdf.xacro), it worked and command trajectory was shown on Rviz . So I am wandering how to integrating the planner.

Looking forward to your reply.

<launch>
  <arg name="robot_name" default="agv"/>
  <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)"/>

  <!-- 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 = "model_xacro" default = "$(find scout_gazebo)/urdf/base.xacro" />
    <param name="robot_description" command="$(find xacro)/xacro '$(arg model_xacro)'" />
   <-- <param name="robot_description" command="$(find xacro)/xacro '$(find smb_description)/urdf/smb.urdf.xacro'" />-->

    <!-- Launch  the robot state publisher -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

    <arg name="world_file" default="$(find gazebo_cave_world)/worlds/cave_world.world"/>
    <include file="$(find gazebo_ros)/launch/empty_world.launch"> 
        <arg name="world_name" value="$(arg world_file)"/>
        <arg name="paused" value="false"/>
        <arg name="use_sim_time" value="true"/>
        <arg name="gui" value="$(arg gazebo_gui_en)"/>
        <arg name="headless" value="false"/>
        <arg name="debug" value="false"/>
        <arg name="verbose" value="false"/>
    </include>

    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model scout -param robot_description -x 10 -y -20 -z 0.5 -Y 3.1415926"  />  
    <node name="message_to_tf" pkg="smb_gazebo" type="message_to_tf" output="screen">
      <param name="odometry_topic" value="ground_truth/state"/>
      <param name="frame_id" value="world"/>
    </node>
    <node name="odom_to_world_tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /world /odom 10"/>

  <arg name="odometry_topic" default="/ground_truth/state"/>
  <node pkg="topic_tools" type="relay" name="vlp_relay" args="/velodyne_points /input_pointcloud" />
  <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="/input_pointcloud" />
    <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="command/trajectory" to="$(arg robot_name)/command/trajectory" />
    <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/agv.rviz"/>
</launch>

[MihirDharmadhikari]

Hi @hhhhgggg80 ,

Sorry for the delay. I see that you have changed the robot_name parameter. In this case, you will have to create a folder with the same name in the config folder where the yaml files are located. Otherwise, the planner won't be able to load the parameters. Second, press the initialize button before pressing start planner. If these things are already done, check what the planner node prints in the terminal when you press start planner. If the planner is unable to plan it will print out the error. Also, check if any errors/warnings are printed in the terminal when you launch the file. Can you let me know the above two things?

Best, Mihir

[hhhhgggg80]

Hi, @MihirDharmadhikari

Thank you for your reply. I followed your advices and checked the module again. The terminal didn't print something like warnings or errs. It seems like that, the module I used does not publish the topic /groud_truth/state. After modifying the gazebo plugin into the urdf file, the planner works fine.

<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">

By the way, if I want to integrate the gbplanner2 with a slam method, I need to remap the robot state to the topic /groud_truth/state. Is it right? Or is there anything else to change?

[MihirDharmadhikari]

Hi @hhhhgggg80 ,

Glad to hear the issue got resolved. To use the planner with a SLAM method, you need to provide the correct odometry topic here or remap it to /ground_truth/state as you mentioned. In addition, you need to specify the correct point cloud topic here. Make sure that your SLAM method provides a tf between the fixed frame and the frame in which the point cloud is expressed. If it provides the tf to another frame on the robot, you will have to add a static transform between that frame and the sensor frame of the point cloud. Finally, add a static transform from the fixed frame of the SLAM solution to the frame world. The planner assumes that the z axis is upwards, i.e. in the opposite direction of gravity.

Best, Mihir

[hhhhgggg80]

@MihirDharmadhikari Thank you for your attentive advises.