本文整理汇总了C++中costmap_2d::Costmap2D::worldToMap方法的典型用法代码示例。如果您正苦于以下问题:C++ Costmap2D::worldToMap方法的具体用法?C++ Costmap2D::worldToMap怎么用?C++ Costmap2D::worldToMap使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类costmap_2d::Costmap2D
的用法示例。
在下文中一共展示了Costmap2D::worldToMap方法的11个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的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;
}
}
示例2: 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);
}
}
示例3: 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);
}
示例4: 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);
}
示例5: 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;
}
示例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(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!");
}
示例7: 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);
}
示例8: setPathCells
//update what map cells are considered path based on the global_plan
void MapGrid::setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan){
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getOriginX(), costmap.getOriginY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
queue<MapCell*> path_dist_queue;
queue<MapCell*> goal_dist_queue;
for(unsigned int i = 0; i < global_plan.size(); ++i){
double g_x = global_plan[i].pose.position.x;
double g_y = 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.path_dist = 0.0;
current.path_mark = true;
path_dist_queue.push(¤t);
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
}
else{
if(started_path)
break;
}
}
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.goal_dist = 0.0;
current.goal_mark = true;
goal_dist_queue.push(¤t);
}
//compute our distances
computePathDistance(path_dist_queue, costmap);
computeGoalDistance(goal_dist_queue, costmap);
}
示例9: cos
/**
* get the cellsof a footprint at a given position
*/
std::vector<upo_navigation::Position2DInt> FootprintHelper::getFootprintCells(
Eigen::Vector3f pos,
std::vector<geometry_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D& costmap,
bool fill){
double x_i = pos[0];
double y_i = pos[1];
double theta_i = pos[2];
std::vector<upo_navigation::Position2DInt> footprint_cells;
//std::vector<geometry_msgs::Point> footprint_spec_ = costmap_ros_->getRobotFootprint();
//if we have no footprint... do nothing
if (footprint_spec.size() <= 1) {
unsigned int mx, my;
if (costmap.worldToMap(x_i, y_i, mx, my)) {
upo_navigation::Position2DInt center;
center.x = mx;
center.y = my;
footprint_cells.push_back(center);
}
return footprint_cells;
}
//pre-compute cos and sin values
double cos_th = cos(theta_i);
double sin_th = sin(theta_i);
double new_x, new_y;
unsigned int x0, y0, x1, y1;
unsigned int last_index = footprint_spec.size() - 1;
for (unsigned int i = 0; i < last_index; ++i) {
//find the cell coordinates of the first segment point
new_x = x_i + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_y = y_i + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
//find the cell coordinates of the second segment point
new_x = x_i + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
new_y = y_i + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
}
//we need to close the loop, so we also have to raytrace from the last pt to first pt
new_x = x_i + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
new_y = y_i + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
new_x = x_i + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
new_y = y_i + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
if(fill) {
getFillCells(footprint_cells);
}
return footprint_cells;
}
示例10: updateCosts
// method for applyting changes in this layer to global costmap
void HANPLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j)
{
boost::recursive_mutex::scoped_lock lock(configuration_mutex_);
if(!enabled_ || (lastTransformedHumans.size() == 0))
{
return;
}
// update map according to each human position
for(auto human : lastTransformedHumans)
{
// get position of human in map
unsigned int cell_x, cell_y;
if(!master_grid.worldToMap(human.pose.position.x, human.pose.position.y, cell_x, cell_y))
{
ROS_ERROR("hanp_layer: no world coordinates for human at x:%f y:%f",
human.pose.position.x, human.pose.position.y);
continue;
}
// general algorithm is to
// create satic grids according to human posture, standing, sitting, walking
// to calcuate cost use a sigmoid function
// however 'sitting' posture is not implemented yet
if(use_safety)
{
if(safety_grid == NULL)
{
ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as safety_grid is empty");
return;
}
// apply safety grid according to position of each human
auto size_x = (int)(safety_max / resolution), size_y = size_x;
// by convention x is number of columns, y is number of rows
for(int y = 0; y <= 2 * size_y; y++)
{
for(int x = 0; x <= 2 * size_x; x++)
{
// add to current cost
auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
+ (double)(safety_grid[x + ((2 * size_x + 1) * y)] * safety_weight);
// detect overflow
if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
{
cost = costmap_2d::LETHAL_OBSTACLE;
}
master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
}
}
}
if(use_visibility)
{
// calculate visibility grid
double yaw = tf::getYaw(human.pose.orientation);
auto visibility_grid = createVisibilityGrid(visibility_max, resolution,
costmap_2d::LETHAL_OBSTACLE, cos(yaw), sin(yaw));
if(visibility_grid == NULL)
{
ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as visibility_grid is empty");
return;
}
// apply the visibility grid
auto size_x = (int)(visibility_max / resolution), size_y = size_x;
for(int y = 0; y <= 2 * size_y; y++)
{
for(int x = 0; x <= 2 * size_x; x++)
{
// add to current cost
auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
+ (double)(visibility_grid[x + ((2 * size_x + 1) * y)] * visibility_weight);
// detect overflow
if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
{
cost = costmap_2d::LETHAL_OBSTACLE;
}
master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
}
}
}
}
// ROS_DEBUG("hanp_layer: updated costs");
}
示例11: tableApprCallback
//.........这里部分代码省略.........
Eigen::Vector3f pos(odom_pose.pose.position.x, odom_pose.pose.position.y, tf::getYaw(odom_pose.pose.orientation));
//cout << x << ", " << y << ":" << curpt.point.x << "," << curpt.point.y << "$";
//cout << double(costmap.getCost(x,y)) << endl;
double foot_cost = footprintCost(pos, 1);
if(foot_cost == 0)
valid_locs.poses.push_back(cpose.pose);
else if(foot_cost != -1)
close_locs.poses.push_back(cpose.pose);
else
invalid_locs.poses.push_back(cpose.pose);
cout << foot_cost << endl;
}
cout << costmap_ros->getRobotFootprint().size() << endl;
valid_pub.publish(valid_locs);
invalid_pub.publish(invalid_locs);
close_pub.publish(close_locs);
geometry_msgs::PoseArray dense_table_poses;
dense_table_poses.header.frame_id = "/base_link";
dense_table_poses.header.stamp = ros::Time::now();
uint32_t i2;
for(uint32_t i=0;i<table_poly.size();i++) {
i2 = i+1;
if(i2 == table_poly.size())
i2 = 0;
double diffx = table_poly[i2].x-table_poly[i].x;
double diffy = table_poly[i2].y-table_poly[i].y;
double len = std::sqrt(diffx*diffx + diffy*diffy);
double ang = std::atan2(diffy, diffx) - 3.14/2;
double incx = std::cos(ang)*0.01, incy = std::sin(ang)*0.01;
for(double t=0;t<len;t+=pose_step) {
double polyx = table_poly[i].x + t*diffx;
double polyy = table_poly[i].y + t*diffy;
geometry_msgs::PoseStamped test_pose, odom_test_pose;
bool found_pose = false;
for(int k=start_dist/0.01;k<max_dist/0.01;k++) {
test_pose.header.frame_id = "/base_link";
test_pose.header.stamp = ros::Time(0);
test_pose.pose.position.x = polyx + incx*k;
test_pose.pose.position.y = polyy + incy*k;
test_pose.pose.orientation = tf::createQuaternionMsgFromYaw(ang+3.14);
tf_listener.transformPose("/odom_combined", test_pose, odom_test_pose);
Eigen::Vector3f pos(odom_test_pose.pose.position.x,
odom_test_pose.pose.position.y,
tf::getYaw(odom_test_pose.pose.orientation));
double foot_cost = footprintCost(pos, 1.0);
// found a valid pose
if(foot_cost >= 0 && foot_cost <= min_cost) {
found_pose = true;
break;
}
uint32_t mapx, mapy;
// break if we go outside the grid
if(!costmap.worldToMap(odom_test_pose.pose.position.x,
odom_test_pose.pose.position.y, mapx, mapy))
break;
double occ_map = double(costmap.getCost(mapx, mapy));
// break if we come across and obstacle
if(occ_map == costmap_2d::LETHAL_OBSTACLE ||
occ_map == costmap_2d::NO_INFORMATION)
break;
}
if(found_pose)
dense_table_poses.poses.push_back(test_pose.pose);
}
}
ROS_INFO("POLY: %d, denseposes: %d", table_poly.size(), dense_table_poses.poses.size());
// downsample and sort dense pose possibilties by distance to
// approach point and thresholded distance to each other
geometry_msgs::PoseArray downsampled_table_poses;
boost::function<bool(geometry_msgs::Pose&, geometry_msgs::Pose&)> dist_comp
= boost::bind(&pose_dist_comp, _1, _2, approach_pt.point);
while(ros::ok() && !dense_table_poses.poses.empty()) {
// add the closest valid pose to the approach location on the table
geometry_msgs::Pose new_pose = *std::min_element(
dense_table_poses.poses.begin(), dense_table_poses.poses.end(),
dist_comp);
downsampled_table_poses.poses.push_back(new_pose);
// remove all poses in the dense sampling which are close to
// the newest added pose
boost::function<bool(geometry_msgs::Pose)> rem_thresh
= boost::bind(&pose_dist_thresh, _1, new_pose.position,
close_thresh);
dense_table_poses.poses.erase(std::remove_if(
dense_table_poses.poses.begin(),
dense_table_poses.poses.end(),
rem_thresh),
dense_table_poses.poses.end());
ROS_INFO("denseposes: %d", dense_table_poses.poses.size());
}
downsampled_table_poses.header.frame_id = "/base_link";
downsampled_table_poses.header.stamp = ros::Time::now();
approach_pub.publish(downsampled_table_poses);
resp.approach_poses = downsampled_table_poses;
ROS_INFO("POLY: %d, poses: %d", table_poly.size(), downsampled_table_poses.poses.size());
delete world_model;
return true;
}