About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Combine FCPP with tracking PID controller

Description: In this tutorial the FCPP is used in combination with a trajectory tracker to let a robot fully cover a given map.

Tutorial Level: INTERMEDIATE

Setup trajectory tracker

The plugin computes a full coverage path. Next step is then to setup a path tracker. We will make use of the tracker in the tracking_pid repository.

The ros launch file 'test_full_coverage_path_planner.launch' is provided in the package as an example:

<launch>
    <arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
    <arg name="target_x_vel" default="0.5"/>
    <arg name="target_yaw_vel" default="0.4"/>
    <arg name="robot_radius" default="0.3"/>
    <arg name="tool_radius" default="0.3"/>

    <!--Move base flex, using the full_coverage_path_planner-->
    <node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="screen" required="true">
        <param name="tf_timeout" value="1.5"/>
        <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/planners.yaml" command="load" />
        <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/local_costmap_params.yaml" command="load" />
        <param name="SpiralSTC/robot_radius" value="$(arg robot_radius)"/>
        <param name="SpiralSTC/tool_radius" value="$(arg tool_radius)"/>
        <param name="global_costmap/robot_radius" value="$(arg robot_radius)"/>
        <remap from="odom" to="/odom"/>
        <remap from="scan" to="/scan"/>

        <remap from="/move_base_flex/SpiralSTC/plan" to="/move_base/SpiralSTC/plan"/>
        <remap from="/move_base_flex/tracking_pid/interpolator" to="/move_base/TrackingPidLocalPlanner/interpolator"/>
    </node>
    <!-- Move Base backwards compatibility -->
    <node pkg="mbf_costmap_nav" type="move_base_legacy_relay.py" name="move_base"/>

    <!-- Mobile robot simulator -->
    <node pkg="mobile_robot_simulator" type="mobile_robot_simulator_node" name="mobile_robot_simulator" output="screen">
        <param name="publish_map_transform" value="true"/>
        <param name="publish_rate" value="10.0"/>
        <param name="velocity_topic" value="/move_base/cmd_vel"/>
        <param name="odometry_topic" value="/odom"/>
    </node>

    <!--We need a map to fully cover-->
    <node name="grid_server" pkg="map_server" type="map_server" args="$(arg map)">
        <param name="frame_id" value="/map"/>
    </node>

    <!--Tracking_pid cannot just accept a nav_msgs/Path, it can only go to a PoseStamped,
       so the path_interpolator drags a PoseStamped over a Path at a given speed-->
    <node name="interpolator" pkg="tracking_pid" type="path_interpolator">
        <param name="target_x_vel" value="$(arg target_x_vel)"/>
        <param name="target_yaw_vel" value="$(arg target_yaw_vel)"/>
        <remap from="path" to="/move_base/SpiralSTC/plan"/>
    </node>

    <!--Tracking_pid tries to get the robot as close to it's goal point as possible-->
    <node name="controller" pkg="tracking_pid" type="controller" output="screen">
        <remap from="move_base/cmd_vel" to="/move_base/cmd_vel"/>
        <remap from="local_trajectory" to="trajectory"/>
        <param name="controller_debug_enabled" value="True"/>
        <param name="track_base_link" value="true"/>
        <param name="l" value="0.5"/>
        <param name="Ki_long" value="0.0"/>
        <param name="Kp_long" value="2.0"/>
        <param name="Kd_long" value="0.5"/>
        <param name="Ki_lat" value="0.0"/>
        <param name="Kp_lat" value="4.0"/>
        <param name="Kd_lat" value="0.3"/>
    </node>

    <!-- Launch coverage progress tracking -->
    <node pkg="tf" type="static_transform_publisher" name="map_to_coveragemap" args="-2.5 -2.5 0 0.0 0 0 map coverage_map 100" />
    <node pkg="full_coverage_path_planner" type="coverage_progress" name="coverage_progress">
        <param name="~target_area/x" value="10" />
        <param name="~target_area/y" value="10" />
        <param name="~coverage_radius" value="$(arg tool_radius)" />
        <remap from="reset" to="coverage_progress/reset" />
        <param name="~map_frame" value="/coverage_map"/>
    </node>

    <!-- Trigger planner by publishing a move_base goal -->
    <node name="publish_simple_goal" pkg="rostopic" type="rostopic"  launch-prefix="bash -c 'sleep 1.0; $0 $@' "
        args="pub --latch /move_base/goal move_base_msgs/MoveBaseActionGoal --file=$(find full_coverage_path_planner)/test/simple_goal.yaml"/>

    <!-- rviz -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find full_coverage_path_planner)/test/full_coverage_path_planner/fcpp.rviz" />
</launch>

The tracker consists of two nodes, an interpolator and a controller. The interpolator converts path poses into a trajectory pose that moves along the path. The controller then tries to follow it. For details on how to setup the tracker please refer to this tutorial.

A coverage tracker is also launched, which keeps track of where the robot tool has been. The location and size of the area to be tracked can be configured:

    <!-- Launch coverage progress tracking -->
    <node pkg="tf" type="static_transform_publisher" name="map_to_coveragemap" args="-2.5 -2.5 0 0.0 0 0 map coverage_map 100" />
    <node pkg="full_coverage_path_planner" type="coverage_progress" name="coverage_progress">
        <param name="~target_area/x" value="10" />
        <param name="~target_area/y" value="10" />
        <param name="~coverage_radius" value="$(arg tool_radius)" />
        <remap from="reset" to="coverage_progress/reset" />
        <param name="~map_frame" value="/coverage_map"/>
    </node>

Launch the application, you will see then the generated full coverage path and the robot tracking it.