How to use dwa_local_planner ROS C++ API
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 ...
Is there some reason you don't want to just use the
move_base
ros node? I have never seen any guidelines for using this local planner as a "standalone" class. I suspect you will need to study themove_base
source code and comments to understand how the interaction is supposed to be done.