How to use dwa_local_planner ROS wrapper C++ API

edit

Hi,

UPDATE: This question was answered. Look at the answer below for a quick how-to :).

I am trying to use the ROS WRAPPER for the dwa_local_planner, however I am having some issues when I call the function dwaComputeVelocityCommands as it throws a segmentation fault on the function dwa_local_planner::DWAPlanner::findBestPath. This is the error throws:

Thread 1 "planner" received signal SIGSEGV, Segmentation fault.
0x00007ffff7b9e282 in dwa_local_planner::DWAPlanner::findBestPath(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> >&) ()
   from /opt/ros/melodic/lib/libdwa_local_planner.so

and here is the full backtrace from GDB:

(gdb) backtrace 
#0  0x00007ffff7b9e282 in dwa_local_planner::DWAPlanner::findBestPath(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> >&) ()
   from /opt/ros/melodic/lib/libdwa_local_planner.so
#1  0x00007ffff7ba8135 in dwa_local_planner::DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped_<std::allocator<void> >&, geometry_msgs::Twist_<std::allocator<void> >&) () from /opt/ros/melodic/lib/libdwa_local_planner.so
#2  0x0000555555580516 in Planner::dwaTrajectoryControl(hsr_planner::ClutterPlannerService const&) ()
#3  0x000055555557f92d in Planner::requestClutterPlan() ()
#4  0x000055555557ed85 in Planner::Planner(tf2_ros::Buffer&, tf2_ros::TransformListener&) ()
#5  0x0000555555580b31 in main ()

I am not sure whether this is a bug in my code or on the API itself, and I have bee trying to solve this issue for a bunch of hours already so I would like to know whether the steps I took are correct or not, or if you have any suggestion, please :).

How I Initialize

I start by creating the tf2 Buffer as well as the TransformListener in the main and then passing these to the constructor of the Class in order to populate the costmap (pointer) and the dynamic planner.

Here is the main:

int main(int argc, char **argv)
{
    // Create ROS node
    ros::init(argc, argv, "planner");
    ROS_INFO("Created Planner node...");

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

    // Create Planner instance
    Planner planner(l_buffer, l_tf);

    // Spin ROS
    ros::spin();

    return 0;
}

and here is the initialization of the members costmap and dynamic planner in the class (or how I do it):

// Create shared pointers instances
m_costMap = new costmap_2d::Costmap2DROS("hsr_costmap", m_buffer);

// Initialize dwa local planner
m_dp.initialize("hrs_dwa_planner", &m_buffer, m_costMap);

Afterwards, I set the global plan using setPlan which is always successful and works well, and proceed to compute the velocities using dwaComputeVelocityCommands passing to it the global pose obtained from the costmap pointer and the Twist object as follows:

    // 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;
    l_cmd_vel.linear.x = 0;
    l_cmd_vel.linear.y = 0;
    l_cmd_vel.linear.z = 0;
    l_cmd_vel.angular.x = 0;
    l_cmd_vel.angular.y = 0;
    l_cmd_vel.angular.z = 0;

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

    // Keep sending commands
    // until goal is reached
    while (!m_dp.isGoalReached())
    {
        ROS_INFO(m_dp.isGoalReached());

        // Compute velocity commands
        if (m_dp.dwaComputeVelocityCommands(l_global_pose, 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 ...