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;
    config.occdist_scale = 0.01;
    config.twirling_scale = 0.0;
    config.stop_time_buffer = 0.2;
    config.oscillation_reset_dist = 0.05;
    config.oscillation_reset_angle = 0.2;
    config.forward_point_distance = 0.325;
    config.scaling_speed = 0.25;
    config.max_scaling_factor = 0.2;
    config.vx_samples = 3;
    config.vy_samples = 10;
    config.vth_samples = 20;
    config.use_dwa = true;
    config.restore_defaults = false;

    printf("config\n");
    // std::cout << config.vx_samples << " " << config.vy_samples << " " << config.vth_samples << std::endl;
    // std::cout << config.sim_time << " " << config.sim_granularity << " " << config.path_distance_bias << std::endl;

    // update generic local planner params
    base_local_planner::LocalPlannerLimits limits;
    limits.max_vel_trans = 0.65;
    limits.min_vel_trans = 0.1;
    limits.max_vel_x = 0.65;
    limits.min_vel_x = 0.0;
    limits.max_vel_y = 0.1;
    limits.min_vel_y = -0.1;
    limits.max_vel_theta = 1.0;
    limits.min_vel_theta = 0.4;
    limits.acc_lim_x = 2.5;
    limits.acc_lim_y = 2.5;
    limits.acc_lim_theta = 3.2;
    limits.acc_lim_trans = 2.5;
    limits.xy_goal_tolerance = 0.2;
    limits.yaw_goal_tolerance = 0.17;
    limits.prune_plan = false;
    limits.trans_stopped_vel = 0.01;
    limits.theta_stopped_vel = 0.01;
    planner_util_.reconfigureCB(limits, config.restore_defaults);

    std::cout << limits.max_vel_trans << " " << limits.min_vel_trans << " " << limits.max_vel_x << std::endl;

    // update dwa specific configuration
    dp_->reconfigure(config);

    dp_->setPlan(global_plan);

    // get current pose
    bool gotPose = false;
    geometry_msgs::PoseStamped current_pose_;
    geometry_msgs::PoseStamped robot_pose;
    robot_pose.header.frame_id = robot_base_frame_;
    robot_pose.header.stamp = ros::Time();
    ros::Time current_time = ros::Time::now();

    try {
        if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
        {
            geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
            tf2::doTransform(robot_pose, current_pose_, transform);
        }
        // use the latest otherwise
        else
        {
            tf_.transform(robot_pose, current_pose_, global_frame_);
        }   
        gotPose = true;
        std::cout << current_pose_.pose.position.x << "  " << current_pose_.pose.position.y << " " << current_pose_.pose.position.z << std::endl;
    }
    catch (tf2::LookupException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    }
    catch (tf2::ConnectivityException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    }
    catch (tf2::ExtrapolationException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    }

    //transformed plan
    bool gotPlan = false;
    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
      ROS_ERROR("Could not get local plan");
    } else {
        gotPlan = true;
    }

    if(transformed_plan.empty()) {
      ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
    }

    printf("dwa_local_planner Received a transformed plan with %zu points.\n", transformed_plan.size());


    // robot foot print
    std::string foorprint = "[[-0.1,-0.1], [0.1,-0.1], [0.1,0.1], [-0.1,0.1]]";
    double padding = 0.01;
    std::vector<geometry_msgs::Point> padded_footprint_;
    costmap_2d::makeFootprintFromString(foorprint, padded_footprint_);
    costmap_2d::padFootprint(padded_footprint_, padding);

    if (gotPose && gotPlan) {
        dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, padded_footprint_);

        // compute velocity command
        geometry_msgs::PoseStamped drive_cmds;
        drive_cmds.header.frame_id = "base_link";

        printf("vel\n");
        std::cout << robot_vel.pose.position.x << "  " << robot_vel.pose.position.y << " " << robot_vel.pose.position.z << std::endl;

        // call with updated footprint
        base_local_planner::Trajectory path = dp_->findBestPath(current_pose_, robot_vel, drive_cmds);
        ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);

        //pass along drive commands

        cmd_vel.linear.x = drive_cmds.pose.position.x;
        cmd_vel.linear.y = drive_cmds.pose.position.y;
        cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);

        printf("cmd\n");
        std::cout << drive_cmds.pose.position.x << "  " << drive_cmds.pose.position.y << std::endl;
        //std::cout << cmd_vel.linear.x << "  " << cmd_vel.linear.y << " " << cmd_vel.angular.z << std::endl;
    }

    delete costmap;
    delete dp_;
}

/*
global plan callback
*/
void globalPlanCallback(const nav_msgs::PathConstPtr &msg) {
    global_plan = msg->poses;
    std::cout << "received plan of length " << global_plan.size() << std::endl;
    receivedPlan = true;
}

/*
Odom callback
*/
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    robot_vel.pose.position.x = msg->twist.twist.linear.x;
    robot_vel.pose.position.y = msg->twist.twist.linear.y;
    robot_vel.pose.position.z = 0;
    robot_vel.header.frame_id = msg->child_frame_id;
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->twist.twist.angular.z);
    tf2::convert(q, robot_vel.pose.orientation);
    robot_vel.header.stamp = ros::Time();
    //printf("vel\n");
    //std::cout << robot_vel.pose.position.x << "  " << robot_vel.pose.position.y << " " << robot_vel.pose.position.z << std::endl;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "dwa_test");
    ros::NodeHandle n;
    ros::Publisher dwa_plan_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 50); //
    costmap_pub = n.advertise<nav_msgs::OccupancyGrid>("/local_costmap", 50);
    ros::Subscriber costmapSub = n.subscribe("/move_base/local_costmap/costmap", 100, localCostmapCallback);
    ros::Subscriber globalPlanSub = n.subscribe("/move_base/GlobalPlanner/plan", 100, globalPlanCallback);
    ros::Subscriber odomSub = n.subscribe("/odom", 100, odomCallback);

    ros::Rate loop_rate(60);

    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0;
    cmd_vel.angular.z = 0;

    // tf listener
    tf2_ros::TransformListener tfListener(tf_);

    while (ros::ok()) {
        //printf("publishing\n");
        dwa_plan_pub.publish(cmd_vel);

        ros::spinOnce(); 
        loop_rate.sleep();
    }
    return 0;

}