void XVMatrix::setValues(RTT::types::carray<double> & inputs)
{
if (sizeCheck(inputs.count()))
{
memcpy(mat, inputs.address(), size() * sizeof(double));
}
else
{
std::__throw_out_of_range(__N("XVMatrix::_M_range_check"));
}
}
void XVMatrix::setValues(double & inputs, std::size_t size)
{
if (sizeCheck(size))
{
memcpy(mat, &inputs, this->size() * sizeof(double));
}
else
{
std::__throw_out_of_range(__N("XVMatrix::_M_range_check"));
}
}
void XVMatrix::setValues(std::vector<double> & inputs)
{
if (sizeCheck(inputs.size()))
{
memcpy(mat, inputs.data(), size() * sizeof(double));
}
else
{
std::__throw_out_of_range(__N("XVMatrix::_M_range_check"));
}
}
//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);
}
//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);
}
//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);
}
