DWA local planner as a stand alone C++ library [closed]
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 ...