Skip to content

Commit

Permalink
Cleanup baed on clang-tidy output
Browse files Browse the repository at this point in the history
  • Loading branch information
@mdrwiega
mdrwiega committed May 29, 2020
1 parent 43308c7 commit 5e07d3f
Show file tree
Hide file tree
Showing 16 changed files with 796 additions and 1,065 deletions.
34 changes: 26 additions & 8 deletions .clang-tidy
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,35 @@
Checks: '-*,
bugprone-*,
clang-analyzer-*,
performance-*,
llvm-namespace-comment,
modernize-redundant-void-arg,
modernize-use-nullptr,
modernize-use-default,
modernize-use-override,
modernize-loop-convert,
modernize-*
readability-avoid-const-params-in-decls,
readability-braces-around-statements,
readability-container-size-empty,
readability-delete-null-pointer,
readability-deleted-default,
readability-else-after-return,
readability-function-size,
readability-identifier-naming,
readability-implicit-bool-conversion,
readability-inconsistent-declaration-parameter-name,
readability-misleading-indentation,
readability-misplaced-array-index,
readability-named-parameter,
readability-non-const-parameter,
readability-redundant-control-flow,
readability-redundant-declaration,
readability-redundant-function-ptr-dereference,
readability-redundant-member-init,
readability-redundant-smartptr-get,
readability-redundant-string-cstr,
readability-redundant-string-init,
readability-simplify-boolean-expr,
readability-container-size-empty,
readability-identifier-naming,
readability-static-accessed-through-instance,
readability-static-definition-in-anonymous-namespace,
readability-uniqueptr-delete-release,
misc-*
'
HeaderFilterRegex: ''
AnalyzeTemporaryDtors: false
Expand Down Expand Up @@ -39,7 +57,7 @@ CheckOptions:
value: '_'
# const static or global variables are UPPER_CASE
- key: readability-identifier-naming.EnumConstantCase
value: UPPER_CASE
value: CamelCase
- key: readability-identifier-naming.StaticConstantCase
value: UPPER_CASE
- key: readability-identifier-naming.ClassConstantCase
Expand Down
4 changes: 2 additions & 2 deletions cliff_detector/include/cliff_detector/cliff_detector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,7 @@ class CliffDetector {
/**
* @brief calcTiltCompensationFactorsForImgRows calculate factors used in tilt compensation
*/
void calcTiltCompensationFactorsForImgRows(double vertical_fov);
void calcTiltCompensationFactorsForImgRows();

private:
// ROS parameters configurated with config file or dynamic_reconfigure
Expand Down Expand Up @@ -222,4 +222,4 @@ class CliffDetector {
depth_nav_msgs::Point32List stairs_points_msg_;
};

}
} // namespace cliff_detector
49 changes: 5 additions & 44 deletions cliff_detector/include/cliff_detector/cliff_detector_node.h
View file
Original file line number Diff line number Diff line change
@@ -1,41 +1,4 @@
/******************************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, Michal Drwiega (drwiega.michal@gmail.com)
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
* TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*****************************************************************************/
/**
* @file cliff_detector_node.h
* @author Michal Drwiega (drwiega.michal@gmail.com)
* @date 10.2015
* @brief cliff_detector package
*/

#ifndef CLIFF_DETECTOR_NODE
#define CLIFF_DETECTOR_NODE
#pragma once

#include <ros/ros.h>
#include <image_transport/image_transport.h>
Expand All @@ -54,10 +17,9 @@

#include <cliff_detector/cliff_detector.h>

namespace cliff_detector
{
class CliffDetectorNode
{
namespace cliff_detector {

class CliffDetectorNode {
public:
CliffDetectorNode(ros::NodeHandle& n, ros::NodeHandle& pnh);
~CliffDetectorNode();
Expand Down Expand Up @@ -133,6 +95,5 @@ namespace cliff_detector
/// Prevents the connectCb and disconnectCb from being called until everything is initialized.
boost::mutex connection_mutex_;
};
};

#endif
} // namespace cliff_detector
118 changes: 48 additions & 70 deletions cliff_detector/src/cliff_detector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ void CliffDetector::detectCliff( const sensor_msgs::ImageConstPtr& depth_msg,
calcGroundDistancesForImgRows(vertical_fov);

// Sensor tilt compensation
calcTiltCompensationFactorsForImgRows(vertical_fov);
calcTiltCompensationFactorsForImgRows();

// Check scan_height vs image_height
if (used_depth_height_ > depth_msg->height)
Expand All @@ -55,60 +55,48 @@ void CliffDetector::detectCliff( const sensor_msgs::ImageConstPtr& depth_msg,
}
}

void CliffDetector::setRangeLimits( const float rmin, const float rmax )
{
if (rmin >= 0 && rmin < rmax)
{
void CliffDetector::setRangeLimits( const float rmin, const float rmax) {
if (rmin >= 0 && rmin < rmax) {
range_min_ = rmin;
}
else
{
else {
range_min_ = 0;
ROS_ERROR("Incorrect value of range minimal parameter. Set default value: 0.");
}
if (rmax >= 0 && rmin < rmax)
if (rmax >= 0 && rmin < rmax) {
range_max_ = rmax;
else
{
}
else {
range_max_ = 10;
ROS_ERROR("Incorrect value of range maximum parameter. Set default value: 10.");
}
}

void CliffDetector::setSensorMountHeight (const float height)
{
if (height > 0)
{
void CliffDetector::setSensorMountHeight (const float height) {
if (height > 0) {
sensor_mount_height_ = height;
}
else
{
else {
sensor_mount_height_ = 0;
ROS_ERROR("Incorrect value of sensor mount height parameter. Set default value: 0.");
}
}

void CliffDetector::setSensorTiltAngle (const float angle)
{
if (angle < 90 && angle > -90)
{
void CliffDetector::setSensorTiltAngle (const float angle) {
if (angle < 90 && angle > -90) {
sensor_tilt_angle_ = angle;
}
else
{
else {
sensor_tilt_angle_ = 0;
ROS_ERROR("Incorrect value of sensor tilt angle parameter. Set default value: 0.");
}
}

void CliffDetector::setUsedDepthHeight(const unsigned int height)
{
if (height > 0)
{
void CliffDetector::setUsedDepthHeight(const unsigned int height) {
if (height > 0) {
used_depth_height_ = height;
}
else
{
else {
used_depth_height_ = 100;
ROS_ERROR("Incorrect value of used depth height parameter. Set default value: 100.");
}
Expand Down Expand Up @@ -223,7 +211,7 @@ void CliffDetector::calcDeltaAngleForImgRows(double vertical_fov)
}
}

void CliffDetector::calcGroundDistancesForImgRows(double vertical_fov)
void CliffDetector::calcGroundDistancesForImgRows([[maybe_unused]] double vertical_fov)
{
const double alpha = sensor_tilt_angle_ * M_PI / 180.0; // Sensor tilt angle in radians
const unsigned int img_height = camera_model_.fullResolution().height;
Expand All @@ -233,23 +221,20 @@ void CliffDetector::calcGroundDistancesForImgRows(double vertical_fov)
dist_to_ground_.resize(img_height);

// Calculations for each row of image
for(unsigned int i = 0; i < img_height; i++)
{
for (unsigned int i = 0; i < img_height; i++) {
// Angle between ray and optical center
if ((delta_row_[i] + alpha) > 0)
{
if ((delta_row_[i] + alpha) > 0) {
dist_to_ground_[i] = sensor_mount_height_ * sin(M_PI/2 - delta_row_[i]) * 1000
/ cos(M_PI/2 - delta_row_[i] - alpha);
ROS_ASSERT(dist_to_ground_[i] > 0);
}
else
{
else {
dist_to_ground_[i] = 100 * 1000;
}
}
}

void CliffDetector::calcTiltCompensationFactorsForImgRows(double vertical_fov)
void CliffDetector::calcTiltCompensationFactorsForImgRows()
{
const double alpha = sensor_tilt_angle_ * M_PI / 180.0; // Sensor tilt angle in radians
const unsigned int img_height = camera_model_.fullResolution().height;
Expand All @@ -258,17 +243,16 @@ void CliffDetector::calcTiltCompensationFactorsForImgRows(double vertical_fov)

tilt_compensation_factor_.resize(img_height);

for(unsigned int i = 0; i < img_height; i++) // Process all rows
{
for (unsigned int i = 0; i < img_height; i++) { // Process all rows
tilt_compensation_factor_[i] = sin(M_PI/2 - delta_row_[i] - alpha)
/ sin(M_PI/2 - delta_row_[i]);
ROS_ASSERT(tilt_compensation_factor_[i] > 0 && tilt_compensation_factor_[i] < 10);
}
}

void CliffDetector::findCliffInDepthImage( const sensor_msgs::ImageConstPtr &depth_msg)
void CliffDetector::findCliffInDepthImage(const sensor_msgs::ImageConstPtr &depth_msg)
{
enum point { Row, Col, Depth };
enum Point { Row, Col, Depth };

const uint16_t* depth_row = reinterpret_cast<const uint16_t*>(&depth_msg->data[0]);

Expand All @@ -277,8 +261,7 @@ void CliffDetector::findCliffInDepthImage( const sensor_msgs::ImageConstPtr &dep
const unsigned int img_height = camera_model_.fullResolution().height;
const unsigned int img_width = camera_model_.fullResolution().width;

if ((block_size_ % 2) != 0)
{
if ((block_size_ % 2) != 0) {
ROS_ERROR("Block size should be even number. Value will be decreased by one.");
block_size_--;
}
Expand All @@ -292,8 +275,7 @@ void CliffDetector::findCliffInDepthImage( const sensor_msgs::ImageConstPtr &dep

// Check if points thresh isn't too big
if (block_points_thresh_ >= (block_size_ * block_size_ / depth_image_step_col_
/ depth_image_step_row_))
{
/ depth_image_step_row_)) {
ROS_ERROR("Block points threshold is too big. Maximum admissible value will be set.");
block_points_thresh_ = block_size_*block_size_ / depth_image_step_col_ / depth_image_step_row_;
}
Expand All @@ -311,40 +293,35 @@ void CliffDetector::findCliffInDepthImage( const sensor_msgs::ImageConstPtr &dep
px_nr.resize(block_size_ * block_size_);

// Four pixels in center of block
const unsigned int c = block_size_ / 2;
std::vector<unsigned int> center_points(4);
const unsigned c = block_size_ / 2;
std::vector<unsigned> center_points(4);
center_points[0] = c * block_size_ + c-1;
center_points[1] = c * block_size_ + c;
center_points[2] = (c-1) * block_size_ + c-1;
center_points[3] = (c-1)*block_size_ + c ;

// Loop over each block row in image, bj - block column
for (unsigned int bj = 0; bj < block_rows_nr; ++bj)
{
for (unsigned bj = 0; bj < block_rows_nr; ++bj) {
// Loop over each block column in image, bi - block row
for (unsigned int bi = 0; bi < block_cols_nr; ++bi)
{
for (unsigned bi = 0; bi < block_cols_nr; ++bi) {
// Start of block processing
unsigned int block_cnt = 0;
unsigned block_cnt = 0;
std::fill(px_nr.begin(), px_nr.end(), -1);

// Loop over each row in block, j - column
for (unsigned int j = 0; j < block_size_; j += depth_image_step_row_)
{
for (unsigned j = 0; j < block_size_; j += depth_image_step_row_) {
// Loop over each column in block, i - row
for (unsigned int i = 0; i < block_size_; i += depth_image_step_col_)
{
for (unsigned i = 0; i < block_size_; i += depth_image_step_col_) {
// Rows from bottom of image
unsigned int row = (img_height - 1 ) - ( bj * block_size_+ j );
unsigned int col = bi * block_size_ + i;
unsigned row = (img_height - 1 ) - (bj * block_size_+ j);
unsigned col = bi * block_size_ + i;
ROS_ASSERT(row < img_height && col < img_width);

unsigned int d = depth_row[row_size * row + col];
unsigned d = depth_row[row_size * row + col];

// Check if distance to point is greater than distance to ground plane
if (d > (dist_to_ground_[row] + ground_margin_mm) &&
d > range_min_mm && d < range_max_mm)
{
d > range_min_mm && d < range_max_mm) {
tpoints[block_cnt][Row] = row;
tpoints[block_cnt][Col] = col;
tpoints[block_cnt][Depth] = d;
Expand All @@ -355,23 +332,24 @@ void CliffDetector::findCliffInDepthImage( const sensor_msgs::ImageConstPtr &dep
}

// Check if number of stairs points in block exceeded threshold value
if (block_cnt >= block_points_thresh_)
{
if (block_cnt >= block_points_thresh_) {
// Block size is even. So first we check four pixels in center of block.
if (px_nr[center_points[0]] > 0)
if (px_nr[center_points[0]] > 0) {
stairs_points.push_back(tpoints[px_nr[center_points[0]]]);
else if (px_nr[center_points[1]] > 0)
}
else if (px_nr[center_points[1]] > 0) {
stairs_points.push_back(tpoints[px_nr[center_points[1]]]);
else if (px_nr[center_points[2]] > 0)
}
else if (px_nr[center_points[2]] > 0) {
stairs_points.push_back(tpoints[px_nr[center_points[2]]]);
else if (px_nr[center_points[3]] > 0)
}
else if (px_nr[center_points[3]] > 0) {
stairs_points.push_back(tpoints[px_nr[center_points[3]]]);
else
{ // Otherwise add all points from block to stairs points vector
}
else { // Otherwise add all points from block to stairs points vector
stairs_points.insert(stairs_points.end(), tpoints.begin(), tpoints.begin() + block_cnt);
}
}
block_cnt = 0;
}
}

Expand Down Expand Up @@ -414,4 +392,4 @@ void CliffDetector::findCliffInDepthImage( const sensor_msgs::ImageConstPtr &dep
ROS_DEBUG_STREAM("Stairs points: " << stairs_points.size());
}

}
} // namespace cliff_detector

0 comments on commit 5e07d3f

Please sign in to comment.