void DiffDriveController::cmdVelCallback(const geometry_msgs::Twist& command)
{
if (isRunning())
{
// check that we don't have multiple publishers on the command topic
if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
{
ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers()
<< " publishers. Only 1 publisher is allowed. Going to brake.");
brake();
return;
}
command_struct_.ang = command.angular.z;
command_struct_.lin = command.linear.x;
command_struct_.stamp = ros::Time::now();
command_.writeFromNonRT (command_struct_);
ROS_DEBUG_STREAM_NAMED(name_,
"Added values to command. "
<< "Ang: " << command_struct_.ang << ", "
<< "Lin: " << command_struct_.lin << ", "
<< "Stamp: " << command_struct_.stamp);
}
else
{
ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
}
}
void Octomapper::get_updated_map(struct pc_map_pair &pc_map_pair) const
{
if (pc_map_pair.map == nullptr)
{
create_map(pc_map_pair);
}
// Traverse entire tree
std::vector<std::vector<float>> odds_sum(
static_cast<unsigned long>(map_options_.lengthGrid()),
std::vector<float>(static_cast<unsigned long>(map_options_.widthGrid()), map_options_.default_logodds));
for (octomap::OcTree::iterator it = pc_map_pair.octree->begin(), end = pc_map_pair.octree->end(); it != end; ++it)
{
// If this is a leaf at max depth, then only update that node
if (it.getDepth() == pc_map_pair.octree->getTreeDepth())
{
auto [x, y] = toMapCoordinates(it.getX(), it.getY());
if (x >= 0 && x < map_options_.lengthGrid() && y >= 0 && y < map_options_.widthGrid())
{
odds_sum[x][y] += it->getLogOdds();
}
else
{
ROS_ERROR_STREAM_THROTTLE_NAMED(10, "Point Outside", "Point outside! (" << x << ", " << y << ")");
}
}
else
{
