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

fix core dumped err when loadplugin

Browse Source
This commit is contained in:
duongtd
2025-12-01 17:34:23 +07:00
parent 2439f2a71d
commit 11ff1baa79
34 changed files with 1177 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
src/array_parser.cpp Normal file → Executable file
View File

377
src/costmap_2d.cpp Normal file → Executable file
View File

@@ -1,61 +1,74 @@
/*********************************************************************
* License & Authors
* - Cung cấp lớp Costmap2D để lưu trữ bản đồ chi phí 2D
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/costmap_2d.h> // Header định nghĩa lớp Costmap2D
#include <cstdio> // Dùng cho thao tác file (fopen, fprintf, ...)
#include <costmap_2d/costmap_2d.h>
#include <cstdio>
using namespace std;
namespace costmap_2d
{
/*****************************************************
* Hàm khởi tạo chính của Costmap2D
* - Tạo bản đồ kích thước size_x × size_y
* - Gán độ phân giải, gốc tọa độ, và giá trị mặc định
*****************************************************/
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value) :
size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
{
access_ = new mutex_t(); // Khóa bảo vệ truy cập đa luồng
access_ = new mutex_t();
// Cấp phát vùng nhớ cho bản đồ
// create the costmap
initMaps(size_x_, size_y_);
// Gán toàn bộ bản đồ bằng giá trị mặc định (ví dụ: NO_INFORMATION)
resetMaps();
}
/*****************************************************
* Xóa bộ nhớ costmap_
*****************************************************/
void Costmap2D::deleteMaps()
{
boost::unique_lock<mutex_t> lock(*access_); // Giữ mutex để đảm bảo thread-safe
delete[] costmap_; // Giải phóng vùng nhớ
// clean up data
boost::unique_lock<mutex_t> lock(*access_);
delete[] costmap_;
costmap_ = NULL;
}
/*****************************************************
* Cấp phát vùng nhớ cho costmap_
*****************************************************/
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
boost::unique_lock<mutex_t> lock(*access_);
if (costmap_ != nullptr) {
delete[] costmap_;
}
costmap_ = new unsigned char[size_x * size_y]; // Mỗi ô lưu 1 byte (0–255)
delete[] costmap_;
costmap_ = new unsigned char[size_x * size_y];
}
/*****************************************************
* Thay đổi kích thước bản đồ
* - Thường dùng khi tạo lại bản đồ mới hoặc khi map động
*****************************************************/
void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
double origin_x, double origin_y)
{
@@ -65,22 +78,18 @@ void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resol
origin_x_ = origin_x;
origin_y_ = origin_y;
initMaps(size_x, size_y); // Cấp phát vùng nhớ mới
resetMaps(); // Reset toàn bộ dữ liệu
initMaps(size_x, size_y);
// reset our maps to have no information
resetMaps();
}
/*****************************************************
* Reset toàn bộ bản đồ về giá trị mặc định
*****************************************************/
void Costmap2D::resetMaps()
{
boost::unique_lock<mutex_t> lock(*access_);
memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}
/*****************************************************
* Reset một vùng con trong costmap
*****************************************************/
void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
boost::unique_lock<mutex_t> lock(*(access_));
@@ -89,48 +98,50 @@ void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsi
memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}
/*****************************************************
* Tạo bản đồ mới chỉ bao gồm 1 cửa sổ (window) con của map hiện tại
*****************************************************/
bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y,
double win_size_x, double win_size_y)
bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
double win_size_y)
{
// Không thể tạo window của chính mình
// check for self windowing
if (this == &map)
{
// ROS_ERROR("Cannot convert this costmap into a window of itself");
return false;
}
deleteMaps(); // Xóa dữ liệu cũ
// clean up old data
deleteMaps();
// Chuyển đổi tọa độ thế giới sang tọa độ lưới
// compute the bounds of our new map
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
|| !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
{
// ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
return false;
}
// Tính kích thước vùng con
size_x_ = upper_right_x - lower_left_x;
size_y_ = upper_right_y - lower_left_y;
resolution_ = map.resolution_;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;
initMaps(size_x_, size_y_); // Cấp phát bộ nhớ mới
// initialize our various maps and reset markers for inflation
initMaps(size_x_, size_y_);
// Sao chép dữ liệu vùng con từ bản đồ gốc
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_,
costmap_, 0, 0, size_x_, size_x_, size_y_);
// copy the window of the static map and the costmap that we're taking
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
return true;
}
/*****************************************************
* Toán tử gán (=): sao chép toàn bộ dữ liệu từ map khác
*****************************************************/
Costmap2D& Costmap2D::operator=(const Costmap2D& map)
{
// check for self assignement
if (this == &map)
return *this;
deleteMaps(); // Xóa map cũ
// clean up old data
deleteMaps();
size_x_ = map.size_x_;
size_y_ = map.size_y_;
@@ -138,85 +149,62 @@ Costmap2D& Costmap2D::operator=(const Costmap2D& map)
origin_x_ = map.origin_x_;
origin_y_ = map.origin_y_;
initMaps(size_x_, size_y_); // Tạo map mới
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); // Sao chép dữ liệu
// initialize our various maps
initMaps(size_x_, size_y_);
// copy the cost map
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
return *this;
}
/*****************************************************
* Constructor sao chép
*****************************************************/
Costmap2D::Costmap2D(const Costmap2D& map) : costmap_(NULL)
Costmap2D::Costmap2D(const Costmap2D& map) :
costmap_(NULL)
{
access_ = new mutex_t();
*this = map; // Gọi lại toán tử gán
*this = map;
}
/*****************************************************
* Constructor mặc định (chưa có bản đồ)
*****************************************************/
// just initialize everything to NULL by default
Costmap2D::Costmap2D() :
size_x_(0), size_y_(0), resolution_(0.0),
origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
{
access_ = new mutex_t();
}
/*****************************************************
* Destructor: giải phóng bộ nhớ
*****************************************************/
Costmap2D::~Costmap2D()
{
deleteMaps();
delete access_;
}
/*****************************************************
* Chuyển khoảng cách thực (mét) sang đơn vị ô
*****************************************************/
unsigned int Costmap2D::cellDistance(double world_dist)
{
double cells_dist = max(0.0, ceil(world_dist / resolution_));
return (unsigned int)cells_dist;
}
/*****************************************************
* Trả về con trỏ dữ liệu costmap
*****************************************************/
unsigned char* Costmap2D::getCharMap() const
{
return costmap_;
}
/*****************************************************
* Lấy giá trị cost tại ô (mx, my)
*****************************************************/
unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
{
return costmap_[getIndex(mx, my)];
}
/*****************************************************
* Gán giá trị cost tại ô (mx, my)
*****************************************************/
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
costmap_[getIndex(mx, my)] = cost;
}
/*****************************************************
* Chuyển từ tọa độ lưới (ô) sang tọa độ thế giới
*****************************************************/
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{
wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
}
/*****************************************************
* Chuyển từ tọa độ thế giới sang tọa độ lưới
*****************************************************/
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
if (wx < origin_x_ || wy < origin_y_)
@@ -225,143 +213,159 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int&
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
return (mx < size_x_ && my < size_y_);
if (mx < size_x_ && my < size_y_)
return true;
return false;
}
/*****************************************************
* Phiên bản không kiểm tra biên (có thể ra ngoài map)
*****************************************************/
void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
{
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
}
/*****************************************************
* Phiên bản ép buộc tọa độ nằm trong biên bản đồ
*****************************************************/
void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
{
// Here we avoid doing any math to wx,wy before comparing them to
// the bounds, so their values can go out to the max and min values
// of double floating point.
if (wx < origin_x_)
{
mx = 0;
}
else if (wx >= resolution_ * size_x_ + origin_x_)
{
mx = size_x_ - 1;
}
else
{
mx = (int)((wx - origin_x_) / resolution_);
}
if (wy < origin_y_)
{
my = 0;
}
else if (wy >= resolution_ * size_y_ + origin_y_)
{
my = size_y_ - 1;
}
else
{
my = (int)((wy - origin_y_) / resolution_);
}
}
/*****************************************************
* Dịch chuyển gốc của bản đồ khi robot di chuyển
* => Dữ liệu được dịch và giữ lại phần chồng lấn
*****************************************************/
void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
{
int cell_ox = int((new_origin_x - origin_x_) / resolution_);
int cell_oy = int((new_origin_y - origin_y_) / resolution_);
// project the new origin into the grid
int cell_ox, cell_oy;
cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
// Nothing to update
if (cell_ox == 0 && cell_oy == 0)
return;
// Cập nhật lại gốc bản đồ mới (theo bội số của resolution)
double new_grid_ox = origin_x_ + cell_ox * resolution_;
double new_grid_oy = origin_y_ + cell_oy * resolution_;
// compute the associated world coordinates for the origin cell
// because we want to keep things grid-aligned
double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
// To save casting from unsigned int to int a bunch of times
int size_x = size_x_;
int size_y = size_y_;
// Tính vùng chồng lấn giữa bản đồ cũ và bản đồ mới
int lower_left_x = min(max(cell_ox, 0), size_x);
int lower_left_y = min(max(cell_oy, 0), size_y);
int upper_right_x = min(max(cell_ox + size_x, 0), size_x);
int upper_right_y = min(max(cell_oy + size_y, 0), size_y);
// we need to compute the overlap of the new and existing windows
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
lower_left_x = min(max(cell_ox, 0), size_x);
lower_left_y = min(max(cell_oy, 0), size_y);
upper_right_x = min(max(cell_ox + size_x, 0), size_x);
upper_right_y = min(max(cell_oy + size_y, 0), size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// Tạo bản đồ tạm để lưu phần chồng lấn
// we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_,
local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
resetMaps(); // Toàn bộ bản đồ được đặt lại giá trị mặc định
// copy the local window in the costmap to the local map
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
// now we'll set the costmap to be completely unknown if we track unknown space
resetMaps();
// update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy;
// Copy vùng chồng lấn vào vị trí mới
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
copyMapRegion(local_map, 0, 0, cell_size_x,
costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
delete[] local_map; // Giải phóng vùng nhớ tạm
// now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
// make sure to clean up
delete[] local_map;
}
/*****************************************************
* Đặt giá trị cost cho vùng đa giác lồi
* (ví dụ: để đánh dấu vùng cấm hoặc vùng obstacle)
*****************************************************/
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;
for (unsigned int i = 0; i < polygon.size(); ++i)
{
MapLocation loc;
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
return false; // Nếu một điểm nằm ngoài map
{
// ("Polygon lies outside map bounds, so we can't fill it");
return false;
}
map_polygon.push_back(loc);
}
std::vector<MapLocation> polygon_cells;
convexFillCells(map_polygon, polygon_cells); // Lấy toàn bộ ô bên trong đa giác
// get the cells that fill the polygon
convexFillCells(map_polygon, polygon_cells);
// set the cost of those cells
for (unsigned int i = 0; i < polygon_cells.size(); ++i)
costmap_[getIndex(polygon_cells[i].x, polygon_cells[i].y)] = cost_value;
{
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
costmap_[index] = cost_value;
}
return true;
}
/*****************************************************
* polygonOutlineCells():
* - Tìm các ô nằm trên biên của đa giác
*****************************************************/
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon,
std::vector<MapLocation>& polygon_cells)
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
for (unsigned int i = 0; i < polygon.size() - 1; ++i)
{
raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
}
if (!polygon.empty())
{
unsigned int last_index = polygon.size() - 1;
// Nối điểm cuối với điểm đầu để khép kín đa giác
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y,
polygon[0].x, polygon[0].y);
// we also need to close the polygon by going from the last point to the first
raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
}
}
/*****************************************************
* convexFillCells():
* - Điền đầy các ô bên trong một đa giác lồi
*****************************************************/
void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
std::vector<MapLocation>& polygon_cells)
void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
if (polygon.size() < 3) // Ít hơn 3 điểm thì không tạo thành đa giác
// we need a minimum polygon of a triangle
if (polygon.size() < 3)
return;
polygonOutlineCells(polygon, polygon_cells); // Lấy biên ngoài
// first get the cells that make up the outline of the polygon
polygonOutlineCells(polygon, polygon_cells);
// Sắp xếp theo trục X để dễ quét cột
// quick bubble sort to sort points by x
MapLocation swap;
unsigned int i = 0;
while (i < polygon_cells.size() - 1)
@@ -371,20 +375,25 @@ void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
swap = polygon_cells[i];
polygon_cells[i] = polygon_cells[i + 1];
polygon_cells[i + 1] = swap;
if (i > 0) --i;
if (i > 0)
--i;
}
else ++i;
else
++i;
}
// Quét từng cột X để điền đầy Y
i = 0;
MapLocation min_pt, max_pt;
MapLocation min_pt;
MapLocation max_pt;
unsigned int min_x = polygon_cells[0].x;
unsigned int max_x = polygon_cells.back().x;
unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
// walk through each column and mark cells inside the polygon
for (unsigned int x = min_x; x <= max_x; ++x)
{
if (i >= polygon_cells.size() - 1) break;
if (i >= polygon_cells.size() - 1)
break;
if (polygon_cells[i].y < polygon_cells[i + 1].y)
{
@@ -407,37 +416,69 @@ void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
++i;
}
// Điền đầy ô từ min_y đến max_y
MapLocation pt;
// loop though cells in the column
for (unsigned int y = min_pt.y; y <= max_pt.y; ++y)
polygon_cells.push_back({x, y});
{
pt.x = x;
pt.y = y;
polygon_cells.push_back(pt);
}
}
}
/*****************************************************
* Các hàm getter cơ bản
*****************************************************/
unsigned int Costmap2D::getSizeInCellsX() const { return size_x_; }
unsigned int Costmap2D::getSizeInCellsY() const { return size_y_; }
double Costmap2D::getSizeInMetersX() const { return (size_x_ - 1 + 0.5) * resolution_; }
double Costmap2D::getSizeInMetersY() const { return (size_y_ - 1 + 0.5) * resolution_; }
double Costmap2D::getOriginX() const { return origin_x_; }
double Costmap2D::getOriginY() const { return origin_y_; }
double Costmap2D::getResolution() const { return resolution_; }
unsigned int Costmap2D::getSizeInCellsX() const
{
return size_x_;
}
unsigned int Costmap2D::getSizeInCellsY() const
{
return size_y_;
}
double Costmap2D::getSizeInMetersX() const
{
return (size_x_ - 1 + 0.5) * resolution_;
}
double Costmap2D::getSizeInMetersY() const
{
return (size_y_ - 1 + 0.5) * resolution_;
}
double Costmap2D::getOriginX() const
{
return origin_x_;
}
double Costmap2D::getOriginY() const
{
return origin_y_;
}
double Costmap2D::getResolution() const
{
return resolution_;
}
/*****************************************************
* Lưu costmap ra file PGM (grayscale) để debug
*****************************************************/
bool Costmap2D::saveMap(std::string file_name)
{
FILE *fp = fopen(file_name.c_str(), "w");
if (!fp)
{
return false;
}
fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
for (unsigned int iy = 0; iy < size_y_; iy++)
{
for (unsigned int ix = 0; ix < size_x_; ix++)
fprintf(fp, "%d ", getCost(ix, iy));
{
unsigned char cost = getCost(ix, iy);
fprintf(fp, "%d ", cost);
}
fprintf(fp, "\n");
}
fclose(fp);

43
src/costmap_2d_robot.cpp
View File

@@ -48,6 +48,8 @@
#include <tf3/utils.h>
#include <exception>
#include <boost/dll/import.hpp>
using namespace std;
namespace costmap_2d
@@ -97,19 +99,19 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
robot::Time last_error = robot::Time::now();
std::string tf_error;
// we need to make sure that the transform between the robot base frame and the global frame is available
while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
{
if (last_error + robot::Duration(5.0) < robot::Time::now())
{
printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = robot::Time::now();
}
// The error string will accumulate and errors will typically be the same, so the last
// will do for the warning above. Reset the string here to avoid accumulation.
tf_error.clear();
}
// // we need to make sure that the transform between the robot base frame and the global frame is available
// while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
// {
// if (last_error + robot::Duration(5.0) < robot::Time::now())
// {
// printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
// robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
// last_error = robot::Time::now();
// }
// // The error string will accumulate and errors will typically be the same, so the last
// // will do for the warning above. Reset the string here to avoid accumulation.
// tf_error.clear();
// }
// check if we want a rolling window version of the costmap
bool rolling_window, track_unknown_space, always_send_full_costmap;
@@ -121,10 +123,10 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
{"./costmap_2d/libstatic_layer.so", "create_static_layer"},
{"./costmap_2d/libinflation_layer.so", "create_inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "create_obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "create_preferred_layer"}
};
// if (private_nh.hasParam("plugins"))
@@ -140,10 +142,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
try
{
// copyParentParameters(pname, type, private_nh);
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
info.path, "create_plugin", boost::dll::load_mode::append_decorations
creators_.push_back(
boost::dll::import_alias<PluginLayerPtr()>(
info.path, info.name, boost::dll::load_mode::append_decorations)
);
PluginLayerPtr plugin = creator();
PluginLayerPtr plugin = creators_.back()();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_);
layered_costmap_->addPlugin(plugin);

0
src/costmap_layer.cpp Normal file → Executable file
View File

0
src/costmap_math.cpp Normal file → Executable file
View File

137
src/footprint.cpp Normal file → Executable file
View File

@@ -27,12 +27,13 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <costmap_2d/costmap_math.h>
#include<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<geometry_msgs/Point32.h>
namespace costmap_2d
{
@@ -173,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
if (error != "")
{
std::printf("Error parsing footprint parameter: '%s'", error.c_str());
std::printf(" Footprint string was '%s'.", footprint_string.c_str());
printf("Error parsing footprint parameter: '%s'\n", error.c_str());
printf(" Footprint string was '%s'.\n", footprint_string.c_str());
return false;
}
// convert vvf into points.
if (vvf.size() < 3)
{
std::printf("You must specify at least three points for the robot footprint, reverting to previous footprint.");
printf("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
return false;
}
footprint.reserve(vvf.size());
@@ -197,7 +198,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
}
else
{
std::printf("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.",
printf("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.\n",
int(vvf[ i ].size()));
return false;
}
@@ -208,66 +209,66 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name)
{
std::string full_param_name;
std::string full_radius_param_name;
std::vector<geometry_msgs::Point> points;
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
// {
// std::string full_param_name;
// std::string full_radius_param_name;
// std::vector<geometry_msgs::Point> points;
// if (nh.searchParam("footprint", full_param_name))
// {
// XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// {
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// if (nh.searchParam("footprint", full_param_name))
// {
// XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// {
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// if (nh.searchParam("robot_radius", full_radius_param_name))
// {
// double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius);
// }
// // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points;
}
// if (nh.searchParam("robot_radius", full_radius_param_name))
// {
// double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius);
// }
// // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points;
// }
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
{
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
// first = false;
// }
// else
// {
// oss << ",[" << p.x << "," << p.y << "]";
// }
// }
// oss << "]";
// nh.setParam("footprint", oss.str().c_str());
}
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
// {
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
// first = false;
// }
// else
// {
// oss << ",[" << p.x << "," << p.y << "]";
// }
// }
// oss << "]";
// nh.setParam("footprint", oss.str().c_str());
// }
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
{
@@ -276,9 +277,9 @@ double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_p
value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
{
std::string& value_string = value;
std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
full_param_name.c_str(), value_string.c_str());
throw std::runtime_error("Values in the footprint specification must be numbers\n");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
@@ -290,10 +291,10 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
footprint_xmlrpc.size() < 3)
{
std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]\n");
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
std::vector<geometry_msgs::Point> footprint;
@@ -306,11 +307,11 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& f
if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
point.size() != 2)
{
std::printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n",
full_param_name.c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form\n");
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
}
pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);

31
src/layer.cpp Normal file → Executable file
View File

@@ -1,10 +1,37 @@
/*
* Copyright (c) 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "costmap_2d/layer.h"
namespace costmap_2d
{
Layer::Layer()
: layered_costmap_(NULL)
, current_(false)

54
src/layered_costmap.cpp Normal file → Executable file
View File

@@ -1,10 +1,46 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/footprint.h>
#include <cstdio>
#include <string>
#include <algorithm>
#include <vector>
#include <memory>
using std::vector;
@@ -49,7 +85,7 @@ namespace costmap_2d
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
size_locked_ = size_locked;
costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
(*plugin)->matchSize();
@@ -76,7 +112,7 @@ namespace costmap_2d
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if (!(*plugin)->isEnabled())
@@ -88,8 +124,8 @@ namespace costmap_2d
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
{
std::printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_,
(*plugin)->getName().c_str());
@@ -105,14 +141,14 @@ namespace costmap_2d
y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
std::printf("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
if (xn < x0 || yn < y0)
return;
costmap_.resetMap(x0, y0, xn, yn);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if ((*plugin)->isEnabled())
@@ -130,7 +166,7 @@ namespace costmap_2d
bool LayeredCostmap::isCurrent()
{
current_ = true;
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if ((*plugin)->isEnabled())
@@ -144,7 +180,7 @@ namespace costmap_2d
footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
for (vector<costmap_2d::PluginLayerPtr>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
(*plugin)->onFootprintChanged();

106
src/observation_buffer.cpp Normal file → Executable file
View File

@@ -1,11 +1,44 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <costmap_2d/observation_buffer.h>
#include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include<tf3/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint>
using namespace std;
using namespace tf3;
@@ -17,8 +50,7 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
string sensor_frame, double tf_tolerance) :
tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(robot::Time::now()),
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
{
@@ -30,11 +62,11 @@ ObservationBuffer::~ObservationBuffer()
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
robot::Time transform_time = robot::Time::now();
tf3::Time transform_time = tf3::Time::now();
std::string tf_error;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
geometry_msgs::TransformStamped transformStamped;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
@@ -50,21 +82,27 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
geometry_msgs::PointStamped origin;
origin.header.frame_id = global_frame_;
origin.header.stamp = transform_time;
origin.header.stamp = data_convert::convertTime(transform_time);
origin.point = obs.origin_;
// we need to transform the origin of the observation to the new global frame
tf3::doTransform(origin, origin,
tf3_buffer_.lookupTransform(new_global_frame,
origin.header.frame_id,
data_convert::convertTime(origin.header.stamp)));
// tf3_buffer_.transform(origin, origin, new_global_frame);
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
origin.header.frame_id, // frame nguồn
transform_time
);
tf3::doTransform(origin, origin, tfm_1);
obs.origin_ = origin.point;
// we also need to transform the cloud of the observation to the new global frame
tf3::doTransform(*(obs.cloud_), *(obs.cloud_),
tf3_buffer_.lookupTransform(new_global_frame,
obs.cloud_->header.frame_id,
data_convert::convertTime(obs.cloud_->header.stamp)));
// tf3_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
obs.cloud_->header.frame_id, // frame nguồn
transform_time
);
tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
}
catch (TransformException& ex)
{
@@ -98,11 +136,19 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
tf3::doTransform(local_origin, global_origin,
tf3_buffer_.lookupTransform(global_frame_,
local_origin.header.frame_id,
data_convert::convertTime(local_origin.header.stamp)));
// tf3_buffer_.transform(local_origin, global_origin, global_frame_);
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
local_origin.header.frame_id, // frame nguồn
data_convert::convertTime(local_origin.header.stamp)
);
tf3::doTransform(local_origin, global_origin, tfm_1);
/////////////////////////////////////////////////
///////////chú ý hàm này/////////////////////////
tf3::convert(global_origin.point, observation_list_.front().origin_);
/////////////////////////////////////////////////
/////////////////////////////////////////////////
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
observation_list_.front().raytrace_range_ = raytrace_range_;
@@ -111,10 +157,13 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
tf3::doTransform(cloud, global_frame_cloud,
tf3_buffer_.lookupTransform(global_frame_,
(cloud.header.frame_id),
data_convert::convertTime(cloud.header.stamp)));
// tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
cloud.header.frame_id, // frame nguồn
data_convert::convertTime(cloud.header.stamp)
);
tf3::doTransform(cloud, global_frame_cloud, tfm_2);
global_frame_cloud.header.stamp = cloud.header.stamp;
// now we need to remove observations from the cloud that are below or above our height thresholds
@@ -213,13 +262,12 @@ bool ObservationBuffer::isCurrent() const
if (expected_update_rate_ == robot::Duration(0.0))
return true;
bool current = (robot::Time::now() - last_updated_) <= expected_update_rate_;
bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
if (!current)
{
printf(
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(),
expected_update_rate_.toSec());
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
}
return current;
}