DWA local planner as a stand alone C++ library [closed]

edit

Hi,

I am trying to write a C++ library that uses the DWA local planner C++ API. Basically, the there would be a function in the library that accepts the global waypoints, robot current position and velocity, local occupancy grid and a set of costmap and planner configuration; the function outputs the command velocity. This library will abstract away the ROS communication part of the DWA ROS C++ API. Later, I will make the library and use it as an external function to work in an python program.

Currently, I am writing a C++ node to test the DWA function. I follows the DWA ROS C++ API implementation and made some modifications. All the data are correctly passed into the DWA C++ API, but it is unable to plan a path, returning negative path cost. I have also tried planning using an empty occupancy grid, but the result is same, unable to find a apth. What could be some possible problem? Any help is appreciated! I have attached the code and the terminal output. Please let me know if I can provide more informations.

The terminal output has:

dwa_local_planner Received a transformed plan with 128 points.
vel
[ERROR] [1619110115.088004617, 16.700000000]: Best: 0.00, 0.00, 0.00, -7.00
Evaluated 315 trajectories, found 0 validcmd
0  0 (cmd_vel)
received plan of length 208

The code:

#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/NavSatFix.h>
#include <signal.h>
#include <std_msgs/String.h>
#include <tf/transform_broadcaster.h>
#include "ros/ros.h"

#include <iostream>
#include <cstring>
#include <math.h>
#include <stdio.h>
#include <sstream>

#include <dwa_local_planner/dwa_planner.h>
#include <base_local_planner/goal_functions.h>
#include <nav_core/base_local_planner.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <costmap_2d/static_layer.h>
#include <cmath>

#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>

#include <tf2_ros/buffer.h>

#include <dynamic_reconfigure/server.h>
#include <dwa_local_planner/DWAPlannerConfig.h>

#include <angles/angles.h>

#include <vector>

#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>

#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/footprint.h>
#include <nav_core/base_local_planner.h>
#include <base_local_planner/latched_stop_rotate_controller.h>

#include <dwa_local_planner/dwa_planner.h>

using namespace dwa_local_planner;

geometry_msgs::Twist cmd_vel;
geometry_msgs::PoseStamped robot_vel;
tf2_ros::Buffer tf_;
nav_msgs::OccupancyGrid::ConstPtr og;
ros::Publisher costmap_pub;
std::string name = "dwa_planner";
std::vector<geometry_msgs::PoseStamped> global_plan;
std::string global_frame_ = "map";
std::string robot_base_frame_ = "base_link";
bool receivedPlan = false;

/*
given costmap, global reference path, current cmd_vel, find best cmd_vel and publish
*/

/*
local costmap callback
*/
void localCostmapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
    std::cout<< "costmap callback!" << std::endl;
    if (!receivedPlan) return;
    og = msg;

    base_local_planner::LocalPlannerUtil planner_util_;
    costmap_2d::Costmap2D* costmap = new costmap_2d::Costmap2D(msg);

    planner_util_.initialize(&tf_, costmap, "map");
    DWAPlanner* dp_ = new DWAPlanner(name, &planner_util_);

    costmap_2d::Costmap2D* cm = planner_util_.getCostmap();

    // set dwa planner config
    DWAPlannerConfig config;
    config.sim_time = 1.7;
    config.sim_granularity = 0.025;
    config.angular_sim_granularity = 0.1;
    config.path_distance_bias = 24;
    config.goal_distance_bias = 32 ...