How to use dwa_local_planner ROS C++ API

edit

Hello,

I am trying to use dwa_local_planner C++ API. I am using Ubuntu 20.04 running ROS Noetic. Below is my following code for same:

Robot.h File:

#include <dwa_local_planner/dwa_planner_ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <iostream>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>

class Robot { 
public:

    Robot();

    void getPath(const nav_msgs::Path::ConstPtr &msg);

    void dwaPlanner();

private:
    ros::NodeHandle nh;
    ros::Subscriber pathSub;
    ros::Publisher cmdVelPub;
    tf2_ros::Buffer* tf1;
    costmap_2d::Costmap2DROS *costmap_ros;
    geometry_msgs::PoseStamped globalPose;
    std::vector<geometry_msgs::PoseStamped> posesVec, vec;
    dwa_local_planner::DWAPlannerROS dwa;
};

Robot.cpp File:

#include <Robot.h>

Robot::Robot() {
    pathSub = nh.subscribe<nav_msgs::Path>("/robot_path/r0", 10, &Robot::getPath, this);
    cmdVelPub = nh.advertise<geometry_msgs::Twist>("/r0/cmd_vel", 10);
}

void Robot::getPath(const nav_msgs::Path::ConstPtr &msg) {
    for (uint i = 0; i < msg->poses.size(); ++i) {
        posesVec.emplace_back(msg->poses[i]);
    }
}

void Robot::dwaPlanner() {
    tf2_ros::Buffer buffer(ros::Duration(10));
    tf2_ros::TransformListener tf(buffer);
    costmap_ros = new costmap_2d::Costmap2DROS("global_costmap", buffer);
    costmap_ros->start();
    costmap_ros->getRobotPose(globalPose);
    dwa_local_planner::DWAPlannerROS dwa;
    dwa.initialize("local_planner", &buffer, costmap_ros);
    if(dwa.setPlan(posesVec)) {
        ROS_INFO("SET");
    }
    else {
        ROS_INFO("NOT SET");
    }
    geometry_msgs::Twist cmdVel;
    cmdVel.linear.x = 0.0;
    cmdVel.angular.z = 0.0;
    while (!dwa.isGoalReached())
    {
        // Update costmaps
        costmap_ros->updateMap();

        // Compute velocity commands
        if (dwa.dwaComputeVelocityCommands(globalPose, cmdVel))
        {
            ROS_INFO("DWA compute cmd_vel: SUCCESS");
        }
        else
        {
            ROS_ERROR("DWA compute cmd_vel: FAILED");
        }
        // Send commands
        cmdVelPub.publish(cmdVel);
    }
}

And here is my main file:

#include <Robot.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "main");
    ROS_INFO("Initializing DWA Planner");
    Robot robot;
    robot.dwaPlanner();
    ros::spin();

    return 0;
}

When I launch this code, my launch just gets killed after it sets the plan:

[ INFO] [1651139647.557302447]: Initializing DWA Planner
[ INFO] [1651139647.575485673]: Loading map from image "/home/chinmay/dwa_ws/src/custom_planner/maps/map_3.pgm"
[ INFO] [1651139647.834757522, 304.169000000]: Read a 672 X 608 map @ 0.025 m/cell
[ INFO] [1651139647.844822415, 304.179000000]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1651139647.857668793, 304.192000000]:     Subscribed to Topics: 
[ INFO] [1651139647.883297257, 304.208000000]: global_costmap: Using plugin "inflation_layer"
[ INFO] [1651139647.979511985, 304.301000000]: Sim period is set to 0.05
[ INFO] [1651139648.071583387, 304.395000000]: Got new plan
[ INFO] [1651139648.071736261, 304.395000000]: SET
[custom_planner_node-3] process has died [pid 122672, exit code -11, cmd 
/home/chinmay/dwa_ws/devel/lib/custom_planner/custom_planner_node __name:=custom_planner_node __log:=/home/chinmay/.ros/log/6d935e04-c6d8-11ec-a371-0566167ff1f1/custom_planner_node-3.log].
log file: /home/chinmay/.ros/log/6d935e04-c6d8-11ec-a371-0566167ff1f1/custom_planner_node-3*.log

However, if I use dwa.computeVelocityCommands it enters the while loop and gives me the ROS_ERROR message of could not find a path.

Here is part of the path for your reference, I have published as nav_msgs/Path but to dwa.setPlan I am passing std::vector<geometry_msgs posestamped="">.

header: 
    seq: 0
    stamp: 
        secs: 13
        nsecs: 512000000
    frame_id: "map"
poses: 
    - 
        header: 
            seq: 0
            stamp: 
                secs: 13
                nsecs: 512000000
            frame_id: "map"
        pose: 
            position: 
                 x: 0.20000000298023224
                 y: 3.799999952316284
                 z: 0.0
            orientation: 
                x: 0.0
                y: 0.0
                z: 0.0
                w: 1.0
    - 
        header: 
            seq: 1
            stamp: 
                secs: 13
                nsecs: 512000000 ...