DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Hiep update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 10:24:18 +07:00
parent 4246453ae6
commit 72b2f3c639
49 changed files with 476 additions and 476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/array_parser.cpp
View File

@@ -34,7 +34,7 @@
#include <sstream>
#include <vector>
namespace costmap_2d
namespace robot_costmap_2d
{
/** @brief Parse a vector of vector of floats from a string.
@@ -112,4 +112,4 @@ std::vector<std::vector<float> > parseVVF(const std::string& input, std::string&
return result;
}
} // end namespace costmap_2d
} // end namespace robot_costmap_2d

6
src/costmap_2d.cpp
View File

@@ -35,12 +35,12 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <cstdio>
using namespace std;
namespace costmap_2d
namespace robot_costmap_2d
{
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value) :
@@ -485,4 +485,4 @@ bool Costmap2D::saveMap(std::string file_name)
return true;
}
} // namespace costmap_2d
} // namespace robot_costmap_2d

14
src/costmap_2d_robot.cpp
View File

@@ -41,9 +41,9 @@
#include <algorithm>
#include <vector>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/utils.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/utils.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
@@ -53,7 +53,7 @@
using namespace std;
namespace costmap_2d
namespace robot_costmap_2d
{
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
layered_costmap_(NULL),
@@ -83,11 +83,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
{
try
{
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["costmap_2d"];
YAML::Node layer = config["robot_costmap_2d"];
std::string global_frame =
loadParam(layer, "global_frame", std::string("map"));
@@ -567,4 +567,4 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point
padded_footprint_, oriented_footprint);
}
} // namespace costmap_2d
} // namespace robot_costmap_2d

18
src/costmap_layer.cpp
View File

@@ -1,6 +1,6 @@
#include<costmap_2d/costmap_layer.h>
#include<robot_costmap_2d/costmap_layer.h>
namespace costmap_2d
namespace robot_costmap_2d
{
void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
@@ -60,7 +60,7 @@ void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, d
has_extra_bounds_ = false;
}
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
void CostmapLayer::updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
@@ -100,7 +100,7 @@ void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i,
}
}
void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
void CostmapLayer::updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j)
{
if (!enabled_)
@@ -119,7 +119,7 @@ void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, i
}
}
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
void CostmapLayer::updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
@@ -138,7 +138,7 @@ void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int m
}
}
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
void CostmapLayer::updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
@@ -161,8 +161,8 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
else
{
int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
if (sum >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else
master_array[it] = sum;
}
@@ -170,4 +170,4 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
}
}
}
} // namespace costmap_2d
} // namespace robot_costmap_2d

2
src/costmap_math.cpp
View File

@@ -27,7 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <costmap_2d/costmap_math.h>
#include <robot_costmap_2d/costmap_math.h>
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
{

10
src/footprint.cpp
View File

@@ -27,15 +27,15 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <costmap_2d/costmap_math.h>
#include <robot_costmap_2d/costmap_math.h>
#include <boost/tokenizer.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_costmap_2d/array_parser.h>
#include <robot_geometry_msgs/Point32.h>
namespace costmap_2d
namespace robot_costmap_2d
{
void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
@@ -301,4 +301,4 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
return footprint;
}
} // end namespace costmap_2d
} // end namespace robot_costmap_2d

6
src/layer.cpp
View File

@@ -27,9 +27,9 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "costmap_2d/layer.h"
#include "robot_costmap_2d/layer.h"
namespace costmap_2d
namespace robot_costmap_2d
{
Layer::Layer()
@@ -53,4 +53,4 @@ const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
return layered_costmap_->getFootprint();
}
} // end namespace costmap_2d
} // end namespace robot_costmap_2d

10
src/layered_costmap.cpp
View File

@@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/footprint.h>
#include <cstdio>
#include <string>
#include <algorithm>
@@ -44,7 +44,7 @@
using std::vector;
namespace costmap_2d
namespace robot_costmap_2d
{
LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
@@ -178,7 +178,7 @@ namespace costmap_2d
void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec)
{
footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
robot_costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
@@ -187,4 +187,4 @@ namespace costmap_2d
}
}
} // namespace costmap_2d
} // namespace robot_costmap_2d

10
src/observation_buffer.cpp
View File

@@ -34,16 +34,16 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace tf3;
namespace costmap_2d
namespace robot_costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
@@ -276,5 +276,5 @@ void ObservationBuffer::resetLastUpdated()
{
last_updated_ = robot::Time::now();
}
} // namespace costmap_2d
} // namespace robot_costmap_2d