本文整理汇总了C++中costmap_2d::Costmap2D类的典型用法代码示例。如果您正苦于以下问题:C++ Costmap2D类的具体用法?C++ Costmap2D怎么用?C++ Costmap2D使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Costmap2D类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: canMove
bool ChooseAccessibleBalls::canMove(float x, float y){
return true;
// ROS_INFO("enter canMove");
// Get a copy of the current costmap to test. (threadsafe)
costmap_2d::Costmap2D costmap;
// ROS_INFO("aaa");
if(costmap_ros != NULL)
costmap_ros->getCostmapCopy( costmap );
// ROS_INFO("bbb");
// Coordinate transform.
unsigned int cell_x, cell_y;
if( !costmap.worldToMap( x, y, cell_x, cell_y )){
return false;
}
double cost = double( costmap.getCost( cell_x, cell_y ));
if(cost <= 127){
return true;
}
else{
return false;
}
}
开发者ID:RCPRG-ros-pkg,项目名称:elektron_ballcollector,代码行数:25,代码来源:choose_accessible_balls.cpp
示例2: updateWithAddition
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION)
master_array[it] = costmap_[it];
else
{
int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else
master_array[it] = sum;
}
it++;
}
}
}
开发者ID:Benson516,项目名称:navigation_old_kinetic,代码行数:32,代码来源:costmap_layer.cpp
示例3: updateWithMax
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
master_array[it] = costmap_[it];
it++;
}
}
}
开发者ID:Benson516,项目名称:navigation_old_kinetic,代码行数:25,代码来源:costmap_layer.cpp
示例4: upTo
void upTo(costmap_2d::Costmap2D &gradient_){
for (int varX = 0; varX < gradient_.getSizeInCellsX(); varX++) {
for (int varY = 0; varY < gradient_.getSizeInCellsY(); varY++) {
//if(gradient_.getCost(varX, varY) == 0)
gradient_.setCost(varX, varY, DEF);
}
}
}
开发者ID:aortizgu,项目名称:ROS,代码行数:9,代码来源:csuro_global_planner.cpp
示例5: isCoordInMap
bool CSUROGlobalPlanner::isCoordInMap(const costmap_2d::Costmap2D &costmap, const float& x, const float& y)
{
int cellI, cellJ;
original_costmap_.worldToMapEnforceBounds(x, y, cellI, cellJ);
if(cellI < costmap.getSizeInCellsX() && cellI >= 0 && cellJ < costmap.getSizeInCellsY() && cellJ >= 0)
return true;
return false;
}
开发者ID:aortizgu,项目名称:ROS,代码行数:10,代码来源:csuro_global_planner.cpp
示例6: updateCosts
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
master_grid.setCost(mx, my, LETHAL_OBSTACLE);
}
}
开发者ID:gempio,项目名称:MSci-Project,代码行数:11,代码来源:simple_layer.cpp
示例7: inMap
bool inMap(costmap_2d::Costmap2D &gradient_, int i, int j)
{
if(i < gradient_.getSizeInCellsX() && i >= 0 && j < gradient_.getSizeInCellsY() && j >= 0)
{
return true;
}
else
{
return false;
}
}
开发者ID:aortizgu,项目名称:ROS,代码行数:11,代码来源:csuro_global_planner.cpp
示例8: updateCosts
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!map_received_)
return;
if (!layered_costmap_->isRolling())
{
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
if (!use_maximum_)
updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
else
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
}
else
{
// If rolling window, the master_grid is unlikely to have same coordinates as this layer
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
tf::StampedTransform transform;
try
{
tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
return;
}
// Copy map data given proper transformations
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
{
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf::Point p(wx, wy, 0);
p = transform(p);
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
if (!use_maximum_)
master_grid.setCost(i, j, getCost(mx, my));
else
master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
}
}
}
}
}
开发者ID:Benson516,项目名称:navigation_old_kinetic,代码行数:51,代码来源:static_layer.cpp
示例9: setTargetCells
//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size())
{
ROS_DEBUG ("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (i = 0; i < adjusted_global_plan.size(); ++i)
{
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
{
MapCell& current = getCell (map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
started_path = true;
}
else if (started_path)
{
break;
}
}
if (!started_path)
{
ROS_ERROR ("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
computeTargetDistance (path_dist_queue, costmap);
}
开发者ID:rsbGroup1,项目名称:frobo_rsd,代码行数:50,代码来源:map_grid.cpp
示例10: getCost
double getCost(float x, float y){
// Get a copy of the current costmap to test. (threadsafe)
costmap_2d::Costmap2D costmap;
costmap_ros->getCostmapCopy( costmap );
// Coordinate transform.
unsigned int cell_x, cell_y;
if( !costmap.worldToMap( x, y, cell_x, cell_y )){
return -1.0;
}
double cost = double( costmap.getCost( cell_x, cell_y ));
// ROS_INFO(" world pose = (%f, %f) map pose = (%d, %d) cost =%f", x, y, cell_x, cell_y, cost);
return cost;
}
开发者ID:RCPRG-ros-pkg,项目名称:elektron_ballcollector,代码行数:15,代码来源:explore.cpp
示例11: updatePathCell
inline bool MapGrid::updatePathCell (MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap)
{
//if the cell is an obstacle set the max path distance
unsigned char cost = costmap.getCost (check_cell->cx, check_cell->cy);
if (! getCell (check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION))
{
check_cell->target_dist = obstacleCosts();
return false;
}
double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist)
{
check_cell->target_dist = new_target_dist;
}
return true;
}
开发者ID:rsbGroup1,项目名称:frobo_rsd,代码行数:25,代码来源:map_grid.cpp
示例12: updateWithOverwrite
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = span*j+min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] != NO_INFORMATION)
master[it] = costmap_[it];
it++;
}
}
}
开发者ID:Benson516,项目名称:navigation_old_kinetic,代码行数:18,代码来源:costmap_layer.cpp
示例13: updateCosts
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(pode_marcar){
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
for(int i = -1; i < 1; i++)
master_grid.setCost(mx+5, my+i, LETHAL_OBSTACLE);
}
pode_marcar = false;
}
// ROS_INFO("Hi!");
}
开发者ID:air-lasca,项目名称:air1,代码行数:18,代码来源:simple_layer.cpp
示例14: updateCosts
void BallPickerLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
const unsigned char* master_array = master_grid.getCharMap();
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
if (clear_flag)
{
master_grid.setCost(i, j, FREE_SPACE);
}
else
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION || old_cost < costmap_[index])
master_grid.setCost(i, j, costmap_[index]);
}
}
}
for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++)
costmap_[i] == NO_INFORMATION;
if (!obstacle_buffer.empty())
{
ROS_INFO("Costmap updated with ball obstacles.");
obstacle_buffer.clear();
std_srvs::Empty emptysrv;
confirm_update_client.call(emptysrv);
}
}
开发者ID:ZdenekM,项目名称:but_robot_demos,代码行数:43,代码来源:ball_picker_layer.cpp
示例15: go
bool go(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int i, int j, int cost)
{
if(inMap(gradient_, i, j) == false){
//ROS_INFO("salgo por no estar en mapa");
return false;
}
if(original_.getCost( i, j) != 0 ){
//ROS_INFO("salgo por ser obstaculo");
return false;
}
if(gradient_.getCost( i, j) <= cost){
//ROS_INFO("salgo por ser igual o menor");
return false;
}
return true;
}
开发者ID:aortizgu,项目名称:ROS,代码行数:19,代码来源:csuro_global_planner.cpp
示例16: canMove
bool canMove(float x, float y){
// Get a copy of the current costmap to test. (threadsafe)
costmap_2d::Costmap2D costmap;
costmap_ros->getCostmapCopy( costmap );
// Coordinate transform.
unsigned int cell_x, cell_y;
if( !costmap.worldToMap( x, y, cell_x, cell_y )){
// res.cost = -1.0;
return false;
}
double cost = double( costmap.getCost( cell_x, cell_y ));
ROS_INFO("cost = %f", cost);
if(cost <= 1){
return true;
}
else{
return false;
}
//ROS_INFO(" world pose = (%f, %f) map pose = (%d, %d) cost =%f", x, y, cell_x, cell_y, cost);
}
开发者ID:RCPRG-ros-pkg,项目名称:elektron_ballcollector,代码行数:22,代码来源:explore.cpp
示例17: updateCosts
void VirtualWallLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
master_grid.setCost(i, j, costmap_[index]);
}
}
}
开发者ID:vicoslab,项目名称:vicos_ros,代码行数:16,代码来源:virtual_wall_layer.cpp
示例18: setLocalGoal
//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i)
{
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
{
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
}
else
{
if (started_path)
{
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path)
{
ROS_ERROR ("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
queue<MapCell*> path_dist_queue;
if (local_goal_x >= 0 && local_goal_y >= 0)
{
MapCell& current = getCell (local_goal_x, local_goal_y);
costmap.mapToWorld (local_goal_x, local_goal_y, goal_x_, goal_y_);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
}
computeTargetDistance (path_dist_queue, costmap);
}
开发者ID:rsbGroup1,项目名称:frobo_rsd,代码行数:54,代码来源:map_grid.cpp
示例19: spread
void spread(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int value, int i, int j, int startCellI, int startCellJ)
{
gradient_.setCost(i, j, value);
//ROS_INFO("(%d, %d) <- %d", i, j, value);
value = value + 5;
if(value>254)
value=254;
if(go(gradient_, original_, i-1,j, value))
spread(gradient_, original_, value, i-1, j, startCellI, startCellJ);
if(go(gradient_, original_, i,j-1, value))
spread(gradient_, original_, value, i, j-1, startCellI, startCellJ);
if(go(gradient_, original_, i+1,j, value))
spread(gradient_, original_, value, i+1, j, startCellI, startCellJ);
if(go(gradient_, original_, i,j+1, value))
spread(gradient_, original_, value, i, j+1, startCellI, startCellJ);
}
开发者ID:aortizgu,项目名称:ROS,代码行数:22,代码来源:csuro_global_planner.cpp
示例20: fillPlan
void fillPlan(costmap_2d::Costmap2D gradient_, std::vector<geometry_msgs::PoseStamped>& plan, const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, int startI, int startJ, int goalI, int goalJ)
{
int iPoint = startI;
int jPoint = startJ;
plan.push_back(start);
double x,y;
int counter = 0;
geometry_msgs::PoseStamped before = start;
geometry_msgs::PoseStamped point;
tf::Quaternion point_quat;
while(isEnd(gradient_, iPoint, jPoint, goalI, goalJ) == false)
{
counter++;
getMin(gradient_, iPoint, jPoint);
gradient_.mapToWorld (iPoint, jPoint, x, y);
point.pose.position.x = x;
point.pose.position.y = y;
point_quat = tf::createQuaternionFromYaw(atan2 (y, x));
point.pose.orientation.x = point_quat.x();
point.pose.orientation.y = point_quat.y();
point.pose.orientation.z = point_quat.z();
point.pose.orientation.w = point_quat.w();
if(counter%2 == 0)
plan.push_back(point);
before = point;
}
plan.push_back(goal);
}
开发者ID:aortizgu,项目名称:ROS,代码行数:37,代码来源:csuro_global_planner.cpp
注:本文中的costmap_2d::Costmap2D类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论