ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I managed to get the dwa_local_planner work. For anyone wanting to set it up, here is a quick tutorial on how to do it. And in case you are wondering, the issues was that the costmap was not initialized correctly as I did not load the parameters in the yaml file, so there was nothing going on there, which made the SEGMENTATION FAULT being thrown.

Costmaps and Launch file

First of all, we have to set up costmaps settings for our global and local costmap. For this, we will make use of .yaml files. We will need three yaml files, one for common costmap settings, one for the global costmap settings, and one for local costmap settings. The files (as of 2020 - Melodic branch) look like this:

Common Parameters

obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.430
inflation_radius: 0.55
origin_x: 0
origin_y: 0

observation_sources: laser_scan_sensor point_cloud_sensor
laser_scan_sensor: {sensor_frame: base_range_sensor_link, data_type: LaserScan, topic: /hsrb/base_scan, marking: true, clearing: true}
point_cloud_sensor: {sensor_frame: head_rgbd_sensor_rgb_frame, data_type: PointCloud2, topic: /hsrb/head_rgbd_sensor/depth_registered/rectified_points, marking: true, clearing: true}

Please change the sensor frame, topic, and dataType fields accordingly

Global Parameters

global_costmap:
  global_frame: "map"
  rolling_window: true
  transform_tolerance: 2.0
  robot_base_frame: "base_link"

plugins: 
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

Local Parameters

local_costmap:
  width: 6.0
  height: 6.0
  resolution: 0.05
  global_frame: "odom"
  rolling_window: true
  update_frequency: 2.0
  publish_frequency: 2.0
  transform_tolerance: 2.0
  robot_base_frame: "base_link"

plugins: 
  - {name: obstacle_layer,  type: "costmap_2d::VoxelLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

Finally, we create a launch file which will launch the node file we created (or will create later), while loading the costmaps parameter defined above in the parameter space. My launch file looked like this:

<launch>

<!-- Run the map server --> 
<!--<node name="map_server" pkg="map_server" type="map_server" args="$(find my_map_package)/my_map.pgm my_map_resolution"/>-->

<node pkg="hsr_planner" type="planner" respawn="false" name="hsr_planner" output="screen">
    <rosparam file="$(find hsr_planner)/params/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
    <rosparam file="$(find hsr_planner)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find hsr_planner)/params/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find hsr_planner)/params/global_costmap_params.yaml" command="load" /> 
</node>

</launch>

Please note to change paths accordingly to your environment and adapt your fields to your robot/topics etc.

Code

Now the fun part :).

In order to initialize the dwa_local_planner instance, we first need to create our costmap/s. In this way:

// TF2 objects
tf2_ros::Buffer l_buffer(ros::Duration(10));
tf2_ros::TransformListener l_tf(l_buffer);

// Initialize costmaps (global and local)
l_local = new costmap_2d::Costmap2DROS("local_costmap", l_buffer);
l_global = new costmap_2d::Costmap2DROS("global_costmap", l_buffer);

// Start map
l_local->start();
l_global->start();

Then we proceed to initialize the dwa_local_planner instance as follows, while trying to compute the velocities to be sent to the robot in order to reach the goal:

// Check that planner is initialized
if (!m_dp.isInitialized())
{
    // Initialize dwa local planner
    m_dp.initialize("hsr_dwa_planner", &m_buffer, m_local);

    ROS_INFO("DWA has been initialized successfully...");
}

// Set global plan for local planner
if (m_dp.setPlan(p_service.response.path))
{
    ROS_INFO("DWA set plan: SUCCESS");
}
else
{
    ROS_ERROR("DWA set plan: FAILED");
}

// Create twist messag to be
// populate by the local planner
geometry_msgs::Twist l_cmd_vel;

// Get robot pose in the map
geometry_msgs::PoseStamped l_global_pose;
m_global->getRobotPose(l_global_pose);

// Keep sending commands
// until goal is reached
while (!m_dp.isGoalReached())
{
    // Update costmaps
    m_local->updateMap();
    m_global->updateMap();

    // Compute velocity commands
    if (m_dp.computeVelocityCommands(l_cmd_vel))
    {
        ROS_INFO("DWA compute cmd_vel: SUCCESS");
    }
    else
    {
        ROS_ERROR("DWA compute cmd_vel: FAILED");
    }

    // Send commands
    m_velPub.publish(l_cmd_vel);
}

This was just to show quickly how to work with the DWA planner standalone, without having to integrate it with the move-base node. In case I said anything wrong, please correct me.

Salam :)