自定义ROS layer的调用日志分析

[ INFO] [1557746278.927167199]: Using plugin “gridlayer”
[ INFO] [1557746278.933583197]: pluglib: GridLayer: matchSize
size=0.000000 0.000000. pos=0.000000 0.000000. resolution:0.000000
[ INFO] [1557746279.073044789]: pluglib: GridLayer: reconfigureCB
enabled_=1
[ INFO] [1557746279.086381217]: pluglib: GridLayer: onInitialize
[ INFO] [1557746279.299917695]: pluglib: GridLayer: matchSize
size=0.000000 0.000000. pos=0.050000 3.000000. resolution:0.000000
[ INFO] [1557746279.302124238]: pluglib: GridLayer: updateBounds
pose=0.066218 -0.020099 -0.007818, min=1.066187 -0.027917 max=1.066187 -0.027917
[ INFO] [1557746279.303423143]: pluglib: GridLayer: updateCosts
rect=(min)0 21 -> (max)1 22
[ INFO] [1557746280.974408845]: pluglib: GridLayer: updateBounds
[ INFO] [1557746279.303423143]: pluglib: GridLayer: updateCosts
[ INFO] [1557746281.302192779]: pluglib: GridLayer: updateBounds
[ INFO] [1557746279.303423143]: pluglib: GridLayer: updateCosts
[ INFO] [1557746281.635429436]: pluglib: GridLayer: updateBounds
[ INFO] [1557746279.303423143]: pluglib: GridLayer: updateCosts
…
rviz修改初始位置后. Pose会变成新的位置. Setting pose: 1.870 8.180 0.037 [frame=map]
[ INFO] [1557749408.408905702]: pluglib: Grid Now: updateBounds. pose=1.889547 8.191123 0.034500, min=2.888952 8.225616 max=2.888952 8.225616
[ INFO] [1557749408.408993152]: pluglib: GridLayer: updateCosts enabled_=1
[ INFO] [1557749408.409073535]: pluglib: GridLayer: updateCosts rect=164 57 -> 165 58

updateBounds 是一直调用的, 调用频率参数是. 调用后如果对*min/max_x/y没有进行改写, 则不调用updateCosts, 否则会调用 updateCosts.
只要找对对应关系即可.
updateBounds. pose=0.074384 -0.026727 -0.007837, min=1.074353 -0.034564 max=1.074353 -0.034564
è master_grid.setCost(21, 0, value); rect=0 21 -> 1 22 (1.074353/0.05 = 21 -0.034564/0.05=0)
Setting pose: 1.580 8.430 0.045 [frame=map]
Grid Now: updateBounds. pose=1.604650 8.445980 0.038716, min=2.603901 8.484687 max=2.603901 8.484687
è master_grid.setCost(52, 169,val) rect=169 52 -> 170 53 (52 = 2.603901/0.05 8.484687/0.05=169 )

一下为源代码
点击(此处)折叠或打开
#include <big_layers/grid_layer.h>
#include <pluginlib/class_list_macros.h>
#include <sys/time.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;

namespace simple_layer_namespace
{

unsigned flag = 0;

GridLayer::GridLayer() {}

void GridLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_);
current_ = true;
default_value_ = NO_INFORMATION;
matchSize();

dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
&GridLayer::reconfigureCB, this, _1, 2);
dsrv->setCallback(cb);

ROS_INFO(“pluglib: GridLayer: %s”, func);
}

void GridLayer::matchSize()
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
ROS_INFO(“pluglib: GridLayer: %s. size=%d %d. pos=%lf %lf. resolution:%lf”, func, master->getSizeInCellsX(), master->getSizeInCellsY(), master->getOriginX(), master->getOriginY(), master->getResolution());
}

郑州不孕不育医院排名:http://wapyyk.39.net/zz3/zonghe/1d427.html
void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
enabled_ = config.enabled;
ROS_INFO(“pluglib: GridLayer: %s, enabled_=%d”, func, enabled_);
}

static struct timeval tv = {0};
void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
struct timeval tv_now;
gettimeofday(&tv_now, NULL);
ROS_INFO(“pluglib: GridLayer: %s. enabled_ = %d, time=%ld”, func, enabled_, (tv_now.tv_sec-tv.tv_sec)*1000 + (tv_now.tv_usec-tv.tv_usec)/1000 );
memcpy(&tv, &tv_now, sizeof(tv));

if (!enabled_)
return;

if (flag == 0)
{
//flag = 1;
}
else return;

double mark_x = robot_x + cos(robot_yaw);
double mark_y = robot_y + sin(robot_yaw);
unsigned int mx;
unsigned int my;

ROS_INFO("pluglib: GridLayer: %s. pose=%lf %lf %lf, mark=%lf %lf ", func, robot_x, robot_y, robot_yaw, mark_x, mark_y);
if(worldToMap(mark_x, mark_y, mx, my)){
setCost(mx, my,LETHAL_OBSTACLE);
ROS_INFO(“pluglib: SetCost: %lf, %lf --> %d, %d”, mark_x, mark_y, mx, my);
}

*min_x = std::min(*min_x, mark_x);
*min_y = std::min(*min_y, mark_y);
*max_x = std::max(*max_x, mark_x);
*max_y = std::max(*max_y, mark_y);

ROS_INFO(“pluglib: Grid Now: %s. pose=%lf %lf %lf, min=%lf %lf max=%lf %lf”, func, robot_x, robot_y, robot_yaw, *min_x, *min_y, *max_x, *max_y);
}

void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
ROS_INFO(“pluglib: GridLayer: %s enabled_=%d”, func, enabled_);
if (!enabled_)
return;

ROS_INFO(“pluglib: GridLayer: %s rect=%d %d -> %d %d”, func, min_j, min_i, max_j, max_i);
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]);
ROS_INFO(" SetCost: %d --> %d, %d", costmap_[index], i, j);
}
}
}

} // end namespace

自定义ROS layer的调用日志分析

猜你喜欢