I'm currently working with the DWAPLANNER for a person-following robot in ROS, and I'm eager to gain a deeper understanding of how this planner incorporates a robot's footprint into trajectory calculations. My goal is to make substantial modifications to the ROS navigation stack for this specific use case. Additionally, I'm keen to comprehend how this footprint interacts with the costmap. Would anyone be able to provide an in-depth explanation or direct me to valuable resources that can illuminate these two critical aspects of the DWAPLANNER in ROS? Your assistance would be greatly appreciated. Thank you!
1 Answer
$\begingroup$ $\endgroup$
From the source code
//we also want to clear the robot footprint from the costmap we're using costmap_ros_->clearRobotFootprint(); https://github.com/strawlab/navigation/blob/master/dwa_local_planner/src/dwa_planner_ros.cpp
void Costmap2DROS::clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose){ std::vector<geometry_msgs::Point> oriented_footprint; //check if we have a circular footprint or a polygon footprint if(footprint_spec_.size() < 3){ //we'll build an approximation of the circle as the footprint and clear that double angle = 0; double step = 2 * M_PI / 72; while(angle < 2 * M_PI){ geometry_msgs::Point pt; pt.x = getInscribedRadius() * cos(angle) + global_pose.getOrigin().x(); pt.y = getInscribedRadius() * sin(angle) + global_pose.getOrigin().y(); pt.z = 0.0; oriented_footprint.push_back(pt); angle += step; } } https://docs.ros.org/en/electric/api/costmap_2d/html/classcostmap__2d_1_1Costmap2DROS.html
https://github.com/strawlab/navigation/blob/master/costmap_2d/src/costmap_2d_ros.cpp
Someone gave you a down vote because there is an expectation the questions are the results of failed efforts where the OP has explained what they have tried, but couldn't, do.
